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

1
= 1, J
2
= 0, ˙J = 0, ¯
J = −M
−1
11
M
12
.
The rank condition (4.5) requires that rank(M
12
) = l, in which case one can write ¯
J
+
=
−M
+
12
M
11
, reducing the rank condition to precisely the “Strong Inertial Coupling” condition
described in [Spong, 1994a]. Now the command in (4.3) reduces to
¨
q
2
= −M
+
12
[M
11
v + (h
1
+ φ
1
)]
(4.10)
The torque command is found by substituting ¨
q
1
= v and (4.10) into (4.2), yielding the
same results as in [Spong, 1994a].
4.1.2
Redundancy Resolution
In situations where l is greater than p, there are infinite solutions to achieve the desired
task-space command. One may use a null space projection to execute a second-priority task
in actuator-space:
Theorem 2 (Task Space PFL w/ Redundancy Resolution). If the actuated joints are
commanded so that
¨
q
2
= ¯
J
+
v − ˙J ˙q + J
1
M
−1
11
(h
1
+ φ
1
) + α(I − ¯
J
+
¯
J)¨
q
ref
2
,
(4.11)
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 ¨
y.
62

As discussed in the last chapter, redundancy resolution is a very important aspect of task
space control, particularly for underactuated systems. The command ¨
q
ref
2
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.
4.1.3
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.
Specifically, the Acrobot was controlled to execute behaviors where the task space consists
of either 1) the angle from the base to the COM, θ
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 effector); Blue is desired, red is actual. Bottom, snapshots of Acrobot during execution.
Red is first link, Blue is second link, Magenta is commanded angle. c 2008 IEEE [Shkolnik
and Tedrake, 2008].
63

effector, θ
end
. Two potential output trajectories were considered: 1) maintain a constant
angle of 1 radian; θ
d
(t) = 1, or 2) follow an arbitrary desired trajectory - in this case defined
as θ
d
(t) = 1 + .5 sin(4t). When the task space is θ
COM
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-effector
angle, however in the special case if the second link is substantially longer than the first link,
then the link does not spin, and instead the system “oscillates” back and forth along the line
specified by y
end
d
(t) = 1, as shown in figure 4-2. This is a demonstration of how the choice
of task space can drastically affect the system dynamics. If the goal is to perform swing-up
behavior, for the Acrobot it may be better to control the end effector. 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.
The dynamic
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.
4.2
Planning
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 efficiency 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 find 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.
64

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.
4.3
Generalized Swing Up Control for N-Link Pendu-
lum with Passive Joints
4.3.1
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 field. The task is to move the
center of mass of the pendulum to the vertical configuration. 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
65

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 find a trajectory
to the goal in reasonable time. Parameters of the system were as follows:
Mass of each link
.2kg
Length
.2m
Moment of Inertia
.05kg · m
2
Torque Limitation
2 Nm
In practice, the method in Algorithm 7 was used for pruning, where the metric function,
M, was based on the energy difference 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
66

Algorithm 6 Simple Planning
Require: discrete set of actions, V ∈ R
N xp
, Depth limit of D, Depth node limit of B
1:
Add-node (x
0
, root)
2:
for depth d = 1 to D do
3:
n ⇐ 0
4:
for each node, x
n
at depth d do
5:
for each V
i
∈ V do
6:
tau
H
⇐ task-control(x
n
, V
i
)
7:
if |tau
H
< tau
LIM IT
| then
8:
Simulate: x
new
⇐ f (x
n
, tau
H
, ∆T )
9:
if NO-CONSTRAINTS-VIOLATED( x
new
) then
10:
Add-node(x
new
, X
n
)
11:
n ⇐ n + 1
12:
end if
13:
end if
14:
end for
15:
end for
16:
if Goal Reached then
17:
RETURN (GOAL REACHED)
18:
end if
19:
if n == 0 then
20:
RETURN (NO PATH FOUND)
21:
end if
22:
if n > B then
23:
prune n-B leaves from depth d
24:
end if
25:
end for
Algorithm 7 Probabilistic Pruning
Require: set of leaves, X
leaves
, leaf limit B, goal X
goal
, 0 < α < 1
1:
Compute a distance metric: H = M (X, X
goal
), 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.
2:
Sort( (α(H/2) + (1 − α) )*Random-Number-Generator
3:
Return best N nodes
67

(a) PAAAA
(b) PAPAA
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 final
three swings for the PAPAA case are shown in Figure 4-5(b).
4.3.2
Computational Efficiency
It is easy to see that Algorithm 6 is
O(D · K · N
(2p)
).
This is substantially better than the case without pruning, which would grow exponentially
on D.
To illustrate the time savings offered 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
p
)
be the average branching factor per node. In the results presented above, I used K=2, D=65
68

(∆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
(2p)
) ∝ 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
m
), the equivalent time would be O(D ∗ K ∗ N
(2p)
) ∝ 30 billion nodes.
4.3.3
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 satisfies the framework of Figure 4-1.
The Task-Space guided RRT satisfies the approach of Figure 4-1. In fact, the control
specified by equation 4.11 can be directly plugged in to line 5 of the CONTROL() function in
Figure 4-6: This figure 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 find plans for swing up on
this system.
69

Figure 4-7: LittleDog: Template and Anchor. c 2008 IEEE [Shkolnik and Tedrake, 2008].
the modified 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 figure 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.
4.4
Discussion
4.4.1
Choice of template
A first 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.
4.4.2
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,
70

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 figure 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 fixed 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 figure 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.
71

(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 difficult because the inertial coupling of the limbs is low. Because
of this, slight modifications 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.
4.4.3
Concluding Remarks
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-effector. 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.
72

Chapter
5
Reachability-Guided Sampling for
Planning Under Differential 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 differential constraints.
Obstacle fields with tunnels, or tubes are notoriously difficult for most planning algorithms to
handle, including RRTs. Differential constraints are particularly challenging. The essential
symptom of this inefficiency 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 differential 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 effectively changes the sampling distribution,
so that points are selected uniformly from a potentially small portion of the configuration
(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 effectively. To
illustrate the generality of the approach, planning for a nonholonomic vehicle through a tight
73

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 briefly
discussed at the end of this chapter, and is addressed in more detail in the next chapter.