Robot Path Planning: An Object-Oriented Approach

Robot
Move(config)
Inverse(pose, solution)
SetInvKinSolver(solver)
NumDOF( )
GetEndEffectorPose( )
SubRobotsBegin( )
subrobots
nodes
parent
children
RobJoint
SetParams(joint_params)
SetJointType(…)
SetJointValue(val)
GetLimits(min, max)
RobFrame
RobFrame(pose_rel_parent)
Figure 3.7:
Class diagram for the Robot class.
coll_table {
"puma_link_0" and "puma_link_3"
"puma_link_0" and "puma_link_4"
"puma_link_1" and "puma_link_4"
}
Figure 3.8:
An example of a (partial) self-collision table for a Puma
robot.

3.5 Design and Implementation
75
The number of solutions to the inverse kinematics problem can be
zero, finite or infinite. In the case the problem has a finite number of
solutions, closed form solutions are often able to find them all, whereas
iterative solvers mostly find the solution that is closest to the initial
configuration. It is often of interest to know if the problem has multiple
solutions, see, e.g., the pick-and-place planner in Chapter 5. Therefore
it was decided that the return value from the solvers must be an object
that can hold multiple solutions. So, instead of returning just a single
robot configuration, inverse kinematics solvers return objects of the class
InvKinSolution
, see Figure 3.9.
Conceptually, the robot class should provide the interface for the in-
verse kinematics. In the class diagram in Figure 3.7 we can see that this
is the case, as the class Robot provides the method Inverse. However,
because we want to vary the way we solve the inverse kinematics prob-
lem without affecting the robot class, all the interface in Robot does is to
forward the request to a solver object. The abstract class InvKinSolver
serves as a base class for all solvers. Because solvers need to have infor-
mation about the robot for which the inverse kinematics will be solved,
InvKinSolver
provides the virtual method SetRobot, see Figure 3.9.
This method is mostly called by the robot object itself when it is given
a new solver. See the interaction diagram in Figure 3.10 for a detailed
description on how a Robot object is given a new solver. From the inter-
action diagram we can see that the robot owns and maintains a copy of
the provided solver. Thus, we can say that we have effectively changed
the guts of the Robot object. This is an example of the Strategy Pattern
as described in [45].
So far we have seen how we can represent different solvers and how we
can assign a specific solver to a robot. To put this machinery to use, we
would like to have a mechanism that allows users to specify the desired
solver in the robot description file. Essentially, this is nothing but a map-
ping from text strings to solver objects. However, if we put this mapping
in a single file, adding new solvers will require users to read and modify
code that has nothing to do with the solver itself. Furthermore, as the
file containing the mapping creates a lot of unnecessary dependencies,
this solution is not so scalable when it comes to maintenance and compi-
lation times. If we require that adding a new solver should be as simple
as possible, we have to look for another solution. Instead of putting the
mapping in a single file, we can put it in a single object. Solvers that we
want to use will have to register themselves to this object, which will work
as a repository for the available solvers. To make sure all solvers register

76
3 A General Robot Model for Path Planning
InvKinRepository
+ RegisterSolver(name, solver)
+ GetSolver(name)
+$ GetInstance( )
- InvKinRepository( )
$ InvKinRepository* the_instance
...
return *the_instance;
solvers
InvKinSolution
+ NumSolutions( )
+ GetSolutions( )
- Clear( )
- AddSolution(config)
num_solutions
RobotConfigs configs
InvKinSolver
operator( )(Transform p, InvKinSolution s)
Clone( )
SetRobot(robot)
friend
PumaKinematics
InvKinNull
InvKinGeneral
Figure 3.9:
Class diagram for the inverse kinematic solvers. Note that
the constructor of InvKinRepository is private, which is key to enforce
a single unique instance.
to the same repository, we must enforce the repository to be a single,
unique object. Furthermore, we want to make it possible for solvers to
register themselves to the repository from anywhere in the program. The
Singleton Pattern meet all three of these requirements, as its intent is to
“ensure a class only has one instance, and provide a global point of access
to it” [45]. The class InvKinRepository, Figure 3.9 uses the Singleton
Pattern to enforce a single instance, which maintains a dynamic mapping
from strings to specific solvers.
Using InvKinRepository, all available solvers can register themselves
once and for all at program startup, before main is entered. This way
we are guaranteed that all solvers are available immediately as we enter
main
. Figure 3.11 shows an example of how a new solver, in this case

3.5 Design and Implementation
77
aSolver
aRobot
anotherSolver
aClient
SetInvKinSolver(aSolver)
new PumaKinematics(*this)
SetRobot(*this)
Clone( ) 
Figure 3.10:
Interaction diagram showing how a robot object is given
a new solver for the inverse kinematics. Note that the robot object owns
the new solver.
namespace {
const PumaKinematics prototype;
const bool isRegistered =
InvKinRepository::GetInstance().RegisterSolver("Puma560Kin",
prototype);
} // anonymous namespace
Figure 3.11:
An example of how a specific solver for the inverse kinemat-
ics is added to the repository of available solvers. Note that this code is
executed at program startup, before main is entered. The solver can now
be retrieved anywhere in the program using the string “Puma560Kin”.
PumaKinematics
, is added to the repository. Important to note is that
the code for registering the solver resides in the file containing source for
PumaKinematics
, and not in the file containing the main function. Thus,
it is the writer of the solver, and not the user, who is responsible for
registering it. The namespace directive in Figure 3.11 has the effect to
make the global variables prototype and isRegistered invisible outside
the source file of PumaKinematics.

78
3 A General Robot Model for Path Planning
3.6
Chapter Summary
In this chapter we have presented a general robot model, particulary
suited for path planning applications.
The Khalil-Kleinfinger nota-
tion [68] is used to describe the kinematic structure of robot. This no-
tation is an extension of the Denavit-Hartenberg notation [39] to allow a
consistent description of tree-like kinematic chains. The main features of
the robot model in this chapter are:
• Tree-like kinematic chains
• Multiple inverse kinematics solvers
• Hierarchical robot composition
• Joint couplings
The possibility to see a complex robot as a combination of simpler
robots introduces several possibilities. The modular approach allows a
robot’s definition to be spread over several files. With different files for
different robot hands, changing the end-effector tool of a robot becomes
easy as changing an include file in the definition of the composed robot.
The most useful aspect of robot composition is that it allows the extrac-
tion of the degrees of freedom that are needed for the task at hand. If
we want a humanoid robot to look in a particular direction, we could for
example extract a subrobot “Head” corresponding to the head and the
neck. Another example is given in Chapter 5, where robot composition is
used in the context of pick-and-place tasks. During the approach phase,
the arm and the hand are considered as one robot. During the transport
phase, however, the joints of the hand must not move, and hence only
the arm is considered. Here robot composition is useful to specify which
degrees of freedom should be used.
A future improvement of the robot model would include a more gen-
eral concept of joint couplings. Currently, joint couplings only allow lin-
ear relationships between the controlling joint and the controlled joints.
There are, however, important cases of nonlinear couplings that would be
useful to consider. The slider-crank mechanism in Figure 3.12 is a funda-
mental machine element that can be found in, e.g., combustion engines,
door closing mechanisms, and in excavator arms. This mechanism can be
considered as planar, closed kinematic chain with one degree of freedom.
In excavator arms, this mechanism is used to convert a translational mo-
tion, induced by a hydraulic cylinder, to a rotational motion. Given the

3.6 Chapter Summary
79
Figure 3.12:
A slider-crank mechanism common in, e.g., combustion
engines and excavator arms. The mechanism can be seen as a closed loop
kinematic chain with one degree of freedom.
position of the hydraulic cylinder, there exists closed form solutions for
the other joint angles.
Other joint couplings could be used to enforce closure constraints, in
which case we a robot with loops in the kinematic structure. It is rec-
ommended, however, that closed-loop kinematic chains are modeled in a
class that is separate from the Robot class: Putting to much functionality
in one and the same class can make it monolithic, complicated, and hard
to use.
A file format and a parser have been developed to allow robot de-
scription files. An example of a description file is given in Appendix E.5.

Chapter 4
Augmenting
RRT-Planners with Local
Trees
Rapidly-exploring Random Trees (RRTs), introduced by LaValle in
1998 [78], has been recognized as a very useful tool for designing efficient
single-shot path planners. Bidirectional RRT-planners work by growing
two configuration-space trees towards each other; one tree is rooted at
the start configuration, and the other is rooted at the goal configuration.
These planners have shown to be efficient for a wide range of problems
and they are even able to solve problems involving differential constraints.
In this chapter we present a method for augmenting bidirectional
RRT-planners with local trees. In the presented examples we will see that
the addition of local trees greatly improves the performance for problems
involving several narrow passages. For problems where local trees are not
beneficial, there is a performance degradation due to the time spent on
growing the local trees. However, compared to the performance gain for
the other examples, this degradation is small.
The next section will give a brief introduction to RRTs. Section 4.2
presents the new algorithm that uses local trees. This algorithm is im-
plemented using the framework described in Chapter 2. The resulting
planner is tested on some problems and the results are presented in Sec-
tion 4.3. The chapter ends with a summary and suggestions for future
research.

82
4 Augmenting RRT-Planners with Local Trees
q
near
q
rand
(a)
q
near
q
rand
q
stop
(b)
Figure 4.1:
(a) If the path between q
near
and q
rand
is collision free, then
q
rand
becomes the new vertex in the tree. (b) If the path to q
rand
is not
collision free, then q
stop
becomes the new vertex. Note that the random
configuration need not be collision free.
4.1
The RRT-ConCon Algorithm
The RRT concept was initially proposed as a tool for solving problems
involving differential constraints [78], i.e., kinodynamic planning. Shortly
after, Kuffner and LaValle [72] showed that RRTs were efficient for holo-
nomic systems as well.
Exploration
Starting from a given configuration, an RRT T is incre-
mentally grown to efficiently explore the configuration space. In each
iteration, a random
1
configuration, q
rand
, is generated. From the vertex
in T that is closest to q
rand
(according to some appropriate metric M),
a new edge is grown towards q
rand
. If no collision is found on the way
towards the random configuration, then q
rand
becomes a new vertex in
T , see Figure 4.1 (a). If, on the other hand, q
rand
cannot be reached
because of an obstacle, then the new edge extends as close as possible
towards the obstacle. In this case, the stopping configuration is the new
vertex to be added in T , see Figure 4.1 (b).
It was shown in [78] that this method leads to a Voronoi-biased growth
1
The configurations need not be random. As pointed out in [79], deterministic,
dense sequences can be used as well.

4.1
The RRT-ConCon Algorithm
83
of T . This means that vertices with a large Voronoi cell
2
have a larger
probability for being extended. This is a good property as large Voronoi
cells represent unexplored areas of the configuration space. It was also
shown in [72] that in the limit, as the number of vertices in T tend to
infinity, the coverage of the configuration space is uniform. Thus, in the
limit there is no bias.
In the case of a dynamic system, connecting two configurations can
lead to a nontrivial control problem. For such problems, the growth
towards q
rand
is incremental, applying a constant control signal over some
time interval ∆t. Wether this strategy reach q
rand
is not so important;
the growth of T is still Voronoi biased. For examples on kinodynamic
planning with this approach, see [78, 81, 82].
Bidirectional Search
It is clear that just growing an RRT will not
solve any path planning problem; it will only explore the C-space. As
described in [79], the simplest way to use an RRT in a planner is to
introduce a bias in the random configurations, such that the goal config-
uration is drawn with some probability p
goal
. This means that the RRT
will both explore and try to reach the goal configuration. The probability
p
goal
that determines the bias towards the goal configuration can be seen
as control parameter that controls the planner’s behavior: A high bias
towards the goal gives a greedy planner that easily get stuck. A low bias,
on the other hand, gives a planner that might spend too much time on
exploration.
Kuffner and LaValle [72] suggested the use of two trees instead of
one, where one tree is rooted at the initial configuration, and the other is
rooted at the goal configuration. This lead to a very efficient bidirectional
planner, which they called RRT-ConCon. The RRT-ConCon algorithm
is shown in Figure 4.2. In an iteration, T
a
is grown towards a random
configuration as described above, generating a new vertex, q
new
. From
the vertex in T
b
that is closest to q
new
is then grown an edge towards q
new
.
If this edge reaches q
new
, the two trees are connected, and a solution is
found. In the next iteration, the roles of the two trees are changed. Thus,
the two trees alternates between growing towards a random configuration
(exploration) and towards each other (connection).
The Connect function, see Lines 4 and 5 in Figure 4.2, first searches
an RRT T for the vertex that is closest to q according to some metric
M
1
. Thereafter a local planner tries to connect the two configurations.
2
Imagine constructing a high-dimensional Voronoi diagram from the vertices in T .

84
4 Augmenting RRT-Planners with Local Trees
RRT ConCon(q
start
, q
goal
)
1
T
a
.Init(q
start
);
T
b
.Init(q
goal
);
2
while
(num nodes < NODES MAX) do
3
GetSample(q
rand
);
4
Connect(T
a
, q
rand
, new node
a
);
5
if
Connect(T
b
, new node
a
, new node
b
) then
6
return
Path(T
a
, T
b
, new node
a
, new node
b
);
7
Swap(T
a
, T
b
);
8
end while
9
return
Failure;
Figure 4.2:
The RRT-ConCon algorithm. The function UniRand re-
turns a uniformly distributed random number in [0, 1]. Note that the
function GetSample need not return a random configuration; it can as
well represent a deterministic sequence, as pointed out in [23, 79].
If Connect fails due to, e.g., a collision, it adds the last collision free
configuration to the tree T and returns false. If no collision was found,
the two configurations were connected and the function returns true. The
metric M
1
is hereafter denoted nearest-neighbor metric.
Note that it is possible that no new vertex at all is created by a call
to Connect. This happens if the vertex in T is so close to an obstacle
that no step is possible towards the other configuration. Thus, in a real
implementation, we would have to check that Connect actually created
a new vertex. To avoid cluttering the algorithm description, we have
chosen to omit such details.
4.2
Local Trees
RRTs have been found to be useful tool for building efficient single-
query planners, see, e.g., [72, 82]. However, if a problem requires passing
through a narrow passage, the RRT variants proposed so far often get
into difficulties. The “narrow passage problem” is of course nothing new
to probabilistic planning methods, but the problem has even more im-
pact on RRT planners than on PRM planners. That is because PRM
methods will save the rare, but valuable, samples that happen to fall in-
side a narrow passage, and sooner or later they will get connected to the
graph. RRT methods, on the other hand, will throw away a potentially

4.2 Local Trees
85
valuable sample if the active tree could not connect with it. Using uni-
form sampling, the planner will probably have to wait a long time until
such an important sample shows up again. The problem becomes even
more pronounced if the solution trajectory has to pass a series of narrow
passages: Once a tree is finished struggling with a passage, the next one
is waiting just around the corner. PRM methods, on the other hand,
have the advantage of being able to treat the narrow passages more or
less in parallel. In a recent paper by Akinc et al. [3], it was noted that in
environments with thin obstacles, like the “hole” problem in Figure 4.8,
RRT planners tended to produce many configurations that were stuck
near the obstacle. They also reported that for such situations, the cost
for a single RRT query approached the cost of the preprocessing phase
for the PRM method.
Based on these observations, is clear that RRT methods need to take
better care of samples that fall into crucial, but hard to reach, regions.
The basic idea that is proposed here, is to let important samples that
cannot yet be reached, spawn a new tree. Such a tree will be called a
local tree, and the start and goal trees will hereafter be denoted global
trees. A local tree is also an RRT, and as it grows it will eventually reach
outside the “hard to reach” region and merge with one of the global trees.
Even two local trees can be merged together if they get close. There are
three important issues that arise when implementing this idea:
• When should a sample be allowed to spawn a local tree?
• How often should the local trees be allowed to grow?
• How often should the planner look for inter-tree connections?
In the extreme case, letting every sample start a new RRT and then
connecting it to, say, the n
c
closest vertices, the algorithm would simulate
the behavior of the PRM approach. This was pointed out by LaValle and
Kuffner [82]. In this case it is clear that the RRT approach would loose
its qualities as an efficient single-query planner, and we would be better
off using a PRM based method right away. So clearly, there is a tradeoff
between how much time the planner should spend on the two global
trees and on the local trees, respectively. In the next section, effective
heuristics, addressing the three issues above, will be discussed. A new
algorithm, called RRT-LocTrees, is also presented.

86
4 Augmenting RRT-Planners with Local Trees
4.2.1
The RRT-LocTrees Algorithm
For a planner using local trees, when to create them is of course one of
the most important issues. As discussed in the previous section, only
important samples should be allowed to create a local tree. Therefore
we need some rule establishing the importance of a sample. If a sample
is collision free and cannot be reached by neither the global trees, nor
the local trees, then the sample is considered as a candidate for creating
a new RRT. These requirements are very similar to the Visbility-PRM
approach of Sim´eon et al. [137], where nodes that are not “visible” from
other parts of the graph become guards. As these requirements are rather
weak, the number of local trees would rise very quickly. To avoid having
too many trees, we set an upper limit on the number of local trees, N
loc
.
The immediate question is of course how the choice of N
loc
affects the
planner and how it is related to the dimension of the configuration space.
As it turns out, a good value on N
loc
is not so dependent on the dimension
of the configuration space, but rather on the number of difficult passages
the solution trajectory has to pass. As RRTs are very effective at quickly
covering large, relatively open areas of the configuration space, local trees
in those areas will quickly connect with another tree and disappear. So,
it is likely that the local trees will get more and more concentrated to the
hot spots of the problem. Thus, N
loc
roughly tells us how many narrow
passages we can treat in parallel.
To check if a sample q is reachable from a tree T , we do a reachability
check using the Connect function. As Connect may add a new vertex
to an RRT, it is clear that just performing the reachability check will
also grow the local trees, as long as a single step could be taken in that
direction. Thus, doing this reachability check for each sample is would
waste too much time and nodes on the local trees. Therefore, if we already
have reached the maximum number of local trees, the reachability test
will be carried out only with probability p
grow
. Thus, p
grow
provides a
parameter for tuning the growth of the two global trees relative the local
trees. Note that this parameter will only have effect once the maximum
number of local trees is reached.
So far we have only dealt with the issues when to create local trees
and when to grow them. However, without trying to connect them with
the global trees (or other local trees), they will not be of much use.
Since a local tree originates from a sample that was hard to reach, there
should be some kind of advancement before we try to connect it with
another tree: If a tree has advanced, it might have done so out of the

4.2 Local Trees
87
RRT LocTrees(q
start
, q
goal
)
1
T
a
.Init(q
start
);
T
b
.Init(q
goal
);
2
while
(num nodes < NODES MAX) do
3
GetSample(q
rand
);
4
if not
Connect(T
a
, q
rand
, new node
a
) then
5
if
(num loc trees < N
loc
) or (UniRand() < p
grow
) then
6
GrowLocalTrees(T
a
, q
rand
);
7
if
T
a
.BoundingBoxGrew() then
8
for all
T
i
: T
i
= T
a
, T
b
do
9
TryMerge(T
a
, T
i
, new node
a
);
10
if
Connect(T
b
, new node
a
, new node
b
) then
11
return
Path(T
a
, T
b
, new node
a
, new node
b
);
12
Swap(T
a
, T
b
);
13 end while
14 return Failure;
Figure 4.3:
The RRT-LocTrees algorithm. The main difference from
the RRT-ConCon algorithm is due to Lines 4-9.
“hard to reach” area, making it reachable for other trees. The measure
of advancement chosen here is the volume of the box bounding of an
RRT in the configuration space. Thus, each time the bounding box of a
tree grows, the new node will be used for connecting this tree with every
other tree. Since it was this node that caused the growth of the bounding
box, it will in some sense be in the “frontier” of that tree. Experimental
results have shown this to be a very effective heuristic. However, it was
found out that an even greater effect was achieved if this heuristic was
extended such that inter-tree connections are tried also if the sample was
successfully reached by a local tree.
The rules described above have been used to implement a new RRT
algorithm, called RRT-LocTrees. The main loop of the algorithm builds
on the RRT-ConCon algorithm[72], described in Figure 4.2. The new al-
gorithm is described in Figure 4.3. To simplify the description of the algo-
rithms, it is assumed that all trees, both global and local, can be accessed
by an index. Furthermore, the two first trees will always be the start tree
and the goal tree. Two important sub-routines, GrowLocalTrees and
TryMerge
, are described in Figure 4.4 and Figures 4.5, respectively.

88
4 Augmenting RRT-Planners with Local Trees
GrowLocalTrees(q, T
act
)
1
if not
IsSatisfied(q) then return;
2
for all
T
i
: T
i
= T
act
do
3
reached = Connect(T
i
, q, new node
i
);
4
if
reached or T
i
.BoundingBoxGrew() then
5
for all
T
j
: j > i, T
j
= T
act
do
6
TryMerge(T
i
, T
j
, new node
i
);
7
if
reached then return;
8
end for
9
if
(num loc trees < N
loc
) then
10
CreateLocalTree(q);
11 return;
Figure 4.4:
Pseudo-code for the GrowLocalTrees sub-routine. Note
that T
act
is either the start tree or the goal tree, depending on the current
direction.
TryMerge(T
c
, T
d
, node in c)
1
if
Connect(T
d
, node in c, new node) then
2
T
c
.Merge(node in c, T
d
, new node);
3
delete T
d
;
4
end if
5
return
;
Figure 4.5:
Pseudo-code for the TryMerge sub-routine.
4.3
Experiments
To study the benefits of using local trees, two planners were tested on
a set of four path planning problems. The first planner uses the orig-
inal RRT-ConCon algorithm, while the second planner uses local trees
together with the heuristics presented in Section 4.2. Both planners were
implemented using the CoPP framework, described in Chapters 2 and 3.
The function Connect, see Section 4.1, uses a local planner to try to con-
nect two configurations. As local planner, the class BinaryConnector,
see Section 2.8.3, was used. This local planner uses a metric M
2
and
a given maximum step size to determine how densely a path segment
has to be checked for collisions. The metric M
2
is here denoted path
metric. If not mentioned otherwise, the BoundingBoxDispl metric from

4.3 Experiments
89
Figure 2.11 is used as path metric.
The software was compiled using Visual C++ and run on a 1.2 GHz
Pentium 3 processor. Only PQP was used for the collision detection.
For the experiments, the four problems in Figures 4.6-4.9 were used
to study the effect of using local trees. In particular, the effect of p
grow
has been examined. In all experiments, the maximum number of local
trees was kept constant at N
loc
= 10. Because the required planning
time is not deterministic, each problem was run 100 times to get reliable
averages. The table presents averages and the minimum and maximum
values of both the running time and the number of nodes used. Tests
with p
grow
= 0 correspond to the RRT-ConCon algorithm.
Initial results for the experiments performed in this section have been
presented in [144]. There are, however, some differences compared to the
results in this section. These differences are mostly due to using an opti-
mized metric for each individual problem; for each problem, we chose the
metric that seemed to give the best results for the RRT-ConCon algo-
rithm. The sometimes large differences from the results in [144] show the
sensitivity of these methods to the choice of metric. They also underline
the importance of documenting all settings for a particular problem, so
that other researchers can do comparisons.
2D-maze
A unit square with 2 DOFs must move from the lower
left corner to the upper right. Even though this problem only is two-
dimensional, RRT-ConCon has trouble because of the many narrow pas-
sages: Each entry to a narrow passage poses a problem. Albeit not so
difficult, but there are many of them. Thus, this is an ideal setting for
testing the approach presented here, as each local tree can grow in its
own corridor, waiting for another to come near. The width of each nar-
row passage is 1.6 times the width of the square. The overall size of the
maze is 100 × 100. The problem is available in the MSL distribution. As
nearest-neighbor metric, the squared Euclidean distance was used. This
was metric was also used as path metric.
From Table 4.1 it is seen that without local trees, the planner suc-
ceeded at all 100 attempts, but the planning times are not so encouraging.
As expected, adding local trees has a very positive effect on this problem,
and the performance is rather insensitive to variations in the parameter
p
grow
. The runs showed that the number of local trees quickly reached
the maximum value and then dropped again as the two main trees cov-
ered more and more of the maze. As can be seen in Figure 4.3, p
grow
will not have any effect as long as the maximum number of local trees

90
4 Augmenting RRT-Planners with Local Trees
is not reached. Since N
loc
is reached only for a very short time for this
problem, the lack of influence from p
grow
can be explained.
C-maze
A C-shaped object must move through a ’simple’ maze. The
regularly spaced squares in the maze will force the object to repeat a
translate-rotate-translate manoeuver. The problem is adopted from the
MSL distribution. As the solution requires passing a large number of
difficult passages in series, it is expected that the local trees approach
will have a large impact on this problem.
As nearest-neighbor metric, the RingMetric from Figure 2.11 was
used. This metric correctly handles the S
1
-topology for the rotational
degree of freedom. The total distance is a weighted sum of the transla-
tions and the rotation:
ρ(q
1
, q
2
) = |q
x1
− q
x2
| + |q
y1
− q
y2
| + 4|RingDiff(q
θ1
, q
θ2
, −π, +π)|.
The function RingDiff is described in Figure 2.7, and in this case the
result will be in [−π, +π]. As the translational coordinates can vary in
[0, 100], the translational parts will on average give a larger contribution.
The low rotational weight has the effect to produce many motions that
are pure rotations, something that is beneficial for this problem.
As can be seen from Table 4.1, RRT-LocTrees again performs much
better than RRT-ConCon. Furthermore, as the maximum number of
local trees is reached over a long time, there is also significant influence
from p
grow
; increasing p
grow
decreases the required planning time.
Hole
A well-known test scene, see Geraerts and Overmars [46]. The
moving object is composed of four identical boxes of dimension 100 ×
20 × 20, such that the bounding box of the final object is 100 × 100 × 180.
The hole has dimension 100 × 100 and the plate thickness is 20. To get
through the hole, the object has to rotate in a complicated manner. Since
the passage in the configuration space will be long and winding, getting
a local tree inside the passage will greatly speed up the solution process.
It was noted that most unreachable samples actually corresponded to
configurations where the object was tightly pressed to the surface of the
plate. The local trees arising from such samples were merged with the
start or the goal tree in just a few iterations, and the maximum number of
local trees was seldom reached. That explains the lack of influence from
p
grow
on this problem. Once a local tree was really inside the passage,
the solution was quickly found.

4.3 Experiments
91
Figure 4.6:
A maze with narrow corridors for a square with 2 DOFs.
In [144], Euler angles were used to represent the rotations. Here we
instead use quaternions, leading to better interpolation, see Figure 2.9,
and to a better metric. The nearest-neighbor metric is the QuatDist
metric, see Figure 2.11. For the translational part, Manhattan distance
is used, with each coordinate normalized to [0, 1]. For the rotational
part, the distance is related to the quaternion inner product, as given by
Equation (B.12). The rotational contribution is weighted by 0.1. Note
that as the bounding box of a quaternion does not make sense, we still
use Euler angles for the bounding box computations.
Manipulator
A Puma manipulator has to move a large wrench from
the upper shelves to the lower shelves, see Figure 4.9. The clearance be-
tween the shelves, starting from the top, is 300, 550 and 250, respectively.
The bounding box of the wrench is 680 × 240 × 80. Due to a nearby wall
and a cupboard, the workspace of the robot is very tight. This is a prob-
lem where the new approach is not expected to gain much, because most
of the local trees will grow in places that are not necessary to visit, e.g.,
between the second and the third shelves. Thus, this is a good problem
for examining the cost of using local trees at a problem where it is not
really beneficial. It is clear from Table 4.1 that the original algorithm
does better in this case. In fact, as p
grow
increases, the performance de-
grades. As nearest-neighbor metric, a weighted Manhattan distance was
used, with unit weights for all degrees of freedom.

92
4 Augmenting RRT-Planners with Local Trees
Figure 4.7:
A maze for a C-shaped object with 3 DOFs.
Figure 4.8:
The rigid body has 6 DOFs and it has to pass through the
hole.
Even if the test suite of problems is small, it seems as if problems
gaining from local trees do so very much, even if p
grow
is small. For
problems not gaining from local trees, the performance will degrade as
p
grow
increases. It is therefore suggested that when augmenting RRT
planners with local trees, using a small value on p
grow
, will make a great
improvement on the overall performance.
4.4
Chapter Summary
Using the idea of local trees makes the planner more closely related
to ’classical’ PRM-methods, while keeping the key ideas of the RRT-

4.4 Chapter Summary
93
Figure 4.9:
The 6 DOF Puma manipulator has to move the wrench from
the upper shelf to the lower shelf. Note the nearby wall and cupboard,
limiting the manipulator workspace.
methods: The Voronoi biased growth of the trees and the incremental
connection with other nodes. The latter property is important for non-
holonomic systems. The important issues with this method seem to be
when to create a local tree, how often the local trees should be allowed
to grow and when to look for inter-tree connections. Spending too much
time on local trees will make the planner loose its benefits as an efficient
single-shot planner. In this paper, simple and powerful heuristics have
been proposed for both these issues, with one parameter controlling the
amount of time spent on the local trees. Although the experimental re-
sults are few so far, the trend seems to be that local trees have a large
impact on difficult problems, even if the parameter p
grow
is very small,
say p
grow
≈ 0.05. This suggests that augmenting RRT-planners with
local trees, using a small value on p
grow
, will have a large effect on the
overall performance.
According to one of the most efficient heuristics, inter-tree connec-
tions should be attempted whenever the bounding box of an RRT grows.
It could, however, be expected that the efficiency of this heuristic de-
creases as the dimension of the configuration space increases; for high-
dimensional configuration spaces, this heuristic can cause too many con-
nection attempts with other trees, as new vertices are likely to increase
the bounding box.

94
4 Augmenting RRT-Planners with Local Trees
p
grow
t
min
t
max
t
avg
n
min
n
max
n
avg
2D-maze
0.0
262.2
983.3
550.7
15086
25449
19941
0.05
0.9
4.2
1.9
883
3314
1683
0.1
0.8
4.0
1.8
803
3131
1596
0.2
0.8
3.7
1.8
782
3055
1662
C-maze
0.0
83.7
727.3
278.7
9409
23637
15531
0.05
14.9
57.5
31.6
4676
9722
6901
0.1
11.3
39.7
19.4
3470
8687
5484
0.2
7.7
27.5
13.4
2962
7319
4604
Hole
0.0
0.3
686.5
42.2
274
36966
7547
0.05
0.5
6.9
2.1
405
4875
1753
0.1
0.6
12.8
2.5
452
7034
2070
0.2
0.2
8.3
2.1
101
5531
1760
Manipulator
0.0
1.3
31.3
9.5
175
2835
950
0.05
4.3
36.1
14.5
369
3260
1339
0.1
4.3
45.6
16.5
432
3939
1534
0.2
4.0
61.2
20.9
363
5335
1884
Table 4.1:
Experimental results for the four problems and various values
for p
grow
. Each problem was run 100 times. Note that p
grow
= 0.0
corresponds to the original RRT-ConCon algorithm, which does not use
any local trees.
Balancing the Trees
The path planning problems studied in this
chapter were balanced in the sense that it was not significantly more
difficult to add nodes to the start tree than to the goal tree, or vice
versa. Thus, applying RRT-ConCon or RRT-LocTrees lead to global
trees that on average were of the same size. Many important problems
do not exhibit this property. Consider for example the Flange
3
problem
in Figure 4.10, where the problem is to find sequence of motions that
remove the flange from the pipe. Using RRT-LocTrees on this type of
problems typically leads to one tree becoming orders of magnitudes larger
(in terms of number of nodes) than the other.
3
http://parasol-www.cs.tamu.edu/groups/amatogroup/benchmarks/mp/

4.4 Chapter Summary
95
Figure 4.10:
The “Flange” benchmark. The goal is to separate the two
parts. Because the start configuration is severely constrained, an efficient
planner should focus more on the start tree.
In [79] a balanced bidirectional RRT-planner is described. It is similar
to RRT-ConCon, but it enforces the two trees to have the same number of
nodes. Thus, focusing more on the tree that is having trouble to explore.
A suggestion for future research is therefore to develop a balanced
version of the RRT-LocTrees algorithm. The major problem is due to
the GrowLocalTrees sub-routine, which can cause one of the global trees
to grow “behind the back” of the other. We have done some initial exper-
iments with a balanced version, and it seems to be possible to combine
the strengths of RRT-LocTrees with those of the balanced planner de-
scribed in [79]. In these experiments, the Flange problem in Figure 4.10
was solved in about two minutes on average.

Chapter 5
Planning of
Pick-and-Place Tasks
In this chapter we will describe a planner capable of solving high-level,
real-life tasks. The tasks considered are of the pick-and-place type, which
are common tasks in both industrial robotics and service robotics. From
the user’s point of view, such a task could be specified simply as “Put
the rice box on the table”, hiding a lot of details such as how to grasp
the object, eventual re-grasping operations and the pose of the arm when
the task is done. Allowing task specifications at this level will of course
increase the complexity of an already hard problem and the question
is how this task can be decomposed, in a general manner, into several
simpler path planning tasks, without imposing unnecessary constraints
on the solutions.
Similar to other approaches, we decompose the task into three parts:
approach, transport, and return. A modified RRT-ConCon planner is
used to plan the motions for each part. The main improvement is that
the planner is able to plan towards several goals at the same. This
is particularly useful if several arm configurations can reach the same
grasp. Furthermore, each RRT-planner maintains its internal state be-
tween queries, allowing efficient backtracking if a previously chosen goal
configuration turns out to be a dead-end.
The next section will give an overview of previous approaches to solve
pick-and-place tasks and manipulation tasks in general. This is followed
by an overview of the proposed planner and its components. Section 5.3

98
5 Planning of Pick-and-Place Tasks
presents the grasp generator concept. This concept is useful for separat-
ing the grasp planning process from the rest of the planner. In Section 5.4
we present a general preprocessing method that help to make the problem
easier for the path planner. The method uses the inverse kinematics of
the robot to convert the start and goal configurations into configuration
space trees. Section 5.5 deals with how motions for the arm and hand
are generated. Section 5.6 presents a simple approach for handling task
constraints.
Due to the random nature of the algorithm, the produced paths are
often jerky and unnecessary long. To be of real use, smoothing of the
solution is necessary. Section 5.7 deals with methods for path smoothing.
In Section 5.8, we will show examples of various tasks solved by the
planner. Two of these examples involve task constraints. The chapter
ends with a summary.
5.1
Related Work
One of the first systems that automatically solved high-level manipulation
tasks was the Handey system [93, 62]. The system required as input poly-
hedral models of the robot’s links, static obstacles and the task object.
To locate the task object, Handey used model-based object recognition
together with a triangulation-based range finder. The combination of
polyhedral objects and a parallel-jaw gripper made it particularly easy
to generate stable grasps. To plan collision free motions, two planners
were used: Large arm movements were planned using a discretized ver-
sion of the configuration space, where the last three joint angles were
kept constant. For small motions near the task object, a potential field
planner was used, constraining the gripper’s motion to the grasp plane.
1
If necessary, the system could insert a planned sequence of regrasping
motions to solve the task.
In his thesis, Kuffner [71] proposed a pick-and-place planner suit-
able for use in animation of autonomous agents. The emphasis was put
on natural-looking motions and near realtime behavior. In one of the
demonstration applications, a virtual chess player planned and executed
commanded moves in near realtime. The decomposition of the task is
the same as that in [93], but here all motions are planned using Rapidly-
exploring Random Trees (RRTs) [78, 72], leading to a more robust and
1
In [93] the grasp plane is defined as a plane parallel to the faces being grasped
and midway between them. We use a different definition in Chapter 6

5.2 Overview of the Planner
99
less heuristic planner (no restrictions on the last three joints, no dis-
cretizations of the configuration space).
Nielsen and Kavraki [109] extended the Probabilistic Roadmap
(PRM) framework to handle manipulation planning. Their approach
uses the manipulation graph, whose edges can be transfer paths, or tran-
sit paths. A transfer path corresponds to a movement where the task
object is grasped and moves along with the robot. Accordingly, a tran-
sit path is a path where the object is left in a stable position and only
the robot is moving. The generality of the approach makes it possible to
solve tasks that require several regrasping motions. However, the planner
has to be initialized with a user-defined set of grasps and stable object
placements.
Whereas the approach in [109] used a discrete set of grasps and ob-
ject placements, the recent approach by Sim´eon et al. [134] could handle
continuous sets. A continuous set of grasps assumes that the set can be
parameterized by some coordinates. This is easy in cases that involve,
e.g., a parallel-jaw gripper and a bar with rectangular cross section. The
example in [134] showed that the planner is capable of finding solutions
to problems that require long sequences of regrasping motions.
The planner presented in this chapter has many similarities with the
one presented in [71]. However, as will be pointed out in Section 5.9, this
planner introduces several improvements over the one in [71].
5.2
Overview of the Planner
In this section we will give an overview of the proposed pick-and-place
planner. We will decompose tasks into a sequence of steps and introduce
the components that are necessary to solve each step. Each concept is
covered in more detail in the subsequent sections.
We will assume that a pick-and-place task can be decomposed into
the following sequence:
• Approach: Move the arm to the grasp configuration, while forming
the hand to a preshape that is compatible with the grasp.
• Grasp: Close the fingers around the task object to form a stable
grasp.
• Transport: Move the grasped object to a specified position, while
keeping the hand configuration fixed.

100
5 Planning of Pick-and-Place Tasks
• Release: Release the task object.
• Return: Move the arm to a specified or default “home” configura-
tion.
This decomposition is similar to that used in [93] and [71]. There is, how-
ever, a class of pick-and-place tasks whose solution cannot be described
by the proposed task decomposition; some tasks require the robot to
move the object to an intermediate position, and then regrasp it to suc-
ceed. Figure 5.1 show some sequences from a task where the robot is
supposed to reposition a cylindrical object. To succeed with the task,
the robot must place the cylinder at an intermediate position where it
can be regrasped, see Figures 5.1 (c) and (d). We will argue here that
tasks that need such regrasping operations can be solved if an interme-
diate pick-and-place task, instantiated by a high-level planner, is solved
first. Thus, any complex pick-and-place task can be decomposed into a
sequence of simpler tasks that follow the proposed task decomposition.
The reason for this distinction between simple and complex pick-and-
place tasks is that regrasping operations in general will require a lot of
semantic knowledge, specific to the environment and the task. First, a
regrasping operation might involve placing the object at some position
other than the start- or goal position. This requires knowledge about
which surfaces in the environment are suitable as support for the object.
Second, cluttered environments could require the robot to reposition an
obstacle, which requires knowledge about which obstacles are moveable.
For these reasons we think that planning of regrasping operations should
take place at a higher level than planning of the basic pick-and-place
task. However, we think it is important that the planner solving the
basic pick-and-place task is capable of communicating the reason of an
eventual failure to the level above. This information can help the higher-
level planner to, e.g., look for a regrasping operation, or reposition a
moveable obstacle. In Section 5.3.3 we look at how statistical informa-
tion that is gathered during the grasp planning process can be used to
detect if a regrasping operation is needed.
Grasp Generators
The most critical decision for any pick-and-place
planner is which grasp to choose; the choice of grasp will greatly influ-
ence the required arm motions. More important, for some grasps there
will be no solution at all. In Section 5.3 we will introduce the concept of
grasp generators. Grasp generators are used to generate an ordered set

5.2 Overview of the Planner
101
(a)
(b)
(c)
(d)
(e)
Figure 5.1:
A task that requires a regrasp operation. A possible regrasp
operation is shown in (c) and (d).
of stable grasps, where each grasp satisfies the task constraints at both
the start and the end position. In case of an eventual failure (i.e., the
set of grasps is the empty set), grasp generators can provide higher-level

102
5 Planning of Pick-and-Place Tasks
planners with sufficient information to modify the current plan. As pre-
viously mentioned, a modification of the plan might involve a regrasping
operation, or repositioning an obstacle that blocks an important path.
Modified RRT-Planner
Once a grasp has been chosen, the arm con-
figuration at the pick up and put down position, respectively, can be
found solving the inverse kinematics problem. Similar to [71], we use
the RRT-ConCon (see also Section 4.1) algorithm to find motions that
connect these configurations, with some important changes. First, we
consider multiple goal configurations instead of a single goal configura-
tion. This will cause the planner to try to connect the start tree to several
goal trees in parallel. Second, the start and goals are no longer points
in C, but trees. The old planner interface is a special case of this new
interface, because a point in C can bee seen as a configuration space tree
with just one node. The possibility to initialize the RRT-planner with
entire trees will be extremely useful; as described in the following para-
graph, a specialized planner can be used to preprocess the start and goal
configurations into trees that will both accelerate the RRT-planner and
help it generate more efficient motions.
Retract Planners
Each transition in the proposed task decomposi-
tion, e.g., from approach to grasping, will involve very constrained mo-
tions near obstacles. Consider Figure 5.2 where the hand is about to
grasp a cylinder; in the close vicinity of the cylinder, the hand is con-
strained to move in directions that are spanned by the y-axis and the
negative z-direction of the end-effector frame. If we already know the set
of admissible motion directions close to the transition, it seems as a waste
of resources (time and computer memory) to not pass this information
to the RRT-planner. We therefore introduce the useful concept of retract
planners. A retract planner will, starting at a given grasp, move away
from the object as far as possible along prescribed workspace directions.
The result will be a tree, rooted at the grasp configuration. Such trees
will hereafter be called retract trees. See Figures 5.4 and 5.5 for examples
of projections of retract trees to the workspace. Note that each node in
a retract tree requires solving the inverse kinematics problem. In Sec-
tion 5.4 we will discuss retract planners in more detail. We will also show
that retract planners are useful for all transitions in the pick-and-place
task.

5.2 Overview of the Planner
103
z
x
y
Figure 5.2:
In the vicinity of the cylinder the hand can only move in the
directions spanned by the y-axis and the negative z-direction. Movement
in the x-direction is blocked by collisions between the fingers and the
cylinder surface.
Task Constraints
Real-life tasks will often involve additional con-
straints that are task specific. A natural example is the robot-butler
scenario, where the robot is about to serve a cup of hot tea. Here we
have an orientational constraint that, in risk of upcoming law suits, bet-
ter not be violated. In Section 5.6 we will discuss how constraints can
be handled, and in Section 5.8 we will show two examples of tasks that
involve constraints.
Path Smoothing
The final solution can be expressed as a concatena-
tion of the approach path, the transport path, and the return path. As
is the case with most randomized planning algorithms, the solution path
will be jerky. In fact, if the problem contains some difficult passage, the
solution path will often exhibit a long sequence of ’random walk’ behav-
ior in the vicinity of the difficult passage. To be of any practical use,
such sequences have to be removed and we have to smooth the resulting
path. In Section 5.7 we will discuss two methods that effectively removes
unnecessary via-points and smoothes the solution path.

104
5 Planning of Pick-and-Place Tasks
5.3
Grasp Generators
The solution of a pick-and-place task must inevitably involve some kind
of grasp planning. Most approaches to grasp planning put the effort in
finding one optimal grasp, where optimal is with respect to some measure
of grasp stability or grasp manipulability. However, this approach fails
to recognize the constraints imposed by the task; the chosen grasp might
be very stable but cause the path planner to fail to plan a valid transport
path. To let the grasp planner take the task constraints into account is
not a solution, as that would make the grasp planning problem as hard
as the original problem we wanted to solve and would also be against
our strategy of decomposing the task. The approach proposed here is to
have grasp planners that produce a, possibly ordered, set of grasps that
are locally valid to the task. Here we define a grasp to be locally valid if
it satisfy the following criteria:
• The grasp must be stable.
• The grasp must be reachable at start and end positions, i.e., there
must exist solutions to the inverse kinematic problem for the arm.
• The grasp must be collision free at start and end positions.
The produced grasps should also have the property that they are distinct
or far away from each other in some sense. As an example, when trying
to grasp a box, grasps that approach different sides of the box can be
considered to be distinct with respect to each other. A grasp planner
working after these principles is hereafter called a grasp generator. Note
that the extra constraints in general simplify the grasp planning process
because they can be used to effectively limit the search space.
5.3.1
Algorithms Suitable as Grasp Generators
For a client using a grasp generator, it does not matter how the grasps
are found as long as it behaves as described above. Below we will look
at some ways a grasp generator can be implemented, which will lead
to a model for the implementation. Ideally, a grasp generator should
be capable of generating a set of feasible grasps given any hand-object
pair. However, developing such a grasp planner would be a formidable
task in itself. In fact, most papers on grasping simplify the problem by
ignoring the hand kinematics in the problem formulation. While this

5.3 Grasp Generators
105
approach has increased the mathematical understanding of the grasping
problem, its practical use is questionable: Of what use is an optimal grasp
if the hand cannot reach the planned contact points? A more pragmatic
approach would be to simply store together with each object a set of
precomputed grasps in a database. This approach was used by, e.g.,
Petersson et al. [117] for implementing a fetch-and-carry system, and by
Kuffner [71] in his thesis on animation of autonomous agents. A database
with precomputed grasps is fast and simple to implement, but it is only
viable if the number of objects handled by the planner is small and new
objects are rarely added.
Smith et al. [140] proposed an algorithm for computing grasps for a
parallel-jaw gripper on polygonal objects. The algorithm runs in O(n
3
)
time to compute and rank O(n
2
) grasps for an n-sided polygon. As this
algorithm satisfy our criteria, it can clearly be seen as a grasp generator.
Fischer and Hirzinger [42] proposed a fast randomized grasp planner
for finding finger-tip grasps on an arbitrary 3D object for the four-fingered
DLR hand. In this method, contact points are generated by ray-casting
from a point inside the object. A set of heuristics is used to quickly
determine if the generated contact points lead to a good grasp. If the
points are good enough, another test makes sure if the contact points
are reachable by the hand. The method is fast and it can easily be
adopted for other hands than the DLR hand. If ordering of the grasps
is not important, then the algorithm could be used right away as grasp
generator. If, on the other hand, an ordered sequence of grasps is desired,
then the algorithm is modified to generate n distinct grasps, which are
sorted according to some quality measure. A drawback with this method
is that it can only plan for fingertip grasps.
Strandberg [142] and Morales [106] presented two similar grasp plan-
ners for the three-fingered Barrett hand. Both planners take a two-
dimensional contour as input. The planner in [142], which is also de-
scribed in more detail in Chapter 6, produces an ordered set of grasps as
output and can thus be seen as a grasp generator.
Miller et al. [102] proposed a grasp planner for the Barrett hand. They
simplified the grasp planning process by using two models of the task
object: one accurate model, and one approximation made of primitive
shapes like cylinders, spheres and boxes. Humans simplify the problem of
finding an appropriate grasp by choosing a prehensile posture appropriate
for the object and the task. Inspired by this, Miller et al. [102] defined a
set of preshapes for the Barrett hand. These preshapes were used with
a set of rules to generate a set of grasp starting positions for a given

106
5 Planning of Pick-and-Place Tasks
primitive shape. From each grasp starting position, the fingers were
closed around the accurate object model, and the resulting grasps were
evaluated and sorted using the grasping simulator GraspIt! [101, 99].
We use a similar approach for the examples in Section 5.8. However,
our approach is simplified by the assumption that the primitive shapes
exactly describe the task object.
In summary, there exist many approaches that can be used for imple-
menting a grasp generator, but a general one-for-all approach is yet to
come. Therefore it is desirable to have a modular approach so that the
grasp planning strategy can easily be changed without affecting the pick-
and-place planner. To achieve this goal we must define an interface to
which all grasp generators must adhere. This is done in the next section.
5.3.2
The Grasp Generator Interface
As seen in the previous section, the implementation of a grasp genera-
tor will vary and it is also likely to be specific for a particular hand or
gripper geometry. Since all implementations do the same thing, namely
produce a set of grasps, this makes an excellent case for the use of in-
heritance to encapsulate the varying behavior and maintain a uniform
interface. Therefore all grasp generators will inherit from the abstract
base class GraspGenerator, see Figure 5.3. The most important part
of the GraspGenerator interface is how clients retrieve the generated
grasps. From a client’s point of view, a grasp generator represents a,
possibly ordered, set of grasps of unknown size. This abstraction sug-
gests that the interface should only allow clients to sequentially iterate
over the set of grasps. Hence, the most important method in the interface
is NextGrasp, see Figure 5.3. With this interface, the number of grasps
and the way they are generated are hidden from the client.
It is clear that the NextGrasp method alone will not make the interface
complete as it does not provide the grasp generator with any information.
A grasp generator will need access to the following:
• A robot hand for the grasping analysis (unless the grasp generator
is a database of precomputed grasps)
• A robot arm for reachability tests
• An object for collision checking
• An geometric object that models the task object

5.3 Grasp Generators
107
GraspGenerator
+ bool NextGrasp(grasp)
+ NumCollFreeAtStart( )
+ NumCollFreeAtGoal( )
+ NumReachableAtStart( )
+ NumReachableAtGoal( )
# GraspIsCollFree(…)
- DoGraspIsCollFree(…)
# GraspIsReachable(…)
- DoGraspIsReachable(…)
num_reachable_start
…
if(DoGraspIsCollFree()) {
++num_reachable;
return true;
}
return false;