Technology visionaries have eagerly anticipated the time when robots will work side
by side with people, taking care of rudimentary chores, assisting the aging population,
exploring areas that are too remote or dangerous for humans, preventing casualties in
Yet robots have so far performed a more perfunctory role, within factories,
in surgical applications, and in certain other niche applications. So what has prevented
robot proliferation outside of these applications? Factory robots clearly outperform human
counterparts in precision and speed of executing certain tasks. The reason for success in the
factory may be attributed to four factors. First, such robots typically plan in fewer than ∼7
degrees of freedom. Second, the machines are usually bolted to the ground, and have large
actuators, so that the robotic arm has complete control over all of its degrees of freedom.
Third, there is relatively little uncertainty in a factory setting. Finally, the factory robot
typically performs rudimentary repetitive motions.
To break out of the factory, the science of robotics must improve.
On one hand,
robotics research has focused on improving hardware, including actuators, energy storage
mechanisms, and sensors. Additionally, improvements are being made in mechanical design
to reduce the burden of the actuators or control systems.
Software and algorithmic
development has focused on several areas, such as state estimation, including ﬁltering,
localization and mapping.
High level “intelligence” is also an important component
of robotics software, which increasingly deals with human-robot interaction, robot-robot
interaction and cooperation, and robot-environment interaction. At a more fundamental
level, however, robots must understand how to move around and execute desired behaviors
within their environment: this is the problem of motion planning.
Consider that the human body has over 600 muscles, and over 200 bones, more than half
of which are found in the hands and feet. Furthermore, more than half of the neurons in the
human brain are found in the cerebellum, a region thought to be responsible for ﬁne-tuning
motor movement. This demonstrates that humans (and animals) are very high dimensional
systems, that devote a signiﬁcant portion of their brain power to controlling movement.
One could make the argument that today, the bottleneck in achieving human or animal-like
agility in a robot, is in the lack of a comprehensive motion planning and control strategy
Figure 1-1: Person and dog cross river by hopping stones. Legs are required, and planning
steps and motions are essential for such a task.
Picture (left) reproduced from
http://www.ﬂickr.com/photos/peturgauti; picture (right) by Alison Plant
that is capable of dealing with the dimension of the planning problem, while also taking
into account the kinematic and dynamic constraints imposed by the robot and the world it
exists in. The dynamic constraints, including the governing equations of motion, become
considerably more challenging for motion planning when the system is not “bolted” to the
ground, as factory robots are.
In this thesis, I present motion planning strategies for systems having many degrees of
freedom, as well as strategies for planning agile maneuvers even when the system is subject to
complex dynamic constraints in an obstacle ﬁlled world. This type of problem is exempliﬁed
by dynamic legged locomotion. In addition to various toy problems, the algorithms developed
in this thesis were applied on LittleDog, a small quadruped robot described in section 1.2,
to ﬁnd continuous bounding trajectories over very rough terrain.
The work in this thesis is largely motivated by the problem of legged locomotion, a task that
animals and humans are clearly very good at (see for example Figure 1-1). At ﬁrst glance,
the task may seem trivial, but the diﬃculty is hidden by the fact that most of the brain’s
processing for locomotion is done at the subconscious level. In fact, the sensorimotor system
accounts for a vast majority of the brain. As mentioned above, the human cerebellum,
a region thought to be responsible for ﬁne-tuning motor behavior, itself makes up more
than 50% of the number of neurons in the entire brain. Even more impressive - this region
receives 200 million input ﬁbers, compared to 1 million ﬁbers carried by the optic nerve. This
processing power is required to enable humans and animals to move and interact gracefully
with the environment. State of the art legged robots, on the other hand (see Figure 1-2),
either move slowly and deliberately, or quickly and reactively. The latter class is typically
blind to the world, and reacts to the environment instead of anticipating it. To become
more agile, robots must move quickly and deliberately, while taking advantage of knowledge
Figure 1-2: State of the art legged robots: (Left) Honda Asimo “running”. Note the ﬂat
foot. (Right) Boston Dynamics’ BigDog climbing a snowy hill.
about the environment.
Even if one ignores the problem of sensing and uncertainty, e.g. if the state of the system
and its environment is fully known, the problem of planning and executing motions for
complicated (high dimensional), and particularly for underactuated mechanical systems is
still very challenging. A system is underactuated if it can not accelerate all of its Degrees of
Freedom (DOF) in arbitrary directions at every instant in time. As mentioned above, part of
the reason that factory robots are successful is because they are bolted to the ground, and do
not have to worry about falling over. Legged robots do not have this luxury. Underactuated
systems are typiﬁed by robots that have more DOFs than actuators, such as the Acrobot
[Murray and Hauser, 1991] or Pendubot [Spong et al., 1995]. Many important classes of
robots are unavoidably underactuated, including most walking, swimming, ﬂying and free-
ﬂoating (space) machines. Legged robots, for example, are underactuated at the interface
between the foot and the ground. The system cannot produce arbitrary ground reaction
forces or moments, as it is only possible to push on the ground, but not possible to pull on
the ground. This is especially true for point-foot walkers (e.g., [Goswami et al., 1996]), but
is also true for ﬂat-foot walkers like Honda’s ASIMO’s [Honda Motor Co., 2004].
In addition to the complexity introduced by the constraints imposed by underactuated
dynamics, real-world robots are subject to additional kinodynamic constraints, for example
position, velocity and torque limits, as well as obstacle collisions. It is natural to formulate
this type of problem with a motion planning framework, the goal of which is to ﬁnd feasible
trajectories that take a model of the system from an initial pose to a desirable goal pose, while
taking into consideration all of the constraints. Sample-based methods, in particular, have
been shown to be eﬀective at solving motion planning problems with complex geometric
constraints. However, search eﬃciency for sample-based planning algorithms is hindered
by two factors: 1) kinematic path planning problems are known to be computationally
Figure 1-3: TOP: LittleDog robot walking on pegs. BOTTOM: LittleDog on hind legs.
diﬃcult, where search is sensitive to dimensionality of the system, and 2) diﬀerential
kinodynamic constraints, especially underactuated dynamics, impose additional challenges
that signiﬁcantly reduce search eﬃciency.
The algorithms in this thesis were developed with the speciﬁc goal of achieving continuous
bounding over extremely rough terrain with the LittleDog robot. LittleDog, is a small, 3kg
point-footed quadruped robot platform shown in Figure 1-3. This robot was developed by
Boston Dynamics Inc. as part of the DARPA sponsored Learning Locomotion program, in
which several universities competed to develop motion planning algorithms to enable the
robot to quickly traverse a variety of terrain. The robot state information is provided by
motion capture and encoders. Additionally, a laser scanned height map of the terrain is
provided to the motion planner before each run. The goal of the program was to push the
limits of what robots could do on rough terrain, given reasonable knowledge about the robot
state and the world.
Unlike walking gaits, bounding gaits are dynamic gaits which cannot be planned in the
conﬁguration space with quasi-steady approximations. LittleDog has 12 actuators (2 in each
hip, and one in each knee), and a 40-dimensional state space. The motion planning problem
is compounded by diﬀerential constraints, including torque and velocity limited motors, and
the fact that this robot is very much underactuated during bounding. Attempting to plan
an agile bounding gait over rough terrain, even in simulation, requires the development of
motion planning tools that are capable of dealing with both, the very high dimensionality
of this robot’s state space, as well as challenging diﬀerential constraints.
Review of the State of the Art
The thesis combines work from several areas, including:
1. Sample-based motion planning
2. Task space feedback control
3. Underactuated Systems and Control
4. Task Space Control of Underactuated Systems
5. Legged Locomotion
Each of these topics is reviewed in turn.
This section provides some background
information related to the thesis as a whole, but each chapter will provide additional
background information pertinent to that chapter.
The book by LaValle [LaValle, 2006] provides a broad overview of state of the art planning
problems and algorithms. Much of the seminal work in spatial (manipulation) planning was
done by Lozano-P´
erez and others [Lozano-Perez and Tomas, 1981,Lozano-Perez et al., 1983],
particularly in deﬁning conﬁguration space (sometimes referred to as C-space, see deﬁnition
below). The C-space uniquely determines the position of each DOF in a system, so that
an entire conﬁguration of the robot is described by a single point. The C-space can be
broken down into free regions which are collision-free, and obstacle regions, corresponding
to a conﬁguration in which some part of the robot is in collision. The task of a planning
algorithm can then be simpliﬁed to ﬁnding a path for a point through free regions of C-space,
from a starting conﬁguration point, to a goal point. Most of the work done in the ﬁeld
assumes fully actuated dynamics (e.g. the robot can move in any direction in conﬁguration
space), but that work has set the framework for planning with obstacles.
Deﬁnition 1. The Conﬁguration Space (C-space) is the set of all possible transfor-
mations (conﬁgurations) that can be applied to a system. A conﬁguration will completely
describe every point on the robot. The conﬁguration space serves as an abstraction layer in
motion planning, whereby algorithms that work generally for C-spaces can be applied to more
speciﬁc systems. The C-space is typically an n-dimensional manifold, where n is the number
of degrees of freedom of the robot.
Deﬁnition 2. The State Space (X) is the set of all possible transformations, velocities, and
other system variables that represent the entire state of a system at a given instant of time.
For rigid body systems with deterministic dynamics, which is representative of the problems
of interest in this thesis, the state space consists of the conﬁguration space augmented with
the set of all possible velocities for each degree of freedom of the conﬁguration space. Note
that C-space and X are used interchangeably in path-planning or motion-planning contexts
when it makes sense to do so.
Deﬁnition 3. Deﬁne the subscript X
to correspond to the subset that is in collision. On
the contrary, the subscript X
refers to the subset that is not in collision, X
= X \X
Path planning for kinematic manipulation tasks, such as the piano-mover problem, has
been shown to be PSPACE-Hard [Reif, 1979]. Despite this time complexity, sample-based
methods have proven to be eﬀective at solving these types of problems, with fast average-
time performance. Algorithms such as Probabilistic RoadMaps (PRMs) [Kavraki et al.,
1996], can be used for single- or multi-query planning in C-space. This method involves
sampling many points in free conﬁguration space before planning. A fast local planner then
decides which proximal points are reachable in order to create a connected graph. After the
graph is constructed it is relatively simple to ﬁnd paths through the nodes from any start to
any end node (hence the term multi-query). PRMs allow for fast querying between multiple
start / ending conditions, but building the graph initially can be time consuming. The PRM
approach may be less suitable for problems with signiﬁcant constraints in dynamics as it
becomes more diﬃcult to ﬁnd even local connections between samples. The approach is
also hindered by dimensionality as the number of samples required in order to maintain a
constant sampling density goes up exponentially with the number of DOF.
An alternative sampling-based planner, which is geared toward quickly ﬁnding a feasible
path from a single initial pose to a single goal pose (“single query”), is the Rapidly-exploring
Random Tree (RRT) [LaValle and Kuﬀner, 2000, LaValle et al., 2001]. In this approach,
a tree is created in C-space. The nodes represent collision free conﬁgurations that can be
reached, and edges represent feasible collision free actions between nodes. The root node of
the tree is the initial conﬁguration of the robot, and the tree is grown until a path is found
to a goal conﬁguration. The standard RRT algorithm [LaValle et al., 2001] is provided for
reference in Algorithm 1 and 2, and is visually illustrated in Figure 1-4.
Algorithm 1 BUILD-RRT (q
for k = 1 to K do
if with some probability P then
⇐ RANDOM-SAMPLE() ∈
Algorithm 2 EXTEND (T, q)
u ⇐ CONTROL(q
if COLLISION-FREE (q
In the ﬁrst step, a C-space sample, q
, is chosen at random, usually from a uniform
distribution over some bounded region of C-space or state space. The closest node on the
, is selected by the NEAREST-NEIGHBOR() function. This function is typically
implemented by choosing the node with the minimum Euclidean distance to the sample,
although in state space the cost-to-go from the node in the tree to the sample would be
more appropriate [Frazzoli et al., 2002, Glassman and Tedrake, 2010, Tedrake, 2009a], but is
diﬃcult to compute.
An action, u is then determined by the CONTROL() function. Applying this action
to the node should make progress towards the random sample, at least with probability
> 0. Finally a new state, q
, is computed in the function NEW-STATE(). For dynamic
systems, this is done by integrating from q
while applying control u for some ∆t. During
integration, the trajectory is checked for kinodynamic feasibility. If it is collision free, the
node and edge are added to the tree.
The expansion process is sometimes biased towards the goal node, which can increase
search speed dramatically. The RRT algorithm is eﬃcient because it introduces a Voronoi
bias, meaning that it is encouraged to explore open (yet unexplored) regions of the
conﬁguration space. Figure 1-4 illustrates a 2D RRT while going through an EXTEND
operation. This ﬁgure also shows Voronoi diagrams associated with the trees. Each Voronoi
region (or cell) contains a single node of the tree. The Voronoi cell delineates the region for
which any samples are mapped by the NEAREST-NEIGHBOR function to the corresponding
node in the tree. Assuming uniform sampling, the probability that a given node of the tree
will be expanded is then directly proportional to the area of its Voronoi cell. Because of
the Voronoi bias, the largest regions of unexplored space, which have large Voronoi cells, are
most likely to be chosen for expansion into these regions. Thus, RRT’s have a tendency to
quickly branch into unexplored regions. When the unexplored regions become smaller and
more uniformly distributed, then the search begins to ﬁll in gaps with increasingly uniform
coverage, ensuring that the algorithm is probabilistically complete, meaning that it will ﬁnd
a path if one exists as K goes to inﬁnity.
Figure 1-4: Steps of the RRT algorithm are shown, with out any obstacles, starting with 30
nodes. The blue lines delineate the Voronoi regions associated with each node. (a): RRT
with 30 nodes. (b): A random sample is drawn (blue circle). (c): The closest node of the
tree is selected, a new node is added. The new node is highlighted in green. Note the change
in Voronoi regions after adding the new node. (d): RRT after 200 expansions.
The PRM is also probabilistically complete as the number of samples goes to inﬁnity.
Note that we have introduced both, PRMs and RRTs, as using random sampling, but in
fact deterministic sampling strategies could be used as well. In that case the algorithms
would be resolution complete, rather than probabilistically complete. In the remainder of
this thesis we usually refer to random sampling, but most of the concepts will generalize to
deterministic sampling as well.
An improvement on the RRT is a bidirectional version called RRT-Connect [Kuﬀner
et al., 2000].
In this algorithm there are two trees - one tree is grown in the forward
direction from the start node, while the other is grown backwards from the goal node. Each
time a node is added to one tree, using the standard RRT expansion step, the other tree is
then iteratively grown towards the new node until a collision occurs, or until the two trees
are successfully connected. The RRT-Connect algorithm can be much faster than the single
RRT for complicated single query planning problems.
Task Space Control
The planning algorithms discussed thus far have been applied primarily in problems within
C-space or state space, where constraints such as joint feasibility and collisions with obstacles
are easily checked in the same space. In some cases it is desirable to specify robot behaviors
in a diﬀerent command space. Consider for example a humanoid robot which is trying to pick
up a cup. There might be inﬁnite robot conﬁgurations which correspond to the humanoid’s
hand being on the cup. Instead of specifying this behavior in joint angles in conﬁguration
space, if we consider the hand position as a task space, this can be more intuitive for the
control designer, and can capture more of the intent of an action.
Deﬁnition 4. Suppose f : C → T is a function mapping from the conﬁguration space, C,
to a lower dimensional space, T , where each conﬁguration is mapped to a single point in T .
We may choose to call f a Task Space Mapping, and T the Task Space (T-space).
The mapping f can be deﬁned somewhat arbitrarily, however for the purposes of motion
planning, it is generally useful to have at least some of the following properties:
1. Reduced Dimension: If the C-space is an n dimensional manifold, the T-space should
be an m dimensional manifold, such that m << n
2. Topological Applicability: There may be topological considerations for a planning space.
For example, it may be beneﬁcial if connected regions of C-space map to connected
regions in T-space, and for the opposite to be true as well, where disconnected regions
of T-space correspond to disconnected regions in C-space
3. Smoothness and diﬀerentiability of the Task space Mapping, (or to higher order, being
k times diﬀerentiable)
4. Goal Speciﬁcity: It is useful if the motion planning goal set G can be speciﬁed by a
point (or region) within T . For this to hold, for all regions of C-space that map to a
goal point speciﬁed in T-space, y
∈ T , the corresponding C-space region should also
be in the goal set
5. Extension to state space: for certain problems, the task space mapping may include
velocity or other state information.
6. Collision Information: Some motion planners can take advantage of having regions of
T-space for which all associated regions of C-space are in collision, so that obstacle
regions of task space can be deﬁned. In this thesis, we do not require this condition.
Deﬁnition 5. The term workspace is a task space consisting of Cartesian coordinates
in 2-D or in 3-D, corresponding to the world where the robot lives and is used to specify
World-frame coordinates of points on the robot.
Choices for task space mappings will be explored later in this thesis, but some examples
might include the workspace position of an end eﬀector, or the position of the robot Center
Because the task-space mapping can be nonlinear and there can be inﬁnite
conﬁgurations that map to a single task-space coordinate, y from a point in C-space, q,
the problem of inverse kinematics can be diﬃcult or even impossible. Fortunately, the task
space mapping can be linearized, presenting an easy to compute relationship between the
velocity in joint space, and the velocity in task space:
y = f (q)
˙y = J · ˙q,
where the Jacobian, J, is deﬁned as:
A trajectory in task space can be mapped to a trajectory in conﬁguration space by using
the Moore-Penrose pseudoinverse. This pseudoinverse is deﬁned for a matrix in which the
rows are linearly independent as:
(J · J
By using the Moore-Penrose pseudoinverse, one can resolve redundancies [Liegeois, 1977]