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

evenly within task space. A comparison of the trees produced, shown in the full configuration
space (for N=3), is shown in Figure 2-4. The figure shows two different views of the same two
trees, the blue corresponding to TS-RRT, and the magenta being the standard RRT. The
rectangular obstacles in task space correspond to large cylindrical (pancake-like) obstacles
in configuration space, shown by red dots in the figure. The goal task point is denoted by
green dots in the figure. The standard RRT produces a tree which seems to evenly spread
out in the 3D configuration space, as expected. The TS-RRT, on the other hand, is more
surface-like, but is able to navigate the obstacles.
The standard RRT could only run up to N=15 in a reasonable time (approx. 20 minutes
to solve). However, the TS-RRRT is able to find trajectories with N = 1500 in less than a
minute. Two example trajectories (and trees) are shown in Figure 2-5. In one trajectory,
the arm bends, and then changes the direction of the bend part way through. In the other
trajectory (bottom), the arm actually tied a knot through itself in order to reach the goal.
Note that in order to handle singularities, the maximum magnitude of angular displace-
ment, ∆q, is restricted. One can see that the starting pose is singular, but because the
planner is sample based it tends to immediately break out of any singular regions. More acute
examples of the influence of singular regions can be seen at the extremes of the workspace
in Figure 2-5. Towards the edges of the reachable workspace, the TS-RRT becomes slightly
irregular, with some nodes being selected repeatedly and expansion restricted. More clever
handling of singularities is possible as is done in standard feedback control, but was not
required for the task presented here as the system performance was still quite good.
2.4
Extensions and Possible Modifications
The algorithm is amenable to various modifications, and problem specific implementations.
40

Figure 2-5:
Two example trajectories and RRT plans, found with N = 1500.
Total
computation time was less than one minute. c 2009 IEEE [Shkolnik and Tedrake, 2009].
2.4.1
Dynamic and Underactuated Systems
It is possible to extend the TS-RRT algorithm to dynamic systems where new states are
generated by integrating the dynamics forward. The torque (or action) used can be computed
41

once per step, or incrementally, where u
ts
is recomputed inside the integration loop as x
near
changes. Algorithm 5 utilized a pseudoinverse Jacobian based velocity controller, but any
preferred method for computing velocity or acceleration based inverse kinematics ( [Nakanishi
et al., 2005] for review) can be utilized and integrated forward to generate a new state given
a desired point in task space. An example controller is analyzed in the next chapter, where
a COM task space controller is derived for LittleDog under the assumption that the robot
is quasi-fully actuated.
This method may also be extended to control underactuated systems, if a carefully chosen
task space is used. This idea is explored in chapter 4, which demonstrates swing-up control of
a 5-link pendulum with up to 3 passive joints, by controlling the center of mass of the system.
Potential applications for planning in high dimensional dynamic systems are far reaching,
and because the task space control allows for hierarchical control (e.g. the secondary control
˙q
ref
is only applied in the null space of ˙x
ref
, but ˙q
ref
may have been computed from another
task space with a null space, allowing for a tertiary controller, and so on). An example of
this might be planning humanoid grasping movement, by using the Cartesian coordinate of
the hand as a primary task, the orientation as a secondary task, and a desirable grip posture
(e.g. to leave fingers open) as a a tertiary task. As another example, in the next chapter
Center of Mass control is used as a primary task, with swing-foot position as a secondary
task to control a quadruped while walking.
2.4.2
On Completeness
The obvious drawback to exploring directly in task space is that the planner may lose
completeness, in the sense that it may no longer be possible to guarantee that a path
will be found, given that the set of trajectories which can be explored has been severely
restricted. Thus the choice of task space is critical, as is the choice of redundancy resolution
(e.g. selection of ˙q
ref
). If a task space can encode actions with enough breadth such that
feasible solutions exist, it remains unnecessary to explore in the full state space. If a solution
exists within the task space, given the redundancy resolution mechanism employed, then
this planner will still remain probabilistically complete. Additionally, depending on the
redundancy resolution mechanism, taking an action (in task space) in one direction and
then taking the reverse action can have the effect that the system returns to the original
point in task space, but to a different point in state space. As such, it may be possible that
all points in state space are reachable with some action sequence, implying that the proposed
algorithm is still probabilistically complete.
The TS-RRT algorithm presented can also be modified so that samples are chosen in
configuration space, as in the standard RRT. The standard NEAREST-NEIGHBOR function
would be used, so that a configuration space metric is used to evaluate and find the closest
node on the tree. The remainder of the algorithm is the same, where a node chosen for
expansion grows toward the sample in task space. Note, it is possible to augment or define the
˙q
ref
term in Algorithm 5, the secondary task in the control function, with: (q
rand
−q
near
)/∆t.
In such a case the system will try to move toward the sample in configuration space, making
the algorithm more similar to the standard RRT. By tuning α, growth can be biased to
42

Figure 2-6: With more challenging obstacles, a hybrid approach can be used to search
efficiently by exploring directly in task space part of the time, and directly in configuration
space the remaining time. c 2009 IEEE [Shkolnik and Tedrake, 2009].
explore more in task space, or to explore more in configuration space, like the standard
RRT. However, it is important to realize that a drawback to such an approach is that the
Voronoi bias within task space will be disrupted by the sampling within configuration space,
because uniform sampling in one space usually does not map to a uniform sampling in the
other space.
2.4.3
Hybrid Approach
Because book-keeping is virtually the same for both the RRT and TS-RRT algorithms, they
are quite interchangeable, and in fact, one can combine both approaches by searching in
configuration space with some probability, while searching in task space the remaining time.
A variation of may be to search in task space until it is detected that the tree is not expanding,
at which point the algorithm can start to sample in configuration space. Any sampling in C-
space ensures probabilistic completeness, while sampling in T-Space allows for faster, more
direct exploration when possible. Figure 2-6 demonstrates the resulting RRT which used
43

such a hybrid approach. Nodes were expanded using either RRT or with TS-RRT with a
probability P = .5, to solve a difficult trajectory problem for a 10 link arm with obstacles
that define very narrow passable channels. The solution in this configuration is typically
found after exploring fewer than 5000 nodes. Interestingly, I was unable to find a trajectory
for this problem setup if using exclusively either the standard-RRT or the non-augmented
TS-RRT, even after 200,000 nodes of exploration.
In the worst case, if all extend operations in C-space are inefficient (approximately
perpendicular to all solution trajectories), then the maximum slowdown of this approach
is a factor of < 1/P compared to searching in T-space alone. However, this may be a small
price to pay in order to achieve probabilistic completeness. Generally, the more challenging
the problem, defined by the degree of complexity of obstacles (e.g. narrow obstacles), the
more it will benefit to explore more often in C-space. A more thorough analysis of such a
hybrid approach is left to future work.
2.5
Concluding Remarks
Task Space Control [Liegeois, 1977] is widely used in feedback control, but has been less
used in the planning community. For some classes of problems, a carefully chosen task space
may improve planning performance because a Voronoi bias encourages exploration of empty
regions in task space, rather than in the full configuration space, which allows solutions
to be found more directly. It is not unreasonable to assume that problems that can be
solved this way in low dimensions can be scaled to higher dimensions in approximately linear
time, as the number of nodes needed to find a path stays approximately constant with N,
and only the time to check collisions and to compute the Jacobian pseudoinverse increases.
Although this chapter was specifically targeted toward modifying the RRT algorithm in
particular, the concept of task space exploration may be a powerful tool to enable a variety
of planning algorithms to work in higher dimensions. Furthermore, the applications are
numerous, especially given that 1) many real world systems are high dimensional and 2) It
is often natural to specify low dimensional tasks for many problems.
44

Chapter
3
Task Space Control for a Quadruped Robot
The previous chapter showed that task space can be useful in the context of efficient motion
planning.
Task space control has been well studied for fully actuated systems such as
robotic arms, however legged robots are underactuated and present additional challenges for
implementing task control. This chapter explores task control for Center of Body (COB),
Center of Mass (COM), and swing leg foot position for the LittleDog robot, essentially laying
a task control framework for generating a walking gait on arbitrary terrain. The method
presented can be generalized to any legged robot. I derive whole body Jacobians that can be
used to control the robot in these task spaces. The Jacobian based task control presents a
natural framework for resolving redundancies, but it is up to the control designer to choose
how exactly to do that. A typical approach for redundancy resolution is known as workspace
centering, which tries to maintain a desirable posture as much as possible, while executing
a task. In this chapter, I explore several redundancy resolution schemes, while executing
task space movements. For simplicity, the robot COM is constrained to be over the support
polygon, which guarantees static stability, that is when the robot is not moving it will not
fall over. However, when executing motions in task space, for example a body movement, the
choice of redundancy resolution becomes very important. One choice we consider minimizes
body pitching and rolling, which maximizes the utility of the static stability metric, allowing
the robot to make faster motions in task space, without falling over.
This chapter derives task space control in some technical detail. The implication for
motion planning is that when using Jacobian based methods, which are naturally supportive
of hierarchical control, the secondary or tertiary control (which is responsible for redundancy
resolution), impacts the ability of the dynamic system to feasibly execute task space motions.
Thus, if using task space control in motion planning, care needs to be taken to ensure that
redundancy is resolved in the best interest of the planner.
Significant portions of this chapter also appear in [Shkolnik and Tedrake, 2007].
45

Figure 3-1: Littledog Robot A picture of the LittleDog robot, executing the task of keeping
its center of mass over the centroid of its support polygon, while moving the front-left foot
in a fast Figure-8 trajectory. The robot has a 6 axis IMU and encoders at its 12 actuated
joints as well as Vicon motion capture system for body position / orientation. The body
weighs 1.8kg, and each leg weighs .25kg; The body is 20cm in length, while the upper arm
is 8cm, and the lower arm is 10cm. c 2007 IEEE [Shkolnik and Tedrake, 2007].
3.1
Motivation for COM Control
The notion of true stability for locomotion is difficult to quantify precisely (see [Tedrake
et al., 2006, Byl and Tedrake, 2009b]), so heuristics are often utilized for this purpose. These
include: maximizing the static stability margin [McGhee and Frank, 1968], maximizing
the Zero-Moment Point (ZMP) margin [Hirai et al., 1998], Resolved Momentum Control
(RMC) [Kajita et al., 2003a], and Zero Spin Center of Pressure (ZSCP) Control [Popovic
et al., 2004], etc (see [Pratt and Tedrake, 2005] for review). Essentially all of these approaches
try to control the center of mass of the entire robot, so we pay particular attention to this
issue in this work. This is true even for ZMP, where a COM trajectory can be computed to
follow a desired ZMP trajectory [Kajita et al., 2003b].
The RMC framework for humanoid robots attempts to control a humanoid robot’s whole-
body linear and angular momentum (or components of these), using a framework similar to
the whole-body Jacobian, while constraining feet movement to specified trajectories [Kajita
et al., 2003a]. Linear momentum divided by mass of the system translates to COM velocity,
so the high level objective of RMC is similar to this work. However, RMC assumes actuated
ankles, and is primarily concerned with how to utilize free DOFs of a humanoid, for example
46

the arms and torso, to help keep the robot stable. The quadruped robot does not have such
flexibility. Further, the work developed here allows for hierarchical control with priority
given to the COM velocity rather than to the feet velocities, whereas RMC does not allow
for this.
The remainder of this chapter is organized as follows: 1) derivations of the partial inverse
kinematics control of the COB; 2) derivations of the whole-body Jacobians associated with
the tasks COM control and swing foot control; 3) three redundancy resolution schemes are
compared in simulation, and results are presented from experiments on the actual robot.
3.2
Single-Leg Inverse Kinematics Control
A first approach to developing a walking controller for the position controlled walking robot
might be to treat all of the legs separately, and control the center of body and the orientation
of the body by moving the legs appropriately. When considering each leg separately, one
can utilize the relation:
P
G
= X
B
n
+ R
B
· P
L
(3.1)
where P
G
∈ R
3×n
contains the feet positions in the global frame; P
L
∈ R
3×n
contains the feet
positions in the robot relative frame; X
B
∈ R
3
is the center of body position and X
B
n
∈ R
3×n
is the X
B
vector repeated n times: [X
B
... X
B
]; R
B
∈ R
3×3
is the rotation matrix of the
body; n is the number of feet being considered (usually 3 or 4). Differentiating and solving
for the feet velocities in the relative frame:
˙
P
L
= R
T
B
· ( ˙
P
G
− ˙
X
B
n
− ˙
R
B
· P
L
)
(3.2)
Note that ˙
P
G
is assumed to be zero for stance feet, and is otherwise the velocity command
of the swing foot. ˙
X
B
and ˙
R
B
are the commanded COB and orientation velocities.
For each leg, i, one can compute the single-leg Jacobians, J
leg
i
∈ R
3×3
, of the foot position
w.r.t. the robot frame. The joint velocities corresponding to each leg are then:
˙q
leg
i
= J
−1
leg
i
· ˙
P
L
i
(3.3)
The result is a control for COB, not COM. Further, it may be useful to allow the body
rotation to be left unspecified. This increases the workspace of the system by allowing
the robot to rotate the body to help reach places that would otherwise be kinematically
infeasible. To deal with these issues, whole-body Resolved Motion Rate Control [Whitney
and D.E., 1969] is utilized.
47

3.3
Whole-Body Jacobian Control
Recall that forward kinematics, transforming joint angles q ∈ R
n
into some task space
x ∈ R
m
, and the differentiation of this relation is given by:
x = f (q)
(3.4)
˙x = J(q) · ˙q
(3.5)
where J (q) ∈ R
m×n
is the whole-body Jacobian associated with the task space of x. The
inverse kinematics with nullspace optimization for redundancy resolution can be solved as
in [Liegeois, 1977]:
˙q = J
+
· ˙x + α(I − J
+
· J) · ˙q
ref
(3.6)
where J
+
= (J
T
J)
−1
· J
T
is the Moore-Penrose pseudoinverse of J, α is a scalar weighting,
and ˙q
ref
∈ R
n
is a low priority command in joint space. For the redundant case where
m < n, the solution, ˙q should achieve the commanded ˙x while also minimizing
˙q − ˙q
ref
.
One may have multiple tasks, and control them in a hierarchical manner [Khatib, 1987],
for example:
˙q
1
= J
+
low
· ˙x
low
+ α
1
(I − J
+
low
· J
low
) · ˙q
ref
˙q = J
+
high
· ˙x
high
+ α
2
(I − J
+
high
· J
high
) · ˙q
1
(3.7)
where x
high
represents a “high priority task” with associated J
high
Jacobian, and conversely
x
low
is a “lower priority task” with associated J
low
Jacobian. ˙q
1
is the joint level command
that would be assigned by the low priority task, and is passed on as a “suggested” command
to the high priority task.
Typically, the lowest priority task in joint space consists of specifying ˙q
ref
to move down
a potential to bring the posture to some standard “favored” position, as in [Khatib et al.,
2004]. Note that it is possible to differentiate (3.5), which would allow for control at the
acceleration or force level (for review see [Nakanishi et al., 2005]). Because the LittleDog
robot is position and velocity controlled, I will limit the discussion here to controllers that
resolve kinematic redundancies at the velocity level.
3.3.1
Jacobian of Center of Body
This section presents a derivation of the Jacobian, J
X
B
(q) =
∂X
B
∂q
a
, of the center of body,
X
B
∈ R
3
, where q ∈ R
18
describes all 18 degrees of freedom, and q
a
∈ R
12
is the actuated
subset of q, not including the 6 unactuated degrees of freedom of the body position and
orientation.
The typical approach for computing the Jacobian would be to define forward kinematics
and then differentiate w.r.t. actuated joint angles. It is not obvious how to do this for the
center of body of the robot, and some assumptions must be made. First, note that given q,
one may compute the position of all feet in both relative and absolute coordinate systems.
48

Given absolute positions of three feet, and three leg lengths, one may compute the position
of the center of body by performing trilateration, e.g. by finding the intersection of three
spheres, centered at the feet positions with radii corresponding the distance between the foot
and COB. However, this derivation is bulky and, is not well defined for cases with four feet.
In order to get around the problem of underactuation, I make an assumption that feet on
the ground do not move, that is to say that there is infinite friction, and the ZMP remains
inside the support polygon. Consider what happens when changing the leg lengths; first
note that the lengths, L ∈ R
4
, are simply the distance from center of body to the feet. This
is defined in either the global coordinate frame, or the robot coordinate frame for each foot,
i:
L
i
=
P
L
i
=
X
B
− P
G
i
(3.8)
Let us assume that all feet are on the ground (otherwise use the subset of three feet which
are on the ground), and the feet are not moving:
∂P
Gi
∂q
a
= 0. One can obtain
∂L
∂q
a
∈ R
4×12
by
differentiating for each leg length individually, corresponding to each row i:
∂L
i
∂q
a
=
∂L
i
∂P
L
i
∂P
L
i
∂q
a
=
∂L
i
∂X
B
∂X
B
∂q
a
(3.9)
These derivatives are well defined, and have geometrically understandable meanings. For
example,
∂L
∂X
B
∈ R
4×3
is the change of leg lengths given a movement of the body (imagine
a table with springs for legs; the springs change length in a well defined way if the table is
moved or rotated). One can now solve for
∂X
B
∂q
a
using the Moore-Penrose pseudoinverse:
∂X
B
∂q
a
=
∂L
∂X
B
+
∂L
∂P
L
∂P
L
∂q
a
(3.10)
where each row i of
∂L
∂X
B
∈ R
4×3
is:
∂L
i
∂X
B
=
∂||P
G
i
− X
B
||
∂X
B
=
[X
B
− P
G
i
]
T
||X
B
− P
G
i
||
and, if one treats P
L
as a vector ∈ R
12
, and define P
L
i
as a vector ∈ R
12
containing all zeros
except the 3 elements corresponding to leg i, then each row i of
∂L
∂P
L
∈ R
4×12
is:
∂L
i
∂P
L
=
∂||P
L
i
||
∂P
L
=
[P
L
i
]
T
||P
L
i
||
49