%%%%% math

Many robotic systems are subject to motion constraints which cannot be
expressed as configuration constraints.  A familiar example of such
a system is a car.  At low speeds, the rear wheels of the car roll
freely in the direction they are pointing, but
they prevent slipping motion in the perpendicular direction.  This
constraint implies that the car cannot translate directly to the side.
We know by experience, however, that this motion constraint does not
imply a constraint on configurations; the car can reach any
position and orientation in the
obstacle-free plane.  In fact, the prevented sideways translation can
be approximated by parallel-parking maneuvers.

The no-slip constraint is a {\em nonholonomic\/} constraint, a 
constraint on the velocity.  In
addition to rolling without slipping, conservation of angular momentum
is a common source of nonholonomic constraints in mechanical
systems.  
%Systems with nonholonomic constraints are sometimes called
%nonholonomic.

If instead of viewing the car as a system subject to a motion
constraint, we considered the fact that there are only two inputs
(speed and steering angle) to control the car's three
degrees-of-freedom, we might call the system {\em underactuated\/}.
Underactuated systems have fewer controls than degrees-of-freedom.
For dynamic systems, such as those described in the previous chapter,
underactuation implies constraints on the possible accelerations of
the system.  
%In this chapter, we will use the word ``underactuated''
%to refer to both kinematic and dynamic systems.
%and they include all nonholonomic systems.
% not really, could have fully actuated spacecraft on a rail,
% some thrusters just produce constraint forces.

In this section we study motion planning for systems that are
underactuated or subject to
motion constraints.  Our first task is to determine if the constraints
actually limit the reachable space of the robot system.  This is a
controllability question.  The next problem is to construct algorithms
that find motion plans satisfying the motion constraints.  A last
problem, not addressed in this chapter, is feedback stabilization of
the motion plans, or nonsmooth or time-varying control laws to stabilize
a state.

We begin in Section~\ref{sec:preliminaries} by providing some background
information on vector fields and their Lie (pronounced ``lee'') algebras.  
Section~\ref{sec:controlsystems} defines the class of control systems
we will consider.  Section~\ref{sec:controllability} describes different
controllability notions and tests for these nonlinear systems.  
Section~\ref{sec:mechanicalsystems} specializes the discussion to 
second-order mechanical systems.  Finally, Section~\ref{sec:planning}
describes a number of methods for motion planning for nonholonomic and
underactuated systems.

\section{Preliminaries}
\label{sec:preliminaries}

%manifolds
%Lie groups
%matrix Lie groups, R^n

First we must decide how generally to define the state spaces of the
robotic systems we will consider.  For example, we could treat the
most general case, allowing the state space of the system to be any
smooth manifold.  This allows us to study, for example, the motion of
a spherical pendulum.  The configuration space of this system is the
sphere $S^2$.  Or we could limit our treatment to systems evolving on
Lie groups, and particularly matrix Lie groups.  This would allow us
to model the orientation of a satellite as a point in $SO(3)$.

In this chapter, we will restrict our attention even further to
systems evolving on ``flat'' vector spaces $\manif = \real^n$.  This
will allow us to get to the main results as quickly as possible.
Also, any smooth $n$-dimensional manifold is locally ``similar''
(diffeomorphic) to $\real^n$, so, equipped with a proper set of local
coordinates, any $n$-dimensional manifold can be treated locally as
$\real^n$.  By making this simplification, we require the use of a local
coordinate system in our computations, and we lose information about
the global structure of the space.  As examples, the true
configuration space of a 2R robot arm is $T^2 = S^1 \times S^1$, which
is compact and doughnut-shaped, while $\real^2$ is not; and a global
representation of the orientation of a satellite is $SO(3)$, which
is different than a local representation using three
Euler angles ($\real^3$). 

Although we focus on vector state spaces, most of the ideas in this 
chapter generalize immediately to general manifolds.

In this chapter, $q \in {\Cs}$ denotes the configuration of the system and 
$x \in \manif$ denotes the state of the system.  If the system is kinematic,
then the state is simply the configuration ($\manif={\Cs}$), and the controls are
velocities.  If the system is a second-order dynamic system,
then $x$ includes both configurations $q$ and velocities $\dot{q}$,
and the controls are accelerations (forces).  We could also have
systems where some controls are velocities and some are forces, which
we might call ``mixed kinematic-dynamic systems.''  The dimension of
the configuration space ${\Cs}$ is $n_{\Cs}$, and the dimension
of the state space $\manif$ is $n$.

We will carry the following
two examples throughout the chapter.

\begin{example}
{\em
{\em Unicycle example.\/} The unicycle is a wheel which rolls upright
on a horizontal plane (Figure~\ref{fig:unicycle}).  The configuration
of the wheel is $q = (q_1,q_2,q_3)^T$, describing the contact point of
the wheel on the plane $(q_1,q_2)$ and the steering angle $q_3$ of the
wheel.  (We could also include the rolling angle of the wheel, i.e.,
the location of the air nozzle on the wheel, in the description of the
configuration, but we will ignore this.)  The system is
kinematic, so $x = (x_1,x_2,x_3)^T = q$, $\manif = {\Cs} = \real^3$, and
$n_{\Cs} = n = 3$.  (Since
we are dealing with local coordinates, we are ignoring the fact that
the global structure of the space is $\real^2 \times S^1$.  This will
not affect the equations of motion, but requires the use of ${\rm mod} 2\pi$
on the third coordinate.)
The controls are the rolling speed of the
wheel and the rate of change of the steering angle.  Sideways
translation of the wheel is prevented by the no-slip constraint
imposed by the wheel.  This example is sometimes known as the rolling
penny or the pizza cutter, and it is similar to a model for a car.
}
\end{example}

\begin{figure}
\begin{center}
\includegraphics{../figure/book/nonholonomic/unicycle2.ps}
\end{center}
\caption{The unicycle system.}
\label{fig:unicycle}
\end{figure}

\begin{example}
{\em
{\em Planar body with thrusters (PBWT) example.\/}  The body moves in a frictionless,
inviscid plane by means of two thrusters (Figure~\ref{fig:planarbody}).  
The mass and inertia of the body (about
the center of mass) are unit.  The line of action of the thrust $u_1$ is through the center of mass,
and the line of action of the thrust $u_2$ is in an orthogonal direction a distance
$d$ from the center of mass.  The configuration is $q = (q_1,q_2,q_3)^T$,
describing the location of the center of mass $(q_1,q_2)$ and the angle 
$q_3$ of the line of action
of the first thruster
relative to the world $q_1$-axis.  The system is second-order dynamic, so
$x = (x_1,x_2,x_3,x_4,x_5,x_6)^T = (q_1,q_2,q_3,\dot{q}_1,\dot{q}_2,\dot{q}_3)^T$,
$\manif = \real^6$, $n_{\Cs} = 3$, and $n = 2n_{\Cs} = 6$.
Gravitational acceleration $a_g$ acts in the $-q_2$ direction, and $a_g$ may be zero.
}
\end{example}

\begin{figure}
\begin{center}
\includegraphics{../figure/book/nonholonomic/planarbody.ps}
\end{center}
\caption{The PBWT.}
\label{fig:planarbody}
\end{figure}

%*** Other possible examples include the snakeboard (mixed system) or car + trailer.

\subsection{Tangent Spaces and Vector Fields}

Let $x(s): \real \rightarrow \manif$ be a
smooth curve on $\manif$ parameterized by $s$.  Then $dx/ds$,
evaluated at $x_0$, is tangent to the curve at $x_0$.
Call this vector $V$.  $V$ is a {\bf tangent vector} which is
tangent to $\manif$ at $x_0$.  $V$ lives in
$T_{x_0} \manif$, the {\bf tangent space} of $\manif$ at $x_0$.  
This space is an $n$-dimensional vector space $\real^n$ consisting of the tangents of all
possible curves passing through $x_0$.  If $\manif$ is a configuration
space, $T_{x_0} \manif$ is the set of all possible velocities of the system
at $x_0$, and if $\manif$ is a state space, $T_{x_0} \manif$ is the set of all 
possible velocities and accelerations of the system
at $x_0$.  (Keep in mind that the tangent spaces at
different points of $\manif$ are different spaces.)
The {\bf tangent bundle} of $\manif$, written $T\manif$, is the $2n$-dimensional
manifold which is the union of all
tangent spaces at all points in $\manif$,
\[
T\manif = \bigcup_{x\in\manif} T_x\manif.
\]
For the systems we study, $T\manif = \manif \times \real^n = \real^{2n}$.
(We note in passing that if $\manif$ is a more general manifold, and it is 
{\em parallelizable\/} (for example, a Lie group), then $T\manif = \manif \times
\real^n$.  The reader should be careful not to generalize improperly, however.
For example, $T S^2 \neq S^2 \times \real^2$, for reasons beyond the scope of 
this chapter.  For an intuitive discussion of this issue, see~\cite{Schutz}.)

% canonical basis for tangent vectors?  \partial/\partial x

A (smooth) {\bf vector field} $g:\manif \rightarrow T\manif$ is a
smooth map from points $x \in \manif$ to tangent vectors $g(x) \in
T_x\manif$.  
%Written in components, $g(x) = (g_1(x), g_2(x),
%\ldots,g_n(x))^T$, where each component is smooth.  
It is possible to
define $C^k$ vector fields, but we will assume that all vector fields
are analytic.  

\begin{example}
{\em
{\em Unicycle (cont.)\/}  A tangent vector for the unicycle is given
by $\dot{x} = (\dot{x}_1,\dot{x}_2,\dot{x}_3)^T$.
The unicycle is capable of rolling forward and backward
and spinning in place.  These two vector fields can be written
$g_1^{uni}(x) = (\cos x_3,\sin x_3,0)^T$ and $g_2^{uni}(x) = (0,0,1)^T$, respectively.
Defining the canonical basis of vector fields to be $\{\partial/\partial x_1,
\partial/\partial x_2, \partial/\partial x_3\}$, the vector fields
can be equivalently written $g_1^{uni}(x) = (\cos x_3) \partial/\partial x_1 +
(\sin x_3) \partial/\partial x_2$ and $g_2^{uni}(x) = \partial/\partial x_3$.
See Figure~\ref{fig:vecfield}.
}
\end{example}

\begin{figure}
\begin{center}
\includegraphics{../figure/book/nonholonomic/vecfield.ps}
\end{center}
\caption{The vector field $g_2^{uni}=(0,0,1)^T$.}
\label{fig:vecfield}
\end{figure}

\begin{example}
{\em
{\em PBWT (cont.)\/} A tangent vector for the PBWT is given by
$\dot{x} = (\dot{x}_1,\dot{x}_2,\dot{x}_3,\dot{x}_4,\dot{x}_5,\dot{x}_6)^T$.
For this system, we can define three vector fields: the {\em drift\/}
vector field $g_0^{pbwt}(x)$ corresponding to the motion of the body when no
thrusters are activated, and the {\em control} vector fields $g_1^{pbwt}(x)$ and
$g_2^{pbwt}(x)$ corresponding to the acceleration when thrusters 1 and 2 are
fired with unit thrust, respectively.  Verify that $g_0^{pbwt}(x) =
(x_4,x_5,x_6,0,a_g,0)^T, g_1^{pbwt}(x) = (0,0,0,\cos x_3,
\sin x_3,0)^T, g_2^{pbwt}(x) = (0,0,0,-\sin x_3,\cos x_3, -d)^T$, and write
these vector fields in the canonical basis $\{ \partial/\partial x_1,
\partial/\partial x_2,\partial/\partial x_3,\partial/\partial x_4,\partial/\partial x_5,
\partial/\partial x_6\}$.  Notice if thruster 1 is fired with unit 
thrust, the system follows the vector field $g_0^{pbwt}(x) + g_1^{pbwt}(x)$.
}
\end{example}

Let $\phi^g$ denote the {\bf flow} of the vector
field $g$, where $\phi^g_t(x)$ gives the system state after following the
flow $\phi^g$ from $x$ for a time $t$.  The flow satisfies the equation
\[
\frac{d}{dt}\phi^g_t(x) = g(\phi^g_t(x)).
\]
The vector field is {\bf complete} if
its flow is defined for all $x$ and $t$.

The curve $\{\phi^g_t(x) | t \in \real\}$ is the {\bf integral curve}
of $g$ containing $x$.  The integral curve describes the set of
reachable points of $\manif$ from $x$ by following the vector field forward and
backward in time. 
This notion can be generalized to the {\em integral manifold\/}
of a set of vector fields $\cal G$, a topic for Section~\ref{ssec:lie}.

\subsection{Distributions and Constraints}

Let $\cal G$ be a set of vector fields, and let
${\rm span}({\cal G})$ be the linear span of vector fields in
$\cal G$, given by all linear combinations of vector fields in $\cal G$.  
At each point $x\in\manif$, these vector fields span a linear subspace
of $T_x\manif$.  ${\cal G}$ is said to generate a {\bf
distribution} ${\cal D}$, which is a smooth assignment of a linear subspace of
$T_x \manif$ for each $x \in \manif$.  A distribution is {\bf
regular} if the dimension of the linear subspace is the same for all
$x$.  If the dimension is $m$, then we say that it is an
$m$-dimensional distribution.

Consider the two-dimensional distribution for the unicycle 
${\rm span}(\{g_1^{uni},g_2^{uni}\}) = u_1(x) g_1^{uni}(x) + u_2(x) g_2^{uni}(x), u_1(x),u_2(x) \in \real$.
We might think of this as the ``positive''
form of the distribution --- feasible motions are generated by linear combinations
of the vector
fields.  A ``negative'' form of the distribution would start
with all motions being feasible, then eliminate those which violate motion
constraints.  For instance, the unicycle distribution could be written
\begin{equation}
{\cal D}(x) = \{\dot{x} \in T_x\manif \; | \; \omega(x) \dot{x} = 0\},\;\;
\omega(x) = (-\sin x_3, \cos x_3, 0),
\label{eq:Pfaffian}
\end{equation}
where ${\cal D}(x)$ is the linear subspace of the distribution at $x\in\manif$.  The row
vector $\omega(x)$ is called a {\bf covector} and lives in the 
{\bf cotangent space} $T^*_x\manif$, the dual of $T_x\manif$ consisting
of all linear functions of elements of $T_x\manif$.  The motion constraint $\omega$
is called a {\bf covector field} or {\bf one-form}.  A set of such motion constraints is
said to define a constraint {\bf codistribution} $\Omega$.  This constraint codistribution 
$\Omega$ is said
to \emph{annihilate} the distribution ${\cal D}$, and vice-versa.
%in mechanics, sometimes generalized forces, and if
%constraints are workless, then f^Tv = 0.
% didn't define cotangent bundle

Of special interest are velocity constraints of the form
\begin{equation}
f(q,\dot{q}) = 0
\label{eq:nonholonomic}
\end{equation}
which cannot be integrated to yield equivalent configuration
constraints.  Such constraints are called {\bf nonholonomic}.
Nonholonomic constraints of the form
\[
a(q) \dot{q} = 0
\]
are sometimes called {\bf Pfaffian} constraints, as discussed
in the previous chapter.  Pfaffian
constraints arise from rolling without slip (e.g., Equation~\eqref{eq:Pfaffian})
and conservation of angular momentum.  Acceleration constraints of the form
\[
f(q,\dot{q},\ddot{q}) = 0
\]
which cannot be expressed as equivalent velocity constraints are 
referred to by some as ``second-order nonholonomic'' constraints, but this
terminology is not standard.

In general, it is not easy to determine if an acceleration constraint can be
integrated to yield an equivalent velocity constraint, or if a velocity constraint
can be integrated to yield an equivalent configuration constraint. In the rest of this
chapter, we will use the ``positive'' form of the distribution and study
the reachable set by vector fields in $\cal G$.

%frobenius:  a regular distribution is integrable if and only if it is involutive.

%the question of integrability of a constraint is bound up with involutivity
%of a distribution and reachable spaces for vector fields

\subsection{Lie Brackets}
\label{ssec:lie}

Let $\cal G$ be a set of vector fields and ${\cal D}$ be the
distribution defined by ${\rm span}({\cal G})$.  We would like to know
the reachable set of $\manif$ by following vector fields in ${\cal D}$.
While this is generally difficult globally, it is possible to learn
something about the reachable set {\em locally\/} by looking at the
{\bf Lie brackets} of vector fields in ${\cal D}$.  Given two vector
fields belonging to ${\cal D}$, the Lie bracket tells us if
infinitesimal motions along these vector fields can be used to locally
generate motion in a direction not contained in ${\cal D}$.  Perhaps the
best known example is the parallel-parking maneuver for a car, or, in
our case, a unicycle.  Direct sideways motion is prohibited by the
no-slip constraint, but sideways motion can be approximated by a
series of forward-backward and turning maneuvers.  The implication of
this is that the locally reachable set of $\manif$ is not
two-dimensional, as the two-dimensional distribution ${\cal D}$ might
seem to indicate, but fully three-dimensional.  The no-slip velocity
constraint does not imply a constraint on reachable configurations.

For two vector fields $g_1, g_2 \in {\cal G}$,
consider the state reached from $x_0 = x(0)$ by first following $g_1$ for a small
time $\epsilon << 1$,
then following $g_2$ for time $\epsilon$, then following $g_1$ for time $-\epsilon$
(equivalently $-g_1$ for $\epsilon$),
then following $g_2$ for time $-\epsilon$ (equivalently $-g_2$ for $\epsilon$).  
This is expressed mathematically as
\begin{equation}
x(4\epsilon) = \phi^{-g_2}_{\epsilon} \circ \phi^{-g_1}_{\epsilon} \circ \phi^{g_2}_{\epsilon} 
\circ \phi^{g_1}_{\epsilon}(x_0).
\label{eq:liebracket}
\end{equation}
We can take a Taylor series in $\epsilon$ to solve the differential
equation \eqref{eq:liebracket} approximately, yielding
\begin{equation}
x(4\epsilon) = x_0 + \epsilon^2 \left(\frac{\partial g_2}{\partial x} g_1(x_0) - 
\frac{\partial g_1}{\partial x} g_2(x_0)\right) + O(\epsilon^3),
\label{eq:liebrackettaylorexpansion}
\end{equation}
where the partial derivatives are evaluated at $x_0$ and
$O(\epsilon^3)$ indicates terms of order $\epsilon^3$, which are dominated
by the term of order $\epsilon^2$ when $\epsilon$ is small.  Note there are
no $O(\epsilon)$ terms.  The
$\epsilon^2$ term is the Lie bracket of $g_1$ and $g_2$ and
represents the approximate net motion of the system.

%*** Derive Lie bracket

The Lie bracket of $g_1$ and $g_2$ is written $[g_1,g_2]$ and is given in local
coordinates by 
\begin{equation}
[g_1,g_2] = \frac{\partial g_2}{\partial x} g_1 - 
\frac{\partial g_1}{\partial x} g_2.
\label{eq:liebracketcoords}
\end{equation}
The Lie bracket $[g_1,g_2]$ (sometimes written ${\rm Ad}_{g_1}g_2$)
defines a new vector field, and if it is not contained in 
${\rm span}({\cal G})$, then it represents a new motion direction which
can be followed approximately.  Locally generating motion in this direction 
is, in a sense, ``slower'' than following the vector field $g_1$ or $g_2$
directly, as the net motion is only $O(\epsilon^2)$ for time $O(\epsilon)$,
where $\epsilon << 1$.  Again, parallel-parking is a well known example,
as approximately generating sideways motion by forward-backward and turning motions
is tedious and time-consuming.
If $[g_1,g_2] = 0$, the two vector fields are said to {\bf commute}.

Since $[g_1,g_2]$ is a vector field, we can calculate its Lie
bracket with another vector field.  A {\bf Lie product of degree $k$}
is a bracket term where the original vector fields appear $k$ times.
For instance, $[[[g_1,g_2],g_1],g_2]$ is a Lie product of degree 4.

\begin{example}
{\em
{\em Unicycle (cont.)\/}  The rolling and turning vector fields for
the unicycle are $g_1^{uni} = (\cos x_3,\sin x_3,0)^T$ and $g_2^{uni} = (0,0,1)^T$, respectively.
So
\begin{eqnarray}
[g_1^{uni},g_2^{uni}] & = & \frac{\partial g_2^{uni}}{\partial x} g_1^{uni} - 
\frac{\partial g_1^{uni}}{\partial x} g_2^{uni}  \nonumber \\
%& = & \left( \begin{array}{ccc}
%\frac{\partial g_2^1}{\partial x_1} & \frac{\partial g_2^1}{\partial x_2} &
%\frac{\partial g_2^1}{\partial x_3} \\
%
%\frac{\partial g_2^2}{\partial x_1} & \frac{\partial g_2^2}{\partial x_2} &
%\frac{\partial g_2^2}{\partial x_3} \\
%
%\frac{\partial g_2^3}{\partial x_1} & \frac{\partial g_2^3}{\partial x_2} &
%\frac{\partial g_2^3}{\partial x_3} 
%\end{array} \right)
& = & \left( \frac{\partial g_2^{uni}}{\partial x_1} \;\; \frac{\partial g_2^{uni}}{\partial x_2} \;\; \frac{\partial g_2^{uni}}{\partial x_3}\right)
g_1^{uni}
%\left( \begin{array}{c}
%g_1^1 \\
%g_1^2 \\
%g_1^3
%\end{array}
%\right)
- 
%\left( \begin{array}{ccc}
%\frac{\partial g_1^1}{\partial x_1} & \frac{\partial g_1^1}{\partial x_2} &
%\frac{\partial g_1^1}{\partial x_3} \\
%
%\frac{\partial g_1^2}{\partial x_1} & \frac{\partial g_1^2}{\partial x_2} &
%\frac{\partial g_1^2}{\partial x_3} \\
%
%\frac{\partial g_1^3}{\partial x_1} & \frac{\partial g_1^3}{\partial x_2} &
%\frac{\partial g_1^3}{\partial x_3} 
%\end{array} \right)
\left( \frac{\partial g_1^{uni}}{\partial x_1} \;\; \frac{\partial g_1^{uni}}{\partial x_2} \;\; \frac{\partial g_1^{uni}}{\partial x_3}\right)
%\left( \begin{array}{c}
%g_2^1 \\
%g_2^2 \\
%g_2^3
%\end{array}
%\right)
g_2^{uni}
\nonumber \\
& = & \left( \begin{array}{ccc}
0 & 0 & 0 \\
0 & 0 & 0 \\
0 & 0 & 0
\end{array}
\right) 
\left( \begin{array}{c}
\cos x_3 \\
\sin x_3 \\
0
\end{array} \right)
-
\left( \begin{array}{ccc}
0 & 0 & -\sin x_3 \\
0 & 0 & \cos x_3 \\
0 & 0 & 0
\end{array}
\right) 
\left( \begin{array}{c}
0 \\
0 \\
1
\end{array} \right) 
\nonumber \\
& = & 
\left( \begin{array}{c}
\sin x_3 \\
-\cos x_3 \\
0
\end{array} \right).
\nonumber
\end{eqnarray}
Note that the Lie bracket motion is to the side, in the direction prevented
by the no-slip constraint.  See Figure~\ref{fig:liebracket}.  
}
\end{example}

\begin{figure}
\begin{center}
\includegraphics{../figure/book/nonholonomic/liebracket.ps}
\end{center}
\caption{Generating a Lie bracket motion for the unicycle, starting from the
origin.  The net motion is approximately to the side, the Lie bracket direction.
It is not exactly to the side, however, due to higher-order terms in $\epsilon$.}
\label{fig:liebracket}
\end{figure}

For vector fields which are \emph{linear} in the state,
e.g., $g_1(x) = Ax$ and $g_2(x) = Bx$, the Lie bracket has
the particularly simple form
\[
[g_1,g_2](x) = (AB-BA)x,
\]
called the {\bf matrix commutator} of $A$ and $B$.  For such vector fields, the
Lie bracket can be calculated without differentiation.  As an example, let
$x = (x_1, x_2, x_3)^T$ and
\[
A = \left( \begin{array}{rrr}
0 & 1 & 0 \\
-1 & 0 & 0 \\
0 & 0 & 0 
\end{array} \right), \;\;
B = \left( \begin{array}{rrr}
0 & 0 & 0 \\
0 & 0 & 1 \\
0 & -1 & 0 
\end{array} \right),
\] 
and let $g_1 = Ax$ and $g_2 = Bx$.  Verify that $[g_1,g_2](x) = (AB-BA)x$ using
Equation~\eqref{eq:liebracketcoords}.

The {\bf Lie algebra} of a set of vector fields $\cal G$, written
$\lie({\cal G})$, is the linear span of all Lie products of vector fields in
$\cal G$.  Let ${\cal D}_1$ be the distribution
defined by ${\rm span}({\cal G})$, and 
\[
{\cal D}_{i+1} = {\cal D}_i \cup {\rm span}(\{[g_1,g_2]|g_1 \in {\cal D}_1,g_2\in{\cal D}_i\}).
\]
Then $\lie({\cal G})$ defines the distribution ${\cal D}_\infty$.  The
series ${\cal D}_1, {\cal D}_2, \ldots$ is called the {\bf filtration} of
the distribution ${\cal D}_1$.  The filtration is {\bf regular} if each
distribution in the filtration is regular.  If the filtration is
regular, then the dimension of the distribution grows at each step of
the construction, or else the construction terminates.  (Of course,
${\rm dim}({\cal D}_i) \leq n = {\rm dim}(\manif)$ for all $i$.)
If the filtration is regular, we are guaranteed a finite value of $k$ such that
$\lie({\cal G})$ defines the distribution ${\cal D}_k = {\cal D}_{k+1} =
\ldots = {\cal D}_\infty$.  This distribution is the
{\bf involutive closure} $\overline{{\cal D}}$ of ${\cal D}$, and
a distribution ${\cal D}$ is {\bf involutive} if ${\cal D} = \overline{{\cal D}}$
(that is, $[g_1,g_2] \in {\cal D}$
for all $g_1,g_2 \in {\cal D}$).  

If the filtration is not regular, then in general there is no way to
know {\em a priori\/} a degree $k$ at which ${\cal D}_k =
\overline{{\cal D}}$.  If there is a degree $k$ at which all Lie
products become zero, then the Lie algebra is called {\bf nilpotent of
  order $k$}.  For example, a set of vector fields polynomial in $x$
form a nilpotent Lie algebra, as repeated derivatives with respect to $x$
will eventually be zero.

{\bf *** could define relative degree, etc.  absolute degree uses the dimension
of ${\cal D}_i$, only relevant for regular?}

%A regular distribution ${\cal D}$ is {\bf involutive} if $[g_1,g_2] \in {\cal D}$
%for all $g_1,g_2 \in {\cal D}$.  The {\bf involutive closure}
%$\overline{{\cal D}}$ of a distribution ${\cal D}$ is given by the first
%distribution in the filtration such that $[g_1,g_2] \in
%\overline{{\cal D}}$ for all $g_1,g_2 \in
%\overline{{\cal D}}$.

The {\bf integral manifold} of ${\cal D}$ containing $x_0$ 
is the set of $\manif$ which can be reached by
vector fields in ${\cal D}$,
and ${\cal D}(x)$ is the tangent
space of the integral manifold at $x$.  
By the well known Frobenius theorem, an $m$-dimensional regular distribution ${\cal D}$
can be integrated to yield an $m$-dimensional integral manifold if and only if
${\cal D}$ is involutive.  
If
the dimension of an involutive distribution is less than $n$,
then the distribution is said to generate a
{\bf foliation} of $\manif$, and each distinct integral manifold is
called a {\bf leaf} of the foliation.  Consider, for example, the
one-dimensional distribution generated by $g_2^{uni} = (0,0,
1)^T$ (turning motion) for the unicycle (Figure~\ref{fig:vecfield}).  The
distribution is one-dimensional, regular, and involutive, and the integral manifolds are
straight lines in $x_3$ (wrapping around at $2\pi$) with fixed position $(x_1,x_2)$.  The
unicycle is confined to the same leaf of the foliation for all time
if it can only follow this vector field.  

The existence of integral manifolds of dimension less than $n$
indicates that the motion constraints actually limit the reachable
state space.  For example, velocity constraints on a kinematic system
might be integrated to yield configuration constraints, indicating
that the original constraints are actually \emph{holonomic}.
Similarly, acceleration constraints on a dynamic system might be
integrated to yield velocity or even configuration constraints.  
%All 
%one-dimensional distributions (i.e., generated by a single vector field)
%are involutive and generate one-dimensional integral manifolds.  This
%is why at least two vector fields will be needed to control systems
%with $n>1$ (Section~\ref{sec:controllability}).

%\begin{theorem}
%{\bf [Frobenius]}  A regular distribution is integrable if and only if
%it is involutive.
%\end{theorem}

%*** note the filtration may not stop here; only stops if it's regular.  so involutive
%closure is only equivalent to the Lie algebra if filtration is regular.

%*** here could define Lie algebra as something with an operation
%satisfying these properties, talk about smallest algebra of smooth
%vector fields containing the vector fields...

Given a finite set of linearly independent vector fields $g_1,\ldots,g_m$, it is
not necessary to generate all possible Lie products of the
vector fields in order to generate the associated filtration.  Lie brackets
satisfy the following properties:
\begin{enumerate}
\item Skew-symmetry:
\[
[g_1,g_2] = -[g_2,g_1]
\]
\item Jacobi identity:
\[
[g_1,[g_2,g_3]] + [g_3,[g_1,g_2]] + [g_2,[g_3,g_1]] = 0.
\]
\end{enumerate}
Taking these properties into account, the {\bf Philip Hall basis} gives a
way to choose the smallest number of Lie products which must be considered
at each degree in order to generate a basis for the Lie algebra.  See the book
by Serre~\cite{Serre} for details.  

*** could give extra details on PH basis from Murray paper.

%note now relation between integrability of constraints, integral manifolds, involutivity
%of distributions, etc.

\begin{example}
{\em
{\em Unicycle (cont.)\/}  From before, we have 
$g_1^{uni} = (\cos x_3,\sin x_3,0)^T$, $g_2^{uni} = (0,0,1)^T$, and $g_3^{uni} = [g_1^{uni},g_2^{uni}] = 
(\sin x_3,-\cos x_3,0)^T$.  
The dimension of the distribution defined by $\{g_1^{uni},g_2^{uni},g_3^{uni}\}$ is
three at all $x \in \manif$, implying that the distribution is
regular.  It is also certainly involutive, since the dimension of
$\manif$ is three.  To see that the three vector fields are linearly
independent, we
define the matrix $(g_1^{uni} \; g_2^{uni} \; g_3^{uni})$ obtained
by placing the column vectors side by side.  
The rank of the matrix is 3 at
all $x \in \manif$, which can be verified by
\[
{\rm det}(g_1^{uni} \; g_2^{uni} \; g_3^{uni}) = 1.
\]
Since the distribution is regular and involutive, it has a three-dimensional
integral manifold, which is the entire space $\manif$.  
${\cal D}_2$ is the involutive closure of ${\cal D}_1$.
The filtration is regular.
}
\end{example}

\begin{example}
{\em
{\em PBWT (cont.)\/}
As derived previously, we have $g_0^{pbwt} =
(x_4,x_5,x_6,0,a_g,0)^T$, $g_1^{pbwt} = (0,0,0,\cos x_3,
\sin x_3,0)^T$, and $g_2^{pbwt} = (0,0,0,-\sin x_3,\cos x_3, -d)^T$.
Lie bracket computations show that
\begin{eqnarray}
g_3^{pbwt} & = & [g_0^{pbwt},g_1^{pbwt}] \nonumber \\
& = & (-\cos x_3, -\sin x_3, 0, -x_6 \sin x_3, x_6 \cos x_3,0)^T \nonumber \\
g_4^{pbwt} & = & [g_0^{pbwt},g_2^{pbwt}] \nonumber \\
& = & (\sin x_3, -\cos x_3, d, -x_6 \cos x_3, -x_6 \sin x_3,0)^T \nonumber \\
g_5^{pbwt} & = & [g_1^{pbwt},[g_0^{pbwt},g_2^{pbwt}]] \nonumber \\
& = & (0,0,0,d \sin x_3, -d \cos x_3,0)^T \nonumber \\
g_6^{pbwt} & = & [g_0^{pbwt},[g_1^{pbwt},[g_0^{pbwt},g_2^{pbwt}]]] \nonumber \\
& = & (-d \sin x_3, d \cos x_3,0,d x_6 \cos x_3, d x_6 \sin x_3, 0)^T. \nonumber
\end{eqnarray}
A computation shows that
\[
{\rm det}(g_1^{pbwt} \; g_2^{pbwt} \; g_3^{pbwt} \; g_4^{pbwt} \; g_5^{pbwt} \; g_6^{pbwt}) = d^4.
\]
The dimension of the distribution defined by these vector fields is six
at all $x \in \manif$ (provided $d \neq 0$),
so the distribution is both regular and involutive.  
The integral manifold is the entire
space $\manif$.  ${\cal D}_4$ is the involutive closure of ${\cal D}_1$.
%The filtration is regular.
}
\end{example}

In the next section we will apply the ideas of this section to study
controllability of underactuated systems, taking into account the fact
that {\em controls\/} determine how the system vector fields are
followed.  It may not be possible to follow arbitrary linear combinations
of system vector fields.
For example, the drift vector field $g_0$ of the PBWT is
fundamentally different than the control vector fields $g_1$ and
$g_2$.

%\begin{quote}
%{\bf {\em An analogy\/}}
%\hspace{0.2in}
%Using Lie brackets to study the locally feasible motion directions for
%a set of vector fields is analogous to using a Taylor series expansion
%to study the local shape of a curve.  Consider the curve $x = f(z) = z^3$ in
%Figure~\ref{fig:localcurves}.  We would like to know if the curve moves in
%both the $+x$ and $-x$ directions in the neighborhood of $(z_0,x_0) = 
%(0,0)$.  To answer this question, consider the Taylor expansion
%\begin{eqnarray}
%x_0 + {\cal D} x & = & f(z_0+{\cal D} z) = f(z_0) + \frac{df}{dz} \delta z + 
%\frac{1}{2!} \frac{d^2f}{dz^2} \delta z^2 + \frac{1}{3!} \frac{d^3f}{dz^3} \delta z^3 +
%\frac{1}{4!} \frac{d^4f}{dz^4} \delta z^4 + \ldots  \nonumber \\
%\delta x & = & 3 z^2|_{z=0} \delta z + 3 z|_{z=0} \delta z^2 + 1|_{z=0} \delta z^3 \nonumber + 0 \ldots \\
%\delta x & = & \delta z^3.
%\end{eqnarray}
%The linearization of this curve at $x=0$ is the same as the linearizations of 
%the curves $x = z^2$ and $x = -z^2$, which have different behaviors
%in the neighborhood
%of $x=0$.  Even by the second-order term
%$d^2f/dz^2$ of the Taylor
%expansion, the curve is confined to $x = 0$; only when we consider
%the third-order term does it become apparent that the curve moves locally
%in both the $+x$ and $-x$ directions.  If we consider $dz/dt \in [-1,1]$ to be
%a control and $x \in \real$ to be the state, this system is STLC at $x=0$.
%
%*** STLC not introduced yet
%
%Locally, Lie brackets of vector fields are analogous to the terms in the Taylor
%expansion.  Higher-degree Lie bracket motions are ``slower,'' 
%much as higher-degree terms in the Taylor expansion correspond
%to smaller change in $x$ for small $\delta z$.  In general, there is no bound
%on how many Lie brackets will need to be taken to determine if motion in
%a specific direction is possible, just as there is no bound on the order
%of the Taylor expansion terms needed to see how the curve changes locally.
%For some vector fields, e.g., polynomial vector fields, Lie bracket terms
%will vanish (the system is nilpotent) at some degree, just as Taylor expansion
%terms for polynomials vanish.
%\end{quote}
%
%\begin{figure}
%\begin{center}
%\includegraphics{../figure/book/nonholonomic/localcurves.ps}
%\end{center}
%\caption{The three curves shown are identically flat in a neighborhood of $z=0$ until
%sufficiently high derivatives of the curve are considered.}
%\label{fig:localcurves}
%\end{figure}

\section{Control Systems}
\label{sec:controlsystems}

A family of vector fields $\cal G$ on a manifold $\manif$ is sometimes
called a {\bf dynamical polysystem}.  
The system is {\bf symmetric} if for every $g \in {\cal G}$, $-g$ is
also in $\cal G$.
%If all vector fields can be
%followed both forward and backward in time, the system is {\bf
%  symmetric}.  Of course, most current robot systems are not capable
%of traveling backward in time.  Thus we say that a family of vector
%fields $\cal G$ is symmetric if, for every $g \in {\cal G}$, $-g$ is
%also in $\cal G$.

The family of dynamical polysystems we will focus on are
{\bf control affine} nonlinear control systems, written
\begin{equation}
\dot{x} = g_0(x) + \sum_{i=1}^{m} g_i(x) u_i, \;\;\; u \in {\cal U} \subset \real^m.
\label{eq:controlaffine}
\end{equation}
$g_0$ is called the {\bf drift vector field}, defining the natural
unforced motion of the system, and the $g_i, i = 1\ldots m$ are
linearly independent {\bf control vector fields}.  The control vector
$u$ belongs to the control set $\cal U$, and $u(t)$ is piecewise
continuous.  If $g_0=0$, the system is called {\bf drift-free} or {\bf
  driftless}.  Kinematic systems (such as the unicycle) may be
drift-free, but dynamic second-order systems (such as the PBWT) are
not.

We will consider two kinds of control sets:
\begin{itemize}
\item ${\cal U}_{\pm}$:  A control set ${\cal U}_{\pm}$
contains the origin of $\real^m$ in the interior of its convex hull.  
In other words, 
the control set \emph{positively} spans $\real^m$ --- any point in
$\real^m$ can be generated by a positive linear combination of elements
of ${\cal U}_{\pm}$.
An example of
such a control set is the cube $\{ u | -1 \leq u_i \leq 1, i = 1\ldots m\}$
centered at the origin of $\real^m$.  Another example consists of
only the vertices of this cube.

\item ${\cal U}_{+}$:  A control set ${\cal U}_{+}$ spans (but does not
positively span) 
$\real^m$ --- any point in $\real^m$ can be generated by a linear 
combination of elements of ${\cal U}_{+}$.
An example of such a control set are the nonnegative controls
$\{u | 0 \leq u_i \leq 1, i= 1\ldots m\}$.  
\end{itemize}

\begin{example}
{\em 
{\em Unicycle (cont.)\/}  The control system for the unicycle is written
$\dot{x} = g_1^{uni}(x) u_1 + g_2^{uni}(x) u_2$.
}
\end{example}

\begin{example}
{\em
{\em PBWT (cont.)\/}  The control system for the PBWT is written
$\dot{x} = g_0^{pbwt}(x) + g_1^{pbwt}(x) u_1 + g_2^{pbwt}(x) u_2$.
}
\end{example}

The system~\eqref{eq:controlaffine} is symmetric if (1) it is
drift-free and (2) the control set is symmetric about the origin, for
example, a cube centered at the origin.  We will abuse the term
slightly and say that a drift-free system is symmetric for any
positive-spanning control set ${\cal U}_{\pm}$, since the controllability
properties we discuss in this chapter are the same for any ${\cal U}_{\pm}$.

If a system has drift
but $g_0 \in {\rm span}(\{g_1,\ldots,g_m\})$, then we
may be able to choose controls $w(x) \in \real^m$ to always cancel
the drift, thereby symmetrizing the system by the controls.  In this
case, the pseudo-control $u \in {\cal U}_{\pm}$ can be added on top of
the drift-canceling control $w(x)$, so the total control vector is $w(x)+u$,
and the system is equivalent to
\[
\dot{x} = \sum_{i=1}^{m} g_i(x) u_i, \;\;\; u \in {\cal U}_{\pm}.
\]
As an intuitive example, imagine your motion as you walk on a
conveyor.  The drift vector field carries you at a constant speed in
one direction.  You can control your own walking speed, however, to
cancel the drift and make progress in the opposite direction.  

%The control set ${\cal U}_{\pm}$ is one example of a control set that \emph{positively}
%spans $\real^m$ --- any point in $\real^m$ can be generated by a 
%positive linear combination of elements of ${\cal U}_{\pm}$.  In fact, any
%controllability property discussed in this chapter that holds for ${\cal U}_{\pm}$
%also holds for any other (possibly discrete) control set that positively spans 
%$\real^m$.  For example, if $m=2$, the discrete control set ${\cal U} = \{(\pm 1,0)^T,(0, \pm 1)^T\}$
%positively spans $\real^m$.
%
%Similarly, the control set ${\cal U}_{+}$ is one example of a control
%set that spans $\real^m$ --- any point in $\real^m$ can be generated by a 
%linear (not necessarily positive) combination of elements of ${\cal U}_{+}$.
%Any controllability property discussed in this chapter that holds for ${\cal U}_{+}$
%also holds for any other (possibly discrete) control set that spans
%$\real^m$.  For example, if $m=2$, the discrete control set ${\cal U}= \{(1,0)^T,(0,-1)^T\}$
%spans $\real^m$.

\section{Controllability}
\label{sec:controllability}

Let $V$ be a neighborhood of a point $x \in \manif$ (i.e., an $n$-dimensional
open set of $\manif$ containing $x$).  Let $R^V(p,T)$ indicate the set of reachable
points at time $T$ by trajectories remaining inside $V$ and
satisfying Equation~\eqref{eq:controlaffine}, and
\[
R^V(p,\leq T) = \bigcup_{0<t\leq T} R^V(p,t).
\]
We define the following four versions of nonlinear
controllability (see Figure~\ref{fig:properties}):
\begin{itemize}
\item The system is {\bf controllable} from $x$ if, for any $x_1 \in \manif$,
there exists a $T>0$ such that $x_1 \in R^\manif(x,\leq T)$.

\item The system is {\bf accessible} from $x$ if $R^\manif(x,\leq T)$ contains
an open subset of $\manif$ for some $T>0$.

\item The system is {\bf small-time locally accessible} (STLA) from $x$ if
$R^V(x,\leq T)$ contains an open subset of $\manif$ for all neighborhoods $V$
and all $T>0$.

\item The system is {\bf small-time locally controllable} (STLC) from $x$ if
$R^V(x,\leq T)$ contains a neighborhood of $x$ for all neighborhoods $V$
and all $T>0$.
\end{itemize}

\begin{figure}
\begin{center}
\includegraphics{../figure/book/nonholonomic/properties.ps}
\end{center}
\caption{Reachable spaces for three systems on $\real^2$.  (a) This system is
accessible from $x$, but neither small-time locally accessible nor small-time
locally controllable.  The reachable set is two-dimensional, but not while
confined to the neighborhood $V$.  (b)  This system is small-time locally
accessible from $x$, but not small-time locally controllable.  The reachable set without
leaving $V$ does not contain a neighborhood of $x$.  (c)  This system is small-time
locally controllable from $x$.}
\label{fig:properties}
\end{figure}

The phrase ``small-time'' indicates that the property holds for any
time $T>0$, and the phrase ``locally'' indicates that the property
holds for arbitrarily small (but full-dimensional) ``wiggle room''
around the initial state.  For practical systems, it might take
finite time to switch between controls (e.g., reversing the motion of
a unicycle or car).  In this case, we might say a system is 
locally, but not small-time, controllable.  We will not worry about
such issues and retain the standard ``small-time locally''
terms.

%For our systems, the phrases ``small-time'' and ``locally'' in the last two
%definitions are redundant, but we will retain both.  For some systems, there
%may be a distinction; for example, a driftless control system which
%takes time to switch between controls (e.g., putting a car into reverse)
%might actually be {\em locally\/} controllable without being {\em small-time\/}
%controllable.  We will not worry about such distinctions here.

If a property holds for all $x \in \manif$, the phrase ``from $x$''
can be removed from the definition.  
%For linear control systems, any
%system satisfying any one of these properties also satisfies all others,
%and the system is simply called controllable.
Figure~\ref{fig:implications} shows the implications among the properties.

Small-time local controllability is of special interest.  STLC implies
that the system can locally maneuver in any direction, and if the
system is STLC at all $x \in \manif$, then the system can follow any
curve in $\manif$ arbitrarily closely.  This allows the possibility to
avoid obstacles, since any motion of a system with no
motion constraints can be approximated by a system that is STLC
everywhere.  Also, if $\manif$ is connected,
%(as with our systems where $\manif = \real^n$), 
then the system is controllable if it is STLC everywhere.

\begin{figure}
\begin{center}
\includegraphics{../figure/book/nonholonomic/implications.ps}
\end{center}
\caption{Implications among the controllability properties.  Dashed arrows
are conditional.  
%These implications hold for general connected manifolds.
}
\label{fig:implications}
\end{figure}

Some physical examples of the various properties:
\begin{itemize}
\item  Imagine setting the hands on a watch by turning a knob that can
spin in only one direction.  Convince yourself that the configuration of
the watch hands (the position of the hour and minute hands) is accessible,
controllable, and STLA, but not STLC.

\item Consider the system on $\real^2$ described by the drift vector field
$g_0 = (x_2^2,0)^T$ and the single control vector field
$g_1 = (0,1)^T$, where $u = u_1 \in [-1,1]$.
Show that the system is accessible and STLA from any $x$ but neither controllable
nor STLC.

\item Consider the system on $\real^2$ described by the drift vector
  field $g_0 = (x_2,0)^T$ and the single control vector field $g_1 =
  (0,1)^T$, where $u \in {\cal U}_{\pm}$.  This is a linear
  double-integrator.  Convince yourself that the system is STLC only
  from states of the form $(*,0)^T$ (zero-velocity states).

\item The unicycle satisfies all the controllability properties if 
${\cal U} =  {\cal U}_{\pm}$.

\item Convince yourself that the unicycle is accessible, STLA, and controllable
in the obstacle-free plane, but
not STLC, if ${\cal U} =  {\cal U}_{+}$.

\item Any system confined to a $k$-dimensional integral manifold, $k < n$,
satisfies none of the controllability properties.  

\end{itemize}

As hinted at in the linear double-integrator example,
for dynamic systems with velocity variables in the state vector,
STLC can only hold at zero velocity.  States with nonzero velocity
result in drift in the configuration variables which cannot be
instantaneously compensated by finite actuation forces. 
{\em Therefore, when we talk about STLC for dynamic systems,
we implicitly mean STLC at zero velocity.\/} 

For linear systems of the form $\dot{x} = Ax + Bu$, there is a single notion of 
controllability which can
be verified by the Kalman rank condition.  For nonlinear systems, such as
those we study, there are a number of notions of controllability, including
the four we have defined here.  A key point is that the linearizations of
systems of interest to us are generally not controllable, meaning that
their controllability is inherently a nonlinear phenomenon.

%*** Note here somewhere that the linearizations are not generally controllable.

%*** ideals and such?

\subsection{Local Accessibility and Controllability}
\label{ssec:STLA}

Of the controllability properties, STLA can be checked by studying
the Lie algebra of the vector fields $g_0,\ldots,g_m$.  The
system~\eqref{eq:controlaffine} is STLA from $x$ if (and only
if for analytic vector fields) it satisfies the
{\bf Lie algebra rank condition (LARC)} --- the Lie algebra of the vector fields,
evaluated at $x$, is the tangent space at $x$, or $\lie(\{g_0,\ldots,g_m\})(x) = T_x
\manif$.  This holds for both ${\cal U}_{\pm}$ and ${\cal U}_{+}$.
For a control set ${\cal U}_{\pm}$, then the LARC
also implies small-time local controllability.  An early version of this result
is due to W.-L. Chow, and is sometimes known as Chow's theorem.

STLA and STLC are local concepts, which can be established by looking
at the behavior of the system in a neighborhood of a state.
Accessibility and controllability, on the other hand, are global
concepts.  As a result, they may depend on things such as the topology of the
space and nonlocal behavior of the system vector fields.

%*** Chow's thm?

\begin{example}
{\em
{\em Unicycle (cont.)\/} As shown previously, the rank of the Lie
algebra is three at all states, so the LARC is satisfied.  Therefore,
for both ${\cal U} = {\cal U}_{+}$ and ${\cal U} = {\cal U}_{\pm}$, 
the unicycle is STLA.  For a control set ${\cal U}_{\pm}$, the system is also
STLC.  In
both cases, the unicycle is globally controllable.  To see this,
consider that motion between any two configurations can be decomposed
into a rotation to point the unicycle to the goal configuration,
translation to the goal position, and rotation to the goal
orientation.

If we eliminate one vector field from the unicycle example, allowing
it only to roll forward and backward ($g_1^{uni}$) or spin in place ($g_2^{uni}$), the unicycle is
confined to an integral curve of the vector field, and none of the
controllability properties is satisfied.
}
\end{example}

Many dynamic systems with nonzero drift, such as the PBWT, are not
symmetric for any control set.  The system may still be STLC at
zero velocity, however, since symmetry plus the LARC is sufficient but
not necessary for STLC.  
%To demonstrate STLC for a dynamic system, we
%need to generalize our notion of ``symmetric.''
%A control system can, in
%effect, be ``symmetrized'' by the controls if controlled vector fields
%can be used to compensate uncontrolled motions.
%As an intuitive example, imagine your motion as you walk on a
%conveyor.  The drift vector field carries you at a constant speed in
%one direction.  You can control your own walking speed, however, to
%overcome the drift and make progress in the opposite direction.
%Because of this, you are STLC in your linear position relative to the stationary
%world.
Sussmann~\cite{Sussmann87} provided a more general
sufficient condition for STLC which includes the symmetric case ($g_0
= 0$ and ${\cal U}= {\cal U}_{\pm}$) as a special case.  First, we
define a Lie product term to be a {\bf bad bracket} if the drift term
$g_0$ appears an odd number of times in the product and each control
vector field $g_i, i\geq 1$ appears an even number of times (including zero).
A {\bf good bracket} is any Lie product that is not bad.  For
example, $[g_1,[g_0,g_1]]$ is a bad bracket and
$[g_2,[g_1,[g_0,g_1]]]$ and $[g_1,[g_2,[g_1,g_2]]]$ are good brackets.
With these definitions, we can state a version of Sussmann's theorem:

\begin{theorem}
The system \eqref{eq:controlaffine} is STLC at $x$ if
\begin{enumerate}
\item $g_0(x) = 0$,
\item ${\cal U} = {\cal U}_{\pm}$,
\item the LARC is satisfied by good Lie bracket terms up to degree $k$, and
\item any bad bracket of degree $j \leq k$ can be 
expressed as a linear combination of good brackets of degree less than
$j$.
\end{enumerate}
\end{theorem}

The intuition behind the theorem is the following.  Bad brackets are
called bad because, after generating the net motion obtained by
following the Lie bracket motion prescription, we find that the
controls $u_i$ only appear in the net motion with even exponents,
meaning that the vector field can only be followed in one direction.
%For example, the control $u_1$ appears as $u_1^2$ after generating
%the bad bracket motion $[g_1,[g_0,g_1]]$.  Because of this, forward motion in
%this bracket direction is possible, but backward motion is not.  
In this sense,
a bad bracket is similar to a drift field, and we must be able to compensate
for it.  Since motions in Lie product directions of high degree are essentially
``slower'' than those in directions with a lower degree, we should only try to
compensate for bad bracket motions by good bracket motions of lower degree.
If a bad bracket of degree $j$ can be expressed as a linear combination of good brackets
of degree less than $j$, the good brackets are said to {\bf neutralize\/} the
bad bracket.  Of course, the only way to neutralize the bad bracket of degree 1,
the drift vector field, is to require $g_0(x) = 0$.  Therefore, this result
only holds at states $x$ where the drift vanishes, i.e., equilibrium states.  
%It is clear,
%however, that as in the example of walking on a conveyor belt, we could further
%generalize the result to allow nonzero drift fields to be neutralized by
%control vector fields, if the drift field is included in the linear
%span of the control vector fields.  This depends on the magnitudes of
%the controls available relative to the drift, and we will not consider
%such cases.

\begin{example}
{\em
{\em PBWT (cont.)\/}  Assume that the PBWT moves in a horizontal plane, so
$a_g=0$.  As before, we define $g_3^{pbwt} = [g_0^{pbwt},g_1^{pbwt}]$, $g_4^{pbwt}=[g_0^{pbwt},g_2^{pbwt}]$, $g_5^{pbwt} = [g_1^{pbwt},[g_0^{pbwt},g_2^{pbwt}]]$, and
$g_6^{pbwt}=[g_0^{pbwt},[g_1^{pbwt},[g_0^{pbwt},g_2^{pbwt}]]]$.  
Again as before, a computation shows that
\[
{\rm det}(g_1^{pbwt} \; g_2^{pbwt} \; g_3^{pbwt} \; g_4^{pbwt} \; g_5^{pbwt} \; g_6^{pbwt}) = d^4.
\]
The LARC is satisfied, so the system is STLA at all states for either control set.  If
${\cal U} = {\cal U}_{\pm}$, we would like to know if the system satisfies 
Sussmann's sufficient
condition for STLC at equilibrium states $x = (q_1,q_2,q_3,0,0,0)^T$, where
$g_0^{pbwt}(x) = 0$.  Because we use bracket terms up to degree 4 to demonstrate 
LARC, we must be able to
neutralize all bad bracket terms of degree 4 or less.  The only such bad bracket
terms are the degree 3 terms
\[
[g_1^{pbwt},[g_0^{pbwt},g_1^{pbwt}]] = (0,0,0,0,0,0)^T 
\]
\[
[g_2^{pbwt},[g_0^{pbwt},g_2^{pbwt}]] = (0,0,0,2 d \cos x_3, 2 d \sin x_3,0)^T. 
\]
The second term is neutralized by $g_1^{pbwt}$.  Therefore, by Sussmann's theorem,
the system is STLC at equilibrium states.

Note that in gravity, $a_g \neq 0$ so $g_0^{pbwt}(x) \neq 0$ at any state, and Sussmann's
theorem does not allow us to prove or disprove STLC.

Finally, consider the case where the PBWT is equipped with a single
thruster.  If the single thruster corresponds to the vector field $g_1^{pbwt}$, the
thrust always passes through the body center of mass, and the angular velocity
of the body cannot be changed.  The system is not accessible.  If the single
thruster corresponds to the vector field $g_2^{pbwt}$, however, we can define the
vector fields $[g_0^{pbwt},g_2^{pbwt}]$, $[g_2^{pbwt},[g_0^{pbwt},g_2^{pbwt}]]$, 
$[g_2^{pbwt},[g_0^{pbwt},[g_0^{pbwt},g_2^{pbwt}]]]$, $[g_2^{pbwt},[g_2^{pbwt},[g_0^{pbwt},[g_0^{pbwt},g_2^{pbwt}]]]]$,  and $[g_0^{pbwt},[g_2^{pbwt},[g_2^{pbwt},[g_0^{pbwt},[g_0^{pbwt},g_2^{pbwt}]]]]]$ and see that the determinant of the
matrix formed by these vectors is $-16 d^8$,
%\[
%{\rm det}(g_2 \; g_3 \; g_4 \; g_5 \; g_6 \; g_7) = -16 d^8,
%\]
indicating that the system is STLA for either control set.  Bad brackets
cannot be neutralized, so Sussmann's theorem cannot be used to show STLC.
Note, however, that reducing to a single control vector field does not
reduce the dimension of the reachable space, as it did for the kinematic
unicycle case.  This is because the dynamic system provides a drift field
with which Lie bracket terms can be generated.

Finally, the PBWT with the single control vector field $g_2^{pbwt}$, $u_2 \in [-1,1]$, 
and $a_g=0$ turns 
out to be (globally) controllable --- any state is reachable in finite
time from any other state~\cite{Lynch99d}.  The proof is beyond the scope
of this chapter.
}
\end{example}

\begin{example}
{\em
{\bf {\em A loose analogy\/}}
%\hspace{0.1in}
Using Lie brackets to study the locally feasible motion directions for
a set of vector fields is not unlike using a Taylor series expansion
to study the local shape of a curve.  Consider the curve $x = f(z) = z^3$ in
Figure~\ref{fig:localcurves}.  We would like to know if the curve moves in
both the $+x$ and $-x$ directions in the neighborhood of $(z_0,x_0) = 
(0,0)$.  To answer this question, consider the Taylor expansion
\begin{eqnarray}
x_0 + \delta x & = & f(z_0+\delta z) = f(z_0) + \frac{df}{dz} \delta z + 
\frac{1}{2!} \frac{d^2f}{dz^2} \delta z^2 + \frac{1}{3!} \frac{d^3f}{dz^3} \delta z^3 +
\frac{1}{4!} \frac{d^4f}{dz^4} \delta z^4 + \ldots  \nonumber \\
\delta x & = & 3 z^2|_{z=0} \; \delta z + 3 z|_{z=0} \; \delta z^2 + 1|_{z=0} \; \delta z^3 \nonumber + 0 \ldots \\
\delta x & = & \delta z^3.
\end{eqnarray}
The linearization of this curve at $x=0$ is the same as the linearizations of 
the curves $x = z^2$ and $x = -z^2$, which have different behaviors
in the neighborhood
of $x=0$.  Even by the second-order term
$d^2f/dz^2$ of the Taylor
expansion, the curve is confined to $x = 0$; only when we consider
the third-order term does it become apparent that the curve moves locally
in both the $+x$ and $-x$ directions.  If we consider $dz/dt \in [-1,1]$ to be
a control and $x \in \real$ to be the state, this system is STLC at $x=0$.

Locally, Lie brackets of vector fields are analogous to the terms in the Taylor
expansion.  Higher-degree Lie bracket motions are ``slower,'' 
much as higher-degree terms in the Taylor expansion correspond
to smaller change in $x$ for small $\delta z$.  In general, there is no bound
on how many Lie brackets will need to be taken to determine if motion in
a specific direction is possible, just as there is no bound on the order
of the Taylor expansion terms needed to see how the curve changes locally.
For some vector fields, Lie bracket terms
will vanish (the system is nilpotent) at some degree, just as Taylor expansion
terms for polynomials vanish.
}
\end{example}

\begin{figure}
\begin{center}
\includegraphics{../figure/book/nonholonomic/localcurves.ps}
\end{center}
\caption{The three curves shown are identically flat in a neighborhood of $z=0$ until
sufficiently high derivatives of the curve are considered.}
\label{fig:localcurves}
\end{figure}

\subsection{Global Controllability}

For kinematic systems which are STLC everywhere on a connected 
manifold, (global) controllability follows easily.  In general, however,
controllability is not easy to decide, as it may depend on nonlocal
features of the control system.  In the special case of a control
system~\eqref{eq:controlaffine} with ${\cal U} = {\cal U}_{\pm}$ and
whose drift vector field repeatedly returns the system to a
neighborhood of its initial state, however, demonstrating controllability
is as easy as demonstrating the LARC.

First, some definitions.  Consider the flow $\phi^{g_0}$ of the drift
vector field.  A point $x \in \manif$ is called {\bf positively
Poisson stable} (PPS) for $g_0$ if for all $T>0$ and any neighborhood
$V$ of $x$, there exists a time $t>T$ such that $\phi^{g_0}_t(x) \in
V$.  The drift vector field $g_0$ is called positively Poisson stable
if the set of PPS points for $g_0$ is dense in $\manif$.

A point $x \in \manif$ is called a {\bf nonwandering point} of $g_0$ if for all time
$T>0$ and any neighborhood $V$ of $x$ there exists a time $t>T$
such that $\phi^{g_0}_t(V) \cap V \neq \emptyset$, where $\phi^{g_0}_t(V) =
\{\phi^{g_0}_t(x) | x \in V\}$.  (A positively Poisson stable point is
necessarily a nonwandering point.)  The {\bf nonwandering set} of $g_0$ is the
set of all nonwandering points of $g_0$.

\begin{definition}
The drift vector field $g_0$ is {\bf weakly positively Poisson stable} (WPPS) if
its nonwandering set is $\manif$.
\label{def:wpps}
\end{definition}

We now state the main theorem, taken from
Lian~{\em et al.\/}~\cite{Lian94}.  Related results can be
found in (Jurdjevic and Sussmann~\cite{Jurdjevic72b}; Lobry~\cite{Lobry74};
Brockett~\cite{Brockett76}; Bonnard~\cite{Bonnard81};
Jurdjevic~\cite{Jurdjevic97}).

\begin{theorem}
Assume that the drift vector field $g_0$ is WPPS.
Then the system~(\ref{eq:controlaffine}) with ${\cal U} = {\cal U}_{\pm}$
is controllable on
$\manif$ if the Lie algebra rank condition (LARC) is satisfied.
\label{thm:Lian}
\end{theorem}

This theorem is a powerful tool for establishing the global
controllability of systems with drift.  Systems with periodic natural
unforced dynamics (such as an undamped planar pendulum) or
phase-volume conserving drift on compact configuration spaces are
examples of systems with WPPS drift vector fields.  (The latter follows
from an application of Poincar\'{e}'s recurrence theorem; see, for
example, the discussion by Arnold~\cite{Arnold}.)  As an example, a
rotating satellite moves on the compact configuration space $SO(3)$,
and its natural unforced motion is energy conserving.  Therefore, the
drift is WPPS.  The LARC can be
satisfied by a single body-fixed control torque, meaning that the
satellite orientation can be controlled (in a global sense) with a single control vector
field.

For systems without WPPS
drift, it may be possible to construct feedback laws which always keep
the system in a periodic orbit.  If the system is always controllable
about these periodic trajectories, that is, if the system can reach 
neighborhoods of the controlled periodic trajectories, then similar
reasoning can be used to demonstrate controllability of the system.

%*** maybe fully develop PBWT example, with linearly controllable with 3
%thrusters, locally controllable with 2, glob with 1

\section{Simple Mechanical Control Systems}
\label{sec:mechanicalsystems}

This section discusses dynamic mechanical control systems in greater
depth, leading to simplified controllability tests and ideas that lead
to reduced-complexity motion planning.  We introduce the minimal set
of ideas from Riemannian geometry that allows us to do this.  We do
not attempt to be rigorous or thorough in our treatment of mechanical
systems from a geometric viewpoint.  Instead, we hope this gentle
introduction, and the applications of these ideas to controllability
and motion planning, motivates the reader to further study (see, for
example, the
excellent books by Abraham and Marsden~\cite{Abraham67}, Marsden and
Ratiu~\cite{Marsden94}, and Boothby~\cite{Boothby}).  This section may
be safely skipped without affecting reading of the rest of the
chapter.

In the previous chapter we derived
equations of motion of the form
\begin{equation}
M(q)\ddot{q} + C(q,\dot{q})\dot{q} + g(q) = T(q) f,
\label{eq:standardform}
\end{equation}
where $f$ is a generalized force vector, $M(q)$ is the inertia
matrix, $C(q,\dot{q})\dot{q}$ are the Coriolis and centrifugal
forces, and $g(q)$ are potential terms.  
We will restrict
our discussion in this section to {\bf simple
mechanical control systems} of this form with
$g(q) = 0$.\footnote{Alternatively, we can think of $-g(q)$ being
added to the to the control force $f$.}

Since we are considering underactuated systems, we can assume the last
$n_{\Cs}-m$ elements of $f$ are zero, that is,
$f = (u^T,0_{n_{\Cs}-m}^T)^T$, where $u \in \real^m$ is the
control vector and $0_{n_{\Cs}-m}$ is an $(n_{\Cs}-m)$-vector
of zeros.
In this case,
$T(q)$ is an $n_{\Cs}\times m$ matrix, and the equations can be written
\begin{equation}
M(q)\ddot{q} + C(q,\dot{q})\dot{q} = T(q) u.
\label{eq:originallagrange}
\end{equation}

We can write the columns of $T(q)$ as $T_i(q)$, so $T(q) = (T_1(q) \;
\ldots \; T_m(q))$.  Now we can define a full rank $n_{\Cs} \times
n_{\Cs}$ matrix $T_{a}(q)$ by appending $n_{\Cs}-m$ linearly
independent columns to $T(q)$,
\[
T_{a}(q) = (T(q) \; T^a_{m+1}(q) \; \ldots \; T^a_{n_{\Cs}}(q)),
\]
where each $T^a_j(q), j = m+1,\ldots,n_{\Cs}$ is orthogonal to $T(q)$
by the inertia matrix $M(q)$; that is, $T_i^T(q) M^{-1}(q) T^a_j(q) = 0$
for all $i=1,\ldots, m$ and $j=m+1,\ldots,n_{\Cs}$.  Then we can
rewrite \eqref{eq:originallagrange} as
\[
M(q)\ddot{q} + C(q,\dot{q})\dot{q} = T_a(q) \left(\begin{array}{c}
u \\ 0_{n_{\Cs}-m} \end{array}\right)
\]
or
\begin{equation}
T^{-1}_{a}(q)(M(q)\ddot{q} + C(q,\dot{q})\dot{q}) = \left(\begin{array}{c}
u \\ 0_{n_{\Cs}-m} \end{array}\right).
\label{eq:dynamicconstraints}
\end{equation}
The last $n_{\Cs}-m$ rows of \eqref{eq:dynamicconstraints} define the
dynamic (second-order) constraints on the feasible motions of the system.
% and the last
%$k$ rows of \eqref{eq:originallagrange} correspond to
%the $k$ dynamic constraints, which can be written
%\begin{equation}
%M_0(q)\ddot{q} + C_0(q,\dot{q}) = T_0(q) \left(\begin{array}{c}u \\ 0_k \end{array}\right),
%\label{eq:dynamicconstraints}
%\end{equation}
%where $M_0$, $C_0$, and $T_0$ are the last $k$ rows of $M$, $C$, and $T$,
%respectively.  

Beginning again with \eqref{eq:originallagrange},
premultiplying both sides by $M^{-1}(q)$ (assuming
full rank), and rearranging, we get 
%\[
%\ddot{q} + M^{-1}(q)C(q,\dot{q})\dot{q} = M^{-1}(q) T(q) u 
%\]
%or
\begin{equation}
\ddot{q} = -M^{-1}(q) C(q,\dot{q})\dot{q} + M^{-1}(q) T(q) u. 
\label{eq:beforeY}
\end{equation}
Writing the $m$ columns of $M^{-1}(q) T(q)$ as $Y_i(q), i = 1\ldots m$, we get
\begin{equation}
\ddot{q} = - M^{-1}(q)C(q,\dot{q})\dot{q} + \sum_{i=1}^m Y_i(q) u_i.
\label{eq:lagrange}
\end{equation}
In other words, $\ddot{q}$ is the sum
of a drift term due to Coriolis and centrifugal effects 
and a term due to the controls.  According to the
development so far, the control system can be expressed in the form \eqref{eq:controlaffine}
by writing the state, drift vector field, and control vector fields as
\[
x = \left(\begin{array}{c} q\\ \dot{q}\end{array} \right) , \;\;
g_0(x) = \left(\begin{array}{c} \dot{q} \\ -M^{-1}(q)C(q,\dot{q})\dot{q} \end{array}\right) ,\;\;
g_i(x) = \left(\begin{array}{c} 0_{n_{\Cs}} \\ Y_i(q) \end{array}\right)
\]
and expressing the control system as
\begin{equation}
\dot{x} = g_0(x) + \sum_{i=1}^m g_i(x) u_i.
\label{eq:secondorderform}
\end{equation}
We can then apply controllability tests as previously described.

\subsection{Simplified Controllability Tests}

The approach described above, while correct, ignores some structure of 
the Lie brackets of a simple mechanical control system.  Lewis and Murray~\cite{Lewis97,Lewis99b}
have investigated simplified controllability tests for simple mechanical
systems starting from rest.  These tests take advantage of
the structure of the Lie brackets to reduce the number of computations.
In the rest of this section, we will assume the control set is
${\cal U} = {\cal U}_{\pm}$.

%In this section we give just a brief introduction to the approach; see
%\cite{Lewis97,Lewis99b} for details.  
First a few ideas from Riemannian geometry.
Given two vector fields $Y_1(q)$ and $Y_2(q)$ on
${\Cs}$, the {\bf covariant derivative} 
%(associated with the inertia matrix $M(q)$) 
of $Y_2(q)$ with respect to $Y_1(q)$ can be defined as the way
$Y_2(q)$ is changing along the direction of $Y_1(q)$.  Consider the
familiar case of a Euclidean configuration space with standard coordinates and the
Euclidean metric, which says that the shortest distance between two
points is a straight line.  In this familiar case, the covariant
derivative of $Y_2(q)$ with respect to $Y_1(q)$ is simply 
$(\partial Y_2/\partial q) Y_1$, that is, the gradient of $Y_2$ projected
to the direction $Y_1$.  The covariant derivative is a vector field.

The Euclidean metric defines an inner product mapping two tangent
vectors (velocities) $v_1, v_2 \in T_q{\Cs}$ at the same 
configuration $q$ to a
real value.  We write this inner product $\inprod{v_1}{v_2}$, which is
$v_1^T v_2$ for the Euclidean metric.  By considering all trajectories
connecting two points $q_1$ and $q_2$ in the Euclidean space, we see
that the integral of this inner product along the trajectories is
minimized by straight line motions.  These straight line motions
minimizing the integral are called ``geodesics'' for the Euclidean
metric.  The Euclidean metric also allows us to define the usual
notion of ``parallel'' tangent vectors at different points of ${\Cs}$.

More generally, the metric defining parallel velocities at different
configurations is not so clear.  For example, for a 2R robot arm, are
two velocities parallel if the velocities of the end-effector are
parallel in Cartesian space?  Or perhaps if the joint velocities are
equivalent?

For the dynamic systems we are interested in, the inner product,
geodesics, and the notion of parallelism are defined by the metric
implicit in the inertia matrix $M(q)$.  This is sometimes called the
kinetic energy metric, since it derives from the kinetic energy
$\frac{1}{2}\dot{q}^TM(q)\dot{q}$.  The inner product defined by this
metric is
\[
\inprod{v_1}{v_2} = v_1^T M(q) v_2, \;\; v_1,v_2 \in T_q {\Cs},
\]
and two vector fields $Y_1$ and $Y_2$ are orthogonal by the inertia
matrix if $\inprod{Y_1}{Y_2} = 0$ holds for all $q$.
This metric is an example of a {\bf Riemannian metric}, as it is
symmetric, positive definite (because the inertia matrix is positive
definite), and bilinear (linear in each of $v_1$ and $v_2$).  This
metric is said to define an {\bf affine connection} $\nabla$ defining the inner
product, geodesics, and parallelism.  The affine connection associated
with $M(q)$ is sometimes called the {\bf Levi-Civita} connection.
When $M(q)$ is the identity matrix, the Euclidean metric is obtained.

Now for the affine connection $\nabla$ associated with $M(q)$, the 
covariant derivative 
of $Y_2(q)$ with respect to $Y_1(q)$ is 
the vector field $\nabla_{Y_1(q)} Y_2(q)$:
\[
\nabla_{Y_1(q)} Y_2(q) = \frac{\partial Y_2(q)}{\partial q} Y_1(q) + M^{-1}(q) Y_1^T(q) \Gamma(q) Y_2(q),
\]
where $\Gamma(q)$ is the set of $n_{\Cs} \times n_{\Cs} \times n_{\Cs}$ Christoffel symbols
of the inertia matrix $M(q)$, as described in the previous 
chapter.\footnote{Many works define a slightly different set of
Christoffel symbols $\Gamma^*$ which have the inverse of the inertia matrix
embedded in them, such that $Y_1^T \Gamma^* Y_2 = 
M^{-1} Y_1^T \Gamma Y_2$.}
(The computation of 
$Y_1^T(q) \Gamma(q) Y_2(q)$ is also
described in the previous chapter.)  The difference from
the covariant derivative in the Euclidean metric is the presence of
the Christoffel symbol terms.  These Christoffel terms carry geometric
information on how the geodesics of the dynamic system ``bend'' in 
this choice of coordinates.  For the Euclidean metric, the geodesics
are straight lines because the Christoffel symbols
are zero.

The covariant derivative allows us to write the equations of
motion \eqref{eq:lagrange} in the following equivalent form:
\begin{eqnarray}
\nabla_{\dot{q}} \dot{q} & = & \frac{\partial \dot{q}}{\partial q} \dot{q} +
M^{-1}(q)\dot{q}^T \Gamma(q) \dot{q} \nonumber \\
\nabla_{\dot{q}} \dot{q} & = & \ddot{q} + M^{-1}C(q,\dot{q})\dot{q} \nonumber \\
\nabla_{\dot{q}} \dot{q} & = & \sum_{i=1}^m Y_i(q) u_i,
\label{eq:covariantsystem}
\end{eqnarray}
where $\dot{q}$ is written 
$\partial/\partial q_1 + \ldots \partial/\partial q_{n_{\Cs}}$ or
$(1,1,\ldots,1)^T$.  Motions $q(t)$ satisfying
\[
\nabla_{\dot{q}(t)} \dot{q}(t) = 0_{n_{\Cs}}
\]
are geodesics of $\nabla$.
Equation~\eqref{eq:covariantsystem} is the familiar 
$a = M^{-1} f$ --- acceleration is equal to the inverse of the mass times
the force.  Here, however, the acceleration $a$ is not just the
second derivative of the coordinates $\ddot{q}$, but 
$\ddot{q} + M^{-1}\dot{q}^T \Gamma \dot{q}$.  We have $a = \ddot{q}$ only
if the Christoffel symbols are zero, which is the case if the inertia
matrix has no dependence on (is invariant to) the configuration $q$.
An example is a point mass moving in a Euclidean space with standard
coordinates.  If we choose a different coordinate system, such
as a polar coordinate system, the Christoffel symbols are
not zero.

The covariant derivative also allows us to define
the {\bf symmetric product} of $Y_1$ and $Y_2$, the
vector field
\[
\symprod{Y_1}{Y_2} = \nabla_{Y_1} Y_2 + \nabla_{Y_2} Y_1,
\]
which is useful in controllability calculations.  But why should
this be so?
After all, we have already seen in \eqref{eq:secondorderform}
that a $Y_i(q)$ from \eqref{eq:lagrange} can be turned into a control vector field $g_i(x)$ on the full state
space $\manif = T{\Cs}$, and that the drift can be expressed as $g_0(x)$, allowing us 
to take Lie brackets as before to test controllability.  If we assume the
system begins from rest, however, we notice that symmetric products of the
$Y_i(q)$ appear again and again in the bracket terms.  In particular, we can identify
the following patterns in the calculations:
\begin{eqnarray}
[g_i(x),g_j(x)] & = & 0, \; i,j = 1 \ldots m \nonumber \\
 \; [g_0(x),g_i(x)] & = & \left(\begin{array}{c} -Y_i(q) \\ 0_{n_{\Cs}} \end{array} \right) , \; i = 1\ldots m \nonumber \\
\; [g_i(x),[g_0(x),g_j(x)]] & = & \left(\begin{array}{c} 0_{n_{\Cs}} \\ \symprod{Y_i(q)}{Y_j(q)}
 \end{array} \right), \; i = 1 \ldots m. \nonumber
\end{eqnarray}
The first shows that drift must be included in a Lie bracket
to create new motion directions.  The second shows that Lie bracketing
the drift with a control vector field has the effect of taking the
``acceleration'' direction of the control vector field to a velocity direction.
The last shows that iterated Lie brackets including the drift field
can be evaluated by calculating lower-degree symmetric products of 
the $Y_i(q)$.  
An added benefit is that the symmetric product operates on vector fields with half
as many elements.
%, further reducing computations. 

The key point is, for
controllability computations for simple mechanical control systems 
beginning from rest, the symmetric product
allows us to think of $Y_i(q)$ \emph{as a control
vector field on the system configuration space ${\Cs}$, without constructing
a higher-dimensional vector field $g_i(q)$ on the full
state manifold $\manif = T{\Cs}$\/}.  The symmetric product
captures the effect of drift in the controllability computations.  

For a control system \eqref{eq:covariantsystem} consisting of a
set of control vector fields ${\cal Y} = \{Y_1, \ldots, Y_m\}$, let the
{\bf symmetric closure} $\symclos({\cal Y})$ be the distribution
defined by ${\cal Y}$ and the iterated symmetric products of these
vector fields.  In this sense, the symmetric closure by the
symmetric product is defined
similarly to the involutive closure by the Lie bracket.  Also, we can
define the degree of a symmetric product to be the number of the
original vector fields $Y_i$ appearing in the expression.  A symmetric
product is {\bf bad} if each of the vector fields appears an even
number of times, and is {\bf good} otherwise.  With these definitions,
Lewis and Murray~\cite{Lewis97,Lewis99b} proved the following theorem,
building on Sussmann's theorem from section~\ref{ssec:STLA}.

\begin{theorem}
\label{thm:symstlc}
Beginning from an equilibrium state $x = (q^T,0_{n_{\Cs}}^T)^T$, the 
system~\eqref{eq:covariantsystem} is
\enumerate
\item small-time locally accessible from $x$ if and only if $\symclos({\cal Y})(q) =
T_q{\Cs}$, and
\item small-time locally controllable from $x$ if $\symclos({\cal Y})(q) = T_q{\Cs}$
and every bad symmetric product can be expressed as a linear combination of
good symmetric products of lower degree.
\end{theorem}

Beginning from an equilibrium state $x = (q^T,0_{n_{\Cs}}^T)^T$, it is
sometimes of interest to understand the locally reachable set of
configurations {\em irrespective\/} of the velocities at those
configurations.  Lewis and Murray define a system to be {\bf small-time locally
configuration accessible\/} (STLCA) from $q$ if the locally reachable set
is open in ${\Cs}$, and {\bf small-time locally configuration
controllable} (STLCC) from $q$ if the locally reachable set on ${\Cs}$
contains $q$ in the interior.  These are somewhat weaker controllability
properties than small-time local accessibility and small-time local
controllability, since they say nothing about the reachable velocities.
Finally, the system is {\bf equilibrium controllable} if the system
can reach any equilibrium state from any other equilibrium state.
%Finally, the system is {\bf small-time locally equilibrium controllable}
%(STLEC) from $q$ if the locally reachable set \emph{at zero velocity} 
%contains $q$ in the interior.  This is stronger than STLCC, but still weaker
%than STLC, which requires the locally reachable set to be open on $T{\Cs}$. 
These definitions are particularly relevant for systems subject to
velocity constraints, which may be configuration controllable
despite being confined to a reachable set of dimension less than $n$.

The following theorem is due to Lewis and Murray~\cite{Lewis97,Lewis99b}.

\begin{theorem}
\label{thm:symstlcc}
Beginning from an equilibrium state $x = (q^T,0_{n_{\Cs}}^T)^T$, the 
system~\eqref{eq:covariantsystem} is
\enumerate
\item small-time locally configuration accessible from $q$ if and only if 
$\lie(\symclos({\cal Y}))(q) =
T_q{\Cs}$, and
\item small-time locally configuration controllable from $q$ 
if $\lie(\symclos({\cal Y}))(q) = T_q{\Cs}$
and if every bad symmetric product can be expressed as a linear combination of
good symmetric products of lower degree.  If these conditions
are satisfied at all $q \in {\Cs}$,
then the system is equilibrium controllable.
\end{theorem}

%For simple mechanical control systems with a single control vector field,
%i.e., ${\cal Y} = \{Y_1\}$, we have the following result from Lewis:
%\begin{theorem}
%If the system~\eqref{eq:covariantsystem} has a single control vector field,
%${\cal Y} = \{Y_1\}$ and ${\rm dim}({\Cs}) > 1$, then it is not
%STLCC (therefore not STLC) from any zero-velocity state $(q^T,0^T)^T$.
%\label{thm:oneinput}
%\end{theorem}
%Note Theorem~\ref{thm:oneinput} applies only to systems without potential.

Roughly speaking, $\symclos({\cal Y})(q)$ corresponds to the reachable velocity directions
from $(q^T,0_{n_{\Cs}}^T)^T$, and
$\lie(\symclos({\cal Y}))(q)$ corresponds to the reachable
configuration directions from $(q^T,0_{n_{\Cs}}^T)^T$.

\begin{example}
{\em
{\em PBWT (cont.)}
The dynamics of the PBWT can be written
\[
\ddot{q} = -M^{-1}(q) C(q,\dot{q})\dot{q} + M^{-1}(q) T(q) u,
\]
where $M^{-1}(q)$ is the $3 \times 3$ identity matrix, $C(q,\dot{q})\dot{q} = 0_{n_{\Cs}}$ (the Christoffel
symbols are all zero), $u = (u_1,u_2)^T$, and
\[
T(q) = \left( \begin{array}{cc}
\cos q_3 & -\sin q_3 \\
\sin q_3 & \cos q_3 \\
0 & -d \end{array} \right).
\]
${\cal Y} = \{Y_1(q),Y_2(q)\}$ consists
of the two columns of $M^{-1}(q) T(q) = T(q)$.  Calculating the symmetric product of
$Y_1(q)$ and $Y_2(q)$, we get
\[
\symprod{Y_1}{Y_2} = \nabla_{Y_1} Y_2 + \nabla_{Y_2} Y_1 = \left(\begin{array}{c}
d \sin q_3 \\
-d \cos q_3 \\
0 \end{array} \right).
\]
We see that ${\rm rank}(Y_1 \;\; Y_2 \;\; \symprod{Y_1}{Y_2}) = 3$ for all $q$, so $\symclos({\cal Y}) = T_q {\Cs}$ 
for all $q$,
and the system is small-time locally accessible by Theorem~\ref{thm:symstlc}.  The bad products are
$\symprod{Y_1}{Y_1} = 0$ and $\symprod{Y_2}{Y_2} = (d \cos q_3, d \sin q_3,0)^T$, which is 
neutralized by $Y_1$, so the system is also
small-time locally controllable by Theorem~\ref{thm:symstlc}.  This confirms our previous
result.

You may also wish to verify that
${\rm rank}(Y_2 \;\; \symprod{Y_2}{Y_2} \;\; \symprod{Y_2}{\symprod{Y_2}{Y_2}}) = 3$, so the PBWT
is STLA with the single thruster $u_2$ by Theorem~\ref{thm:symstlc}.  This also confirms our
previous result.
}
\end{example}

%\begin{example}
%{\em
%In the previous chapter, we derived the dynamics of an RP robot arm, shown
%in Figure~\ref{fig:RParm}.  Here we will consider the same arm in zero gravity
%and an actuator only at the first joint.
%
%The dynamics of the arm can be written
%\begin{eqnarray}
%M(q) \ddot{q} + C(q,\dot{q})\dot{q} & = & T(q) f \nonumber \\
%\left(\begin{array}{cc}
%I_1 + I_2 + m_1 r_1^2 + m_2 q_2^2 & 0 \\
%0 & m_2
%\end{array}
%\right) \ddot{q} + \left(\begin{array}{c}
%2 m_2 q_2 \dot{q}_2 \dot{q}_2 \\
%- m_2 q_2 \dot{q}_1^2
%\end{array} \right) & = & \left( \begin{array}{cc} 1 & 0 \\ 0 & 1 \end{array} \right)
%\left(\begin{array}{c} u_1 \\ 0 \end{array}\right). \nonumber
%\end{eqnarray}
%The control vector field is $Y_1(q) = (1/(I_1 + I_2 + m_1 r_1^2 + m_2 q_2^2),0)^T$, the 
%first column of $M^{-1}(q)T(q)$.  We calculate
%\begin{eqnarray}
%\nabla_{Y_1} Y_1 & = & \frac{\partial Y_1}{\partial q} Y_1 + M^{-1} Y_1^T \Gamma Y_1 \nonumber \\
%& = & \left(\begin{array}{cc} 0 & \frac{-2 m_2 q_2}{(I_1+I_2+m_1 r_1^2+m_2 q_2^2)^2} \\
%0 & 0 \end{array} \right)
%\left(\begin{array}{c}
%\frac{1}{I_1+I_2+m_1 r_1^2+m_2 q_2^2} \\ 0 \end{array}\right) + \nonumber \\
%& & \left(\begin{array}{cc}
%\frac{1}{I_1+I_2+m_1 r_1^2 + m_2 q_2^2} & 0 \\
%0 & \frac{1}{m_2}
%\end{array} \right)
%\left(\begin{array}{c}
%0 \\ \frac{-m_2 q_2}{(I_1 + I_2 + m_1 r_1^2 + m_2 q_2^2)^2}
%\end{array} \right) \nonumber \\
%& = & \left(\begin{array}{c} 0 \\ \frac{-q_2}{(I_1+I_2+m_1 r_1^2+m_2 q_2^2)^2} \end{array} \right).
%\nonumber
%\end{eqnarray}
%Noting that $\symprod{Y_1}{Y_1} = 2 \nabla_{Y_1} Y_1$, it is easy to see that
%${\rm rank}(Y_1 \; \symprod{Y_1}{Y_1}) = 2$ at all $q$ such that $q_2 \neq 0$, demonstrating
%by Theorem~\ref{thm:symstlc} that the RP arm is STLA with a single actuator
%at all $q$ away from $q_2 = 0$.  
%}
%\end{example}
%
%\begin{figure}
%\begin{center}
%\includegraphics{../figure/book/nonholonomic/RParm.ps}
%\end{center}
%\caption{The RP robot arm.}
%\label{fig:RParm}
%\end{figure}

\begin{example}
{\em
{\em Hopper example.\/}
Consider a simple model of a planar hopping single-leg robot in flight,
ignoring the translational motion of the body.
We will model the body of the hopper as a rigid body pinned to
a wall by a revolute joint at its center of mass.  The inertia
of the body is $I$ about its center of mass.  An extensible massless
leg is attached to the center of mass of the body, and the foot of
the leg is a point mass $m$.  The configuration of the system is
$q = (q_1,q_2,q_3)^T$, where $q_1$ is the angle of the body relative
to an inertial frame, $q_2$ is the angle of the leg relative to the
inertial frame, and $q_3 > 0$ is the extension of the leg (Figure~\ref{fig:robotleg}).
The leg has two actuators, one providing a torque to control the orientation
of the leg relative to the body, and the other providing a force
to control the extension
of the leg.  We would like to know if it possible to control the configuration
$q$ using only these two actuators.  

\begin{figure}
\begin{center}
\includegraphics{../figure/book/nonholonomic/robotleg.ps}
\end{center}
\caption{A simple single-leg hopping robot.}
\label{fig:robotleg}
\end{figure}

This system is subject to a velocity constraint:  the 
total angular momentum of the system is zero throughout the motion.  
This constraint implies
that the dimension of the reachable state space can be no more than five,
while $n = 2 n_{\Cs} = 6$.
This rules out the possibility of STLA and STLC, so instead we focus on 
the reachable configurations.

The system mass matrix and its inverse are
\[
M(q) = \left( \begin{array}{ccc}
I & 0 & 0 \\
0 & m q_3^2 & 0 \\
0 & 0 & m
\end{array} \right), \;\;
M^{-1}(q) = \left( \begin{array}{ccc}
\frac{1}{I} & 0 & 0 \\
0 & \frac{1}{mq_3^2} & 0 \\
0 & 0 & \frac{1}{m}
\end{array} \right),
\]
and the only non-zero Christoffel symbols are $\Gamma^2_{23} = \Gamma^2_{32} = mq_3$ and
$\Gamma^3_{22} = -mq_3$.  The matrix $T(q)$ describing the generalized
forces from the actuators is
\[
T(q) = \left( \begin{array}{rr}
1 & 0 \\
-1 & 0 \\
0 & 1
\end{array} \right),
\]
and $Y_1(q) = (1/I, -1/mq_3^2,0)^T, Y_2(q) = (0,0,1/m)^T$.  Calculations show that
\begin{equation}
\symprod{Y_1}{Y_1} = \left( \begin{array}{c}
0 \\
0 \\ 
-\frac{2}{m^2 q_3^3}
\end{array} \right), \;\;
\symprod{Y_1}{Y_2} = \symprod{Y_2}{Y_2} = 0, \;\;
%\nonumber \\
[Y_1,Y_2] = \left( \begin{array}{c}
0 \\
-\frac{2}{m^2 q_3^3} \\
0 \end{array} \right). 
%[Y_1,\symprod{Y_1}{Y_1}] = \left( \begin{array}{c}
%0 \\
%\frac{4}{m^3 q_3^6} \\
%0 \end{array} \right).
\nonumber
\end{equation}
We see that $Y_1$, $Y_2$, and $[Y_1,Y_2]$ span $T_q{\Cs}$ for
all $q$ with $q_3>0$.  Also, the bad symmetric product $\symprod{Y_1}{Y_1}$ is 
neutralized by $Y_2$ and the bad symmetric product $\symprod{Y_2}{Y_2}$
is zero. 
Therefore, by Theorem~\ref{thm:symstlcc},
the system is small-time locally configuration controllable
at all $q$ and equilibrium controllable.  We also see that the distribution
$\symclos({\cal Y})$ is only two-dimensional, so the system is not STLA
by Theorem~\ref{thm:symstlc}.
}
\end{example}


\subsection{Kinematic Reductions for Motion Planning}

In the controllability tests above, the symmetric product essentially
allows us to treat the $Y_i$ similar to velocity vector fields on a
configuration space, 
%i.e., as a kinematic system, 
thus halving the dimension of the vector fields in our controllability
calculations.  It is also sometimes possible to plan trajectories
for underactuated dynamic systems as if they were kinematic systems.
This reduction decreases the dimension of the search space by a 
factor of two.  Since many search algorithms run in time exponential
in the dimension of the search space, this reduction can greatly
speed up motion planning.
%decrease the computational complexity of motion planning.
%Since this reduction decreases the dimension of the search space by
%a factor of two, from the phase space to the configuration space,
%the computational complexity of motion planning can be greatly decreased.

Consider the original second-order dynamic system
\begin{equation}
\nabla_{\dot{q}} \dot{q} = \sum_{i=1}^m Y_i(q) u_i, \;\; u \in \real^m,
\label{eq:dynamicmodel}
\end{equation}
and a first-order driftless kinematic system
\begin{equation}
\dot{q} = \sum_{i=1}^{l} V_i(q) w_i, \;\; w \in \real^l, \;\; w(t) \; {\rm  continuous},
\label{eq:kinematic}
\end{equation}
where the set of control (velocity) vector fields is written
${\cal V} = \{V_1,\ldots,V_l\}$.
We make the extra stipulation that $w(t)$ be continuous because
discontinuous velocities would require infinite forces.  Also, in
this section on kinematic reductions, the control sets are taken
to be unbounded for simplicity.

A kinematic system \eqref{eq:kinematic} is a {\bf
kinematic reduction} of a dynamic system \eqref{eq:dynamicmodel}
if all feasible trajectories for the kinematic system are also 
feasible for the dynamic system.  We further say that a dynamic
system \eqref{eq:dynamicmodel} is {\bf fully reducible to a 
kinematic system} if there exists a kinematic reduction such that
all feasible trajectories of the dynamic system, starting with
an initial velocity in ${\rm span}({\cal Y})$,
are also trajectories
of the kinematic reduction.  
%In this case, the kinematic reduction and
%the original dynamic system are fundamentally equivalent.  
For example, all fully actuated dynamic systems are fully reducible to
kinematic systems---we can equivalently assume the controls are either 
(continuous) velocities or accelerations.

Some underactuated dynamic systems are also fully reducible to 
kinematic systems.  The test is given by the following theorem
due to Lewis~\cite{Lewis99}.

\begin{theorem}
A second-order dynamic system \eqref{eq:dynamicmodel} is fully reducible to
a kinematic system if and only if $\symclos({\cal Y}) = {\rm span}({\cal Y})$.
%If the dynamic system is fully reducible, then a kinematic reduction with
%${\rm span}({\cal V}) = {\rm span}({\cal Y})$ is a kinematic version of
%the original system.
\label{thm:kinematic}
\end{theorem}

\begin{example}
{\em
{\em Hopper (cont.) \/}
Our previous calculations showed that $\symprod{Y_1}{Y_2} = \symprod{Y_2}{Y_2} = 0$ 
and $\symprod{Y_1}{Y_1} \in {\rm span}(Y_2)$.  Therefore $\symclos({\cal Y}) =
{\rm span}({\cal Y})$ and the hopper is fully reducible to a kinematic
system with ${\cal V} = {\cal Y}$.  Intuitively, this is because the second-order
dynamic constraint of the hopper can be integrated to a velocity constraint:
conservation of angular momentum.  Therefore, any motion possible by controlling
force and torque (accelerations) is also possible by controlling velocities.
%Intuitively, this is because the
%second-order dynamic constraint of the hopper can be integrated to 
%a velocity constraint: conservation of angular momentum.  Therefore,
%any motion obtained by control forces and torques can be obtained 
%by controlling the leg extension velocity and the angular velocity of
%the leg relative to the body.
}
\end{example}

A kinematic reduction which can generate all trajectories of
the dynamic system (with initial velocity in ${\rm span}({\cal Y})$)
could be called a
{\bf rank $m$ kinematic reduction}, as it must have $m$
linearly independent control vector fields $V_i$,
and these vector fields must satisfy
${\rm span}({\cal V}) = {\rm span}({\cal Y})$.
%$m$ linearly independent control vector 
%fields $V_1, \ldots, V_m$.  
The class of underactuated systems admitting rank $m$ kinematic
reductions is relatively small, so we would like to explore kinematic
reductions for more general underactuated systems.  In particular, a
system which is not fully reducible to a kinematic system may
nonetheless admit a {\bf rank $1$ kinematic reduction} $\dot{q} =
V_1(q) w_1(t)$.  A rank $1$ kinematic reduction has a single control
vector field $V_1$, also known as a {\bf decoupling vector field}.
(The word ``decoupling'' stems from the fact that motion planning for
the second-order system along an integral curve of such a vector field
can be decoupled into choosing the distance traveled along the
integral curve, followed by time-scaling the path according to
actuator limits.  This brings to mind the decoupled trajectory
planning approach for fully actuated systems in the previous chapter.)

An underactuated dynamic system can follow the integral curve of a
decoupling vector field at any speed and acceleration, i.e., for any
continuous $w_1(t)$.  A dynamic system \eqref{eq:dynamicmodel} can
have no more than $m$ linearly independent decoupling vector fields at
any $q$.  For a fully reducible dynamic system, every vector field in
${\rm span}({\cal Y})$ is a decoupling vector field.

How do we know if a proposed decoupling vector field $V \in {\rm span}({\cal Y})$
is actually decoupling?  The following theorem provides the answer
(Bullo and Lynch~\cite{Bullo2001}).

\begin{theorem}
A vector field $V$ is a decoupling vector field of the second-order
dynamic system \eqref{eq:dynamicmodel} if and only if $V \in {\rm span}({\cal Y})$
and $\nabla_V V \in {\rm span}({\cal Y})$. 
\label{thm:decoupling} 
\end{theorem}

Stated another way, from the definition of the kinematic reduction
\eqref{eq:kinematic} for the single control vector field $V(q)$, we have
\begin{eqnarray}
\dot{q} & = & V(q) w \\
\ddot{q} & = & V(q) \dot{w} + \frac{\partial V}{\partial q} V(q) w^2,
\end{eqnarray}
and plugging these into \eqref{eq:dynamicconstraints}, we see that $V(q)$
is decoupling if and only if the last $n_{\Cs}-m$ rows of
\eqref{eq:dynamicconstraints} (the dynamic constraints)
are satisfied for all $w, \dot{w} \in \real$.

\begin{example}
{\em
{\em PBWT (cont.)\/}
Decoupling vector fields for the PBWT are $V_1 = (\cos q_3, \sin q_3, 0)^T$
and $V_2 = (-\sin q_3, \cos q_3,-d)^T$.  To verify this by Theorem
\ref{thm:decoupling}, we see that $V_1 = Y_1$, $V_2 = Y_2$, $\nabla_{V_1} V_1 = 0$,
and $\nabla_{V_2} V_2 = (d\cos q_3,d\sin q_3,0)^T \in {\rm span}(Y_1)$.
Note, however, that $\symprod{Y_1}{Y_2} \not\in {\rm span}({\cal Y})$,
so the PBWT is not fully reducible to a kinematic system; i.e., arbitrary
linear combinations of $V_1$ and $V_2$ are \emph{not} decoupling vector
fields.

Consider the physical meaning of these decoupling vector fields.
$V_1$ is pure translation of the PBWT along the line of action of the
thruster $u_1$.  It is clear that translation in this direction is possible
at any speed and acceleration by proper choice of the thrust.  $V_2$
corresponds to rotation of the PBWT about a point fixed relative to the 
body, the \emph{center of percussion} or \emph{center of oscillation}
of the PBWT relative to the line of action of thruster $u_2$.  This point 
is a distance $1/d$ from the center of mass on a line through the center 
of mass and perpendicular to the line of action of $u_2$ 
(see Figure~\ref{fig:planarbodyCP}).  Rotation about this point is possible
at any speed and acceleration by using $u_2$ to provide the torque to 
rotate the PBWT and using
$u_1$ to keep the center of percussion stationary.
}
\end{example}

\begin{figure}
\begin{center}
\includegraphics{../figure/book/nonholonomic/planarbodyCP.ps}
\end{center}
\caption{The two decoupling vector fields for the PBWT:  translation
along the line of action of $u_1$, and rotation about the center of
percussion with respect to $u_2$.}
\label{fig:planarbodyCP}
\end{figure}

In some cases the decoupling vector fields for a second-order system
are apparent by inspection.
In other cases it is possible to calculate the decoupling vector
fields by solving a system of quadratic
equations.  To see this, recognize that any
decoupling vector field $V(q)$ is contained in ${\rm span}({\cal Y})$,
so can be written in the form
\[
V(q) = h_1(q) Y_1(q) + \ldots + h_m(q) Y_m(q), \;\; h_i(q) \in \real.
\]
The problem is to solve for functions $h_i(q)$ such that
$X_c^T(M\nabla_{V} V) = 0$ at all $q$, where the 
$X_c(q), c = 1\ldots n_{\Cs}-m$ are 
linearly independent 
basis vectors of $\real^{n_{\Cs}}$ which are
orthogonal (according to $M(q)$) to ${\cal Y}(q)$ at all $q$.

Kinematic reductions allow us to plan a path using a
kinematic system, then ``lift'' this path to a trajectory for 
the full dynamic system.  One important issue is to understand
the locally reachable configurations of the kinematic reductions.
Beginning from an equilibrium state $(q^T,0_{n_{\Cs}}^T)^T$, we say that a 
second-order dynamic system \eqref{eq:dynamicmodel}
is {\bf small-time locally kinematically
controllable} (STLKC) from $q$ if there exists a set of decoupling
vector fields $V_1, \ldots, V_p$ such that $\lie(\{V_1,\ldots,V_p\})
= T_q{\Cs}$.  This means that a kinematic system which can only
move along these decoupling vector fields is STLC at $q$.
If this holds for all
$q \in {\Cs}$, the dynamic system is {\bf kinematically controllable},
meaning that it is equilibrium controllable by using only 
decoupling vector fields.  
%In other words, the set of decoupling
%vector fields is ``rich'' enough to steer the system between any
%two equilibrium states.  
These decoupling vector fields can therefore
be used as primitives in a \emph{kinematic} motion planner, reducing the complexity
of the search for a feasible trajectory.  A planned path would then consist
of the concatenation of integral curves of the decoupling vector fields.
Switches between decoupling vector fields must occur at zero velocity.

The reader may easily verify that both the PBWT and the hopper
are STLKC by two decoupling vector fields.  For the PBWT, any configuration
is reachable by concatenating integral curves of $V_2$, $V_1$, and 
$V_2$ (rotation, translation, and rotation). 

*** should I be using $Y_1^{pbwt}$ here similar to with the $g$ vector
fields?  But I'm not relabeling the $M$ or $T$ matrices.  Be consistent.

\subsection{Simple Mechanical Systems with Nonholonomic Constraints}

The ideas discussed in this section also apply to dynamic
systems subject to nonholonomic constraints, such as rolling
without slipping~\cite{Bullo2002}.  Such systems can be expressed
\begin{eqnarray}
T_{a}^{-1}(q)(M(q)\ddot{q} + C(q,\dot{q})\dot{q} - A^T(q) \lambda) & = & \left(\begin{array}{c} u \\ 0_{n_{\Cs}-m}\end{array}\right) \label{eq:constraineddynamics} \\
A(q) \dot{q} & = & 0_k, \label{eq:velocityconstraints}
\end{eqnarray}
where \eqref{eq:constraineddynamics}
is analogous to \eqref{eq:dynamicconstraints}.
$T_{a}(q)$ is a full-rank $n_{\Cs}\times n_{\Cs}$ matrix
whose first $m$ columns indicate how the control forces $u$ act on the
generalized coordinates, $A(q)$ is a $k \times n_{\Cs}$ matrix 
describing the $k$ Pfaffian velocity constraints $A(q) \dot{q} = 0_k$,
and $A^T(q) \lambda \in \real^{n_{\Cs}}$ is a set of constraint forces, where $\lambda \in
\real^k$ are Lagrange multipliers.  
For this constrained system,
we need only define a modified covariant
derivative $\widetilde{\nabla}$ relevant to vector fields in the
distribution ${\cal D}$ satisfying the Pfaffian constraints~\eqref{eq:velocityconstraints}.  The same
simplified controllability tests and conditions for kinematic
reductions hold for these systems with this modified
covariant derivative.  
Keep in mind that reachable velocities are confined to an 
$(n_{\Cs}-k)$-dimensional distribution ${\cal D}$ due to the 
$k$ velocity constraints.
Therefore, the most we can hope for at any $q$ is STLCC and
$\symclos({\cal Y}) = {\cal D}$.

As before, a velocity vector field $V(q) \in {\cal D}$ is
decoupling if and only if the dynamic constraints of the last $n_{\Cs}-m$
rows of \eqref{eq:constraineddynamics} are satisfied for
$\dot{q} = V(q) w$ and $\ddot{q} = V(q) \dot{w} + \frac{\partial V}{\partial q}
V w^2$ 
for any $w, \dot{w} \in \real$.

For vector fields $Y_1,Y_2 \in {\cal D}$, the constrained covariant
derivative $\widetilde{\nabla}$ is defined by
\[
\widetilde{\nabla}_{Y_1} Y_2 = P (\nabla_{Y_1} Y_2),
\]
where $P$ is the projection to the motion directions orthogonal
to the constraints as described in the previous chapter.  Letting
$Y_i^\prime, i = 1 \ldots m$ be the columns of $M^{-1} T(q)$, i.e.,
the unconstrained control vector fields, 
we define the constrained control vector fields $Y_i = P(Y_i^\prime)$, 
the projection to the free motion directions.  We then study controllability
and kinematic reductions using the constrained covariant derivative
$\widetilde{\nabla}$ and the constrained control vector fields $Y_i$.
%The full constrained dynamics can be written
%\[
%P(\nabla_{P(\dot{q})} P(\dot{q})) = \sum_{i=1}^m Y_i u_i
%\]
%with the constrained control vector fields $Y_i$.

A formula for calculating the constrained Christoffel symbols
$\widetilde{\Gamma}$ is given by Lewis~\cite{Lewis2000}, which also holds
for vector fields not restricted to ${\cal D}$.  Bullo and
Zefran~\cite{Bullo2002b} give a computationally simpler formulation for vector
fields restricted to ${\cal D}$.  This formulation uses an
orthogonal basis of vector fields of the distribution ${\cal D}$
to construct modified Christoffel symbols.  Choosing a basis of
vector fields for the free motions of a
nonholonomically constrained system is analogous to choosing
a coordinate basis for the reachable configuration space of a
holonomically constrained system,
as discussed in the previous chapter.

\begin{example}
{\em
In the previous chapter we considered the example of a knife-edge
sliding on a plane.  For this system, the inertia matrix $M(q)$ and the
constraint $A(q)$ are given by
\[
M(q) = \left(\begin{array}{ccc} m & 0 & 0 \\ 0 & m & 0 \\ 0 & 0 & I
\end{array}\right), \;\; A(q) = (\sin q_3, -\cos q_3, 0).
\]
Assume that the available controls are $u_1$, a force in the $q_1$
direction, and $u_3$, a torque along $q_3$.  Then
\[
T(q) = \left(\begin{array}{cc} 1 & 0 \\ 0 & 0 \\ 0 & 1 \end{array}\right), \;\;
M^{-1}(q)T(q) = \left(\begin{array}{cc} \frac{1}{m} & 0 \\ 0 & 0 \\ 0 & \frac{1}{I}
\end{array}\right),
\]
and $Y_1^\prime = (1/m,0,0)^T, Y_2^\prime = (0,0,1/I)^T$.  Using the
projection matrix $P$ defined by $A$ and $M$
\[
P = \left(\begin{array}{ccc} \cos^2 q_3 & \sin q_3 \cos q_3 & 0 \\
\sin q_3 \cos q_3 & \sin^2 q_3 & 0 \\
0 & 0 & 1 \end{array}\right),
\]
we get 
\[
Y_1 = P(Y_1^\prime) = (\cos^2 q_3/m, \sin q_3 \cos q_3/m, 0)^T, \;\;
Y_2 = P(Y_2^\prime) = (0,0,1/I)^T,
\]
\[
\symprod{Y_1}{Y_1} = \symprod{Y_2}{Y_2} = 0_3, \;\;
\symprod{Y_1}{Y_2} = \left(-\frac{\sin 2 q_3}{2 mI}, -\frac{\sin^2 q_3}{mI},0\right)^T = 
-\frac{\tan q_3}{I} Y_1,
\]
\[ 
[Y_1,Y_2] = \left(\frac{\sin 2 q_3}{mI}, -\frac{\cos 2 q_3}{mI},0\right)^T, \;\; [Y_2,[Y_1,Y_2]] = \left(2\frac{\cos 2 q_3}{mI^2},2\frac{\sin 2 q_3}{mI^2},0\right)^T.
\]
We see that $\symclos(\{Y_1,Y_2\}) = {\rm span}(\{Y_1,Y_2\})$,
so the system is fully reducible to a kinematic system by 
Theorem~\ref{thm:kinematic}.  The control (velocity) vector fields 
of the rank 2 kinematic reduction are $Y_1$,
motion along straight lines (when $q_3 \neq \pm \pi/2$), and
$Y_2$, spinning in place.  
%Since $\symclos(\{Y_1,Y_2\})$ defines just
%a two-dimensional distribution, the system is neither small-time
%locally accessible nor controllable

The distribution and filtration defined by $Y_1$ and $Y_2$ are not regular, as
$Y_1$ vanishes at $q_3 = \pm \pi/2$.  We also see that
\[
{\rm det}(Y_1 \;\; Y_2 \;\; [Y_1,Y_2]) = \frac{\cos^2q_3}{m^2 I^2},
\]
so we cannot conclude local configuration accessibility until we 
construct $[Y_2,[Y_1,Y_2]]$ and see
\[
{\rm det}(Y_2 \;\; [Y_1,Y_2] \;\; [Y_2,[Y_1,Y_2]) = \frac{2}{m^2 I^4},
\]
so the system is STLCA and STLCC at all $q$ by Theorem~\ref{thm:symstlc}.  
}
\label{example:knife-edgecontrol}
\end{example}


%\section{Review of Kinematic Reductions}
%
%Here we review some notions that allow us to treat the snakeboard
%as a kinematic system, taken from the works (Lynch-Shiroma IJRR paper,
%Bullo-Lynch TRA paper, Bullo-Lewis-Lynch MTNS paper).  
%
%A second-order underactuated dynamic system subject to nonholonomic velocity
%constraints can be written in the following form:
%\begin{eqnarray}
%M(q)\ddot{q} + C(q,\dot{q})\dot{q} + g(q) & = & T(q) \left(\begin{array}{c} u \\ 0_{n-m}\end{array}\right) + A^T(q) \lambda 
%\label{eq:dynamics} \\
%A(q) \dot{q} & = & 0_p, \label{eq:velocityconstraints}
%\end{eqnarray}
%where $q \in {\Cs} = \real^n$ is a vector of generalized coordinates, $u \in \real^m$
%is a vector of generalized control forces,
%$M(q)$ is a full-rank $n \times n$ inertia matrix, $C(q,\dot{q})\dot{q} \in \real^n$ is a
%vector of centrifugal and Coriolis terms with $C$ linear in $\dot{q}$, 
%$g(q) \in \real^n$ is a vector of
%gravitational forces, $T(q)$ is a full-rank $n\times n$ matrix whose first $m$ columns
%indicate how the control forces $u$ act on the generalized coordinates,
%$A(q) \in \real^{p \times n}$ describes the $p$
%Pfaffian velocity constraints $A(q) \dot{q} = 0_p$, and 
%$A^T(q) \lambda$ is a set of constraint forces,
%where $\lambda \in \real^p$ are Lagrange multipliers.  Since the system
%is underactuated, $m < n$.
%
%We can rearrange \eqref{eq:dynamics} to obtain
%\begin{equation}
%T^{-1}(q)(M(q)\ddot{q} + C(q,\dot{q})\dot{q} + g(q) - A^T(q) \lambda) = \left(\begin{array}{c} u \\ 0_{n-m}\end{array}\right). \label{eq:dynamics2}
%\end{equation}
%The last $n-m$ rows of \eqref{eq:dynamics2} represent dynamic constraints
%on the motion of the system.
%Any motion of the system $q(t)$ must satisfy the $p$ velocity constraints
%\eqref{eq:velocityconstraints} as well as the $n-m$ dynamic constraints in
%\eqref{eq:dynamics2}.
%
%Consider a motion of the system $q(t)$ such that the $p$ velocity
%constraints \eqref{eq:velocityconstraints} and the $n-m$ dynamic
%constraints in \eqref{eq:dynamics2} are satisfied for any
%arbitrary time-scaling of the motion, $s(t)q(t), s(t) \in \real$.
%Then $q(t)$ is said to be a \emph{kinematic} motion for the system,
%and the velocity vector field $V(q) = dq(t)/dt : {\Cs} \rightarrow T{\Cs}$
%is called a \emph{decoupling vector field}.  We call the first-order
%kinematic control system
%\[
%\dot{q} = V(q) w(t), \;\; w(t) \in \real \; {\rm and \; continuous}
%\]
%a \emph{kinematic reduction} of the dynamic
%system, meaning that any trajectory of the kinematic reduction
%can be realized by the full dynamic system.  This means that if
%we restrict the possible motions of the full dynamic system to 
%motion along decoupling vector fields, we can treat the system
%as if it were a kinematic system.  We say that the system is
%\emph{locally kinematically controllable} if there exists a set
%of decoupling vector fields satisfying the Lie algebra rank condition.
%This means that the system is small-time locally
%controllable on its equilibrium configuration space.  Motion 
%planning between zero-velocity states then becomes the problem
%of finding a concatenation of integral curves of the decoupling
%vector fields leading from the initial to the goal configuration.
%Switches between the decoupling vector fields must occur at
%zero velocity, so for fast path execution, it is useful to find
%motions which minimize the number of switches needed between
%vector fields.
%It is precisely this problem that this paper solves for the snakeboard.
%
%\section{The Snakeboard Model}
%
%(Bullo and Lewis) have shown that the snakeboard admits two linearly
%independent decoupling vector fields satisfying the Lie algebra rank
%condition.  Therefore, these two velocity vector fields can be used
%to plan motions between any two configurations.
%
%
%
%here maybe briefly describe the matrices $M(q)$, $T(q)$, and
%$A(q)$ for the snakeboard, from which the equations above can
%be solved.  Then give the two decoupling vector fields.


\section{Motion Planning}
\label{sec:planning}

Motion planning for underactuated systems has been the subject of a
great deal of recent research, and the useful results
could easily fill several books (see, for example, 
the books edited by Li and Canny~\cite{Li93} and
Laumond~\cite{Laumond98}).  In this section we summarize a few useful approaches.
The approaches can be classified by the type of system they apply to
(e.g., the structure of the equations of motion, and with or
without control constraints or drift)
or the nature of the problem (with or without obstacles
or cost function to be minimized).  Motion planning approaches with roots
in control theory tend to 
apply to systems with particular structure and
no obstacles, while approaches based on search algorithms are 
computationally intensive and are suited to finding
collision-free trajectories among obstacles.  Some approaches attempt
to combine the benefits of control-theoretic and search-based methods.

The problem is to find a trajectory $(x(t),u(t)), t \in [0,t_f]$ satisfying
\eqref{eq:controlaffine}
such that $x(0) = x_0,x(t_f) = x_f$.  In the presence of obstacles, where
${\Cs}_{free}$ represents the free configuration space, we also require
$q(t) \in {\Cs}_{free}, t \in [0,t_f]$.


%*	- motion planning for cars, cars and trailers (Latombe, Laumond, Lumelsky IROS 2000, 
%Mason and Balkcom, Pin, lots...) 
%*	- holonomic planning followed by transform for STLC systems 
%*	- optimization approaches, nonsingular loops (Wen, Sontag, Sussmann, Fernandes, others) 
%*	- geometric phase, iterative closed loops 
%*	- RRT 
%*	- differentially flat systems (Murray, Fliess, Martin, others) 
%*	- chained form systems (many) 
%*	- kinematically controllable systems (Bullo and Lynch) 
%*	- averaging/oscillatory approaches 
%*	(- mixed kinematic and dynamic) 
%*	- fictitious inputs, Lafferiere's method 
%*	some other references: Latombe book, Canny and Li book, Murray book, Mason book, Wen article, my ME 495 lectures, Sastry book, Laumond book, Svestka, Sekhavat
%

%cite Brockett, Murray and Sastry, Wen

\subsection{Optimal Control}

For some simple underactuated systems, it is possible to solve 
analytically for optimal controls transferring the system from
one state to another using the ideas developed in the previous
chapter.  Consider, for example, a driftless system with 
$m=2$ controls and $n=3$ states and the
control vector fields $g_1 = (1,0,x_2)^T$ and $g_2 = (0,1,0)^T$.
This system can be written
\begin{equation}
\left(\begin{array}{c} \dot{x}_1 \\ \dot{x}_2 \\ \dot{x}_3 \end{array} \right)
= \left(\begin{array}{cc} 1 & 0 \\ 0 & 1 \\ x_2 & 0 \end{array}\right)
\left( \begin{array}{c} u_1 \\ u_2 \end{array} \right).
\label{eq:brockettsystem}
\end{equation}
Assume the time of motion is fixed, $t_f = 1$, and the objective is to
minimize a measure of the control input energy:
\begin{equation}
J = \frac{1}{2} \int_0^{t_f} \| u \|^2 dt.
\label{eq:inputenergy}
\end{equation}
Then the Hamiltonian is written
\[
{\cal H} = \frac{1}{2} (u_1^2 + u_2^2) + \lambda_1 u_1 + 
\lambda_2 u_2 + \lambda_3 x_2 u_1.
\]
Solving the necessary condition $\partial {\cal H} / \partial u = 0$,
we get
\begin{equation}
u_1 = - \lambda_1 - \lambda_3 x_2, \;\;\; u_2 = -\lambda_2.
\label{eq:brockettoptimal}
\end{equation}
The adjoint equation $\dot{\lambda} = -\partial {\cal H} / \partial x$
indicates that $\lambda_1$ and $\lambda_3$ are constant, and 
$\dot{\lambda}_2 = -\lambda_3 u_1$.  Differentiating \eqref{eq:brockettoptimal}
with respect to time, we get
\begin{eqnarray}
\dot{u}_1 & = & - \dot{\lambda}_1 - \dot{\lambda}_3 x_2 - \lambda_3 \dot{x}_2 = -\lambda_3 u_2 \nonumber \\
\dot{u}_2 & = & -\dot{\lambda}_2 = \lambda_3 u_1. \nonumber
\end{eqnarray}
These differential equations imply that optimal controls
$u_1(t)$ and $u_2(t)$ are $90^\circ$
out-of-phase sinusoids of the same amplitude and frequency, i.e.,
\begin{eqnarray}
u_1(t) & = & u_1(0) \cos(\lambda_3 t) - u_2(0) \sin(\lambda_3 t) \label{eq:u1form} \\
u_2(t) & = & u_1(0) \sin(\lambda_3 t) + u_2(0) \cos(\lambda_3 t). \label{eq:u2form}
\end{eqnarray}
Given $x(0)$ and $x(1)$, the integrals of the equations of motion
\eqref{eq:brockettsystem} define three equations to solve for 
$\lambda_3$, the frequency of the sinusoids, and the
constants $u_1(0)$ and $u_2(0)$, defining the phase and 
amplitude $u_1(0)^2 + u_2(0)^2$.

These equations may be difficult to solve generally, but one simple case
is of particular interest.  We will choose controls so that at the end
of the motion, $x_1$ and $x_2$ have returned to their initial values,
while $x_3$ has changed from $x_3(0) = 0$ to $x_3(1) = x_{3,f}$.  In
this case, $\lambda_3 = 2 k \pi$, where $k$ is any nonzero integer.  This assures
that 
\[
\int_{0}^{1} u_1(t) dt = \int_{0}^{1} u_2(t) dt = 0,
\]
so $x_1$ and $x_2$ return to their initial values.  Plugging 
$\lambda_3 = 2 k \pi$ into the the controls \eqref{eq:u1form} and
\eqref{eq:u2form}, and putting the controls into the 
objective function \eqref{eq:inputenergy},
we find that 
\[
J = \frac{1}{2}(u_1(0)^2 + u_2(0)^2).
\]
The cost of the motion is independent of the choice of $k$, so we
choose $k = \pm 1$.

Integrating the equation of motion for $x_3$, we find that
\[
x_3(1) = -\frac{u_1(0)^2 + u_2(0)^2}{4 \pi}
\]
for $k=1$, and
\[
x_3(1) = \frac{u_1(0)^2 + u_2(0)^2}{4 \pi}
\]
for $k=-1$.  Therefore, if $x_{3,f} > 0$, we choose $k=-1$ and any choice
of $u_1(0)$ and $u_2(0)$ satisfying the condition $x_3(1) = x_{3,f}$ (all choices
have the same cost).  If $x_{3,f}<0$, we choose $k=1$ and
proceed similarly.  Notice that there is a one-dimensional set
of solutions in the two-dimensional $(u_1(0),u_2(0))$ space, as 
we only need to satisfy the single equation $x_3(1) = x_{3,f}$.

The motion described above suggests a strategy for motion planning for
a more general class of systems.
%systems such as \eqref{eq:brockettsystem}.  
First, use the controls to
drive $m$ state variables directly to their goal values.
Then perform motions which return these state variables to their goal
values, but cause a desired net motion in the other state variables.

*** this example is in between two paragraphs that follow each other,
maybe this should go later

As an example, beginning from rest,
an astronaut in space can control the orientation of
his body by moving his arms in a cyclic pattern.  
At the end of a
cycle, the \emph{shape} (arm joint angles) of the astronaut is
restored, but the orientation of his body has changed.  (Keep in mind
that the astronaut's total angular momentum is zero throughout the motion
since there are no external forces.)
We can decompose the astronaut's configuration into {\bf shape} variables
(also called {\bf base}
variables), describing the variables over which he has direct control
(the configuration of his arms), and {\bf fiber} variables (the
orientation of his body), which are coupled to the controls in 
a state-dependent manner.  In the example system \eqref{eq:brockettsystem},
the shape variables are $x_1$ and $x_2$ and the fiber variable is
$x_3$.  

Many driftless systems can be transformed to a form similar to 
\eqref{eq:brockettsystem}, allowing the control strategy of directly
steering the shape variables to their goal configurations, and
then performing closed loops (for example, sinusoids)
in the shape variables to achieve 
desired motions in the fiber variables.  For example, the unicycle
can be converted to this form by performing a coordinate transformation
and an input feedback transformation.  Define new coordinates
$z = A(x) x$ and transformed controls
$v = B(x) u$, where 
\[
A(x) = \left(\begin{array}{ccc} 0 & 0 & 1 \\
\cos x_3 & \sin x_3 & 0 \\
\sin x_3 & -\cos x_3 & 0 
\end{array}
\right) \;\;\;\;
B(x) = \left(\begin{array}{cc} 0 & 1 \\
1 & x_2 \cos x_3 - x_1 \sin x_3
\end{array}
\right).
\]
Then the original unicycle system is transformed to an equivalent system of the
form \eqref{eq:brockettsystem}:
\[
\left(\begin{array}{c}
\dot{x}_1 \\ \dot{x}_2 \\ \dot{x}_3 \end{array} \right) =
\left(\begin{array}{cc} \cos x_3 & 0 \\ \sin x_3 & 0 \\ 0 & 1 \end{array}\right)
\left(\begin{array}{c} u_1 \\ u_2 \end{array}\right) \; \Longleftrightarrow \;
\left(\begin{array}{c}
\dot{z}_1 \\ \dot{z}_2 \\ \dot{z}_3 \end{array} \right) =
\left(\begin{array}{cc} 1 & 0 \\ 0 & 1 \\ z_2 & 0 \end{array} \right)
\left(\begin{array}{c} v_1 \\ v_2 \end{array}\right)
\]
A system like this is an example of a broader class of \emph{chained-form} systems,
which are the topic of the next section.

The motion strategy of driving the shape variables to their goal values
and then performing closed loops generally results in sub-optimal motions, but
it is rarely possible to solve the optimality conditions analytically.
In any case, the quadratic ``energy-like'' objective function discussed above may
not have much physical meaning for an input-transformed system such as the 
unicycle above.
Section~\ref{ssec:optimization} discusses numerical methods
for finding approximately optimal motion plans.

%Another example is the single-leg hopper described in
%Section~\ref{sec:mechanicalsystems}: by performing a closed loop in
%the extension of the leg and the angle of the leg relative to the
%body, it is possible to cause a net change in the orientation of the
%body.  
%Underactuated systems can often be decomposed into {\bf shape}
%variables, describing the variables over which we have direct control
%(e.g., $x_1$ and $x_2$ in the example system \eqref{eq:brockettsystem}
%above, or the configuration of the astronaut's arms), and {\bf fiber}
%variables, which are coupled to the controls in a state-dependent
%manner (e.g., $x_3$ in the example above, or the orientation of the
%astronaut's body).

%geometric phase, shape/base and fiber variables

\subsection{Steering Chained-Form Systems Using Sinusoids}

Consider the following system with $m=2$ controls and $n$ states,
generalizing the system with $n=3$ described above:
\begin{equation}
\dot{x} = u_1 g_1(x) + u_2 g_2(x), \;\;
g_1(x) = \left(\begin{array}{c} 1 \\ 0 \\ x_2 \\ x_3 \\ \vdots \\ x_{n-1}\end{array}\right),\;
g_2(x) = \left(\begin{array}{c} 0 \\ 1 \\ 0 \\ 0 \\ \vdots \\ 0 \end{array}\right).
\label{eq:chainedform}
\end{equation}
Such a system is said to be in {\bf chained form}.  Considering a
Lie product of the form $[g_1,[g_1, \ldots[g_1,[g_1,g_2]]]]$, where
$g_1$ appears $k$ times, we find that the Lie product has 
a value $(-1)^k$ in the $k+2$ component of the vector field, and
zeros in all other components.  It is clear, then, that the Lie
algebra $\lie(\{g_1,g_2\})$ is full rank, and the system is
STLC at all $x$ for ${\cal U}={\cal U}_{\pm}$.

To steer such a system, we can generalize the approach presented
earlier.  First, drive $x_1$ and $x_2$ to their final values.  Then
choose controls $u_1(t)$ and $u_2(t)$ to be sinusoids at integrally
related frequencies.  For example, consider
\begin{eqnarray}
u_1(t) & = & a \sin 2\pi t \nonumber \\
u_2(t) & = & b \cos 2k\pi t, \label{eq:sincontrols}
\end{eqnarray}
where $k$ is a positive integer.  Then $\dot{x}_3$ has components
at frequency $2\pi (k-1)$, $\dot{x}_4$ has components at $2 \pi (k-2)$,
etc.  
%If the frequency is greater than zero, the net effect of the
%periodic controls for one 
Applying the controls for $t_f=1$ will return
the $x_1 , \ldots, x_{k+1}$ variables to their initial values --- the
nonzero frequency over the cycle means that $\dot{x}_1,\ldots,\dot{x}_{k+1}$
integrate to zero net change.
For the
variables $x_{k+2},\ldots,x_n$, however, the periodic controls will
result in a nonzero DC (zero frequency) component in their time-derivatives, 
meaning that they will be changed
over the cycle.  The net change to $x_{k+2}$ can be computed to be
\begin{equation}
x_{k+2}(1) - x_{k+2}(0) = \left(\frac{a}{4 \pi}\right)^k \frac{b}{k!}.
\label{eq:sincoefficients}
\end{equation}
Therefore, an algorithm to drive the system to the goal state is
the following:
\begin{enumerate}
\item Drive the variables $x_1$ and $x_2$ directly to their goal values.
\item For each $k = 1,\ldots,n-2$, in ascending order, apply the controls
\eqref{eq:sincontrols} for time $t_f = 1$ with $a$ and $b$ selected
according to \eqref{eq:sincoefficients} to drive $x_{k+2}$ to its desired
value, leaving all $x_1,\ldots,x_{k+1}$ unchanged.
\end{enumerate}

A number of $n$-state $2$-input systems can be transformed to the
chained form \eqref{eq:chainedform}, including tractor-trailer systems
with multiple trailers, or the unicycle including the location of its air nozzle in the 
configuration.  There are also other forms similar to the
chained form presented here, including forms with more than
two inputs; see, for example,~\cite{Murray93,Murray94}.

*** this could use an example, or more references

A number of motion planning algorithms based on series expansions of
the equations of motion use sinusoidal inputs at appropriately chosen
frequencies and phases to ``tickle'' the system in desired Lie bracket
directions.  This idea also applies to dynamic systems with
drift.  For a locomotion system where motions of the shape variables
induce motion of the fiber variables, for example, these sinusoidal
inputs generate ``gaits'' for the system.

*** cite Ostrowski, Bullo, others

\subsection{Nonlinear Optimization}
\label{ssec:optimization}

The previous chapter outlined a method for using gradient-based
nonlinear optimization
to find locally optimal trajectories for a fully actuated dynamic
system.  The same approach can be applied to underactuated systems.
In this case, we choose a finite parameterization of the control history
$u(t)$, since any \emph{a priori} parameterization of the state $x(t)$
will likely describe trajectories which are infeasible for the system
due to the underactuation constraints.

{\bf *** or choose $y(t)$ and satisfy extra constraints, or both ***}

Two features of this approach are:  (1) it is very general---motion
planning problems for many underactuated systems, including those with
drift, can be encoded as nonlinear programs; and (2) the ability
to minimize an objective function may result in motions that are ``efficient''
in some way.  Significant drawbacks, however, are 
as outlined in the previous chapter:  
(1)  a good initial guess must be provided to the
solver, as the solver will find only a locally optimal solution; and (2)
numerical difficulties, singularities, and non-convexity may prevent the solver from
converging to a solution.  The generality of the approach means
that it uses little information about the particular structure of the system
to ensure convergence.
%The generality of the approach is also its weakness.  
%In some cases, nonlinear optimization can be combine

For convex systems, systems with particular
structure~\cite{Milam2000}, or particular choices of the control
parameterization, it may be possible to demonstrate favorable
convergence properties for nonlinear optimization.  In general,
however, there are no guarantees that nonlinear optimization will
be able to find any solution, let alone a good solution, to a 
particular problem.  
%A ``good'' representation of the system 
%with a ``good'' parameterization of the control can help the
%solver find the solution, 

*** here, maybe something on Fernandes, Gurvits, and Li work, they
use a penalty function


\subsection{Gradient Methods for Driftless Systems}

To improve the convergence properties of gradient-based motion planning,
we focus on the class
of driftless systems and give up on finding optimal motions.
Let $u^p = (u^p_1,\ldots,u^p_r)^T \in \real^r$
be a finite parameterization of the
control history $u(t)$, for example, the coefficients of truncated
Fourier series for the control inputs.  Let the time of motion be $t_f=1$, 
and define an
end-state map $f$ which maps the initial state $x_0$ and the control $u^p$
to a final state of the system $x_f$:
\[
x_f = f(x_0,u^p).
\]
The end-state map $f$ is typically obtained numerically.

Now the problem is to find a $u^{p}$ so that the desired goal state
$x_g$ is reached.  Define the end-state error 
vector $e = (e_1,\ldots,e_n)^T$ to be
\[
e = f(x_0,u^p) - x_g.
\]
We would like to know the direction to change $u^p$ to move $x_f$ 
in the direction $-e$, to reduce the error.  This direction
in the $u^p$ space is $v \in \real^r$, where
\[
v^T = -e^T \left(\frac{\partial f(x_0,u^p)}{\partial u^p}\right) = 
-(e_1,\ldots,e_n) \left(\begin{array}{ccc} 
\frac{\partial f_1}{\partial u^p_1} & \ldots & \frac{\partial f_1}{\partial u^p_r} \\
\vdots & & \vdots \\
\frac{\partial f_n}{\partial u^p_1} & \ldots & \frac{\partial f_n}{\partial u^p_r}
\end{array} \right).
\]
%and $\partial f/\partial u^p$ is calculated numerically.
Given a current guess $u^p(i)$, we can update it as follows:
\[
u^p(i+1) = u^p(i) + \alpha v(i),
\]
where $\alpha$ is a small positive constant.  We then calculate
the new vector $v(i+1)$ for $u^p(i+1)$ and iterate until we
reach a $k$ such that $\|f(x_0,u^p(k)) - x_g\|<\epsilon$ for
a small constant $\epsilon$.

This algorithm is guaranteed to converge to a solution for
a sufficiently small $\alpha$ if there
are no state or control constraints, and if $\partial f/\partial u^p$
is rank $n$ everywhere.  The rank condition means that any point
in a sufficiently small neighborhood of $x_f = f(x_0,u^p)$ is reachable
by a small change to $u^p$, indicating that it is possible to move
the error vector $e$ in any direction.
Generically, if the system is STLC
everywhere and we have a rich enough control parameterization $u^p$,
the matrix $\partial f/\partial u^p$ will have rank $n$.  

If $\partial f/\partial u^p$ loses rank at $u^p(i)$, then there are
one or more directions in which we cannot move the error vector $e$.
Such a $u^p(i)$ is
a singular control and could cause
the algorithm to get stuck.  In this case, we add a control to the
end of the motion, where the control is chosen randomly from the
set of controls which result in no net motion of the system.
Such a control is easy to generate because the system
is driftless.  Because such a control is generically nonsingular,
it is said to generate a {\bf generic loop} for the system.  If we
append the random control to the control $u^p$ and treat this as 
the new control, the total control vector is no longer singular, and the
algorithm can continue.

There are many possible variants of this basic approach
\cite{Sontag93,Sontag95,Sussmann93,Divelbiss93,Wen96}.  For 
example, we could define a path on the error space from e(0) to
the origin, then choose
the iterates $u^p(i)$ to force the error to track this path.  
(The approach described above implicitly uses a straight line
path in the error space.)  We could also perform more sophisticated
gradient descent, perhaps using the Hessian $\partial^2 f/\partial u^{p2}$.
Certain obstacle constraints can also be
incorporated.  This motion planning method has been applied to
find paths for a truck pulling multiple trailers~\cite{Divelbiss93,Wen96}.

*** check Sontag's paper, too

\subsection{Differentially Flat Systems}

*** includes chained-form systems

{\bf Differentially flat} systems (Fliess {\em et al.\/}~\cite{Fliess92,Fliess95},
Martin~{\em et al.\/}~\cite{Martin97})
have a structure which make motion planning
(in the absence of control and configuration constraints such as obstacles)
particularly simple.  For a differentially flat system with $x \in \real^n$ and
$u \in \real^m$, there exists a set
of $m$ functions of the state, the control, and its derivatives
\[
y_i = h_i(x,u,\dot{u},\ldots,u^{(r)}), \;\; i = 1 \ldots m
\]
such that the states and control inputs can be expressed as functions
of $y$ and its time derivatives:
\begin{eqnarray}
x & = & \phi(y,\dot{y},\ldots,y^{(p)}) \nonumber \\
u & = & \psi(y,\dot{y},\ldots,y^{(p)}). \nonumber
\end{eqnarray}
The $y_i, i = 1\ldots m$ are known as the {\bf flat outputs}.  Armed
with a set of flat outputs, the problem of finding a feasible
trajectory $(x(t),u(t)), x(0) = x_0, x(t_f) =x_f, t \in [0,t_f]$ for
the underactuated system is equivalent to the problem of finding a
curve $y(t)$ satisfying constraints on $y(0), \dot{y}(0), \ldots,
y^{(p)}(0)$ and $y(t_f), \dot{y}(t_f), \ldots, y^{(p)}(t_f)$ specified
by $x_0$ and $x_f$.  In other words, the problem of finding 
a trajectory satisfying the underactuation constraints
%control inputs to satisfy a set of differential equations 
becomes the relatively
simple algebraic problem of finding a curve to fit the start and end
constraints.  Any curve $y(t)$ 
maps directly to a consistent pair of state and control histories $x(t)$ and $u(t)$.
%directly maps to the state and control histories $x(t)$ and $u(t)$.

The flat outputs for dynamic systems are often a function of
configuration variables only, and sometimes are just the location of
particular points on the system.  Unfortunately, there is no
systematic way to determine if a system is differentially flat, or
what the flat outputs for a system are.  Many important systems have
been shown to be differentially flat, however.  These systems include
the PBWT, chained-form systems such as the unicycle, and some models 
of cars and tractor-trailer systems.

\begin{example}
{\em
{\em Unicycle (cont.)\/}
The flat outputs for the unicycle are $y_1 = x_1$ and $y_2 = x_2$.  The state $x$
and controls $u$ can be derived from the flat outputs and their derivatives as
follows:
\begin{eqnarray}
(x_1,x_2,x_3) & = & \left(y_1,y_2,\tan^{-1} \frac{\dot{y}_2}{\dot{y}_1}\right) \label{eq:stateflat} \\
(u_1,u_2) & = & \left(\sqrt{(\dot{y}_1)^2 + (\dot{y}_2)^2}, \frac{\dot{y}_1 \ddot{y}_2 - \ddot{y}_1 \dot{y}_2}{(\dot{y}_1)^2 + (\dot{y}_2)^2}\right). \label{eq:controlflat}
\end{eqnarray}
The orientation $x_3$ is not well defined as a function of the flat outputs 
when the unicycle is not translating.
The sign 
of $u_1$ should be
consistent with the angle $x_3$.

Now we would like to find a feasible trajectory from $x_0 = (0,0,0)^T$
to $x_f = (1,1,0)^T$.  Since there are six state variables in the specification
of the start and goal points, there are six constraints on the flat outputs
$y$ and their derivatives at the beginning and end of motion.  These constraints
can be written
\begin{equation}
\begin{array}{ll}
y_1(0) = 0 & \;\;\; y_2(0) = 0 \\
y_1(t_f) = 1 & \;\;\; y_2(t_f) = 1 \\
& \;\;\; \dot{y}_2(0) = 0 \\
& \;\;\; \dot{y}_2(t_f) = 0
\end{array}
\nonumber
\end{equation}
where the last two constraints indicate that the initial and final
motion of the unicycle must be along the $x$-axis, indicating that the
wheel is oriented with the $x$-axis.  The simplest polynomial
functions of time that have enough free coefficients to satisfy these constraints are
\begin{eqnarray}
y_1(t) & = & a_0 + a_1 t \nonumber \\
y_2(t) & = & b_0 + b_1 t + b_2 t^2 + b_3 t^3. \nonumber
\end{eqnarray}
Setting the time of motion $t_f = 1$ and using the constraints to solve for
the polynomial coefficients, we get
\begin{eqnarray}
y_1(t) & = & t \nonumber \\ 
y_2(t) & = & 3 t^2 - 2 t^3. \nonumber
\end{eqnarray}
The state $x(t)$ and control $u(t)$ can be obtained from Equations~\eqref{eq:stateflat}
and \eqref{eq:controlflat}.
The unicycle path for this trajectory is shown in Figure~\ref{fig:unicycleflat}.
}
\end{example}

\begin{figure}
\begin{center}
\includegraphics{../figure/book/nonholonomic/unicycleflat.ps}
\end{center}
\caption{A feasible path for the unicycle from $(0,0,0)^T$ to $(1,1,0)^T$.}
\label{fig:unicycleflat}
\end{figure}

In fitting a curve $y(t)$, we must choose a family of curves with enough
degrees-of-freedom to satisfy the initial and terminal constraints.  We may
choose a family of curves with more degrees-of-freedom, however, and use
the extra degrees-of-freedom to (1) 
satisfy control bounds on $u(t)$, (2) avoid obstacles in the configuration
space, and/or (3) minimize a cost function.  Incorporating these conditions in
the calculation of $y(t)$ typically requires resorting to numerical optimization
methods, and is a topic of current research.  
A good way to generate an initial guess for the optimization is to 
solve exactly for a minimal number of coefficients to satisfy the initial and
terminal constraints, setting the other coefficients to zero.

For the PBWT, the flat outputs are
\begin{eqnarray}
y_1 & = & x_1 + \frac{1}{d} \cos x_3 \nonumber \\
y_2 & = & x_2 + \frac{1}{d} \sin x_3. \nonumber
\end{eqnarray}
$(y_1,y_2)$ is a fixed point on the PBWT, at the PBWT's center of percussion
or center of oscillation with respect to the location of the thrusters.
If the PBWT is initially at rest, the center of percussion is the instantaneously
unaccelerated point for nonzero thrust $u_2$.

For a car with steerable front wheels and parallel, fixed-orientation rear wheels,
the flat outputs are the Cartesian coordinates of the point halfway between the
rear wheels.  If the car is towing a two-wheel trailer hitched midway between the rear
wheels, the flat outputs are the coordinates midway between the wheels of the trailer.
If there are more trailers, all hitched midway between the wheels of the trailer
in front, the coordinates of the midpoint of the wheels of the last trailer are
flat outputs for the entire system.  The orientation of the car and each trailer
can be determined from sufficiently high time-derivatives of the evolution of these
two coordinates.  All chained-form systems are flat with the first
and last states $x_1$ and $x_n$, as the flat outputs.

The paper by Martin~{\em et al.\/}~\cite{Martin97}
provides a catalog of systems
known to be flat.  Some notable results include the following.  A
system of the form~\eqref{eq:controlaffine} with $n$ states and
$m=n-1$ inputs is flat if it is STLC.  A driftless
system~\eqref{eq:controlaffine} with $n$ states and $m=n-2$ inputs is
flat if it is STLC. 

Cite also the Murray reference~\cite{Murray95b} and the Agrawal book.

\subsection{Cars and Cars Pulling Trailers}

From a motion planning perspective, easily the most heavily studied
examples of nonholonomic systems are the kinematic car and the car
pulling one or more trailers.  Because of the obvious applications
of automatic motion planning to systems like these, a great deal of
effort has been spent to derive efficient and complete motion planners
for these systems moving in cluttered environments.  The excellent
book edited by Laumond~\cite{Laumond98} is focused entirely on these systems.
In this section, we provide a brief review of important concepts and
approaches to motion planning for cars and cars pulling trailers.

\subsubsection{Cars}

We focus on driftless kinematic models of cars, where the inputs
are velocities.  Alternatively, we could consider dynamic extensions
of these models, where the inputs are accelerations.

A kinematic model of a standard car is shown in Figure~\ref{fig:car}.
The location of the point midway between the rear wheels is
$(x_1,x_2)$, the orientation of the car is $x_3$, and the steering
angle is $x_4$.  To ensure
that the wheels do not slip,  each of the front wheels must be perpendicular to the
line through the wheel and the rotation center of the car. 
Therefore, $x_4$ is measured at a
``virtual'' wheel midway between the two front wheels.  The wheelbase
is $l$.

\begin{figure}
\begin{center}
\includegraphics{../figure/book/nonholonomic/car.ps}
\end{center}
\caption{A model for a car turning at its minimum turning radius.}
\label{fig:car}
\end{figure}

If the control $u_2$ is the rate of change of the steering angle
$x_4$ and $u_1$ is the driving speed of the car, measured at the 
point midway between the rear wheels, the control system can
be written
\[
\dot{x} = g_1(x) u_1 + g_2(x) u_2, \;\;
g_1(x) = \left(\begin{array}{c}\cos x_3 \\ \sin x_3 \\ \frac{1}{l} \tan x_4 \\ 0\end{array}\right), \;\;
g_2(x) = \left(\begin{array}{c}0 \\ 0 \\ 0\\ 1\end{array}\right).
\]
Typically the steering angle is limited to
$-\gamma < x_4 < \gamma, \gamma \in (0,\pi/2)$
giving the car a minimum turning radius $r_{min}= l/\tan^{-1} \gamma$.  
The control set for this system is ${\cal U}_{\pm}$, and this symmetric system
can be shown to be STLC by the LARC.  
Since the car is STLC at all configurations, the steering angle limits
do not affect the reachable positions and orientations of
the car.  Interestingly, this means that a car that cannot turn right,
for instance, can reach any position and orientation among obstacles
that a fully functional car can.  

Since we are primarily concerned with the position and orientation
of the body of the car, we could decide to eliminate the steering
angle $x_4$ from the representation of the configuration, and treat
the steering angle as part of the control.  Consider the modified control
inputs $(v,\omega)$, where $v$ is the linear speed of the car and
$\omega$ is the angular velocity of the body of the car.
In this case, the control system becomes
\begin{equation}
\left(\begin{array}{c} \dot{x}_1 \\ \dot{x}_2 \\ \dot{x}_3 \end{array} \right) =
\left(\begin{array}{c} \cos x_3 \\ \sin x_3 \\ 0\end{array} \right) v +
\left(\begin{array}{c} 0 \\ 0 \\ 1\end{array} \right) \omega,
\label{eq:carmodel}
\end{equation}
which is identical to the unicycle, except that the control set satisfies
the turning radius constraint $|\omega| < |v/r_{min}|$.  This does not affect 
the symmetry of the system, however, so it is still STLC.

As with the unicycle, if the car is limited to driving forward only,
then it is globally controllable (in the absence of obstacles) but not STLC.  
In this section we will focus only on STLC models of cars.

Another car model of interest is the \emph{differential-drive} car.
In this case, the front wheels are replaced by a casters which roll
freely in any direction.  The rear wheels are parallel and their
speeds are controlled independently.  If the speeds of the two rear
wheels are $u_1$ and $u_2$, and the configuration of the car
is $(x_1,x_2,x_3)^T$ as in Figure~\ref{fig:car}, the control
system can be written
\[
\left(\begin{array}{c} \dot{x}_1 \\ \dot{x}_2 \\ \dot{x}_3 \end{array} \right) =
\left(\begin{array}{c} \frac{1}{2} \cos x_3 \\ \frac{1}{2} \sin x_3 \\ \frac{1}{l} \end{array}\right) u_1 +
\left(\begin{array}{c} \frac{1}{2} \cos x_3 \\ \frac{1}{2} \sin x_3 \\ \frac{-1}{l} \end{array}\right) u_2.
\]
With the input transformation $v = (u_1+u_2)/2$ and $\omega = (u_1-u_2)/l$,
the system again becomes the unicycle.  The major difference with
the standard car model is the lack of a turning radius constraint.

here:  STLC for these two models can be interpreted using the 
sideways maneuver.

STLC for these two car models implies the following important
consequence:  if there is a free path from $q_0$ to $q_g$
for the car body moving
\emph{without any nonholonomic motion constraint}, i.e., moving
as a free-flying planar body, and if there is an open set of 
free space about each configuration in the path, then there is
a free path for the car.  Stated equivalently, if ${\Cs}_{free}$
is connected and open so that every $q \in {\Cs}_{free}$ 
has a neighborhood of free space, then there exists a path for
the car from any $q_0 \in {\Cs}_{free}$ to any $q_g \in {\Cs}_{free}$.
This implies that it is possible to parallel park your car into
any parking space $\epsilon >0$ longer than
your car.  However, the number of direction reversals
in your parking maneuver grows proportionally to $1/\epsilon^2$~\cite{Laumond98},
so you could be there a while when $\epsilon$ is small!

Let's turn our focus to the simplified standard car 
model~\eqref{eq:carmodel}, with a
limited turning radius.  One question that has received considerable
attention is the following: Given a start and goal configuration for
the car moving in an obstacle-free space, find the path that minimizes
the arclength traveled by the point midway between the rear wheels.
If we assume a bound on the linear velocity $|v| < v_{max}$, and no
acceleration constraints, then
this path also corresponds to the minimum-time motion.
This problem has been solved due in large part to contributions
by Reeds and Shepp~\cite{Reeds90}, Sussmann and
Tang~\cite{Sussmann91b}, and Sou\`{e}res and Laumond~\cite{Soueres96}.
See also the chapter by Sou\`{e}res and Boissonat~\cite{Soueres98}.

Reeds and Shepp~\cite{Reeds90} showed that the optimal path
must be one of a discrete and computable set of paths.  It turns out that 
each member
of this set consists of a concatenation of straight line segments
and circular arcs at the car's minimum turning radius.  If $C$
indicates a circular arc segment, $S$ indicates a straight
line segment and $|$ indicates a cusp in the motion where 
the linear velocity $v$ changes sign, the optimal path is guaranteed to be contained
in the following set of path types:
\begin{eqnarray}
& & \{ C|C|C, \;\; CC|C, \;\; C|CC, \;\; CC_a|C_aC, \;\; C|C_aC_a|C, \nonumber \\
& & C|C_{\pi/2}SC, \;\; CSC_{\pi/2}|C, \;\; C|C_{\pi/2}SC_{\pi/2}|C, \;\; CSC \}. \nonumber
\end{eqnarray}
The subscript $a$ indicates an arc of angle $a$.  One or more
of the segments may be zero length.

could mention parallel-parking primitive, note we can get sideways
motion for any ball of free space.  could also show the ``metric''
showing reachable points at same time.  This is useful for intuition
for the reader.


One problem that has received particular attention is finding
paths which minimize the arclength of the point between the rear
wheels of the car.

now Reeds-Shepp, then maybe Balkcom



note after this that can go anywhere body with no motion constraints
can go.


note control limits don't change controllability results (except
in case of forward only), but change planners!

model for car and diff-drive car, could include angles of the wheels,
but most motion planners don't bother, using only kinematics

Reeds-Shepp, approaches based on it, maybe Mason/Balkcom

\paragraph{Grid search}
  Barraquand and Latombe (optimal)
 good thing about it is it's straightforward, easy to understand; and
finds optimal trajectories; negatives might be computational complexity,
not an exact solution
 
\paragraph{Holonomic to nonholonomic path transformation}
approaches:

  Laumond TRA planner (mention Jacobs and Canny)

\paragraph{Randomized methods}

  Any PRM approach, Svestka

\paragraph{Smoothing}

could also mention smoothing

\subsubsection{Cars Pulling Trailers}
models
can convert to chained form

approaches:
  sinusoids, and drift-free approach above, flatness (position of center
    of last wheels)
  Barraquand and Latombe
  RPP (according to Laumond, p. 35)
  successive constraints
  
see Laumond book, get paper by Balkcom

include trailers, successive constraints, PRM's with Reeds-Shepp

\subsection{Kinematically Controllable Systems}

\subsection{Rapidly Exploring Random Trees}

requires very little system structure, just a decent distance metric,
which depends on motion constraints

\subsection{Laferriere and Sussmann}

but probably too much math involved, see Murray book

\section*{Problems}

\begin{enumerate}

%\item In the knife-edge example of
%Example~\ref{example:knife-edgecontrol}, show that 
%$\symclos(\{Y_1,Y_2\})$ spans the tangent space at all $q$.
%\label{problem:knife-edge}

\item waiting

\end{enumerate}

%\begin{quote}
%{\em Exercise.\/} Find the motion constraint $\omega(x) \dot{x} = 0$
%describing the prohibited acceleration for the PBWT
%in zero gravity ($a_g=0$).
%\end{quote}

for a disk rolling on a plane, can write holonomic and nonholonomic constraints.

\begin{quote}
{\em Exercise.\/}  Using a prop to mimic the unicycle, translate the unicycle
forward ($g_1$) ten centimeters, rotate CCW ($g_2$) approximately 20 degrees, translate 
backward ($-g_1$) ten centimeters, and rotate 
back to its original orientation ($-g_2$).  This is an approximation to the Lie bracket
motion.  Why is the net translation of the unicycle not exactly sideways (the
direction of the Lie bracket)?  See Figure~\ref{fig:liebracket}.
\end{quote}

write holonomic and nonholonomic (Pfaffian)
constraints for unicycle, and on a line

derive Lie bracket expression \eqref{eq:liebrackettaylorexpansion}

for the hopper in \ref{sec:mechanicalsystems}, which are the shape variables
and which are the fiber variables?

a kinematic reduction of the hopper treats the leg extension
velocity and angular velocity as inputs, and the Pfaffian constraint
is conservation of angular momentum (zero in this case).  Write the 
driftless kinematic model.

for the problem above, put it in chained form.

could include chained form for tractor trailer

include wheel angles in diff-drive car model, write control system,
see if it's STLC.

write car models as dynamic models (controlling accelerations),
determine if STLC

all chained-form systems are flat with flat outputs being the
1st and last states, $x_1$ and $x_n$.  Prove it, and give
$\phi$ and $\psi$.

