Robot Path Planning: An Object-Oriented Approach

DistEnginePQP
AddPair(GeomT1 a,
GeomT2 b)
DistEngineGJK
AddPair(GeomConvex a,
GeomConvex b)
AddPair(GeomConvexGrp a,
GeomConvexGrp b)
AddPair(GeomConvex a,
GeomConvexGrp b)
SetTrackingMode(…)
PairBasedDistEngine
Add(DistancePair p)
pairs
DistancePair
Figure 2.5:
Classes that encapsulate collision detection for a set of
geometric objects.
plate, using the same technique described in Appendix D.1 for the class
DistPairPQP
; here reference counting techniques are used to avoid du-
plicate PQP Model objects. In the case of DistEngineGJK, we would like
to use it transparently with any combination of the convex types de-

2.5 Configuration Space Interpolation
33
scribed in Appendix C. Therefore, AddPair is overloaded on the possible
combinations, see Figure 2.5.
If we want to mix different distance computation algorithms, then we
can make use of the DistPair class hierarchy from the previous section.
The class PairBasedDistEngine also have a method AddPair, but the
argument of this method is instead a reference to a DistPair object. In
the light of this class, the two previous classes might seem unnecessary,
because this general class can handle several algorithms. It is true that
this class reduces couplings and is a good as an abstraction. However, if
users intend to use only one distance algorithm, then they have to pay for
the overhead of a virtual function call for each pair. Although no timing
experiments have been made yet, this cost is probably not negligible,
considering the thousands of distance queries usually made for solving a
path planning problem.
2.5
Configuration Space Interpolation
The vertices of a roadmap graph represent collision-free configurations,
and the graph edges represent trajectories between neighboring vertices.
The trajectory represented by an edge is, for holonomic problems, im-
plicitly given by interpolation of the two configurations that the edge
connects. Thus, an edge is associated with an interpolation method.
11
It turns out that the interpolation method that should be used is prob-
lem dependent, so simple linear interpolation is not always appropriate.
In this section we will discuss some of the more common interpolation
methods, and see how they can be incorporated into the framework.
2.5.1
Interpolation of Revolute Joints
Linear interpolation works fine for many cases, but is obviously unsuitable
for revolute joints that do not have any joint limits. The joint angle of
a such a joint is naturally associated with the unit circle, denoted by
S
1
, and an interval of length 2π, say [−π, +π]. Thus, for joints whose
topology is given by S
1
, linear interpolation is clearly inappropriate; it
cannot handle the wrap-around that occurs when the joint angle passes
one of the limits, therefore problems that require more than one full
11
For problems involving dynamic systems, an edge has to be associated with a
system input instead.

34
2 A Framework for Path Planning
(a)
(b)
Figure 2.6:
(a) A maze on the surface of a torus. (b) The same maze,
stretched out on a planar surface. The start and goal positions are also
drawn.
rotation cannot be solved. Another problem is that linear interpolation
will not always take the shortest path around the circle.
Revolute joints are not the only joints with a topology homeomorphic
to S
1
: Consider the maze in Figure 2.6 (a), which is drawn on the surface
of a torus. If the maze is cut loose and stretched out on a planar surface,
it will look like in Figure 2.6 (b). In the figure is also drawn the start
and goal position of a robot moving in the maze. The topology of this
planar maze is such that if the robot crosses one of the borders, it will
suddenly appear on the opposite side of the maze. Joints with this type
of behavior will hereafter be denoted ring joints. This example might
seem to be of only theoretical interest, but there are applications where
it could be useful: Computer games often use this topology to let, e.g.,
space ships cross one side of the screen and appear on the opposite side.
Planning algorithms that are capable of handling this topology could be
used to guide the computer controlled opponents in the game.
A general ring joint can take values in the interval [θ
min
, θ
max
], where
the ends of the interval are declared to be equivalent, forming a closed
loop. When interpolating between two values θ
1
, and θ
2
we want to make
sure that we take the shortest path around the circle. Pseudo-code for
achieving this is shown in Figures 2.7 and 2.8.

2.5 Configuration Space Interpolation
35
RingDiff(θ
1
, θ
2
, θ
min
, θ
max
)
1
δθ = θ
2
− θ
1
;
2
if
(δθ < −(θ
max
− θ
min
)/2) then
3
δθ = δθ + (θ
max
− θ
min
);
4
else if
(δθ > (θ
max
− θ
min
)/2) then
5
δθ = δθ − (θ
max
− θ
min
);
6
return
δθ;
Figure 2.7:
A function for computing the “shortest path” difference
between two ring joint configurations θ
1
and θ
2
.
RingInterp(θ
start
, θ
end
, t, θ
min
, θ
max
)
1
θ
t
= θ
start
+ t ∗ RingDiff(θ
start
, θ
end
, θ
min
, θ
max
);
2
if
(θ
t
< θ
min
) then
3
θ
t
= θ
t
+ (θ
max
− θ
min
);
4
else if
(θ
t
> θ
max
) then
5
θ
t
= θ
t
− (θ
max
− θ
min
);
6
return
θ
t
;
Figure 2.8:
Interpolation between two ring joint values by the fraction
t ∈ [0, 1].
2.5.2
Interpolation of Rigid Body Orientations
Rigid body orientations are represented by the special orthogonal group,
SO(3), but it is not straightforward how we should interpolate between
two rotations R
0
, R
1
∈ SO(3). A common approach is to parameterize
SO(3) with one of the many Euler angle set conventions and see interpo-
lation between two orientations as an interpolation on S
1
×S
1
×S
1
. Thus,
each Euler angle is seen as an individual revolute joint and is interpolated
according to the algorithm in Figure 2.8.
It turns out, however, that interpolating between two orientations
using Euler angles has several drawbacks: The computer graphics com-
munity has for a long time recognized that interpolation of rotations
using Euler angles results in unnatural and jerky motions of rigid bodies.
Kuffner [70] pointed out that Euler angle interpolation causes the volume
swept out by the rigid body to be unnecessarily large. This is disadvanta-

36
2 A Framework for Path Planning
geous to path planning as a large swept-volume increases the probability
of a collision. Craig [36] showed that there are 24 possible Euler angle
set conventions that we can choose from. Even though a couple of these
angle sets have become more or less standard, the interpolated motion
will depend on which particular angle set is used.
Another way to represent rotations is using unit quaternions. Quater-
nions are a generalization of the complex numbers, and they can be used
to represent three dimensional rotations just as unit complex numbers
can represent rotations in the plane. As the set of all unit quaternions
form a 4D unit sphere, the problem of smooth interpolation can be seen as
the problem of finding the great-circle arc between two points on the 4D
sphere. The resulting interpolation scheme is called spherical linear inter-
polation (SLERP) and is given by Equation (B.10). See Appendix B.2.3
for more material on quaternions.
It can be shown that SLERP corresponds to rotation around a fixed
axis with constant angular velocity. This behavior correspond with our
intuition about how interpolation between two orientations should be-
have.
Figure 2.9 shows some examples illustrating the differences between
Euler angle interpolation and SLERP. The H-shape is constrained to
move on the surface of a sphere. This can be seen as if the shape is rigidly
attached to a spherical joint at the center of the sphere. The left column
shows interpolation between two orientations using Euler angles. The
right column shows interpolation between the same orientations using
SLERP. From these examples we see that the motions resulting from
Euler angle interpolation are less intuitive and in general result in larger
swept volumes. It is also seen that SLERP moves the shape the same
amount in each step, which is due to its property of constant rotational
velocity.
2.5.3
Car-Like Robots
Car-like robots involve nonholonomic constraints that at each configura-
tion limit the available velocities. If we neglect the dynamics, the robot
configuration can be described by three variables, (x, y, θ). For car-like
robots, Reeds and Shepp [123] showed that the shortest path between
two configurations consists of at most five segments. The segments are
either linear or circular arcs with a radius equal to the minimum turning
radius of the robot. Furthermore, the optimal path consists of at most
two cusps, corresponding to points where the robot changes from forward

2.5 Configuration Space Interpolation
37
Figure 2.9:
The H-shaped object is constrained to move on surface of
the sphere. The left column shows interpolation between two orientations
using Euler angles. The second column shows interpolation between the
same orientations using SLERP.

38
2 A Framework for Path Planning
motion to backward motion, or vice versa. The possible combinations of
segments constitute a family of 48 curves, known as Reeds and Shepp
curves.
This optimal path is of course only possible in the absence of obstacles,
but the Reeds and Shepp curves can still be used for solving path planning
problems involving car-like robots. In a PRM approach, we could let
the graph edges be defined by the optimal Reeds and Shepp curve for
each pair of neighboring vertices. This approach shows the need for
an interpolation method that interpolates along the optimal Reeds and
Shepp curve between two configurations. For path planning with car-like
robots, see, e.g., Bicchi et al. [15], ˇ
Svestka and Overmars [147], Vendittelli
et al. [152], Song and Amato [141].
2.5.4
Interpolation Objects
To separate path planners from the particularities of interpolation, the
framework must provide an abstraction for interpolation. Such an ab-
straction can be achieved either with function pointers to various inter-
polation methods, or with interpolation objects. Here we have chosen the
latter approach because of the added flexibility: Interpolation objects can
contain state variables that affect the interpolation. This is not possible
with function pointers.
Figure 2.10 shows a class diagram for different interpolation objects.
In addition to the Interpolate method, the base class also provides a
Clone
method so that interpolation objects can always be copied, even
if the exact type of the object should be unknown. The ability to clone
objects this way has shown to simplify the usage of the framework in
terms of ownership issues and memory handling. This pattern is therefore
used for all lightweight objects that are meant to be used as configuration
arguments for a planner.
The QuatInterp class uses spherical linear interpolation [131] between
two quaternions, as described by Equation (B.10). Linear interpolation
is used for any additional degrees of freedom. The RingInterp class is
used for problems where some degrees of freedom are ring joints, i.e.,
homeomorphic to S
1
. It uses the algorithms shown in Figure 2.7 and 2.8
to interpolate between the ring joints, and linear interpolation for the
remaining joints.
To be able to compare the tradeoffs involved in using SLERP instead
of Euler angle interpolation, CoPP also provides a class EulerInterp.
This class is actually a special case of the RingInterp class and could

2.6 Metrics
39
EulerInterp
Interpolator
Interpolate(q1, q2, t, qt)
Clone( )
LinearInterp
QuatInterp
RingInterp
min_vals
max_vals
ring_joints
Figure 2.10:
Interpolation classes.
have been implemented using inheritance. As the ranges of the rotational
joints are known in advance, we instead chose a more efficient implemen-
tation than using that of RingInterp.
The interpolation objects are mostly used inside local planners that
try to connect two configurations with a simple path, see Figure 2.13.
The actual path is determined by the interpolation object. Interpolation
objects are also used together with the Path class, where they are used
to compute intermediate configurations for a sequence of via-points, see
Appendix E.1.
2.6
Metrics
The definition of a metric is of fundamental importance to any sampling-
based path planner, but it is far from clear which metric one should
choose. Using the straightforward Euclidean distance measure is in most
cases not so good, because the topology of the configuration space is often
very different from the Euclidean space. In this section we will present
some of the most commonly used metrics for path planning.
A metric space is a topological space X together with a real valued
function ρ : X × X → R (called a metric) such that, for every x, y, z ∈ X,
ρ(x, y) ≥ 0,
(2.1)

40
2 A Framework for Path Planning
ρ(x, y) = 0 ⇔ x = y,
(2.2)
ρ(x, y) = ρ(y, x),
(2.3)
ρ(x, y) + ρ(y, z) ≥ ρ(x, z).
(2.4)
Thus, a metric ρ should be positive, be symmetric, and be zero if and only
if the two points are equal. The last equation states that a metric must
also fulfill the triangle inequality. As an example, if x = (x
1
, x
2
, . . . , x
n
)
and y = (y
1
, y
2
, . . . , y
n
) are two points of R
n
, then the metric
ρ(x, y) = (x
1
− y
1
)
2
+ (x
2
− y
2
)
2
+ · · · + (x
n
− y
n
)
2
1/2
(2.5)
is the well-known Euclidean distance in R
n
.
For nonholonomic path planning it is common to use pseudo-metrics,
i.e., a metric that does not fulfill all of the metric properties [79]. As
these metrics try to estimate the cost-to-go, they often fail to satisfy the
symmetry property. As an example, consider planning for a sailboat:
Due to the wind and other factors, the cost of travelling a distance in
one direction will not be equal to the cost of going back in the opposite
direction. In this section we will not discuss pseudo-metrics, but we note
that they are useful for nonholonomic planning.
As will be shown below, several metrics has been suggested and tested
in the context of path planning, but still it is far from clear which metric
to choose. Most important is of course that the metric reflect the topology
of the configuration space. Still, many metrics involve the choice of one or
more weights, whose value can affect the planner performance. Another
important guideline is to consider the purpose of the operation involving
the metric: When moving from one configuration to another, one must
make sure that no collisions occur on the path between them. In this case
one would like to use a metric that relates to the maximum displacement
of any point on the robot. If this displacement is smaller than the distance
to the nearest obstacle, then the path is clear. In PRMs, distance metrics
are used to determine which nodes one should try to connect to, using
a local planner. Nodes that are close should be more likely candidates
than those that are far away. In this case, the ideal metric should be the
optimal cost-to-go, the cost for moving along the optimal path between
the two given configurations. However, finding this cost is at least as
difficult as solving the original problem. Therefore simpler metrics must
be used that hopefully reflect the true cost-to-go. Thus, the metric is
used as a heuristic for guiding the path planner.

2.6 Metrics
41
2.6.1
Useful Metrics for Path Planning
Manhattan Metrics
One of the simplest configuration space metrics
is the so-called Manhattan distance or L
1
-distance, which is defined as
ρ(x, y) =
n
i=1
|x
i
− y
i
|,
(2.6)
where n is the dimension of the configuration space. This metric has been
widely used, especially in path planners using a discretized representation
of the configuration space, see, e.g., Autere and Lehtinen [10] and Van
Geem [151]. The reason for its popularity is part due to its simplicity, but
also because it is better than the Euclidean distance in the case of grid-
based planners. This is due to the fact that grid-based planners rarely
allow diagonal motions, causing the Manhattan distance to be a better
estimate of the distance. Another advantage of the Manhattan metric
is its low cost, compared to, e.g., the Euclidean distance that requires a
square root. As the metric is computed many thousands of times, the
cost of computing the metric can be an important factor.
Studying a serial manipulator it is seen that moving one joint will
move all links subsequent to that joint. Thus, moving a joint closer to
the base of the robot will in most cases cause a larger gross motion of the
robot than moving a joint closer to the end-effector. The more links that
are in motion, the higher the probability for a collision. This observation
suggests that the cost for moving a joint should be higher the closer to the
base it is. The simplest way to achieve this is to extend Equation (2.6)
to a weighted Manhattan distance:
ρ(x, y) =
n
i=1
c
i
|x
i
− y
i
|,
c
i
> 0
(2.7)
where c
i
is the cost associated with joint i. An example of a path planner
using the weighted Manhattan distance is that proposed by Isto [60]. The
planner used several heuristics to guide its search, where each heuristic
had its own set of weights for the Manhattan distance.
Rigid Body Metrics
Amato et al.[6] performed an extensive study
on the effect the choice of metric had on the performance of PRMs in the
context of rigid body problems. They found that the weighted Euclidean
distance had the best efficiency on the tested problems. They also found
that the relative importance of the translational distance between two

42
2 A Framework for Path Planning
configurations increased as the environment became more cluttered. A
weighted Euclidean metric is, however, not the most appropriate for rigid
body problems, as it does not capture the topology of the configuration
space. It would be more natural to use a metric that is based on SE(3),
the group of rigid body transformations. The problem is that the defini-
tion of a metric on SE(3) must involve the choice of a length scale, see,
e.g., Murray et al. [107], which affects how translational motion and ro-
tational motion are weighted in the metric. As discussed in Section 2.5.2,
interpolation between two orientations using SLERP is equivalent to mov-
ing along a great-circle arc on a 4D sphere. The end-points of the arc
are equivalent to the unit quaternions that correspond to the two ori-
entations. As the travelled distance, the arc length, is proportional to
the angle between the two quaternions, a metric for rotations could be
based on the inner product of the quaternions, see Equation (B.12). As
metric for SE(3) we could then use the weighted sum of the translational
distance and the quaternion distance.
Displacement Metrics
Because a robot consists of one or more mov-
ing rigid bodies, other approaches to defining a metric could be based on
the distance between point sets in R
3
. A common metric of this type is
the maximum distance any point on the robot has displaced between two
configurations [77]:
ρ(x, y) = max
a∈A
||a(x) − a(y)||,
(2.8)
where A denotes the robot. Because this metric is expensive to compute,
one often approximates the robot links with their bounding boxes instead.
Then the maximum displacement of the vertices the bounding boxes gives
an upper bound for the distance given by Equation (2.8). This metric
is particulary useful when verifying if a straight line path between two
configurations is collision free, see, e.g., Baginski [11]. Its usefulness stems
from the fact that it in most cases gives a conservative estimate of the
movement between two configurations.
2.6.2
Implemented Classes for Metrics
CoPP provides several predefined metric classes. They all inherit from
the abstract class Metric, which defines the pure virtual functions
Distance
and Clone. Figure 2.11 shows the base class Metric together
with the most commonly used metric classes in CoPP.

2.6 Metrics
43
The class BoundingBoxDispl is a bounding-box displacement metric:
For a set of geometric objects, either the maximal displacement, or the
summed displacement between two configurations is computed. The abil-
ity to add and remove geometric objects is useful for problems where the
number of moving objects is not constant. A typical example is a pick-
and-place task, where the grasped object suddenly moves along with the
robot. Note that BoundingBoxDispl is a template, with the type of the
moving system as template parameter. To conform with the template,
the moving system must have a function Move, as shown in Figure 2.11.
If the template is used together with the robot class described in Chap-
ther 3, the generated metric objects will have a reference to a robot; each
call to Distance will move the robot to the two configurations and find
the maximum (or summed) bounding box displacement. This metric has
shown to be really useful when checking wether a path segment is colli-
sion free or not; when the displacement between two successive samples
on the segment is below a user defined threshold, then it is assumed that
no collisions occur between these two samples.
The class RingMetric is used for problems where one or more de-
grees of freedom are ring joints, described in Section 2.5.1. Such degrees
of freedom arise, for example, in two-dimensional rigid body problems,
where the rotation of the body is not limited. A ring joint can take values
in [θ
min
, θ
max
], and the “shortest path” difference between the values θ
1
and θ
2
, defined by the function in Figure 2.7, is here used to define a
metric for the ring joints. For a single ring joint, we define the distance
as:
ρ(θ
1
, θ
2
) = |RingDiff(θ
1
, θ
2
, θ
min
, θ
max
)|.
(2.9)
The metric RingMetric is a weighted Manhattan metric, where the con-
tribution from the ring joints is computed according to Equation (2.9).
The class QuatDist is a metric for rigid body problems. For the
translational part, the Manhattan distance is used. For the rotational
part, the distance is related to the inner product of two quaternions,
see Equation (B.12). The total distance is a weighted sum of these two
contributions. It could be argued that it would be more appropriate to
use the Euclidean distance for the translational part, but for the prob-
lems tested so far, involving many thousands of distance computations,
the speed of the Manhattan metric has had a larger effect on the total
planning time than the exactness of the Euclidean metric.

44
2 A Framework for Path Planning