Robot Path Planning: An Object-Oriented Approach

WeightedManhattan
weights
Metric
Distance(q1, q2)
Clone( )
Euclidean
BoundingBoxDispl
AddGeom(geom)
RemoveGeom(geom)
use_summed_displ
QuatMetric
rot_weight
agent
Geom
geoms
AgentT
Move(config)
RingMetric
min_vals
max_vals
ring_joints
weights
Figure 2.11:
Class diagram for the metric classes.
2.7
Configuration Space Sampling
For sampling-based planning methods, an important issue is how the sam-
ples are generated. The most common approach is to draw samples with
a uniform probability over the entire configuration space. For problems
involving narrow passages, a better strategy is to bias the distribution
to the narrow passages. The next section presents several techniques for
doing this. For planning problems involving constraints, uniform ran-
dom sampling is not effective because most samples fail to satisfy the
constraint. Section 2.7.2 presents some approaches where random sam-
ples are generated on the constraint surface. For rigid body problems,
care must be taken not to introduce unwanted bias in the generated ro-
tations. This issue is discussed in Section 2.7.3. Section 2.7.4 mentions
deterministic sampling and Section 2.7.5 presents classes for various sam-
pling strategies.

2.7 Configuration Space Sampling
45
2.7.1
Narrow Passage Sampling
It was early noted that planners using uniform sampling will get into trou-
ble as soon as the solution requires passing through a narrow passage in
the configuration space [64]; because such passages occupy a very small
subset of the configuration space, the probability of randomly guessing
a configuration in the passage is prohibitively small. Sampling with a
uniform distribution will essentially require covering of C
f ree
before find-
ing the necessary configurations in the narrow passage. To overcome this
problem, several sampling strategies have been proposed with the goal
to increase the likelihood of sampling ’difficult’ areas of the configuration
space.
Boor et al. [20] proposed the Gaussian sampling strategy to concen-
trate samples near the boundaries of the configuration space obstacles.
Two samples are drawn each time such that the distance between them is
a stochastic variable with a normal distribution. The standard deviation
of the distribution is a parameter of the algorithm; a smaller standard de-
viation will generate configurations that are closer to the obstacle bound-
aries. The important step in the algorithm is to return a collision free
sample only if the other sample is not collision free. Thus, if both samples
are collision free, they are rejected. In [20] the method was successfully
applied to two-dimensional problems involving a moving polygon. How-
ever, it is not clear how to extend this method to robots with kinematic
chains because the distance metric and the standard deviation for the
distribution become difficult to choose. A too small standard deviation
will cause the strategy to throw away most of the generated configuration
pairs due to both configurations being collision free or both causing a col-
lision. A too large standard deviation, on the other hand, will just turn
the strategy into a more expensive version of the uniform distribution
sampler.
Amato et al. [5] propose that, for samples that are not collision free,
binary search search along random directions can be used for quickly
generating configurations near the boundary of C
obs
. Thus, colliding con-
figurations act as seeds for configurations near the C-space obstacles.
The medial axis of the free configuration space, also referred to as
the generalized Voronoi diagram, is a useful tool in path planning. This
stems from the fact that it has a lower dimension than the free space
but is still a complete representation for path planning purpose. Fur-
thermore, the medial axis represents paths with maximal clearance to
the obstacles. However, generating the medial axis for the free space

46
2 A Framework for Path Planning
is an expensive operation, making it intractable for all but the simplest
problems. But Wilmarth et al. [155] show a relatively simple method for
projecting random configurations onto the medial axis without having an
explicit representation of it. The projection method is also applied to con-
figurations that are inside configuration space obstacles. It was shown
that this extension greatly increases the number of samples in narrow
passages if the surrounding obstacles are ’thick’. An example was shown
for a single rigid body moving through a maze in 3D, but extending the
method to kinematic chains seems to be difficult.
Holleman and Kavraki [56] also proposed a projection method, but
they use the medial axis of the workspace instead. Their method can
be extended to kinematic chains by assigning handle points to each link,
which is not so satisfying as long as these have to be chosen manually.
Yang and LaValle [157] proposed a randomized perturbation scheme for
enhancing samples which can be seen as an approximation of the sampling
onto the medial axis. The method works well for kinematic chains.
Kazemi and Mehrandezh [65] used the theory of potential flow and
harmonic functions to bias the sampling towards the narrow passages.
They noted that regions of high fluid velocity often corresponded to nar-
row passages in the robot’s configuration space. Thus, by biasing the
sampling distribution toward high velocity regions, they achieved bet-
ter coverage of the narrow regions. In a similar approach, Aarno et
al. [1] used a harmonic potential in W to bias the sampling; the potential
reaches high levels near obstacle boundaries. As the dimension of W is at
most 3, the cost of computing the potential in W is usually much lower
than computing a potential in C. The drawback is that there need not
be a direct correspondence between the narrow passages in W and those
in C.
2.7.2
Constraint Based Sampling
Path planning problems may involve other constraints in addition to the
collision-free constraint. For such problems, drawing samples with a uni-
form distribution could be inefficient if few samples satisfy the constraint.
A better approach would be to use a constraint-based sampling method
that only generates samples that satisfy the constraint.
For robots with closed-loop kinematic chains, there is a closure-
constraint that all configurations must satisfy. Thus, any loops must
remain closed. The chance of generating a configuration at random that
satisfy the closure constraints is very small, so other methods are needed.

2.7 Configuration Space Sampling
47
In the method of Cort´es et al. [35], loops in a kinematic chains are cut.
Each cut defines an active chain and a passive chain. Random sampling
and forward kinematics are used for the active chains. The passive chains
are then forced to close the open loops using inverse kinematics. To be
really effective, this method assumes that closed-form inverse kinematics
exist for the passive chains.
Oriolo et al. [113] considered the problem of a redundant robot that
has to move among obstacles along a given end-effector path. As the
robot is constrained to follow the end-effector path, it has to use its re-
dundancy to avoid the obstacles. The sampling method in [113] generated
samples that satisfied the end-effector constraint.
2.7.3
Uniformly Distributed Rotations
Euler angles are often used to parameterize the rotation of rigid bodies.
Kuffner [70] pointed out that drawing each Euler angle from a uniform
distribution will not generate uniformly distributed orientations; if the
orientations are visualized on a 3D sphere, as in Figure 2.9, the distri-
bution will have a clear bias towards the poles of the sphere. This bias
can have a negative effect on path planners, as excessive sampling might
be needed to achieve a crucial orientation. Thus, special care must be
taken to generate orientations that are truly uniformly distributed. The
same care must be taken if the rotations are represented by quaternions.
Shoemake [132] presented a simple algorithm for generating uniformly
distributed quaternions.
2.7.4
Deterministic Sampling
Recently, the need for random sampling has been questioned, see e.g.,
Branicky et al. [23]. First of all, unless some physical process is used, the
random numbers generated by a computer are actually pseudo-random.
That is, they are generated by a deterministic algorithm. As these num-
bers actually are deterministic, then it should be possible to use other
deterministic sequences that are better suited. Pseudo-random number
generators are designed to meet performance criteria that are based on
uniform probability densities. These criteria might not be the best for
sampling-based path planning.
In [23] good results were obtained with so called Hammersley points
and Halton points. These are deterministic point sets that are specifically
designed to have properties such as low dispersion and low discrepancy.

48
2 A Framework for Path Planning
2.7.5
Sampling Strategy Classes
Clearly, there are many suggestions for how to sample the configuration
space, each with its own merits and drawbacks. Despite the many vari-
ations, they all do the same thing, namely produce configuration space
samples. This observation, combined with the fact that configuration
space sampling is an essential part of many planners, suggests that the
sampling strategy should be encapsulated in an object. This is an ex-
ample of the Strategy pattern described in [45], where the interface to a
family of related algorithms is defined in an abstract base class. Letting
all sampling strategies inherit from an abstract class has several advan-
tages: First, changing sampling strategy is as simple as changing one line
of code, instantiating a different strategy object, which makes compari-
son of strategies and testing new ones extremely easy. Second, advanced
planners might adapt to the problem at hand by switching strategies at
runtime.
The class ConfigSpaceSampler is the base class for sampling strate-
gies. Clients ask for new samples through the function GetSample and
they should ideally not know or depend on how this sample was pro-
duced; it could have been drawn from a uniform distribution or it could
have been produced using a more elaborate scheme. In fact, samples
need not be random at all. They might as well be deterministic! Be-
cause some sampling approaches might fail to produce a new sample, a
boolean return value from GetSample is used to indicate if a new sample
was produced or not.
Even the simplest sampling strategy will need some information about
the configuration space it is supposed to sample. In most cases, it is
enough to know the lower and upper limits of the configuration space.
More elaborate strategies, like obstacle based sampling [5], will need more
information. Because the base class interface does not allow such infor-
mation to be passed to a sampling strategy, all the needed information
has to be provided at the creation of a sampling strategy. So the price to
pay for a simple and uniform query interface is that the creation becomes
non-uniform and that sampling strategies sometimes share objects with
the planner.
As seen in Figure 2.12, there are a number of sampling strategies in
CoPP, of which UniformSampler hardly needs any detailed exposition.
The class ObstBasedSampler implements the obstacle based sampling
strategy presented in [5]. This is an example of a strategy that has to
share a lot of information, i.e., the robot and the distance engine, with

2.7 Configuration Space Sampling
49
HaltonSampler
min_vals
range_vals
counter
ConfigSpaceSampler
GetSample(sample )
Clone( )
UniformSampler
min_vals
range_vals
HammersleySampler
min_vals
range_vals
counter
max_samples
ObstBasedSampler
AllowCollFreeSamples(...)
NumDrawnSamples( )
NumCollidingSamples( )
SetNumDirections(n)
DistEngine
IsCollisionFree(  )
MinDist( )
robot
dist_engine
Robot
Move( )
UniformQuats
min_vals
range_vals
Figure 2.12:
Classes that encapsulate different sampling strategies.
the planner. For rigid body problems, the class UniformQuats should
be used. The translational part is drawn with a uniform distribution
from a box in R
n
, while the rotational part is generated from uniformly
distributed quaternions, as described in [132].
The HaltonSampler and the HammersleySampler both use determin-
istic point sets. To keep track of which point to generate next, both
strategies use an internal counter that keep track of how many times
GetSample
has been called. Note that the Hammersley point set is finite;
its size is a user-specified parameter. This class is an example where the
return value of GetSample has to be used to indicate that the sampling
strategy has run out of samples.

50
2 A Framework for Path Planning
2.8
Local Planners
The concept of local planners is important to planning methods like PRM
and its relatives. In PRM-methods, local planners are used to find con-
nections between pairs of nearby configurations, and in RRTs they are
used to extend one state towards another. As the number of attempted
connections can exceed several thousands, the overall efficiency is closely
related to what local planner is used.
An elaborate local planner that is able to “turn around corners” could
find more connections than a simple one that only moves along a straight
line. However, due to using more time per query, it is not clear wether
the more elaborate planner would be more efficient than the simple one.
Thus, there is an important tradeoff for local planners between success
rate and average query time. There is a clear trend towards very simple
local planners that only check wether a given path segment (often de-
termined by an interpolation method) is collision free or not. The low
success rate of such simple planners is compensated by their simplicity
and efficiency; in case the given path segment is not collision free they re-
turn very quickly, allowing a large number of path segments to be tested
in short time.
Other local planner methods could be based on potential fields or on
some heuristic rule, such as the rotate-at-s planner proposed by Amato
et al. [6]. The rotate-at-s planner is a family of planners, parameterized
by the parameter 0 ≤ s ≤ 1.
2.8.1
Checking Path Segments
The simplest local planner just tries to verify wether a path segment be-
tween two given configurations is collision free or not. The path segment
is often implicitly given in that it is determined by the two configurations
and a provided interpolation method. Even though this planner is con-
ceptually simple, the problem it tries to solve it is by no means a simple
one: To efficiently determine wether a given path is collision free is an
extremely difficult problem. An exact method that never fails to detect
a collision along a path will take too long time to be practical. Therefore
approximate methods are used, at the risk of missing a collision.
Difficulties arise because collision detection algorithms only check
wether particular configuration is collision free or not, when the prob-
lem is to check a whole range of motion. In practice, the most common
method to verify collision free path segments is to do static collision de-

2.8 Local Planners
51
tection at a finite number of points along each segment. The question is,
how dense do we have to sample a given segment? To keep the proba-
bility of missing a collision low, it is important that the relative motion
between each collision check is small. Thus, sampling a segment until,
e.g., the bounding box displacement between all samples is less than some
threshold seems as a good idea. The sampling of the segment could be
done in an incremental fashion, from one end towards the other. It is in
general, however, more efficient to recursively divide the path segment in
smaller and smaller segments and do a collision check on the mid-point of
each segment. For a collision free path segment, there is no difference be-
tween the two approaches. If there is a collision, however, then the second
approach will find this quicker, see e.g., Geraerts and Overmars [46].
For rigid body problems, it is possible to find exact upper bounds
on the displacement between two configurations. These bounds depend
on the object geometry, the center of rotation, and assumes that the
motion taken between the two configurations is given by interpolation
on SE(3) (or SE(2) for 2D problems). Thus, given the distance to the
nearest obstacle at two configurations, and the upper bounds on the
displacement, then a path segment is either classified as collision free
with certainty, or more samples are needed.
There are also other methods, which are not based on sampling of the
path segment. These methods have the advantage of being more exact
than the sampling based techniques, but are, on the other hand, also
more costly. Gilbert and Hong [47] formulated the collision detection
problem as a root finding problem, where the root (if it exists) is the first
collision on the given path of motion. Another technique is to check for
collision between the volumes that are swept out by the geometric objects
as they move, see Xavier [156]. A drawback with methods that use swept-
volumes is that they are conservative; two objects might occupy the same
space, but at different times. Cameron [25] suggested that the sweeping
operation instead should be done in both space and time, generating
four dimensional objects. Unless the motions are very simple, these four-
dimensional objects are difficult to construct.
2.8.2
Local Planner Interface
The purpose of a local planner is to find a connection between two con-
figurations. As mentioned in Section 2.8, most local planners are deter-
ministic and only check a segment that is determined by an interpolation
method. Thus, if a connection exists, clients would know how the con-

52
2 A Framework for Path Planning
necting path segment looks like, and a boolean result is all the information
needed. More elaborate local planners could, however, find a non-trivial
path connecting the two configurations. In such a case, the client must
be given the path also. A uniform interface to all kinds of local plan-
ners must hence return a path also. The resulting interface is shown in
Figure 2.13.
The general interface of the abstract class ConfigConnector makes
it easy to encapsulate almost any planning algorithm and use as a local
planner. One could, for example, use a PRM planner with RRT as local
planner, making it easy to compare the tradeoff between the speed and
connection rate of the local planner.
2.8.3
Example of a Flexible Local Planner
So far we have mostly discussed concepts in isolation from each other.
Here we will see an example of how several concepts can be combined to
form a flexible local planner.
We want to make a local planner that uses the recursive sampling
technique, described in Section 2.8.1, to verify wether path segments are
collision free or not. The recursion proceeds until the distance according
to a given metric is less than a specified threshold. The actual path that
is checked is determined by a given interpolation method.
Most path planning problems only involve finding a collision free path.
Thus, the local planner should have an object for collision detection as
well. However, path planning problems could involve other constraints
as well, as discussed in Section 5.6. A more general approach is therefore
to use an object that represents a binary constraint that answers wether
a given configuration is satisfied. Note how this abstraction also removes
the need to explicitly reference the moving system; it is done inside the
constraint object.
Thus, the local planner should have access to: a metric, an interpo-
lation method, and a binary constraint. The resulting class is named
BinaryConnector
, and is shown in Figure 2.13. As the internal compo-
nents can be changed, the planner is very flexible, and can be used for a
wide range of path planning problems.
If BinaryConnector is used in a PRM planner, the Connect method
should return false and an empty path as soon as a collision is found. If
it is used in an RRT planner, then we would like it to find the path that
moves as far as possible along the segment towards the end configuration,
see Figure 4.1 (b). The local planner can switch to this behavior with

2.9 Chapter Summary
53
metric
constr
BinaryConnector
SetMaxStepSize(…)
SetToPersistent(…)
SetMetric(…)
SetInterpolator(…)
SetConstraint(…)
interp
Metric
BinaryConstraint
ConfigConnector
bool Connect(from, to, path)
Interpolator
Figure 2.13:
A class diagram for local planners.
Note how
BinaryConnector
uses composition to gain flexibility.
the SetToPersistent method. To summarize, we have a flexible local
planner that can be used in both PRM and RRT planners.
2.9
Chapter Summary
In this chapter we have presented the design and implementation of a new
framework for path planning named CoPP. Compared to other frame-
works, CoPP models a richer, yet more decoupled, set of concepts. The
strong decoupling allow implementations of concepts to vary more inde-
pendent of each other. Furthermore, it allows path planning applications
to include only the exact subset of needed concepts. In this respect, the
framework can be seen as a set of LEGO blocks for building path planners,
where blocks that represent the same concept can be switched seamlessly.
The following list summarizes the key features of the framework:
• The framework is easy to extend, either with variations on existing
concepts, or with completely new concepts.
• The framework make it easy to do fair comparisons between varia-
tions of a concept, such as different collision detection algorithms.
• There are few restrictions on the type of planning algorithms that
can be implemented with the framework.

54
2 A Framework for Path Planning
• The framework is system independent.
• Adaptive planners that changes between different algorithms de-
pending on the problem, are easily implemented.
In this thesis we have only considered holonomic systems. The frame-
work is, however, easily extended to handle dynamic systems as well.
The major change is a completely new agent interface, with which the
path planner communicates. Following the design of the MSL framework,
this interface would have methods for applying an input to the dynamic
system over a certain time interval. Integration methods internal to the
agent would take care of computing the resulting state. Admittedly, ex-
isting path planners that want to handle dynamic systems would now
have to change to this new interface. However, as CoPP provides path
planning components and not path planner classes, there is nothing in
CoPP that has to change due to the added agent interface.
The CoPP framework has been tested to compile both with Microsoft
Visual C++ 6.0 and with gcc 3.2.2. The framework has been used to
implement a wide range of RRT planners, a PCD [91] planner, and the
pick-and-place planner in Chapter 5. The framework is also being tested
on a real robot platform in a project about sensor-based path planning
and fault detection.
12
12
This project is carried out at the Centre for Autonomous Systems by: Daniel
Aarno, Frank Lingelbach, and Paul Sundvall.

Chapter 3
A General Robot Model
for Path Planning
Path planning deals with the problem of finding motion strategies for
movable objects or articulated structures. An articulated structure can
be used to model things like, e.g., the motion of a computer animated
character, a robotic manipulator or a complex protein molecule. From
hereafter we will call an articulated structure a robot. The goal in this
chapter is to develop a general robot class that is able to represent a
wide range of different robot types. The main purpose of this class is to
maintain the kinematic structure of the robot and to provide functions
for both the forward and the inverse kinematics.
It is important to have a systematic method for describing the kine-
matic structure of a robot. The next section will go through some of the
proposed notations. It will be pointed out that the standard Denavit-
Hartenberg notation [39] is incapable of describing robots with tree-like
kinematic structures. We use instead the notation of Kleinfinger and
Khalil [68], which in the case of a serial kinematic chain reduces to that
of Denavit and Hartenberg.
To reduce the number of actuators in, e.g., robot hands and hyper-
redundant robots, it is a common design strategy to mechanically couple
joints together. To allow modeling of such robots, the proposed robot
model incorporates the concept of joint couplings, which is discussed in
Section 3.2. Another important idea of the proposed robot model is
that simple robots can be combined to form a more complex robot. For

56
3 A General Robot Model for Path Planning
example, a model of humanoid robot can be seen as if it is composed
several other robots: two arm robots, two leg robots, a trunk robot and
so on. Composition of robots is discussed in Section 3.4.
Section 3.3 deals with the important, but often difficult, problem of
inverse kinematics. The main idea is the separation of inverse kinematics
solvers from the robot itself. This way, users can switch between general
solvers and specific closed form solutions at runtime.
In Section 3.5, the design and implementation of the robot class is
described. In particular, we describe how new inverse kinematic solvers
can added to the framework with a minimum amount of work. A robot
can be associated with a particular solver based on a directive in a robot
description file. An example of a robot description file is given in Ap-
pendix E.5.
The resulting robot class presented in this chapter, is useful for many
types of path planning problems, but we point out that the CoPP frame-
work does not hinge upon it. Other models of motion, e.g., dynamic
systems, are easily added to the framework.
3.1
Notation for Articulated Structures
To be able to instantiate a specific robot, we must have a precise nota-
tion for describing it. A robot is said to be a collection of rigid links and
joints connecting the links with each other. The purpose of a joint is
to constrain the relative motion between two connected links such that
the number of degrees of freedom is somewhere between one and five.
(Allowing the relative motion to have six degrees of freedom, or reducing
it to zero, is hardly interesting). The term lower pair is used to describe
the connection between a pair of bodies when the relative motion is char-
acterized by two surfaces sliding over one another. There are in fact six
possible lower pair joints [36]. When it comes to actuated joints, the
revolute and prismatic joints are by far the most usual. Passive joints,
common in, e.g., parallel robots, are often of the spherical type (ball in
socket). Another common passive joint is universal joint, or Cardan joint.
In the following, we will restrict ourself to revolute and prismatic joints.
This restriction is not severe as any other joints can be formed from a
suitable combination of revolute and prismatic joints.

3.1 Notation for Articulated Structures
57
3.1.1
Denavit-Hartenberg Notation
In order to deal with general articulated structures we must have a sys-
tematic way of describing them. Several methods have been proposed
and among them the Denavit and Hartenberg (D-H) notation [39], and
its variants [36, 115], is the most popular. Below we will give a brief
description of the D-H notation and an augmentation of it to handle
tree-like structures as well.
The D-H notation has become the de facto standard for describing
serial manipulators. The main assumption is that the robot can be de-
scribed as a single kinematic chain and that the joints connecting its links
are either prismatic (translational DOF) or revolute (rotational DOF).
The latter is no real restriction since the other basic joint types can be
modelled as a concatenation of degenerate (i.e., zero link length) revolute
and prismatic joints. A serial robot consists of n + 1 links, where link
0 is the fixed base and link n is the terminal link. The numbering of
joints and links is such that joint i connects links i − 1 and i. To describe
the pose of link i, a frame F
i
is attached to it. The frame is defined
such that the axis Z
i
is coincident with the axis of movement for joint
i, hereafter simply called the joint axis. The axis X
i
is defined as the
mutual perpendicular between joint axes i and i + 1. If the joint axes are
not parallel, this will also uniquely define the origin of frame F
i
since the
closest points on the joint axes are also unique. The axis Y
i
formed by
the right-hand rule to completely specify the frame. Defining the frames
of each link in this manner, it turns out that only four parameters are
needed to describe frame i relative to frame i − 1. These parameters are:
• α
i
: the angle between Z
i−1
and Z
i
about X
i−1
• a
i
: the distance from Z
i−1
to Z
i
along X
i−1
• θ
i
: the angle between X
i−1
and X
i
about Z
i
• d
i
: the distance from X
i−1
to X
i
along Z
i
See Figure 3.1 for a picture of two adjacent frames with the corresponding
parameters. In the literature, these parameters are known as the link
twist, the link length, the joint angle and the joint offset, respectively.
From Figure 3.1 we see that we can derive the transform
i−1
i
T , de-
scribing the pose of link i relative to link i − 1, as a concatenation of two
rotations and two translations:

58
3 A General Robot Model for Path Planning
i−1
i
T = R
X
(α
i
)D
X
(a
i
)R
Z
(θ
i
)D
Z
(d
i
),
(3.1)
where R
X
stands for rotation about the X-axis and D
X
stands for dis-
placement along the X-axis. See, e.g., the book by Craig [36] for details.
Evaluating Equation (3.1) gives:
i−1
i
T =




cos θ
i
− sin θ
i
0
a
i
cos α
i
sin θ
i
cos α
i
cos θ
i
− sin α
i
−d
i
sin α
i
sin α
i
sin θ
i
sin α
i
cos θ
i
cos α
i
d
i
cos α
i
0
0
0
1




(3.2)
For a revolute joint, θ
i
will be the joint variable, while all the other pa-
rameters are constant. For a prismatic joint, d
i
will be the joint variable.
The pose of the last link relative the base of the robot can be found
by applying all the transforms from the base leading up to the last link:
0
n
T =
0
1
T
1
2
T . . .
n−1
n
T
(3.3)
Note that this equation also gives the pose of all the intermediate links
relative the base.
One of the main reasons for the popularity of the D-H notation is its
simplicity and compactness; for any link we only need to specify three
parameters, while the fourth one is the joint variable. This compactness
comes from the clever choice of the frames fixed to each link, reducing
the number of parameters to a minimum. The main drawback of the D-H
notation is that it runs into trouble when trying to represent kinematic
chains with more than one branch, i.e., a link has more than two joints.
If a link i connected to joint i, also is connected to m other joints, the
definition of frame i fixed to this link becomes ambiguous. This is because
X
i
is defined from the relationship between joint axis i and the next joint
axis. In this case we have m choices for the next joint axis. If we choose
one of the m joints, say joint j, as the next joint axis, then the frame for
link i and j can be defined as usual, but the frames for the other m − 1
one links cannot be defined using the standard D-H parameters because
X
i
does not point towards the right joint axis.
To overcome this problem, Sheth and Uicker [130] (S-U) proposed a
new notation for robots with tree-like or closed loop kinematic chains.
In their method, two frames are assigned to each link, and the result-
ing transformation matrix can be seen as composed of two parts: One

3.1 Notation for Articulated Structures
59
Axis i − 1
Axis i
α
i
a
i
d
i
θ
i
Z
i−1
X
i−1
Z
i
X
i
L
i−1
L
i
Figure 3.1:
Illustration of the Denavit-Hartenberg notation. As joint i
is a revolute joint, θ
i
is the joint variable.
constant transform related to the shape of the link, called the shape ma-
trix, and one variable transform representing the joint motion, called the
joint matrix. In the S-U notation, six parameters are needed to define
the shape matrix. The joint type and the joint variable defines the joint
matrix. The large number of frames, two for every link, and parameters
makes the S-U notation much more complicated than its D-H counter-
part. Because of this, the D-H notation has always been the choice unless
studying robots with loop kinematics. Thus, for a unified notation to be
used for both serial robots and robots with loop kinematics, it should re-
duce to the simple D-H notation in the case of a serial manipulator. This
is exactly the case of the notation proposed by Kleinfinger and Khalil [68]
(K-K). In the case of a serial manipulator, their notation coincide with
that the D-H notation described above, but for a branching link, two ex-
tra parameters, together with the usual D-H parameters, are needed for
each extra frame. Next we will describe the K-K notation in detail for a
tree-structure robot. In the case of a serial manipulator the notation is

60
3 A General Robot Model for Path Planning
identical to D-H notation described above.
3.1.2
Kleinfinger-Khalil Notation
It is assumed that the robot is composed of n + 1 links and n joints.
Due to the tree-like structure of the robot, it can have m end-effectors,
where an end-effector is a leaf-node of the tree representing the robot’s
kinematic structure. Each link and joint will have a number identifying
it. The numbering is done in the following way:
• The base link will always be link 0.
• The joint and link numbering are both increasing when traversing
the tree from the base to an end-effector.
• Joint i connects link a(i) and link i, where a(i) is the number of
the link preceding link i when coming from the base. Link i moves
with joint i.
• Frame i is considered fixed with respect to link i.
This defines how to number links, joints and frames. The next steps
tell how to define the frame associated with each link. As long as a link
has only two joints, the frames are defined just as in the D-H case above.
In the case that the link i has more than two joints, find the mutual
perpendicular between Z
i
and each of the succeeding axes Z
j
on the same
link, where i = a(j) and j = k, l, . . . . Let one of these perpendiculars
define the X
i
axis. It was suggested in [68] that one should choose the
perpendicular corresponding to the joint on which the longest branch was
articulated. Together with Z
i
, the other perpendiculars, lets call them
X
′
i
, X
′′
i
, . . . , form a set of intermediate frames F
′
i
, F
′′
i
, . . . that are fixed
with respect to link i. The key idea of the K-K notation is that only
two parameters, ǫ and γ, are needed to describe a transform that takes
the frame F
i
to one of the frames F
′
i
, F
′′
i
, . . . . Once there, the usual
D-H parameters can be used. Thus the K-K notation can be seen as
augmenting the D-H notation. The two extra parameters are defined as
• ǫ
i
: the distance from X
i
to X
′
i
along Z
i
• γ
i
: the angle between X
i
and X
′
i
about Z
i
See Figure 3.2 for an example. If ǫ
i
= 0 and γ
i
= 0, then Equation (3.2)
changes to

3.2 Modeling Joint Couplings
61
i−1
i
T =




Cγ
i
Cθ
i
− Sγ
i
Cα
i
Sθ
i
−Cγ
i
Sθ
i
− Sγ
i
Cα
i
Cθ
i
Sγ
i
Cθ
i
+ Cγ
i
Cα
i
Sθ
i
−Sγ
i
Sθ
i
+ Cγ
i
Cα
i
Cθ
i
Sα
i
Sθ
i
Sα
i
Cθ
i
0
0
Sγ
i
Sα
i
a
i
Cγ
i
+ d
i
Sγ
i
Sα
i
−Cγ
i
Sα
i
a
i
Sγ
i
− d
i
Cγ
i
Sα
i
Cα
i
d
i
Cα
i
+ ǫ
i
0
1




(3.4)
As pointed out by Craig [36], the D-H notation is ambiguous. That
is also the case of the K-K notation. Below some guidelines from [36]
are given for how to choose the frames in case a parameter is not well-
defined. To begin with, the choice of Z
i
is ambiguous, since there are
two directions in which we can point Z
i
when making it coincident with
joint axis i. In the case of joint axis a(j) and j being parallel, the choice
of origin location for frame a(j) is arbitrary. Here Craig [36] suggests to
choose the origin such that d
a(j)
becomes zero, which is possible only if
the joint is revolute. If the two joint axes intersect, e.g., in the case of a
spherical wrist, then the mutual perpendicular is not defined. In such a
case, the X
a(j)
axis is chosen to be perpendicular to the plane containing
both joint axes, leaving two choices for the direction of X
a(j)
. The origin
of frame a(j) is located at the intersection of the two joint axes. The
choice of the first frame of the robot is arbitrary, but by convention [36]
it is chosen such that F
0
= F
1
when the first joint variable is zero. This
will make sure that a
1
= 0 and α
1
= 0. Additionally, d
1
= 0 for a
revolute joint and θ
1
= 0 for a prismatic joint.
It was shown in [68] that the K-K notation can also be applied in
the case of a robot with loop kinematics. By cutting each loop at an
arbitrary joint, such a robot can be transformed to an equivalent tree-
structure robot if a loop-closure constraint is added for each loop that is
cut.
3.2
Modeling Joint Couplings
To reduce the number of actuators it is common to introduce couplings
between joints, such that a single actuator can drive two or more joints.
An example of this is the BarrettHand, where four actuators are used to
actuate eight joints. This design made it possible to mount all actuators

62
3 A General Robot Model for Path Planning
α
j
Z
i
a
j
ǫ
k
γ
k
L
i
L
i
Z
j
θ
j
d
k
X
i
L
j
L
k
Z
k
X
k
X
′
i
X
j
Figure 3.2:
Illustration of the Kleinfinger-Khalil notation. The figure
is from [44], with kind permission from Lorenzo Fl¨
uckiger.
and control electronics inside the wrist, avoiding troublesome solutions
like tendon actuation with the actuators far away from the hand. Mod-
eling a simple parallel-jaw gripper also becomes easier if we allow joint
couplings; with a joint coupling we can enforce the two jaws to always
move an equal amount, but in opposite directions.
Here joint couplings are modelled using the concepts of active and
passive joints, where an active joint can control any number of passive
joints. We consider only linear couplings, meaning that the input to a
passive joint is determined by the input to the active joint and a gear
factor. It follows that the number of degrees of freedom for a robot is
equal to the number of active joints.
There are examples of tentacle-like robots in the literature, see, e.g.,
Hannan and Walker [52] and Immega and Antonelli [59]. Such robots of-
ten have very low stiffness, making them suitable for interactions with hu-
mans. Moreover, their extreme flexibility allow them to wrap around ob-
jects, i.e., whole arm manipulation. The robot of Hannan and Walker [52]
consists of a flexible spine, which has 16 two DOF joints. The spine is
divided into four segments, and each segment is controlled by two pairs of
tendons. When the robot is actuated, the curvature along each segment
is approximately constant. Using the proposed joint coupling model, we
can easily build a robot model that has the same properties; each segment

3.2 Modeling Joint Couplings
63
Figure 3.3:
An approximation of a continuum robot using coupled
joints. The robot is divided into four segments, with eight revolute joints
in each segment. The two first joints in each segment control the oth-
ers such that the curvature is constant along each segment. In total the
robot has eight degrees of freedom.
consists of eight revolute joints, where the first two control the others.
With all gear factors equal to one, we achieve constant curvature along
each segment. The resulting ’elephant trunk’, see Figure 3.3, has eight
degrees of freedom.
Modern industrial manipulators often have loops in the kinematic
structure to increase the stiffness and the payload capacity of the manip-
ulator. Solving the kinematics of manipulators with closed loop kinematic
chains is not so straightforward in that it requires satisfying the loop con-
straints. Often iterative solution techniques are required. However, there
is an important sub-class of such manipulators that we can handle with-
out, again using joint couplings. Robots like the Acma SR400 robot,
see [67], have a loop in the shape of a parallelogram. When such a par-
allelogram is deformed, all the joint angles change by the same amount.
Thus, we can model a parallelogram structure using one active joint that
control three passive joints, see Figure 3.4.
In summary, joint couplings are useful to model:
• Robots with physical joint couplings
• Snake-like continuum robots
• Robots with a parallelogram structure

64
3 A General Robot Model for Path Planning
Figure 3.4:
A closed loop kinematic structure where the loop constraint
is satisfied using joint couplings. Due to the parallelogram structure, the
relationships between the joint angles are linear.
3.3
Inverse Kinematics
The problem of finding the configuration that takes the robot end effec-
tor to a given position and orientation is called the inverse kinematics
problem. This is generally a much harder problem to solve than the
forward kinematics problem, which was just a concatenation of all the
transformations leading to the end effector. The inverse kinematic prob-
lem may have zero, multiple or infinite number of solutions, depending
on the wanted pose and the structure of the robot.
Most path planning examples presented in the literature are of the
type “find a path from q
a
to q
b
”, where q
a
and q
b
are two points in the
robot’s configuration space. However, when we specify robot tasks we
hardly want to deal with details such as joint angles. Instead we want to
give specifications like “robot, go to the kitchen” or “robot, pick up the
bottle”. This leaves it up to the robot (planner) to figure out the joint
angles needed to reach the locations necessary for the task.
Clearly, for high-level task specifications we need methods for solving
the inverse kinematics problem. It would therefore be useful if the robot
class could provide a general solver. However, numerical solvers tend to

3.3 Inverse Kinematics
65
be slow and they often only find one solution even if there are more.
Allowing only one solver would therefore be very inefficient for those
robots where we have a closed form solution. The method chosen here
is to have a repository of various solvers, from which users can choose at
runtime.
In the following, we will discuss a straightforward method to numeri-
cally solve the inverse kinematics problem. We will test it on two robots
and also compare its efficiency to a closed form solution. Finally we will
see how different solvers coexist in CoPP.
3.3.1
The Jacobian
Any numerical method for solving the inverse kinematics for a general
robot will most likely need the manipulator Jacobian. For robots living
in R
3
, the Jacobian will be a 6 × m matrix, relating the m joint speeds
to the instantaneous velocity of the end-effector frame:
˙
X
= J(q) ˙q,
(3.5)
where the end-effector velocity is composed of both the linear and the
rotational velocities, ˙
X
= V
T
Ω
T T
. If not stated otherwise, the end-
effector velocities are measured relative the base-frame of the robot. The
column i of the Jacobian can be interpreted as the velocity of the end
effector when joint i has unit velocity and all other joints are locked. This
suggests that J can be computed column by column, looking at one joint
at a time. Below we will look at the velocity contributions to the end-
effector frame from a prismatic joint and a revolute joint, respectively.
The velocities will be expressed relative the end-effector frame itself, but
a simple transformation can express them relative another frame, e.g.,
the base frame.
Joint k produces the linear velocity
n
V
k
and the angular velocity
n
Ω
k
with respect to the frame n. If joint k is prismatic, then we have [67]:
n
V
k
=
n
Z
k
˙q
k
(3.6)
n
Ω
k
= 0
(3.7)
where
n
Z
k
is the z-axis of frame k expressed in frame n.
If joint k instead is a revolute joint we get [67]:

66
3 A General Robot Model for Path Planning
n
V
k
= (
n
Z
k
×
n
P
k
) ˙q
k
(3.8)
n
Ω
k
=
n
Z
k
˙q
k
(3.9)
where
n
P
k
is the vector connecting frame k to frame n, expressed relative
frame n. Introducing the auxiliary variable σ
k
, which is defined as
σ
k
=
1
if joint k is prismatic,
0
if joint k is revolute,
(3.10)
the velocity contribution from joint k can be written in the general form
n
V
k
= [σ
k
n
Z
k
+ ¯
σ
k
(
n
Z
k
×
n
P
k
)] ˙q
k
,
(3.11)
n
Ω
k
= ¯
σ
k
n
Z
k
˙q
k
,
(3.12)
where ¯
σ
k
= 1 − σ
k
. Summing the contributions from all joints from the
base to the end-effector, we get the Jacobian as
J
=
σ
1
Z
1
+ ¯
σ
1
(Z
1
× P
1
) . . .
σ
n
Z
n
+ ¯
σ
n
(Z
n
× P
n
)
¯
σ
1
Z
1
. . .
¯
σ
n
Z
n
,
(3.13)
where the leading superscript n has been omitted.
The Jacobian for a system with coupled joints will of course be dif-
ferent from that of the non-coupled system, since one or several joint
velocities can expressed in terms of the velocity of other joints. If we
have a coupling between joint i and j such that ˙θ
i
= k
ij
˙θ
j
, where k
ij
is
the ’gear factor’, then it is seen that the jth column is given by
c
j
= c
j
+ k
ij
c
i
.
(3.14)
The ith column is discarded, so each coupling will reduce the number of
columns in the Jacobian by one.
Note that the Jacobian in Equation (3.13) gives the end-effector ve-
locities relative to the end-effector frame itself. To express the velocities
relative another frame F
i
, we can use the following transformation:
i
J
=


i
n
R
0
3×3
0
3×3
i
n
R


n
J
,
(3.15)
where
i
n
R
is the rotation matrix that specify the orientation of the end-
effector frame relative frame F
i
and
n
J
is from Equation (3.13).

3.3 Inverse Kinematics
67
3.3.2
A Numerical Solver Based on the Pseudo-
Inverse
Equation (3.5) relates joint velocities to end-effector velocities. It can
also provide us with a differential model of the manipulator:
∆X = J(q)∆q.
(3.16)
This differential model will remain valid as long as ∆X and ∆q are
sufficiently small. The main idea when solving the inverse kinematics
numerically is to let ∆X be the pose error and use Equation (3.16) to
solve for ∆q. However, just inverting J will in most cases not work
because the Jacobian may not be a square matrix. Instead we use the
pseudo-inverse J
+
of J. The pseudo-inverse of a matrix does always exist
and can easily be computed from the singular-value decomposition of the
matrix, see Golub and Van Loan [49]. Furthermore, it can be shown that
the solution given by
∆q = J
+
∆X
(3.17)
minimizes the residual J∆q − ∆X
2
.
Let
0
n
T
d
denote the desired end-effector pose and
0
n
T
c
the current end-
effector pose. The following algorithm can be used to iteratively solve for
the inverse kinematics problem [67]:
• Initialize q
c
by the current configuration.
• Compute
0
n
T
c
using the forward kinematics.
• Compute the position error dX
p
and the rotation error dX
r
, rep-
resenting the difference between
0
n
T
d
and
0
n
T
d
.
• If dX
p
and dX
r
are sufficiently small, then q
d
= q
c
and the itera-
tion terminates.
• To remain in the validity domain of the differential model we must
introduce the thresholds S
p
and S
r
on dX
p
and dX
r
respectively
such that:
–
If dX
p
> S
p
, then dX
p
=
dX
p
dX
p
–
If dX
r
> S
r
, then dX
r
=
dX
r
dX
r
• Compute the Jacobian
0
J
(q
c
), denoted as J.

68
3 A General Robot Model for Path Planning
• Compute the joint variation dq
=
J
+
dX
, where dX
=
dX
T
p
, dX
T
r
T
.
• Update the current joint configuration: q
c
= q
c
+ dq.
• Return to the second step.
If it were not for the two thresholds S
p
and S
r
, this algorithm would
run efficiently for robots of any size. According to [67], the values 0.2
meter and 0.2 radians are acceptable for industrial robots of typical size.
For robots considerably larger, these small thresholds would make the
algorithm take small and inefficient steps. Tests by the author have also
shown that too large values of S
p
and S
r
can make the iterations vary
erratically a long time before converging.
Another, more severe drawback of this simple algorithm is that takes
no concern to joint limits; the solution provided by the algorithm can
have joint angles that are outside the valid range for the robot. As long
as the robot only contains revolute joints with 2π range this is not a
problem, but as the joint ranges decrease, more and more solutions fall
outside the joint limits.
A Comparison Between two Solvers
In the following we will describe an experiment setup to compare the
general solver against a closed form solution for the Puma 560 robot.
The closed form solution is based on [116]. Each solver was called 10,000
times, each time with a different desired end-effector pose. The determin-
istic pose-sequence was determined using the Halton class described in
Section 2.7; applying the forward kinematics on each configuration from
the Halton sequence gave the corresponding end-effector pose. Before
the call to the solver, the robot was moved back to the previous config-
uration. This had of course no effect on the closed form solution, but
it makes the test repeatable as the behavior of the general solver is af-
fected by the initial configuration. After each call to the solver, the total
number of solutions was incremented by the number of solutions found
at the current call. If no solution at all was found, this was reported as
a failure; as each end-effector pose was generated from the forward kine-
matics, there should be at least one solution to each query. The results,
together with the time needed for all queries, is reported in Table 3.3.2.
Not only is the closed form solution much faster, (by a factor 34), but

3.3 Inverse Kinematics
69
closed form solution
general solver
time [s]
0.641
21.7
success rate
100%
52.7%
solutions / query
4.8
0.52
Table 3.1:
A comparison between the closed form solution and the gen-
eral solver for the Puma inverse kinematics. The table shows the outcome
of 10,000 calls to each solver. Only once did the general solver fail be-
cause it exceeded the maximum number of iterations. The other times it
failed because the generated configuration violated the joint limits.
also more reliable; it never failed to find a solution, and it reported on av-
erage 4.8 solutions to each query. The general solver, on the other hand,
did not look for multiple solutions and found a solution to only about
half the queries. The reason for the high failure rate is the algorithms
ignorance of joint limits; only once did it not converge to the desired
end-effector pose within the maximum number of iterations, which was
set to 1,000, but the found solution often violated the joint limits of one
or more joints. It was found that repeating a failed query with a different
initial configuration made the solver converge to a valid configuration.
Generating Constrained Motions
As seen from the previous example, the iterative solver has great prob-
lems with joint limits. Considering that the joints of the Puma have
large ranges, we might expect that the problem becomes more severe for
robots with more joints and smaller joint ranges. Therefore we might
ask ourselves if our simple solver can be of any good use at all if we
have robots with joint limits. Experiments have shown that if the initial
configuration is close to a solution configuration, then the iterative solver
converges very quickly. This suggests that the solver should be suitable
for iterative generation of constrained end-effector motions; if we know
the initial configuration and move along the end-effector trajectory with
small steps, then the solver should have a better chance of converging to
each successive configuration.
To test this idea, the solver was used to generate different constrained
trajectories for the hyper-redundant trunk robot shown in Figure 3.3.
With the base fixed, this robot has eight degrees of freedom and average
range of each joint is only 29 degrees. The constrained motions corre-
sponded to: linear end-effector paths, fixed orientation and combinations

70
3 A General Robot Model for Path Planning
Figure 3.5:
The end-effector is constrained to move along a straight
line with constant orientation. The motion was incrementally generated
using the general solver for the inverse kinematics.
of them. In all cases, the solver had no problem with the joint limits
until the robot reached really awkward configurations. Also, the motions
were generated very quickly as the number of iterations for solving each
step was usually very small. An example is shown in Figure 3.5, where
the end-effector was forced to follow a linear path with constant orienta-
tion. This shows that the general solver can be of use in path planning
problems where we have constraints on, e.g., the orientation.
3.4
Robot Composition
Consider the mobile manipulator in Figure 3.6. If we consider the mo-
bile base, the arm and the hand together, the system has a total of 13
degrees of freedom. Sending a model of this system to a grasp planner

3.4 Robot Composition
71
would seem very inappropriate, as there are many degrees of freedom
available in the model that are not necessary for the task. Likewise, for
a navigation task we would only want to consider the degrees of freedom
corresponding to the base. Thus, we would like to have a mechanism that
easily separates out the parts of the robot that are relevant to the task at
hand. A natural approach would be to see the mobile manipulator as a
complex robot composed of several sub-robots, where the sub-robots are
the mobile platform, the arm and the hand, respectively. An even finer
granularity can be achieved if we also see the fingers of the hand as sub-
robots. It would thus be nice if the robot class would support hierarchical
compositions of robots into more complex robots. The robot would then
have methods for retrieving a particular sub-robot. To a client, this sub-
robot can be viewed as a complete robot on its own. The only difference,
not visible from the outside though, is that movements of this sub-robot
cause changes that are propagated to all nodes in the kinematic graph
that appears below the sub-robot. As an example, the arm and hand
are not sub-robots of the platform, but moving the platform will change
the location of them. When propagating the movements to the attached
robots one can take advantage of the fact that no joint has moved, so the
resulting movement is just a translation and a rotation of each sub-robot
as a whole.
Extracting a sub-robot from a complex robot can be seen as a way of
temporarily locking some joints of the complex robot, thereby reducing
the effective number of degrees of freedom. This could be very useful
when planning, e.g., a pick-and-place task; when the hand has grasped
the object we only consider the sub-robot that correspond to the arm.
The hierarchical robot model has a lot of other advantages also. Lets say
we want to model a humanoid. Now, applying InverseKinematics to the
whole humanoid would not make much sense. If we instead apply the
same function on the sub-robot “Head”, we would get the joint angles
for making the robot look in the specified direction. Likewise, applying
InverseKinematics to “LeftArm” would give the joint angles necessary for
the arm to reach the specified position. Thus, by extracting a particular
sub-robot we can, as a useful side effect, in an easy way express our
intentions. If the hierarchical composition of robots is built into the
syntax for generating robots from text files we also gain the benefits of
modularity. Returning to our humanoid again: If the left and right leg
are identical apart from the way they are attached to the hip, then it
would both be error prone and unnecessary amount of work to define
them both in the same file. Instead the legs could be defined in a single

72
3 A General Robot Model for Path Planning
Obelix
XR4000
ArmHand
Puma
BarrettHand
13 DOFs
10 DOFs
3 DOFs
4 DOFs
6 DOFs
Figure 3.6:
The left-hand figure shows Obelix, a mobile robot that
consists of a Nomadics XR4000 platform, a Puma manipulator and a
Barrett hand. The right-hand figure shows how this complex robot can
be seen as a composition of simpler robots. The arrowheads show how
sub-robots are attached to each other. Moving a sub-robot will cause any
attached robot to follow the motion as a single rigid body.
file “leg.rob”, which is then included twice in “humanoid.rob”. The only
thing differing between the two include statements would be the name of
the robots and their transformation with respect to the frame of the hip.
Another example: Suppose we want to change the hand on on our model
of the mobile manipulator in Figure 3.6 to a parallel-jaw gripper. If the
hand and the parallel-jaw gripper are defined in two separate files, then
all we have to do is change a file name in one include statement in the
file defining the mobile manipulator.
3.5
Design and Implementation
In this section we describe the design of the class Robot and some imple-
mentation details.
Robots are modeled as a set of moving coordinate frames. The rela-
tionships between the frames can be described by a graph that has no

3.5 Design and Implementation
73
loops. This graph is implemented in terms of RobNode objects, that can
either be joints, or a coordinate frame that is fixed with respect to its
parent frame, see Figure 3.7. To each frame, we can attach any number
of geometric objects, the robot’s geometric links.
The Robot class itself, maintains a graph of RobNode objects and
possible sub-robots. A robot object also has an object representing a
solver for the inverse kinematics. This solver object can either be a
general numerical solver, or a solver dedicated for a particular robot.
The structure of a robot can be hardcoded in the program, but it is
more flexible to read a robot-description file and let the program build
the corresponding robot. For an example of a simple robot-description
file, see Appendix E.5.
3.5.1
Self-Collision Table
When planning for kinematic chains, not only do we have to check for col-
lisions between the links and the environment, but also between the links
themselves. That is, we have to look for robot self-collisions. For a robot
with n physical links, there are n(n − 1) link pairs that can collide with
each other. However, adjacent links that are connected at a joint can be
considered to be in contact all the time, and does not have to be checked.
Furthermore, joint limits and design of robots tend to eliminate many of
the remaining link pairs. So which link pairs should be considered for
self-collision check is very dependent on the particular robot. Therefore
a robot specification can contain a user defined self-collision table. As
each link geometry is required to have a unique identifier, this table is
simply a list of string pairs. An example of a self-collision table is shown
in Figure 3.8. If robot definitions from several files are put together to
a complex robot, their self-collision tables are automatically merged as
well.
3.5.2
Multiple Inverse Kinematics Solvers
As mentioned in Section 3.3, high level planning will, at some point,
involve solving the inverse kinematics problem. To meet this need, CoPP
provides several inverse kinematics solvers. Which one is actually used is
specified by the user in the robot description file. Here we will describe
the interface to these solvers. It will also be shown that CoPP is designed
such that users can add their own solvers to the framework with a minimal
effort.

74
3 A General Robot Model for Path Planning
RobNode
Update( )
GetWorldFrame( )
GetGeoms( )
AddChild(node)
Accept(RobNodeVisitor v)
Transform pose_rel_parent
parent->GetWorldFrame() *
pose_rel_parent;
Geom
AttachToFrame(...)
link_geoms