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

∂P
L
∂q
a
=




J
leg
1
0
0
0
0
J
leg
2
0
0
0
0
J
leg
3
0
0
0
0
J
leg
4




∈ R
12×12
3.3.2
Jacobian of Body Rotation
By differentiating (3.1) w.r.t. q
a
j
, for each joint, j, and assuming that stance feet are not
slipping so that
∂P
Gi
∂q
a
= 0 for each stance foot i, one finds that:
∂R
B
∂q
a
j
· P
L
= −
∂X
B
n
∂q
a
j
− R
B
·
∂P
L
∂q
a
j
(3.11)
The Moore-Penrose pseudoinverse is applied to solve for J
R
Bj
=
∂R
B
∂q
aj
∈ R
3×3
for each
joint j:
J
R
Bj
= −
∂X
B
n
∂q
a
j
+ R
B
·
∂P
L
∂q
a
j
· (P
L
)
+
(3.12)
3.3.3
Center of Mass Jacobian
The conversion from local frame (L) to global frame (G) for the center of mass, X
M
∈ R
3
is
specified by:
X
M
G
= X
B
+ R
B
· X
M
L
(3.13)
Then solve for J
X
M G
=
∂X
MG
∂q
a
, where each column j is:
J
X
M Gj
=
∂X
B
∂q
a
j
+ R
B
·
∂X
M
L
∂q
a
j
+
∂R
B
∂q
a
j
· X
M
L
(3.14)
3.3.4
Swing Foot Jacobian
The conversion from relative to global coordinates for the position of a specific foot, X
SW
∈
R
3
, is specified by:
X
SW
G
= X
B
+ R
B
· X
SW
L
(3.15)
Then solve for J
X
SW G
=
∂X
SW G
∂q
a
, where each column j is:
J
X
SWGj
=
∂X
B
∂q
a
j
+ R
B
·
∂X
SW
L
∂q
a
j
+
∂R
B
∂q
a
j
· X
SW
L
(3.16)
50

3.3.5
Hierarchical Controller
As shown in (3.7), the Jacobians and associated null spaces can be “stacked” in a hierarchical
manner. Given the goal of stable walking over rough terrain, it is logical to give the control
of ˙
X
M
G
the highest priority. Of second priority, then, is ˙
X
SW
G
. The final step in developing
the full control law is choosing an appropriate ˙q
ref
to plug into (3.7). Workspace centering is
established by choosing ˙q
ref
along the gradient of the potential:
q
a
− q
0
for some favored
position, q
0
∈ R
12
.
A proposed alternative to workspace centering is to compute the partial IK solution, as
in section 3.2; for each leg, i, the corresponding elements of ˙q
ref
are:
˙q
ref
i
= J
−1
leg
i
˙
P
L
i
(3.17)
Using the single-leg inverse kinematics represents a dynamic redundancy resolution
method, as opposed to workspace centering which always tries to move the robot back
to the static q
0
position. The final control is specified by:
˙q
1
= J
+
SW
G
· ˙x
SW
G
+ (I − J
+
SW
G
· J
SW
G
) · ˙q
ref
˙q = J
+
M
G
· ˙x
M
G
+ (I − J
+
M
G
· J
M
G
) · ˙q
1
(3.18)
Kinematic joint limits can also be included as a task. This control is only activated when
joints are close to their limits, and is thus not included here for clarity.
3.4
Results
3.4.1
Simulation
The result of the preceding analysis distinguishes three potential controllers: 1) control by
single-leg inverse-kinematics, where body orientation must also be commanded (in the case
of the simulations below, I use zero pitch and roll, and keep yaw constant); 2) Whole-body
Jacobian RMRC based control using static redundancy resolution which chooses trajectories
that stay as close as possible to a standing pose (static redundancy resolution); and 3) A
hybrid approach, based on whole-body Jacobian control which chooses trajectories that are
as close as possible to the single-leg IK solutions (dynamic redundancy resolution).
In this section I explore the performance of these three controllers by looking at a
simulation of a quadruped robot with parameters similar to the LittleDog robot.
The
physics based dynamics simulation was constructed with SD/FAST, and the controller was
implemented in Matlab. A spring-damper ground model is utilized with point-feet contacts,
with reasonable ground reaction forces (drawn in small blue lines in the figures).
The
simulation has 4 test-cases to illustrate the performance in sample COM and swing foot
trajectory tasks.
51

• CASE 1: Figure 8 trajectory for COM, with 4 feet on ground. In this case, a figure
8 desired trajectory is tracked with the whole-robot center of mass (see Figure 3-4).
Four feet remain on the ground.
• CASE 2: Figure 8 trajectory for COM, with 3 feet on ground. In this case, a figure
8 desired trajectory is tracked with the whole robot center of mass (see Figure 3-5).
Three feet remain on the ground, and one foot is raised in the air.
Figure 3-2 shows the ZMP and center of mass, comparing the work-centering and
hybrid approaches. The whole-body Jacobian with work-centering utilizes more drastic
motions, with the involvement of all limbs, which results in the ZMP straying far from
the COM projection. This undermines the static stability assumption that was made
when selecting the figure 8 COM trajectory to follow, and results in the robot eventually
toppling over. On the other hand, the ZMP corresponds fairly well with the COM
projection in the hybrid controller, as this controller minimizes drastic movements.
Thus, even though the robot is moving fairly quickly (executing the figure 8 motion
in 3 seconds), the static stability margin is reasonable to use with the controller using
the dynamic redundancy resolution. This illustrates why the hybrid controller is more
stable when executing fast movements.
• CASE 3: Small, fast, figure 8 trajectory for swing leg. In this case, a small figure 8
is to be tracked by the front right leg, while the COM is to remain over the centroid
of the support polygon (see Figure 3-6). In this test, the entire figure 8 trajectory is
completed quickly (in 1 sec.).
• CASE 4: Very large, slower, figure 8 trajectory for swing leg. In this case, a very large
figure 8, requiring the robot to pitch its body up and down to complete it accurately,
is to be tracked by the front right leg, while the center of mass is to remain over the
centroid of the support polygon (see Figure 3-7). In this test, the figure 8 trajectory
is completed fairly slowly, over a period of 15 seconds.
3.4.2
Real Robot
The tests were attempted on the actual robot, with results for the first three cases of the
hybrid controller (with feedback) shown in Figure 3-3. A photo of the robot executing the
small figure 8 task is in Figure 3-1. The gains on this feedback controller were difficult
to tune, with a trade off between oscillations and significant foot slippage vs tracking
performance.
Performance was deteriorated compared to simulation because feet were
slipping, which violated our Jacobian assumptions that grounded feet were not moving.
This was enhanced by oscillations due to feedback latencies. Test case 4, large figure-8 with
swing foot, could not be achieved at all due to feet slipping.
In another approach, joint trajectories were generated by using the simulator with the
hybrid controller for the test cases presented above. These trajectories were recorded, and
passed to the actual LittleDog robot as a feed-forward command. The performance was quite
52

good, and resembled the performance seen in the simulators. If feet did not slip, tracking
looked much better than the feedback control shown in Figure 3-3.
3.5
Discussion
In the prior chapter, I illustrated that fast path planning can be achieved by biasing a search
in task space. This is achieved by sampling in task space, and is complemented, if possible,
by an inner control loop which regulates the system in task space. In this chapter, I explored
task space control for a quasi-actuated quadruped robot. The controller developed can be
applied within a search based planning algorithm, for example by defining a task space
consisting of the horizontal position of the robot COM, the robot heading (yaw), and the
position of the swing foot. This leaves a 5 DOF task space, significantly reducing the robot’s
18 dimensional configuration space.
Figure 3-2: ZMP (red) vs COM (blue) for Case 2 with work-centering (bottom) and hybrid
control (top). The triangle depicts the support polygon. This figure illustrates that ZMP
more closely follows the COM trajectory in the hybrid control, which is the reason why
the hybrid control is less likely to fall when executing fast motions under static stability
assumptions. c 2007 IEEE [Shkolnik and Tedrake, 2007].
53

To achieve task control for LittleDog, I derived whole-body Jacobians for center of mass
and swing foot trajectory control of a point-foot quadruped robot, despite the fact that the
forward kinematics for center of body are ill-defined. Three control methods were explored,
including 1) a partial IK solution; and whole-body solutions using either 2) work-space
centering and 3) a hybrid controller which utilizes the partial IK solution for dynamic
redundancy resolution.
The controllers were tested on several test cases in simulation,
designed to force the robot to move dynamically or to the edge of kinematic feasibility. The
hybrid controller maximized the utility of the static stability criteria, while also following the
commanded trajectories with the least error on all four tests. The results demonstrate that
task space control is sensitive to how redundancies are resolved. With careful consideration of
the problem at hand, it is possible to achieve improved performance over standard approaches
such as work space centering, which in a planning framework such as discussed in Chapter
2, would result in improved completeness and faster search times because fewer expansion
attempts would result in failure.
Figure 3-3: Experiment Results Cases 1-3 shown (top to bottom). c 2007 IEEE [Shkolnik
and Tedrake, 2007].
54

 
 
1.46
1.48
1.5
1.52
1.5
0.44
0.46
0.48
0.5
0.52
0.54
0.56
RMS Error: 0.00711
1.46 1.48 1.5 1.52 1.54
0.44
0.46
0.48
0.5
0.52
0.54
0.56
RMS Error: 0.0147
1.46 1.48 1.5
1.52 1.54
0.46
0.48
0.5
0.52
0.54
0.56
RMS Error: 0.00311
Time 
Single Leg Jacobian 
Whole Body Jacobian, 
with 2
ndary
 work space 
centering  
Whole Body Jacobian with 
2
ndary
 Single Leg Jacobian 
and work space centering  
Figure 3-4: Simulation Results: Case 1. COM figure-8 Trajectory with four legs. Left:
Partial IK. Performance is reasonably good. Center: Whole-body Jacobian with Work
centering. The centering appears to interfere with this task. Note, it is possible to reduce
gains on the centering, but this results in worse performance in other tests. Right: Hybrid
approach. The trajectory following appears very good, and results in the lowest RMS Error.
Bottom: red line is the commanded trajectory (2 seconds), blue line is the actual trajectory.
c 2007 IEEE [Shkolnik and Tedrake, 2007].
55

1.44
1.46
1.48
0.48
0.49
0.5
0.51
0.52
0.53
0.54
0.55
RMS Error: 0.0179
1.4
1.42
1.44
1.46
1.48
1.5
0.48
0.5
0.52
0.54
RMS Error: 0.0282
1.44
1.46
1.48
0.48
0.49
0.5
0.51
0.52
0.53
0.54
0.55
RMS Error: 0.00213
Time 
Single Leg Jacobian 
Whole Body Jacobian, 
with 2
ndary
 work space 
centering  
Whole Body Jacobian with 
2
ndary
 Single Leg Jacobian 
and work space centering  
Figure 3-5: Simulation Results: Case 2. The red leg designates the swing foot in the
air. Left: Partial IK. The robot is unable to follow the specified trajectory at the speed
given.Center: Whole-body Jacobian with Work centering. The robot can not finish the
task as he rotates forward and eventually flips over. Right: Hybrid approach. This is the
only approach that is able to follow the trajectory even somewhat closely. Bottom: red
line is the commanded trajectory (3 seconds), blue line is the actual trajectory.
c 2007
IEEE [Shkolnik and Tedrake, 2007].
56

1.38
1.4
1.42
0.55
0.56
0.57
0.58
0.59
0.6
0.61
0.62
0.63
RMS Error: 0.00965
1.38
1.4
1.42
0.55
0.56
0.57
0.58
0.59
0.6
0.61
0.62
0.63
RMS Error: 0.00378
Fell over 
without
attaining 
trajectory
Time
Single Leg Jacobian 
Whole Body Jacobian, 
with 2
ndary
 work space 
centering
Whole Body Jacobian with 
2
ndary
 Single Leg Jacobian 
and work space centering  
Figure 3-6: Simulation Results: Case 3. The red leg designates the swing foot, which is
executing a figure-8 trajectory. Left: Partial IK. Performance is poor particularly because
joint limits are reached. Center: Whole-body Jacobian with Work centering. Rapid twisting
(jerking) motions cause the robot to loose footing and topple over; the trajectory at this
speed can not be achieved with this controller.
Right: Hybrid approach.
The robot
seems to minimize twisting and turning motions, and achieves reasonable trajectory following
performance. Bottom: red line is the commanded trajectory (1 second), blue line is the
actual trajectory. c 2007 IEEE [Shkolnik and Tedrake, 2007].
57

 
1.35 1.4 1.45
0
0.05
0.1
0.15
0.2
0.25
0.3
RMS Error: 0.014
Single Leg Jacobian 
Whole Body 
Jacobian, with 2
ndary
 
work space centering 
Whole Body Jacobian 
with 2
ndary
 Single Leg 
Jacobian and work space 
Time 
1.35 1.4 1.45
0
0.05
0.1
0.15
0.2
0.25
0.3
RMS Error: 0.0034
Desired 
(red dash) 
vs. actual 
(blue solid) 
trajectory of 
feet 
(projected 
into plane), 
and RMS 
error 
1.35 1.4 1.45
0
0.05
0.1
0.15
0.2
0.25
0.3
RMS Error: 0.0052
Figure 3-7: Simulation Results: Case 4. The red leg designates the swing foot, which is
executing a figure-8 trajectory. Left: Partial IK. Performance is poor particularly because
joint limits are reached. Center: Whole-body Jacobian with Work centering. Tracking
of the figure 8 is good, but COM tracking is affected here, and the robot goes though
twisting motions. Right: Hybrid approach. The robot seems to minimize twisting and
turning motions, and trajectory following has lowest RMS error. Bottom: red line is the
commanded trajectory (15 seconds), blue line is the actual trajectory. c 2007 IEEE [Shkolnik
and Tedrake, 2007].
58

Chapter
4
Task Space Control and Planning for
Underactuated Systems
The previous chapters discussed task space control in the context of reducing dimensionality
within a motion planning framework. As we saw in Chapter 2, task space control is well
defined for fully actuated systems. In the last chapter, we saw how a legged robot, which
is technically an underactuated system, can be treated as if it is fully actuated - but only
when executing conservative motion trajectories which are constrained so that the robot
always maintains a large support polygon on the ground. To achieve animal-like agility,
however, motion planning should not be restricted by such limiting constraints. Consider,
for example, trying to plan a dynamic climbing maneuver with LittleDog, in which two feet
are lifted from the ground at the same time (As shown in Figure 4-7). In this case, the robot
does not have a support polygon - since the feet form a line contact on the ground. The
interface between the robot and the ground is essentially a passive degree of freedom.
In this chapter, I derive task space control for systems with passive degrees of freedom by
using partial feedback linearization. This allows one to plan motions for underactuated
robots, which may have many degrees of freedom, directly in a low-dimensional task
space. The derivation presented contains a pseudo-Jacobian, which preserves the ability
to implement hierarchical control so that redundancy can be resolved with lower-priority
tasks.
The potential of this approach is demonstrated on the classical problem of swing-up of
a multi-link arm with passive joints. By choosing the COM of the multi-link arm as a task
space, a simple search based motion planner is able to find feasible swing up trajectories
by probing actions in the low-dimensional task space. A general idea of a motion planning
framework for high-dimensional underactuated systems is shown in Figure 4-1. It should be
evident that the TS-RRT described in Chapter 2 fits in the context of this framework.
Note that significant portions of this chapter also appear in [Shkolnik and Tedrake, 2008].
59

Figure 4-1: Schematic of an underactuated manipulator (top left) and the lower-dimensional
fully-actuated model (bottom left) that defines the task space of the problem.
Plans
generated in task space are mapped back to the active joints of the underactuated system,
and are verified to satisfy any constraints in the underactuated system.
(White circles
represent passive joints.) c 2008 IEEE [Shkolnik and Tedrake, 2008].
4.1
Task Space Control of Underactuated Systems
Without loss of generality, the equations of motion of an underactuated system can be broken
down in two components, one corresponding to unactuated joints, q
1
, and one corresponding
to actuated joints, q
2
:
M
11
¨
q
1
+ M
12
¨
q
2
+ h
1
+ φ
1
= 0
(4.1)
M
21
¨
q
1
+ M
22
¨
q
2
+ h
2
+ φ
2
= τ
(4.2)
with q ∈
n
, q
1
∈
m
, q
2
∈
l
, l = n − m. Consider an output function of the form,
y = f (q),
60

with y ∈
p
, which defines the task space. Define J
1
=
∂f
∂q
1
, J
2
=
∂f
∂q
2
, J = [J
1
, J
2
].
Theorem 1 (Task Space PFL). If the actuated joints are commanded so that
¨
q
2
= ¯
J
+
v − ˙J ˙q + J
1
M
−1
11
(h
1
+ φ
1
) ,
(4.3)
where ¯
J = J
2
− J
1
M
−1
11
M
12
. and ¯
J
+
is the right Moore-Penrose pseudo-inverse,
¯
J
+
= ¯
J
T
(¯
J¯
J
T
)
−1
,
then
¨
y = v.
(4.4)
subject to
rank ¯
J = p,
(4.5)
Proof. Differentiating the output function:
˙y = J ˙q
¨
y = ˙J ˙q + J
1
¨
q
1
+ J
2
¨
q
2
.
Solving 4.1 for the dynamics of the unactuated joints:
¨
q
1
= −M
−1
11
(M
12
¨
q
2
+ h
1
+ φ
1
)
(4.6)
Substituting, we have
¨
y = ˙J ˙q − J
1
M
−1
11
(M
12
¨
q
2
+ h
1
+ φ
1
) + J
2
¨
q
2
(4.7)
= ˙J ˙q + ¯
J¨
q
2
− J
1
M
−1
11
(h
1
+ φ
1
)
(4.8)
=v
(4.9)
Note that the last line required the rank condition (4.5) on ¯
J to ensure that the rows of ¯
J
are linearly independent, allowing ¯
J¯
J
+
= I.
In order to execute a task space trajectory one could command
v = ¨
y
d
+ K
d
( ˙y
d
− ˙y) + K
p
(y
d
− y).
Assuming the internal dynamics are stable, this yields converging error dynamics, (y
d
− y),
when K
p
, K
d
> 0 [Slotine and Li, 1990]. For a position control robot, the acceleration
command of (4.3) suffices. Alternatively, a torque command follows by substituting (4.3)
and (4.6) into (4.2).
61

4.1.1
Collocated and Non-Collocated PFL
The task space derivation above provides a convenient generalization of the partial feedback
linearization (PFL) [Spong, 1994a], which encompasses both the collocated and non-
collocated results. If one chooses y = q
2
(collocated), then
J
1
= 0, J
2
= 1, ˙J = 0, ¯
J = 1, ¯
J
+
= 1.
From this, the command in (4.3) reduces to ¨
q
2
= v. The torque command is then
τ = −M
21
M
−1
11
(M
12
v + h
1
+ φ
1
) + M
22
v + h
2
+ φ
2
,
and the rank condition (4.5) is always met.
If one chooses y = q
1
(non-collocated), then
J