Robot Path Planning: An Object-Oriented Approach

Path
AddViaPoint(time, config)
Length(metric)
Sample(time, config, interpolator)
Figure E.1:
A class for representing paths in the configuration space
as a sequence of via-points. The path to take between the via-points is
determined by the interpolator argument.
determined by a chosen interpolation scheme. The most simple is of
course linear interpolation, but in general the appropriate interpolation
method is problem dependent.
The class Path, see Figure E.1, represents sequences of via-points
in an n-dimensional configuration space. Each via-point consists of a
configuration (or input signal in case of a dynamic system) and a time
value. In addition to being the basic return type of many planners, it
is also used to produce off-line animations in VRML format. The class
supports useful operations, such as concatenating two paths. Often it
is useful to compute the length of a path, e.g., to choose the shortest
path among several alternatives. Therefore the class also has a Length
method. The immediate question is of course with which metric this
length is computed. The choice of metric is left to the user, who can
provide an object of type Metric as argument to Length. If no argument
is provided, the Manhattan metric is used as the default choice.
Smoothing algorithms like Adaptive Shortcut, see Hsu et al. [58], need
to sample the path between the via-points. To produce smooth anima-
tions, there is also a need for dense sampling between the via-points.
How these intermediate via-points are computed depend on the chosen
interpolation scheme. Thus, the Sample method requires an interpola-
tion object, see Figure E.1. If no interpolation object is provided, linear
interpolation is used.
E.2
Binary Constraints
The most basic requirement of a planned motion is that it must be col-
lision free. For many applications there are also additional requirements
that have to be satisfied. For example, to provide safety margins, a trajec-
tory could be required to keep a minimum clearance to the obstacles. For

E.3 Problem Class
215
welding tasks and pick-and-place tasks, there are orientation constraints
that the robot’s end-effector must satisfy. A flexible path planning system
should thus be able to handle a wide variety of constraints.
Many path planning algorithms work by probing the C-space to test
wether a configuration is collision-free or not. A more general question to
ask is wether the configuration is satisfied, leading to the concept of binary
constraints. A planner that is implemented in terms of this abstract
concept is more flexible in that it can handle different types of constraints.
Binary constraints are represented by objects of type BinaryConstraint,
see Figure E.2. In addition to specifying an interface, this class uses the
Template Method pattern [45] to automate the gathering of statistics.
These statistics could be used to compare the number of collision checks
needed by two different planning algorithms to solve the same problem.
The class CollFreeConstr in Figure E.2 is the most commonly used
constraint class, and it tests wether a configuration is collision-free or
not. To make this class more generic, it is a template class, with the
type of the moving system and the type of the collision detection object
as template arguments. Thus, CollFreeConstr can be used with rigid
bodies, robot objects, or any other moving system that has a function
Move
with the correct signature.
The class OrientationConstr represents orientation constraints on
a robot’s end-effector. The constraint is specified by: a direction vector
attached to the end-effector frame, a direction vector attached to the
fixed world frame, and a maximum deviation angle. The constraint is
satisfied if the angle between the moving direction vector and the fixed
direction vector is less than the maximum deviation angle. To avoid
moving a robot twice for every configuration to be tested, this class also
checks wether a configuration is collision free.
E.3
Problem Class
There are some basic components that need to be defined for every basic
path planning problem. They are: a robot, a set of obstacles, a start-
ing point, and at least one goal point. There are of course variations,
like problems involving multiple robots and more elaborate task specifi-
cations, but the ideas presented here can be applied to them as well.
Writing a path planner will also require writing a lot of code that
has nothing to do with the path-planning algorithm that is to be tested.
Code to instantiate a path planning problem definitely falls into this

216
E Framework Details
BinaryConstraint
+ bool IsSatisfied(config)
+ GetNumQueries( )
+ GetNumSatisfied( )
+ Clone( )
- bool DoIsSatisfied(config)
num_queries
num_satisfied
OrientationConstr
Vector3D global_dir
Vector3D local_axis
max_dev_angle
CollFreeConstr
objSet
agent
AgentT
Move(config)
ObjSetT
IsCollisionfree( )
Robot
robot
objSet
ObjectSet
Figure E.2:
Class diagram for different types of constraints. Note that
CollFreeConstr
is a class template.
category. To shorten the development time and let developers focus more
on their algorithms, the CoPP framework provides a class representing
a basic path planning problem. This class, named PathPlanProblem,
is completely decoupled from any path planner classes and should be
thought of more as a utility class. Given a problem definition file, the
constructor takes care of building the required objects. In the case of an
error the current parser will provide a, hopefully, helpful error message.
In addition to the robot and the geometric objects, the problem class
also contains formatted information about things like: which metric to
use; which sampling strategy to use, which interpolation method to use,
and the maximum allowed step size. How this information should be
interpreted and used is up the specific application; the problem class is
thus like a recipe, from which we build the desired path planner. With
this approach, every component of the path planner can be specified in
a configuration file.

E.4 Visualization
217
The CoPP framework provides different geometric types. What type
has the geometric objects in the PathPlanProblem class? The approach
to store references to the base class Geom is ruled out because it would
hide all class specific interfaces. It then seems as there would have to
be at least one problem class for every geometric class. However, as the
purpose of the problem class is to serve as a container for the components
of a path planning problem, it should be a perfect candidate for a C++
class template. Based on this idea, PathPlanProblem is implemented
as a class template with two template arguments; one for the geometric
type of the obstacles and one for the geometric type of the robot links.
Because the most common case is that both template parameters have
the same type, the second template parameter defaults to the first. Next
we show an example of how to instantiate two different path planning
problems:
PathPlanProblem
problem1(file1);
PathPlanProblem problem2(file2);
In the first case, TriangleSet is used for all geometric objects, whereas
the second case uses GeomConvex for the robot links. Note that changing
template parameters also changes the parsers used internally by the class.
This makes it easy to extend the class for use with other geometric types;
just provide a parser with a conforming interface for the new geometric
type.
E.4
Visualization
Visualization of the output from a path planner is an important part of a
path planning application. It is, however, most often the parts concern-
ing visualization that cause existing frameworks to be system dependent,
thereby reducing the number of potential users. To avoid that situa-
tion, we choose to output animations in the VRML file format. See the
textbook by Ames et al. [8] for an excellent introduction to VRML. As
there are free VRML viewers for the most common operating systems,
this solution avoids most of the portability issues. Furthermore, stor-
ing animations in VRML files means less work for the user; saving an
animation with traditional graphical user interfaces requires the user to
convert the ongoing animation on the screen to, e.g., the mpeg format.
With the approach taken here, the VRML file itself is a perfect format
for the animation; put on a web-page, it will provide more information

218
E Framework Details
GeomVisitor
+VisitTriangleSet(obj)
+VisitGeomConvex(obj)
+VisitGeomConvexGrp(obj)
VrmlGraphics
+Draw(Geom geom)
+Draw(Robot robot) 
…
-VisitTriangleSet(obj)
-VisitGeomConvex(obj)
...
draw TriangleSet
geom.Accept(*this);
Figure E.3:
Class diagram for VrmlGraphics. Private inheritance is
used to restrict the access to the Visit methods.
to visitors because it is interactive. That is, visitors can zoom in and
out and change the view of the scene as they like. Unless the geometric
models in the animation is made up of several thousands of triangles, the
VRML file format is also a very compact method for saving an animation.
In CoPP, output to a VRML file is handled by objects of the class
VrmlGraphics
, see Figure E.3. The class has a number of Draw methods
for drawing geometric objects, robots, coordinate frames and animations.
Note that the interface of VrmlGraphics never mentions any concrete
geometric types, only the base class Geom. An important design issue is
therefore how to determine the specific type of the geometric object, such
that we can know how to draw it. This design problem was solved using
the Visitor pattern from [45]; as seen from Figure E.3, VrmlGraphics
inherits from the abstract class GeomVisitor. Here, private inheritance
is used, so that the Visit-methods can only be called from within the
VrmlGraphics
class itself. Each Visit... method will now take care
of drawing the corresponding geometric objects. An interaction diagram
that illustrates the Visitor pattern in this context is shown in Figure E.4.

E.5 Robot Description Files
219
aClient
aVrmlGraphics
aGeomConvex
aTriangleSet
Draw(aTriangleSet)
Draw(aGeomConvex)
Accept(*this)
Accept(*this)
VisitTriangleSet(*this)
VisitGeomConvex(*this)
DrawTriangleSet(aTriangleSet)
DrawGeomConvex(aGeomConvex)
Figure E.4:
An interaction diagram that illustrates how geometric ob-
jects of different types are drawn. Note that neither the client, nor the
graphics object, need to know the geometric type when calling Draw.
E.5
Robot Description Files
Robot models can be loaded at runtime using robot description files. A
robot description file contains information about:
• The kinematic structure of the robot
• Joint types and joint limits
• Joint couplings
• Links to geometry files
• Robot composition
• Links that can self-collide

220
E Framework Details
The kinematic structure of a robot is described by a tree of “nodes”,
where each node represents a moving coordinate frame. The parent of a
node is determined by the pred x directive, where x is a number iden-
tifying the parent node. The relative position between the coordinate
frames and how they move is determined by the Kleinfinger-Khalil pa-
rameters of the nodes. A node can also represent a coordinate frame that
is fixed with respect to its parent. This type of node is useful to represent
constant offsets, for example a manipulator’s position in the world.
Without any geometric objects, a robot would just be a set of moving
coordinate frames. To give a robot geometric links, one or more geomet-
ric objects can be attached to each frame. It is mandatory that each
geometric object is given a name; these names are used to specify which
links of the robot can collide with each other. The potential self-collisions
are listed in a collision table, which is also a part of the description file.
When attaching a geometric object, it can be given an optional offset and
a material specification. This allows the same geometry file to be reused
at several places in the same description file. As an example, consider a
robot hand where the finger links are identical, except for some offset or
color. In this case, a single geometry file, describing a finger link, can be
used several times.
Figure E.5 shows an example of a robot description file, describing
a simple SCARA robot. As the example robot is a single kinematic
chain without any branches, the Denavit-Hartenberg notation is suffi-
cient. Thus, the two extra parameters of the Kleinfinger-Khalil notation,
ǫ and γ, are optional. The resulting robot is shown in Figure E.6.
We assume that there exists a specific inverse kinematics solver,
named ScaraInvKin, for this robot type. To use this solver, the robot
description file uses the directive inverse kin "ScaraInvKin". If no
solver is specified, a general numerical solver is used instead.

E.5 Robot Description Files
221
robot "simple_scara"
{
# choose a specific solver
# for the inverse kinematics
inverse_kin "ScaraInvKin"
frame 0
{
transform
{
# determines the robot’s
# position in the world
translation 340 -200 980
}
# the root node
must
# refer to itself
pred 0
geom
{
name "link0"
file "link0.geom"
}
}
joint 1
{
parameters
{
a
0.0
alpha 0.0
d
0.0
theta 0.0
}
type revolute
limits
{ -180.0d 180.0d }
pred 0
geom
{
name "link1"
file "link1.geom"
}
}
joint 2
{
parameters
{
a
50.0
alpha
0.0
d
8.5
theta
0.0
}
type revolute
limits
{ -154.0d 154.0d }
pred 1
geom
{
name "link2"
file "link2.geom"
}
}
joint 3
{
parameters
{
a
45.0
alpha 180.0d
# nonzero joint offset
d
40.0
theta
0.0
}
type prismatic
limits
{ -25.0 40.0 }
pred 2
geom
{
name "link3"
file "link3.geom"
}
}
coll_table
{
"link0" and "link3"
}
}
Figure E.5:
An example of a robot description file for a robot with three
degrees of freedom. The resulting robot is shown in Figure E.6.

Figure E.6:
A simple SCARA robot with three degrees of freedom. The
world frame and the end-effector frame are also drawn.

Appendix F
The Boost Libraries
At this point we have covered most of the building blocks of the CoPP
framework. There are, however, still a few functionalities that has to
be added to make the framework easy to work with: random number
generators, matrix classes, graph classes, and parsers. On the one hand,
as we do not want to reinvent the wheel, such general purpose code should
come from existing public-domain libraries. On the other hand, we also
want to keep the number of external libraries to a minimum. This makes
the Boost libraries an ideal candidate, as it provides all the functionalities
listed above. The Boost web-site
1
provides free peer-reviewed portable
C++ libraries. The quality of the libraries is of very high standard; some
of them will be included in the C++ Standards Committee’s upcoming
C++ Standard Library Technical Report as a step toward becoming part
of a future C++ Standard.
The next two sections will describe two Boost libraries that are partic-
ularly useful in CoPP: the Bost Graph Library, and the parser generator
framework Spirit.
F.1
Boost Graph Library
Almost all path planning algorithms, that are not purely potential field
based, use some graph or tree structure. As the algorithms for searching
and traversing a graph are independent of the data that is stored in it, it
seems as a good idea to separate these and provide an abstract data type
1
www.boost.org

224
F The Boost Libraries
for all graphs. The Boost Graph Library (BGL) [133] provides generic
graph algorithms for constructing, modifying, traversing, and searching
graphs. Using template parameters, users can determine the data type
stored in the graph vertices and the graph edges, respectively. Other tem-
plate arguments can be used to determine wether the graph should be
directed or not, and other properties such as the underlying memory rep-
resentation. Experiments by Lee et al. [84] showed that for breadth-first
search, depth-first search, and Dijksktra’s algorithm, the BGL
2
was 5 to
7 times faster than the purely object-oriented LEDA C++ library [97].
This is probably due to BGL relying on compile time polymorphism,
whereas LEDA relies on runtime polymorphism: Compile time polymor-
phism avoids the overhead of virtual function calls and allows the com-
piler to do more optimizations. The conclusion is that the efficiency and
the generic nature of BGL makes it an ideal choice for graph intensive
path planning methods like PRM.
F.2
Spirit
It is inevitable that initializing a path planning application is rather pars-
ing intensive; geometric models, robot descriptions, and problem defini-
tions are all usually loaded from text files. As the expressiveness of these
description files increases, so does the complexity of the corresponding
parser. For really small projects, it is often enough to hand-code a parser
using some ad-hoc approaches. However, if the project requires several
parsers, or if the language specification becomes complex, more system-
atic approaches are needed. The usual approach when writing parsers
is to use parser generator tools like YACC [61, 87] and ANTLR [114].
With these tools, the grammar of the language is specified in a special
file. The grammar definition is then used by the tool to generate C or
C++ code for the corresponding parser. Tools like YACC and its com-
panion FLEX [85] are well known, scalable and generate fast and compact
parsers. However, users that want to modify an existing parser, or write
a new one, might not be familiar with these tools. Furthermore, as the
output from YACC and FLEX is pure C-code, they are sometimes awk-
ward to work with if you want the result of the parsing to be a C++
object. Finally, the many extra files makes maintenance harder.
2
Before becoming a part of the Boost libraries, BGL was named GGCL, which
stands for Generic Graph Component Library. Thus, in [84] the name GGCL is used
instead.

F.2 Spirit
225
A more recent approach to writing parsers is to use XML [53]. Here
the drawback is that interfacing the XML-parser with your application
is not so straightforward.
The approach used here is to use the Spirit parser generator frame-
work [38] that is available in Boost. The key idea of Spirit is to use
the operator overloading feature of C++ to allow EBNF-style grammar
specifications to be written directly in the source code. This is a major
advantage over the approaches of YACC, ANTLR and XML because: (1)
there is one tool less to learn about, (2) less files to maintain, and (3)
no interface troubles between the parser and the application. The Spirit
framework also relies heavily on templates and generic programming.
The benefit is that the one and same parser definition can transparently
be used to parse input from a file, an input stream or even a string in
memory. Furthermore, existing parsers can be composed at compile-time
to more complex parsers.
As a short example of how Spirit works, we will write a parser that
recognizes a comma-separated list of real numbers. Spirit comes with
several predefined parser primitives that are used to build more complex
parsers. One such parser is real p, which recognizes and parses real
numbers. Another such parser is ch p, which recognizes a single charac-
ter. We can now use these parser primitives, together with overloaded
operators, to build a more complex parser:
real p >> *(ch p(’,’) >> real p)
This parser will recognize input that is a comma-separated list of real
numbers. The operator >> is overloaded such that a >> b means “b
must follow a”. The expression *a means that a must be matched zero
or more times. The operator * is also known as the Kleene star and in
conventional notation it appears after the expression it modifies. How-
ever, as C++ has no postfix * operator, Spirit requires the Kleene star
operator to appear before the expression.
The parser just presented is not of much use; it is merely a recognizer
that can tell us wether the input matched its grammar. To be of real
use, we must bind semantic actions to the parser, actions that we want
to be executed every time the parser finds a match. In this case we would
probably want to store all the numbers in a vector. Spirit also comes with
a set of predefined semantic actions. One of those is append, which fits
our purpose perfectly. Assume that we have defined a variable v that is
of type vector. We can now put our list of real numbers in v
with the following parser:

226
F The Boost Libraries
const char* ParseNumbers(const char* str,
vector& v)
{
return parse(str,
real_p[append(v)] >> *(ch_p(’,’) >> real_p[append(v)]),
space_p).stop;
}
Figure F.1:
A function for parsing a comma-separated list of real num-
bers. The list items can be separated by any amount of white space.
The function returns a pointer to the first character not consumed by
the parser.
real p[append(v)] >> *(ch p(’,’) >> real p[append(v)])
From this example we see that Spirit uses the [] operator to attach
semantic actions to a parser. Users can easily define their own semantic
actions, which they can attach to parsers.
Finally, to activate our parser we have to call one of the many parse
functions available in Spirit. If we assume that our input is stored in an
ordinary character array, we can wrap the parser call in a function as
shown in Figure F.1.
The function in Figure F.1 will consume the input in str as long as it
matches the specified grammar, or until the end of the string is reached.
The return value is a pointer that points to the last character that was not
consumed. Often the input contains characters that we are not interested
in, such as white space or comments. To decide which characters to skip,
the parse function needs a third argument, a skip parser that acts like a
filter on the input. In this example, we used the predefined space p as
skip parser. The space p argument causes our parser to simply ignore
any white space.
Hopefully this little example has conveyed the main ideas behind
Spirit. To conclude:
• Spirit is easy to learn and relieves users from the need to learn a
specific parser generator tool.
• The ability to define grammars directly in the C++ code leads to:
–
Shorter development times
–
Easier maintenance

F.2 Spirit
227
–
More reuse of domain specific parsers
• The resulting parsers are compact, flexible and reasonable fast.
On the downside we can mention that due to being completely tem-
plate based and relying heavily on template meta-programming, Spirit
pushes the compiler to its limits; complex parsers might require the user
to increase, e.g., the compiler heap limit. Furthermore, the occurrence of
a syntax error during compilation will cause the compiler to utter almost
indecipherable error messages, spanning over several lines. Despite of
these two drawbacks, we think that Spirit is well fit for small- to medium
sized languages.

References
[1] D. Aarno, D. Kragic, and H. I. Christensen. Artificial potential
biased probabilistic roadmap method. In IEEE International Con-
ference on Robotics and Automation, volume 1, pages 461–466, New
Orleans, LA, USA, Apr. 2004.
[2] J. M. Ahuactzin and A. Portilla. A basic algorithm and data struc-
tures for sensor-based path planning in unknown environments. In
IEEE/RSJ International Conference on Intelligent Robots and Sys-
tems, volume 2, pages 903–908, Takamatsu, Japan, Nov. 2000.
[3] M. Akinc, K. E. Bekris, B. Y. Chen, A. M. Ladd, E. Plaku, and
L. E. Kavraki. Probabilistic roadmaps of trees for parallel com-
putation of multiple query roadmaps. In Eleventh Int. Symp. on
Robotics Research, Siena, Italy, Oct. 2003.
[4] A. Alexandrescu. Modern C++ Design: Generic Programming
and Design Patterns Applied. The C++ In-Depth Series. Addison-
Wesley, 2001.
[5] N. M. Amato, O. B. Bayazit, L. K. Dale, C. Jones, and D. Vallejo.
OBPRM: An obstacle-based PRM for 3D workspaces. In Proc.
third International Workshop on the Algorithmic Foundations of
Robotics, 1998.
[6] N. M. Amato, O. B. Bayazit, L. K. Dale, C. Jones, and D. Vallejo.
Choosing good distance metrics and local planners for probabilistic
roadmap methods. IEEE Transactions on Robotics and Automa-
tion, 16(4):442–447, Aug. 2000.
[7] N. M. Amato and G. Song. Using motion planning to study protein
folding pathways. Journal of Computational Biology, 9(2):149–168,
2002.

230
References
[8] A. L. Ames, D. R. Nadeau, and J. L. Moreland. VRML 2.0 Source-
book. John Wiley & Sons, second edition, 1997.
[9] M. A. Arbib, T. Iberall, and D. Lyons. Schemas that integrate
vision and touch for hand control. In M. A. Arbib and A. R. Hanson,
editors, Vision, Brain, and Cooperative Computation, pages 489–
510. MIT Press, 1987.
[10] A. Autere and J. Lehtinen. Robot motion planning by a hierarchical
search on a modified discretized configuration space. In IEEE/RSJ
International Conference on Intelligent Robots and Systems, vol-
ume 2, pages 1208–1213, Grenoble, France, Sept. 1997.
[11] B. Baginski. Efficient dynamic collision detection using expanded
geometry models. In IEEE/RSJ International Conference on Intel-
ligent Robots and Systems, volume 3, pages 1714–1720, Grenoble,
France, Sept. 1997.
[12] C. B. Barber, D. P. Dobkin, and H. T. Huhdanpaa. The Quickhull
algorithm for convex hulls. ACM Transactions on Mathematical
Software, 22(4):469–483, Dec. 1996.
[13] J. Barraquand, B. Langlois, and J.-C. Latombe. Numerical poten-
tial field techniques for robot path planning. IEEE Transactions
on Systems, Man and Cybernetics, 22(2):224–241, 1992.
[14] O. B. Bayazit, G. Song, and N. M. Amato. Ligand binding with
OBPRM and user input. In IEEE International Conference on
Robotics and Automation, volume 1, pages 954–959, Seuol, Korea,
May 2001.
[15] A. Bicchi, G. Casalino, and C. Santilli. Planning shortest bounded-
curvature paths for a class of nonholonomic vehicles among obsta-
cles. In IEEE International Conference on Robotics and Automa-
tion, volume 2, pages 1349–1354, Nagoya, Japan, May 1995.
[16] A. Blake and M. Isard. Active Contours. Springer-Verlag, 1998.
[17] R. Bohlin and L. E. Kavraki. Path planning using lazy PRM.
In IEEE International Conference on Robotics and Automation,
volume 1, pages 521–528, San Francisco, CA, USA, Apr. 2000.

References
231
[18] R. Bohlin and L. E. Kavraki. A randomized algorithm for robot
path planning based on lazy evaluation.
In S. Rajasekaran,
P. Pardalos, J. Reif, and J. Rolim, editors, Handbook on randomized
computing. Kluwer Academic Publishers, 2001.
[19] G. M. Bone and Y. Du. Multi-metric comparison of optimal 2D
grasp planning algorithms. In IEEE International Conference on
Robotics and Automation, volume 3, pages 3061–3066, Seoul, Ko-
rea, May 2001.
[20] V. Boor, M. H. Overmars, and A. F. van der Stappen. The Gaussian
sampling strategy for probabilistic roadmap planners. In IEEE
International Conference on Robotics and Automation, volume 2,
pages 1018–1023, Detroit, MI, USA, May 1999.
[21] J. Borenstein, H. R. Everett, and L. Feng.
Navigating Mobile
Robots: Systems and Techniques. A. K. Peters, Ltd., 1996.
[22] C. Borst, M. Fischer, and G. Hirzinger. Grasp planning: How
to choose a suitable task wrench space. In IEEE International
Conference on Robotics and Automation, New Orleans, LA, USA,
Apr. 2004.
[23] M. S. Branicky, S. M. LaValle, K. Olson, and L. Yang. Quasi-
randomized path planning. In IEEE International Conference on
Robotics and Automation, volume 2, pages 1481–1487, Seoul, Ko-
rea, May 2001.
[24] H. Bruyninckx, S. Demey, and V. Kumar. Generalized stability of
compliant grasps. In IEEE International Conference on Robotics
and Automation, volume 3, pages 2396–2402, Leuven, Belgium,
May 1998.
[25] S. Cameron. Collision detection by four-dimensional intersection
testing. IEEE Transactions on Robotics and Automation, 6(3):291–
302, June 1990.
[26] S. Cameron. A comparison of two fast algorithms for computing
the distance between two convex polyhedra. IEEE Transactions on
Robotics and Automation, 13(6):915–920, Dec. 1997.
[27] S. Cameron. Enhancing GJK: Computing minimum and penetra-
tion distances between convex polyhedra. In IEEE International

232
References
Conference on Robotics and Automation, volume 4, pages 3112–
3117, Albuquerque, NM, USA, Apr. 1997.
[28] S. Cameron. Motion planning and collision avoidance with complex
geometry. In Proceedings of the IEEE Industrial Electronics Society,
volume 4, pages 2222–2226, Aachen, Germany, Sept. 1998.
[29] S. Cameron and J. Pitt-Francis. Using OxSim for path planning.
Journal of Robotic Systems, 18(8):421–431, Aug. 2001.
[30] J. A. Castellanos and J. D. Tard´
os. Mobile Robot Localization and
Map Building: A Multisensor Fusion Approach. Kluwer Academic
Publishers, 1999.
[31] K. Chung. An efficient collision detection algorithm for polytopes in
virtual environments. M. Phil. Thesis, Dept. of Computer Science,
University of Hong Kong, Sept. 1996.
[32] J. D. Cohen, M. C. Lin, D. Manocha, and M. Ponamgi.
I-
COLLIDE: An interactive and exact collision detection system for
large-scale environments. In Proc. Symposium on Interactive 3D
Graphics, pages 189–196, Apr. 1995.
[33] J. O. Coplien. Advanced C++: Programming Styles and Idioms.
Addison-Wesley, 1991.
[34] T. H. Cormen, C. E. Leiserson, and R. L. Rivest. Introduction to
Algorithms. The MIT Press, MacGraw-Hill, June 1990.
[35] J. Cort´es, T. Sim´eon, and J. Laumond. A random loop genera-
tor for planning the motions of closed kinematic chains using PRM
methods. In IEEE International Conference on Robotics and Au-
tomation, volume 2, pages 2141–2146, Washington, DC, USA, May
2002.
[36] J. J. Craig.
Introduction to Robotics: Mechanics and Control.
Addison-Wesley, second edition, 1989.
[37] T. Danner and L. E. Kavraki. Randomized planning for short in-
spection paths. In IEEE International Conference on Robotics and
Automation, volume 2, pages 971–976, San Francisco, CA, USA,
Apr. 2000.

References
233
[38] J. de Guzman and D. Nuffer. The Spirit library: Inline parsing in
C++. C/C++ Users Journal, 21:22, Sept. 2003.
[39] J. Denavit and R. S. Hartenberg. A kinematic notation for lower-
pair mechanisms based on matrices. Journal of Applied Mechanics,
22:215–221, 1955.
[40] R. Earnshaw, N. Magnenat-Thalmann, D. Terzopoulos, and
D. Thalmann. Computer animation for virtual humans. IEEE
Computer Graphics and Applications, 18(5):20–23, Sept. 1998.
[41] C. Ferrari and J. Canny. Planning optimal grasps. In IEEE Inter-
national Conference on Robotics and Automation, volume 3, pages
2290–2295, Nice, France, May 1992.
[42] M. Fischer and G. Hirzinger. Fast planning of precision grasps for
3D objects. In IEEE/RSJ International Conference on Intelligent
Robots and Systems, volume 1, pages 120–126, Grenoble, France,
Sept. 1997.
[43] M. Fischer and G. Hirzinger. Fast planning of precision grasps for
three-dimensional objects. Advanced Robotics, 12(5):535–549, 1999.
[44] L. Fl¨
uckiger. Interface pour le pilotage et l’analyse des robots bas´ee
sur un g´en´erateur de cin´ematiques. PhD thesis, ´
Ecole Polytech-
nique F´ed´erale de Lausanne, EPFL, Lausanne, Switzerland, 1998.
[45] E. Gamma, R. Helm, R. Johnson, and J. Vlissides. Design Patterns:
Elements of Reusable Object-Oriented Software. Addison-Wesley
professional computing series. Addison-Wesley, 1995.
[46] R. Geraerts and M. H. Overmars. A comparative study of prob-
abilistic roadmap planners.
Technical Report UU-CS-2002-041,
Dept. of Computer Sci., Utrecht Univ., Utrecht, the Netherlands,
2002.
[47] E. G. Gilbert and S. M. Hong. A new algorithm for detecting the
collision of moving objects. In IEEE International Conference on
Robotics and Automation, volume 1, pages 8–14, Scottsdale, AZ,
USA, May 1989.
[48] E. G. Gilbert, D. W. Johnson, and S. S. Keerthi. A fast proce-
dure for computing the distance between two complex objects in

234
References
three-dimensional space. IEEE Journal of Robotics and Automa-
tion, 4(2):193–203, Apr. 1988.
[49] G. H. Golub and C. F. Van Loan. Matrix Computations. The Johns
Hopkins University Press, second edition, 1989.
[50] F. Gravot, R. Alami, and T. Sim´eon. Playing with several roadmaps
to solve manipulation problems. In IEEE/RSJ International Con-
ference on Intelligent Robots and Systems, volume 3, pages 2311–
2316, EPFL, Lausanne, Switzerland, Oct. 2002.
[51] L. J. Guibas, D. Halperin, H. Hirukawa, J.-C. Latombe, and R. H.
Wilson. A simple and efficient procedure for polyhedral assembly
partitioning under infinitesimal motions. In IEEE International
Conference on Robotics and Automation, volume 3, pages 2553–
2560, Nagoya, Japan, May 1995.
[52] M. W. Hannan and I. D. Walker. The ’elephant trunk’ manipulator,
design and implementation. In IEEE International Conference on
Advanced Intelligent Mechatronics, volume 1, pages 14–19, Como,
Italy, July 2001.
[53] E. R. Harold and W. S. Means. XML in a Nutshell: A Desktop
Quick Reference. O’Reilly, second edition, Jan. 2001.
[54] M. Hernando and E. Gambao. New concept of visibility tetrahe-
dra for fast robot motion planning. In IEEE/ASME International
Conference on Advanced Intelligent Mechatronics, volume 2, pages
1340–1345, Como, Italy, July 2001.
[55] M. Hernando and E. Gambao. Visibility analysis and genetic al-
gorithms for fast robot motion planning. In IEEE/RSJ Interna-
tional Conference on Intelligent Robots and Systems, volume 3,
pages 2413–2418, EPFL, Lausanne, Switzerland, Oct. 2002.
[56] C. Holleman and L. E. Kavraki.
A framework for using the
workspace medial axis in PRM planners. In IEEE International
Conference on Robotics and Automation, volume 2, pages 1408–
1413, San Francisco, CA, USA, Apr. 2000.
[57] W. S. Howard and V. Kumar. On the stability of grasped objects.
IEEE Transactions on Robotics and Automation, 12(6):904–917,
Dec. 1996.

References
235
[58] D. Hsu, J.-C. Latombe, and S. Sorkin. Placing a robot manipula-
tor amid obstacles for optimized execution. In IEEE International
Symposium on Assembly and Task Planning, pages 280–285, Porto,
Portugal, July 1999.
[59] G. Immega and K. Antonelli. The KSI tentacle manipulator. In
IEEE International Conference on Robotics and Automation, vol-
ume 3, pages 3149–3154, Nagoya, Japan, May 1995.
[60] P. Isto. Path planning by multiheuristic search via subgoals. In
Proc. 27th Int. Symposium on Industrial Robots, pages 712–726,
1996.
[61] S. C. Johnson. Yacc: Yet another compiler-compiler. Technical
Report Computing Science Technical Report No. 32, Bell Labora-
tories, Murray Hill, New Jersey, 1975.
[62] L. Jones, Joseph and T. Lozano-P´erez.
Planning two-fingered
grasps for pick-and-place operations on polyhedra. In IEEE Inter-
national Conference on Robotics and Automation, volume 1, pages
683–688, Cincinnati, OH, May 1990.
[63] L. E. Kavraki and J.-C. Latombe. Randomized preprocessing of
configuration space for fast path planning. In IEEE International
Conference on Robotics and Automation, volume 3, pages 2138–
2145, San Diego, CA, USA, May 1994.
[64] L. E. Kavraki, P. ˇ
Svestka, J.-C. Latombe, and M. H. Overmars.
Probabilistic roadmaps for path planning in high-dimensional con-
figuration spaces. IEEE Transactions on Robotics and Automation,
12(4):566–580, Aug. 1996.
[65] M. Kazemi and M. Mehrandezh. Robot navigation using harmonic
function-based probabilistic roadmaps. In IEEE International Con-
ference on Robotics and Automation, volume 5, pages 4765–4770,
New Orleans, LA, USA, Apr. 2004.
[66] J. Kerr and B. Roth. Analysis of multifingered hands. The Inter-
national Journal of Robotics Research, 4(4):3–17, 1986.
[67] W. Khalil and E. Dombre. Modeling, identification & control of
robots. Hermes Penton Science, 2002.

236
References
[68] W. Khalil and J. F. Kleinfinger. A new geometric notation for
open and closed-loop robots. In IEEE International Conference on
Robotics and Automation, volume 3, pages 1174–1179, 1986.
[69] D. Kirkpatrick, B. Mishra, and C. K. Yap. Quantitative Steinitz’s
theorems with applications to multifingered grasping. In Proceed-
ings of the 20th ACM Symposium on Theory of Computing, pages
341–351, 1990.
[70] J. J. Kuffner. Effective sampling and distance metrics for 3D rigid
body path planning. In IEEE International Conference on Robotics
and Automation, volume 4, pages 3993–3998, New Orleans, LA,
USA, Apr. 2004.
[71] J. J. Kuffner Jr. Autonomous Agents for Real-Time Animation.
PhD thesis, Stanford University, Stanford, CA, USA, Dec. 1999.
[72] J. J. Kuffner Jr. and S. M. LaValle. RRT-Connect: An efficient
approach to single-query path planning. In IEEE International
Conference on Robotics and Automation, volume 2, pages 995–1001,
San Francisco, CA, USA, Apr. 2000.
[73] J. B. Kuipers. Quaternions and Rotation Sequences. Princeton
University Press, 1999.
[74] F. Lamiraux, E. Ferr´e, and E. Vall´ee. Kinodynamic motion plan-
ning: Connecting exploration trees using trajectory optimization
methods. In IEEE International Conference on Robotics and Au-
tomation, volume 4, pages 3987–3992, New Orleans, LA, USA, May
2004.
[75] P. T. Lansbury Jr. Evolution of amyloid: What normal protein
folding may tell us about fibrillogenesis and disease. In Proc. Natl.
Acad. Sci. USA, volume 96, pages 3342–3344, Mar. 1999.
[76] E. Larsen, S. Gottschalk, M. C. Lin, and D. Manocha. Fast distance
queries with rectangular swept sphere volumes. In IEEE Interna-
tional Conference on Robotics and Automation, volume 4, pages
3719–3726, San Fransisco, CA, Apr. 2000.
[77] J.-C. Latombe. Robot Motion Planning. Kluwer Academic Pub-
lishers, 1991.

References
237
[78] S. M. LaValle. Rapidly-exploring random trees: A new tool for path
planning. Technical Report TR 98-11, Computer Science Dept.,
Iowa State Univ., Oct. 1998.
[79] S. M. LaValle.
Planning Algorithms.
2004.
available at
http://msl.cs.uiuc.edu/planning.
[80] S. M. LaValle and J. E. Hinrichsen.
Visibility-based pursuit-
evasion: The case of curved environments. IEEE Transactions on
Robotics and Automation, 17(2):196–202, Apr. 2001.
[81] S. M. LaValle and J. J. Kuffner Jr. Randomized kinodynamic plan-
ning. In IEEE International Conference on Robotics and Automa-
tion, volume 1, pages 473–479, Detroit, MI, USA, May 1999.
[82] S. M. LaValle and J. J. Kuffner Jr. Rapidly-exploring random
trees: Progress and prospects. In In Workshop on the Algorithmic
Foundations of Robotics, 2000.
[83] S. M. LaValle, D. Lin, L. J. Guibas, J.-C. Latombe, and R. Mot-
wani. Finding an unpredictable target in a workspace with obsta-
cles. In IEEE International Conference on Robotics and Automa-
tion, volume 1, pages 737–742, Albuquerque, NM, USA, Apr. 1997.
[84] L.-Q. Lee, J. G. Siek, and A. Lumsdaine. The generic graph com-
ponent library. In ACM SIGPLAN conference on Object-oriented
programming, systems, languages, and applications, pages 399–414,
Denver, CO, USA, 1999.
[85] M. E. Lesk and E. Schmidt. Lex – a lexical analyzer generator.
Technical Report Computing Science Technical Report No. 39, Bell
Laboratories, Murray Hill, New Jersey, July 1975.
[86] E. Levey, C. Peters, and C. O’Sullivan. New metrics for evalua-
tion of collision detection techniques. Technical Report TCD-CS-
1999-55, Dept. of Computer Sci., Univ. of Dublin, Trinity College,
Dublin, Ireland, Nov. 1999.
[87] J. R. Levine, T. Mason, and D. Brown. lex & yacc. O’Reilly, second
edition, Oct. 1992.
[88] Z. Li and S. S. Sastry. Task-oriented optimal grasping by multi-
fingered robot hands. IEEE Journal of Robotics and Automation,
4(1):32–44, Feb. 1988.

238
References
[89] M. C. Lin and J. F. Canny. A fast algorithm for incremental dis-
tance calculation. In IEEE International Conference on Robotics
and Automation, pages 1008–1014, Sacramento, CA, Apr. 1991.
[90] Q. Lin, J. W. Burdick, and E. Rimon. A stiffness-based quality
measure for compliant grasps and fixtures. IEEE Transactions on
Robotics and Automation, 16(6):675–688, Dec. 2000.
[91] F. Lingelbach. Path planning using probabilistic cell decomposi-
tion. In IEEE International Conference on Robotics and Automa-
tion, volume 1, pages 467–472, New Orleans, LA, USA, Apr. 2004.
[92] T. Lozano-P´erez.
Spatial planning: A configuration space ap-
proach. IEEE Transactions on Computers, C-32(2):108–120, Feb.
1983.
[93] T. Lozano-P´erez, J. L. Jones, E. Mazer, P. O´Donnel, W. E. L.
Grimson, P. Tournassoud, and A. Lanusse. Handey: A robot sys-
tem that recognizes, plans, and manipulates. In IEEE International
Conference on Robotics and Automation, volume 4, pages 843–849,
Mar. 1987.
[94] L. Lu and S. Akella. Folding cartons with fixtures: A motion plan-
ning approach. IEEE Transactions on Robotics and Automation,
16(4):346–356, Aug. 2000.
[95] G. Mantriota. Communication on optimal grip points for con-
tact stability.
The International Journal of Robotics Research,
18(5):502–513, May 1999.
[96] X. Markenscoff and C. H. Papadimitriou. Optimum grip of a poly-
gon. The International Journal of Robotics Research, 8(2):17–29,
Apr. 1989.
[97] K. Mehlhorn and S. N¨
aher. LEDA: A Platform for Combinatorial
and Geometric Computing. Cambridge University Press, Nov. 1999.
[98] S. Meyers. More Effective C++. Addison-Wesley, 1996.
[99] A. T. Miller. GraspIt!: A Versatile Simulator for Robotic Grasping.
PhD thesis, Department of Computer Science, Columbia Univer-
sity, June 2001.

References
239
[100] A. T. Miller and P. K. Allen. Examples of 3D grasp quality com-
putations. In IEEE International Conference on Robotics and Au-
tomation, volume 2, pages 1240–1246, Detroit, MI, USA, May 1999.
[101] A. T. Miller and P. K. Allen. GraspIt!: A versatile simulator for
grasp analysis. In Proceedings ASME International Mechanical En-
gineering Congress & Exposition, pages 1251–1258, Orlando, FL,
USA, Nov. 2000.
[102] A. T. Miller, S. Knoop, H. I. Christensen, and P. K. Allen. Au-
tomatic grasp planning using shape primitives. In IEEE Interna-
tional Conference on Robotics and Automation, volume 2, pages
1824–1829, Taipei, Taiwan, Sept. 2003.
[103] B. Mirtich. V-Clip: Fast and robust polyhedral collision detection.
ACM Transactions on Graphics, 17(3):177–208, July 1998.
[104] B. Mirtich and J. Canny. Easily computable optimum grasps in
2-D and 3-D. In IEEE International Conference on Robotics and
Automation, volume 1, pages 739–747, San Diego, CA, USA, May
1994.
[105] A. Morales, P. J. Sanz, and A. P. del Pobil. Vision-based com-
putation of three-finger grasps on unknown planar objects.
In
IEEE/RSJ International Conference on Intelligent Robots and Sys-
tems, volume 2, pages 1711–1716, EPFL, Lausanne, Switzerland,
Oct. 2002.
[106] A. Morales, P. J. Sanz, A. P. del Pobil, and A. H. Fagg. An ex-
periment in constraining vision-based finger contact selection with
gripper geometry. In IEEE/RSJ International Conference on In-
telligent Robots and Systems, volume 2, pages 1693–1698, 2002.
[107] R. M. Murray, Z. Li, and S. S. Sastry. A Mathematical Introduction
to Robotic Manipulation. CRC Press, 1994.
[108] V.-D. Nguyen. Constructing force-closure grasps. The International
Journal of Robotics Research, 7(3):3–16, June 1988.
[109] C. L. Nielsen and L. E. Kavraki. A two level fuzzy PRM for ma-
nipulation planning. In IEEE/RSJ International Conference on
Intelligent Robots and Systems, volume 3, pages 1716–1721, Taka-
matsu, Japan, Nov. 2000.

240
References
[110] C. Nissoux, T. Sim´eon, F. Lamiraux, J. Cortes, and C. van Geem.
Move3D Programming Manual. LAAS-CNRS, Groupe Robotique
et Intelligence Artificielle, Toulouse, France, Mar. 2001.
[111] C. J. Ong and E. G. Gilbert. The Gilbert-Johnson-Keerthi algo-
rithm: A fast version for incremental motions. In IEEE Interna-
tional Conference on Robotics and Automation, pages 1183–1189,
Albuquerqe, NM, USA, Apr. 1997.
[112] C. J. Ong and E. G. Gilbert. Fast versions of the Gilbert-Johnson-
Keerthi distance algorithm: Additional results and comparisons.
IEEE Transactions on Robotics and Automation, 17(4):531–539,
Aug. 2001.
[113] G. Oriolo, M. Ottavi, and M. Vendittelli. Probabilistic motion
planning for redundant robots along given end-effector paths. In
IEEE/RSJ International Conference on Intelligent Robots and Sys-
tems, volume 2, pages 1657–1662, EPFL, Lausanne, Switzerland,
Oct. 2002.
[114] T. J. Parr and R. W. Quong. ANTLR: A predicated-LL(k) parser
generator. Software–Practice and Experience, 25(7):789–810, July
1995.
[115] R. P. Paul. Robot manipulators: Mathematics, programming and
control. The computer control of robot manipulators. The MIT
Press, Cambridge, Massachusetts and London, England, fifth edi-
tion, 1983.
[116] R. P. Paul and H. Zhang. Computationally efficient kinematics for
manipulators with spherical wrists based on the homogenous trans-
formation representation. The International Journal of Robotics
Research, 5(2):32–44, 1986.
[117] L. Petersson, P. Jensfelt, D. Tell, M. Strandberg, D. Kragic, and
H. I. Christensen. Systems integration for real-world manipulation
tasks. In IEEE International Conference on Robotics and Automa-
tion, volume 3, pages 2500–2505, Washington, DC, USA, May 2002.
[118] J. Pettr´e, T. Sim´eon, and J. Laumond. Planning human walk in
virtual environments. In IEEE/RSJ International Conference on
Intelligent Robots and Systems, volume 3, pages 3048–3053, EPFL,
Lausanne, Switzerland, Oct. 2002.

References
241
[119] N. S. Pollard.
Parallel Methods for Synthesizing Whole-Hand
Grasps from Generalized Prototypes. PhD thesis, Department of
Electrical Engineering and Computer Science, Massachusetts Insti-
tute of Technology, 1994.
[120] J. Ponce and B. Faverjon. Computing three-finger force-closure
grasps of polygonal objects. IEEE Transactions on Robotics and
Automation, 11(6):868–881, Dec. 1995.
[121] F. P. Preparata and M. I. Shamos. Computational Geometry: An
Introduction, chapter 6.4. Springer-Verlag, 1985.
[122] C. Qin, S. Cameron, and A. McLean. Towards efficient motion
planning for manipulators with complex geometry. In Proc. IEEE
International Symposium on Assembly and Task Planning, pages
207–212, Pittsburg, PA, USA, Aug. 1995.
[123] J. A. Reeds and L. A. Shepp. Optimal paths for a car that goes
both forwards and backwards. Pacific Journal of Mathematics,
145(2):367–393, 1990.
[124] M. Reggiani, M. Mazzoli, and S. Caselli. An experimental evalua-
tion of collision detection packages for robot motion planning. In
IEEE/RSJ International Conference on Intelligent Robots and Sys-
tems, volume 3, pages 2329–2334, EPFL, Lausanne, Switzerland,
Oct. 2002.
[125] J. H. Reif. Complexity of the mover’s problem and generalizations.
In Proceedings IEEE Symposium of Foundations of Computer Sci-
ence, pages 421–427, San Juan, Puerto Rico, Oct. 1979.
[126] J. Rumbaugh,
M. Blaha,
W. Premerlani,
F. Eddy,
and
W. Lorensen. Object-Oriented Modeling and Design. Prentice Hall,
1991.
[127] J. Rumbaugh, I. Jacobson, and G. Booch. The Unified Modeling
Language Reference Manual. Addison-Wesley, second edition, 2004.
[128] A. Sahbani, J. Cort´es, and J. Cort´es. A probabilistic algorithm for
manipulation planning under continuous grasps and placements. In
IEEE/RSJ International Conference on Intelligent Robots and Sys-
tems, volume 2, pages 1560–1565, EPFL, Lausanne, Switzerland,
Oct. 2002.

242
References
[129] Y. Sato, M. Hirata, T. Maruyama, and Y. Arita. Efficient collision
detection using fast distance-calculation algorithms for convex and
non-convex objects. In IEEE International Conference on Robotics
and Automation, volume 1, pages 771–778, Minneapolis, MN, USA,
Apr. 1996.
[130] P. N. Sheth and J. J. Uicker. A generalized symbolic notation for
mechanisms. J. of Engineering for Industry, Transactions of the
ASME, 93:102–112, 1971.
[131] K. Shoemake. Animating rotations with quaternion curves. In
Proceedings of SIGGRAPH ’85, pages 245–254, San Francisco, CA,
USA, July 1985.
[132] K. Shoemake. Graphics Gems III, chapter Uniform Random Ro-
tations, pages 124–132. Academic Press, San Diego, CA, USA,
1992.
[133] J. G. Siek, L.-Q. Lee, and A. Lumsdaine. The Boost Graph Library.
The C++ In-Depth Series. Addison-Wesley, 2002.
[134] T. Sim´eon, J. Cort´es, A. Sahbani, and J.-P. Laumond. A manipula-
tion planner for pick and place operations under continuous grasps
and placements. In IEEE International Conference on Robotics and
Automation, volume 2, pages 2022–2027, Washington, DC, May
2002.
[135] T. Sim´eon, J.-P. Laumond, and F. Lamiraux. Move3D: a generic
platform for path planning. In IEEE International Symposium on
Assembly and Task Planning, pages 25–30, Fukuoka, Japan, May
2001.
[136] T. Sim´eon, J.-P. Laumond, and F. Lamiraux. Towards a software
development kit for motion synthesis in virtual worlds. In IEEE In-
ternational Conference on Virtual Systems and Multimedia, pages
854–863, Berkely, CA, USA, Oct. 2001.
[137] T. Sim´eon, J.-P. Laumond, and C. Nissoux. Visibility based prob-
abilistic roadmaps for motion planning. Advanced Robotics, 14(6),
2000.
[138] B. H. Simov, G. Slutzki, and S. M. LaValle. Pursuit-evasion using
beam detection. In IEEE International Conference on Robotics and

References
243
Automation, volume 2, pages 1657–1662, San Francisco, CA, USA,
Apr. 2000.
[139] M. H. Singer. A general approach to moment calculation for poly-
gons and line segments. Pattern Recognition, 26(7):1019–1028, Jan.
1993.
[140] G. Smith, E. Lee, K. Goldberg, K. B¨
ohringer, and J. Craig. Com-
puting parallel-jaw grips. In IEEE International Conference on
Robotics and Automation, volume 3, pages 1897–1903, Detroit, MI,
USA, May 1999.
[141] G. Song and N. M. Amato. Randomized motion planning for car-
like robots using C-PRM. In IEEE/RSJ International Conference
on Intelligent Robots and Systems, volume 1, pages 37–42, Maui,
HI, USA, Nov. 2001.
[142] 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.
[143] 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.
[144] 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.
[145] S. Sundaram, I. Remmler, and N. M. Amato. Disassembly sequenc-
ing using a motion planning approach. In IEEE International Con-
ference on Robotics and Automation, volume 2, pages 1475–1480,
Seoul, Korea, May 2001.
[146] M. Suppa, P. Wang, K. Gupta, and G. Hirzinger. C-space explo-
ration using noisy sensor models. In IEEE International Conference
on Robotics and Automation, volume 5, pages 4777–4782, New Or-
leans, LA, USA, Apr. 2004.
[147] P. ˇ
Svestka and M. H. Overmars. Coordinated motion planning
for multiple car-like robots using probabilistic roadmaps. In IEEE
International Conference on Robotics and Automation, volume 2,
pages 1631–1636, Nagoya, Japan, May 1995.

244
References
[148] M. Teichmann. A grasp metric invariant under rigid motions. In
IEEE International Conference on Robotics and Automation, vol-
ume 3, pages 2143–2148, Minneapolis, MN, USA, Apr. 1996.
[149] C.-H. Tsai, J.-S. Lee, and J.-H. Chuang. Path planning of 3-D
objects using a new workspace model. In IEEE International Sym-
posium on Computational Intelligence in Robotics and Automation,
pages 420–425, Alberta, Canada, July 2001.
[150] G. van den Bergen. A fast and robust GJK implementation for
collision detection of convex objects. Journal of Graphics Tools,
4(2):7–25, 1999.
[151] C. Van Geem. On using a Manhattan distance-like function for
robot motion planning on a non-uniform grid in configuration space.
Technical Report 94-74a, Research Inst. for Symbolic Computation,
Joh. Kepler Univ., Linz, Austria, 1994.
[152] M. Vendittelli, J.-P. Laumond, and C. Nissoux. Obstacle distance
for car-like robots. IEEE Transactions on Robotics and Automa-
tion, 15(4):678–691, Aug. 1999.
[153] A. Watt. 3D Computer Graphics. Addison-Wesley, third edition,
2000.
[154] J. Wernecke. The Inventor Mentor. Addison-Wesley, 1994.
[155] S. A. Wilmarth, N. M. Amato, and P. F. Stiller. MAPRM: A
probabilistic roadmap planner with sampling on the medial axis of
the free space. In IEEE International Conference on Robotics and
Automation, volume 2, pages 1024–1031, Detroit, MI, USA, May
1999.
[156] G. Xavier, Patrick. Fast swept-volume distance for robust colli-
sion detection. In IEEE International Conference on Robotics and
Automation, volume 2, pages 1162–1169, Albuquerque, NM, USA,
Apr. 1997.
[157] L. Yang and S. M. LaValle. An improved neighborhood graph
approach. In IEEE International Conference on Robotics and Au-
tomation, volume 1, pages 254–259, Washington, DC, USA, May
2002.

---

Document Outline

• Abstract
• Acknowledgments
• Contents
• 1 Introduction
• 2 A Framework for Path Planning
• 3 A General Robot Model for Path Planning
• 4 Augmenting RRT-Planners with Local Trees
• 5 Planning of Pick-and-Place Tasks
• 6 Grasp Planning for a Three-Fingered Hand
• Chapter 7 Grasp Stability Evaluation
• 8 Summary and Suggestions for Future Work
• Appendix A Class Diagram Notation
• Appendix B Rigid Body Transformations
• Appendix C Geometric Representations
• Appendix D Pairwise Collision Detection with PQP and GJK
• Appendix E Framework Details
• Appendix F The Boost Libraries
• References