Sample-Based Motion Planning in High-Dimensional and Differentially-Constrained Systems by

Chapter
1
Introduction
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
war, etc.
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 filtering,
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 fine-tuning
motor movement. This demonstrates that humans (and animals) are very high dimensional
systems, that devote a significant 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
13

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.flickr.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 filled world. This type of problem is exemplified
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 find continuous bounding trajectories over very rough terrain.
1.1
Motivation
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 first glance,
the task may seem trivial, but the difficulty 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 fine-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 fibers, compared to 1 million fibers 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
14

Figure 1-2: State of the art legged robots: (Left) Honda Asimo “running”. Note the flat
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 typified 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, flying and free-
floating (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 flat-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 find 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 effective at solving motion planning problems with complex geometric
constraints. However, search efficiency for sample-based planning algorithms is hindered
by two factors: 1) kinematic path planning problems are known to be computationally
15

Figure 1-3: TOP: LittleDog robot walking on pegs. BOTTOM: LittleDog on hind legs.
difficult, where search is sensitive to dimensionality of the system, and 2) differential
kinodynamic constraints, especially underactuated dynamics, impose additional challenges
that significantly reduce search efficiency.
1.2
LittleDog
The algorithms in this thesis were developed with the specific 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
16

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
configuration 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 differential 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 differential constraints.
1.3
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.
1.3.1
Sample-Based Planning
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 defining configuration space (sometimes referred to as C-space, see definition
below). The C-space uniquely determines the position of each DOF in a system, so that
an entire configuration 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 configuration in which some part of the robot is in collision. The task of a planning
17

algorithm can then be simplified to finding a path for a point through free regions of C-space,
from a starting configuration point, to a goal point. Most of the work done in the field
assumes fully actuated dynamics (e.g. the robot can move in any direction in configuration
space), but that work has set the framework for planning with obstacles.
Definition 1. The Configuration Space (C-space) is the set of all possible transfor-
mations (configurations) that can be applied to a system. A configuration will completely
describe every point on the robot. The configuration space serves as an abstraction layer in
motion planning, whereby algorithms that work generally for C-spaces can be applied to more
specific systems. The C-space is typically an n-dimensional manifold, where n is the number
of degrees of freedom of the robot.
Definition 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 configuration space augmented with
the set of all possible velocities for each degree of freedom of the configuration space. Note
that C-space and X are used interchangeably in path-planning or motion-planning contexts
when it makes sense to do so.
Definition 3. Define the subscript X
obs
to correspond to the subset that is in collision. On
the contrary, the subscript X
f ree
refers to the subset that is not in collision, X
f ree
= X \X
obs
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 effective 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 configuration 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 find 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 significant constraints in dynamics as it
becomes more difficult to find 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 finding a feasible
path from a single initial pose to a single goal pose (“single query”), is the Rapidly-exploring
Random Tree (RRT) [LaValle and Kuffner, 2000, LaValle et al., 2001]. In this approach,
a tree is created in C-space. The nodes represent collision free configurations that can be
reached, and edges represent feasible collision free actions between nodes. The root node of
the tree is the initial configuration of the robot, and the tree is grown until a path is found
to a goal configuration. 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.
18

Algorithm 1 BUILD-RRT (q
0
)
1:
T.init(q
0
)
2:
for k = 1 to K do
3:
if with some probability P then
4:
q
rand
⇐ RANDOM-SAMPLE() ∈
N
5:
else
6:
q
rand
⇐ q
goal
7:
end if
8:
EXTEND(T,q
rand
)
9:
end for
Algorithm 2 EXTEND (T, q)
1:
q
near
⇐ NEAREST-NEIGHBOR(q,T)
2:
u ⇐ CONTROL(q
near
, q
rand
)
3:
q
new
⇐ NEW-STATE(q
near
, u)
4:
if COLLISION-FREE (q
near
, q
new
) then
5:
T.add-node(q
near
, q
new
, u)
6:
end if
In the first step, a C-space sample, q
rand
, is chosen at random, usually from a uniform
distribution over some bounded region of C-space or state space. The closest node on the
tree, q
near
, 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
difficult 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
new
, is computed in the function NEW-STATE(). For dynamic
systems, this is done by integrating from q
near
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 efficient because it introduces a Voronoi
bias, meaning that it is encouraged to explore open (yet unexplored) regions of the
configuration space. Figure 1-4 illustrates a 2D RRT while going through an EXTEND
operation. This figure 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
19

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 fill in gaps with increasingly uniform
coverage, ensuring that the algorithm is probabilistically complete, meaning that it will find
a path if one exists as K goes to infinity.
(a)
(b)
(c)
(d)
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.
20

The PRM is also probabilistically complete as the number of samples goes to infinity.
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 [Kuffner
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.
1.3.2
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 different command space. Consider for example a humanoid robot which is trying to pick
up a cup. There might be infinite robot configurations which correspond to the humanoid’s
hand being on the cup. Instead of specifying this behavior in joint angles in configuration
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.
Definition 4. Suppose f : C → T is a function mapping from the configuration space, C,
to a lower dimensional space, T , where each configuration 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 defined 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 beneficial 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 differentiability of the Task space Mapping, (or to higher order, being
k times differentiable)
4. Goal Specificity: It is useful if the motion planning goal set G can be specified by a
point (or region) within T . For this to hold, for all regions of C-space that map to a
21

goal point specified in T-space, y
g
∈ 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 defined. In this thesis, we do not require this condition.
Definition 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 effector, or the position of the robot Center
of Mass.
Because the task-space mapping can be nonlinear and there can be infinite
configurations that map to a single task-space coordinate, y from a point in C-space, q,
the problem of inverse kinematics can be difficult 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 defined as:
J =
∂f
∂q
.
A trajectory in task space can be mapped to a trajectory in configuration space by using
the Moore-Penrose pseudoinverse. This pseudoinverse is defined for a matrix in which the
rows are linearly independent as:
J
+
= J
T
(J · J
T
)
−1
.
By using the Moore-Penrose pseudoinverse, one can resolve redundancies [Liegeois, 1977]