Robot Path Planning: An Object-Oriented Approach

TriangleSet
AddTriangle(…)
GetTriangles( )
ConvexABC
ConvexABC(points)
GetHalfEdges( )
half_edges
GeomConvex
GeomConvex(points)
GeomConvexGrp
GetParts( )
parts
Material
pose
Transform
Figure C.2:
Class diagram for the geometry classes currently imple-
mented in CoPP.
applications. A natural way to construct such an object would be to com-
pute the convex hull of a set of points. Several algorithms for computing
convex hulls efficiently have been proposed. One of the more popular is
the quickhull algorithm, Barber et al. [12], which is publicly available in

C.3 Convex Polyhedra
201
0
1
2
3
he arr = [4, 8, 12, 16, 1, 2, 3, −1, 0, 2, 3, −1, 0, 1, 3, −1, 0, 1, 2, −1]
Figure C.3:
An example showing the half edge structure for a tetra-
hedron. The first four entries in the array are offsets for each vertex
entry.
the software package Qhull. CoPP uses Qhull, but to make it easier to
use, CoPP provides a wrapper class called Hull3D.
Without any additional topological information, the class for convex
polyhedra is nothing but an indexed face set whose faces happen to form a
convex polyhedron. Clearly there should be more topological information
we can utilize for such a special instance of an indexed face set. One of
the most popular collision detection algorithms for convex polyhedra is
GJK, originally proposed by Gilbert et al. [48]. The original algorithm
used as input only two set of vertices, one for each convex hull. Later
Cameron [27] showed that the algorithm could be more efficient if it is
provided additional information in the form of an adjacency structure
called half edges. The half edge structure tells us which vertices we
can reach in one step by following the edges leading from the vertex
we are currently standing at. This type of adjacency information has
also been used by Sato et al. [129], Ong and Gilbert [111, 112], and
several others. Therefore it was decided that this information is part
of the representation of convex polyhedra. The half edge structure can
be implemented in many ways, for example as a linked list as in [129].
Here we choose to use an array of indices into the vertex array. An
example of how the half edge structure looks like for a tetrahedron is
shown in Figure C.3. The concrete class GeomConvex in Figure C.2 is
used to represent convex polyhedra. Its constructor accepts a set of
points and invokes Hull3D to compute the corresponding convex hull.
It is also seen in Figure C.2 that GeomConvex inherits from an abstract
class ConvexABC. The reason for this intermediate class will be explained
in the next section.

202
C Geometric Representations
C.4
Non-Convex Objects and Groups of
Convex Objects
The family of convex polyhedra is rather restricted. We can broaden
it significantly by also consider shapes that are formed by the union of
several convex polyhedra; then we can represent any shape that can be de-
composed into a finite set of convex objects. A naive attempt would be to
implement such shapes as an array of GeomConvex, where all the objects
move together because they are attached to the same frame. Perform-
ing collision detection between two such shapes would have O(M
1
M
2
)
complexity, where M
1
and M
2
are the number of convex parts in each
object. This is clearly not the kind of performance we are looking for, so
we need to organize the parts better. Bounding volumes such as spheres
and boxes are often used in the early steps of collision detection as an
attempt to quickly rule out pairs of objects that are well separated. Be-
cause we are already using convex polyhedra, why not use the convex hull
of the parts as a bounding volume? This will give us the tightest convex
bounding volume possible. The convex hull of all the parts will hereafter
be denoted closure. If the closures remain separated most of the time,
those M
1
M
2
collision checks will seldom be required. We can take this
idea one step further by using a recursive definition. Our objects will
then be a hierarchy of convex polyhedra and closures.
C.4.1
Three Possible Designs
How such a hierarchical model should be implemented depends of course
on our intended usage of it, but also on our notion of the grouped ob-
ject: We can choose to view a group of convex objects as just any non-
convex object, where the occurring closures are nothing but an imple-
mentation artifact. Alternatively, we can choose to view the closures
as objects in their own right. It turns out that the appropriate de-
sign will depend to a large extent on which view we take. Figure C.4
shows three possible designs that were considered. If we adopt the first
view, then the resulting class has no properties in common with the
class GeomConvex; it is just implemented in terms of it. The class would
have methods such as GetClosure and GetParts, which both return ob-
jects of type GeomConvex. An informative name for this class would be
GroupOfConvex
.
If we instead adopt the second view, then a group of convex objects
actually is a convex object in itself. This view is correct if that convex

C.4 Non-Convex Objects and Groups of Convex Objects
203
object is the closure of the group. The IS-A relationship suggest that
we should use inheritance. However, inheriting directly from a concrete
class such as GeomConvex will often lead to constrained class hierarchies
and subtle traps that are easy to fall into. See, e.g., the book by Mey-
ers [98] why inheriting from concrete classes should be avoided in general.
Therefore a new abstract class should be introduced, ConvexABC, from
which all other convex objects inherit. With this step taken, we could
choose to implement hierarchies of convex objects using the Composite
pattern [45], see Figure C.4 (b), or we could use the approach shown
in Figure C.4 (c). The implementation using the Composite pattern is
appealing because of its clarity in showing the recursive definition of the
composite objects. However, as clients use GetParts, they will not know
wether these parts themselves are compositions. After all, the intent
of the Composite pattern is to hide from clients wether or not they are
dealing with composites. The Composite pattern would have been a good
solution if collision detection was part of the ConvexABC itself; as each
object know its own type, it can take appropriate actions. Because the
philosophy in CoPP is to enforce a good separation of concerns, collision
detection is not part of the geometry interface. Therefore the Composite
pattern is not appropriate in this situation. Note that the solution to add
the method GetParts to the interface of ConvexABC is to be considered
bad design in this situation: It is not an intrinsic property of convex
objects to have parts. (Unless those parts are vertices, faces, etc.)
In the third design, shown in Figure C.4 (c), the recursive definition
is carried out on the class itself. As in Figure C.4 (b), the data struc-
tures needed to represent a single convex polyhedron is obtained through
the inheritance relationship. In the general case this derived part is the
closure of the group. A single convex polyhedron will be modeled as a
group with no parts. In this case, the closure and the actual polyhedron
are the same.
The designs in Figure C.4 (a) and in Figure C.4 (c) are both sound
and the main difference lies in how we view the closures. However, it was
found that the class diagram in Figure C.4 (c) leads to several implemen-
tation advantages such as more code reuse and simpler code. Therefore
the class GeomConvexGrp was chosen before GroupOfConvex.

204
C Geometric Representations
GroupOfConvex
GetClosure( )
GetParts( )
closure
parts
GeomConvex
(a)
ConvexABC
GeomConvex
ConvexComposite
GetParts( )
parts
(b)
ConvexABC
GeomConvex
GeomConvexGrp
GetParts( )
parts
(c)
Figure C.4:
Three possible designs for modeling a union of convex
polyhedra.
C.4.2
An Example of a Hierarchical Geometric Ob-
ject
With all the machinery in place, it is time to take a look at an example
geometry. Figure C.5 (a) shows a simple model of a book shelf.
1
The
book shelf is clearly non-convex, but is easily decomposed into 20 convex
parts, all of them regular boxes. If all these parts are put together into a
single group of convex objects, the resulting GeomConvex object encloses
1
Frequent IKEA customers may recognize it to be the Ivar book shelf.

C.4 Non-Convex Objects and Groups of Convex Objects
205
the parts with their closure. In this particular case, the closure becomes
extremely simple; except for a few extra vertices due to diagonal struts,
the closure becomes a regular box. The shelf, together with a slightly
transparent closure, is shown in Figure C.5 (b).
For the purpose of efficient collision detection, however, this arrange-
ment of the parts is far from optimal; once an object penetrates the
closure of the shelf, there are again 20 more collision checks to perform.
Because the sides of the shelf contain six parts each, and they are com-
pact, i.e., they are rather box-like themselves, it is a good idea to let each
side be a group of its own. The resulting model is shown in Figure C.5 (c),
where the outermost closure has been made totally transparent to bet-
ter show the inner closures. The GeomConvex object for the shelf will
now require storage for three more convex polyhedra in addition to the
original 20. This is a typical example of the tradeoff between memory
and speed. In general though, the memory overhead for this hierarchical
representation is modest, because the number of vertices in a closure is
usually much lower than the sum of the vertices of the parts that form
it. Admittedly, the book shelf example is a bit extreme in this respect:
All the parts have together 160 vertices, whereas the closure has only 16.
In a typical pick-and-place task involving an object placed on one
of the shelves, there would be many configurations where the closure of
the shelf is penetrated by both the robot arm and the grasped object.
To confirm a collision-free configuration that places the robot between
two shelves, the arrangement in Figure C.5 (c) will require 11 collision
tests. Without the two inner closures, such configurations would require
21 collision checks. Thus, the added complexity in the arrangement of
the parts pays off in terms of less collision checks. Of course there exist
degenerate cases as well: Consider a cylindrical rod that is long enough
to collide with the closure of both sides at the same time. If the rod
is placed between two shelves such that it touches the closures of both
sides, then 23 collision checks will be required to confirm a collision free
configuration.
The two steps required to make a hierarchical model as the one just
shown, decomposition into convex parts and grouping, are done manually
in the description file. It would of course be nice if at least one of the
steps could be automated, but the study of such algorithms is beyond
the scope of this thesis.

(a)
(b)
(c)
Figure C.5:
Illustration of how a non-convex object is represented as
an hierarchy of convex polyhedra. The hierarchical representation is vi-
sualized using three different views of the object.

Appendix D
Pairwise Collision
Detection with PQP and
GJK
The CoPP framework currently supports two different collision detection
algorithms, PQP [76] and Enhanced GJK [27]. In this appendix we look
at how these algorithms are encapsulated behind the uniform interface
defined in CoPP.
In Section D.1 we look at C++ specific techniques that allow differ-
ent geometric types to be used with PQP. The only requirement on the
geometric type is that it must support triangulization. Section D.2 deals
with Enhanced GJK and distance computations for convex polyhedra. In
Section D.3, techniques for mixing different collision detection algorithms
are briefly discussed.
D.1
Encapsulating PQP
The Proximity Query Package (PQP), developed by Larsen et al. [76], is a
package that provides many different types of proximity queries on pairs
of geometric objects. It does not assume any specific topology on these
objects; an object is just seen as a set of triangles, from which PQP builds
its own internal representation, which has the type PQP Model. Users of
PQP can easily create objects of type PQP Model by adding the triangles

208
D Pairwise Collision Detection with PQP and GJK
one at a time and then tell PQP to build the internal representation.
Thus, any object that we can triangulate can be used with be PQP.
The class that encapsulates PQP for a pair of geometric objects is called
GeomPairPQP
, and it inherits from WitnessPair, see Figure D.1.
Objects of type GeomPairPQP always reference a pair of geometric ob-
jects. The best way to enforce this constraint is to only define construc-
tors that require pairs of geometric objects. The question is, what type
should these geometric types have? A straightforward solution would be
to introduce a new geometric class (derived from Geom) that internally
stores a PQP Model. This geometric class would then be compatible with
PQP. However, this approach would go against one of the main goals with
the CoPP framework, namely that changing an algorithm should require
as little work as possible. If, for one reason or another, a user would
like to change to another collision detection algorithm, then she has to
change the geometric type as well. If the geometric type is changed,
chances are that other things will change too. This chain reaction is
caused by the, rather unnecessary, coupling between the geometric type
and the collision detection algorithm via the type PQP Model. In princi-
ple, we should be able to use GeomPairPQP with any geometric type that
can be triangulated. If we know how to triangulate a certain geometric
type, then it is easy to provide a conversion from that type to PQP Model.
Thus, the constructor of GeomPairPQP should accept any combination of
geometric types for which both types support this conversion. We could
achieve such a generous constructor if we overload it on every combina-
tion we are interested in. However, this solution is not very scalable as
the interface of GeomPairPQP would become very cluttered by all the dif-
ferent constructors. Furthermore, as users add new geometric types that
they want to use with this class, they will have to add the appropriate
constructor too, which is definitely not what most users would expect.
There is a solution to the problem with the cluttered interface of
GeomPairPQP
, which also minimizes the coupling between this class and
classes for geometric types. This solution builds on the template facility
available in the C++ language. For every geometric type that we want
to use with PQP, there has to exist an overloaded version of the function
CreatePQP
:
PQP_Model* CreatePQP(const TriangleSet&
obj);
PQP_Model* CreatePQP(const GeomConvex&
obj);
PQP_Model* CreatePQP(const GeomConvexGrp& obj);
...

D.1 Encapsulating PQP
209
CollisionPair
+ Collides( )
+ GetNumCollisions( )
- DoCollides(  )
num collisions
if (DoCollides()) { 
  ++num_collisions; 
  return true; 
}
return false;
DistancePair
+ Collides(tolerance)
+ Distance( ) 
+ DistanceSqrd( )
- DoCollides(tolerance)
impl
PairGJKBase
GeomPairGJK
WitnessPair
+ GetClosestPoints(p1, p2)
MixedPairGJK
MixedPairGJK(GeomConvex       a, 
                         GeomConvexGrp b)
LeafPairGJK
LeafPairGJK(ConvexABC a,
                      ConvexABC b)
ConvexGrpPairGJK
GeomPairPQP
GeomPairPQP(GeomT1 a, 
                         GeomT2 b) 
PairGJK_Base
PairGJK_Base(ConvexABC a,
                         ConvexABC b)
SetToTracking(…)
bool UseSimplex( ) 
GetSimplex( ) 
SimplexGJK last_result 
bool is_tracking
Figure D.1:
Classes that encapsulate proximity queries for pairs of
geometric objects. The constructor of GeomPairPQP is a template that
accepts any geometric types, as long as there exist matching versions of
the function CreatePQP.

210
D Pairwise Collision Detection with PQP and GJK
The ellipses indicate that we would add an overloaded version for ev-
ery new geometric type. Note that these functions are not part of the
interface for any geometric type, hence no unnecessary couplings are in-
troduced. Now we can make the constructor to GeomPairPQP a C++
template, with the geometric types as template arguments. At compile
time, the compiler will look for the appropriate versions of CreatePQP
and generate the needed constructors for us. In effect, we have got n
2
constructors for the price of one, where n is the number of overloaded
versions of CreatePQP.
A final note on this class: As each PQP Model can consume large
amounts of memory, creating two such objects for every GeomPairPQP
would be very wasteful. Therefore, the class uses reference counting
techniques to ensure that only one PQP Model per geometric object is
created.
D.2
Encapsulating Enhanced GJK
Enhanced GJK [27] is a fast algorithm for computing the minimum dis-
tance between two convex polyhedra. An advantage of this algorithm
is that it requires very little preprocessing of the geometric data. The
base class for convex polyhedra, ConvexABC, already contains all the data
needed by the algorithm, so an object for doing collision detection on a
pair only needs two references to such objects. To speed up queries in
situations with strong temporal coherence, each pair object will also con-
tain cached information about the latest query. This information is the
simplex (in the Minkowski difference of the two polyhedra) to which GJK
converged the last time.
The class GeomPairGJK, see Figure D.1, takes care of distance
queries on a pair of convex polyhedra. It inherits from the abstract
PairGJK Base
, that implements code for maintaining the cached infor-
mation from each query. The class GeomPairGJK is used as an extra level
of indirection; it only forwards queries to an object of type LeafPairGJK,
MixedPairGJK
, or ConvexGrpPairGJK. The extra level indirection make
sure that clients only have to deal with GeomPairGJK, no matter which
types of convex objects are used. This is an example of the Handle/Body
idiom, see Coplien [33].
To model non-convex objects we introduced the class GeomConvexGrp,
which provides hierarchical compositions of convex objects. Distance
queries on a pair of such objects will traverse the two hierarchies and

D.3 Mixing Different Algorithms
211
find the pair of leaf objects that minimize the distance. Collision detec-
tion queries are made much faster, because parts of the hierarchies can
simply be skipped if their bounding spheres or closures do not intersect.
The class ConvexGrpPairGJK deals with pairs whose geometries are com-
positions of convex polyhedra. It also inherits from PairGJK Base. For
pairs of hierarchical objects, the cached information only applies to root
object of each hierarchy.
D.3
Mixing Different Algorithms
A problem that is often discussed in association with pairwise collision de-
tection is that of double-dispatch; when calling a virtual function through
a pointer to an object, the function that is actually executed will de-
pend on the dynamic type of the object pointed to (i.e., single-dispatch).
However, sometimes we would like choice of function to depend on the
dynamic type of two objects. As a motivating example, consider that we
want to take advantage of the many closed form solutions that exist for
computing the distance between pairs of simple objects. Closed-form ex-
pressions exist for the distance between, e.g., spheres, rectangular boxes
and line segments. If we want to take advantage of this we are faced with
the double-dispatch problem; the collision checking code that is executed
depend on if we are checking, e.g., a sphere-sphere pair, or a sphere-line
pair. Languages lake CLOS, the Common Lisp Object System, has built-
in support for double-dispatching [98], whereas C++ has not. Therefore,
solutions double-dispatch problem in C++ depend on various tricks to
emulate it. Numerous authors have suggested how to solve this prob-
lem in C++, see e.g., Meyers [98] and Alexandrescu [4]. Most of the
solutions make use of runtime type information or double virtual func-
tion calls to lookup the correct function. Doing this every time for a
collision check can result in a notable overhead, making the approach
with specialized collision detection algorithms unattractive. However, if
no new objects are likely to be created once the application is running
(as opposed to, e.g., video games), we can do all the expensive lookups
once and for all at initialization time. Once the correct DistancePair is
created, it also carries with it the correct distance algorithm. At a design
pattern level, this could be implemented using the Factory Method pat-
tern [45]; a class GeomPairFactory is responsible for associating a pair
of geometric objects with the correct distance algorithm via the function
CreateGeomPair
. At the implementation level, we could, e.g., use the

212
D Pairwise Collision Detection with PQP and GJK
generic Factory and BasicDispatcher class templates available in the
Loki library by Alexandrescu [4].
Sometimes it is sufficient though to do the lookup only at compile
time. This is the case if the type of each geometric object is known be-
forehand, or if we want to be able to change the geometric types in our
program without affecting collision detection code. Compile-time lookup
is easily achieved using a factory method implemented as a function tem-
plate:
template
DistancePair* CreateDistancePair(GeomT1& a, GeomT2& b);
This function would lack an implementation for the general case, but
for every combination of geometric types that we are interested in, there
would exist specialized versions of the function template. Suppose now
that we change the geometric type in our code from, say, TriangleSet to
GeomConvexGrp
, then the compiler would automatically switch to another
specialization of CreateDistancePair. Thus, the change of geometric
type would require a minimum of changes in the source code. The use of
a factory method also opens the door for other possibilities: Suppose that
experiments show that Enhanced GJK is faster than PQP on queries in-
volving compositions of convex polyhedra that are not too complex. Then
we could implement the factory method to produce ConvexGrpPairGJK
in situations that favor Enhanced GJK, and GeomPairPQP otherwise.

Appendix E
Framework Details
In this appendix we cover some details of the CoPP framework that are
not covered elsewhere. Section E.1 presents a class for representing paths
in the configuration space. By delegating some of its behavior to metric
objects and interpolation objects, this class can model many different
types of paths. Section E.2 deals with binary constraints, which are
useful abstractions, allowing a planner to transparently handle different
types of constraints.
Section E.3 introduces a problem class, which is used to hold the
definition of a path planning problem. In addition to robots, and obsta-
cles, object of this class can also contain information about which metric,
interpolation method, or sampling method to use.
To avoid introducing platform dependencies, CoPP uses the platform
independent VRML-file format to store animations. Section E.4 gives an
example on how the Visitor pattern [45] can be used to make a function
behave as if it was virtual with respect to a class hierarchy, without
being part of it. Here the pattern is used to allow a uniform interface for
drawing objects of different geometric types.
The last section gives an example of a robot description file for a
simple SCARA robot.
E.1
Configuration Space Path
The result from a path planner is often a finite series of via-points in
the configuration space. The path to take in-between the via-points is

214
E Framework Details