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

Sample-Based Motion Planning in High-Dimensional
and Differentially-Constrained Systems
by
Alexander C. Shkolnik
Submitted to the Department of Electrical Engineering and Computer Science
in partial fulfillment of the requirements for the degree of
Doctor of Philosophy
at the
MASSACHUSETTS INSTITUTE OF TECHNOLOGY
February 2010
c Massachusetts Institute of Technology 2010. All rights reserved.
Author . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Department of Electrical Engineering and Computer Science
January 29, 2010
Certified by . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Russ Tedrake
Associate Professor
Thesis Supervisor
Accepted by . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Terry P. Orlando
Chair, Department Committee on Graduate Students

2

Sample-Based Motion Planning in High-Dimensional
and Differentially-Constrained Systems
by
Alexander C. Shkolnik
Submitted to the Department of Electrical Engineering and Computer Science
on January 29, 2010, in partial fulfillment of the
requirements for the degree of
Doctor of Philosophy
Abstract
State of the art sample-based path planning algorithms, such as the Rapidly-exploring
Random Tree (RRT), have proven to be effective in path planning for systems subject to
complex kinematic and geometric constraints. The performance of these algorithms, however,
degrade as the dimension of the system increases. Furthermore, sample-based planners rely
on distance metrics which do not work well when the system has differential constraints. Such
constraints are particularly challenging in systems with non-holonomic and underactuated
dynamics. This thesis develops two intelligent sampling strategies to help guide the search
process. To reduce sensitivity to dimension, sampling can be done in a low-dimensional task
space rather than in the high-dimensional state space. Altering the sampling strategy in
this way creates a Voronoi Bias in task space, which helps to guide the search, while the
RRT continues to verify trajectory feasibility in the full state space. Fast path planning is
demonstrated using this approach on a 1500-link manipulator. To enable task-space biasing
for underactuated systems, a hierarchical task space controller is developed by utilizing
partial feedback linearization. Another sampling strategy is also presented, where the local
reachability of the tree is approximated, and used to bias the search, for systems subject to
differential constraints. Reachability guidance is shown to improve search performance of the
RRT by an order of magnitude when planning on a pendulum and non-holonomic car. The
ideas of task-space biasing and reachability guidance are then combined for demonstration
of a motion planning algorithm implemented on LittleDog, a quadruped robot. The motion
planning algorithm successfully planned bounding trajectories over extremely rough terrain.
Thesis Supervisor:
Russ Tedrake, Associate Professor
Thesis Committee:
Tom´
as Lozano-P´
erez, Professor of Computer Science and Engineering, MIT
Nicholas Roy, Associate Professor of Aeronautics and Astronautics, MIT
Steven M. LaValle, Professor of Computer Science, University of Illinois
3

4

Acknowledgments
Many people have influenced my thinking over the years, and have directly or otherwise
contributed to the work developed in this thesis. First, I would like to express tremendous
gratitude to my advisor, Russ Tedrake. Russ’s passion for robotics is infectious, and I was
fortunate to be his first student. As Russ’s lab grew over the years, he made himself available
to his students as a priority. Russ provided just the right balance of a hands-on approach,
pushing me when needed, but otherwise giving me tremendous freedom.
I would also like to express my appreciation for the other members of my thesis committee,
including Nick Roy, who in addition to being on my committee was also a co-PI during the
first two phases of the LittleDog project, and Tom´
as-Lozano P´
erez and Steve LaValle, who
have been instrumental in the field of motion planning. The interaction with my committee
was insightful, and helpful in giving me a comprehensive view of the problems I was trying
to address.
The LittleDog project has been an enormous team effort, and it is worth acknowledging
the efforts of the team. I worked closely with Michael Levashov in Phase III of the project.
Michael developed and identified a physics-based model of LittleDog, which was used in
the motion planning algorithms developed in this thesis. As I write this, Michael and Ian
Manchester continue to work with the robot, to achieve feedback stabilization to get these
plans to execute on the robot. During phase II, Katie Byl, now an assistant professor at UC
Santa Barbara, perfected the open-loop half-bound maneuver, enabling the robot to quickly
get over gates, steps and gaps with the dog. At the same time, Sam Prentice implemented
an A* planner for LittleDog, which planned footholds and body poses for walking over
rough terrain. Khashayar Rohanimanesh was a postdoc in the lab, and we worked closely
on low level through high level control for LittleDog throughout Phase I of the project.
Other team members, including Patrycja Missiuro, Emma Brunskill, and Stephen Proulx
also contributed in the first phases. Finally, I should mention the insightful interaction
with the other LittleDog team PI’s and students, as well as Boston Dynamics (developer of
the robot) and DARPA (Who funded the project). Additionally, the work was made much
smoother by TIG, the infrastructure group at CSAIL.
In addition to LittleDog, I had the opportunity to work with and mentor several
undergraduate student researchers (UROPS), all of whom were insightful and diligent. I
worked with Elena Glassman on control for a self-righting maneuver of a falling cat robot,
which was eventually built in hardware by Steve Proulx and then by Fumiya Iida, a postdoc
in our lab. Elena is continuing her studies in the lab as a PhD student, and is studying
dynamic distance metrics for sample-based kinodynamic planning in state space. Michael
Price was another talented UROP who did some early coding on LittleDog. Lauren White
developed clean code in SD/FAST and C to generate physics based models of multi-link
arms. The models were made compatible with Matlab, and were used in the motion planning
algorithms in this thesis. Her code became the basis for our planar 5-link LittleDog model
which Michael Levashov further developed. Finally, Sara Itani worked on LittleDog in Phase
III, and helped tune an openloop bounding gait on the robot on flat terrain.
The resources of MIT are vast, and I particularly enjoyed interacting with the students
5

and postdocs that study robotics, both in research and in social environments. Tom Kollar
and I had semi-regular research meetings, and Tom helped me define the N-Link arm as a
task space toy problem. Matt Walter was instrumental in writing the RG-RRT ICRA paper,
which is a chapter in this thesis. There were many others, but I especially want to mention
some of the folks that I had the most opportunity to interact with, including Rick Cory,
John Roberts, Albert Huang, Olivier Koch, Alex Bahr, and the other members of 32-33x
and Russ’s lab.
The research in this thesis was made possible by funding that came from DARPA
(Learning Locomotion Program) and the NSF (through a Graduate Research Fellowship).
I’d like to acknowledge several people who mentored me, and shaped my research career.
This includes Shuuji Kajita who I was able to work with through a Summer Research
Program funded by the NSF and JSPS; Tommy Poggio and Gabriel Krieman who were
my first research advisors at MIT; Steve Potter and Thomas DeMarse, who supervised
my masters thesis at the Georgia Tech NeuroEngineering Lab; David Edwards and Pat
Marsteller, who introduced my to research at Emory; and Chet Bacon, a (now retired)
electronics teacher who sparked my interest in computer science.
Outside of my research at MIT, I’ve been involved with LiquidPiston, a startup company
which my father (Nick) and I founded in 2003 to develop a new type of combustion engine.
I was fortunate to be able to work so closely with my father on this project. Of course, I
have to thank Russ for allowing me the freedom to pursue this project, as well as all of the
people I’ve had a chance to work with through LiquidPiston.
I’d also like to thank my mother, Valentina, and the rest of my friends and family – I
could not have asked for a better support network.
Finally, I am forever indebted to my wife, Lauren, who has stood by me and encouraged
me through these years. Lauren has been a beacon of stability and understanding in my life.
6

Contents
1
Introduction
13
1.1
Motivation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
14
1.2
LittleDog
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
16
1.3
Review of the State of the Art . . . . . . . . . . . . . . . . . . . . . . . . . .
17
1.3.1
Sample-Based Planning . . . . . . . . . . . . . . . . . . . . . . . . .
17
1.3.2
Task Space Control . . . . . . . . . . . . . . . . . . . . . . . . . . . .
21
1.3.3
Underactuated Systems . . . . . . . . . . . . . . . . . . . . . . . . . .
23
1.3.4
Task space control of underactuated systems . . . . . . . . . . . . . .
24
1.3.5
Templates and Anchors . . . . . . . . . . . . . . . . . . . . . . . . . .
25
1.3.6
Legged Locomotion . . . . . . . . . . . . . . . . . . . . . . . . . . . .
25
1.4
Contributions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
27
1.5
Outline . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
28
2
Path Planning with a Bias in Task Space
31
2.1
Task Space in the Context of Path Planning . . . . . . . . . . . . . . . . . .
31
2.2
TS-RRT: Planning in Task Space with the RRT . . . . . . . . . . . . . . . .
34
2.2.1
TS-RRT Definitions
. . . . . . . . . . . . . . . . . . . . . . . . . . .
34
2.2.2
TS-RRT . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
35
2.3
Scalability of TS-RRT compared to RRT . . . . . . . . . . . . . . . . . . . .
36
2.3.1
Path Planning for the N-Link Arm . . . . . . . . . . . . . . . . . . .
36
2.3.2
Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
40
2.4
Extensions and Possible Modifications
. . . . . . . . . . . . . . . . . . . . .
40
2.4.1
Dynamic and Underactuated Systems . . . . . . . . . . . . . . . . . .
41
2.4.2
On Completeness . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
42
2.4.3
Hybrid Approach . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
43
2.5
Concluding Remarks . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
44
3
Task Space Control for a Quadruped Robot
45
3.1
Motivation for COM Control . . . . . . . . . . . . . . . . . . . . . . . . . . .
46
3.2
Single-Leg Inverse Kinematics Control
. . . . . . . . . . . . . . . . . . . . .
47
7

3.3
Whole-Body Jacobian Control . . . . . . . . . . . . . . . . . . . . . . . . . .
48
3.3.1
Jacobian of Center of Body
. . . . . . . . . . . . . . . . . . . . . . .
48
3.3.2
Jacobian of Body Rotation . . . . . . . . . . . . . . . . . . . . . . . .
50
3.3.3
Center of Mass Jacobian . . . . . . . . . . . . . . . . . . . . . . . . .
50
3.3.4
Swing Foot Jacobian . . . . . . . . . . . . . . . . . . . . . . . . . . .
50
3.3.5
Hierarchical Controller . . . . . . . . . . . . . . . . . . . . . . . . . .
51
3.4
Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
51
3.4.1
Simulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
51
3.4.2
Real Robot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
52
3.5
Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
53
4
Task Space Control and Planning for Underactuated Systems
59
4.1
Task Space Control of Underactuated Systems . . . . . . . . . . . . . . . . .
60
4.1.1
Collocated and Non-Collocated PFL
. . . . . . . . . . . . . . . . . .
62
4.1.2
Redundancy Resolution
. . . . . . . . . . . . . . . . . . . . . . . . .
62
4.1.3
Example: Feedback Control of the Acrobot . . . . . . . . . . . . . . .
63
4.2
Planning . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
64
4.3
Generalized Swing Up Control for N-Link Pendulum with Passive Joints
. .
65
4.3.1
Simulations of a 5-link pendulum . . . . . . . . . . . . . . . . . . . .
65
4.3.2
Computational Efficiency . . . . . . . . . . . . . . . . . . . . . . . . .
68
4.3.3
Task space guided RRT
. . . . . . . . . . . . . . . . . . . . . . . . .
69
4.4
Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
70
4.4.1
Choice of template . . . . . . . . . . . . . . . . . . . . . . . . . . . .
70
4.4.2
Applications to locomotion . . . . . . . . . . . . . . . . . . . . . . . .
70
4.4.3
Concluding Remarks . . . . . . . . . . . . . . . . . . . . . . . . . . .
72
5
Reachability-Guided Sampling for
Planning Under Differential Constraints
73
5.1
RRT Planning Under Differential Constraints
. . . . . . . . . . . . . . . . .
74
5.1.1
Basic RRT Operation . . . . . . . . . . . . . . . . . . . . . . . . . . .
74
5.1.2
Performance with Kinodynamic Constraints . . . . . . . . . . . . . .
74
5.1.3
RRT Modifications . . . . . . . . . . . . . . . . . . . . . . . . . . . .
76
5.2
Reachability-Guided RRT . . . . . . . . . . . . . . . . . . . . . . . . . . . .
77
5.2.1
Problem Formulation . . . . . . . . . . . . . . . . . . . . . . . . . . .
78
5.2.2
The RG-RRT Algorithm . . . . . . . . . . . . . . . . . . . . . . . . .
78
5.3
Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
84
5.3.1
Swing Up Control for an Underactuated Pendulum . . . . . . . . . .
85
5.3.2
Motion Planning for the Acrobot . . . . . . . . . . . . . . . . . . . .
85
5.3.3
Motion Planning for a Simple Nonholonomic Car . . . . . . . . . . .
86
5.4
Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
87
5.4.1
On Completeness . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
87
5.4.2
Approximating the Reachable Set . . . . . . . . . . . . . . . . . . . .
89
5.4.3
Regions of Inevitable Collision . . . . . . . . . . . . . . . . . . . . . .
90
8

5.4.4
Hybrid Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
90
5.4.5
Motion Primitives . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
90
5.4.6
Concluding Remarks . . . . . . . . . . . . . . . . . . . . . . . . . . .
91
6
Application: Motion Planning to Achieve Bounding Over Rough Terrain
with LittleDog
93
6.1
Bounding on Rough Terrain . . . . . . . . . . . . . . . . . . . . . . . . . . .
93
6.2
LittleDog Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
94
6.3
Motion Planning Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . .
97
6.3.1
Problem Formulation . . . . . . . . . . . . . . . . . . . . . . . . . . .
97
6.3.2
Macro-Actions / Motion-Primitive
. . . . . . . . . . . . . . . . . . .
98
6.3.3
Approximating the Reachable Set . . . . . . . . . . . . . . . . . . . . 100
6.3.4
Sampling in Task Space
. . . . . . . . . . . . . . . . . . . . . . . . . 102
6.3.5
Simulation Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . 102
6.3.6
Experimental Results . . . . . . . . . . . . . . . . . . . . . . . . . . . 106
6.4
Concluding Remarks . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 106
7
Conclusions and Future Directions
109
7.1
Summary
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 109
7.2
Future Directions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 111
7.2.1
Further Analysis of Task Space Mappings
. . . . . . . . . . . . . . . 111
7.2.2
Analysis of Reachability . . . . . . . . . . . . . . . . . . . . . . . . . 112
7.2.3
RG-RRT for kinematic planning . . . . . . . . . . . . . . . . . . . . . 112
7.2.4
Application and Stabilization . . . . . . . . . . . . . . . . . . . . . . 113
7.2.5
General extensions . . . . . . . . . . . . . . . . . . . . . . . . . . . . 113
7.2.6
Concluding Remarks . . . . . . . . . . . . . . . . . . . . . . . . . . . 113
Bibliography
115
9

10

List of Figures
1-1
Person and dog cross river by hopping stones. . . . . . . . . . . . . . . . . .
14
1-2
State of the art: Asimo running; Big dog climbing.
. . . . . . . . . . . . . .
15
1-3
LittleDog robot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
16
1-4
Steps of the RG-RRT algorithm. . . . . . . . . . . . . . . . . . . . . . . . . .
20
2-1
RRT Planning within a 2D plane that slices through a 3D C-space . . . . . .
32
2-2
Projection of RRT into End Effector Cartesian Coordinates for a 5-link arm
37
2-3
Nodes in RRT tree vs number of links in planar arm
. . . . . . . . . . . . .
38
2-4
RRT Comparison Example with Voronoi Bias in C-space vs T-Space . . . . .
39
2-5
Example Trajectories Found with RRT for 1500 Link Arm . . . . . . . . . .
41
2-6
Example RRT Solution Using Hybrid Voronoi Bias, used in Presence of
Complicated Obstacles . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
43
3-1
LittleDog Robot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
46
3-2
ZMP vs COM projection comparison using different redundancy resolutions .
53
3-3
Task space feedback control results on real robot . . . . . . . . . . . . . . . .
54
3-4
Redundancy resolution comparison when moving COM with 4 stance legs . .
55
3-5
Redundancy resolution comparison when moving COM with 3 stance legs . .
56
3-6
Redundancy resolution comparison when moving swing leg in small fast motion 57
3-7
Redundancy resolution comparison when moving swing leg in large slow motion 58
4-1
Planning Framework for underactuated systems using task space control
. .
60
4-2
Acrobot Task Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
63
4-3
A low-dimensional template for the underactuated swing-up task. . . . . . .
65
4-4
COM Trajectories . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
66
4-5
Swingup trajectory of PAAAA and PAPAA
. . . . . . . . . . . . . . . . . .
68
4-6
RRT using task space search on PAAAA . . . . . . . . . . . . . . . . . . . .
69
4-7
LittleDog: Template and Anchor
. . . . . . . . . . . . . . . . . . . . . . . .
70
4-8
COM Stabilization using PFL . . . . . . . . . . . . . . . . . . . . . . . . . .
71
5-1
Example state space trajectory for pendulum . . . . . . . . . . . . . . . . . .
75
5-2
The reachable set, R(x), for the underactuated pendulum . . . . . . . . . . .
77
11

5-3
Steps of the RG-RRT algorithm demonstrated on a pendulum. . . . . . . . .
79
5-4
RG-RRT Voronoi diagrams for a pendulum. . . . . . . . . . . . . . . . . . .
82
5-5
Comparison of RG-RRT and RRT on pendulum
. . . . . . . . . . . . . . .
83
5-6
Example of standard RRT on pendulum system . . . . . . . . . . . . . . . .
84
5-7
Standard RRT vs RG-RRT for dynamic car problem
. . . . . . . . . . . . .
86
5-8
Comparison of Voronoi regions when approximating the Reachable Set
. . .
88
6-1
LittleDog Robot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
94
6-2
Dog bounding up stairs . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
95
6-3
LittleDog Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
96
6-4
Virtual obstacle function for LittleDog Bounding
. . . . . . . . . . . . . . .
97
6-5
Trajectory showing first half of a double bound
. . . . . . . . . . . . . . . .
99
6-6
Trajectory showing second half of a double bound . . . . . . . . . . . . . . .
99
6-7
Maneuver Automaton for LittleDog Bounding . . . . . . . . . . . . . . . . . 101
6-8
Bounding Trajectory over logs . . . . . . . . . . . . . . . . . . . . . . . . . . 103
6-9
Bounding Trajectory over logs, continued . . . . . . . . . . . . . . . . . . . . 104
6-10 Bounding over logs with LittleDog
. . . . . . . . . . . . . . . . . . . . . . . 105
6-11 LittleDog Model compared to Actual Trajectories . . . . . . . . . . . . . . . 107
6-12 The Sensing and Control Environment for LittleDog . . . . . . . . . . . . . . 108
12