= 1, J
= 0, ˙J = 0, ¯
J = −M
The rank condition (4.5) requires that rank(M
) = l, in which case one can write ¯
, reducing the rank condition to precisely the “Strong Inertial Coupling” condition
described in [Spong, 1994a]. Now the command in (4.3) reduces to
v + (h
The torque command is found by substituting ¨
= v and (4.10) into (4.2), yielding the
same results as in [Spong, 1994a].
In situations where l is greater than p, there are inﬁnite solutions to achieve the desired
task-space command. One may use a null space projection to execute a second-priority task
Theorem 2 (Task Space PFL w/ Redundancy Resolution). If the actuated joints are
commanded so that
v − ˙J ˙q + J
) + α(I − ¯
and the condition (4.5) is met, then
y = v.
Proof. The proof is identical to Theorem 1, because the null-space terms are canceled by the
pre-multiplication of ¯
J, and therefore do not contribute to ¨
As discussed in the last chapter, redundancy resolution is a very important aspect of task
space control, particularly for underactuated systems. The command ¨
can be thought
of as a lower-priority task, which will be executed in the null-space of the high priority
task. If the command is zero, the controller will return the solution with the smallest norm.
More clever redundancy resolution tasks can help ensure the underactuated system stays
dynamically within a regime where there are more actions that can be taken, a characteristic
which can improve search performance.
Example: Feedback Control of the Acrobot
As an initial experiment, the task space controller was implemented on a two link arm with
a passive base joint. From herein, N-link arms with mixtures of Passive and Actuated joints
will be referred to by shorthand notation with combinations of P’s and A’s representing the
respective type of joint. This two link system, PA, is known as the Acrobot [Spong, 1995].
Direct application of task-space PFL on this system without planning clearly demonstrates
the power of the feedback linearization as well as the need for motion planning.
Speciﬁcally, the Acrobot was controlled to execute behaviors where the task space consists
of either 1) the angle from the base to the COM, θ
or 2) the angle from base to the end
Figure 4-2: Acrobot Control: Top, trajectory of control output function (angle from base to
end eﬀector); Blue is desired, red is actual. Bottom, snapshots of Acrobot during execution.
Red is ﬁrst link, Blue is second link, Magenta is commanded angle. c 2008 IEEE [Shkolnik
and Tedrake, 2008].
. Two potential output trajectories were considered: 1) maintain a constant
angle of 1 radian; θ
(t) = 1, or 2) follow an arbitrary desired trajectory - in this case deﬁned
(t) = 1 + .5 sin(4t). When the task space is θ
and the torque is unconstrained
both output trajectories are followed well, but the controller commands excessive torques
and the actuated joint spins. Similar results were obtained for controlling the end-eﬀector
angle, however in the special case if the second link is substantially longer than the ﬁrst link,
then the link does not spin, and instead the system “oscillates” back and forth along the line
speciﬁed by y
(t) = 1, as shown in ﬁgure 4-2. This is a demonstration of how the choice
of task space can drastically aﬀect the system dynamics. If the goal is to perform swing-up
behavior, for the Acrobot it may be better to control the end eﬀector. As the controller can
only operate on one degree of freedom with one actuator, treating the system as a pendulum
and controlling the center of mass angle ignores the constantly changing length, which makes
the pendulum (angle to COM) a poor choice of template. With redundant degrees of freedom
for cases where N > 2, however, we may regulate both the angle and length of the pendulum,
and so this becomes the task space of choice.
Similar results can be attained with higher DOF systems. These simulation experiments
reveal the need for motion planning to enforce joint and torque limits.
constraint imposed by the rank condition in equation 4.5 was never actually violated -
degeneracy in the Jacobian always presented itself with excessive torques before the mapping
became truly singular.
Planning in task space, when done correctly, is a much lower-dimensional planning problem
than planning directly in the state space of the robot.
In a real sense, the control
mapping from task space to (underactuated) joint space is a mechanism for exploiting known
properties of the dynamics of the robot in order to dramatically improve the eﬃciency of
search. To be practical, the task-space planner must respect constraints that exist only in
the higher-dimensional state space (such as joint and torque limits). The general concept of
planning in a low dimensional task space and verifying constraints in joints space is illustrated
in Figure 4-1.
As shown in Chapter 2, planning in higher dimensions can be improved by considering
actions in lower dimensional task space, (¨
y). Planning can be accomplished by computing
a mapping from task accelerations to joint torques, then simulating the full system given
those torques, and verifying that no constraints are violated. The intent is that the reduced
set of potential actions, combined with the task space control mapping is presented in this
chapter, can be powerful enough to enable the planner to ﬁnd a path from start to goal.
However, reachability to the goal will also depend on the choice of redundancy resolution.
Depending on the redundancy resolution, the task controller does not necessarily carve out
a manifold within state space. The controller constrains the actions, but the entire feasible
state space may still be reachable. Thus, completeness remains a function of the higher-level
planner, and is not necessarily constrained by the controller.
Figure 4-3: A low-dimensional template for the underactuated swing-up task. c 2008 IEEE
[Shkolnik and Tedrake, 2008].
If the task space is of low enough dimension, a simple quasi-exhaustive tree search may
be performed. More clever algorithms such as RRTs will have better performance, but I
begin by exploring a near brute force search approach illustrated in Algorithm 6. Such an
approach would become intractable if planning in a high-dimensional joint space, however
tractability is maintained when using a reduced dimensional task space controller. Except for
line 23, this is essentially a brute force search algorithm. The pruning step of line 23 sets a
maximum branching value for each depth, and keeps the tree from expanding exponentially.
Generalized Swing Up Control for N-Link Pendu-
lum with Passive Joints
Simulations of a 5-link pendulum
Consider a general, and classical, problem in underactuated control - the swing-up task
(e.g., [Spong, 1994b]) on a multi-link arm in a gravitational ﬁeld. The task is to move the
center of mass of the pendulum to the vertical conﬁguration. Many of the control solutions
investigated in this problem use energy-based methods; for simple pendulum, regulating the
total energy of the system is enough to place the system on a homoclinic orbit [Fantoni and
Lozano, 2002]. Most work on multi-link pendulum with passive joints have focused on cases
with 2 or 3 links [Spong, 1995,Lynch et al., 2000]. With more degrees of freedom, if searching
directly for a torque trajectory over the actuators, the problem is crippled by the curse of
dimensionality [Bellman, 1957]. In addition to increased system dimension, the problem is
Figure 4-4: COM Trajectories. c 2008 IEEE [Shkolnik and Tedrake, 2008].
further complicated by considering limitations of joint positions, and torques.
For reasons alluded to previously, a two-DOF pendulum, with control over angle and
length, was used as a template. This template and an example 5-Link pendulum is illustrated
in Figure 4-3. Using this approach, the algorithm was run on the following systems: AAAAA,
PAAAA, PAPAA, and PAPAP. In all cases, the motion planner was able to ﬁnd a trajectory
to the goal in reasonable time. Parameters of the system were as follows:
Mass of each link
Moment of Inertia
.05kg · m
In practice, the method in Algorithm 7 was used for pruning, where the metric function,
M, was based on the energy diﬀerence between a given state and the goal, and penalized by
a weighted sum of joint space distance away from the straight pose, where joints closest to
the base were more heavily weighted and therefore encouraged to be straighter than joints
further from the base. This metric encourages pumping energy into the system, but tries
to stay away from poses which wrap around. Nodes are probabilistically pruned, biased by
Algorithm 6 Simple Planning
Require: discrete set of actions, V ∈ R
, Depth limit of D, Depth node limit of B
for depth d = 1 to D do
n ⇐ 0
for each node, x
at depth d do
for each V
∈ V do
⇐ f (x
, ∆T )
if NO-CONSTRAINTS-VIOLATED( x
n ⇐ n + 1
if Goal Reached then
RETURN (GOAL REACHED)
if n == 0 then
RETURN (NO PATH FOUND)
if n > B then
prune n-B leaves from depth d
Algorithm 7 Probabilistic Pruning
Require: set of leaves, X
, leaf limit B, goal X
, 0 < α < 1
Compute a distance metric: H = M (X, X
), where H is normalized between 0
(furthest) and 1 (closest). This metric can also account for ”‘bad poses”’ or any other
costs on the state.
Sort( (α(H/2) + (1 − α) )*Random-Number-Generator
Return best N nodes
Figure 4-5: Swingup trajectory of PAAAA and PAPAA. c 2009 IEEE [Shkolnik et al., 2009].
the metric, a step that helps to ensure that some feasible paths remain if most high-scoring
nodes are actually headed to a dead-end.
Example center of mass trajectories for all four cases are compared in Figure 4-4. An
entire swing-up trajectory is illustrated for the PAAAA case in Figure 4-5(a), and the ﬁnal
three swings for the PAPAA case are shown in Figure 4-5(b).
It is easy to see that Algorithm 6 is
O(D · K · N
This is substantially better than the case without pruning, which would grow exponentially
To illustrate the time savings oﬀered by the dimensionality reduction, consider the
PAAAA example of the preceding section, with 1 passive joint, 4 actuators, and a 2-DOF
task space consisting of the angle and length to COM (l = 1, m=4, p=2). Let K = (B/N
be the average branching factor per node. In the results presented above, I used K=2, D=65
(∆T=.15), N=11. Then if the search is done in the lower dimensional task space where p=2,
the search time is O(D ∗ K ∗ N
) ∝ 2 million nodes. If instead the search was attempted
with the same discretization factor in the full joint space (note the branching factor blows
up to B = K ∗ N
), the equivalent time would be O(D ∗ K ∗ N
) ∝ 30 billion nodes.
Task space guided RRT
The planning algorithm presented above was a simple, quasi brute-force approach, and was
meant to demonstrate the power of the dimensionality reduction in the search, as a planning
algorithm with similar search density in the full state space would have been computationally
intractable. The contribution here is in addressing the underactuated control as a search
problem, and using task space to constrain the search. Note, however, that any planner can
be used if it satisﬁes the framework of Figure 4-1.
The Task-Space guided RRT satisﬁes the approach of Figure 4-1. In fact, the control
speciﬁed by equation 4.11 can be directly plugged in to line 5 of the CONTROL() function in
Figure 4-6: This ﬁgure shows the projection of an RRT tree built on a PAAAA system,
showing the system COM angle and angular velocity. The projection of the root of the tree
is shown by the blue circle, and the goal points are shown in green (wrapped around 2*pi).
One can see that this projection looks much like an RRT for a pendulum. Using the COM
as a task space, the Task Space Guided RRT is able to quickly ﬁnd plans for swing up on
Figure 4-7: LittleDog: Template and Anchor. c 2008 IEEE [Shkolnik and Tedrake, 2008].
the modiﬁed RRT algorithm presented in Algorithm 5, in section 2.2.2. This results in a fast
RRT based planning algorithm that utilizes the task space feedback controller to constrain
explored actions. An example of the result found with this RRT on a PAAAA system, after
approximately 20 seconds of search on a standard quad-core Pentium computer, is shown in
Figure 4-6. This ﬁgure illustrates a projection of the RRT tree from the 10 dimensional state
space to the COM angle and COM angular velocity. The COM length is part of the task
space, but is neglected here for clarity. It is interesting that despite being a 5 link system,
the projection looks very much like an RRT run on a 1-D torque limited pendulum.
Choice of template
A ﬁrst attempt to generate a swing up controller on the 5 link arm with passive joints, was
done by using a 1-DOF pendulum as a template. It seemed logical that length might be
stabilized in the Null space, and the planner could focus on increasing energy by controlling
only the angle of the COM. In practice, the torques required to keep the 5-link pendulum
of constant length were too great, and exhaustive search failed. Allowing the length to vary
lets the system act more like a spring. For example, when the COM angular velocity is high,
and inertia is greatest, increasing the COM-length helps minimize torques and keeps the
system within torque bounds.
Applications to locomotion
Many legged robots can be modeled by rigid body dynamics. Consider for example a sagittal
model of the LittleDog quadruped robot. When the robot is standing on its back legs, and
if the robot is moving the two back legs in unison, and moving the two front legs in unison,
then the system becomes equivalent to a 5-link PAAAA model. Figure 4-7 shows the robot
in this position, as well as a corresponding 5-link model and a 2-DOF template.
The techniques developed in this chapter may be utilized in a bounding gait planner,
which can be stabilized in task space using partial feedback linearization. Such stabilization
may allow for slight corrections in COM trajectory if possible. Figure 4-8 illustrates an
example of this for LittleDog. The top row of this ﬁgure illustrates two trajectories. For
simplicity, both images in the top row show a trajectory initialized to the same joint pose,
and the actuated joints remain in this ﬁxed pose throughout the trajectory. The passive joint
at the ground is initialized at 2.8 rad/sec (Left), or 2.0 rad/sec (Right). The ﬁgure on the
bottom left shows the COM angle trajectory over time, illustrating how the two trajectories
Figure 4-8: A: Trajectory with initial COM angular velocity of 2.8 rad/sec; joints held in
constant pose. B: Trajectory with initial COM angular velocity of 2.0 rad/sec; joints held
in constant pose. C: Initial COM angular velocity of 2.0 cm/sec; joints are actuated using
COM / PFL control, to try to emulate the COM trajectory with initial velocity of 2.8 rad
/sec. Bottom Left: COM vs time comparing the three trajectories.
(A and B) diverge over time. In C, we show that by applying PFL, the robot is able to
emulate the COM trajectory in A (2.8 rad/sec), even though it started with a passive joint
velocity of 2.0 rad/sec.
An interesting observation is that because the actual robot has 12 actuated DOF, the null
space may be utilized for foot placement. However, in practice, planning or PFL stabilization
for the LittleDog robot is diﬃcult because the inertial coupling of the limbs is low. Because
of this, slight modiﬁcations to the COM trajectory require a relatively large joint movement.
Furthermore, constraints such as torque and velocity limits break down the capabilities of a
pure PFL controller. However, there are related applications, in which inertial coupling may
be stronger, for example in humanoid robots. The application of a task space controller in
these applications is left to future work.
In this chapter I demonstrated a framework for planning in underactuated systems by
utilizing task space control and partial feedback linearization. The power of this approach
was demonstrated by application of a feedback controller in the Acrobot (PA), which was
able to closely track an arbitrary trajectory of its end-eﬀector. Feedback control can fail
due to constraints on the system, including torque limitations, physical obstacles in the
environment, joint limits, etc. The task space controller was used in a simple motion planner,
as well as in a TS-RRT in order to quickly search for trajectories from start to goal without
violating constraints. These planners were able to take advantage of the reduced action
space imposed by the task space control. Planning was successful in producing swing-up
behavior in 5-link models with anywhere from 0 to 4 passive joints. This toy problem lays
the foundation for real-world applications, including planning and stabilizing dynamic gaits
of a quadruped robot, and toe-roll in a humanoid.
In order to reduce the dimensionality by the task space mapping, the algorithm requires
strong inertial coupling. If there is weak inertial coupling, as in the case for LittleDog, which
has very light limbs, the PFL controller is not able to produce arbitrary accelerations in the
task space without violating torque and other constraints of the full system. For these cases,
alternative planning strategies that take into account the dynamics should be considered.
One idea to do this is presented in the next chapter.
Reachability-Guided Sampling for
Planning Under Diﬀerential Constraints
Chapters 2 - 4 have focused on the idea of using task space control to reduce the
dimensionality when planning on high-dimensional systems. The last chapter in particular
considered task space control and motion planning for systems with underactuated dynamics.
This chapter addresses planning for more general problems with diﬀerential constraints.
Obstacle ﬁelds with tunnels, or tubes are notoriously diﬃcult for most planning algorithms to
handle, including RRTs. Diﬀerential constraints are particularly challenging. The essential
symptom of this ineﬃciency is that nodes at the boundaries of these constraints tend to be
sampled and expanded upon repeatedly, but little progress is achieved in expanding the tree
towards unexplored regions of state space.
In this chapter I describe the Reachability Guided RRT (RG-RRT), which uses a new
sampling strategy for the RRT algorithm, based on an approximation of the regions of state
space that are local-time reachable from the tree. Taking into account local reachability
incorporates information about the dynamics, or any other diﬀerential constraints, into the
sampling strategy, and provides a dramatic improvement in performance in these severely
constrained systems. The reachability-guided sampling method is based on the observation
that sampling points at random and checking collisions is relatively cheap, but going through
the expansion step of the RRT, which typically requires integrating dynamics, is an expensive
step - both in its instantaneous cost and in the added cost of having a larger tree. Therefore,
the algorithm attempts to build sparse trees through kinodynamic obstacles, with a simple
heuristic that quickly rejects samples if it is likely that the tree can not grow in the direction
of this sample. This type of rejection sampling eﬀectively changes the sampling distribution,
so that points are selected uniformly from a potentially small portion of the conﬁguration
(or state) space in which the tree is capable of growing.
In order to illustrate the mechanisms involved with the new sampling strategy, the focus
in this chapter is on a low-dimensional torque-limited pendulum swing-up example, where
the Voronoi regions can be illustrated and the entire tree can be visualized eﬀectively. To
illustrate the generality of the approach, planning for a nonholonomic vehicle through a tight
corridor is also presented. In both cases, the algorithm makes a substantial improvement
in planning speed. Applicability of the algorithm to high dimensional systems is brieﬂy
discussed at the end of this chapter, and is addressed in more detail in the next chapter.