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

state space should be used. However, such a cost function is impossible to compute precisely
for most problems, and is expensive even to approximate. The RG-RRT addresses this by
using “local reachability” to guide the sampling when building the tree. More precisely,
as the Tree is being constructed, the algorithm keeps track of regions which can easily be
reached from the current tree. In the algorithm, random samples are drawn from a uniform
distribution in state space, but samples that are closer to the tree rather than to the reachable
set of the tree are discarded. This effectively dynamically shapes the sampling distribution
to produce a significant improvement in RRT motion plan search efficiency under differential
constraints, as demonstrated on toy problems including the Pendulum, Acrobot, and Simple
Dynamic Car Model.
Chapter 6: task space biasing and reachability guidance are combined to demonstrate
motion planning in application to quadruped bounding over rough terrain. This chapter
elaborates on motion primitives in their relation to Reachable sets. A simple motion primitive
is presented for the LittleDog robot. This motion primitive creates a task space bias, and
is easily conformed to the reachability framework. The resulting motion planner is able to
plan bounding motions for this small quadruped over rough terrain including steps and logs.
Chapter 7: concludes the thesis, and provides some future directions.
29

30

Chapter
2
Path Planning with a Bias in Task Space
The reduction of the kinematics and/or dynamics of a high-dimensional robotic manipulator
to a low-dimension “task space” has proven to be an invaluable tool for designing feedback
controllers.
When obstacles or other kinodynamic constraints complicate the feedback
design process, motion planning techniques can often still find feasible paths, but these
techniques are typically implemented in the high-dimensional configuration (or state) space.
A Voronoi bias in the task space can dramatically improve the performance of randomized
motion planners, while still avoiding non-trivial constraints in the configuration (or state)
space. This idea is explored in detail in this chapter. The potential of task space search is
demonstrated by planning collision-free trajectories for a 1500 link arm through obstacles to
reach a desired end-effector position.
Significant portions of this chapter also appear in [Shkolnik and Tedrake, 2009].
2.1
Task Space in the Context of Path Planning
Humans and animals have hundreds of bones and muscles in their bodies. State of the art
planning approaches, such as the RRT, can plan for systems with a moderate number of
dimensions, but to achieve animal-like agility, it may become necessary to plan in higher
dimensional systems.
In this Chapter, I demonstrate that fast planning in very high
dimensional systems can, at least under certain conditions, be achieved by exploring actions
in a lower dimensional task space. This approach is commonly used in feedback control
design, and is often called Task Space Control [Liegeois, 1977] (see section 1.3.2). Consider
a humanoid robot which needs to find its way from a start pose to cross a room full of
obstacles, and pick up an object [Kuffner et al., 2001]. For such problems, it is natural to
consider the task space. Rather than specifying a particular set of desired joint angles for the
whole robot which accomplishes a grasping task, one may consider only the hand position.
There are an infinite number of potential goal configurations which correspond to the desired
hand position. Having a large set of goals can sometimes make planning easier, as there will
be more trajectories that lead to a goal configuration. Furthermore, the defined task space
31

can be used to guide the search, even when planning still occurs in the high-dimensional
space in order to enforce all of the system constraints.
There are several recent works that utilize task space in the context of motion planning.
In [Bertram et al., 2006], for example, the sampling of points is biased by a heuristic function
which includes distance to the goal, so that points closer to the goal in task space are more
often randomly selected. The selected nodes are then extended with a randomly applied
action. In a variation of this work by [Weghe et al., 2007], a standard RRT is implemented
which occasionally chooses to grow toward the goal in task space by using the Jacobian
Transpose method. The goal of that work was to solve the problem of inverse kinematics
at the goal region because infinite configurations may correspond to a single point in task
space. Recently, [Diankov et al., 2008] proposed an approach that enables bidirectional
RRT search [Kuffner et al., 2000], where a backward tree (grown from the goal) is grown
in task space, and a forward tree (from the start pose) is grown in the full configuration
space. Occasionally, the forward tree attempts to follow the backward tree using either
random sampling of controls or the Jacobian transpose. These works utilize task space as
an intuitive way of defining an infinite (continuous) set to describe a goal position, rather
Figure 2-1: Simple example, demonstrating an RRT planning in a 3D configuration space,
while constrained to actions in a 2D task space. The task depicted is to find a path from the
starting node (blue circle), to the goal in task space (green line), while avoiding an obstacle
(Black Box). For simplicity, one may resolve redundancy by restricting the search to lie
in a plane (shown by red dash) perpendicular to the goal line, but other suitable choices
for redundancy resolution would not be constrained to a manifold. The intersection of the
obstacle on this plane is shown in blue dash line. For problems like this, planning in a lower
dimensional projection can be much more efficient than planning in the full space. c 2009
IEEE [Shkolnik and Tedrake, 2009].
32

than using task space as a potential way to constrain the exploration of the full space.
Another recent work, [Zucker et al., 2008] uses reinforcement learning to bias search based
on features in the workspace. Similar to [Diankov et al., 2008] and [Zucker et al., 2008], the
exploring/exploiting tree in [Rickert et al., 2008] also uses workspace obstacle information
to guide the planner search. In probabilistic road maps (PRMs), some works demonstrate
that workspace information can be taken advantage of by sampling along the medial axis,
or the subset of points in the workspace that are furthest from obstacles [Holleman et al.,
2000], or by dynamically sampling in regions by using information of workspace geometry
while taking into account the past history of sampling [Kurniawati and Hsu, 2006].
Note, that in many of these approaches, the planner takes advantage of collision
information in the workspace. This is not always possible however, and the work presented in
this chapter does not require this property. The benefits of this will be most evident in later
chapters, where we use robot COM as a task space, and there is little collision information
that is relevant in that space.
Another body of literature has focused on sample based planning algorithms for systems
that are constrained to a manifold defined by a lower-dimensional workspace, for example for
a robot that is writing on a wall, it must keep the end effector on the surface of the wall [Tang
et al., 2006]. In that work samples are constructed in configuration space that meet the
constraints imposed by the lower-dimensional manifold, but no attention is given to how the
particular choice of sampling affects planner performance, which is important given that there
may be infinite ways to construct such a sampling schema. In the ATACE framework [Yao
et al., 2005], configurations are sampled uniformly in C-space, while a workspace constraint
is imposed by utilizing a Jacobian pseudoinverse method. A similar C-space sampling based
method with task space constraints is provided by [Stilman, 2007]. The method I present is
different in that samples are obtained directly within task space. Doing so creates a Voronoi
bias in task space, rather than in Configuration Space, which for many systems leads to more
direct exploration. Another difference is that the method presented here does not operate on
a lower dimensional manifold, but instead considers the projection into task space. Because
of this, the method described here could search the full high dimensional C-space, despite
the attention to exploring in the lower dimensional task space.
Constraining a search to a lower dimensional space can be a powerful notion.
For
visualization, consider for example the task of planning in a 3D configuration space. The
goal might be to find a path from a starting coordinate to some region of the space, while
avoiding obstacles. Suppose the goal region is a line. Then one way to explore the space is
to grow in the plane which is perpendicular to the line, and intersects the starting node (as
shown in Figure 2-1). If there were no obstacles present, this type of search could produce
the most direct (shortest) path in configuration space. Because this plane slices through the
3D configuration space, as long as the plane has a path from start to goal, then it would
make sense to actually plan while constrained to this 2D plane, which drastically reduces
the set of potential solutions that need to be explored. Now imagine if rather than a 3D
configuration space, there is a large DOF system. If a plane can be defined which captures
a large portion of the feasible set of trajectories from start to goal, then it would be much
33

more efficient to plan within this plane rather than the full configuration space. A potential
objection would be that if the planning problem can be solved in this low-dimensional space,
then it is not a challenging planning problem. However, I would argue that in fact many of
the success stories in whole-body motion planning have this property.
This chapter proposes a framework for task space biased exploration for search
algorithms. A standard forward RRT [LaValle, 1998] is modified with a task space Voronoi
bias.
This is accomplished by sampling in the low dimensional task space, and then
growing the tree towards the sample in task space. The growth can be done efficiently
using the Jacobian Pseudoinverse (J
+
) method, commonly used in feedback task space
controllers, which eliminates the need for exact inverse kinematics solutions.
This task
space will have an associated null space, the use of which is left to the freedom of the
algorithm implementation. The pseudoinverse method allows for redundancy resolution via
a hierarchical control structure which takes advantage of the null space. One can think
of the standard RRT as randomly exploring in the null space. However, when actions are
constrained (which is usually the case), then as the dimensionality increases, it is increasingly
likely that a random vector in configuration space is orthogonal to a direction in task space,
producing a trade off between exploring directly in task space, or exploring the configuration
space.
If random samples are drawn from a uniform distribution over the task space, and a
euclidean distance metric is used for the NearestNeighbor() function, then the proposed
algorithm will have the property that the Voronoi bias associated with the RRT will be in
task space instead of in configuration space. In fact, these two spaces are quite different,
and for certain problems, the Voronoi bias in task space can significantly improve the search
performance. The algorithm is demonstrated for path planning a collision free trajectory
in an N-link arm, with the task of putting the end-effector at a desired coordinate. The
resulting planner is able to efficiently plan paths with up to 1500 degrees of freedom in
under a minute.
The proposed method can be thought of as planning in task space, while checking and
validating constraints in the full configuration (or state) space. The forward RRT was chosen
for its simplicity, but this method can be extended to a variety of planning algorithms,
including PRMs [Kavraki et al., 1996], bidirectional RRT’s, e.g. [Kuffner et al., 2000] -
either by implementing BiSpace [Diankov et al., 2008], or directly if goal(s) are known in
configuration space.
2.2
TS-RRT: Planning in Task Space with the RRT
2.2.1
TS-RRT Definitions
In this chapter, a vector in configuration space is denoted with q ∈
N
, and a vector in
task space with x ∈
M
, with an associated task space mapping x = f (q), and a Jacobian
defined as J ∈
M ×N
=
∂f (q)
q
.
34

2.2.2
TS-RRT
The proposed planner, given in Algorithm 3, is called Task Space RRT (TS-RRT). It
is similar to the standard RRT approach, described in Algorithm 1 in section 1.3.1, except
that a random sample, x
rand
is generated directly in task space, typically using a uniform
distribution over a bounded region. The TS-EXTEND() function, shown in Algorithm 4,
is similar to the standard RRT EXTEND() function in Algorithm 2, but works in task
space rather than C-space. The TS-NEAREST-NEIGHBOR() function selects the nearest
point on the tree in task space. Each node on the tree contains both C-space and task
space information, so the computation of TS-NEAREST-NEIGHBOR() would be less than
the comparable function in a standard RRT. A full configuration space action is generated
in TS-CONTROL(). Ideally, this control attempts to grow the nearest node of the tree
towards the random sample in task space. There are many ways of implementing this type
of control, and the action can even be random, as done in [Bertram et al., 2006]. A more
Algorithm 3 BUILD-TS-RRT (q
0
)
1:
T.init(q
0
)
2:
for k = 1 to K do
3:
if with some probability P then
4:
x
rand
⇐ RANDOM-SAMPLE() ∈
M
5:
else
6:
x
rand
⇐ x
goal
7:
end if
8:
TS-EXTEND(T,x
rand
)
9:
end for
Algorithm 4 TS-EXTEND (T, x)
1:
[q
near
, x
near
] ⇐ TS-NEAREST-NEIGHBOR(x,T)
2:
u ⇐ TS-CONTROL(q
near
, x
near
, x
rand
)
3:
q
new
⇐ NEW-STATE(q
near
, u)
4:
x
new
⇐ f (q
new
)
5:
if COLLISION-FREE (q
near
, q
new
) then
6:
T.add-node(q
near
, q
new
, x
near
, u)
7:
end if
Algorithm 5 Example TS-CONTROL(q
near
, x
near
, x
rand
)
1:
u
ts
⇐ (x
rand
− x
near
)/∆t
2:
u
ts
⇐ CROP-WITHIN-LIMITS(u
ts
)
3:
J ⇐ COMPUTE-JACOBIAN(q
near
)
4:
˙q
ref
= SECONDARY-CONTROL(q
near
, x
rand
)
5:
˙q ⇐ J
+
· u
ts
+ α(I − J
+
· J) · ˙q
ref
6:
return
∆q ⇐ ˙q · ∆t
35

efficient approach is to use the Jacobian pseudoinverse in a feedback type controller, as shown
in Algorithm 5). A new state in C-space, q
new
is integrated using the same NEW-STATE()
function as in the RRT. After checking for collisions in the full C-space, q
new
, its task space
mapping x
new
, and the control action are added to the tree.
Because the sampling occurs in task space, the Voronoi bias when growing the tree is
now in this space, rather than in the configuration space. This encourages the planner to
quickly explore empty regions within task space, which results in more efficient planning,
assuming paths in this space exist and there are no narrow channels, using the selected task
space mapping. Approaches for ensuring completeness are discussed in section 2.4.2.
The growth mechanism in Algorithm 5 allows for a secondary command, ˙q
ref
, to be
defined in terms of the configuration (state) space. If left as zero, then this controller will
simply take the shortest path within the configuration (state) space. However, this function
can be used to encourage the system to go toward certain configurations as much as possible,
and can be similar in spirit to the workspace centering (e.g. [Sentis and Khatib, 2005]), used
in the more typical feedback based task space control.
2.3
Scalability of TS-RRT compared to RRT
2.3.1
Path Planning for the N-Link Arm
In this section, a kinematic search problem is explored, in which an N-link arm needs to find
a suitable trajectory from a given start pose, with the task of putting its end effector at a
particular coordinate. Figure 2-2 demonstrates a 5-link version of this problem. The task
space utilized is the Cartesian coordinate of the end effector. This path planning problem
does not have dynamics, and therefore does not have any second-order effects including
gravity and torque limits. However, the joints are limited to move less than .05 radians
per each ∆t, and obstacles are imposed within the Cartesian plane, which are checked for
collisions against all links. Furthermore, joint limits of +/- 2.5 radians were imposed on all
links. This problem was explored with a constant set of obstacles, shown in red in Figure
2-2. A constant start pose (all angles = 0), and a constant desired goal pose were used. The
number of links, N, is variable, with the link length set to 1/N so that the total arm length
is always 1.
Two algorithms were explored on this problem. First, a standard forward RRT was
implemented. Because there are infinitely many potential goal poses which correspond to
the goal task in this problem, the configuration space was randomly sampled until 20 poses
were found that were collision free and whose end effector positions were close to the desired
position. The RRT Algorithm 1 was then run, with samples drawn uniformly in configuration
space (∈
N
with bounds of +/- pi). With probability P=.1, one of the 20 goal configurations
was chosen at random to be used as the sample to grow the tree toward. A Euclidean metric
(all joints weighted equally) was used in NEAREST-NEIGHBOR. Node growth occurred by
computing the vector between a sample and the closest node on the tree, and cropping the
components of this vector to be within +/- .05 radians.
36

Figure 2-2: Projection of RRT into End Effector Cartesian Coordinates for a 5-link arm
(shown in Magenta). The starting configuration ([0, 0, 0, 0, 0]
T
) is shown in Blue, and the
achieved goal configuration is shown in green. TOP: Standard RRT search was performed
in the 5-D configuration space. This tree has
2000 nodes. Obstacles in the workspace are
shown in red, with collision checking performed for 10 evenly spaced points along each link.
BOTTOM: the resulting RRT using TS-RRT is shown, where exploration is constrained
to the 2D task space. This tree has about 150 nodes. c 2009 IEEE [Shkolnik and Tedrake,
2009].
37

In addition to the standard RRT, a forward TS-RRT was run on the same problem.
The Jacobian pseudoinverse was used to calculate a desirable change in q to grow a selected
node toward a sample in task space. The null space reference command utilized was set to
∆q
ref
= −q, which encourages the arm to straighten when possible. ∆q was normalized
so that the max norm was .05. Random samples were taken uniformly in task space (∈
2
with bounds of +/- 1.1), and a Euclidean distance metric was used in task space to select
the closest nodes on the tree. With probability .1 the goal task was chosen as the sample.
Most of the code between the two implementations was shared, with an attempt to minimize
tuning toward each problem / implementation.
Figure 2-3: The number of nodes that the RRT algorithm searches before finding a feasible
trajectory for the problem shown in figure 2-3 is shown as a function of N, the number
of dimensions. The obstacles and goal task are held constant, and the link lengths are
set to 1/N so that the total arm length remains constant. Note that the x axis is on
a log scale, starting with N=2, going up to N=1000. RED: TS-RRT, which remains
approximately constant. BLUE: full configuration space RRT, which grows super-linearly.
c 2009 IEEE [Shkolnik and Tedrake, 2009].
38

Figure 2-4: TOP/BOTTOM are two rotated views of the same 3D plot. This figure shows
the configuration space view of an RRT tree (magenta), and a TS-RRT (blue), both for a
3-link arm. Obstacles (same as those shown in Figure 2-2) are shown by red dots, and the
goal task point is shown by green dots in the configuration space. The standard RRT has
good coverage in the space, as expected. Both trees converge to a collision free trajectory,
but it clear that the TS-RRT is much more direct.
c 2009 IEEE [Shkolnik and Tedrake,
2009].
39

2.3.2
Results
Each RRT was run 20 times, while varying the number of links (N) in the arm from 2
to 1000 (TS-RRT), and from 2 to 15 (RRT). The median number of nodes explored by
each tree is shown in Figure 2-3, as a function of N. The standard RRT quickly started
exploring thousands of nodes as N increased above 4, while the TS-RRT exploration remained
approximately constant. Of course, the Jacobian pseudoinverse and null space calculations
are expensive to compute, but in my implementation, using standard Matlab functions, they
added an overhead of
50 percent to the time needed to grow a node, including collision
checking. As an example of the actual time to compute paths, the TS-RRT algorithm took
4-8 seconds with N = 200 (approximately 200 nodes), which was comparable to the standard
RRT when using N = 6 (approximately 1500 nodes). Note, the time difference per node is
due mainly to increased computation for collision checking in higher dimensions, rather than
to the Pseudoinverse computation.
A comparison of an example of trees produced by each algorithm, with N=5, projected
into task space, is shown in Figure 2-2. It is clear that the TS-RRT searches more directly and