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

when the dimension of the configuration space exceeds the dimension of the task space, as
is often the case, by iteratively applying:
˙q = J
+
· ˙y + (I − J
+
J) ˙q
ref
.
This equation will produce a joint velocity, ˙q, the projection of which exactly (instanta-
neously) follows a desired task velocity, ˙y, if it is possible to do so, and then minimizes the
squared error in the commanded secondary joint velocity command, ˙q − ˙q
ref
. The secondary
command offers a redundancy resolution mechanism, which is executed in the null space of
22

the Jacobian (only if it is possible to do so). If the null space term is omitted, the command
will be the joint velocities with the smallest norm.
Task space control has been developed in depth for humanoid robot motion planning,
where the secondary task typically involves maintaining a desirable base position of the robot
whenever possible [Khatib et al., 2004]. This is achieved by defining ˙q
ref
as a workspace-
centering PD type controller, ˙q
ref
= Kp · (q
d
− q) + Kd · ( ˙q
d
− ˙q) where the desired position
is set to an arbitrary constant pose, and the desired velocity is zero. This type of control
was also extended to operation space, where ground reaction forces are commanded [Sentis
and Khatib, 2005], and can be used to command velocities or accelerations (see [Nakanishi
et al., 2005] for review). For underactuated systems, in Chapter 3, I show that the choice of
redundancy resolution mechanism is particularly important for maintaining dynamic stability
(preventing the robot from falling over) [Shkolnik and Tedrake, 2007].
1.3.3
Underactuated Systems
A significant body of research is focused on control of underactuated systems (see [Tedrake,
2009b] for a good overview). Multi-link arms with passive joints are a representative subset of
general underactuated systems. Mathematically, such models can be used to describe planar
walking machines when rolling around the toe joint, or to describe the LittleDog robot
standing on its two hind legs. Although underactuated systems are typically not feedback
linearizable, it is possible to achieve a partial feedback linearization (PFL) [Spong, 1994a], in
which the dynamics of some subset of the degrees of freedom are replaced (through feedback)
with the dynamics of a simple linear system. This subset is not restricted to simply the
actuated joints, but can also be used to exploit inertial coupling to linearize the dynamics of
the unactuated joints. The machinery developed for partial feedback linearization includes
a very compact and straightforward mechanism for reasoning about, and controlling, the
couplings between actuated and unactuated joints in robotic manipulators, and has been
demonstrated in a swing up task on the Acrobot [Spong, 1994b].
The controllability of arms with passive joints, and a PFL-like control strategy was
addressed by [Arai and Tachi, 1991], where a point-to-point controller was developed for a
3 link arm which relies on the passive joints having brakes. This early work demonstrated
that if there are more actuators than passive joints, then the passive joints can be controlled
to position them in a desired configuration. After this occurs, brakes are used to hold the
passive joints in place while the actuated joints are then moved to bring the whole system to a
desired configuration. Later, the work was extended using a “time-scaling” algorithm to arms
without brakes, but under zero gravity assumptions (e.g. in a horizontal plane) [Arai et al.,
1998,Lynch et al., 2000]. The idea of treating the underactuated system as a regular dynamic
system with a nonholonomic constraint, and conditions for integrability, and stability were
addressed in [Oriolo and Nakamura, 1991]. It was shown that without gravity, an arm
with passive joints is not controllable, and with gravity, it is. [Reyhanoglu et al., 1999] also
addressed controllability and stabilizability using Spong’s PFL framework.
Another body of literature addressed stabilization of such systems around an equilibrium
point. See [Berkemeier et al., 1999] for one such technique, and for an overview of related.
23

Alternative approaches such as [Stojic et al., 2000] study a 3 DOF biped with a passive toe
joint; feedback control of the horizontal COM acceleration is achieved by only looking at
linear terms in the dynamics, using LQR to stabilize around a point. Similarly, [Yamakita
et al., 2003] studied a 2 DOF biped with passive toe joint (Acrobot) and controlled the
angular momentum. The technique was extend to 3 DOF by ignoring non-linear terms, and
used to control landing in a gymnastic robot using feedback control. Lastly, there are also
energy based approaches, e.g. [Fantoni et al., 2000], though methods such as [Spong, 1994a]
that use PFL also use intuition of energy to achieve swing-up.
1.3.4
Task space control of underactuated systems
Some of the prior work in underactuated control has combined task space control with partial
feedback linearization to enable feedback control within task space. The work in [Stojic et al.,
2000] and [Yamakita et al., 2003] provide approximations of this approach by dropping
nonlinear terms. Arai [Arai et al., 1993] developed a control for the Cartesian coordinates of
an end effector for an arm with passive joints. That work assumed the task-space dimension
exactly equals the number of actuators, and uses the Jacobian inverse. Point-to-point control
(from a given position to a desired position, with zero velocities) is achieved with brakes on
the passive joints. [Jain et al., 1993] also developed a similar feedback control via generalized
Jacobians (invertible) for free floating systems applied to underactuated systems, which
resulted in a computationally efficient approach to finding the equations of motion.
Xu and Gu [Gu and Xu, 1994] developed an adaptive control framework to account for
dynamic errors due to parameter uncertainty while controlling Cartesian coordinates, for a
class of arms where the Jacobian is assumed invertible. The work also discussed stability
and internal dynamics of the workspace control approach, and determined that stability
analysis remains an open problem. Bergerman [Bergerman, 1996] extended these works to
include concepts in motion planning, though the more recent developments of randomized
kinodynamic planners were not developed by that point. The work presents a planner for
Cartesian space control for an arm with passive joints with brakes. The motion planning
consists of iteratively moving a bit, then applying the brake. He also applied sliding control
/ robust control to account for model error, and analyzed the actuability of the system. The
work in [Shin and Lee, 1997] also developed Cartesian control, with simple path planning
to avoid dynamic singularities. This was done by looking at constraints in task space for a
two DOF arm. Lastly, [Hyon et al., 2004] developed a controller for a planar (gymnastic)
Back-Handspring robot, including COM (Cartesian) control with a passive joint. A state-
machine (e.g. Raibert) type controller was used to enable a cyclic gait, and all parameters
were tuned by hand.
The prior work in task space control with passive degrees of freedom has largely focused
on low-dimensional (2 or 3 DOF) systems. The task space is used because it is intuitive, and
may offer some advantage over joint control, but none of these prior works have examined
redundancy resolution mechanisms.
Furthermore, these works have not addressed the
potential advantage of the dimensionality reduction that is possible for higher dimensional
systems, and have not approached the problem as a feasible trajectory search problem.
24

1.3.5
Templates and Anchors
The concept of “embedding” low-dimensional dynamics into a high-dimensional dynamical
system has a long history in control, and robotic locomotion research is no exception
(e.g., [Westervelt et al., 2002, Poulakakis and Grizzle, 2007]). Full and Koditschek, [Full
and Koditschek, 1999] gave this idea some broad appeal by describing “templates and
anchors”, and suggesting that the embedding of simple models (the templates) can describe
neuromechanical invariants in real animals (the anchors). One of the hallmark examples of
this work is the spring-loaded inverted pendulum (SLIP) model, which explains center-of-
pressure to center-of-mass dynamics in an incredible diversity of creatures, and also has been
used to design control systems for dynamic locomotion in robots [Altendorfer et al., 2000].
Most work in templates and anchors (e.g., [Nakanishi et al., 2000, Saranli and Koditschek,
2003]) use an inverse-dynamics mapping to produce the embedding; consequently when the
anchor is underactuated, the template must be chosen carefully to guarantee that dynamics
remain invertible.
1.3.6
Legged Locomotion
The problem of fast locomotion over rough terrain has long been a research topic in robotics,
beginning with the seminal work by Raibert in the 1980’s [Raibert, 1986, Raibert et al.,
1986]. Many recent approaches have utilized compliant or mechanically clever designs that
enable passive stability using open-loop gaits, or otherwise use reflexive control algorithms
that tend to react to terrain. Recent applications of these strategies, for example on the
Rhex robot [Altendorfer et al., 2001] and BigDog [Raibert et al., 2008], have produced
impressive results, but these systems do not take into account knowledge about upcoming
terrain. Planning methods such as the RRT are well developed for kinematic path planning in
configuration space focusing on maneuvers requiring dexterity, obstacle avoidance, and static
stability, but are not suitable for highly dynamic motions, governed largely by underactuated
dynamics. In this thesis, I attempt to bridge this gap, by demonstrating a framework for fast
global motion planning that respects kinematic and dynamic constraints. This is a necessary
step to enable agile locomotion, combining careful foot placement and fast dynamics, on a
position controlled robot.
A popular method to achieve legged robot control is to use a dynamic stability criteria
for gait generation or feedback stabilization (see [Pratt and Tedrake, 2005] for review of
various metrics). One approach is to keep the center of pressure, or the projection of the
Zero Moment Point (ZMP), within the support polygon defined by the convex hull of the
feet contacts on the ground. While the ZMP is regulated to remain within the support
polygon, the robot is guaranteed not to roll over any edge of the support polygon. In this
case, the remaining degrees of freedom can be controlled as if the system is fully actuated
using standard feedback control techniques applied to fully actuated systems. By using
some simplifying assumptions, such approaches have been successfully demonstrated for gait
generation and execution on humanoid platforms such as the Honda Asimo [Sakagami et al.,
2002, Hirose and Ogawa, 2007], and the HRP series of walking robots [Kaneko et al., 2004].
25

These humanoid robots are designed with large feet; while walking, the robot usually has one
or both feet on the ground, so a large support polygon exists either in one foot, or in the area
between both feet, during double-support. The ZMP constraint defines differential equations
which can be simplified by considering lower-dimensional “lumped” models. One approach
models the HRP-2 robot as a cart (rolling mass) on top of a table (with a small base that
should not roll over), and applies preview control [Kajita et al., 2003b] to generate a center of
mass (COM) trajectory while regulating the ZMP position. The ZMP is only defined on flat
terrain, but some extensions can be applied to extend the idea to 3D, including using hand
contacts for stability, for example by considering the Contact Wrench Sum (CWS) [Sugihara,
2004].
To achieve motion planning for walking, one approach [Kuffner et al., 2003,Kuffner et al.,
2002] uses the RRT to find a collision free path in configuration space, while constraining
configurations to those that are statically stable. The robot is statically stable when the
center-of-mass is directly above the support polygon, therefore guaranteeing that the robot
will not roll over as long as the motion is executed slowly enough. After finding a statically
feasible trajectory of configurations, the trajectory is smoothed and filtered, and verified to
ensure that the ZMP is always in the support polygon. This approach has been extended
to account for moving obstacles and demonstrated on the Honda Asimo [Chestnutt et al.,
2005]. An alternative approach is to first generate a walking pattern while ignoring obstacles
and collisions, and then use random sampling to modify the gait to avoid obstacles while
verifying the CWS constraints [Harada et al., 2007]. These planners are impressive in the
dimension of the planning problem they are able to handle. However, the resulting plans are
relatively slow and cautious.
A quasi-static planner was also developed to achieve climbing behavior and walking on
varied terrain with HRP-2 [Hauser et al., 2008,Hauser, 2008]. Contact points and equilibrium
(static) stances, acting as waypoints, are first chosen by using a Probabilistic RoadMap
(PRM) [Kavraki et al., 1996]. The planner tries to find paths through potentially feasible
footholds and stances, while taking into account contact and equilibrium constraints to
ensure that the robot maintains a foot or hand hold, and does not slip. Motion primitives
are then used to find quasi-static local motion plans between stances that maintain the
non-slip constraints.
Although ZMP and other measures of stability have advanced the state of the art in
planning gaits over rough terrain, the stability constraints result in overly conservative
dynamic trajectories. Because such control systems do not reason about underactuated
dynamics, the humanoid robots do not perform well when walking on very rough or
unmodeled terrain, cannot move nearly as quickly as humans, and use dramatically more
energy (scaled) than a human [Collins et al., 2005].
Animals clearly do not constrain
themselves to such a regime of operation. Much more agile behavior takes place precisely
when the system is operating in an underactuated regime, for example when the COP is
outside of the support polygon, or when there is no support polygon, and the legged robot
is essentially falling and catching itself. Treating the problem this way, e.g., allowing the
legged machine to fall in a controlled way with every step and then knowing how to proceed,
26

by taking another step, is in some sense a more satisfying approach than ZMP (or other
dynamic stability) based control, which breaks down if the robot begins to fall. Furthermore,
underactuated dynamics might be exploited - for example, a humanoid robot can walk faster
and with longer strides by rolling the foot over the toe, rather than constraining the foot to
be flat on the ground.
Feedback control of underactuated “dynamic walking” bipeds has recently been ap-
proached using a variety of control methods, including virtual holonomic constraints
[Westervelt et al., 2003, Chevallereau et al., 2003, Westervelt et al., 2007] and time-
varying LQR on a transverse linearization [Manchester et al., 2009]. While these methods
demonstrate impressive behavior for a stereotyped gait, they need to be combined with
motion planning to take into account information about the environment, in order to enable
fast locomotion over very rough terrain.
The DARPA Learning Locomotion project, utilizing the LittleDog robot, has pushed
the envelope of walking control using careful foot placement. Much of the work developed
has combined path planning and motion primitives to enable crawling gaits on rough terrain
e.g. [Rebula et al., 2007,Kolter et al., 2008,Pongas et al., 2007,Ratliff et al., 2009]. Our team’s
approach initially utilized heuristics over the terrain, to assign a cost to potential footholds.
A* search was then applied to find feasible stances over the lowest cost footholds, and a
ZMP based body motion and swing-foot motion planner was used to generate trajectories to
walk over rough terrain [Shkolnik et al., 2007]. In the later phases of the project, we began
to integrate dynamic lunging, to move two feet at a time [Byl et al., 2008].
1.4
Contributions
The primary idea in this thesis is to intelligently guide search so that a sample-based motion
planning algorithm can more efficiently find plans in high dimensional dynamic systems. The
contributions of this thesis are summarized below. Contributions (1,3,5) are more general,
and can be applied toward a variety of motion planning problems. The methods developed
for these three contributions may be utilized on their own, or in combination with each other.
1. Task-space biasing: To reduce sensitivity to system dimension, a Voronoi bias in a
low-dimensional task space can be used to improve search efficiency for some problems.
This idea is applicable in general to kinematic path planning problems, or kinodynamic
motion planning problems.
2. Task-space control on a four legged quadruped platform is explored, in conditions
where the robot is assumed statically stable. The choice of redundancy resolution is
shown to have an important effect on walking stability.
3. Task-space control is derived for underactuated systems by using partial feedback
linearization. The derivation presented enables a task space Voronoi bias for systems
that are underactuated, complementing contribution (1). Furthermore, the derivation
27

includes provisions for optimal hierarchical redundancy resolution, the importance of
which was demonstrated in contribution (2).
4. A polar representation of center of mass is suggested as a task space mapping for
multi-link arms with passive joints. Task space control is applied within a planning
framework on this system to achieve swing-up motions. Mathematically, this type of
multi-link model can represent a quadruped standing on two legs, or a biped rolling
over its foot.
5. Reachability Guidance: The Reachability-Guided RRT is developed to enable efficient
randomized search for problems with differential constraints by changing the sampling
distribution to account for local reachability of the tree.
6. A combination of Task-space biasing and reachability guidance is demonstrated to
efficiently plan bounding motions over rough terrain for the LittleDog quadruped robot.
1.5
Outline
The thesis is generally divided into two sections. Following this introduction, chapters 2-4
deal with task space control, particularly with respect to integrating task space control in a
motion planning framework, where the task space effectively reduces the dimensionality of
the search to improve search efficiency. Chapter 5 describes the Reachability-Guided Tree to
improve search efficiency in the presence of differential and other kinodynamic constraints.
In chapter 6, techniques from task space biasing and reachability guidance are applied toward
a motion planner for controlling the LittleDog robot in a dynamic bounding task over rough
terrain.
More specifically, the chapters have the following organization:
Chapter 2: In this chapter I develop the Task-space biased RRT algorithm which
explicitly alters the RRT sampling strategy to be in a lower dimensional task space, making
the planning more efficient. The efficiency is improved by introducing a Voronoi Bias in the
task space. The algorithm is demonstrated on an N-link arm in a 2D workspace, with the
path-planning task of moving the end of the arm to a goal point in the workspace while
avoiding obstacles. The number of nodes explored by the algorithm was empirically shown
to remain approximately constant in N, allowing the algorithm to solve this problem with up
to 1500 dimensions in less than a minute, compared to the standard RRT implementation
which could only solve < 20 dimensions in the same amount of time, implying that the task
space bias can significantly improve search efficiency for some problems.
Chapter 3: hierarchical task space control of the Center of Mass and Swing Leg position
of a quadruped robot is explored. Because the quadruped has more degrees of freedom than
the task space, there are infinite solutions to this control problem. A whole-body Jacobian is
derived for the quadruped, allowing optimization within the null space to pick an appropriate
control from the infinite possibilities. The ZMP is shown to more closely follow the COM
28

projection when minimizing body rotations through the null space, allowing the robot to
execute COM and swing-leg movements more precisely and more quickly without falling
over. The implication is that the null space associated with a task-space Jacobian can be
used to improve performance. In this chapter, static stability is assumed, which requires 3
or 4 of the legs to be on the ground at a time. In this way, classical feedback linearization
techniques are applied, treating the robot as if it is fully actuated.
Chapter 4: task space control is extended to the case for when a system is underactuated.
This can happen, for example in a quadruped when only 2 feet are on the ground, or in a
humanoid when the foot is rolling (over the toe). A derivation for a task space controller
for underactuated systems is achieved by combining task space control in acceleration space
with partial feedback linearization. The resulting derivation can be generally applied to
underactuated systems which have more control degrees of freedom than the dimension of
the task space. Extending prior work, this derivation also cleanly presents a mechanism
for optimal redundancy resolution, while also demonstrating rank conditions for when the
controller can be used. When the task space consists of directly controlling the passive
Degrees of Freedom of the system, the rank condition is the same as the Strong Inertial
Coupling condition for pure partial feedback linearization.
Chapter 5:
the Reachability Guided Rapidly exploring Random Tree (RG-RRT)
algorithm is described.
The standard RRT algorithm typically relies on the Euclidean
distance metric to create a Voronoi bias in Euclidean Configuration space, when in fact a
dynamic distance indicating the cost to get from one point in state space to another point in