A path specifies the set of configurations a robot achieves as it
moves from one configuration to another, and path planning (e.g.,
finding collision-free paths) is a kinematic/geometric problem.  A
path is not a complete description of the motion of a robot system,
however, as the timing of the motion is not specified.  A {\bf
trajectory} is a path {\em plus\/} a specification of the time at
which each configuration is achieved.  Trajectory planning is not only
a geometric problem, but also a dynamic problem.  Finding feasible
trajectories of a system obeying dynamics requires knowledge of the
masses and inertias of the system, actuator limits, and forces such as
gravity and friction.  Since we are now dealing with system dynamics,
we can pose optimal control problems such as finding 
minimum-energy motions.  Trajectory planning is sometimes called {\em
%kinodynamic planning.}

Since trajectory planning requires a full dynamic model of the robotic
system, in this chapter we review the Lagrangian method for deriving
equations of motion.  We then study two approaches to
trajectory planning for a dynamic system: the decoupled approach,
which involves first searching for a path in the configuration space and
then finding a time scaling for the path subject to the actuator
limits; and the direct approach, where the search takes place in the
system's state space.  Examples of the latter approach include optimal
control and numerical optimization, grid-based searches, and randomized
methods. %, and potential fields.  
In this chapter we focus on {\em fully
actuated\/} systems---systems with an actuator for each
degree-of-freedom.

In Section~\ref{sec:prelims} we review some definitions used 
throughout the chapter.  In Section~\ref{sec:lagrange} we review
the Lagrangian approach to deriving equations of motion for a
mechanical system.  This section includes a brief detour into the
dynamics of a rotating three-dimensional rigid body, which are not usually derived
using a Lagrangian formulation.  In Section~\ref{sec:decoupled}
we describe an algorithm for finding the time-optimal time scaling
for a path subject to actuator limits, and describe how this can
be used in a decoupled trajectory planner.  In Section~\ref{sec:direct},
we outline several approaches to trajectory planning directly in
the system state space, including optimal control, gradient-based 
numerical methods, and dynamic programming.  Other methods, such
as rapidly exploring random trees and potential fields, are
described in separate chapters.

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

Recall that a {\bf path} $q(s)$ is a smooth curve parameterized by $s \in [0,1]$ on
the configuration space ${\cal Q}$, $q : [0,1] \rightarrow {\cal Q}$.
For simplicity, we will assume that a path never backtracks as $s$
increases monotonically.  The path is collision-free if $q(s) \in
{\cal Q}_{free}$ for all $s \in [0,1]$.  A {\bf time scaling} $s(t)$
is a map $s : [0,t_f]
\rightarrow [0,1]$, assigning an $s$ value to each time $t \in
[0,t_f]$.  Together, a path and a time scaling specify a trajectory
$q(s(t))$, or $q(t)$ for short.  $s(t)$ should be twice-differentiable
with $\dot{s}(t) >0$ for all $t \in (0,t_f)$.
% could define $T{\cal Q}$, state

\section{Lagrangian Dynamics}
\label{sec:lagrange}

The equations of motion for a mechanical system 
%relating a set of control forces and torques to the motion of the system 
can be generated in
a variety of ways.  While all are equivalent, the number of computations
and the size of the resulting expressions (before simplification) may vary.
% Hamiltonian, Newton-Euler, Kane
In this chapter we use a Lagrangian formulation, which is based on the
kinetic and potential energy of the system.  Lagrange's equations
provide a straightforward recipe,
amenable to computer implementation (using, e.g., Mathematica or Maple),
for calculating equations of motion.


Let $q = (q_1, \ldots, q_{n_{\cal Q}})^T \in \real^{n_{\cal Q}}$ be the vector of {\bf
generalized coordinates} representing the configuration of the system
on the $n_{\cal Q}$-dimensional configuration space, and let $\force =
(\force_1,\ldots,\force_{n_{\cal Q}})^T \in \real^{n_{\cal Q}}$ be the vector of external {\bf generalized
forces} acting on the generalized coordinates.  For example, for a
robot arm, the generalized coordinates would typically be the joint
angles for revolute joints and the joint translations for prismatic
joints, and the generalized forces would be torques about the joints
or forces along the joints, respectively.

The {\bf Lagrangian} $L$ of a mechanical system is written as the
kinetic energy minus the potential energy
\[
L(q,\dot{q}) = K(q,\dot{q}) - V(q),
\]
where $K$ is the kinetic energy, a function of the configuration and
the velocity, and $V$ is the potential energy, a function of the 
configuration only.  With this, the Lagrangian equations of motion 
can be written
\[
\frac{d}{dt}\frac{\partial L}{\partial \dot{q}_i} - \frac{\partial L}{\partial q_i} = \force_i,
\;\; i = 1\ldots n_{\cal Q},
\]
or simply
\begin{equation}
\frac{d}{dt}\frac{\partial L}{\partial \dot{q}} - \frac{\partial L}{\partial q} = \force.
\label{eq:lagrange}
\end{equation}

\begin{example}
{\em 
Consider a planar body described by the generalized coordinates $q =
(q_1,q_2,q_3)^T \in \real^2 \times S^1$, where $(q_1, q_2) \in \real^2$
specify the location of the center of mass of the body in the plane
and $q_3 \in S^1$ specifies the orientation of the body
(Figure~\ref{fig:planarbody}).  The generalized forces $\force =
(\force_1, \force_2,\force_3)^T \in \real^3$ are the linear forces
$(\force_1,\force_2)$ through the center of mass and the torque $\force_3$ about the
center of mass.  A gravitational acceleration $a_g \geq 0$ acts in the $-q_2$ direction,
$(0,-a_g,0)^T$.
The mass of the planar body is $m$ and $I$ is the scalar inertia
about an axis through the center of mass and out of the page.

The Lagrangian for this system is 
\[
L = \frac{1}{2} m(\dot{q}_1^2 + \dot{q}_2^2) + \frac{1}{2} I \dot{q}_3^2 - ma_g q_2,
\]
that is, the sum of the linear and angular kinetic energies minus the potential
energy.  Applying \eqref{eq:lagrange}, we get
\begin{eqnarray}
m \ddot{q}_1 & = & \force_1 \\
m \ddot{q}_2 - ma_g & = & \force_2 \\
I \ddot{q}_3 & = & \force_3.
\end{eqnarray} 
}
\label{ex:planarbody}
\end{example}

\begin{figure}
\begin{center}
\includegraphics{../figure/book/trajectory/planarbody}
\end{center}
\caption{A planar body.}
\label{fig:planarbody}
\end{figure}

\begin{figure}
\begin{center}
\includegraphics{../figure/book/trajectory/RParm}
\end{center}
\caption{The RP manipulator.}
\label{fig:RParm}
\end{figure}

\begin{example}
{\em Figure~\ref{fig:RParm} shows a robot arm consisting of one
revolute joint and one prismatic (translational) joint.  This type of
robot is called an RP manipulator, for \emph{r}evolute \emph{p}rismatic.  The
configuration of the robot is $(q_1,q_2)^T$, where $q_1$ gives the
angle of the first joint from a world frame $x_1$-axis, and $q_2 > 0$
gives the distance of the center of mass of the second link from the
first joint.  The center of mass of the first link is a distance $r_1$
from the first joint.  The first link has mass $m_1$ and
inertia $I_1$ about the center of mass, and the second
link has mass $m_2$ with inertia $I_2$ about the center of mass.  A
gravitational acceleration $a_g \geq 0$ acts in the $-x_2$ direction of a
world frame.  To derive the Lagrangian for this RP arm, we will
consider the two links' contributions independently.

The kinetic energy of the first link can be expressed as
\[
K_1(q,\dot{q}) = \frac{1}{2} m_1 v_1^2 + \frac{1}{2} I_1 \omega_1^2,
\]
where $v_1$ and $\omega_1$ are the linear velocity of the link
center of mass and angular velocity of the link, respectively.
We have
\begin{eqnarray}
v_1 & = & r_1 \dot{q}_1 \nonumber \\
\omega_1 & = & \dot{q}_1 \nonumber
\end{eqnarray}
yielding the expression
\[
K_1(q,\dot{q}) = \frac{1}{2} m_1 r_1^2 \dot{q}_1^2 + \frac{1}{2} I_1 \dot{q}_1^2.
\]
The potential energy of the first link is
\[
V_1(q) = m_1 a_g r_1 \sin q_1.
\]

The kinetic energy of the second link can be expressed as
\[
K_2(q,\dot{q}) = \frac{1}{2} m_2 v_2^2 + \frac{1}{2} I_2 \omega_2^2,
\]
where $v_2$ and $\omega_2$ are the linear velocity of the link
center of mass and angular velocity of the link, respectively.
We have
\begin{eqnarray}
v_2 & = & \sqrt{\dot{q}_2^2 + (q_2\dot{q}_1)^2} \nonumber \\
\omega_2 & = & \dot{q}_1 \nonumber
\end{eqnarray}
yielding
\[
K_2(q,\dot{q}) = \frac{1}{2} m_2 (\dot{q}_2^2 + (q_2\dot{q}_1)^2) + \frac{1}{2} I_2 \dot{q}_1^2.
\]
The potential energy of the second link is
\[
V_2(q) = m_2 a_g q_2 \sin q_1.
\]

Using the kinetic and potential energies of the two links,
the Lagrangian for the system is $L = K_1+K_2-V_1-V_2$:
\[
L = \frac{1}{2} ((I_1 +I_2 + m_1 r_1^2 + m_2 q_2^2) \dot{q}_1^2 + m_2 \dot{q}_2^2) - a_g \sin q_1 (m_1 r_1 + m_2 q_2).
\]
Applying Lagrange's equations, we get
\begin{eqnarray}
\force_1 & = & (I_1 + I_2 + m_1 r_1^2 + m_2 q_2^2) \ddot{q}_1 + 2m_2 q_2 \dot{q}_1 \dot{q}_2  \nonumber \\
& & + a_g(m_1 r_1 + m_2 q_2) \cos q_1 \label{eq:maniptau1} \\
\force_2 & = & m_2 \ddot{q}_2 - m_2 q_2 \dot{q}_1^2 + a_g m_2 \sin q_1. \label{eq:maniptau2}
\end{eqnarray}
\label{ex:RParm}
}
\end{example}

\subsection{Standard Forms for Dynamics}
\label{ssec:standardforms}

Let's take a closer look at Equations (\ref{eq:maniptau1}) and 
(\ref{eq:maniptau2}), since they display much of the interesting structure
of many second-order mechanical systems.  On the right-hand side of
each equation, there is a term depending on the second derivatives of
the configuration variables, a term quadratic in the first derivatives
of the configuration variables, and a
term depending only on the configuration variables.  
%It will be the same for all mechanical systems we will study.  
These terms can be
collected to write the dynamics in the following standard form:
\begin{equation}
\force = M(q)\ddot{q} + C(q,\dot{q})\dot{q} + g(q),
\label{eq:robotcoriolis}
\end{equation}
where $M(q) \in \real^{n_{\cal Q}\times n_{\cal Q}}$ is a symmetric, positive definite
{\bf mass} or {\bf inertia matrix}, $C(q,\dot{q})\dot{q} \in \real^{n_{\cal Q}}$ is a vector of velocity
product forces with $C(q,\dot{q}) \in \real^{n_{\cal Q} \times n_{\cal Q}}$
linear in $\dot{q}$, and 
$g(q) \in \real^{n_{\cal Q}}$ is a vector of gravitational forces.  
Equations (\ref{eq:maniptau1}) and (\ref{eq:maniptau2}) can be
written in the form (\ref{eq:robotcoriolis}), where
\begin{equation}
M(q) = \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), 
\nonumber
\end{equation}
\begin{equation}
C(q,\dot{q})\dot{q} = \left(
\begin{array}{c}
2 m_2 q_2 \dot{q}_1 \dot{q}_2 \\
-m_2 q_2\dot{q}_1^2
\end{array}
\right), \;\;
g(q) = \left(
\begin{array}{c}
a_g(m_1 r_1 + m_2 q_2) \cos q_1 \\
a_g m_2 \sin q_1
\end{array}
\right).
\nonumber
\end{equation}
The form (\ref{eq:robotcoriolis}) is compact, but the term
$C(q,\dot{q})\dot{q}$ masks the structure of the velocity product terms.
If we consider the individual components of this vector,
$C(q,\dot{q})\dot{q} = (c_1(q,\dot{q}),\ldots,c_{n_{\cal Q}}(q,\dot{q}))^T$,
we find that
\begin{equation}
c_i(q,\dot{q}) = \sum_{j=1}^{n_{\cal Q}} (\sum_{k=1}^{n_{\cal Q}} \Gamma^i_{jk}(q) \dot{q}_j \dot{q}_k)
\label{eq:coriolistochristoffel}
\end{equation}
where 
\begin{equation}
\Gamma^i_{jk}(q) = \frac{1}{2} \left(\frac{\partial M_{ij}(q)}{\partial q_k} + \frac{\partial M_{ik}(q)}{\partial q_j} - \frac{\partial M_{kj}(q)}{\partial q_i}\right)
\end{equation}
and $M_{ij}(q)$ is the entry in the $i$th row and $j$th column of $M(q)$.
The $n^3_{\cal Q}$ scalars $\Gamma^i_{jk}(q)$ are
known as the {\bf Christoffel symbols} of the inertia matrix 
$M(q)$.  
In \eqref{eq:coriolistochristoffel}, squared velocity terms (where $j=k$)
are known as {\bf centrifugal} terms, and velocity product terms
where $j\neq k$ are known as {\bf Coriolis} terms.  For example, the
centrifugal term $c_2(q,\dot{q})$ indicates that keeping the prismatic
joint stationary as the revolute joint rotates requires the linear actuator at
the prismatic joint to apply a force.  The Coriolis term $c_1(q,\dot{q})$
indicates that the actuator at the revolute joint must apply a torque
to keep the two joints moving at a constant velocity.  This is because
the inertia of the robot about the first joint is changing during the
motion, meaning that the angular momentum is changing.

Velocity product terms arise due to the non-inertial reference
frames implicit in the generalized coordinates $q$.  The {\em geodesic\/}
motions of the system---how it naturally moves when
$\force - g(q) =0$---are 
not ``straight lines'' in this choice of coordinates, and the
Christoffel symbols carry geometric information on how the geodesics
``bend'' in this choice of coordinates.
%The Christoffel
%symbols carry geometric information on how the configuration space
%``bends'' and ``twists'' in this choice of coordinates, where the
%bending and twisting defines the ``straight line'' {\em geodesic\/}
%motions of the system --- how it naturally moves when $\force=0$.  
When we wish to emphasize the dependence of the velocity product terms on
the Christoffel symbols, which in turn are determined by the inertia
matrix, we write
\begin{equation}
\force = M(q)\ddot{q} + \dot{q}^T \Gamma(q) \dot{q} + g(q),
\label{eq:robotchristoffel}
\end{equation}
where $\Gamma(q) \in \real^{n_{\cal Q}\times n_{\cal Q} \times n_{\cal Q}}$.  The term 
$\dot{q}^T \Gamma(q) \dot{q}$ can be calculated as shown in
\eqref{eq:coriolistochristoffel}.  Conceptually, $\Gamma(q)$
can be viewed as an $n_{\cal Q}$-dimensional column vector, where each element 
of the vector is $\Gamma^{i}(q)$, an $n_{\cal Q} \times n_{\cal Q}$ 
symmetric matrix with elements
$\Gamma^{i}_{jk}, j,k = 1\ldots n_{\cal Q}$.  The $n_{\cal Q}$ scalars 
$\dot{q}^T \Gamma^{i}(q) \dot{q}$ then make up the final column vector
of centrifugal and Coriolis terms,
as shown in the following example.

\begin{example}
{\em 
For the RP arm of Example~\ref{ex:RParm}, there are $n_{\cal Q}^3 = 8$
Christoffel symbols.  The only nonzero Christoffel symbols are
$\Gamma^{1}_{12} = \Gamma^{1}_{21} = m_2 q_2$ and $\Gamma^{2}_{11} = - m_2 q_2$.
The Coriolis and centrifugal terms $\dot{q}^T \Gamma(q) \dot{q}$ can be 
calculated as follows:
\begin{eqnarray}
\dot{q}^T \Gamma(q) \dot{q} & = & (\dot{q}_1 \;\; \dot{q}_2) 
\left(\begin{array}{c}  \Gamma^1(q) \\
\Gamma^2(q) \end{array} \right) 
\left( \begin{array}{c} \dot{q}_1 \\ \dot{q}_2 \end{array} \right) \nonumber \\
& = & (\dot{q}_1 \;\; \dot{q}_2)
\left(\begin{array}{c} \left[\begin{array}{cc} 0 & m_2 q_2 \\ m_2 q_2 & 0 \end{array}
\right] \\
\left[\begin{array}{cc} -m_2 q_2 & 0 \\ 0 & 0 \end{array} \right] \end{array} \right)
\left(\begin{array}{c} \dot{q}_1 \\ \dot{q}_2 \end{array} \right) \nonumber \\
& = & \left( \begin{array}{c} 2 m_2 q_2 \dot{q}_1 \dot{q}_2 \\
-m_2 q_2 \dot{q}_1^2 \end{array} \right). \nonumber
\end{eqnarray}
}
\end{example}

The dynamics (\ref{eq:robotcoriolis}) are specific to
mechanical systems where the actuators act directly on the
generalized coordinates.  For example, a robot arm typically
has an actuator at each joint.  A more general form of the
dynamics of second-order mechanical systems is
\begin{equation}
T(q) f = M(q)\ddot{q} + C(q,\dot{q})\dot{q} + g(q),
\label{eq:generalcoriolis}
\end{equation}
where $f$ are the actuator forces and 
$T(q)$ specifies how the actuators act on the generalized
coordinates, as a function of the system configuration.  As an example,
consider replacing the motors at the joints of our
two-joint RP arm of Example~\ref{ex:RParm} with two thrusters attached
to the center of mass of the second link.  These thrusters
provide a force $f = (f_1,f_2)^T$ expressed in the world $(x_1,x_2)$ frame.
If $\force$ are the generalized forces at the joints as before, then
\begin{eqnarray}
\force & = & T(q) f \nonumber \\
\left( \begin{array}{c} \force_1 \\ \force_2 \end{array} \right) & = &
\left( \begin{array}{cc} -q_2 \sin q_1 & q_2 \cos q_1 \\
\cos q_1 & \sin q_1 \end{array} \right)
\left( \begin{array}{c} f_1 \\ f_2 \end{array} \right).
\end{eqnarray}
In this example, $T(q)$ is simply the transpose of the manipulator
{\bf Jacobian} at the center of mass of the second link.  
The manipulator Jacobian is used in robot kinematics and dynamics,
and details can be found in many robotics textbooks.

If $T(q)$ is full rank, dynamics of the form (\ref{eq:generalcoriolis})
can be put in the form (\ref{eq:robotcoriolis}) by 
%pre-multiplying both
%sides of \eqref{eq:generalcoriolis} by $T^{-1}(q)$. 
defining ``virtual''
actuators $\force = T(q) f$, and transforming any actuator limits
on $f$ to limits on $\force$.  This is sometimes
called a ``feedback transformation'' since the transformation from
$f$ to $\force$ depends on $q$.

Finally, mechanical systems are often
subject to dissipative forces such as dry Coulomb friction or viscous
damping.  After deriving the equations of motion ignoring these
forces, they can be added to the dynamics at
the end.  The simplest dissipation model is linear damping or viscous
friction, which produces a generalized force proportional to $\dot{q}$
resisting the motion.  In this case, the equations of motion 
can be modified to
\[
\force = M(q)\ddot{q} + C(q,\dot{q})\dot{q} + g(q) + B(q) \dot{q},
\]
where $B(q)$ is a matrix of damping coefficients.

%\begin{example}
%A tumbling rigid body has a diagonal inertia matrix 
%\[
%\mathbb{I} = \left(\begin{array}{ccc}
%I_{xx} & 0 & 0 \\
%0 & I_{yy} & 0 \\
%0 & 0 & I_{zz}
%\end{array}
%\right)
%\]
%when measured in a body-fixed frame aligned with the principal
%axes of inertia and located at the center of mass.  The Lagrangian
%of the tumbling body is given by its kinetic energy
%\[
%L = \frac{1}{2} \omega^T \mathbb{I} \omega = \frac{1}{2}(I_{xx}\omega_x^2 + I_{yy} \omega_y^2 + I_{zz} \omega_z^2),
%\]
%where $\omega^T = (\omega_x,\omega_y,\omega_z)$ is the angular velocity
%of the body about the principal axes.  Let $\force_1, \force_2, \force_3$ be
%the torques applied about these axes, respectively.  Applying Lagrange's
%equations, we get
%\begin{eqnarray}
%\force_1 & = & 
%\end{eqnarray}
%
%newton-euler equations from the kinetic energy, use mass matrix $\mathbb{M}$ and
%coordinates $q$
%\end{example}

\paragraph{Inertia Matrix}

As we have seen, the inertia matrix $M(q)$ determines the
equations of motion, except for gravitational and dissipation forces.
Another way to see this is by observing that
the kinetic energy of a mechanical system is determined by its
inertia matrix, and can be written
\begin{equation}
K(q,\dot{q}) = \frac{1}{2} \dot{q}^T M(q) \dot{q}.
\label{eq:KEfrominertia}
\end{equation}
In some cases, such as the planar body of Example~\ref{ex:planarbody},
the inertia matrix can be written independent of the configuration $q$
and the Christoffel symbols become zero.  This means that the dynamics
are \emph{invariant} to the configuration---they ``look'' the same from
any configuration.  For the planar body,
the inertia matrix is
\[
M(q) = M = \left(\begin{array}{ccc} m & 0 & 0 \\ 0 & m & 0 \\ 0 & 0 & I
\end{array}\right).
\]

For some robots, such as a mobile manipulator consisting of a
robot arm mounted on a cart, 
the dynamics are invariant to some configuration variables (such as
the cart's position and orientation on the floor) but not others
(such as the arm's configuration).

\subsection{Dynamics of a Rotating Rigid Body}

Until now, we have been using the concepts of ``center of mass'' of a body
and ``inertia about an axis'' in order to get to the use of Lagrange's equations
as quickly as possible.  We will now provide formal definitions of
these quantities.  

Let ${\cal B}$ be a rigid body occupying a volume $V \subset \real^3$, and
let $r$ be a vector
from the origin to a point in ${\cal B}$.  The mass density of ${\cal B}$
as a function of the location $r$ is defined by $\rho(r)$.  Then the
mass of ${\cal B}$ is the volume integral of the mass density
\[
m = \int_V \rho(r) dV,
\]
and the {\bf center of mass} is the weighted average of the mass density
\[
r_{cm} = \frac{1}{m} \int_V r \rho(r) dV.
\]
When a body moves freely in space, it is convenient to express its
dynamics in a coordinate frame attached to its center of mass.  
%When it is constrained to rotate about an axis, it is convenient to
%express its dynamics in a coordinate frame with one axis along the
%axis of rotation.
If $q$ is the location of the center of mass in an inertial frame,
the translational kinetic energy of the body can be written
$K = \frac{1}{2}\dot{q}^T m \dot{q} = \frac{1}{2} m ||\dot{q}||^2$.
Applying Lagrange's equations yields the familiar equation
\begin{equation}
f = m \ddot{q},
\label{eq:familiartranslation}
\end{equation}
where $f$ is the linear force applied to the body expressed in the
inertial frame.

\subsubsection{Planar Rotation}

When a body is confined to move in a plane, a single configuration
variable $q$ can be used to describe its orientation.  Planar rotation
occurs when a body rotates about a fixed revolute joint,
or when a body moves freely on a frictionless plane.  When the body
rotates about a fixed axis of rotation, we can define an $x$-$y$-$z$
inertial frame with the $z$-axis along the axis of rotation.  When
the body moves freely, we can define an $x$-$y$-$z$ inertial frame
at the center of mass of the body, with the $z$-axis orthogonal to the
plane of motion.  (Since we are focusing on rotational motion, the center
of mass can be assumed stationary.)

The kinetic energy
of a body rotating in the plane is the integral over the body of the differential
kinetic energy at each point $r = (x,y,z)^T$:
\[
K = \int_V \frac{1}{2} \rho(r) v(r)^2 dV,
\]
where $q$ is the angle of the body,
$\dot{q}$ is the angular velocity, and
$v(r) = \dot{q}\sqrt{x^2 + y^2}$ is the linear velocity at
$r$.  Therefore we can write the kinetic energy in the
form~\eqref{eq:KEfrominertia}
\begin{equation}
K = \frac{1}{2}\dot{q}^2 \int_V \rho(r)(x^2+y^2) dV = 
\frac{1}{2} \dot{q}^T I_{zz} \dot{q},
\label{eq:planarrotation}
\end{equation}
where
\begin{equation}
I_{zz} = \int_V \rho(r) (x^2+y^2) dV
\label{eq:Izz}
\end{equation}
is the inertia of the body about the $z$-axis.  If the body is uniform
density, \eqref{eq:Izz} simplifies to
\[
I_{zz} = m \int_V (x^2+y^2) dV,
\]
where $m$ is the mass of the body.  Applying Lagrange's equations
to \eqref{eq:planarrotation}, we get the well known form analogous
to \eqref{eq:familiartranslation}
\begin{equation}
\tau_z = I_{zz}\ddot{q},
\label{eq:familiarrotation}
\end{equation}
where $\tau_z$ is the torque about the $z$-axis.

If we choose a $z$-axis through the center of mass of the body and
a parallel $z^\prime$-axis a distance $d$ away, then the scalar
inertias $I_{zz}$ and $I_{z^\prime z^\prime}$ are related by the
{\bf parallel-axis theorem} for planar rotation:
\begin{equation}
I_{z^\prime z^\prime} = I_{zz} + md^2.
\end{equation}
The proof of this theorem is straightforward and in fact 
is implicit in our derivation
of the equations of motion of the RP arm.

\subsubsection{Spatial Rotation}

(This section requires extra mathematical machinery,
and can be safely skipped if the reader is not interested in the
dynamics of a rotating spatial body.)

Finally we come to the dynamics of a rotating spatial body.  In our
Lagrangian formulation, we first choose a set of coordinates $q$,
express the Lagrangian in terms of $q$ and $\dot{q}$, and derive the
equations of motion.  To do this for a rotating spatial body, we can
choose $q$ to be three angles describing the orientation of the body
in a world frame.  Then we can express the kinetic energy of the body
as a function of $q$ and $\dot{q}$ and proceed as before.  If we do
this, however, the inertia matrix $M(q)$ will be extremely complex for any
choice of $q$, providing little insight into the nature of the motion.
The equations of motion are rarely written this way.  Another problem
is that no choice of three orientation variables $q$ can provide a
smooth, global coordinatization of the space of orientations.  In the
same way that latitude and longitude coordinates for the Earth ``go
bad'' at the poles, where the longitude changes discontinuously, any
choice of three coordinates to represent orientations will have
singularities.  (For motions away from these bad orientations,
however, three coordinates work just fine, so this is not the most
serious problem.  In fact, we have a similar problem representing a
single angle by a real number, which requires the use of ${\rm mod} 2
\pi$ arithmetic or multiple coordinate charts.)

Instead of defining the angular velocity as the time-derivative of
coordinates, we define $\omega_s =
(\omega_{x_s},\omega_{y_s},\omega_{z_s})^T$ to be the angular velocity
of the body about the $x_s$-$y_s$-$z_s$ axes of a stationary inertial
frame at the center of mass of the body.  The linear velocity at a
point $r_s = (x_s,y_s,z_s)^T$ on the body is $\omega_s \times r_s$.
The total kinetic energy of the body can be written
\begin{equation}
K = \frac{1}{2} \int_V \rho(r_s) (\omega_s \times r_s)^T (\omega_s \times r_s) dV,
\label{eq:spatialKE1}
\end{equation}
which can be simplified to
\begin{equation}
K = \frac{1}{2} \omega_s^T \left(\int_V \rho(r_s) \left( \begin{array}{ccc}
y_s^2 + z_s^2 & -x_s y_s & -x_s z_s \\
-x_s y_s & x_s^2 + z_s^2 & -y_s z_s \\
-x_s z_s & -y_s z_s & x_s^2+y_s^2 \end{array} \right) dV \right) \omega_s,
\label{eq:spatialinertia}
\end{equation}
or
\begin{equation}
K = \frac{1}{2} \omega_s^T \mbI_s \omega_s.
\label{eq:spatialKE}
\end{equation}
$\mbI_s$ is the symmetric positive definite
inertia matrix for the body written in the inertial
frame.  Because $\mbI_s$ is defined in the world frame, it changes as
the body rotates.

The {\bf angular momentum} of the body is $P = \mbI_s \omega_s$,
and the torque $\tau_s = (\tau_{xs},\tau_{ys},\tau_{zs})^T$ 
acting on the body, expressed in the
inertial frame, is the rate of change of $P$:
\begin{eqnarray}
\tau_s & = & \frac{dP}{dt} \nonumber \\
& = & \frac{d \mbI_s}{dt} \omega_s + \mbI_s \frac{d \omega_s}{dt} \nonumber \\
& = & \omega_s \times \mbI_s \omega_s + \mbI_s \dot{\omega}_s. 
\label{eq:Euler}
\end{eqnarray}
This is known as {\bf Euler's equation} in the inertial frame.
Using the notation
\[
\widehat{\omega}_s = \left(\begin{array}{ccc}
0 & -\omega_{z_s} & \omega_{y_s} \\
\omega_{z_s} & 0 & -\omega_{x_s} \\
-\omega_{y_s} & \omega_{x_s} & 0
\end{array}\right),
\]
we can express \eqref{eq:Euler} as the matrix equation
\begin{equation}
\tau_s = \widehat{\omega}_s \mbI_s \omega_s + \mbI_s \dot{\omega}_s.
\label{eq:Eulermatrix}
\end{equation}
$\widehat{\omega}_s$ is the {\bf skew-symmetric
matrix representation} of $\omega_s$.

We still do not have a representation of the orientation of the body in
the world frame, however.  We need an
equation for the evolution of the body's orientation (the \emph{kinematic} equation)
to go with Equation
\eqref{eq:Eulermatrix} for the evolution of the velocity (the \emph{dynamic}
equation).

To do this, define a frame $x$-$y$-$z$ attached to the body at its
center of mass.
Our representation of the orientation of the body will be as a $3\times 3$
matrix 
\[
R = \left(\begin{array}{ccc} 
\tilde{x}_1 & \tilde{y}_1 & \tilde{z}_1 \\
\tilde{x}_2 & \tilde{y}_2 & \tilde{z}_2 \\
\tilde{x}_3 & \tilde{y}_3 & \tilde{z}_3 \\
\end{array} \right),
\]
where $\tilde{x} = (\tilde{x}_1,\tilde{x}_2,\tilde{x}_3)^T$ is the unit vector
in the body $x$-direction expressed in the inertial coordinate frame.
The vectors $\tilde{y}$ and $\tilde{z}$ are defined similarly 
(Figure~\ref{fig:rotationmatrix}).  $R$ is often
called the {\bf rotation matrix} representation of the orientation.  This
representation uses nine numbers to represent the three degrees-of-freedom,
so there are six constraints on the numbers:  each column is a unit vector
(three constraints) and the columns are orthogonal to each other (three
constraints).  This implies the identity
\[
R^{-1} = R^T.
\]
Also, because we are assuming right-handed frames,
the determinant of $R$ is $+1$.  The matrices satisfying these
conditions are called the $3\times 3$ {\bf special orthogonal} matrices,
or $SO(3)$.  ``Special'' refers to the fact that the determinant is $+1$,
not $-1$.

\begin{figure}
\begin{center}
\includegraphics{../figure/book/trajectory/rotationmatrix}
\end{center}
\caption{The rotation matrix for a body is obtained by expressing the
unit vectors $\tilde{x}$, $\tilde{y}$, and $\tilde{z}$ of the body
$x$-$y$-$z$ frame in the inertial frame $x_s$-$y_s$-$z_s$.}
\label{fig:rotationmatrix}
\end{figure}

Each column vector of $R$ moves according to the angular velocity $\omega_s$,
so the kinematics of the rotating rigid body can be written
\begin{equation}
\dot{R} = \omega_s \times R = \widehat{\omega}_s R.
\label{eq:spatialkinematics}
\end{equation}
Together, equations~\eqref{eq:spatialkinematics} and \eqref{eq:Eulermatrix} describe
the motion of a rotating rigid body in a spatial frame.  
The use of the rotation matrix representation of the orientation allows
us to write the kinematics in a simple and globally correct fashion,
which is not possible with any 
choice of three coordinates.

One difficulty with the equations of motion in an inertial frame is
that $\mbI_s$ changes as the body rotates.  It would be more
convenient and intuitive to define the equations of motion in a frame
fixed to the body, where a {\em body\/} inertia matrix
$\mbI$ is unchanging.  To do so, we use the coordinate frame $x$-$y$-$z$
attached to the body at its center of mass.  The angular velocity in
this frame is written $\omega = (\omega_x,\omega_y,\omega_z)^T$ and
the external torque is written $\tau =
(\tau_x,\tau_y,\tau_z)^T$.  These are related to 
$\omega_s$ and $\tau_s$ by the following equations:
\begin{eqnarray}
\omega_s & = & R \omega \nonumber \\
\tau_s & = & R \tau. \nonumber
\end{eqnarray}
The inertial frame coordinates $r_s = (x_s,y_s,z_s)^T$ of a point are related
to its coordinates in the body frame $r = (x,y,z)^T$ by
\[
r_s = R r.
\]
The kinematic equations in the two frames are related by
\[
\dot{R} = \widehat{\omega}_s R = R \widehat{\omega}.
\]
Plugging these relations into \eqref{eq:spatialKE1} and simplifying, we
find 
\begin{eqnarray}
K & = & \frac{1}{2} \omega^T \mbI \omega \label{eq:bodyKE} \\
\mbI & = & R^T \mbI_s R,
\end{eqnarray}
where the symmetric positive definite body-fixed inertia matrix is given by
\begin{equation}
\mbI = \left(\begin{array}{ccc} I_{xx} & I_{xy} & I_{xz} \\
I_{yx} & I_{yy} & I_{yz} \\
I_{zx} & I_{zy} & I_{zz} \end{array} \right) =
\int_V \rho(r) \left( \begin{array}{ccc}
y^2 + z^2 & -x y & -x z \\
-x y & x^2 + z^2 & -y z \\
-x z & -y z & x^2+y^2 \end{array} \right) dV.
\label{eq:bodyinertia}
\end{equation}
The (possibly non-unique) eigenvectors of $\mbI$ define the {\bf
principal axes of inertia} of the body.  If the body $x$-$y$-$z$ frame
is chosen so that the axes are aligned with principal axes of inertia,
all off-diagonal terms of $\mbI$ are zero, and $I_{xx}, I_{yy}, I_{zz}$
are the principal moments of inertia.  In the general case, one
principal axis is the axis of maximum inertia, one principal axis is
the axis of minimum inertia, and the third principal axis (the
intermediate axis of inertia) is orthogonal.  Because of symmetries,
however, the inertia about two or three of the principal axes might be
identical.  Often the principal axes of inertia of a body are evident
from symmetries (Figure~\ref{fig:inertias}).

\begin{figure}
\begin{center}
\includegraphics{../figure/book/trajectory/inertias}
\end{center}
\caption{Inertias about principal axes of inertia for four different
uniform density volumes.  Note that the principal axes of inertia are
not unique for the sphere and cylinder.}
\label{fig:inertias}
\end{figure}

To derive the dynamics in the body frame, we cannot simply take
the time-derivative of $\mbI \omega$, since this is not defined
in an inertial frame.  
(The time-derivative of momentum in an inertial
frame is a generalized force, while the time-derivative of this 
expression in the body frame is not.)
Instead, we begin with \eqref{eq:Eulermatrix}
\[
\tau_s = \widehat{\omega}_s \mbI_s \omega_s + \mbI_s \dot{\omega}_s,
\]
and plug in $\tau_s = R \tau$, $\omega_s = R \omega$, $\mbI_s = R \mbI R^T$, 
and $\widehat{\omega}_s = \dot{R} R^T$ to get
\begin{eqnarray}
R \tau & = & \dot{R} R^T R \mbI R^T R \omega + R \mbI R^T(\frac{d}{dt} (R \omega)) \nonumber \\
& = & \dot{R} R^T R \mbI R^T R \omega + R \mbI R^T(\dot{R} \omega + R \dot{\omega}). \nonumber
\end{eqnarray}
Recognizing from our identities that 
$\dot{R}\omega = R \widehat{\omega} \omega = 0$, and premultiplying both
sides by $R^{-1} = R^T$, we get
\[
\tau = R^T \dot{R} R^T R \mbI R^T R \omega + R^T R \mbI R^T R \dot{\omega}.
\]
Plugging in $\widehat{\omega} = R^T \dot{R}$ and noticing that $R^T R$ is the
identity matrix, this simplifies
to
\[
\tau = \widehat{\omega} \mbI \omega + \mbI \dot{\omega}.
\]
This is Euler's equation in the body frame.  Note that it has the same form
as Euler's equation in the spatial frame.  Collecting together the
kinematics and dynamics in one place, the equations of motion in the
body frame are written
\begin{eqnarray}
\dot{R} & = & R \widehat{\omega} \label{eq:bodykinematics} \\
\tau & = & \widehat{\omega} \mbI \omega + \mbI \dot{\omega}.
\label{eq:Eulerbody}
\end{eqnarray}
The big advantage of this form over the spatial equations is that $\mbI$
is constant.

If the body $x$-$y$-$z$ frame is aligned
with the principal axes so that all off-diagonal terms of $\mbI$ are zero,
\eqref{eq:Eulerbody} simplifies to
\begin{equation}
\left(\begin{array}{c} \tau_{x} \\ \tau_{y} \\ \tau_{z} \end{array}\right) =
\left(\begin{array}{l} I_{xx} \dot{\omega}_x + (I_{zz}-I_{yy}) \omega_y \omega_z \\
I_{yy} \dot{\omega}_y + (I_{xx}-I_{zz}) \omega_x \omega_z \\
I_{zz} \dot{\omega}_z + (I_{yy}-I_{xx}) \omega_x \omega_y
\end{array}
\right).
\label{eq:Eulersimple}
\end{equation}
One key implication of \eqref{eq:Eulerbody} and
\eqref{eq:Eulersimple} is that $\dot{\omega}$
may not be zero even if $\tau$ is zero.  Although the angular momentum
and kinetic energy of a rotating body are constant when no external 
torques are applied, the angular velocity of the body may not be 
constant.  For further interpretation of \eqref{eq:Eulersimple}, see
the mechanics textbook by Symon~\cite{Symon} or the robotic
manipulation textbook by Mason~\cite{Masonbook}.

When it is difficult to solve the integrals of \eqref{eq:bodyinertia}
directly to find $\mbI$, it may be possible to decompose the body
into simpler components and solve for (or look up in a table) the
inertia matrix at the center of mass of each component separately.  If
we then transform these inertia matrices to a common frame, we can
simply add them to get the inertia matrix for the composite body in
that common frame.  This transformation can be accomplished by a
translation followed by a rotation, as outlined below (see
Figure~\ref{fig:inertiatransform}).

Let $\mbI_{i}$ be the inertia matrix of the $i$th component,
expressed in its own local coordinate frame $x_i$-$y_i$-$z_i$ at its
center of mass.  Let $r_i$ be the vector from the origin of the local frame to the
origin of the common frame $x$-$y$-$z$, expressed in the local frame.  The inertia
$\mbI_{i}$ can be expressed in a frame aligned with $x_i$-$y_i$-$z_i$,
but located at the origin of the common frame, using the
{\bf parallel-axis theorem}
\begin{equation}
\mbI_{i}^{\prime} = \mbI_{i} + m_i(||r_i||^2 I_3 - r_i r_i^T),
\label{eq:parallelaxis}
\end{equation}
where $m_i$ is the mass of the $i$th component and
$I_3$ is the $3\times3$ identity matrix.  
Now let $R_i$ denote the rotation matrix describing
the orientation of this translated local frame relative to the common 
frame.  Rotating the inertia matrix into the common frame, we get
\[
\mbI_{i}^{\prime\prime} = R_i \mbI_{i}^\prime R_i^T.
\]
The matrix $\mbI_{i}^{\prime\prime}$ is the inertia of the $i$th 
component expressed in the common frame.
Performing this translation and rotation for all 
$k$ components of the body, the total inertia of the body in the common
frame is $\mbI = \mbI_{1}^{\prime\prime} + \ldots + \mbI_{k}^{\prime\prime}$.

\begin{figure}
\begin{center}
\includegraphics{../figure/book/trajectory/inertiatransform}
\end{center}
\caption{The inertia matrix of body ${\cal B}_i$, expressed in a
frame $x_i$-$y_i$-$z_i$ at the center of mass of the body, can be 
expressed in another
frame $x$-$y$-$z$ by a translation and rotation.}
\label{fig:inertiatransform}
\end{figure}

%Finally we come to the dynamics of a rotating spatial body.  In our
%Lagrangian formulation, we first choose a set of coordinates $q$,
%express the Lagrangian in terms of $q$ and $\dot{q}$, and derive the
%equations of motion.  To do this for a rotating spatial body, we can
%choose $q$ to be three coordinates describing the orientation of the
%body in a world frame.  Then we can express the kinetic energy of the
%body as a function of $q$ and $\dot{q}$ and proceed as before.  We
%will not do this, however, for two important reasons:
%\begin{enumerate}
%\item Just like latitude and longitude coordinates for the Earth ``go bad''
%at the poles, where a small step can discontinuously change the longitude
%up to 180$^\circ$, any choice of three coordinates for the orientation
%of a body will go bad at some orientations.  No three coordinates will give
%a smooth coordinatization of all orientations of the body.  For small
%motions away from these bad orientations, however, three coordinates work
%just fine, so this is not our most serious problem.  (In fact, we have
%the same problem representing an angle by a real number, requiring
%the use of $\mod 2 \pi$.)
%
%\item  If we follow this approach, the inertia matrix $M(q)$ becomes an 
%exceedingly complex function of $q$.  On the other hand, since the material
%properties of a body are not changing as it moves, there should
%be a simple representation of the inertia of a body which is independent
%of its current configuration.
%\end{enumerate}

\paragraph{Lagrange's equations revisited}

We have gone to great lengths to avoid choosing three generalized
coordinates and using Lagrange's equations!  Now that we have done
this work and developed some understanding of the body inertia
matrix $\mbI$, it will be easier to see how Lagrange's equations could be
applied.  

\begin{figure}
\begin{center}
\includegraphics{../figure/book/trajectory/eulerangles}
\end{center}
\caption{Euler angles for a rotating body.}
\label{fig:eulerangles}
\end{figure}

Figure~\ref{fig:eulerangles} shows a choice of coordinates $q=(q_1,q_2,q_3)^T$
due to Euler.
The body $x$-$y$ plane intersects the spatial $x_s$-$y_s$ plane along a line
called the {\em line of nodes\/}.  The coordinate $q_1$ is the angle from
the $x_s$-axis to the line of nodes, 
$q_2$ is the angle from the line of nodes
to the body $x$-axis, and $q_3$
is the angle from the $z_s$-axis to the body $z$-axis.
With some work, the angular velocity $\omega = (\omega_x,\omega_y,\omega_z)^T$ 
can be expressed in
terms of these coordinates:
\begin{eqnarray}
\omega_x & = & \dot{q}_3 \cos q_2 + \dot{q}_1 \sin q_2 \sin q_3 \label{eq:eulercoord1} \\
\omega_y & = & -\dot{q}_3 \sin q_2 + \dot{q}_1 \cos q_2 \sin q_3 \label{eq:eulercoord2} \\
\omega_z & = & \dot{q}_2 + \dot{q}_1 \cos q_3. \label{eq:eulercoord3}
\end{eqnarray}
Plugging these into the kinetic energy $K = \frac{1}{2} \omega^T \mbI \omega$,
we get the kinetic energy in the form $K = \frac{1}{2} \dot{q}^T M(q) \dot{q}$,
as we are used to.  From there we can apply Lagrange's equations as before
to get the dynamic equations of motion for generalized torques acting
along the coordinates.  

A good thing about this formulation is that we have used the fewest
possible numbers to represent the orientation, and the kinematics
are trivial.  Significant drawbacks are the complexity of the equations,
as well as the singularities in the coordinate representation.

This section has hinted at how one might understand dynamics on
configuration spaces that are not easily described by a subset of
$\real^n$.  This leads to the idea of global ``coordinate free''
formulations of dynamics.  For a more thorough and general treatment,
which introduces the notion of Lie groups, see the robotics textbook
by Murray, Li, and Sastry~\cite{Murray94}.

%\[
%m = \int_{V} \rho(x) dV
%\]
%the linear component is given by the familiar expression 
%$\frac{1}{2} v^T m v = \frac{1}{2} m ||v||^2$, where $v$ is the linear
%velocity vector of the center of mass of the body measured in an inertial
%coordinate frame.  By a loose analogy, the angular component is 
%\[
%\frac{1}{2} \omega^T \mathbb{I} \omega,
%\]
%where $\omega$ is the angular velocity of the body and $\mathbb{I}$ is the 
%{\bf inertia matrix} of the body.\footnote{More precisely, $\mathbb{I}$ is a
%{\em tensor\/}.}
%
%inertia formulas, parallel-axis theorem

\subsection{Velocity Constraints}

In Section~\ref{ssec:standardforms} we derived a standard form for
the equations of motion of a mechanical system.
Suppose that our system is now also subject to a set of $k$ 
linearly independent constraints linear in velocity, i.e., of the form
\begin{equation}
A(q)\dot{q} = 0, 
\label{eq:Pfaffianconstraints}
\end{equation}
where $A(q)$ is a $k \times n_{\cal Q}$ matrix, and the $k$ rows
of $A(q)$ are written $a^j(q) = (a^j_1,\ldots,a^j_{n_{\cal Q}}), 
j = 1 \ldots k$.
Such constraints are called {\bf Pfaffian} constraints.  One
source of Pfaffian constraints is rolling without slipping, such
as in a wheeled mobile robot.
Another source is contact constraints at the end effector
of a robot manipulator
(e.g., velocities into or away from a constraint surface are disallowed).

Since the constraints \eqref{eq:Pfaffianconstraints} are
satisfied throughout motion, we can differentiate the left-hand
side and set it equal to zero:
\[
A(q)\ddot{q} + \dot{A}(q) \dot{q} = 0.
\]

The constrained Lagrange equations can then be written
\begin{eqnarray}
\frac{d}{dt}\frac{\partial L}{\partial \dot{q}} - \frac{\partial L}{\partial q} = M(q) \ddot{q} + C(q,\dot{q}) \dot{q} + g(q) & = &
\force + A^T(q) \lambda \label{eq:constraineddynamics} \\
A(q) \dot{q} = \dot{A}(q)\dot{q} + A(q)\ddot{q} & = & 0, \label{eq:constrainedkinematics}
\end{eqnarray}
where $\lambda = (\lambda_1,\ldots,\lambda_k)^T$ 
are the {\bf Lagrange multipliers}.  $\lambda_j a^j(q)$ is the 
generalized force
applied by constraint $j$ to maintain the constraint.  The constrained
Lagrange equations yield $n_{\cal Q}+k$ equations to be solved for the $n_{\cal Q}+k$ variables
$\ddot{q}$ and $\lambda$.
%Since the constraints \eqref{eq:Pfaffianconstraints} are identically satisfied
%throughout the motion, we can differentiate the left-hand side and
%set it equal to zero.

If we are not interested in calculating the $k$ constraint forces,
we can use \eqref{eq:constrainedkinematics} to
eliminate $\lambda$ from \eqref{eq:constraineddynamics}.  
Solving \eqref{eq:constraineddynamics}
for $\ddot{q}$ and plugging into \eqref{eq:constrainedkinematics}, 
dropping the dependence of $M$, $A$, and $g$ on $q$ and $C$ on
$q,\dot{q}$, we get
\[
\dot{A}\dot{q} + AM^{-1}(\force+A^T\lambda -C\dot{q}-g) = 0.
\]
Now solving for $\lambda$, we get
\[
\lambda = (AM^{-1}A^T)^{-1} (-\dot{A}\dot{q} + AM^{-1}(C\dot{q} + g -\force)).
\]
Recognizing that $-\dot{A}\dot{q} = A\ddot{q}$,
plugging back into \eqref{eq:constraineddynamics}, and manipulating,
we get
\[
({\cal I}-A^T(AM^{-1}A^T)^{-1}AM^{-1}) (M\ddot{q} + C\dot{q} + g - \force) = 0,
\]
where ${\cal I}$ is the identity matrix.
If we define 
\[
P_\force = {\cal I}-A^T(AM^{-1}A^T)^{-1}AM^{-1},
\]
then we get the form
\begin{equation}
P_\force (M\ddot{q} + C\dot{q} + g) = P_\force \force.
\label{eq:simpleconstrainedprelim}
\end{equation}
The $n_{\cal Q} \times n_{\cal Q}$ matrix $P_\force$ is only rank $n_{\cal Q}-k$,
so we cannot pre-multiply both sides of \eqref{eq:simpleconstrainedprelim}
by $P_\force^{-1}$; if we could, we would be left with the unconstrained
dynamics.  The projection matrix $P_\force$
projects generalized forces to the components which do work on the
system.  The remaining forces, defined by the projection $({\cal I}-P_\force)$,
are the components in the constrained directions.  These two sets of
forces are orthogonal to each other with respect to the inertia matrix.  
In other words, 
\[
(P_\force \force)^T M^{-1} ({\cal I}-P_\force) \force = 0
\]
for any $\force$.

%If we define
%\[
%\tilde{M} = P_\force M, \;\; \tilde{C} = P_\force C, \;\; \tilde{g} = P_\force g, \;\;
%\tilde{\force} = P_\force \force,
%\]
%the equations can be written
%\begin{equation}
%\tilde{M}(q)\ddot{q} + \tilde{C}(q,\dot{q}) \dot{q} + \tilde{g}(q) = \tilde{\force},
%\label{eq:simpleconstrained}
%\end{equation}
%where $\tilde{M}$ is the projected inertia matrix, $\tilde{C}$ is the
%projected Coriolis matrix, $\tilde{g}$ is the projected gravitational
%force, and $\tilde{\force}$ is the component of $\force$ orthogonal to the
%constraints.
%{\bf *** describe the projected Christoffel symbols, partly in constrained
%direction, partly not, since $\dot{q}_i$ is not feasible generally; also, 
%the above is only correct if I use $C(q,\dot{q})$ instead of
%$C(q,\dot{q})\dot{q}$.  This whole paragraph could be eliminated... ***}

Defining the matrix $P = M^{-1} P_\force M$ and rearranging 
\eqref{eq:simpleconstrainedprelim}, we get the equivalent form
\begin{equation}
P\ddot{q} = PM^{-1}(\force-C\dot{q}-g).
\label{eq:simpleconstrainedqddot}
\end{equation}
Here the rank $n_{\cal Q}-k$ matrix $P$ projects general velocities
$\dot{q}$ to velocities satisfying the motion constraints.  The
projection $({\cal I}-P)$ is the orthogonal projection by the inertia matrix.
In other words,
\[
(P \dot{q})^T M ({\cal I}-P)\dot{q} = 0
\] 
for any $\dot{q}$.  The matrices $P$ and $P_\force$ satisfy
$P = P_\force^T$.  Further discussion on these projections can be
found in \cite{Boothby,Liu2002,Marsden94}.

\begin{figure}
\begin{center}
\includegraphics{../figure/book/trajectory/knife-edge}
\end{center}
\caption{A knife-edge on a plane.}
\label{fig:knife-edge}
\end{figure}

\begin{example}
{\em
A knife-edge can slide on a horizontal plane in the direction it is pointing
or spin about its contact point with the plane, but it cannot slide
orthogonal to its heading direction.
Let $q = (q_1,q_2,q_3)^T$ represent
the configuration of the knife-edge, where $(q_1,q_2)$ denotes the contact
point on the plane and $q_3$ denotes the heading direction (Figure~\ref{fig:knife-edge}).    If the mass
of the knife-edge is $m$ and its inertia is $I$ about an axis through the
contact and orthogonal to the plane, the Lagrangian is the kinetic energy
\[
L = \frac{1}{2} m (\dot{q}_1^2+\dot{q}_2^2) + \frac{1}{2} I \dot{q}_3^2.
\]

The single constraint can be written
\[
\dot{q}_1 \sin q_3 - \dot{q}_2 \cos q_3 = 0,
\]
or $A(q) = a^1(q) = (\sin q_3, -\cos q_3,0)$.  Differentiating this constraint,
we get
\begin{equation}
\ddot{q}_1 \sin q_3 + \dot{q}_1 \dot{q}_3 \cos q_3 - \ddot{q}_2 \cos q_3 + \dot{q}_2 \dot{q}_3 \sin q_3 = 0.
\label{eq:constraintdiff}
\end{equation}
Applying Lagrange's equations, we get
\begin{eqnarray}
m \ddot{q}_1 & = & \force_1 + \lambda_1 \sin q_3 \label{eq:q1} \\
m \ddot{q}_2 & = & \force_2 - \lambda_1 \cos q_3 \label{eq:q2} \\
I \ddot{q}_3 & = & \force_3. \label{eq:q3}
\end{eqnarray}
Solving \eqref{eq:constraintdiff}--\eqref{eq:q3}, we get
\begin{eqnarray}
\ddot{q}_1 & = & \frac{1}{m}(\force_1 \cos^2 q_3 + (\force_2 - m \dot{q}_1 \dot{q}_3) \cos q_3 \sin q_3 - m \dot{q}_2 \dot{q}_3 \sin^2 q_3) \label{eq:q1dd} \\
\ddot{q}_2 & = & \frac{1}{m}(\force_2 \sin^2 q_3 + (\force_1+m \dot{q}_2 \dot{q}_3) \cos q_3 \sin q_3 + m \dot{q}_1 \dot{q}_3 \cos^2 q_3) \\
\ddot{q}_3 & = & \frac{\force_3}{I} \\
\lambda_1 & = & (\force_2 - m \dot{q}_1 \dot{q}_3) \cos q_3 - (\force_1 + m \dot{q}_2 \dot{q}_3) \sin q_3, \label{eq:q3dd}
\end{eqnarray}
where $(\force_1,\force_2)$ are the forces along the $(q_1,q_2)$ directions,
and $\force_3$ is the torque about an axis through the contact point and 
orthogonal to the plane.

Alternatively, we could study the projected dynamics.  The mass matrix
$M(q)$ is 
\[
M(q) = \left(\begin{array}{ccc} m & 0 & 0 \\ 0 & m & 0 \\ 0 & 0 & I\end{array}
\right)
\]
and we calculate
the projection matrix
\[
P_\force = \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).
\]
Note that the first two rows (and columns) of $P_\force$ are linearly dependent.
In this example, $C(q,\dot{q})\dot{q}$ and $g(q)$ are zero, so the
projected dynamics become
\[
P_\force M(q)\ddot{q} = P_\force \force,
\]
which yields the two independent equations of motion in the unconstrained
directions
\begin{eqnarray}
m (\ddot{q}_1 \cos q_3 + \ddot{q}_2 \sin q_3) & = & \force_1 \cos q_3 + \force_2 \sin q_3 \nonumber \\
I \ddot{q}_3 & = & \force_3.\nonumber
\end{eqnarray}
The constraint equation~\eqref{eq:constraintdiff} completes
the system of three equations, and we solve to get 
\eqref{eq:q1dd}--\eqref{eq:q3dd} above.
}
\end{example}

As the previous example shows, even for very simple systems with
constraints, the expressions can quickly become unwieldy.  When
possible, it may be preferable to choose a reduced set of generalized
coordinates to eliminate the constraints.  For example, if $a^j(q) =
\frac{\partial c(q)}{\partial q}$ for some function $c(q) = 0$, then
the velocity constraint is actually the time derivative of a
configuration constraint $c(q)=0$, which reduces the dimension of the
configuration space by one.  Therefore, it is possible to reduce the
number of generalized coordinates by one and eliminate the constraint.
As an example, the planar motion of a point on a circle can be
represented using coordinates $(x,y)$ and the velocity constraint
$x\dot{x} + y\dot{y} = 0$.  This velocity constraint can be integrated
to the configuration constraint $x^2 + y^2 = R$, however, where the
radius $R$ is defined by the initial position of the point.  This
configuration constraint allows an unconstrained description of the
configuration using a single angle coordinate $\theta$.  When a velocity
constraint can be integrated to a configuration constraint, as in this
case, the constraint is called {\bf holonomic}.  
{\bf Nonholonomic} constraints are velocity constraints that cannot be
integrated to a configuration constraint.

In the rest of this chapter, we will 
assume that any holonomic constraints have been eliminated so that
we are dealing with a minimal set of coordinates.  Motion planning
with nonholonomic
constraints is left to the next chapter.

%Efficient ways to do this for some systems are described by Bullo and Zefran.

%Equations of this form
%can be used to model a wide variety of systems, from robot arms to vehicles
%and spacecraft.
%If the equations are
%in the more general form \ref{eq:generalcoriolis}, they can be transformed
%to the form \ref{eq:robotcoriolis} by defining ``virtual'' actuators
%$\force^\prime = T(q) \force

\section{Decoupled Trajectory Planning}
\label{sec:decoupled}

Given a collision-free path $q(s)$ for a robot system, what is the
fastest trajectory that follows this path?  In other words, what is
the time-optimal time scaling $s(t)$ subject to actuator constraints?
This question is of considerable importance for maximizing the
productivity of robot systems when a path has been given by task
specifications.  This problem has been solved elegantly 
by Shin and McKay~\cite{Shin85} and Bobrow~{\em et
al.\/}~\cite{Bobrow85}, with subsequent enhancements by 
Pfeiffer and Johanni~\cite{Pfeiffer87}, Slotine and
Yang~\cite{Slotine89}, and Shiller and Lu~\cite{Shiller92}.

Let us assume that the equations of motion of our system
are in the standard form~\eqref{eq:robotcoriolis} or
\eqref{eq:robotchristoffel}.  
The robot is subject to the actuator limits
\begin{equation}
\force_i^{min}(q,\dot{q}) \leq \force_i \leq \force_i^{max}(q,\dot{q}).
\label{eq:actuatorlimits}
\end{equation}
In general, the actuator limits may be functions of the system configuration
and velocity.  For example, the torque available to accelerate a DC motor
decreases as its angular velocity increases.  The simplest example of actuator
limits are the symmetric, state-independent bounds
\[
|\force_i| \leq \force_i^{max}.
\]

For a given path $q(s)$, we can substitute 
\begin{eqnarray}
\frac{dq}{ds} \dot{s} & = & \dot{q} \label{eq:pathvel} \\
\frac{d^2q}{ds^2} \dot{s}^2 + \frac{dq}{ds} \ddot{s} & = & \ddot{q} \label{eq:pathacc}
\end{eqnarray}
into \eqref{eq:robotchristoffel} to get
\begin{equation}
M(q(s)) \left( \frac{d^2q}{ds^2} \dot{s}^2 + \frac{dq}{ds}\ddot{s} \right) + \left( \frac{dq}{ds}\dot{s} \right)^T \Gamma(q(s)) \left(\frac{dq}{ds}\dot{s}\right) + g(q(s)) = \force
\end{equation}
or
\begin{equation}
\left( M(q(s))\frac{dq}{ds} \right)\ddot{s} + \left(M(q(s)) \frac{d^2q}{ds^2} + \left(\frac{dq}{ds}\right)^T \Gamma(q(s)) \frac{dq}{ds}\right) \dot{s}^2 + g(q(s)) = \force.
\label{eq:armdynamics}
\end{equation}
These equations can be expressed compactly as the vector equation
\begin{equation}
a(s) \ddot{s} + b(s) \dot{s}^2 + c(s) = \force
\label{eq:constrained}
\end{equation}
defining the dynamics constrained to the path $q(s)$.
$a(s)$, $b(s)$, and $c(s)$ are inertial, velocity product, and
gravitational terms in terms of $s$, respectively.

As the robot travels along the path $q(s)$, its state at any time is identified by
$(s,\dot{s})$.  Actuator limits can be expressed as a function of the path state by
substituting \eqref{eq:pathvel} into 
\eqref{eq:actuatorlimits}, yielding $\force^{min}(s,\dot{s})$ and
$\force^{max}(s,\dot{s})$.
Therefore, at all times the system must satisfy the constraints
\begin{equation}
\force^{min}(s,\dot{s}) \leq a(s)\ddot{s} + b(s) \dot{s}^2 + c(s) \leq \force^{max}(s,\dot{s}).
\label{eq:torqueconstraints}
\end{equation}
Let $L_i(s,\dot{s})$ and $U_i(s,\dot{s})$ be the minimum and 
maximum accelerations satisfying the $i$th component of 
(\ref{eq:torqueconstraints}), and define
\begin{equation}
\alpha_i(s,\dot{s}) = \frac{\force^{max}_i(s,\dot{s}) - b_i(s) \dot{s}^2 - c_i(s)}{a_i(s)},
\;\; \beta_i(s,\dot{s}) = 
\frac{\force^{min}_i(s,\dot{s}) - b_i(s) \dot{s}^2 - c_i(s)}{a_i(s)}.
\end{equation}
Then $U_i(s,\dot{s}) = \alpha_i(s,\dot{s})$, 
$L_i(s,\dot{s}) = \beta_i(s,\dot{s})$ if $a_i(s)>0$,
and $U_i(s,\dot{s}) = \beta_i(s,\dot{s})$, 
$L_i(s,\dot{s}) = \alpha_i(s,\dot{s})$ if $a_i(s)<0$.
(If $a_i(s)=0$ for any $i$, the system is at a {\bf zero inertia point}, and we
will set aside this possibility until later in the chapter.)  We define
\[
L(s,\dot{s}) = \max_{i\in 1 \ldots n_{\cal Q}} L_i(s,\dot{s}), \;\;
U(s,\dot{s}) = \min_{i\in 1 \ldots n_{\cal Q}} U_i(s,\dot{s}).
\]
The actuator limits~(\ref{eq:torqueconstraints}) can then be expressed as 
\begin{equation}
L(s,\dot{s}) \leq \ddot{s} \leq U(s,\dot{s}).
\label{eq:constraints}
\end{equation}.

%\begin{equation}
%\ddot{s}_{min}(s,\dot{s}) \leq \ddot{s} \leq \ddot{s}_{max}(s,\dot{s})
%\end{equation}
%where 
%\[
%\ddot{s}_{max}(s,\dot{s}) = \min_{i\in 1\ldots n} \left(\frac{\force^{max}_i(s,\dot{s}) - b(s) \dot{s}^2 - c(s)}{a(s)}\right),
%\;\; \ddot{s}_{min}(s,\dot{s}) = 
%\max_{i \in 1\ldots n} \left(\frac{\force^{min}_i(s,\dot{s}) - b(s) \dot{s}^2 - c (s)}{a(s)}\right)
%\]
%if $a(s) > 0$, and
%\[
%\ddot{s}_{min}(s,\dot{s}) = \max_{i \in 1 \ldots n} \left(\frac{\force^{max}_i(s,\dot{s}) - b(s) \dot{s}^2 - c(s)}{a(s)}\right),
%\;\; \ddot{s}_{max}(s,\dot{s}) = 
%\min_{i \in 1 \ldots n} \left(\frac{\force^{min}_i(s,\dot{s}) - b(s) \dot{s}^2 - c (s)}{a(s)}\right)
%\]
%if $a(s)<0$.
%If $a(s) = 0$, the system is at a dynamic singularity, and we will set aside this
%possibility for now.

The problem can now be stated:

\begin{quote}
Given a path $q(s) : [0,1] \rightarrow {\cal Q}$, an initial state $(0,\dot{s}_0)$
and a final state $(1,\dot{s}_f)$, $\dot{s}_0,\dot{s}_f \geq 0$, 
find a monotonically increasing
twice-differentiable time scaling
$s(t) : [0,t_f] \rightarrow [0,1]$ that (1) satisfies $s(0) = 0, \dot{s}(0) = \dot{s}_0,
s(t_f) = 1, \dot{s}(t_f) = \dot{s}_f$, and
(2) minimizes the total travel time $t_f$ along
the path while respecting the actuator constraints \eqref{eq:constraints}
for all time $t \in [0,t_f]$.
\end{quote}

We can conveniently visualize this problem in the $(s,\dot{s})$ state
space.  At any state $(s,\dot{s})$, the constraints
\eqref{eq:constraints} specify a range of feasible accelerations along
the path, $[L(s,\dot{s}),U(s,\dot{s})]$.  This range can be
interpreted as a cone of tangent vectors in the state space, as
illustrated in Figure~\ref{fig:mintime}.  The problem is to find a
curve from $(0,\dot{s}_0)$ to $(1,\dot{s}_f)$ such that $\dot{s}\geq
0$ everywhere and the tangent at each state is inside the cone at that
state.  Further, the curve should maximize the speed $\dot{s}$ at
each $s$ to minimize the time of motion.  A consequence of this is
that the curve always follows the upper or lower bound of the cone
(maximum or minimum acceleration) at each state.\footnote{Except perhaps at
zero inertia points, as described later.}  This kind of trajectory is called a ``bang-bang''
trajectory, and at least one of the actuators is always saturated.
The heart of the time-scaling problem is to find
the switching points between maximum and minimum acceleration.

\begin{figure}
\begin{center}
\includegraphics{../figure/book/trajectory/mintime}
\end{center}
\caption{At each point $(s,\dot{s})$ in the phase plane we can draw a motion cone
defined by the maximum and minimum accelerations $\ddot{s}$ satisfying
the actuator limits.  The time-optimal trajectory from
$(0,\dot{s}_0)$ to $(1,\dot{s}_f)$ is the curve that maximizes the
area underneath it while remaining on the boundary of the motion
cones.  In this example, the trajectory switches between maximum and
minimum acceleration three times.  The velocity limit curve indicates
the states where the cone collapses to a single tangent vector, and
the gray region represents inadmissible states where the cone
disappears---no feasible actuation will keep the system on the
path.}
\label{fig:mintime}
\end{figure}

At some states $(s,\dot{s})$, the actuation constraints
\eqref{eq:constraints} indicate that no feasible
acceleration will allow the system to continue to follow the path.  
Such regions of the state space
are shown in gray in Figure~\ref{fig:mintime}.  We will call these
regions {\bf inadmissible} regions.  At any inadmissible state, the
robot is doomed to leave the path immediately.  At admissible states,
the robot may still be doomed to eventually leave the path.  This
happens if any integral curve originating from the state, with
tangents remaining inside the tangent cones, eventually must reach
the inadmissible region.

We will assume that, for any $s$, the robot is strong enough to
maintain its configuration statically, so all $\dot{s}=0$ states
are admissible.  We will
also assume that as $\dot{s}$ increases from zero for a given
$s$, there will be at most one switch from admissible to inadmissible.
This occurs at the {\bf velocity limit
curve} $v(s), v: s \rightarrow \dot{s}$, where 
\begin{equation}
L(s,\dot{s}) = U(s,\dot{s}).
\label{eq:velocitylimit}
\end{equation}
The velocity limit $v(s)$ is obtained by equating
$L_i(s,\dot{s}) = U_j(s,\dot{s})$ for all $i,j = 1 \ldots n_{\cal Q}$ 
and solving each equation for $\dot{s}$ (if a solution exists).
Call the solution $\dot{s}_{ij}(s)$.  For all $i,j$, keep 
the minimum value:  
$v(s) = \min_{i,j} \dot{s}_{ij}(s)$.\footnote{In general,
Equation~\eqref{eq:velocitylimit} may be satisfied for multiple values
of $\dot{s}$ for a single value of $s$.  This may occur due to friction
in the system, weak actuators which cannot hold each configuration
statically, or the form of the actuation limit functions.
In this case, there may be inadmissible
``islands'' in the phase plane.
This significantly complicates the
problem of finding an optimal time scaling, and we will ignore this
possibility.  See~\cite{Shin85} for a time-scaling algorithm
for this case.}  

Note that because of the $\max(\cdot)$ and $\min(\cdot)$ functions used
in calculating $L(s,\dot{s})$, $U(s,\dot{s})$,
and $v(s)$, these functions are generally not smooth.

As mentioned earlier, the problem is to find the switches between
maximum and minimum acceleration.  The following algorithm uses
numerical integration to find the set of switches, expressed as the
$s$ values at which the switches occur.

\vspace*{0.1in}

\noindent
{\bf Time-scaling algorithm}
\begin{enumerate}

%Step 1
\item Initialize an empty list of switches ${\cal S} = \{ \}$
and a switch counter $i=0$.  Set $(s_i,\dot{s}_i) = (0,\dot{s}_0)$.

%Step 2
\item Integrate the equation $\ddot{s} = L(s,\dot{s})$
{\em backward\/} in time from $(1,\dot{s}_f)$ until the velocity
limit curve is penetrated (reached transversally, not tangentially), $s=0$,
or $\dot{s}=0$ at $s<1$.  There is no solution to the problem if
$\dot{s}=0$ is reached.
Otherwise, call this phase plane curve $F$ and
proceed to the next step.

%Step 3
\item Integrate the equation $\ddot{s} = U(s,\dot{s})$ 
forward in time from 
$(s_i,\dot{s}_i)$.  Call this curve $A_i$.  Continue
integrating until either $A_i$ crosses $F$ or $A_i$ penetrates the
the velocity limit curve.  (If $A_i$ crosses $s=1$ or $\dot{s}=0$
before either of these two cases occur, there is no solution
to the problem.)
If $A_i$ crosses $F$,
then increment $i$, let $s_i$ be the $s$ value
at which the crossing occurs, and append $s_i$ to the list of switches
$\cal S$.  The problem is solved and $\cal S$ is the solution.
If instead the velocity limit curve is penetrated, 
let $(s_{lim},\dot{s}_{lim})$ be the point of penetration and
proceed to the next step.

%Step 4
\item Search the velocity limit curve $v(s)$ forward
in $s$ from $(s_{lim},\dot{s}_{lim})$ until finding the first point
where the feasible acceleration ($L = U$ on
the velocity limit curve) is tangent to the velocity limit curve.
If the velocity limit is $v(s)$, then a point $(s,v(s))$
satisfies the tangency condition if
$\frac{dv}{ds}|_{s} = U(s,v(s))/v(s)$.
Call the first tangent point reached $(s_{tan},\dot{s}_{tan})$.\footnote{An
alternative approach to finding $(s_{tan},\dot{s}_{tan})$ is to 
choose a point $(s_{lim},\dot{s}^\prime)$, where $\dot{s}^\prime < \dot{s}_{lim}$,
integrate $L$ forward from there, and check if the solution
penetrates the velocity limit curve.  If so, choose $\dot{s}^{\prime\prime} <
\dot{s}^\prime$ and repeat, continuing until the forward integration
just touches the velocity limit curve tangentially.  The tangent point
is $(s_{tan},\dot{s}_{tan})$.}
From $(s_{tan},\dot{s}_{tan})$, integrate the curve $\ddot{s} =
L(s,\dot{s})$ backward in time until it intersects
$A_i$.  Increment $i$ and call this new curve $A_i$.  Let $s_i$ be the
$s$ value of the intersection point.  This is a switch point from
maximum to minimum acceleration.  Append $s_i$ to the list $\cal S$.

%At this point, we wish to
%find points on the velocity limit curve where the feasible
%acceleration ($L=U$ on the velocity limit curve)
%is tangent to the velocity limit curve.  This is because the optimal
%solution will be tangent to the velocity limit curve, and switches
%will occur at these points.

%Choose the maximum velocity $\dot{s}^\prime <
%\dot{s}_{lim}$ (perhaps by binary search) such that the curve $\ddot{s} =
%L(s,\dot{s})$, integrated forward from
%$(s_{lim},\dot{s}^\prime)$, does not penetrate
%the velocity limit curve.  This curve will touch the velocity limit curve
%tangentially at
%$(s_{tan},\dot{s}_{tan})$.  

%Step 5
\item  Increment $i$, and set $(s_i,\dot{s}_i) = (s_{tan},\dot{s}_{tan})$. 
This is a switch point from minimum to maximum acceleration.  Append $s_i$ to the list
$\cal S$.  Go to step 3.
\end{enumerate}

An illustration of the time-scaling algorithm is shown in 
Figure~\ref{fig:mintimealgorithm}.

\begin{figure}
\begin{center}
\includegraphics{../figure/book/trajectory/mintimealgorithm}
\end{center}
\caption{An illustration of the time-scaling algorithm.  {\bf (Step 2)} Beginning from
$(1,\dot{s}_f)$, the minimum acceleration is integrated backward until the velocity
limit curve is reached.  The resulting phase plane curve is denoted $F$.
{\bf (Step 3)} Beginning from $(0,\dot{s}_0)$,
the maximum acceleration is integrated forward until the velocity
limit curve is reached at $(s_{lim},\dot{s}_{lim})$.  This phase plane curve
is denoted $A_0$.  
{\bf (Step 4)} 
The velocity limit curve $v(s)$ is searched forward from $s_{lim}$ until
a point is found where the feasible acceleration is tangent to the
limit curve.  The figure shows the feasible accelerations at points
on the velocity limit curve as arrows.  An arrow becomes tangent
to the velocity limit curve at $(s_{tan},\dot{s}_{tan})$.
From $(s_{tan},\dot{s}_{tan})$,
the minimum acceleration is integrated backward until it reaches $A_0$.  This
curve is called $A_1$.  The
$s$ value of the intersection is $s_1$ and is added to the switch list,
${\cal S} = \{s_1\}$.  
%A value $\dot{s}^\prime < \dot{s}_{lim}$
%is chosen so that the minimum acceleration, integrated forward from $(s_{lim},\dot{s}_{lim})$,
%yields a curve that touches the velocity limit curve tangentially.  Call this
%point of tangency $(s_{tan},\dot{s}_{tan})$.  From $(s_{tan},\dot{s}_{tan})$,
%the minimum acceleration is integrated backward until it reaches $A_0$.  This
%curve is called $A_1$.  The
%$s$ value of the intersection is $s_1$ and is added to the switch list,
%${\cal S} = \{s_1\}$.  
{\bf (Step 5)} $(s_2,\dot{s}_2)$ is set equal to $(s_{tan},\dot{s}_{tan})$, and
$s_2$ is added to the switch list, ${\cal S} = \{s_1,s_2\}$.  {\bf (Step 2)} Maximum
acceleration is integrated forward from $(s_2,\dot{s}_2)$
until it hits $F$.  This curve is denoted $A_2$, and the $s$ value of the intersection,
$s_3$, is added to the switch list, yielding ${\cal S} = \{s_1,s_2,s_3\}$.  The
algorithm terminates.
}
\label{fig:mintimealgorithm}
\end{figure}

\subsection{Zero Inertia Points}

Until now, we have been making the assumption that each $a_i(s)$ is always nonzero.
In this usual case, the velocity limit occurs
when $L(s,\dot{s}) = U(s,\dot{s})$.  If $a_i(s) = 0$, however, the force at the
$i$th actuator is independent of $\ddot{s}$, and therefore
the $i$th actuator 
defines no acceleration constraints $L_i(s,\dot{s}), U_i(s,\dot{s})$.
Instead, it defines directly a {\em velocity\/} constraint using 
(\ref{eq:torqueconstraints}):
\begin{equation}
\force^{min}_i(s,\dot{s}) \leq b_i(s) \dot{s}^2 + c_i(s) \leq \force^{max}_i(s,\dot{s}).
\label{eq:velocityconstraint}
\end{equation}
In the case of a zero inertia point where $k$ of the $a_i(s)$ are
zero, let $\dot{s}^{max}_{zip}(s)$ be the maximum velocity satisfying
all $k$ constraints of the form (\ref{eq:velocityconstraint}), and let
$\dot{s}^{max}(s) = \min(v(s),s^{max}_{zip}(s))$.  Then
$\dot{s}^{max}(s)$ is the {\em true\/} velocity limit curve, 
generalizing the curve
$v(s)$ by allowing
for the possibility of zero inertia points.

If a point on the velocity limit curve $\dot{s}^{max}(s)$ is
determined by a zero inertia point velocity constraint, then
$L(s,\dot{s}) < U(s,\dot{s})$ at this point.  This point is called a
{\bf critical} point.  If, in addition, either $U(s,\dot{s})$
integrated forward from this point, or $L(s,\dot{s})$ integrated
backward from this point, would result in immediate penetration of the
velocity limit curve, then the point is called {\bf singular}.  See
Figure~\ref{fig:criticalpoints}.

\begin{figure}
\begin{center}
\includegraphics{../figure/book/trajectory/criticalpoints}
\end{center}
\caption{At critical and singular points, $U(s,\dot{s}) \neq L(s,\dot{s})$.
At a singular point, following $U$ forward or $L$ backward results in
penetration of the velocity limit curve.}
\label{fig:criticalpoints}
\end{figure}

At a singular point $(s_s,\dot{s}^{max}(s_s))$, let
\[
\ddot{s}_{tangent} = \dot{s}^{max}(s_s) \frac{d \dot{s}^{max}(s_s)}{ds}
\]
be the acceleration defined by the tangent to the velocity
limit curve.  If the velocity limit curve is not differentiable
at the singular point (as is often the case), then define
\begin{eqnarray}
\ddot{s}_{tangent}^+ & = &\dot{s}^{max}(s_s^+) \frac{d \dot{s}^{max}(s_s^+)}{ds} \nonumber \\
\ddot{s}_{tangent}^- & = &\dot{s}^{max}(s_s^-) \frac{d \dot{s}^{max}(s_s^-)}{ds} \nonumber
\end{eqnarray}
to be the right and left limits, respectively.  Then, to prevent
penetration of the velocity limit curve, the maximum feasible acceleration
at $(s_s,\dot{s}^{max}(s_s))$ is
\[
\ddot{s}^{max} = \min(\ddot{s}_{tangent}^+,U).
\]
Similarly, the minimum feasible acceleration is
\[
\ddot{s}^{min} = \max(\ddot{s}_{tangent}^-,L).
\]
This is shown graphically in Figure~\ref{fig:singularpoint}.

\begin{figure}
\begin{center}
\includegraphics{../figure/book/trajectory/singularpoint}
\end{center}
\caption{At this singular point on the velocity limit curve, integrating
$L(s,\dot{s})$ backward in time would result in penetration of the velocity
limit curve.  The true minimum feasible acceleration at this point is
$\ddot{s}^{min}$, defined by the left tangent of the velocity limit curve
at the singularity.
Similarly, integrating $U(s,\dot{s})$ forward results in penetration.
The true maximum feasible acceleration is $\ddot{s}^{max}$, defined
by the right tangent at the singularity.}
\label{fig:singularpoint}
\end{figure}

Critical points occur on a lower-dimensional subset of
the robot's configuration space where $M(q)$ is not full rank.  
If the path passes through this
subset transversally, the path will have isolated critical points.
If the path travels along this lower-dimensional subset, however,
we may have a continuous {\bf critical arc} of critical points.
Similarly, we may have {\bf singular arcs}.  

We can now modify the time scaling algorithm to properly account
for zero inertia points.  At singular points, we integrate
$\ddot{s}^{min}(s,\dot{s})$ and $\ddot{s}^{max}(s,\dot{s})$
instead of $L(s,\dot{s})$ and $U(s,\dot{s})$.
This will also allow the algorithm to ``slide'' along singular
arcs using an acceleration between $L(s,\dot{s})$ and
$U(s,\dot{s})$, instead of switching rapidly back and forth
between them.  In Step 4, we search the velocity limit curve
$\dot{s}^{max}(s,\dot{s})$ for any critical or tangent
point, not just tangent points.  

%If the path $q(s)$ is not sufficiently smooth, other strange
%artifacts could appear in the velocity limit curve, like discontinuities.

\begin{figure}
\begin{center}
\includegraphics{../figure/book/trajectory/RParmpath}
\end{center}
\caption{The path followed by the RP manipulator.}
\label{fig:RParmpath}
\end{figure}

\begin{example}
{\em 
Consider the two-joint RP robot arm with the dynamics derived in
Example~\ref{ex:RParm}.  We have planned a path to follow the straight
line shown in Figure~\ref{fig:RParmpath}, and we wish to find the
time-optimal time scaling of the path.  Let $x = (x_1,x_2)^T$ be the
Cartesian coordinates of the center of mass of the second link, as
shown in the figure.  The path we wish to follow, parameterized by
$s$, is expressed as
\begin{equation}
x(s) = (x_1(s),x_2(s))^T = (2s-1,1)^T, \;\; s\in [0,1].
\label{eq:xins}
\end{equation}
The first thing we need to do is express this path in the generalized
coordinates $q=(q_1,q_2)^T$ of Figure~\ref{fig:RParm}.  To do this, we
can define the {\bf forward kinematics} of the robot arm to be the
mapping $\phi$ from joint coordinates $q$ to Cartesian coordinates $x$:
\begin{eqnarray}
x & = & \phi(q) \\ \nonumber
(x_1,x_2)^T & = & (q_2 \cos q_1, q_2 \sin q_1)^T \nonumber
\end{eqnarray}
The {\bf inverse kinematics} $\phi^{-1}$ gives the joint coordinates $q$
as a function of the Cartesian coordinates $x$.  For some robot systems,
the inverse kinematics has multiple solutions for some $x$ and no
solutions for other $x$.
In our case, the solution is
unique within the reachable workspace of the robot (where $q_2\geq 0$):
\begin{eqnarray}
q & = & \phi^{-1}(x) \nonumber \\
(q_1,q_2)^T & = & ({\rm atan2}(x_2,x_1),\sqrt{x_1^2 + x_2^2})^T
\label{eq:inversekin}
\end{eqnarray}
where ${\rm atan2}(x_2,x_1)$ is the two argument arctangent returning
the unique angle in $[-\pi,\pi)$ of the Cartesian point $(x_1,x_2)$ (where
$x_1^2+x_2^2\neq 0$).
Plugging \eqref{eq:xins} into \eqref{eq:inversekin}, we find the 
parameterized path in joint coordinates
\begin{equation}
q(s) = ({\rm atan2}(1,2s-1),\sqrt{4s^2-4s+2})^T.
\label{eq:qins}
\end{equation}
Differentiating, we get the velocity and acceleration
\begin{eqnarray}
\dot{q} & = & \left(\frac{-\dot{s}}{2s^2-2s+1},\frac{(4s -2)\dot{s}}{\sqrt{4s^2-4s+2}} \right)^T \label{eq:velins} \\
\ddot{q} & = & \left(\begin{array}{cc}\frac{(4s-2)\dot{s}^2 + (-2s^2+2s-1)\ddot{s}}{(2s^2-2s+1)^2} \\ 
\frac{\sqrt{2}(\dot{s}^2 + (4s^3-6s^2+4s-1)\ddot{s})}{(2s^2-2s+1)^{3/2}} \end{array}\right). \label{eq:accins}
%\ddot{q} & = & \left(\frac{(4s-2)\dot{s}^2 + (-2s^2+2s-1)\ddot{s}}{(2s^2-2s+1)^2}, \frac{\sqrt{2}(\dot{s}^2 + (4s^3-6s^2+4s-1)\ddot{s})}{(2s^2-2s+1)^{3/2}} \right)^T \label{eq:accins}
\end{eqnarray}
Earlier we derived the equations of motion
\begin{eqnarray}
\force_1 & = & (I_1 + I_2 + m_1 r_1^2 + m_2 q_2^2) \ddot{q}_1 + 2m_2 q_2 \dot{q}_1 \dot{q}_2  \nonumber \\
& & + a_g(m_1 r_1 + m_2 q_2) \cos q_1 \\
\force_2 & = & m_2 \ddot{q}_2 - m_2 q_2 \dot{q}_1^2 + a_g m_2 \sin q_1.
\end{eqnarray}
Substituting in \eqref{eq:qins} -- \eqref{eq:accins}, we get
\[
a(s) \ddot{s} + b(s)\dot{s}^2 + c(s) = \force,
\]
where
\begin{eqnarray}
a(s) & = & 
\left(
\begin{array}{c}
\frac{I_1 + I_2 + 2m_2 + m_1 r_1^2 - 4 m_2 s+ 4m_2s^2}{-2s^2+2s-1} \\
\frac{\sqrt{2}m_2(2s-1)}{\sqrt{2s^2-2s+1}}
\end{array}
\right) \\
b(s) & = & 
\left(
\begin{array}{c}
\frac{2(I_1+I_2+m_1 r_1^2)(2s-1)}{(2s^2-2s+1)^2} \\
0
\end{array}
\right) \\
c(s) & = & 
\left(
\begin{array}{c}
(m_1 r_1 + m_2 \sqrt{4s^2-4s+2}) a_g \cos({\rm atan2}(1,2s-1)) \\
m_2 a_g \sin({\rm atan2}(1,2s-1))
\end{array}
\right).
\end{eqnarray}
Note that $s=\frac{1}{2}$ defines a zero inertia point, as $a_2(\frac{1}{2})=0$.
(Understand this intuitively by considering the $s = \frac{1}{2}$ point
in Figure~\ref{fig:RParmpath}.)
In this case there is no velocity constraint
due to the second actuator, since $b_2 = 0$.  (See Problem~\ref{problem:RParmb2}).

We now choose the following parameters for the robot arm:
$m_1 = 5$~kg, $I_1 = 0.1$~kg-m$^2$, $r_1 = 0.2$~m, $m_2 = 3$~kg, and $I_2 = 0.05$~kg-m$^2$.  The actuator
limits are taken to be $\pm 20$~N-m for joint 1 and $\pm 40$~N for
joint 2, and gravity is $a_g = 9.8 m/s^2$.
Figure~\ref{fig:timescale-new} shows the time-optimal trajectory along
the path for these choices.  The minimum-time execution of the path
is approximately 0.888~s.  Note that one of the actuators is
saturated at all times.  In this example, the velocity limit curve
is never reached, so there is just one switch between maximum 
and minimum acceleration.
}
\label{ex:RParmpath}
\end{example}

\begin{figure}
\begin{center}
\includegraphics{../figure/book/trajectory/timescale-new}
\end{center}
\caption{The time-optimal actuator histories and trajectory of the
RP manipulator along the straight line path of Example~\ref{ex:RParmpath}.} 
\label{fig:timescale-new}
\end{figure}

\subsection{Global Time-Optimal Trajectory Planning}

The time-scaling algorithm finds the time-optimal trajectory along a
given path.  What if our real goal is to find the time-optimal
trajectory between two states when we are free to choose any path?
Can we use the time-scaling algorithm in conjunction with a 
collision-free path planner?  Conceptually, imagine running the
time-scaling algorithm on all possible paths between the start and
goal states.  Then the fastest of these is the global time-optimal
trajectory.

Naturally, the problem is how to efficiently test a large number of
possible paths.  One approach to this problem was proposed by Shiller
and Dubowsky~\cite{Shiller91}.  The approach is quite involved, and we
only sketch it here.  The first step is to define a grid on the
workspace and construct all collision-free paths (without sharp turns)
between the start and goal states moving along edges or diagonals of
the grid.  The next step is to quickly compute rough lower-bound
estimates of the traveling times of these paths using a maximum
velocity limit during the motion.  The fastest paths are selected and
smoothed by using the grid points as control points for cubic splines.
The best of these paths is then submitted to the full time-scaling
algorithm, generating an upper bound on the optimal travel time.  All
paths with lower bounds above this upper bound can be pruned.  Of the
remaining paths, the lower bounds are more carefully calculated, and
the process continues, using increasingly accurate estimates of the
lower bounds as the number of candidate paths is reduced.  When the
pruning process has ended, only the best path in each
path-neighborhood is considered further.  These best paths are 
submitted to a local optimization which may locally alter the
paths to allow them to be executed more quickly.
This process uses the travel times
returned by the time-scaling algorithm as the objective function.

This approach combines collision-free path planning and time scaling
in an iterative fashion to arrive at a global near-time-optimal
trajectory.  In the next section, we discuss methods that do not
separate the path planning and time scaling problems, but solve 
directly for a collision-free trajectory.

%practical note, can't follow these trajectories

\section{Direct Trajectory Planning}
\label{sec:direct}

In Section~\ref{sec:decoupled}, trajectory planning is decoupled into
collision-free path planning followed by time scaling.  In this
section, we study methods for planning the trajectory directly in
the state space.  If we are interested in finding trajectories that
optimize some cost function, such as motion time or expended energy,
ideas from optimal control theory provide necessary conditions on the
trajectories.  Unfortunately, these conditions are complex for almost
any robot system and cannot be solved analytically.  Because of this,
we consider two numerical approaches:  nonlinear optimization and
grid-based search (dynamic programming).

First we describe how to transform the optimal control problem to a
finite-dimensional parameter optimization problem, allowing nonlinear
optimization to be used to numerically solve the optimality
conditions.  If the problem is well formulated (e.g., the objective
and constraint functions are sufficiently smooth), nonlinear
optimization may result in rapid convergence to a locally optimal
trajectory.  Drawbacks of this approach are that the method requires
an initial guess (possibly provided by a grid-based search method),
and the locally optimal solution reached generally depends heavily on
this guess.  Also, evaluation of constraint and objective functions,
and their gradients, may be computationally costly.

We then introduce a grid-based search method allowing the user to
control how near to time-optimal the resulting solution is, while
meeting ``safety'' requirements on obstacle avoidance.  The solution
is approximate in that the goal state may not be exactly reached, but
the user has control over how large the final error can be.  An
advantage is that this is a global approach, unlike gradient-based nonlinear
optimization.  A drawback of grid-based search is that the size of the grid grows
exponentially in the dimension of the state space, and the approach
may not be practical for high-dimensional systems.

A third approach is to use rapidly-exploring random trees (RRT's), as
described in Chapter ??.  This approach trades off optimality for
planner run-time---it may be able to quickly find a feasible
trajectory which is in no sense optimal.

Finally, a fourth approach is based on artificial potential fields
(Chapter ??).  An artificial potential field is constructed based on
the obstacles and the goal to make the goal configuration attractive (at least
locally).  The robot senses its current configuration,
calculates the gradient of the artificial potential at this
configuration, and applies the gradient forces at the actuators.
(Damping forces are also included to stabilize the goal
configuration.)  A potential field, therefore, implicitly defines a
trajectory to the goal from all initial states.  This approach is
fundamentally different from the previous ``open loop'' approaches in
that a feedback law is specified for all robot states.

\subsection{Optimal Control}

Given a dynamic system
\begin{equation}
M(q) \ddot{q} + C(q,\dot{q})\dot{q} + g(q) = \force,
\label{eq:motionequations}
\end{equation}
we would like to find a solution $(q(t),\force(t)), t \in [0,t_f]$ to
\eqref{eq:motionequations} that avoids obstacles and joint limits, 
respects actuator limits, and
takes the system from the initial state $(q_0,\dot{q}_0)$ at time $t=0$ to the
final state $(q_f,\dot{q}_f)$ at time $t=t_f$.  Of all the trajectories
that accomplish this, we might want to find one that minimizes some
objective function $J$.  In general, $J$ might be a function of the
controls, the trajectory, and the total motion time $t_f$, i.e.,
$J = J(\force(t),q(t),t_f)$.  

Before proceeding further, let's express the state of the system
as $x = (q^T,\dot{q}^T)^T$, and rewrite the equations of motion in the
general form
\begin{equation}
\dot{x}(t) = f(x(t),\force(t),t),
\label{eq:stateequations}
\end{equation}
where $f$ is the vector differential equation describing the
kinematics and dynamics of the system.  In our case, the state
equations do not change with time, so $f(x,\force,t) = f(x,\force)$.  The
state equations (\ref{eq:stateequations}) can be viewed as 
constraints defining the relationship between $x(t)$ and $\force(t)$.  Let the
objective function $J$ be written
\begin{equation}
J = \int_{0}^{t_f} {\cal L}(x(t),\force(t),t) dt,
\label{eq:costfunction}
\end{equation}
where the integrand ${\cal L}$ is called the {\bf Lagrangian}.  (The
Lagrangian ${\cal L}$ for optimal control is actually a generalization of
the Lagrangian $L$ for dynamics.)
A typical choice
of ${\cal L}$ is ``effort,'' modeled as a quadratic function of
the control, e.g., ${\cal L} = \force^T W \force$, where $W$ is a positive definite
weighting matrix, for example the identity matrix.  Another common choice
is to leave $t_f$ free and choose
${\cal L}=1$, implying $J = t_f$, a minimum-time problem.

For now, let us ignore the problem of actuator limits and obstacles,
and assume that the final time $t_f$ is fixed.
The problem then is to find a state and control history 
$(x(t),\force(t)), t \in [0,t_f]$ that satisfies the constraints
(\ref{eq:stateequations}), satisfies the terminal conditions
$x(t_f) = x_f$, and minimizes the cost (\ref{eq:costfunction}).
To write a necessary condition for optimality, we define the
{\bf Hamiltonian} ${\cal H}$
\begin{equation}
{\cal H}(x(t),\force(t),\lambda(t),t) = {\cal L}(x(t),\force(t),t) + \lambda^T f(x(t),\force(t)),
\label{eq:hamiltonian}
\end{equation}
where $\lambda(t)$ is a vector of {\bf Lagrange multipliers}.\footnote{These
Lagrange multipliers play a similar role to those in dynamics with 
constraints---they are used to enforce constraints, here the state equation
constraints.}  Then the {\bf Pontryagin Minimum Principle}\footnote{Often
known as the Pontryagin Maximum Principle.  In our case, we are minimizing
a cost function; we could equivalently maximize a utility function,
or the negative of the cost
function.} says that at an optimal solution $(x^*(t),\force^*(t),\lambda^*(t))$,
\begin{equation}
{\cal H}^*(t) = {\cal H}(x^*(t),\force^*(t),\lambda^*(t),t) \leq {\cal H}(x^*(t),\force(t),\lambda^*(t),t).
\label{eq:PMP}
\end{equation}
In other words, if the control history $\force^*(t)$ is optimal, then at any
time $t$, any other feasible control $\force(t)$ will give an ${\cal H}(t)$ 
greater 
than or equal to that of the optimal ${\cal H}^*(t)$.  In the absence
of any other constraints on the state and control, then, a
necessary condition for optimality can be written
\begin{equation}
\frac{\partial {\cal H}}{\partial \force} = 0.
\label{eq:optimalitycondition}
\end{equation}
This says that the linear sensitivity of the Hamiltonian to changes
in $\force$ is zero, meaning 
that the control is {\bf extremal}, but not necessarily optimal.
This is analogous to evaluating $dy(t)/dt$ to determine the extrema
of a scalar function $y(t)$.  To discover if an extremum is a minimum or
a maximum, higher derivatives must be considered.  A sufficient
condition for local optimality of a solution is that the Hessian
of the Hamiltonian be positive definite along the trajectory of the solution:
\begin{equation}
\frac{\partial^2{\cal H}}{\partial \force^2} > 0.
\label{eq:hamiltonianhessian}
\end{equation}
This is known as the convexity or Legendre-Clebsch condition.

%At an extremal solution, 
The Lagrange multipliers evolve according to
the adjoint equation
\begin{equation}
\dot{\lambda} = -\frac{\partial {\cal H}}{\partial x}.
\label{eq:adjoint}
\end{equation}
Equations~\eqref{eq:optimalitycondition} and \eqref{eq:adjoint} 
sometimes contain sufficient information to solve
for the extremal trajectories.

\begin{example}
{\em
Consider a simple one degree-of-freedom double integrator system,
$\ddot{q} = \force$, such as a point mass on a line actuated by a 
force.  Let $x = (x_1,x_2)^T = (q,\dot{q})^T$, so the
equations of motion can be written in the form
\[
\dot{x} = f(x,\force), \;\; \left(\begin{array}{c} \dot{x}_1 \\ \dot{x}_2 \end{array}\right) 
= \left( \begin{array}{c} x_2 \\ \force \end{array} \right).
\]
Choose the objective function
\[
J = \int_{0}^{t_f} \force^2 dt.
\]
Then the Hamiltonian is
\[
{\cal H} = \force^2 + \lambda_1 x_2 + \lambda_2 \force
\]
and the necessary condition (\ref{eq:optimalitycondition}) is written
\[
\frac{\partial {\cal H}}{\partial \force} = 2 \force + \lambda_2 = 0.
\]
The adjoint equation (\ref{eq:adjoint}) is written
\[
\dot{\lambda} = - \frac{\partial {\cal H}}{\partial x}, \;\; 
\left(\begin{array}{c} \dot{\lambda}_1 \\ \dot{\lambda}_2 \end{array} \right) = 
- \left( \begin{array}{c} 0 \\ \lambda_1 \end{array} \right).
\]
From these equations, we have $\lambda_1$ is constant, $\lambda_2$ is a
linear function of time, and $\force$ is a linear function of time,
e.g., $\force(t) = c_0 + c_1 t$.  

Now we can specify the initial and final state for the system to solve for
the control and state history.  Let $x(0) = (0,0)$ and $x(t_f) = (d,0)$,
that is, the system starts at rest and ends at rest having moved a distance $d$
in time $t_f$.
Then we have the stopping conditions that the first and second integral
of $\force(t)$ evaluated over $[0,t_f]$ be zero and $d$,
respectively:
\begin{eqnarray}
x_2(t_f) = \int_0^{t_f} \force(t) dt & = & 0 \nonumber \\
x_1(t_f) = \int_0^{t_f} \int_0^{t} \force(\eta) d \eta\; dt & = & d. \nonumber
\end{eqnarray}
Solving, we get $c_0 = 6d/t_f^2,c_1 = -12d/t_f^3$
defining the extremal control history.  The extremal
state history is obtained by integration.

To check if this extremal solution is a minimizer, we can
use the convexity condition.  In this case,
the Hessian is the scalar $\partial^2{\cal H}/\partial \force^2 = 2 > 0$, indicating
that the solution is indeed (locally) optimal.  
}
\end{example}

In the previous example, the convexity condition is satisfied.
However, if an extremum is achieved
($\partial {\cal H}/\partial \force = 0$)
but convexity is not
($\partial^2 {\cal H}/\partial \force^2$ is only positive {\em semi\/}-definite),
it does not mean that the control is not optimal.  In this case,
auxiliary conditions have to be satisfied to ensure optimality.
An optimal control of this type is an example of a {\bf singular}
optimal control, as in the previous section with
time-optimal control at zero inertia points.

What if there are actuator limits or obstacles?  At an optimal
solution, the minimum principle~(\ref{eq:PMP}) will always be satisfied, but
the control or state history may bump up against limits preventing
(\ref{eq:optimalitycondition}) from being satisfied.  
The optimal solution may be constrained by these limits
rather than the extremality condition \eqref{eq:optimalitycondition}.
Consider the one degree-of-freedom double integrator above, with the
actuator limits $|\force| <
\force^{max}$, and choose the minimum-time objective function
\[
J = \int_{0}^{t_f} 1 dt,
\]
where the time $t_f$ is left free.  The Hamiltonian is ${\cal H} = 1 +
\lambda_1 x_2 + \lambda_2 \force$, and $\partial {\cal
H}/\partial \force = \lambda_2$ does not contain the control variable.
Therefore, it provides no information on the choice of $\force(t)$.
Further, $\partial^2{\cal H}/\partial \force^2$ is zero, so
${\cal H}$ is not convex.  We know, however, that the optimal solution
for this problem is a ``bang-bang'' trajectory, just like the bang-bang
trajectories found by the time-scaling algorithm.

We can recover the bang-bang solution using the minimum principle
(\ref{eq:PMP}).  We write
\begin{eqnarray}
{\cal H}^*(t) = {\cal H}(x^*(t),\force^*(t),\lambda^*(t)) & \leq & {\cal H}(x^*(t),\force(t),\lambda^*(t)) \nonumber \\
1+\lambda_1^*(t) x_2^*(t) + \lambda_2^*(t) \force^*(t) & \leq & 1+\lambda_1^*(t) x_2^*(t) + \lambda_2^*(t) \force(t) \nonumber \\
\lambda_2^*(t) \force^*(t) & \leq & \lambda_2^*(t) \force(t). \nonumber
\end{eqnarray}
Therefore, $\force^*(t)$ is the maximum feasible value $\force^{max}$
when $\lambda_2^*(t) < 0$ and the minimum feasible value $-\force^{max}$
when $\lambda_2^*(t) > 0$.  As before, $\lambda_2$ is a linear function
of time, and the terminal state conditions allow us to find the
complete solution.
%Using the terminal state as
%before, we can solve for the linear function $\lambda_2^*(t)$ which
%defines the switching time.

Only for very simple systems is it possible to solve the optimality
conditions analytically.  In most cases, it is necessary to resort
to numerical methods, such as gradient methods or dynamic programming.
In the next section we discuss gradient-based nonlinear optimization.
In the following section we introduce a grid-based search method for
finding near-time-optimal trajectories based on dynamic programming.

For more on optimal control, see the excellent 
texts~\cite{Bryson69,Bryson98,Kirk70,FLewis95,Pontryagin62,Stengel94}.

\subsection{Nonlinear Optimization}

The general problem can be stated 
\begin{eqnarray}
{\rm find} & & t_f, q(t), \force(t) \\
{\rm minimizing} & & J(\force(t),q(t),t_f) \\
{\rm subject \; to} & & M(q(t))\ddot{q}(t) + C(q(t),\dot{q}(t))\dot{q} + g(q(t)) = \force(t), \label{eq:dynamicconstraints} \\ 
& & \hspace{2in} 0 \leq t \leq t_f \nonumber \\
& & \force^{min}(q(t),\dot{q}(t)) \leq \force(q(t),\dot{q}(t)) \leq \force^{max}(q(t),\dot{q}(t)), \label{eq:forceconstraints} \\
& & \hspace{2in} 0 \leq t \leq t_f \nonumber \\
& & h(q(t)) \leq 0, \;\;\; 0 \leq t \leq t_f \label{eq:obstacleconstraints} \\
& & q(0) = q_0, \; \dot{q}(0) = \dot{q}_0 \\
& & q(t_f) = q_f, \; \dot{q}(t_f) = \dot{q}_f,
\end{eqnarray}
where $h(q) \leq 0$ are configuration inequality constraints representing
obstacles and joint limits.  

To approximately solve this problem by nonlinear optimization, we must
approximate the continuous constraints (\ref{eq:forceconstraints}) and 
(\ref{eq:obstacleconstraints}) by a finite number of constraints.
This is typically done by ensuring that the constraints are satisfied
at a fixed number of points distributed evenly over the interval
$[0,t_f]$.  We must also choose a finite-parameter representation of
the state and control histories.  We have 
three choices of how to do this:
\begin{enumerate}
\item {\em Parameterize the trajectory $q(t)$.\/}  In this case,
we solve for the parameterized trajectory directly.  The control
forces $\force$ at any time are calculated using
(\ref{eq:dynamicconstraints}).  

\item {\em Parameterize the control $\force(t)$.\/}  We solve
for $\force(t)$ directly, and calculating 
the state $(q(t),\dot{q}(t))$ requires integrating the dynamic
equations of motion (\ref{eq:dynamicconstraints}).  
 
\item {\em Parameterize both $q(t)$ and $\force(t)$.\/}  We have a
larger number of variables, since we are parameterizing
both $q(t)$ and $\force(t)$.  Also, we have a larger number of constraints,
as $q(t)$ and $\force(t)$ must
satisfy the dynamic equations (\ref{eq:dynamicconstraints})
explicitly, typically at a fixed number of points distributed evenly
over the interval $[0,t_f]$.  We must be careful to
choose their parameterizations to be consistent with each other, so 
that the dynamic equations can be satisfied at these points.
\end{enumerate}

A trajectory or control history can be parameterized in any number of
ways.  The parameters can be the coefficients of a polynomial in time,
the coefficients of a truncated Fourier series, spline coefficients,
wavelet coefficients, piecewise constant acceleration or torque
segments, etc.  In addition to the parameters for the state or control
history, the total time $t_f$ may be another control parameter.  The
choice of parameterization has implications on the efficiency of the
calculation of the state and control.  The choice of parameterization
also determines the sensitivity of the state and control to the
parameters, and whether each parameter affects the profiles at all
times $[0,t_f]$ or just on a finite time support base.  These are
important factors in the stability and efficiency of the numerical
optimization.

Let $X$ be the vector of control parameters to be solved.  Assuming that
either $q(t)$ or $\force(t)$ has been parameterized (but not both), and
that the $k+1$ constraint checks are spaced at $\Delta t = t_f/k$ intervals,
the nonlinear optimization can be written
\begin{eqnarray}
{\rm find} & & X \\
{\rm minimizing} & & J(X) \label{eq:objective} \\
{\rm subject \; to} & & \force^{min}(X,j\Delta t) \leq \force(X,j\Delta t) \leq \force^{max}(X,j\Delta t), \label{eq:numoptcon1} \\
& & \hspace{2in} j = 0 \ldots k \nonumber \\
& & h(X,j \Delta t) \leq 0, \;\;\; j=0 \ldots k  \\
& & q(X)(0) = q_0, \; \dot{q}(X)(0) = \dot{q}_0 \\
& & q(X)(t_f) = q_f, \; \dot{q}(X)(t_f) = \dot{q}_f. \label{eq:numoptcon4}
\end{eqnarray}

A nonlinear program of this type can be solved by a number of methods,
including sequential quadratic programming (SQP).  Any solver will
require the user to provide functions to take a guess $X$ and
calculate the objective function \eqref{eq:objective} and the constraints
\eqref{eq:numoptcon1} -- \eqref{eq:numoptcon4}.  Often
the objective function will have to be calculated by numerical
integration.  All solvers also need the gradients of the objective
function and the constraints with respect to $X$.  These can be
calculated numerically by finite differences, or, if possible,
calculated analytically.  Finally, most solvers make use of
Hessians of the objective function and constraint functions with
respect to $X$.  Most solvers update a numerical approximation to
these Hessians rather than requesting the user to provide these.
Details on different methods for nonlinear optimization can be found
in \cite{Gill,More,Nocedal99}.  Code for nonlinear optimization includes
FSQP and CFSQP~\cite{FSQP}, 
NPSOL~\cite{NPSOL}, 
and routines in the IMSL~\cite{IMSL}, NAG~\cite{NAG}, and
MATLAB Optimization Toolbox libraries.

The most important point is that for any of these solvers to work, the
objective and constraints must be sufficiently smooth with respect to
the control parameters $X$.  They must be at least $C^1$, but usually
$C^2$ so that Hessian information can be used to speed convergence.  A
key part of the problem formulation is ensuring this smoothness.  
When possible, the gradients, and even the Hessians,
should be calculated analytically.  This will minimize the possibility
of the solver failing to converge due to numerical problems, a very
real practical concern!  Even if
the objective function is calculated approximately by numerical
integration, it may be possible to calculate the exact gradient of
this approximation analytically.

Since nonlinear optimization uses local gradient information,
it will converge to a locally optimal solution.  For some
problems, the control parameter space will be littered with many
local optima.  Therefore, the solution achieved will depend heavily
on the initial guess.  To ensure a good solution, the process can be
started from several initial guesses, keeping the best local optimum.
Nonlinear optimization can also be used as a final step
to locally improve a trajectory found by some other global search method.

%note CFSQP keeps feasible iterates --- finds solution satisfying
%all linear constraints and nonlinear inequality constraints

\subsection{Grid-based Search}

An alternative approach specifically for time-optimal trajectory planning
uses grid search.  As motivation, consider
the simple double-integrator system
\begin{eqnarray}
\ddot{q} & = & a \nonumber \\
|a| & \leq & a_{max}, \nonumber
\end{eqnarray}
where $a$ is the acceleration control.
Let $q$ be one-dimensional, so the system can be viewed
as a point mass moving on a line with a control force.
The time-optimal control from an initial state $(q_0,\dot{q}_0)$ to a goal
state $(q_f,\dot{q}_f)$ is bang-bang---the actuator is saturated at all
times.  For this simple example, the solution uses maximum acceleration
followed by maximum deceleration.

Things will not be so simple when we deal with multi-dimensional
problems with obstacles and velocity limits, 
so let's consider a grid-based approach that
we will be able to generalize to more dimensions.  First, we
discretize the control set to $\{-a_{max},0,a_{max}\}$.  Next,
we choose a timestep $h$.  Now, beginning from $(q_0,\dot{q}_0)$, we
integrate the three controls forward in time by $h$ to obtain three
new states.  Think of the initial state $(q_0,\dot{q}_0)$ as the root
of a tree, and the three new states as children of the root (Figure~\ref{fig:tree}).  From 
each of these three, we integrate the controls forward to obtain a
new level of the tree.  We continue in a breadth-first fashion.
If the trajectory to a new node in the tree passes through an
obstacle or exceeds a velocity limit, this node is pruned from the
tree.  The search continues until a trajectory reaches a state in a
neighborhood of the goal state.  
The trajectory is specified by the
piecewise-constant controls to traverse the tree from the root node to this
final node.  
Since the search is breadth-first, exploring all reachable states at time
$kh$ before moving on to time $(k+1)h$, the trajectory is
time-optimal for the chosen discretization of time and controls.

\begin{figure}
\begin{center}
\includegraphics{../figure/book/trajectory/tree}
\end{center}
\caption{The search tree for three controls.}
\label{fig:tree}
\end{figure}

Why do we call this a grid-based search?  Because each of the
nodes reached during the growth of the tree lies on a regular grid
on the $(q,\dot{q})$ state space.  From any state, the new
state obtained by integrating one of the discretized accelerations for time $h$ will
involve a change in $\dot{q}$ equal to an integral multiple of $a_{max} h$
and a change in $q$ equal to an integral multiple of $\frac{1}{2}a_{max} h^2$.
An example of such a grid is shown in Figure~\ref{fig:grid}.  The
search tree shown on this grid is two levels deep, beginning at
$(q_0,\dot{q}_0) = (0,0)$.  The key point here is that,
given some bounds on $q$ and
$\dot{q}$, the
size of the grid is easily computed, so
an upper-bound on the computational complexity of the search of this
grid is also easily computed.  

\begin{figure}
\begin{center}
\includegraphics{../figure/book/trajectory/grid}
\end{center}
\caption{The search tree of Figure~\ref{fig:tree} shown on the state space grid.  The
search begins at (0,0).  The actual trajectories between vertices
are not straight lines, but quadratics.}
\label{fig:grid}
\end{figure}

Let us now consider a more general problem statement.  The system
is described by
$q \in D \subset \real^{n_{\cal Q}}$, where $D$ is a bounded a subset of
$\real^{n_{\cal Q}}$, and velocity and acceleration bounds of the form
\begin{eqnarray}
|\dot{q}_i| & \leq & v_{max}, \;\; i = 1 \ldots n_{\cal Q} \nonumber \\
|\ddot{q}_i| & \leq & a_{max}, \;\; i = 1 \ldots n_{\cal Q}. \nonumber
\end{eqnarray}
Note
that this is a very limited class of systems, as the maximum feasible $\ddot{q}_i$'s
are constant and independent of the state $(q,\dot{q})$.
An example of such a system might be a point in 
$n_{\cal Q}$-dimensional Euclidean space with a thruster for each degree-of-freedom,
or a Cartesian robot arm consisting of all prismatic joints and
actuators with state-independent bounds.
The problem is to find a collision-free, approximately time-optimal trajectory from 
$(q_0^*,\dot{q}_0^*)$ (at or near the desired start state $(q_0,\dot{q}_0)$)
to a goal state near the desired goal state $(q_f,\dot{q}_f)$.  

The algorithm uses a discretized control set $A$ consisting of the
cross-products of $\{-a_{max},0,a_{max}\}$ for each degree-of-freedom,
yielding $3^{n_{\cal Q}}$ distinct controls (Figure~\ref{fig:extremals}).  These controls result in a regular
grid on $2n_{\cal Q}$-dimensional state space, similar to Figure~\ref{fig:grid}.
The algorithm is described in pseudo-code
below.

\begin{figure}
\begin{center}
\includegraphics{../figure/book/trajectory/extremals}
\end{center}
\caption{The control discretization for $n_{\cal Q}=2$.}
\label{fig:extremals}
\end{figure}

\newpage

\begin{tabbing}
\hspace{0.1in} \= \hspace{0.1in} \= \hspace{0.1in} \= \hspace{0.1in} \= \hspace{0.1in} \= \\
\rule{4.5in}{.01in} \\
{\bf procedure} {\em grid\_search} \\
\> Place start node $(q_0^*,\dot{q}_0^*)$ at root of tree $T$ (level 0)\\
\> Set $level = 0$, $solved=$ FALSE, $ANS =\{\}$ \\
\> {\bf While} not $solved$ {\bf do} \\
\> \> {\bf If} no nodes in level $level$ of $T$, {\bf return} FAILURE \\
\> \> {\bf For} each $node$ in level $level$ of $T$ {\bf do} \\
\> \> \> {\bf For} each $control$ in $A$ {\bf do} \\ 
\> \> \> \> Integrate $control$ for time $h$ from $node$, getting $newnode$ \\
\> \> \> \> {\bf If} trajectory does not pass close to an obstacle, does not exceed
$v_{max}$, \\
\> \> \> \> and $newnode$ has not previously been reached, add $newnode$ \\
\> \> \> \> as child of $node$ to $T$ \\
\> \> \> \> {\bf If} trajectory enters the goal neighborhood, set $solved =$ TRUE \\
\> \> \> \> and store $newnode$ in list $ANS$ \\
\> \> \> {\bf end for} \\
\> \> {\bf end for} \\
\> \> Increment $level$ \\
\> {\bf end while} \\
\> For each node in $ANS$, find the trajectory that reaches the goal neighborhood
first.  \\
{\bf end}\\
\rule{4.5in}{.01in} 
\end{tabbing} 

This algorithm is straightforward, except for one twist: the algorithm
prunes a node if the trajectory passes {\em close\/} to an obstacle,
not just if it passes through an obstacle.  Thus the algorithm will
only return trajectories that are {\em safe\/}.  We define a trajectory
to be $\delta_v(c_0,c_1)${\em -safe\/} if there exists a speed-dependent ball of free configurations
about each $q$ in the trajectory, where the radius of the ball is
$c_0 + c_1 ||\dot{q}_i||$.  $c_0$ and $c_1$ are safety parameters.

Since the algorithm uses a finite timestep $h$, any trajectory it
finds will only be an {\em approximately\/} time-optimal safe
trajectory.  Instead of directly choosing $h$, the user could have
control over a parameter $\epsilon, 0 < \epsilon < 1$, which defines
the crudeness of the approximation.  Larger values of $\epsilon$
correspond to cruder approximations, and the timestep $h$ goes to zero
as $\epsilon$ goes to zero.  As we will see, $\epsilon$ may be viewed
as a measure of how much we will allow $\delta_v(c_0,c_1)$-safety to
be violated.

Although the algorithm itself is straightforward, analysis of the
algorithm is quite involved.  Given a desired safety margin and
$\epsilon$, we would like to know how to choose $h$ to guarantee
completeness of the algorithm, and how $\epsilon$ and the safety
margin relates to the algorithm's running time.  This is the problem
that was studied in detail by Donald and Xavier
\cite{Donald95b}, building on work by Canny, Donald, Reif, and 
Xavier~\cite{Canny88,Donald93b}.
Their analysis holds for a point robot with $n_{\cal Q}=2$ or $3$ moving
among polygonal or polyhedral obstacles.  They give us the following
result.

\begin{theorem}
Let $l$ be the diameter of the robot configuration space, $c_0$ and $c_1$
be safety parameters, and $v_{max}$ and $a_{max}$ be the velocity
and acceleration limits.  Let $0<\epsilon<1$.  Assume there exists a
$\delta_v(c_0,c_1)$-safe trajectory from $(q_0,\dot{q}_0)$ to $(q_f,\dot{q}_f)$
taking time $T_{opt}$ by some control function with $|\ddot{q}_i| \leq a_{max}$
for all $i$ and $t\in[0,T_{opt}]$.  Choose the largest $h$ so that
\[
h \leq \frac{v_{max}}{a_{max}}, \;\; h \leq \frac{c_0 \epsilon}{2 a_{max}c_1(1-\epsilon)+ 5 v_{max}},
\]
and $v_{max}$ is an integral multiple of $a_{max} h$.  Choose an approximate
starting state $(q_0^*,\dot{q}_0^*)$ where, for each coordinate $i$,
\begin{eqnarray}
\dot{q}_0^* & = & {\rm the \; multiple \; of }\; a_{max} h \; {\rm closest \; to } \; \dot{q}_0 \nonumber \\
q_0^* & = & q_0 - \frac{h^2}{2}(\dot{q}_0 - \dot{q}_0^*). \nonumber
\end{eqnarray}
Define the goal neighborhood to be all points
within $(\frac{5 a_{max}h^2}{2},2 a_{max}h)$ of $(q_f,\dot{q}_f)$.  (Note that
the distance from $(q_0^*,\dot{q}_0^*)$ to $(q_0,\dot{q}_0)$, and the
distance from any point in the goal neighborhood to $(q_f,\dot{q}_f)$, is $O(\epsilon)$.)  Then
the algorithm outlined above is guaranteed to find a 
$(1-\epsilon)\delta_v(c_0,c_1)$-safe trajectory taking at most time $T_{opt}$
from $(q_0^*,\dot{q}_0^*)$ to the goal neighborhood.  The running time
of the algorithm is
\[
O\left( c^{n_{\cal Q}} N \left(\frac{v_{max}(a_{max}c_1 + v_{max})^3 l}{a_{max}^2 c_0^3 \epsilon^3}\right)^{n_{\cal Q}}\right),
\]
where $n_{\cal Q} = 2$ or $3$, $c$ is a constant, and $N$ is the number of faces
in the obstacles.  In terms of the dimension $n$ and the 
approximation variable $\epsilon$, the running time goes as 
$O((\frac{1}{\epsilon})^{3n_{\cal Q}})$, i.e., polynomial in $\epsilon$ and
exponential in $n_{\cal Q}$.
\end{theorem}

The proof of this theorem is beyond the scope of this chapter, and it 
depends on efficient goal and safety checking 
between grid vertices.  
One important property of the algorithm is that
the $(1-\epsilon)\delta_v(c_0,c_1)$-safe trajectory found by the algorithm
may be quite different than the time-optimal $\delta_v(c_0,c_1)$-safe
trajectory.  The only guarantee is that the running time of the approximate
trajectory will be no greater than $T_{opt}$.

Figure~\ref{fig:optimalpaths} shows a cartoon example of different kinds of optimal
paths for a point in the plane.  The true time-optimal trajectory, with
no consideration for safety, is a straight-line motion between the start and the
goal.  In this example, the
time-optimal $\delta_v(c_0,c_1)$-safe trajectory avoids the narrow
passage, as it would require unacceptably slow speeds to be safe.  Finally,
the algorithm outlined above finds the approximately time-optimal
$(1-\epsilon)\delta_v(c_0,c_1)$-safe trajectory
from an approximate start state to an approximate goal state.

\begin{figure}
\begin{center}
\includegraphics{../figure/book/trajectory/optimalpaths}
\end{center}
\caption{A time-optimal motion, a $\delta_v(c_0,c_1)$-safe time-optimal
motion, and a solution found by the algorithm.  The dashed box at the goal
is the the goal neighborhood projected to the configuration space.}
\label{fig:optimalpaths}
\end{figure}

We would like to generalize the grid-search algorithm to handle more
general dynamic systems of the form \eqref{eq:robotcoriolis}, such
as open-chain robot manipulators.  Unlike
the previous case, the dynamics are not decoupled generally, and the
current state of the system affects the feasible $\ddot{q}$:
\begin{equation}
\ddot{q} = M^{-1}(q) (\force - C(q,\dot{q})\dot{q} - g(q)).
\label{eq:feasibleaccels}
\end{equation}
To simplify matters somewhat, we will assume constant bounds on the
available controls, possibly different for each $i$:
\begin{equation}
|\force_i| \leq \force_i^{max}.
\label{eq:torquebox}
\end{equation}
At a given state $(q,\dot{q})$, Equation~\eqref{eq:feasibleaccels}
transforms the rectangular parallelepiped in $\force$ space implied by
\eqref{eq:torquebox} to a parallelepiped in the $\ddot{q}$ space, as
shown in Figure~\ref{fig:accelbounds}.  Let ${\bf A}(q,\dot{q})$ denote the
state-dependent parallelepiped of feasible $\ddot{q}$.
We assume that ${\bf A}(q,0)$ contains the origin
of the $\ddot{q}$ space in its interior for all $q \in {\cal Q}$, i.e.,
the actuators are strong
enough to hold the robot stationary at any configuration.

\begin{figure}
\begin{center}
\includegraphics{../figure/book/trajectory/accelbounds}
\end{center}
\caption{The equations of motion turn the feasible controls $\force$ 
into a parallelepiped ${\bf A}(q,\dot{q})$ of feasible $\ddot{q}$.}
\label{fig:accelbounds}
\end{figure}

To apply the algorithm from before, imagine placing a constant grid on
the $\ddot{q}$ space, as shown in Figure~\ref{fig:accelbounds},
discretizing the feasible accelerations.  For a fixed time interval
$h$, these controls will again create a regular grid of reachable
points on the state space.  For the current state, we use the
$\ddot{q}$ grid points inside ${\bf A}(q,\dot{q})$ as
our set of actions $A$.  One problem is that ${\bf A}(q,\dot{q})$ 
changes during the time $h$ interval, so that a $\ddot{q}$ which was
feasible at the beginning of the time step may no longer be feasible
at the end.  Another problem is that it might happen that there is
{\em no\/} grid point which is feasible at both the beginning and end
of the time step.
Worse yet, if $M^{-1}(q)$ ever loses rank, then ${\bf A}(q,\dot{q})$
collapses to zero volume, and no point on a fixed grid
is likely to lie in it.  

To prevent ${\bf A}(q,\dot{q})$ from collapsing, we assume a lower
bound $\lambda_{min}$ on the lowest eigenvalue of $M(q)$ during any
motion.  This tells us how ``skinny'' ${\bf A}(q,\dot{q})$ can
become, providing information on how to choose the $\ddot{q}$ grid
spacing so that there are always grid points inside the region.  For
a given timestep $h$, we also have to choose a conservative approximation
$\hat{{\bf A}}(q,\dot{q}) \subset {\bf A}(q,\dot{q})$ such that any
$\ddot{q}$ inside $\hat{{\bf A}}(q(t),\dot{q}(t))$ stays inside 
${\bf A}(q(t+\delta t),\dot{q}(t+\delta t))$ for all $\delta t \in [0,h]$
(Figure~\ref{fig:conservative}).
To construct the conservative approximation $\hat{{\bf A}}(q,\dot{q})$,
we need to know how quickly $M(q)$ can change, which can be
bounded by global bounds on the derivatives of $M(q)$ with respect to time.  
In other words, properties of $M(q)$ and its derivatives must be used
to avoid the problems outlined in the previous paragraph.  Details
can be found in \cite{Heinzinger89}.

\begin{figure}
\begin{center}
\includegraphics{../figure/book/trajectory/conservative}
\end{center}
\caption{${\bf A}(q,\dot{q})$ is the parallelepiped of instantaneously feasible 
accelerations and $\hat{{\bf A}}(q,\dot{q})$ is a conservative approximation to
the set of accelerations which are feasible over the entire timestep of duration $h$.}
\label{fig:conservative}
\end{figure}

The idea is to choose the $\ddot{q}$ grid spacing (which may be
different for each $\ddot{q}_i, i = 1\ldots n$) and the timestep $h$
so that any feasible trajectory of the system using the full
acceleration capabilities ${\bf A}(q,\dot{q})$ can be approximately
tracked by trajectories using the discretized controls $A$, chosen to
be the $\ddot{q}$ grid points lying inside $\hat{{\bf
A}}(q,\dot{q})$.\footnote{Alternatively, $A$ could consist only of
$\ddot{q}$ grid points near the boundary of $\hat{{\bf
A}}(q,\dot{q})$.} The allowable tracking error depends on a
user-defined approximation parameter $\epsilon, 0 <
\epsilon < 1$, where the allowable tracking error goes to zero as
$\epsilon$ goes to zero.  

As before, we can define a trajectory to be $\delta_v(c_0,c_1)$-safe
if all real-space obstacles are avoided by a distance of at least $c_0
+ c_1 ||\dot{q}||$ at all points along the trajectory.  It has been
shown that if there exists a $\delta_v(c_0,c_1)$-safe trajectory from
$(q_0,\dot{q}_0)$ to $(q_f,\dot{q}_f)$ taking time $T_{opt}$, then the
procedure {\em grid\_search\/} outlined above will find a
$(1-\epsilon)\delta_v(c_0,c_1)$-safe trajectory from
$(q_0^*,\dot{q}_0^*)$ to $(q_f^*,\dot{q}_f^*)$ (where the errors from
the desired initial and final states are $O(\epsilon)$) taking no
more than time $(1+\epsilon)T_{opt}$.  The 
timestep $h$ and $\ddot{q}$
grid-spacing are polynomial in $\epsilon$,
and the choice of these parameters involves lengthy calculations.
The running time of the
algorithm has been shown to be polynomial in $\epsilon$ and
exponential in the degrees-of-freedom 
$n_{\cal Q}$~\cite{Jacobs89b,Jacobs89c,Heinzinger90,Donald95c}.  
%The choice
%of the grid spacing and $h$ depend on complex calculations, which
%are omitted here.

The grid-search algorithm is attractive because it is possible to
prove its completeness (using the concept of safe trajectories) and to
understand how the running time depends on an approximation parameter
$\epsilon$.  This allows the user to trade-off computation time
against the quality of the trajectory.  There have been no
implementations of the algorithm for more than a few
degrees-of-freedom, however, because in practice the computation time
and memory requirements grow quickly with the number of degrees-of-freedom.

The practical running time of the algorithm could be improved by
using search heuristics which favor first exploring from nodes
close to the goal state.  The rapidly exploring random tree (RRT)
is a method for trajectory planning based on a heuristic which 
biases the search to evenly explore the state space.  Like the grid-based
algorithm, it discretizes the controls and chooses a constant, finite timestep.
Unlike the grid-based algorithm, it gives up on any notion of optimality
and settles for only probabilistic completeness,
attempting to quickly find any feasible trajectory.  

\subsection{Rapidly Exploring Random Trees}

a few sentences and refer to previous chapter

\subsection{Potential Fields}

a few sentences and refer to previous chapter

\section*{Problems}

\begin{enumerate}

\item Use Lagrange's equations to derive the equations of motion of a
2R (two revolute joints) robot arm operating in a vertical plane.
The first link has length $L_1$, mass $m_1$, and inertia $I_1$ about
the center of mass, and the center of mass is a distance $r_1$ from
the first joint.  For the second link, $m_2$, $I_2$, and $r_2$
are defined similarly (Figure~\ref{fig:2Rarm}).  Put the equations
of motion in the standard form \eqref{eq:robotcoriolis}.

\begin{figure}
\begin{center}
\includegraphics{../figure/book/trajectory/2Rarm}
\end{center}
\caption{The 2R robot arm.}
\label{fig:2Rarm}
\end{figure}

\item  Find the eight Christoffel symbols for the mass matrix of
Problem 1.  Explain which Christoffel symbols are equal in general.

\item Use Lagrange's equations to find the equations of motion of a
PR robot arm.  Provide your own drawing and parameters and solve
with these parameters.  Put the equations in the standard form
\eqref{eq:robotcoriolis}.

\item Choose a frame aligned with the principal axes of inertia,
and find the inertia matrix of a round tube of length $L$, inner diameter
$d_1$, outer diameter $d_2$, and density $\rho$.  Remember that 
inertia matrices in a common frame can be added (and subtracted).

\item The inertia matrix of a body in a coordinate frame $x_1$-$y_1$-$z_1$
at the center of mass of the body
is $\mbI_{1}$.  The orientation of this coordinate frame is $R_1$ 
relative to a frame $x$-$y$-$z$.  The origin of $x$-$y$-$z$ is at
$r_1$ in the frame $x_1$-$y_1$-$z_1$.  Transform the inertia matrix 
$\mbI_{1}$ to an inertia matrix $\mbI$ expressed in the
$x$-$y$-$z$ frame, where
\[
\mbI_{1} = \left( \begin{array}{ccc}
1 & 0 & 0 \\
0 & 2 & 0 \\
0 & 0 & 3 
\end{array} \right), \;\;
r_1 = \left(\begin{array}{c}
3 \\
0 \\
2 \\
\end{array} \right), \;\;
R_1 = \left( \begin{array}{ccc}
0 & -1 & 0 \\
1 & 0 & 0 \\
0 & 0 & 1
\end{array} \right).
\]
Also provide a drawing of the two frames showing their position and
orientation relative to each other.

\item Consider a barbell constructed of two spheres of radius 10cm
welded to the ends of a right circular cylinder bar
of length 20cm and radius 2cm.
Each body is a solid volume constructed of
steel, with a mass density of 7850kg/m$^3$.  Find the approximate inertia 
matrix in a principal-axis frame
at the center of mass.

\item Prove the parallel-axis theorem~\eqref{eq:parallelaxis}.

\item Derive equations \eqref{eq:eulercoord1} -- \eqref{eq:eulercoord3}.

\item Write a program to simulate the tumbling motion of a rigid body
in space.

\item A point of mass $m$ moves in three-dimensional space, actuated
by three thrusters, with equations of motion $\force = m \ddot{q}$
(no gravity).
Now imagine that the mass (still with three thrusters) 
is constrained to move on a sphere of
radius 1 centered at the origin of the inertial frame.
Write the Pfaffian constraint and solve for
$\ddot{q}$ and the Lagrange multiplier $\lambda$.
\label{problem:pointonsphere}

\item Find the projection matrix $P_\force$ for problem~\ref{problem:pointonsphere}
and write the constrained equations of motion in the 
form~\eqref{eq:simpleconstrainedprelim}.

\item In problem~\ref{problem:pointonsphere},
it is possible to reduce the number of generalized
coordinates from three to two and eliminate the Lagrange multiplier.
Choose generalized coordinates $q = (q_1,q_2)^T$ to describe the position of the point
on the sphere, and use Lagrange's equations to solve for the dynamics
in these coordinates.  Explain what the generalized forces are.
Give the Christoffel symbols in these coordinates.  

\item For a point mass moving in a gravity-free plane, use polar
  coordinates and Lagrange's equations to derive the equations of
  motion.  Give the Christoffel symbols in these coordinates.
  Describe a set of coordinates whose Christoffel symbols are all
  zero.

\item For the RP manipulator of Example~\ref{ex:RParmpath}, write a program
which accepts a straight line path and draws the velocity limit curve.

\item Implement the time-scaling algorithm for the RP arm 
of Example~\ref{ex:RParmpath} following a
straight line path specified by the user.  Try to write the program
in a modular fashion, so systems with different dynamics can use many
of the same routines. 

\item In step 2 of the time-scaling algorithm, explain why there is no
solution to the time-scaling problem if $\dot{s}=0$ is reached.  Give
a simple example of this case.

\item In step 3 of the time-scaling algorithm, explain why there is no
solution to the time-scaling problem if the forward integration reaches
$s=1$ or $\dot{s}=0$ before crossing the velocity limit curve or the curve
$F$.  Give a simple example of this case.

\item In Example~\ref{ex:RParmpath}, explain intuitively why $b_1(s) \neq 0$ 
but $b_2(s) =0$, in terms of the arm dynamics and the manipulator path.
\label{problem:RParmb2}

\item At a zero inertia point $s$, $a_i(s) = 0$.  Explain the implications
if $b_i(s)$ is also
zero.  Should the algorithm be modified to handle this case? 
Remember, we are assuming the robot is strong enough to maintain
any configuration statically.  Does it matter if the actuator limits are
state-dependent?

\item We would like to find optimal motions for the RP robot arm
of Example~\ref{ex:RParmpath}.
The Lagrangian defining the objective function is 
${\cal L} = w_1 \force_1^2 + w_2 \force_2^2$, where $w_1$ and $w_2$ are
positive weights.  For a fixed time of
motion $t_f$, write the Hamiltonian, the necessary condition for optimality
\eqref{eq:optimalitycondition}, and the adjoint equation \eqref{eq:adjoint}.

\item For the previous problem, choose a parameterization of the joint
trajectories and use a nonlinear programming package to find optimal point-to-point
trajectories for the RP arm.  A good initial guess for the joint trajectories would
be one that exactly satisfies the start and goal state constraints.  Comment
on any difficulties you encounter using nonlinear programming.

% this is not a good problem
%\item Consider a point mass in the plane with dynamics $\force = m\ddot{q}$.
%You would like to find motions from rest at $q_0$ to rest
%at $q_f$ minimizing $w_1 \force_1^2 + w_2 \force_2^2$, where $w_1,w_2 > 0$.
%Can you solve for the optimal control analytically using 
%\eqref{eq:optimalitycondition} and \eqref{eq:adjoint}?  Write a 
%nonlinear program and find a numerical approximation to the optimal
%control for a particular choice of start and goal configuration and
%weights $w_1, w_2$.  If you were able to solve the problem analytically, 
%the result of the nonlinear program should match your analytical results.  

\item Implement the procedure {\em grid\_search\/} for a point moving
in a planar world with polygonal obstacles.  

\end{enumerate}







