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

Concluding Remarks
This chapter described a motion planner for a simulation of the LittleDog quadruped robot
to achieve bounding on rough terrain. The robot has a 40 dimensional state space, which
was reduced to a 16 dimensional state space in a planar model. A physics based model was
developed and identified using data from the real robot. An efficient RRT based planner
was implemented. This planner used motion primitives to reduce the size of the action
space. Corresponding to the idea of task-space guided RRT search, sampling was done in
a 5-dimensional subset of the state space. The reduction in sampling dimension ignores
pitch, vertical position, and velocities which significantly improves search efficiency. To
handle challenging dynamic constraints associated with bounding, Reachability Guidance
was used, so that random samples were chosen such that they were closer to reachable
regions of the tree than the tree itself. This ensures that the expansion step of the RRT
algorithm can make progress in the direction of the sample, which vastly improves RRT
performance. The RRT motion planner was demonstrated to achieve bounding over steps
and over logs, with terrain height differences corresponding to approximately 50% of the
leg length.
Without Reachability Guidance, 12 hours was not sufficient to plan in the
otherwise identical conditions. Using task-space biasing and Reachability Guidance, the
106

planner returned feasible trajectories within minutes. A primary reason for this is that a
standard RRT implementation does not look ahead. Many of the nodes on the outer edges
of the tree – the exact nodes which the Voronoi Bias selects most often, because they are
near the unexplored regions of state space – correspond to nodes that are either incapable
of bounding on the next step because not enough energy is carried over, or have too much
energy and no possible action could be applied on the next step to keep the robot from
falling over. Expanding on these nodes is futile, but the standard RRT will try to expand
(a) x position
(b) y position
(c) Body pitch
(d) Back hip joint position
Figure 6-11: LittleDog Model compared to Actual Trajectories for a bounding motion on flat
terrain: Unactuated coordinates: x (top left), y (top right), and body pitch (bottom left);
trajectory of one of the joints (bottom right). The thick red line shows a trajectory generated
by planning with RRT’s using the simulation model. The blue lines are 10 open-loop runs
of the same trajectory on a real LittleDog robot. It is clear that the model captures the
actuated dynamics very well. The unactuated dynamics are highly nonlinear, but the model
captures much of the system behavior. The response of the robot itself is unstable, even
from very similar starting conditions. Figure by Michael Levashov.
107

them repeatedly causing the algorithm to fail to find a plan.
The motion primitives used for the motion planner were tested on the robot, and shown
to be capable of bounding on rough terrain, including the logs terrain board that we were
able to plan on. The trajectories implemented on the robot using the motion primitives
are smooth, and our model captures the resulting dynamics quite well for short trajectories.
However, the forward simulation and the actual behavior of the robot tended to diverge after
about 1 second, despite having a relatively elaborate physics model of the robot that was
identified on data collected from the robot. This is not entirely surprising, since open-loop
bounding trajectories executed on the robot tend to diverge in a similar time frame.
Current work by the MIT LittleDog team is focused on constructing a feedback controller
to stabilize the bounding motion plans. As an alternative approach, along the lines of LQR-
Trees [Tedrake, 2009a], the motion plan returned may be combined with feedback control,
for example based on TV-LQR, which would have some measurable basin of attraction. The
plan could then be optimized by maximizing the overall size of the basin of attraction, or by
maximizing the narrowest part of the basin, while maintaining the goal position.
We expect this motion planner can be generalized, and potentially applied to a variety of
other systems. As future work, we will try similar planning approaches on a dynamic biped
walking over rough terrain, and on a forklift operating in a highly constrained environment.
LittleDog Robot
Joint Positions
Delay: 1 ΔT
Position Commands
Motion Capture
Joint Encoders
Control
Computer
PD Controller
Joint
Motors
IMU - Rate Gyros
Delay: 2 ΔT
Delay: 2 ΔT
Body Angle Rates
Body Absolute
Position
1 ΔT = 10ms
Figure 6-12: The Sensing and Control Environment for LittleDog. Figure by Ian Manchester.
108

Chapter
7
Conclusions and Future Directions
7.1
Summary
This thesis has drawn on useful ideas from several bodies of research. First, the development
of sample-based planning algorithms, such as the RRT, has significantly advanced the state
of the art, enabling fast path planning in the presence of challenging geometric constraints
in configuration space. Sample-based planning is sensitive to dimension of the action space.
However, for many problems of interest, a system’s state can be projected into a lower
dimensional task space, while preserving the intentions of the planner. This idea has been
exploited in task space feedback controllers, which translate commands in task space into
commands in the system’s action space. I showed that sample-based planning can be made
much more efficient, at least for certain types of problems, if a Voronoi bias is implemented
in a low dimensional task space rather than in configuration space. For the RRT, this is
accomplished by sampling directly within task space, and selecting for expansion the node
in the tree, the projection of which is closest to the sample in task space. To maintain
completeness while preserving the task space Voronoi bias, the algorithm can occasionally
sample in configuration space. The resulting planning algorithm quickly found plans for
a 1500-link arm given the task of moving the end-effector from a start position to a goal
position while avoiding obstacles collisions. When using task-space guidance on this task,
the search efficiency, defined by the number of expansion attempts required by the planner
to find a path to the goal, was relatively invariant to the number of links in the arm.
A task space Voronoi bias can be implemented in many ways. The simplest method is to
apply a random control action for each expansion step in the RRT, while sampling in task
space to choose the node to expand upon. Alternatively, a task space feedback controller may
be used to directly guide the expansion of a node towards a sample in task space. The Moore-
Penrose pseudoinverse was used for the controller in the 1500-link path planning problem.
Despite the dimension, the Jacobian pseudoinverse and matrix multiplications were quick to
compute using this approach. Task space control was then explored in application to the
LittleDog robot. If at least three feet are in contact with the ground and do not slip or leave
the ground, then the robot can move in all directions in task space, as if it is fully actuated.
109

Derivations for robot center-of-mass and swing-foot task control were presented and used
within a hierarchical task controller. The findings showed that redundancy resolution is
important when considering the implementation of a task space controller. The choice of
redundancy resolution directly affects what task-space actions can be feasibly implemented
by the robot. This has direct implications if trying to use task space guidance for motion
planning.
Task space control is usually used in fully actuated systems, where a Jacobian can be
defined for the task.
For underactuated systems, partial feedback linearization can be
used to control a subset of the degrees of freedom. By combining these two approaches,
a controller was derived that takes advantage of the system’ equations of motion to produce
a mapping from accelerations in task space, into torques and forces of the controlled degrees
of freedom. The derivation allows for hierarchical control including redundancy resolution.
The controller was used to demonstrate motion planning to achieve swing-up on a 5-link arm
with passive joints. The planner probed actions in task space, which significantly reduced
the computational complexity of the search. A pendulum with prism joint turned out to
be a good template for this type of problem. This corresponds to controlling the center of
mass of the 5-link arm, represented in Polar form. In the standard Template-and-Anchor
framework, a template is carefully chosen which encompasses the interesting underactuated
dynamics of the system, and the motion planning is done on this simplified system. Instead
of this, by using the underactuated task space controller, a template is treated as if it is
fully-actuated. Without kinodynamic constraints such as torque limits and collisions, the
task space controller would exactly produce the instantaneous commanded motions in task
space, assuming that a rank condition is met, which represents strong inertial coupling.
When kinematic and additional dynamic constraints of the anchor are incorporated, the
fully-actuated template is amenable to search based planning algorithms.
A primary motivation in this thesis was to control the dynamic behavior of the LittleDog
robot, to traverse rough terrain with agile, dynamic motions. The robot has very light limbs
compared to the body, making the inertial coupling of the system weak. Thus, alternative
strategies were considered for motion planning on this system. Reachability guidance was
developed as a framework to achieve fast sample-based motion planning given arbitrary
differential constraints. The RG-RRT addresses the general problem that the Euclidean
distance metric is a very poor approximation of the cost-to-go, which the RRT relies to
figure out which nodes in a tree can be most effectively expanded in state space towards a
given sample. For an RRT that uses a Euclidean distance metric, the result is that even
for low-dimensional problems such as the pendulum, the Voronoi Bias results in repeated
expansion attempts of outer nodes in the tree, yet these nodes are often unable to expand
outward because of the differential constraints. Reachability-guidance, on the other hand,
takes into account regions of state space that are locally (quickly) reachable from the set
of already explored nodes of the tree. The Euclidean distance to these reachable regions is
then used as a heuristic to determine whether the tree can actually expand towards a given
sample. The RG-RRT uses this heuristic while doing rejection sampling in state space. This
is accomplished by throwing away samples that are closer to the tree itself than they are
110

to the reachable regions of the tree. The RG-RRT algorithm greatly accelerates motion
planning for dynamic systems, compared to a standard RRT. The algorithm demonstrated
an order of magnitude reduction in search speed on the pendulum and nonholonomic car
planning problems.
The reachable regions for continuous systems can be difficult to compute. The hull of
the reachable set is most important to determine, and this can sometimes be approximated
by discretizing the action space. However, doing so is exponential in control dimension, and
such an approach would not scale to a system like LittleDog. To address this, reachability
guidance can be combined with motion primitives, and task-space biasing. The RG-RRT
turns out to work very naturally with motion primitives, because the reachable set can
be computed by integrating through the primitive, and Euclidean distance can adequately
determine which directions in state space a tree is able grow in. Notably, this holds true even
if the motion primitive produces a large jump in state space. A simple motion primitive for
LittleDog bounding, corresponding to opening or closing the posture of the robot, was useful
for producing smooth bounding behavior. Integrating through this type of motion primitive
will either fail because of a collision, or because the robot has too much or too little energy
going into the action. Otherwise, if the primitive does not fail, the resulting state will be
far away in state space from the corresponding tree node which is being expanded. Because
of this, a very coarse discretization of actions in this motion primitive is sufficient to tell
the planner what areas of state space, if any, a given tree node is able to grow towards.
The resulting motion planner quickly planned feasible bounding trajectories over very rough
terrain, despite the 40-dimensional robot state space.
7.2
Future Directions
Several potential future directions are described in the following sections.
7.2.1
Further Analysis of Task Space Mappings
In this thesis I presented several examples of task space mappings. For kinematic path
planning problems, the workspace coordinate of a point on the robot is often used as a task
space. The end-effector position of an N-link arm is one example of this. The workspace
contains information about kinematic obstacles, so a collision in workspace is also a collision
in configuration space. This fact can be exploited by planners, and some of the recent related
work in path planning has taken advantage of this [Zucker et al., 2008, Diankov et al., 2008].
Work space information is less useful in dynamic applications, for example a task space
such as the system center of mass. Many related works in underactuated motion planning
including work on Templates and Anchors [Full and Koditschek, 1999], and humanoid robot
control (e.g. [Kajita and Tani, 1991]) have used similar task space mappings. These works
attempt to characterize the dominating underactuated dynamics of a system by using a low
dimensional underactuated template such as a pendulum, or pendulum and spring. The
approach in this thesis was different by assuming a fully-actuated task space, and using
111

efficient search methods to explore feasible commands in this low-dimensional space. A
task space that encodes much of the useful behavior in state space is a crucial underlying
assumption in this thesis, and this depends on the choice of task space mapping.
To compare mappings, one might characterize the span of feasible actions in state space
that can be generated with a given task space. A “better” mapping will be one which has
more feasible options that it can present to the planner. Carefully analyzing differences
between task spaces, and methods to select an optimal task space is left to future work.
7.2.2
Analysis of Reachability
Reachability guidance offers a conceptually simple method for guiding search on dynamic
systems by altering the distance metric to include information about local reachability. How
to compute local reachability, and how to compute a distance to this reachable region, can be
complicated. In this thesis I focused on approximations of the reachable set. An improved
understanding of local reachability might be achieved by analyzing the actual (non-heuristic)
reachable sets, for special classes of systems that are amenable to such analysis, or perhaps
for very simple dynamic systems, for which the reachable set can be analytically computed.
One could then try to characterize the effect of approximating the reachable set, including
any resulting bounds on performance of the planner.
Another line of research would be a systematic method for generating the reachable
set using approximations of the dynamics which might be easier to work with. The work
in [Hofmann and Williams, 2006], is relevant as it abstracts the full dynamics to multiple
SISO systems, and solves Linear Programming problems to approximate sets of feasible
trajectories, called flow-tubes, which satisfy given constraints imposed by the planner. Other
work in progress done by Russ Tedrake, and fellow lab-mates, may also blend well with the
RG-RRT ideas. Russ has been developing LQR-Trees [Tedrake, 2009a], which combines RRT
type search with LQR feedback control. Elena Glassman has been working on an LQR based
heuristic to estimate the cost-to-go as a distance metric [Glassman and Tedrake, 2010]. These
approaches use linearized dynamics to approximate dynamic distance between samples and
the tree. The linearization is locally valid, which might be exploited when computing local
reachability. These types of ideas may be useful as a replacement for the Euclidean distance
metric in the RG-RRT, or could be used to approximate the reachable set. This might be
done by finding the surface defined by a level set of the LQR-based distance metric. If the
cost function is the time-to-go, then the level-set over state space that has a cost equal to
the RRT time step, given a fixed start state, would produce an estimate of the reachable set.
7.2.3
RG-RRT for kinematic planning
Another interesting extension of reachability guidance is in application to kinematic path
planning (in configuration space, instead of state space). The locally reachable set, without
differential constraints, would be a hypersphere centered around the node in the tree.
The resulting distance metric would then be Euclidean distance, so the algorithm would
behave much like a standard RRT. A sphere can quickly be tested for collisions with
112

neighboring points.
If the sphere is known to be in collision somewhere, the reachable
set for the given node can be approximated by discretizing the sphere and checking the
resulting set for collisions. The result is similar to the dynamic domain RRT [Yershova
et al., 2005, Jaillet et al., 2005], but changes the sampling region more specifically based on
geometric information around the tree.
7.2.4
Application and Stabilization
The algorithms described in this thesis were applied in simulation to robot manipulator
path planning, and motion planning problems including the underactuated (torque-limited)
pendulum, the nonholonomic car, and to a planar model of the LittleDog robot.
The
algorithms might be applied to a broad range of other systems. For example, humanoid
walking using toe-roll is mathematically very similar to the LittleDog robot when it stands
on its hind or front legs, and the algorithms in this thesis may be useful for planning agile
humanoid walking on rough terrain.
Of course, to execute a plan in hardware, some form of feedback stabilization is typically
be required. There are many techniques that can do this, and are beyond the scope of this
thesis, but the planning algorithm might need modification to work optimally with a given
feedback strategy.
7.2.5
General extensions
The algorithms in this thesis may be generalized to other interesting motion planning
problems. This may include, for example:
1. Time-varying problems, involving moving obstacle regions
2. Planning assuming multi-robot coordination
3. Optimal (or quasi-optimal) motion planning that minimizes a cost function through a
bias toward low cost plans during exploration
4. Planning for systems with uncertainty. Consider, for example planning in belief space,
which can be thought of as a very high dimensional and underactuated problem. Both
the TS-RRT and the RG-RRT may be directly applied in this space.
7.2.6
Concluding Remarks
After decades of research, robot technology is beginning its exodus from the factory floor.
Production cars are able to park themselves, and some cars brake in response to dangerous
situations, or least alert drivers of a potential driving problem. Under the DARPA Learning
Locomotion program, several university teams produced excellent results demonstrating
walking over rough terrain using the LittleDog robot.
Larger robots such as BigDog
and a variety of Japanese humanoid robots are consistently improving with time. Motion
113

planning has been a limiting factor for many applications in robotics. To be effective and
computational tractable, a planning algorithm must favor heuristics that quickly solve the
planning problem, most of the time. Sample-based planners are good at handling geometric
complexities. The performance of these planners can be improved by sampling intelligently.
By incorporating ideas from task space control, and by using the dynamics of the system
to bias the sampling, motion planners are able to solve high-dimensional problems with
challenging kinodynamic constraints.
114

Bibliography
[Altendorfer et al., 2001] Altendorfer, R., Moore, N., Komsuoglu, H., Buehler, M., Jr., H. B.,
McMordie, D., Saranli, U., Full, R., and Koditschek, D. (2001). RHex: A biologically
inspired hexapod runner. Autonomous Robots, 11(3):207–213.
[Altendorfer et al., 2000] Altendorfer, R., Saranli, U., Komsoglu, H., Koditschek, D., Brown,
H. B., Buehler, M., Moore, N., McMordie, D., and Full, R. (2000).
Evidence for
spring loaded inverted pendulum running in a hexapod robot. In Proceedings of the 7th
International Symposium on Experimental Robotics (ISER).
[Arai et al., 1998] Arai, H., Tanie, K., Shiroma, and N. (1998). Time-scaling control of an
underactuated manipulator. Robotics and Automation, 1998. Proceedings. 1998 IEEE
International Conference on, 3:2619–2626 vol.3.
[Arai et al., 1993] Arai, H., Tanie, K., Tachi, and S. (1993).
Dynamic control of a
manipulator with passive joints in operational space. Robotics and Automation, IEEE
Transactions on, 9(1):85–93.
[Arai and Tachi, 1991] Arai, H. and Tachi, S. (1991). Position control of manipulator with
passive joints using dynamic coupling. IEEE transactions on Robotics and Automation,
7(4).
[Bellman, 1957] Bellman, R. (1957). Dynamic Programming. Princeton University Press.
[Bergerman, 1996] Bergerman, M. (1996).
Dynamics and Control of Underactuated
Manipulators. PhD thesis, Carnegie Mellon University.
[Berkemeier et al., 1999] Berkemeier, M.D., Fearing, and R.S. (1999). Tracking fast inverted
trajectories of the underactuated acrobot. Robotics and Automation, IEEE Transactions
on, 15(4):740–750.
[Bertram et al., 2006] Bertram, D., Kuffner, J., Dillmann, R., Asfour, and T. (2006). An