Robot Path Planning: An Object-Oriented Approach

Collision Detection/
Distance Computation
Planner
- metrics
- interpolation 
- sampling
Models of Motion 
- rigid body 
- kinematic chain 
move objects 
change configuration 
collisions/distances
Geometric Representations
- triangle sets 
- convex polyhedra 
- octrees 
position and 
orientation
Figure 2.1:
Illustration of the main components that are involved when
solving a path planning problem. The arrows show the flow of infor-
mation for a typical implementation. The dashed line indicates a more
passive relationship; the geometric objects that need to be checked for
collisions are usually registered once at startup to the object responsible
for collision detection.
2.2
Framework Overview
As mentioned in the introduction to this chapter, contributions in the
path planning field tend to be variations of concepts. Thus, the concepts
are stable, whereas their implementation is not. The CoPP framework
can be seen as a collection of a number of, largely, orthogonal concepts
that have proven useful in the field of path planning. To get an overview
of the framework, we have grouped the concepts into four categories,
as shown in Figure 2.1. The categories are: geometric representation,
collision detection, motion generation, and planner concepts. These cat-
egories can also be seen as modules in a path planning application, where
the arrows in Figure 2.1 show the typical flow of information between the
modules.
Geometric Representation
As path planning involves moving geo-
metric objects, one of the most basic issues is how we should represent
the geometry of the world and the objects that move in it. Typically,

2.2 Framework Overview
17
the chosen representation is tightly coupled to the method used for col-
lision detection, but here we want to see the geometric representation as
a concept that can vary as freely as possible. At the highest level, as
described by Figure 2.1, we only require a geometric object to have have
a few basic properties such as a position, an orientation, and a bounding
box, determining the object’s extent in space. We make no distinction
between stationary and moving objects; all objects are seen as moveable.
More details on geometric representations can be found in Section 2.3.
Models of Motion
In addition to a description of the environment, a
path planning problem also involves one or more moving agents, denoted
by A in the case of a single agent. An agent can be any moving system,
ranging from a single rigid body or a robot manipulator, to a full-blown
dynamic model of an aircraft. The point here is that with the proposed
model for geometric representation, an agent is totally decoupled from
the method we use for representing geometric objects; the agent simply
forwards its current position and orientation to the geometric objects that
model its physical parts, see Figure 2.1. From this perspective, agents
are simply rules for generating motion. In this thesis, we focus on two
types of agents, rigid bodies and robots that can be described by tree-like
kinematic chains. The robot model is covered in Chapter 3.
Collision Detection
Path planning algorithms depend heavily on
methods for collision detection and the efficiency of the collision detection
algorithm that is used can greatly affect the total planning time. To de-
couple the planner from the collision detection algorithm we use concepts
like distance engines, which are objects that provide a uniform interface
for various proximity queries. Geometric objects that we want to check
for collisions are typically registered to a distance engine at initialization.
The distance engine can, depending on its type, use the geometric objects
right away, or it may create its own internal representation of them. This
weak relationship between the distance engine and the geometric objects
is indicated with the dashed line in Figure 2.1. We point out that, due to
being totally decoupled from the path planner, the distance engine con-
cept is very useful even in applications unrelated to path planning, e.g.,
in applications for physics simulation, or in computer games. Distance
engines and related concepts are covered in Section 2.4.

18
2 A Framework for Path Planning
Planner Concepts
The last category in Figure 2.1, planner concepts,
is more directly related to path planning than the other categories. Ex-
amples of concepts in this category are:
• Interpolation methods, Section 2.5
• Metrics, Section 2.6
• Sampling strategies, Section 2.7
• Local planning methods, Section 2.8
• Path smoothing, Section 5.7
Design Rationale
Throughout the design of CoPP, we have aimed to
follow sound object-oriented techniques to achieve a framework that is:
• Easy to use
• Easy to extend
• Loosely coupled
• Portable
• Efficient
Among the techniques and principles we have used are the design pat-
terns in [45]. We have for example used the Template Method pattern to
automate (from the point of view of the derived classes) the gathering of
useful statistics, such as the number of collision checks.
For each concept mentioned in this section there is a corresponding
class hierarchy. The result is a set of small, loosely coupled class hier-
archies, where each hierarchy ideally has one well-defined responsibility,
e.g., interpolation between two configurations. Each class hierarchy orig-
inates from an abstract class, whose only purpose is to define a uniform
interface to the modeled concept. The idea is that planners that only refer
to the abstract interface of a concept can be reconfigured with any vari-
ation of that concept. Thus, making it easy to perform fair comparisons
between variations of a concept, which was one of the main motivations
behind the framework. An additional advantage is that we have in effect
got a multitude of planners at the price of one; just a reconfigure the
planner with another combination of concepts.

2.3 Dealing with Geometry
19
CoPP is implemented in C++, and in addition to general object-
oriented techniques, CoPP also uses some C++ specific features such
as templates and overloading. These two features in combination have
helped to provide generic solutions in areas where new concepts are likely
to be added. These generic solutions make the framework easy to extend.
Experience has shown that the portability problems for existing
frameworks, such as MSL, are mostly due to the choice of a platform
specific graphics API. To minimize such platform dependencies, we have
chosen to use the platform independent VRML [8] language for visual-
ization (see Appendix E.4).
To visualize the design, we use a variant of the class diagram nota-
tion introduced by Rumbaugh et al. [126]. A brief introduction to the
class diagram notation, together with some added conventions, is given
in Appendix A.
External Dependencies
Any software project of moderate size will,
unless we are willing to reinvent the wheel over and over again, depend on
a number of external libraries. It is desirable, however, to keep the num-
ber of external dependencies as small as possible. The Boost libraries
5
are free peer-reviewed C++ libraries of high quality. We have found that
Boost contains a lot of functionalities that are useful in the context of
path planning. Examples are: a wide variety of random number gen-
erators, a parser generator framework, and a library of generic graph
functions [133]. Thus, using Boost we can localize a lot of functionality
to a single external dependency. The usage of Boost within CoPP is
described in Appendix F.
Other external dependencies are the collision detection algorithms
PQP [76] and Enhanced GJK [27], and the Qhull program [12] for com-
puting convex hulls.
2.3
Dealing with Geometry
A study of proposed planners in the literature and path planning soft-
ware shows that the way geometry is represented is often tightly coupled
to the collision-detection algorithm that is used. Indeed, much work has
focused on finding suitable data structures for fast collision detection al-
gorithms. To be able to visualize the planner results, the data structure
5
www.boost.org

20
2 A Framework for Path Planning
must also be suitable for rendering. It turns out that these two require-
ments often are incompatible, so many path planners use two set of data
structures for each geometric object; one for collision detection and one
for visualization. For some planners, the planning method itself requires
adding “embellishments” to an otherwise standard geometry represen-
tation. Examples of this are workspace potential field methods, which
require each geometric object to have a set control points attached to it,
see, e.g., Tsai et al. [149] and Barraquand et al. [13]. At each configura-
tion, the forces resulting from the potential field are summed over all the
control points to give a resultant force and torque acting on the object.
Other examples are the method of Holleman and Kavraki [56] to sample
the medial axis of the workspace and the visibility tetrahedron concept of
Hernando and Gambao [54, 55], which also need special points attached
to each geometry. In the planning method suggested by Baginski [11],
each link of the robot is covered with several protective layers of increas-
ing thickness. These layers are used both to speed up collision checks
along path segments and to provide an estimate of the penetration depth
in case of a collision.
It is clear from the above that testing different collision detection algo-
rithms and developing new planning methods is difficult if the path plan-
ner framework restricted to one single geometric representation. There-
fore it was decided that CoPP should support several geometric represen-
tations through an extensible class hierarchy, where all geometric objects
inherit from a single base class.
2.3.1
Requirements on Geometric Types
Deciding which properties should be common to all geometric types is an
important design decision, as these properties form the base class inter-
face. However, as each added property in general impose a constraint,
too many of them will lead to a constrained hierarchy that only allow
certain types of geometric representations. We have found that once a
path planner is initialized, i.e., geometries are loaded and collision detec-
tion is setup, geometries are mostly moved around and queried for their
current position and orientation, which is in agreement with the flow of
information in Figure 2.1. Based on this observation, we require that
all geometric objects must be moveable and that they know about their
position and orientation relative a world frame. Furthermore, we have
also found that it is often useful to know about the spatial extent of a
given geometry. Therefore the base class interface will support queries

2.3 Dealing with Geometry
21
Geom
GetPose( )
Move(Transform t)
Move(Vector args)
SetMoveFormat(format)
GetBoundingBox(…)
AttachToFrame(…)
Accept(GeomVisitor v)
material
Material
Transform
pose
Figure 2.2:
Class diagram for the geometry base class.
about the (oriented) bounding box of the geometry. The bounding box
can be used to speedup collision detection, since we can quickly discard
pairs of geometries whose bounding boxes do not overlap. It is also used
to estimate the displacement of a robot link as the robot moves from one
configuration to another (see Section 2.6 on metrics).
We have found that these requirements form a base class interface that
is rich enough to be useful, yet imposes hardly no constraints on which
geometric representations we can use. The base class of all geometric
types is called Geom and it is shown in a class diagram in Figure 2.2.
From Figure 2.2 we can see that the base class declares a virtual
function Accept that takes a single argument of type GeomVisitor. This
is a prerequisite for the Visitor pattern [45], whose intent can be described
as adding new virtual methods to a class hierarchy without modifying
the hierarchy. The methods we want to add are put in a class that
derives from GeomVisitor. An example of the Visitor pattern is given
in Appendix E, where it is used to determine how to draw a geometric
object.
For visualization purposes, geometric objects also have material prop-
erties. The class Material in Figure 2.2 has the same type of data as the
material descriptions used in, e.g., VRML [8] and Open Inventor [154].
It can be used to model a wide range of effects such as shininess, specu-
lar color and transparency. Even though only a subset of these settings
are used most of the time, we have found that the transparency setting
is particularly useful for visualizing trajectories or hierarchically com-
posed geometric objects. See Figure 5.14 in Chapter 5 or Figure C.5 in
Appendix C for examples.

22
2 A Framework for Path Planning
2.3.2
Moving Objects Around
The most important property of the geometric types used in CoPP is
that they can be moved around. Moving an object means that we specify
a position and an orientation for it. This operation is straightforward,
but it is not so clear how the interface for the operation should look like.
This difficulty is mainly because there are so many ways to represent
rotations. We could, for example, use rotation matrices, quaternions, or
one of the many Euler angle set conventions, see Appendix B. Which
representation is most effective depend both on the representation used
internally by the path planner and on the type of path planning problem:
As pointed out by Kuffner [70], the choice of rotation representation can
have a big impact on the required planning time for rigid body problems.
For these reasons we do not want to restrict ourselves to one specific
format for moving objects around.
In Figure 2.2 we can see that the base class interface has two functions
named Move. In the first function, the argument is a Transform object,
see Appendix B.1, thus the orientation is specified by a rotation matrix.
The format used by the second function is less clear as its only argument
is a vector. How the contents of this vector is interpreted depends on an
internal parameter of the geometric object. Depending on the value of
this parameter, which can be changed through the SetMoveFormat, the
vector can be interpreted as Euler angles, as a rotation axis and an angle,
or as a quaternion. Any additional elements in the vector are interpreted
as translations.
With this approach, we get a uniform interface for all but one of the
formats, resulting in several benefits. First, without any constraints ge-
ometric objects behave as free-flying rigid bodies. Thus, for rigid body
problems, it is possible to use geometric objects right away, without inter-
vention of an object containing the equations of motion, as in Figure 2.1.
Second, it is easy to experiment with different formats to see which one is
most efficient. Finally, the second Move function has the same signature
as that of the robot class in Chapter 3. This similarity in the interfaces
opens the possibility to write C++ class templates that accept any type
of moving agent, as long as it has a function named Move with a match-
ing signature. Examples of this approach are given in Figure 2.11 and
Figure E.2.
So far we have only seen direct methods for moving geometric objects;
direct in the sense that the client directly communicates with the object
to be moved. We have found that moving objects this way can be both

2.3 Dealing with Geometry
23
inefficient and limiting for some type of planners. Therefore there is also
an indirect method for moving objects. This method uses the concept
of moving coordinate frames, to which we can attach any number of
geometric objects. Once an object is attached to a frame, it will move
along with the frame. As an example, we can think of a robot manipulator
with a moving coordinate frame at each joint. To make the robot’s links
move we just attach them to the corresponding frame. When we ask
a link about its current pose, it will return the pose of the frame it is
connected to.
The member function AttachToFrame takes a single Transform and
attaches the object to it. An overloaded version of this function takes two
Transform
arguments, where the second argument specifies a constant
offset between the moving frame and the object. This second version is
especially convenient in pick-and-place planners, where a grasped object
suddenly moves along with the robot. When an object is grasped, the
following code makes sure it moves correctly together with the robot:
Transform offset = robot.GetEndEffectorPose().Inverse() *
grasped_object.GetPose();
grasped_object.AttachToFrame(robot.GetEndEffectorPose(),
offset);
The second argument specifies the pose of the grasped object relative
the end effector, and is obtained from the inverse of Equation (B.6) in
Appendix B.
2.3.3
Concrete Geometric Types
With the base class in place, we can start defining concrete geometric
types with representations like polygon meshes, B´ezier patches, construc-
tive solid geometry, octrees, or any other representation that serves our
purposes. Currently there are three concrete geometric types in CoPP:
triangle sets, convex polyhedra, and objects that are hierarchies of con-
vex polyhedra. The reason for choosing these geometric types is that
most available collision detection algorithms can be divided in two major
classes: those that work on triangle sets, and those that work on convex
polyhedra. Thus, to support common collision detection algorithms, tri-
angle sets and convex polyhedra was a natural first choice to experiment
with. As the class of convex polyhedra is rather restricted, we also added
a type that can handle non-convex objects that can be decomposed into

24
2 A Framework for Path Planning
a finite set of convex polyhedra. Details about these classes can be found
in Appendix C.
To make it easy to construct rather complex environments and ob-
jects, we have developed a VRML-like file format in which geometric
objects can be defined. The parser for this format was developed with
Spirit parser generator framework. Spirit is part of the Boost libraries
and is briefly described in Appendix F.
2.4
Collision Detection
Collision detection is a vital part of any path planner. Furthermore,
because path planners spend most of their time on collision or distance
queries, the efficiency of the collision detection algorithm will greatly af-
fect the overall efficiency of the planner. In this section, we will review
some proposed collision-detection and distance-computation algorithms.
We will also introduce classes whose intent is to provide a uniform inter-
face for several types of proximity queries. As these classes are totally
decoupled from planner concepts, they could be used in other applications
as well. For example in physics simulations or in computer games.
The basic collision detection problem is about determining wether two
geometric objects share a common region.
6
This formulation lead to algo-
rithms that work on pairs of objects. However, in simulations containing
hundreds or even thousands of objects, a pairwise approach is often im-
practical due to the O(n
2
) cost. Therefore collision detection algorithms
for large scale environments are often divided into two parts: The broad
phase, which identify pairs objects that have to be considered for possible
collisions, and the narrow phase, which perform exact collision checking
on the pairs that were not removed in the broad phase.
The efficiency of collision detection algorithms is very coupled to the
data structures used to model the geometric objects. Over the years,
many different representations for geometric objects have been proposed.
To reduce the spatial complexity, many algorithms uses hierarchical rep-
resentations, see, e.g., Larsen et al. [76]. Other algorithms are efficient
due to special assumptions about the objects, such as convexity. In the
next section we will discuss algorithms that work on convex polyhedra.
6
As pointed out by Cameron in [28], we should distinguish between interference
detection and collision detection; the former checks wether two static objects overlap
in space, whereas the latter checks wether two objects interfere over a whole range of
motions. In this thesis we follow the mainstream and use the term collision detection
for both problems.

2.4 Collision Detection
25
2.4.1
Convex Polyhedra and Collision Detection
Convex polyhedra have been studied extensively in the context of the
minimum distance problem. The reason is that the minimum distance
problem in this case can be cast as a linear programming problem, al-
lowing us to use well-known results from convex optimization. In fact,
it can be shown that the collision detection problem for convex polyhe-
dra can be done in linear time (in terms of the total number of vertices)
in the worst case, see Lin and Canny [89]. However, as we soon will
see, there are several algorithms that, under the assumption of tempo-
ral coherence, exhibit almost-constant time complexity. The algorithms
for convex polyhedra fall into two main categories: simplex-based and
feature-based. The simplex-based algorithms treat the polyhedron as the
convex hull of a point set and perform operations on simplices defined
by subsets of these points. Feature-based algorithms, on the other hand,
treat the polyhedron as a set of features, where the different features are
the vertices, edges and faces of the polyhedron.
Simplex-Based Algorithms
One of the most well-known simplex-based algorithms is the Gilbert-
Johnson-Keerthi (GJK) algorithm [48]. Conceptually, the GJK algorithm
work with the Minkowski difference of the two convex polyhedra. The
Minkowski difference is also a convex polyhedron and the minimum dis-
tance problem is reduced to finding the point in that polyhedron that is
closest to the origin; if the polyhedron includes the origin, then the two
polyhedra intersect. However, forming the Minkowski difference explic-
itly would be a costly approach. Instead GJK work iteratively with small
subsets, simplices, of the Minkowski difference that quickly converge to a
subset that contains the point closest to the origin. For problems involv-
ing continuous motion, the temporal coherence between two time instants
can be exploited by initializing the algorithm with the final simplex from
the previous distance computation. Experiments in [48] showed that the
algorithm has a linear complexity in terms of the total number of vertices,
except for a few degenerate cases.
Since the original paper [48], several improvements on GJK have been
published. Cameron [26, 27] suggested the use of adjacency information
for each vertex to speedup the algorithm. With this adjacency infor-
mation, the algorithm uses hill-climbing to achieve faster convergence.
This variant of GJK is often referred to as Enhanced GJK. Experi-
ments in [26, 27] involving tracking pairs of convex polyhedra showed

26
2 A Framework for Path Planning
that the computational time was close to constant when the total num-
ber of vertices was varied between 10 and 500. This almost-constant
time complexity has been confirmed by Ong and Gilbert [111, 112]. Both
Cameron [26, 27] and Ong and Gilbert [111, 112] point out that this be-
havior only holds for situations with high temporal coherence. Nagle
7
pointed out that there are cases where the GJK algorithm experiences
problems with numerical errors; in situations where two faces, very differ-
ent in size, are almost parallel, the algorithm might end up in an infinite
loop. This was in fact experienced during tests of the pick-and-place
planner in Chapter 5; at certain stages of a task, the robot hand was
moving close to parallel to the table top, causing Enhanced GJK
8
to cy-
cle when computing the distance between the finger links and the table.
In a later version of his implementation of Enhanced GJK
9
, Cameron
addressed this problem by changing the termination condition and also
returning an error parameter. This version has not yet been tested in the
CoPP framework.
The robustness of GJK was also addressed by van den Bergen [150].
Furthermore, efficient caching of repeated dot products tend to make this
the fastest GJK implementation currently available.
To summarize:
• GJK is a fast algorithm for solving the minimum distance problem
for convex polyhedra.
• In applications with strong temporal coherence, repeated distance
computations have an almost-constant time complexity, indepen-
dent of the total number of vertices involved.
• There are only two numerical tolerances and the algorithm is easy
to implement.
• If care is taken to detect cycling, the GJK algorithm is very robust.
It is also interesting to note that R
3
is just a special case for the original
formulation of GJK; it remains valid in higher dimensions as well.
7
J. Nagle. GJK collision detection algorithm wanted. Posted on the
comp.graphics.algorithms newsgroup, Apr. 1998
8
The Oxford version of GJK, version 2.1.
Available at web.comlab.ox.ac.uk/oucl/work/stephen.cameron/distances/
9
version 2.4

2.4 Collision Detection
27
(a)
(b)
(c)
Figure 2.3:
Illustration of the Voronoi regions for a simple geometry.
The figures show, from left to right, the Voronoi regions of a vertex, an
edge, and a face.
Feature-Based Algorithms
A polyhedron can be seen as a set of features, where the different fea-
tures are vertices, edges and faces. The Lin-Canny closest features al-
gorithm [89] traverses the surfaces of two convex polyhedra to find the
closest pair of features. This search is made very efficient because of the
use of precomputed Voronoi regions for each feature. The Voronoi re-
gions basically divides the exterior of each polyhedron into semi-infinite
regions, where each region belongs to a specific feature. Figure 2.3 shows
examples of the three types of Voronoi regions for a simple geometry.
Once the closest features are found and cached, subsequent queries will
run in expected constant time. Even though Lin-Canny is considered to
be among the fastest algorithms for this problem, it has several draw-
backs as pointed by Mirtich [103]. First, it cannot handle the case of
intersecting polyhedra, in which case the algorithm cycle forever. Sec-
ond, other geometrically degenerate situations can cause the algorithm
to cycle as well. Third, the implementation depend on over six numerical
tolerances that have to be tweaked. It should be mentioned though that
the implementation of Cohen et al. [32] is able to handle the intersecting
case also, but at the cost of extra pre-processing.
Addressing the shortcomings of Lin-Canny, Mirtich [103] developed

28
2 A Framework for Path Planning
the V-Clip algorithm. Experiments in [103] show that V-Clip is both
more robust and faster than both Lin-Canny and Enhanced GJK. How-
ever, as pointed out by Levey et al. [86], one often overlooked drawback
of both Lin-Canny and V-Clip is the huge memory footprint of its data
structures. They report that for each edge added to a polyhedron, the
V-Clip data structure requires 168 bytes of storage.
2.4.2
Proximity Queries on Pairs of Objects
At the lowest level, proximity queries deal with pairs of objects. Thus,
although the algorithm for solving a query varies, the concept of a pair
of geometric objects remains invariant. This invariant can be expressed
with an abstract class that specify an interface for proximity queries on
a pair. Concrete classes implement specific algorithms and each object
will have references to two geometric objects. Additional information
that can be stored in such objects is the result of the previous query;
several algorithms utilize such information to obtain almost-constant time
complexity in situations with strong temporal coherence.
The most important issue when designing this abstract class is its
interface; after all, its whole purpose is to define an interface to which
all derived classes must conform. As proximity queries come in several
flavors, we could easily think of many functions that should go into the
interface, e.g., Collides for a binary answer, Distance for the mini-
mum distance, and ClosestPoints for the two points that realize the
minimum distance between the two objects. However, a too fat interface
would severely restrict the types of algorithms we could use and also make
the class hierarchy hard to extend. Some collision detection algorithms,
for example the separating vector algorithm in [31], do not compute dis-
tances, they just give a binary answer depending on wether the objects
collide. With a base class that requires the implementation of distance
queries, such algorithms would be ruled out. Thus, the problem is how
to define a uniform interface for different proximity queries, without im-
posing any constraints on the type of algorithms the class hierarchy can
support. If we rank algorithms according to the amount of information
they provide in the following way:
1. Binary answer queries
2. Minimum distance queries
3. Closest points queries

2.4 Collision Detection
29
DistancePair
+ Collides(tolerance)
+ Distance( )
+ DistanceSqrd( )
- DoCollides(tolerance)
WitnessPair
+ GetClosestPoints(p1, p2)
CollisionPair
+ Collides( )
+ GetNumCollisions( )
- DoCollides(  )
num_collisions
if (DoCollides()) {
++num_collisions;
return true;
}
return false;
Figure 2.4:
Classes that encapsulate proximity queries for pairs of geo-
metric objects.
then it is clear that each rank extends the capabilities of the previous
one. Thus, we have in effect found an inheritance hierarchy. We have
chosen the names of the corresponding classes to be CollisionPair,
DistancePair
, and WitnessPair. The name WitnessPair was chosen
because the two points that realize the minimum distance are often called
witness points in the literature [27]. A class diagram for the resulting
hierarchy is shown in Figure 2.4.
Note that none of the abstract classes have any members that refer-
ence a pair of geometric objects; it is up to the derived classes to decide
which specific type of geometric objects they accept. Still, clients can
access the pair through the base class interface using the pure virtual
functions GetFirst and GetSecond, which all derived classes have to
implement.
The class CollisionPair supports only the binary collision query
through the member function Collides. This function is implemented
using the Template Method pattern [45] to automate the computation of
statistics like the number of collisions detected for a particular pair. The

30
2 A Framework for Path Planning
derived class DistancePair provides an overloaded version of Collides
that takes a tolerance parameter. If the distance between the two objects
is larger than the tolerance, then the function returns true. This is useful
in planning applications that use safety margins. Also, tolerance queries
are often faster and easier to answer than distance queries, see, e.g.,
Larsen et al. [76].
In its current version, the CoPP framework has been tested with
Enhanced GJK [27] for convex polyhedra and PQP [76] for triangle sets.
The classes that encapsulate these two algorithms
10
are shown in the
class diagram in Figure 2.4. Details about the implementation of these
classes can be found in Appendix D.
A useful extension of the class hierarchy in Figure 2.4 is to add classes
that can handle callback functions, i.e., a user-defined action that should
be activated on some event. A callback function could for example com-
pute new velocities for two dynamic objects that collide. Another exam-
ple relates to planning based on potential fields; each time the distance
between a robot link and an obstacle is computed, a callback function
could be called to compute the repulsive force and accumulate it to the
robot link. After all distance computations have been made, each robot
link contains the accumulated forces due to the obstacles.
2.4.3
Classes for Dealing with Sets of Objects
So far we have only discussed how to deal with pairs of geometric objects.
However, at a higher level we must also be concerned about how to deal
with many moving and stationary objects. If the number of objects is
small, then we can use the straightforward approach to do a collision
check on every pair. As the number of objects grow larger, this approach
would quickly become infeasible due to its O(n
2
) complexity in terms
of the number objects. Other methods rely on dividing the workspace
into regions, where each object is assigned to one or more regions. Only
objects that reside in the same region need to be checked against each
other. The collision detection package I-Collide [32] uses a sweep-and-
prune method to quickly rule out pairs that clearly do not intersect. This
method uses the fact that two axis aligned bounding boxes intersect if
and only if their projections on each coordinate axis also intersect. The
complexity is thereby reduced from O(n
2
) to O(n + m), where m is the
number of axis-aligned bounding boxes that do intersect.
10
In case of PQP, the word algorithm is misleading, as it actually is a package
providing different types of proximity queries.

2.4 Collision Detection
31
It turns out that most broad-phase collision detection algorithms
make use of temporal coherence and/or information about the behavior
of the moving objects. If the goal is to simulate moving bodies according
to the law of physics, then valuable information is available in terms of
the velocity and acceleration of each moving object. The problem is that
in path planning applications that only use kinematics, such information
is not available. Furthermore, most path planners tend to have short
periods of strong temporal coherence, i.e. when checking a path between
two samples, followed by bursts of discontinuous jumps, i.e., checking
random samples. This behavior makes it hard to use many of the pro-
posed broad-phase algorithms. That, together with the relatively small
size of most considered problems, is probably the reason why most path
planners still use the simple O(n
2
) approach.
Object Sets and Distance Engines
For simple binary collision checking on a set of objects, we introduce the
abstract class ObjectSet, see Figure 2.5. Its implementation is very sim-
ilar to that of CollisionPair; the main method is IsCollisionFree,
which is also implemented using the Template Method pattern to auto-
mate the computation of statistics.
A more advanced type of object set is one that supports various dis-
tance queries as well. Such an object set is represented by the abstract
class DistEngine, Figure 2.5. The method MinDist returns the mini-
mum distance over all active pairs in the set. The concept of an active
pair is left to the subclasses to define; typically the active pairs would
involve checking all moving objects against each other and all moving
objects against all stationary objects.
At this stage we have not put any constraints on how to register a pair
or an object to these sets. We have not even mentioned the name of any
geometric type yet. So these two classes serve the purpose to introduce
two useful concepts and to establish an interface for these concepts.
As in the case with the pair-based classes, there are also concrete
distance engine classes for specific algorithms. The concrete classes are
DistEnginePQP
and DistEngineGJK, which are both very similar to their
pair-based versions, see Appendix D. Both classes use straightforward
pairwise collision checking, so no broad-phase techniques are implemented
yet. The interface for registering geometric objects for collision detection
is similar; both classes have a method AddPair, but the types of the
arguments differ. In the case of DistEnginePQP, the method is a tem-

32
2 A Framework for Path Planning
++num_calls;
free = DoIsCollisionFree();
time_used += ElapsedTime();
return free;
ObjectSet
+ IsCollisionFree( ) 
+ GetCollidingPair(...)
# DoIsCollisionFree(  )
num_calls
time_used
DistEngine
+ IsCollisionFree(tolerance)
+ MinDist(  )
# DoIsCollisionFree(tolerance)
# DoMinDist(  )