Robot Path Planning: An Object-Oriented Approach

Robot Path Planning:
An Object-Oriented Approach
Morten Strandberg
TRITA–S3–REG–0401
ISSN 1404–2150
ISBN 91-7283-868-X
Automatic Control
Department of Signals, Sensors and Systems
Royal Institute of Technology (KTH)
Stockholm, Sweden, 2004
Submitted to the School of Electrical Engineering, Royal Institute of
Technology, in partial fulfillment of the requirements for the degree of
Doctor of Philosophy.

Copyright c 2004 by Morten Strandberg
Robot Path Planning: An Object-Oriented Approach
Automatic Control
Department of Signals, Sensors and Systems
Royal Institute of Technology (KTH)
SE-100 44 Stockholm, Sweden
Tel. [phone]
Fax. [phone]
http://www.s3.kth.se

iii
Abstract
Path planning has important applications in many areas, for exam-
ple industrial robotics, autonomous systems, virtual prototyping, and
computer-aided drug design. This thesis presents a new framework for
developing and evaluating path planning algorithms. The framework is
named CoPP (Components for Path Planning). It consists of loosely cou-
pled and reusable components that are useful for building path planning
applications. The framework is especially designed to make it easy to do
fair comparisons between different path planning algorithms.
CoPP is also designed to allow almost any user-defined moving sys-
tem. The default type of moving system is a robot class, which is capable
of describing tree-like kinematic chains. Additional features of this robot
class are: joint couplings, numerical or closed-form inverse kinematics,
and hierarchical robot representations. The last feature is useful when
planning for complex systems like a mobile platform equipped with an
arm and a hand.
During the last six years, Rapidly-exploring Random Trees (RRTs)
have become a popular framework for developing randomized path plan-
ning algorithms. This thesis presents a method for augmenting bidirec-
tional RRT-planners with local trees. For problems where the solution
trajectory has to pass through several narrow passages, local trees help
to reduce the required planning time.
To reduce the work needed for programming of industrial robots, it
is desirable to allow task specifications at a very high level, leaving it up
to the robot system to figure out what to do. Here we present a fast and
flexible pick-and-place planner. Given an object that has to be moved to
another position, the planner chooses a suitable grasp of the object and
finds motions that bring the object to the desired position. The planner
can also handle constraints on, e.g., the orientation of the manipulated
object.
For planning of pick-and-place tasks it is necessary to choose a grasp
suitable to the task. Unless the grasp is given, some sort of grasp planning
has to be performed. This thesis presents a fast grasp planner for a three-
fingered robot hand. The grasp planner could be used in an industrial
setting, where a robot is to pick up irregularly shaped objects from a
conveyor belt. In conjunction with grasp planning, a new method for
evaluating grasp stability is presented.

v
Acknowledgments
First I would like to thank my advisors, Professor Bo Wahlberg and Pro-
fessor Henrik Christensen, for giving me the opportunity to be a graduate
student at the Centre for Autonomous Systems. As my toughest critics,
they have also been of invaluable help during the writing of this thesis.
I am especially indebted to Frank Lingelbach for many fruitful dis-
cussions on path planning, for proof reading, and for being a brave CoPP
test pilot.
I am thankful to the following persons for suggestions and correc-
tions to the manuscript: Fredrik Almqvist, Fredrik Niemel¨
a, and Paul
Sundvall.
Finally, I thank my wife Firozeh and our children Nadja, Jonatan and
Peter for their love and support during these years; I promise that you
will see more of me from now on!
This research has been sponsored by the Swedish Foundation for
Strategic Research through the Centre for Autonomous Systems at KTH.
The support is gratefully acknowledged.

Contents
1
Introduction
1
1.1
Path Planning Applications . . . . . . . . . . . . . . . . .
2
1.2
Notation and Terminology . . . . . . . . . . . . . . . . . .
4
1.3
Probabilistic Roadmap Methods . . . . . . . . . . . . . .
6
1.4
Outline and Contributions of the Thesis . . . . . . . . . .
8
1.5
Publications . . . . . . . . . . . . . . . . . . . . . . . . . .
11
2
A Framework for Path Planning
13
2.1
Related Work . . . . . . . . . . . . . . . . . . . . . . . . .
14
2.2
Framework Overview . . . . . . . . . . . . . . . . . . . . .
16
2.3
Dealing with Geometry . . . . . . . . . . . . . . . . . . .
19
2.3.1
Requirements on Geometric Types . . . . . . . . .
20
2.3.2
Moving Objects Around . . . . . . . . . . . . . . .
22
2.3.3
Concrete Geometric Types . . . . . . . . . . . . .
23
2.4
Collision Detection . . . . . . . . . . . . . . . . . . . . . .
24
2.4.1
Convex Polyhedra and Collision Detection . . . . .
25
2.4.2
Proximity Queries on Pairs of Objects . . . . . . .
28
2.4.3
Classes for Dealing with Sets of Objects . . . . . .
30
2.5
Configuration Space Interpolation
. . . . . . . . . . . . .
33
2.5.1
Interpolation of Revolute Joints . . . . . . . . . . .
33
2.5.2
Interpolation of Rigid Body Orientations . . . . .
35
2.5.3
Car-Like Robots . . . . . . . . . . . . . . . . . . .
36
2.5.4
Interpolation Objects . . . . . . . . . . . . . . . .
38
2.6
Metrics . . . . . . . . . . . . . . . . . . . . . . . . . . . .
39
2.6.1
Useful Metrics for Path Planning . . . . . . . . . .
41
2.6.2
Implemented Classes for Metrics . . . . . . . . . .
42
2.7
Configuration Space Sampling . . . . . . . . . . . . . . . .
44
2.7.1
Narrow Passage Sampling . . . . . . . . . . . . . .
45

viii
Contents
2.7.2
Constraint Based Sampling . . . . . . . . . . . . .
46
2.7.3
Uniformly Distributed Rotations . . . . . . . . . .
47
2.7.4
Deterministic Sampling . . . . . . . . . . . . . . .
47
2.7.5
Sampling Strategy Classes . . . . . . . . . . . . . .
48
2.8
Local Planners . . . . . . . . . . . . . . . . . . . . . . . .
50
2.8.1
Checking Path Segments . . . . . . . . . . . . . . .
50
2.8.2
Local Planner Interface . . . . . . . . . . . . . . .
51
2.8.3
Example of a Flexible Local Planner . . . . . . . .
52
2.9
Chapter Summary . . . . . . . . . . . . . . . . . . . . . .
53
3
A General Robot Model for Path Planning
55
3.1
Notation for Articulated Structures . . . . . . . . . . . . .
56
3.1.1
Denavit-Hartenberg Notation . . . . . . . . . . . .
57
3.1.2
Kleinfinger-Khalil Notation . . . . . . . . . . . . .
60
3.2
Modeling Joint Couplings . . . . . . . . . . . . . . . . . .
61
3.3
Inverse Kinematics . . . . . . . . . . . . . . . . . . . . . .
64
3.3.1
The Jacobian . . . . . . . . . . . . . . . . . . . . .
65
3.3.2
A Numerical Solver Based on the Pseudo-Inverse .
67
3.4
Robot Composition . . . . . . . . . . . . . . . . . . . . . .
70
3.5
Design and Implementation . . . . . . . . . . . . . . . . .
72
3.5.1
Self-Collision Table . . . . . . . . . . . . . . . . . .
73
3.5.2
Multiple Inverse Kinematics Solvers . . . . . . . .
73
3.6
Chapter Summary . . . . . . . . . . . . . . . . . . . . . .
78
4
Augmenting RRT-Planners with Local Trees
81
4.1
The RRT-ConCon Algorithm . . . . . . . . . . . . . . . .
82
4.2
Local Trees . . . . . . . . . . . . . . . . . . . . . . . . . .
84
4.2.1
The RRT-LocTrees Algorithm . . . . . . . . . . . .
86
4.3
Experiments . . . . . . . . . . . . . . . . . . . . . . . . . .
88
4.4
Chapter Summary . . . . . . . . . . . . . . . . . . . . . .
92
5
Planning of Pick-and-Place Tasks
97
5.1
Related Work . . . . . . . . . . . . . . . . . . . . . . . . .
98
5.2
Overview of the Planner . . . . . . . . . . . . . . . . . . .
99
5.3
Grasp Generators . . . . . . . . . . . . . . . . . . . . . . . 104
5.3.1
Algorithms Suitable as Grasp Generators . . . . . 104
5.3.2
The Grasp Generator Interface . . . . . . . . . . . 106
5.3.3
Feedback to Grasp Generator Clients . . . . . . . . 107
5.4
Retract Planners . . . . . . . . . . . . . . . . . . . . . . . 109
5.5
Planning of Arm and Hand Motions . . . . . . . . . . . . 110

Contents
ix
5.5.1
Multiple Goals . . . . . . . . . . . . . . . . . . . . 112
5.5.2
RRT-Planners and Retract Trees . . . . . . . . . . 113
5.5.3
Backtracking . . . . . . . . . . . . . . . . . . . . . 114
5.5.4
Efficient use of Robot Composition . . . . . . . . . 115
5.6
Task Constraints . . . . . . . . . . . . . . . . . . . . . . . 116
5.7
Path Smoothing . . . . . . . . . . . . . . . . . . . . . . . 117
5.7.1
Path Optimization . . . . . . . . . . . . . . . . . . 118
5.7.2
Spline Paths . . . . . . . . . . . . . . . . . . . . . 120
5.8
Examples . . . . . . . . . . . . . . . . . . . . . . . . . . . 123
5.9
Chapter Summary . . . . . . . . . . . . . . . . . . . . . . 130
6
Grasp Planning for a Three-Fingered Hand
135
6.1
Related Work . . . . . . . . . . . . . . . . . . . . . . . . . 136
6.2
Problem Description . . . . . . . . . . . . . . . . . . . . . 137
6.3
The Grasp Planner . . . . . . . . . . . . . . . . . . . . . . 139
6.3.1
An Overview . . . . . . . . . . . . . . . . . . . . . 139
6.3.2
Global Contour Characteristics . . . . . . . . . . . 143
6.3.3
Choosing Good Thumb Positions . . . . . . . . . . 145
6.3.4
Use of Precomputed Trajectories . . . . . . . . . . 146
6.3.5
Grasp Quality Evaluation . . . . . . . . . . . . . . 149
6.4
Examples of Planned Grasps . . . . . . . . . . . . . . . . 151
6.5
Chapter Summary . . . . . . . . . . . . . . . . . . . . . . 154
7
Grasp Stability Evaluation
157
7.1
Grasp Analysis Introduction . . . . . . . . . . . . . . . . . 158
7.2
Related Work . . . . . . . . . . . . . . . . . . . . . . . . . 160
7.3
Grasp Evaluation Procedure . . . . . . . . . . . . . . . . . 164
7.3.1
An Algorithm for Solving the Min-Max Problem . 166
7.3.2
Improvement from Sorting the Hyperplanes . . . . 168
7.3.3
Disturbance Forces and Object Vertices . . . . . . 170
7.4
Examples . . . . . . . . . . . . . . . . . . . . . . . . . . . 172
7.5
Chapter Summary . . . . . . . . . . . . . . . . . . . . . . 173
8
Summary and Suggestions for Future Work
179
A Class Diagram Notation
183
B Rigid Body Transformations
187
B.1 The Homogeneous Transformation Matrix . . . . . . . . . 187
B.1.1 A Class for Homogeneous Transformations
. . . . 190
B.2 Representing Rotations . . . . . . . . . . . . . . . . . . . 191

x
Contents
B.2.1 Rotation Matrices . . . . . . . . . . . . . . . . . . 191
B.2.2 Euler Angles . . . . . . . . . . . . . . . . . . . . . 192
B.2.3 Quaternions . . . . . . . . . . . . . . . . . . . . . . 193
B.3 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . 196
C Geometric Representations
197
C.1 Indexed Face Sets . . . . . . . . . . . . . . . . . . . . . . . 198
C.2 Triangle Sets . . . . . . . . . . . . . . . . . . . . . . . . . 199
C.3 Convex Polyhedra . . . . . . . . . . . . . . . . . . . . . . 199
C.4 Non-Convex Objects and Groups of Convex Objects . . . 202
C.4.1 Three Possible Designs . . . . . . . . . . . . . . . . 202
C.4.2 An Example of a Hierarchical Geometric Object . 204
D Pairwise Collision Detection with PQP and GJK
207
D.1 Encapsulating PQP
. . . . . . . . . . . . . . . . . . . . . 207
D.2 Encapsulating Enhanced GJK . . . . . . . . . . . . . . . . 210
D.3 Mixing Different Algorithms . . . . . . . . . . . . . . . . . 211
E Framework Details
213
E.1 Configuration Space Path . . . . . . . . . . . . . . . . . . 213
E.2 Binary Constraints . . . . . . . . . . . . . . . . . . . . . . 214
E.3 Problem Class . . . . . . . . . . . . . . . . . . . . . . . . . 215
E.4 Visualization . . . . . . . . . . . . . . . . . . . . . . . . . 217
E.5 Robot Description Files . . . . . . . . . . . . . . . . . . . 219
F The Boost Libraries
223
F.1 Boost Graph Library . . . . . . . . . . . . . . . . . . . . . 223
F.2 Spirit
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 224
References
229

Chapter 1
Introduction
In its most basic form, robot path planning is about finding a collision-
free motion from one position to another. Efficient algorithms for solving
problems of this type have important applications in areas such as: indus-
trial robotics, computer animation, drug design, and automated surveil-
lance. It is therefore not surprising that the research activity in this field
has been steadily increasing over the last two decades.
In the first part of this thesis, we study how object-oriented design
methods can be of use in the context of path planning. The result is an
object-oriented framework that makes it easy to develop and compare
path planning algorithms. Using this framework, two new algorithms
have been developed; one for the basic form of the path planning problem,
and one aimed for pick-and-place tasks.
The last chapters of the thesis are about grasp planning, which is a
related but slightly different type of planning; whereas path planning is
about finding motions, grasp planning is about finding a single configu-
ration, i.e., the grasp, that satisfy certain specifications.
The next section gives examples of application areas for path planning
algorithms. Section 1.2 introduces the terminology and notation that
is used in the path planning community. Section 1.3 presents a type
of randomized path planning algorithms that are most common today,
namely probabilistic roadmap methods (PRMs). The framework to be
presented in the thesis is not constrained to a particular planning method,
but will focus on PRM-like methods. Hence, Section 1.3 is useful to
understand the motivation behind the framework.
The final section gives an overview of the thesis and its contributions.

2
1 Introduction
1.1
Path Planning Applications
Apart from the obvious application areas in industrial robotics and au-
tonomous systems in general, there are also other useful application areas
for path planning. In this section we list some interesting examples to-
gether with relevant references.
Industrial and Service Robotics
In industrial applications, the
robot motion has to be carefully programmed for each new task. As
this programming can be both laborious and time consuming, there is
much to win if this process could be made semi-autonomous. That is, a
path planning algorithm could be used to give suggestions on collision-
free motions. The robot programmer could then choose to accept or to
modify the generated motions, before making them part of the robot
program.
Unlike industrial robots, service robots have to operate in unpre-
dictable and unstructured environments.
Such robots are constantly
faced with new situations for which there are no preprogrammed mo-
tions. Thus, these robots have to plan their own motions. Path planning
for service robots are much more difficult due to several reasons. First,
the planning has to be sensor-based, implying incomplete and inaccurate
world models. Second, real-time constraints means limited resources for
planning. Third, due to incomplete models of the environment, planning
could involve secondary objectives, with the goal to reduce the uncer-
tainty about the environment.
Navigation for mobile robots is closely related to sensor-based path
planning in 2D, and can be considered as a mature area of research [21,
30]. Sensor-based planning for manipulators, on the other hand, is still a
very open research problem. One of the first systems capable of sensor-
based planning for manipulation was the Handey system [93, 62]. Based
on laser range data, this system could plan pick-and-place tasks for a ma-
nipulator equipped with a parallel-jaw gripper. For two recent examples,
see Ahuactzin and Portilla [2], and Suppa et al. [146].
Computer Animation
There is a growing integration of computer
animation with artificial intelligence to create virtual actors [40, 71]. Au-
tonomous or semi-autonomous virtual actors lead to higher productivity
as animators can now use high-level commands instead of specifying long
sequences of key-frames. Furthermore, with physics-based simulation, a
higher degree of realism can be achieved [40]. In his thesis, Kuffner [71]

1.1 Path Planning Applications
3
used path planning techniques to create autonomous characters for real-
time animation. Pettr´e et al. [118] were able to plan walking motions
for a virtual character with 57 degrees of freedom. The type of motion
used could be determined from motion capture data, allowing for realistic
animations. The techniques used in [71] and [118] can also be used in
computer games to create more convincing and more skilled computer-
controlled characters.
Virtual Prototyping
Virtual prototyping involves modeling and sim-
ulation of all important aspects of a prototype, e.g., mechanical design,
kinematics, and dynamics, accompanied by realistic visualization. For
a mechanical assembly like an engine, one important aspect of the final
product is maintainability: How easy is it to reach and replace a cer-
tain part in the assembly? Without a physical prototype, such questions
can be difficult to answer, and path planning algorithms could thus be
a useful tool. For efficient manufacturing, we are interested in assem-
bly strategies that are as efficient as possible. Guibas et al. [51] and
Sundaram et al. [145] studied algorithms for the automatic generation of
efficient assembly/disassembly strategies. Another related application is
the problem of automated carton folding, see Lu and Akella [94].
Surveillance Planning
For automated inspection or surveillance of
indoor environments it is important to find a short paths that covers the
entire environment. This problem, also known as the watchman route
problem, was considered by Danner and Kavraki [37]. Their approach
is general in that different visibility constraints can be imposed on the
sensor model and it is also applicable for three-dimensional regions. In a
variation of the watchman route problem, there is one or more intruders
present that has to be detected. This problem is harder because intrud-
ers are assumed to move arbitrarily fast and can play “hide-and-seek”
by sneaking back into areas already covered by the surveillance agent.
A solution, if it exists, will cover the environment and detect all intrud-
ers, independent of their number and maximum speed. LaValle et al. [83]
dealt with this problem in the case of one or more robots with omnidirec-
tional vision and a polygonal environment. LaValle and Hinrichsen [80]
later extended this work to include curved environments. In the case of
a beam-like sensor and a polygonal environment, Simov et al. [138] pre-
sented an O(n
3
) complete algorithm, where n is the number of vertices
in the environment.

4
1 Introduction
Computational Biology
A somewhat unexpected application area for
path planning algorithms is within the fields of computational biology
and chemistry. In these fields path planning algorithms have been used
to study flexible ligand docking [14], and protein folding pathways [7].
Ligand docking is important in the area of computer-aided drug de-
sign, where the goal is to find a small molecule (the ligand) that is able to
dock with a target protein (the receptor). A potential docking configura-
tion must not only correspond to a configuration of low potential energy;
it must also be accessible to the ligand from an outside location. Bayazit
et al. [14] used path planning algorithms to find docking configurations
for ligand-protein pairs. The found docking configurations were close to
the real ones.
Protein molecules are long chains of amino acids. These molecule
chains will, under normal circumstances, fold themselves into a close-
packed, low-energy configuration. This folding process is important to
understand for several reasons: The protein’s function is determined by
its three-dimensional structure, and disturbed protein folding is related
to diseases, such as cystic fibrosis and Alzheimer’s disease [75]. The
folding process is hard to capture experimentally because it happens so
quickly [7], and therefore simulation is a necessary tool. Amato and
Song [7] studied protein folding as a path planning problem toward a
low-energy configuration. Because protein molecules can have several
thousands of degrees of freedom, these problems are very hard to solve
even for small proteins.
1.2
Notation and Terminology
Here we give a brief introduction to common path-planning terminology.
A path planning problem involves one or more robots
1
moving in
W = R
3
. (For a two-dimensional problem we have W = R
2
.) A robot is
denoted with A, and a robot’s configuration is usually denoted with q.
If the robot is a kinematic chain, then the configuration q is typically a
vector containing the joint angles. If the robot is a free-flying rigid body,
then the configuration consists of a translational part and a rotational
part. Depending on how we represent rotations, the rotational part could
be three Euler angles or it could be a quaternion (see Appendix B). The
1
Note the we use the word robot in a very broad meaning here: It can be used to
mean a kinematic chain, a rigid body, or even an aircraft.

1.2 Notation and Terminology
5
closed subset of world, W, that is occupied by the robot in configuration
q is denoted by A(q). Thus, A(q) ⊂ W.
Obstacle Region
The obstacle region, O, is defined as the set of all
points in W that belong to one or more obstacles. If the obstacles are
represented by closed subsets of W, then the obstacle region is also a
closed subset, O ⊂ W.
Configuration Space
The space of all possible robot configurations is
denoted by C, and is called the configuration space of the robot. The con-
figuration space concept is very important in path planning. For a robot
arm with n joints, where each joint has a limited range, the configura-
tion space is simply R
n
. Other problems can have a configuration space
with a completely different topology: Consider a two-dimensional world
with a rigid body that can translate and rotate. Due to the unbounded
rotational degree of freedom, the configuration space for this problem is
R
2
× S
1
, where S
1
denotes the unit circle.
Configuration Space Obstacles
The set of all robot configurations
for which the robot intersects an obstacle is denoted by C
obs
. This set is
defined as:
C
obs
= {q ∈ C | A(q) ∩ O = ∅ }
(1.1)
Equation (1.1) can be seen as a mapping of the obstacles in W to obstacles
in the configuration space C. The resulting regions are often denoted
configuration space obstacles, or C-space obstacles for short. Note that
for robots consisting of several moving objects, the definition of C
obs
in
Equation (1.1) have to be extended to include self-collisions.
The remaining portion of the configuration space is called the free
space. The free space is defined and denoted as C
f ree
= C \ C
obs
.
Since the seminal work of Lozano-P´erez [92], most planning algo-
rithms work in the configuration space. Transforming the problem from
W to the C-space using Equation (1.1) is a useful abstraction: Path plan-
ning problems that look very different in W still look similar in a C-space
formulation, allowing the same type of algorithm to solve widely differ-
ent problems. Note, however, that the transformation in Equation (1.1)
is carried out explicitly only for very simple problems of low-dimension.
Most algorithms instead work by probing C to find out wether a config-
uration belongs to C
f ree
.

6
1 Introduction
Piano Mover’s Problem
The goal of a path planning problem is thus
to find a path in C
f ree
that connects an initial configuration, q
i
, with a
goal configuration, q
g
. This particular type of problem is often referred
to as the Piano Mover’s Problem in the literature. In the formulation of
the Piano Mover’s Problem there is no concept of time; it is enough to
find a sequence of translations and rotations that takes the robot from
the initial configuration to the goal configuration, and the velocities and
accelerations are of no concern. If we want to solve the problem, and at
the same time respect the dynamic constraints of the particular system,
then we have an instance of a kinodynamic motion planning problem. In
this thesis we only briefly touch upon kinodynamic planning. For work
on kinodynamic motion planning, see, e.g., LaValle and Kuffner [81] and
Lamiraux et al. [74].
For a more formal definition of the concepts in this section, see the
classic textbook by Latombe [77]. A more recent alternative is the book
by LaValle [79].
1.3
Probabilistic Roadmap Methods
A path planning algorithm that is guaranteed to find a solution if one
exists, and report failure if there is no solution, is said to be complete.
Reif [125] showed that the Piano Mover’s Problem is NP-hard. This
result implies that complete algorithms are only practical for problems
with a low-dimensional configuration space. The complexity of complete
path planning methods lead researchers to seek heuristic methods with
weaker notions of completeness, such as probabilistic completeness [64].
An algorithm that is probabilistically complete is guaranteed to find the
solution to a solvable problem in finite time [147].
In a series of ground breaking papers, Kavraki and Latombe [63],
ˇ
Svestka and Overmars [147], and Kavraki et al. [64] laid the ground
for probabilistic roadmap methods (PRM). PRM-methods work in two
phases: a learning phase, and a query phase. In the learning phase (also
called construction phase or preprocessing phase in the literature), the
configuration space is sampled for collision-free configurations. These
configurations form the vertices in a graph called a roadmap. A simple
local planner is used to look for connections between nearby vertices. If
a connection is found, an edge is added to graph, connecting the corre-
sponding vertices. The idea is that the roadmap will eventually give a
sufficient representation of C
f ree
.

1.3 Probabilistic Roadmap Methods
7
(a) Roadmap
q
i
q
g
(b) Query
Figure 1.1:
(a) An example of a roadmap for a two-dimensional config-
uration space. (b) An example of a roadmap query. The resulting path
is shown by the thicker lines.
In the query phase, the initial configuration, q
i
, and the goal con-
figuration, q
g
, are connected to the graph using the same local planner
that was used for the learning phase. If this connection succeeds, then
the path planning problem has been reduced to a graph search problem,
which can be answered in fractions of a second. Figure 1.1 (a) shows
an example of a roadmap for a two-dimensional configuration space. An
example of a query to the same roadmap is shown in Figure 1.1 (b).
PRM methods are particularly useful if repeated queries are expected
for the same environment; then the cost for constructing the roadmap
is amortized over each query. Thus, given a static environment, the
overall efficiency of a PRM-planner usually increases with the number of
queries. Due this behavior, there is often a distinction between single
shot planners, and multiple query planners. PRM-planners have for a
long time been thought of as multiple query planners due to the costly
learning phase, but recent contributions by Bohlin and Kavraki [17, 18]
have changed on that; they showed that the costly operation of verifying
wether path segments are collision free could be postponed until the query
phase. Thus, the constructed roadmap contains many infeasible edges,
but these are detected and deleted during the query phase. With this
Lazy PRM approach, the number of required collision detections were
significantly reduced, making the approach competitive as a single shot

8
1 Introduction
planner as well.
Variations
Since the original work in [63, 147, 64], numerous varia-
tions, improvements, and extensions have been suggested. Already in [64]
it was pointed out that using uniform sampling of the C-space would not
be efficient in case the problem involves a narrow passage. To get through
the passage, the roadmap must have at least one vertex in that area.
However, as the critical region usually covers an extremely small portion
of the configuration space, the chance of getting a configuration there at
random is very small. Therefore many researchers have suggested sam-
pling schemes that bias the distribution towards the narrow passages of
the configuration space.
Other variations explore different heuristics for determining between
which vertices connection attempts should be made. These heuristics
often involve the choice of a metric, according to which the distance
between two configurations is determined. Throughout the thesis we will
use the words vertices and nodes interchangeably.
In Chaper 2 we will describe many of these PRM-methods in more
detail. In that chapter we will also present an object-oriented framework
that offer a systematic way to deal with all these variations.
1.4
Outline and Contributions of the Thesis
Chapter 2
Chapter 2 describes an object-oriented framework for developing and
comparing path planning algorithms. The framework consists of reusable
components that accelerate the development of path planning algorithms.
There exist many other path planning frameworks with the purpose to
ease and speedup the development of new algorithms, but the framework
in this thesis is specifically designed to make it easy to compare different
algorithms. This feature is an important contribution as it addresses
the lack of fair comparisons between the many similar path planning
algorithms that exist. The following list summarizes the key features of
the framework:
• The framework makes it easy to do fair comparisons between vari-
ations of a concept, such as different collision detection algorithms.
• There are few restrictions on the type of planning algorithms that
can be implemented with the framework.

1.4 Outline and Contributions of the Thesis
9
• The framework is platform independent.
• Adaptive planners that change between different algorithms de-
pending on the problem, are easily implemented.
Chapter 2 can also be read as an introduction to several important topics
such as metrics, sampling, and collision detection.
Chapter 3
Chapter 3 describes a general class for representing robots. The class
can describe robots with tree-like kinematic chains and joint couplings.
The possibility to add joint couplings makes it easy to model robots
with coupled degrees of freedom, such as, e.g., the Barrett hand.
2
Joint
couplings also allow us to model certain types of robots with closed kine-
matic chains; an important feature as many industrial robots have closed
kinematic chains to increase the stiffness of the structure.
The robot class also provides features like general and specific in-
verse kinematics and robot composition. These two features have shown
to be useful in high-level path planning applications like pick-and-place
planning, see Chapter 5.
Chapter 4
Rapidly-exploring Random Trees (RRTs) [78] have become a popular
framework for solving path planning problems. In Chapter 4 we address
some of the weaknesses of bidirectional RRT-planners and present an im-
proved algorithm. The improved algorithm uses local trees, in addition to
the ones rooted at the start and goal configuration. Several experiments
show that the new algorithm performs much better than the original one
in case the solution has to pass several difficult regions.
Chapter 5
In Chapter 5 we present a pick-and-place planner that builds upon the
work by Kuffner [71]. The proposed planner is fast; it can solve every-
day pick-and-place tasks in a few seconds. Some improvements over the
planner in [71] allow the new planner to handle difficult problems more
2
The Barrett hand is a three-fingered robot hand from Barrett Technology, Inc.
See also Chapter 6 for a description.

10
1 Introduction
efficiently. Other contributions in this chapter are: the use of multi-
ple goals, and a useful model for the separation of the grasp generation
process from the trajectory planning.
The second part of the thesis focuses on grasp planning and grasp sta-
bility. To be of real use, a robot must be able to interact with its envi-
ronment by picking up and manipulating objects. Thus, for robots that
autonomously interact with the environment, grasp planning and path
planning are needed in combination.
Chapter 6
In Chapter 6 we present a fast grasp planner for a three-fingered hand.
The planner uses a two-dimensional contour of the object as input and
the result is an ordered set of stable grasps. This contribution could be
useful for example in industrial applications where a robot is to pick up
objects of unknown shape from a conveyor belt.
Chapter 7
Grasp planning is often formulated as an optimization problem, where
the goal is to find the best grasp according to some criterion. In Chapter 7
we present a novel approach to grasp stability evaluation. The approach
is based on the grasp’s ability to withstand disturbance forces that act
on the surface of the grasped object. Previous approaches have often
treated the grasp stability analysis as independent of the object geometry,
whereas we claim that the stability depends on the geometry also. Our
approach takes the geometry into account, and it can also incorporate
task information to derive a task directed quality measure for a grasp.
Appendices
Much of the framework in Chapter 2 is illustrated using a variant of the
class diagram notation introduced by Rumbaugh et al. [126]. Appendix A
gives a brief introduction to class diagrams. Here we also make clear the
additional conventions that are used in this thesis.
Appendix B gives a short introduction introduction to rigid body
transformations and various ways of representing rotations.
The framework presented in Chapter 2 is designed to support different
geometric representations. Users of the framework can define their own

1.5 Publications
11
geometric types, or they can use one of the three types that the frame-
work provides: triangle sets, convex polyhedra, and hierarchies of convex
polyhedra. These three geometric types are covered in Appendix C.
In a similar manner, the framework is not bound to a specific collision
detection algorithm. The collision detection algorithms that have been
tested so far are PQP [76] and Enhanced GJK [27]. These algorithms are
covered in Appendix D.
Appendix E covers additional details of the framework, such as visu-
alization, task constraints, and path representation.
Appendix F presents parts of the Boost libraries that have been found
useful for implementing path planning algorithms. Examples are: generic
graph functions, random number generation, matrix classes, and parsing.
1.5
Publications
Some of the work presented in this thesis has been published elsewhere.
The material in Chapters 4, 6, and 7 is presented in part in the following
papers:
• M. Strandberg and B. Wahlberg. A method for grasp stability
evaluation based on disturbance force rejection. submitted in Aug.
2004 to IEEE Transactions on Robotics.
• M. Strandberg. Augmenting RRT-planners with local trees. In
IEEE International Conference on Robotics and Automation, vol-
ume 4, pages 3258–3262, New Orleans, LA, USA, Apr. 2004.
• M. Strandberg. A fast grasp planner for a three-fingered hand based
on 2D contours. In 33rd International Symposium on Robotics,
Stockholm, Sweden, Oct. 2002. IFR, Swira.
• M. Strandberg. A grasp evaluation procedure based on distur-
bance forces. In IEEE/RSJ International Conference on Intelligent
Robots and Systems, volume 2, pages 1699–1704, EPFL, Lausanne,
Switzerland, Oct. 2002.
The following publication contains results that are relevant, but not
covered in the thesis.
• L. Petersson, P. Jensfelt, D. Tell, M. Strandberg, D. Kragic and
H. I. Christensen. Systems integration for real-world manipulation

12
1 Introduction
tasks. In IEEE International Conference on Robotics and Automa-
tion, volume 3, pages 2500–2505, Washington, DC, USA, May 2002.

Chapter 2
A Framework for Path
Planning
Since the original work in [63, 147, 64], many variations of the basic
PRM algorithm have been published. Examples are: different sampling
schemes, different metrics, and various local planners. However, it is
difficult to compare these contributions objectively as “they were tested
on different types of scenes, using different underlying libraries, imple-
mented by different people on different machines” [46]. This problem
underlines the importance of a homogeneous software platform, where
changing, e.g., the collision detection algorithm is as simple as changing
one line of code or a directive in a configuration file. Only then can we
truly compare two versions of the same concept.
To address this problem, we will in this chapter present an object-
oriented framework for building path planners. The design rationale be-
hind the framework is plug-and-play capability between variations of all
the major concepts in path planning. Apart from the framework itself,
the contributions in this chapter are the identification of a set of or-
thogonal concepts, and the definition of coherent and efficient interfaces
to these concepts. The resulting framework is a set of loosely coupled
components that can be combined and extended to build path planners.
To emphasize the component-based approach, the framework is named
CoPP, which stands for Components for Path Planning.
As this is not the first attempt to create a framework for path plan-
ning, the next section will describe other approaches and we will see how

14
2 A Framework for Path Planning
they compare to CoPP. In Section 2.2 we give an overview of the frame-
work and we also group the different path planning concepts into four
high-level categories. These categories make it easier to get an overview
of the different concepts that are modeled in the framework, but they can
also serve as a model for how path planning applications are built using
the framework.
1
For each of the four categories in Section 2.2, we identify
a number of concepts, such as metrics, sampling, and collision detection.
The most important of these concepts are covered in Sections 2.6–2.8.
The object-oriented design of each concept is visualized using a variant
of the class diagram notation introduced by Rumbaugh et al. [126]. A
brief introduction to the class diagram notation is given in Appendix A.
Throughout this chapter we try to focus on the high-level aspects of the
design, but readers who are interested in more details can find additional
material in the appendices.
2.1
Related Work
There are some recent papers where the need for fair comparisons be-
tween different path planning algorithms have been addressed. Amato et
al. [6] studied the effect of using different metrics and local planners for
rigid body problems. Reggiani et al. [124] used pluggable distance com-
putation functions to perform an extensive study of of existing collision
detection packages. Geraerts and Overmars [46] studied existing sam-
pling techniques and node adding techniques using a framework called
SAMPLE (System for Advanced Motion PLanning Experiments). Stud-
ies like these are important in the correct assessment of the merits and
drawbacks of different algorithms and is something that should be done
each time a new algorithm has been developed. It is therefore highly
desirable to have an extendable framework in which we can do compar-
isons between variations on different concepts. The CoPP framework is
designed such that doing experiments like those in [6, 124, 46] becomes
very easy.
There are a number path planning frameworks that have been pre-
sented in the literature, some of which are also publicly available. Among
the more popular is the Motion Strategy Library (MSL) developed at
the University of Illinois.
2
MSL provides planners based on Rapidly-
1
Note that the we do not specify a specific architecture for path planners. We focus
instead on building blocks that can be used to build a path planner.
2
MSL is available at msl.cs.uiuc.edu/msl

2.1 Related Work
15
exploring Random Trees (RRTs), PRMs and forward dynamic program-
ming. A nice feature about MSL is that its design allows holonomic and
dynamic systems to be treated in a uniform manner. Thus, from a plan-
ner’s point of view it does not matter wether the system is dynamic or
not. However, there are also some drawbacks with the design of MSL: In
some areas the design does not have a good separation of concerns and
the granularity of the modeled concepts is too coarse, making it hard to
vary concepts individually. Another problem is that due to the chosen
method for visualization, MSL does not work under Windows. In CoPP,
we have avoided most of the portability issues by using the platform
independent VRML [8] language for visualization.
Another framework is OxSim, developed at Oxford University, see
Qin et al. [122], Cameron [28], and Cameron and Pitt-Francis [29]. The
design of OxSim focuses on the separation of distance computations and
geometric issues from the planner; distance computations is provided by
a module based on the Enhanced GJK algorithm [27], also developed
at Oxford. Due to the GJK algorithm only working on convex objects,
OxSim is currently limited to objects that can be described as the union of
convex polyhedra. The framework also require problems to be formulated
in terms of a kinematic chain, i.e., a serial manipulator. Both PRM
planners and potential field planners have been tested in OxSim, see [122,
29].
The, in our opinion, most mature and successful framework for
path planning so far is the Move3D framework developed by Sim´eon
et al. [135, 136]. This opinion is partly based on the number of new
PRM-techniques that has been developed and tested with Move3D,
see [35, 50, 134, 118]. Move3D has also resulted in a commercial spin-off
product named KineoWorks
3
, which might be the reason why Move3D is
not publicly available. Move3D is implemented in pure C, and the man-
ual [110] reveals that the set of modeled concepts is roughly the same
as in CoPP. Another feature common to both frameworks is the support
for several collision detection algorithms. However, because of the strong
encapsulation offered by object-oriented C++, we believe that new con-
cepts can be added more easily to CoPP than to Move3D.
4
3
www.kineocam.com
4
Object-oriented programming is of course possible in C as well, but it usually
requires much more work and discipline of the programmer.

16
2 A Framework for Path Planning