Robot Path Planning: An Object-Oriented Approach

GraspGeneratorA
GraspGeneratorB
Figure 5.3:
From the client’s point of view, a grasp generator is a just
a sequence of grasps, which are retrieved with the NextGrasp method.
The base class uses the Template Method pattern [45] to automate the
process of collecting important statistics, such as the number of reachable
grasps that were found.
• The initial pose for the task object
• Valid end poses for the task object
A natural approach is to provide the first four objects at the creation
of a grasp generator, whereas the last two items are provided via some
initialization method. Note that we allow the specification of multiple
end poses for the task object; if all other criteria are fulfilled, then it
is sufficient if a grasp is reachable and collision-free for at least one of
the provided end poses. The importance of multiple end poses will be
discussed in Section 5.5.1.
5.3.3
Feedback to Grasp Generator Clients
If a grasp generator fails to produce any valid grasps at all, it would
be useful for clients to know the reason for this. With such information

108
5 Planning of Pick-and-Place Tasks
clients can modify the original plan in order to find a solution. We have
found that useful information can be gathered during the generation of
the set of grasps. Therefore GraspGenerator maintains and provides the
following data:
• The number of stable grasps
• The number of collision free grasps found at the initial pose
• The number of reachable grasps found at the initial pose
• The number of collision free grasps found at the end poses
• The number of reachable grasps found at the end poses
If a grasp generator fails to produce any grasps, a client can diagnose the
cause of failure using the statistical data. For example, if the number of
reachable grasps at the initial pose is zero the object is probably too far
away to be grasped. If the arm is mounted on a mobile platform, the
client can try to plan for a better position of the platform. If the object
instead is reachable but there are no collision free grasps at the initial
pose, the object is probably in a very cluttered area of the workspace.
A modified plan could then involve repositioning (if possible) one of the
obstacles that are in the way. As a final example, if there are no collision
free grasps at any of the possible end poses of the object, the task could
be modified by
1. Repositioning (if possible) one of the obstacles that are in the way.
2. Inserting an intermediate position where the object is regrasped
(see Figure 5.1 for an example).
Thus, by collecting simple statistics during the grasp planning, important
information can be conveyed to the client.
A drawback with this approach of gathering statistics is that it re-
quires implementers of the derived grasp generator classes to remember
to update every variable when appropriate. Because this would be very
error prone indeed, the base class should make sure that these variables
are updated at the right time in all derived classes. It is clear that the
statistics variables must be updated each time we check if a grasp is col-
lision free or if a grasp is reachable. We have here used the Template
Method pattern [45] to automate the process of updating all the statisti-
cal variables. The pseudo-code in Figure 5.3 gives an example how it is

5.4 Retract Planners
109
done for one of the variables. This way we ensure that the statistics are
updated correctly for all derived classes and that there is no (easy) way
of circumventing this mechanism.
5.4
Retract Planners
To grasp an object, the robot hand must of course move very close to it,
meaning that the hand and arm motions are very constrained right before
the object is grasped. Motion is often further constrained by surrounding
obstacles like, e.g., a table on which the object is standing. This means
that path planners will spend most of their time planning for the final
part of the approach path, where the motion is most constrained. Not
only is this wasteful, most randomized planners will generate unnatural
hand and arm motions in such constrained situations, zigzagging between
obstacles while approaching the goal configuration. This should not be
necessary as we often have a good idea about how the constrained motions
look in the workspace; typically the final part of the approach path will
be a straight line in the workspace that leads to the grasp configuration.
Furthermore, studies on human grasping have shown that humans early
form their hand into a prehensile posture that is kept until the (gradual)
transition from approaching to grasping begins, see, e.g., Arbib et al. [9].
Based on these observations, we propose to preprocess the goal config-
urations, i.e., the grasps, into configuration space trees. The branches
of such a tree will correspond to linear motions in the workspace with a
constant hand preshape. We believe that such a tree will not only ac-
celerate the following path planner, but also help it to find more natural
motions.
To preprocess a grasp configuration, we start with the hand at the
grasp configuration. The hand is opened to an appropriate preshape and
then moved away from the task object along linear workspace paths. The
number of paths and their directions are determined from a set of user-
defined directions that are initially given relative to the end effector. A
planner responsible for the preprocessing phase moves the arm and the
hand stepwise along each direction, until an object is hit or some joint
limit is reached. Each successful step will generate a node in the tree.
We will call the resulting tree a retract tree to emphasize that the hand
is moved away from the task object. Retract trees are generated by a
special type of planners that we call retract planners.
Moving along directions that are specified in the workspace implies

110
5 Planning of Pick-and-Place Tasks
that each node in a retract tree requires solving the inverse kinematics of
the robot arm. This is not a problem as each Robot object, see Chapter 3,
is assured to at least have a numerical implementation of the inverse
kinematics. It is clear though that, using the inverse kinematics, each
node in the retract tree is more expensive than a node produced using
the forward kinematics, as in PRMs and RRTs. However, this is nothing
but the ever occurring tradeoff between computational cost and node
quality: Here we pay slightly more for each node in the hope that its
placement will be much more effective compared to a randomly produced
configuration.
A retract planner is useful not only for finding paths for grasping an
object, but also for finding lift-off and put-down directions when holding
the object: When we put down an object on a planar surface, the last
part of the motion is often parallel to the normal of the surface and
the orientation of the object is roughly constant. Thus, retract planners
can be applied even to this case, with the only difference that we use
another set of retract directions and that the hand is closed. In the same
way we see that retract trees are useful in every transition of the pick-
and-place task: from approaching to grasping, from lift-off to transport,
from transport to put-down, and so on. Figures 5.4 and 5.5 show some
examples of computed retracted trees. We see that the size of a retract
tree can be used as a rough measure of how constrained the robot motion
is at the root configuration of the tree.
If nodes of a retract tree reach into areas with much free space, the
job is made much easier for a subsequent RRT-planner as it will quickly
bridge large open areas and connect with the retract tree. Note that
a retract tree in no way limit or constrain the possible solutions; it is
just a guide whose advice can be followed or ignored. In this thesis we
use retract trees together with RRT-planners, but they could as well be
used with PRMs. In a PRM setting, retract trees can be seen as initially
disconnected components of the roadmap graph.
5.5
Planning of Arm and Hand Motions
Once a grasp has been chosen, all we have to do is solve three path
planning problems on the standard form move-from-A-to-B. This will
give us the approach path, the transport path, and the return path. To
plan these paths we could in principle use any of the numerous planners
proposed in the literature. We could use, for example, the Probabilistic

5.5 Planning of Arm and Hand Motions
111
(a)
(b)
Figure 5.4:
Retract trees with the hand in preshape configuration. Both
figures show the same grasp, but the arm configurations are different,
resulting in retract trees of different size. Note that some retract tree
nodes are hidden inside the arm links.
(a)
(b)
Figure 5.5:
Retract trees with the hand in grasp configuration. In (a)
we see that without any surrounding obstacles, the resulting retract tree
becomes very large. In (b) two obstacles have been placed around the
task object, resulting in a much smaller retract tree.
Cell Decomposition (PCD) planner proposed in [91], which has shown to
be extremely effective for a large class of problems. However, we will here
use a variation of the RRT-ConCon algorithm, described in Section 4.1.
The reason for this choice is that this algorithm can easily be adapted in
several ways that are very useful in the context of pick-and-place tasks.

112
5 Planning of Pick-and-Place Tasks
Most important are:
• It can handle multiple goals in parallel.
• It can be initialized with configuration space trees instead of single
points.
In the following sections, we will discuss why these modifications are
important in this context.
5.5.1
Multiple Goals
Given a grasp, i.e., a hand configuration and a transformation that de-
scribes the pose of the hand relative the object, we can use the inverse
kinematics of the arm to compute the necessary arm configuration. Sup-
pose now that the solution to the inverse kinematic problem is not unique,
i.e., there are several arm configurations that reach the grasp. Which one
should we choose? We could choose a configuration based on some heuris-
tic, such as the one that is closest to the start or the goal configuration.
However, the configuration we choose can be a dead end, meaning that
there exist no approach, transport, or return path. In [71] the solution
was to try another grasp if no path was found. This implies that each
RRT-query is given a limited amount of resources to find a solution, e.g.,
time or maximum number of nodes. If no solution is found, we instantiate
a new query towards another goal point. This approach has a number of
drawbacks. First, for each repeated query, significant time will be spent
on rebuilding the RRT rooted at the start configuration. Second, how
much resources should we give to each RRT-query? Too much resources
and we risk spending too much time on a dead end. The other way
around and we risk preempting a query when a solution was just around
the corner.
The solution to this dilemma is to consider all possible goals (or at
least a subset of them) at once. Thus, for each new node in the start
tree, we try to connect each goal tree to this node. The same holds for
the other direction; for each new node in a goal tree, we try to connect
the start tree to this node. This approach can be seen as we are looking
for several solutions in parallel. The cost is of course that we have to
build and maintain N
G
− 1 extra goal trees, where N
G
is the number of
goals. Experiments have shown that if one of the provided goals is easy
to reach, i.e., no narrow passages and only a few sharp turns, then the
RRT-planner converges so quickly that the time spent on the other goal

5.5 Planning of Arm and Hand Motions
113
trees is almost negligible. The worst case scenario for this approach is
when there exist a solution for all the provided goals, but each solution
is very hard to find. In this case, testing each goal separately would be
faster.
Multiple goals appears frequently in the context of pick-and-place
tasks, not just because of multiple solutions to the inverse kinematics
problem. As mentioned in Section 5.3, grasp generators are used to
generate a set of grasps that are compatible with the task. The approach
with multiple goals allow us to treat multiple grasps in parallel as well.
Real-life tasks will most likely involve a goal region for the task object,
rather than a single goal point. For example, a task specification like
“Put the book on the top shelf” will leave it to the service robot to
find suitable regions on the shelf, where it can place the book. Goal
regions also arise naturally for objects like cylinders, that have rotational
symmetry; if the rotation around the cylinder’s axis is irrelevant to the
task, then the goal position together with the possible orientations form
a goal region. The idea here is that goal regions can be approximated
with a few representative positions and orientations.
So, we have seen that allowing multiple goals is useful in situations
where we know where we start but are not sure about where to go. There
is of course a limit, where too many goals will make the query inefficient.
A general guideline is not to add a goal that is close to an already existing
goal. In the examples in Section 5.8, the number of goals varied between
one and twenty. The exact number depended on the number of specified
goal positions, the number of grasps and arm configurations for each
grasp. In Section 5.5.3 we will see that multiple goals are also useful in
case we reach a dead-end and have to backtrack, or if we are interested
in optimizing the solution.
5.5.2
RRT-Planners and Retract Trees
In Section 5.4 we introduced the concept of retract planners. They can
be used at each transition of the task to generate retract trees, whose
nodes lie along natural directions in the workspace. If this information is
passed to an RRT-planner we will gain the following:
• Accelerated planning: The retract tree will often lead out from nar-
row and difficult passages, making the task easier for the RRT-
planner.

114
5 Planning of Pick-and-Place Tasks
• Natural transitions: If the RRT-planner connect to a branch of the
retract tree, the transition will look smooth and natural.
For these reasons we will generalize the interface to the RRT-planner,
such that it can be initialized with configuration space trees instead of
single points.
If our goal is to automatically generate natural-looking animations of
human figures, as in [71], there is a slight risk using retract trees; if the
RRT-planner connects with the end of a very long branch of the retract
tree, then the generated motion could look unnaturally stiff, as if the
hand would be guided by invisible tracks. Using the via-point removal
algorithm in Section 5.7.1 this risk is eliminated; if it is possible to connect
to a node further in on the branch of the retract tree, the algorithm will
find that node.
5.5.3
Backtracking
Assume that we have found an approach path and a transport path, but
the planner fail
2
to find a return path. That means that the chosen
arm configuration or object end position was a dead-end, and we have
to backtrack to find a new transport path towards another end configu-
ration. With a minor change to the RRT-planner, we can reuse much of
the work done while planning the transport path. If the planner retains
its state (i.e., start and goal trees) when returning a solution, we can
easily go back and ask it to resume planning. If one of goals turned out
to be a dead end, we can tell the planner to delete the corresponding goal
tree. If the planner has no goal trees left, then we backtrack one step
further, i.e., we try to find a new approach path. A backtracking step is
illustrated in Figure 5.6.
Thus, if we use multiple goals and retain the planner state, we can
deal more efficiently with backtracking events than if goals would be
treated sequentially. Furthermore, if we are interested in improving our
solution, we can resume planning for each remaining goal and keep the
best solution.
2
Note that the notion of planner failure will again require that each RRT-query is
given a limited amount of resources. However, in this case we do at least know that
none of the provided goal configurations has a simple solution path.

5.5 Planning of Arm and Hand Motions
115
start
approach path
transport path
return path
grasp
release
home
Figure 5.6:
For each transition the planner does a bidirectional search
towards multiple goals. Here the current release configuration is a dead-
end as the absence of dashed lines indicates that there exists no return
path. Therefore the planner must backtrack to find another transport
path. Because each RRT-planner saves its internal state from the last
query, a new path is likely to be found in shorter time than the first one.
5.5.4
Efficient use of Robot Composition
When planning the approach and return paths, we want to consider the
degrees of freedom for the arm and the hand together. That is, we want to
treat the arm and the hand as a single kinematic structure. The situation
is different when planning the transport path, because then we want to
keep the hand configuration fixed and just let the hand follow the motion
of the arm. This could be easily accomplished if we had a mechanism
for locking certain joints of a robot. However, such an approach would
require the pick-and-place planner to know which degrees of freedom
control the hand and which control the arm. Furthermore, as we have
already seen in Section 5.3, grasp generators deal with the arm and the
and separately. Thus, we are in fact working with three different robots
(or different views of the same robot): an arm, a hand, and a robot that
is a combination of the arm and the hand. Here the robot composition
mechanism introduced in Chapter 3 comes to good use; the planner work
with a robot that is a composition of the arm and the hand, as shown
in Figure 5.7. Note that the robot Arm in Figure 5.7 does not have
to conform to the usual notion of a robot arm; it could as well be the
combination of a manipulator and a mobile platform.
So, when planning the transport path, the RRT-planner is given the
sub-robot that corresponds to the arm, thereby fixing the hand configu-
ration automatically. The fact that the hand and the task object move
together as single object also opens up for a useful optimization; if we

116
5 Planning of Pick-and-Place Tasks
ArmHand
Arm
Hand
Figure 5.7:
Robot composition provides a convenient mechanism for
moving only parts of a complex kinematic structure. The arrow indicates
that moving Arm alone will cause Hand to follow as a single rigid body.
introduce a new geometric object that is the union of the task object and
the hand, then we could significantly reduce the number of moving parts.
This has shown to be useful for the examples in Section 5.8, as much time
was otherwise spent on checking for self-collisions between the numerous
finger links and the arm.
5.6
Task Constraints
Robot tasks often involve constraints that vary in nature from task to
task. As mentioned in Section 5.2, a typical scenario is a service robot
serving a cup of tea, a task with a simple orientation constraint.
3
As
another example, consider a hyper-redundant robot that has to perform
a welding task in a tight environment. Here the tip of the welding tool
has to follow the prescribed path with high accuracy. The demands on
the tool orientation, however, are much lower; as long as the tool tip
follows the path, the tool’s orientation should be free to vary within
some specified tolerance. The orientation tolerance could be specified as
a cone emanating from the current tool-tip position, and centered around
a nominal tool orientation.
How should we handle such constraints? The simplest approach would
be to see the constraints just as extensions of the C-obstacle concept.
Thus, every configuration is tested against the constraint with a binary
result. This approach makes it easy to test new types of constraints with-
out changing the planning algorithm; the new type of constraint is simply
represented by a class derived from the abstract class BinaryConstraint,
3
A more rigorous approach would also include acceleration constraints. However,
we assume here that the accelerations are low enough to be ignored.

5.7 Path Smoothing
117
see Appendix E. As shown in Section 5.8, this simple approach works
well together with RRTs.
In a more elaborate approach, constraints could support interpola-
tion between two configurations. If the interpolation succeeds, the in-
terpolated motion would satisfy the constraint. With constraint spe-
cific interpolation methods, it would be easy to include constraints in
the PRM-framework as well; using the constraint specific interpolation
method instead of the traditional linear interpolation, much more con-
nections will be found, resulting in a denser roadmap.
In the MSL framework, each problem instance has an internal state
space model, a set of inputs, and an integration method. Clients can
change the internal state using the provided inputs together with the
integration method. With this general approach, problems with both
nonholonomic and dynamic constraints have been solved [78, 82].
Even with constraint-specific interpolation methods, PRM-methods
will suffer from drawing many samples that do not satisfy the constraint.
(RRT-methods will not suffer so much from this problem, as they do
not require samples to be satisfied, see Figure 4.1 (b).) A further en-
hancement of the constraint concept would be to provide a constraint
specific sampling method as well. This approach was used by Oriolo et
al. [113] to solve path planning problems for redundant robots, where the
end-effector path was prescribed.
Thus, the optimal constraint object would not only answer wether
configurations are satisfied, but also provide constraint specific interpo-
lation and sampling methods. For the examples in this chapter though,
we are not changing the interpolation or sampling methods.
5.7
Path Smoothing
The solution from path planners that use randomization is often far from
optimal and not so appealing: The motions are jerky and the solutions of-
ten contain detours. Because the research has focused much on path plan-
ning algorithms per se, the post processing step, involving path length
reduction and smoothing, has often been neglected.
4
However, if the goal
is to actually use the solution path, rather than just studying path plan-
ning algorithms for their own sake, then we must consider postprocessing
methods. Typical examples are computer animation, where we want nat-
4
The fact that omitting the postprocessing step results in much shorter planning
times can also have something to do with it.

118
5 Planning of Pick-and-Place Tasks
ural looking motions, and an industrial workcell, where the emphasis is
on efficient and smooth motions.
In this section we will look at two methods that supplement each other
to produce a smooth solution path. The first method was developed by
Hsu et al. [58], and it effectively eliminates redundant via-points and
reduces the path length. Apart from just describing the method in [58],
we also discuss two alternative termination criteria. The second method
converts a piece-wise linear path to a cubic-spline path, resulting in a very
smooth path with natural velocity constraints between each transition.
This method has to the author’s knowledge not been published before.
The idea is to first use the method from [58] to produce a short piece-
wise linear path. If the application demands smooth joint trajectories,
this path is converted, using the second method,to a smooth spline path.
The resulting joint trajectories are C
2
continuous everywhere, except at
the transitions (e.g., from approaching to grasping).
5.7.1
Path Optimization
For industrial applications, it is desirable to find paths that can be exe-
cuted as quickly as possible. A natural cost function for a path is there-
fore the time it takes to execute it. Under the assumptions that all joints
can reach their maximum speed in negligible amount of time, the time
it takes to traverse a straight line path in C from p = (p
1
, p
2
, . . . , p
n
) to
q = (q
1
, q
2
, . . . , q
n
) is given by
ρ(p, q) = max
1≤i≤n
|p
i
− q
i
|
σ
i
,
(5.1)
where n is the number of joints and σ
i
is the maximum speed of i. In [58]
it was shown that the minimum-cost path is a path that is locally straight
at each point where it is not touching a C-obstacle. In fact, this holds
for all cost functions that are equal to the path length according to some
metric. This is due to metrics obeying the triangle inequality:
ρ(p, q) ≤ ρ(p, r) + ρ(r, q).
Thus, replacing any intermediate via-points with a straight-line sub-path
will always lead to a shorter path.
The Shortcut algorithm in [58] recursively break a path γ into two
sub-paths γ
1
= (ν
1
, ν
2
, . . . , ν
[N/2]
) and γ
1
= (ν
[N/2]
, ν
[N/2]+1
, . . . , ν
N
) and
check wether γ
1
or γ
2
can be replaced by straight-line paths. However, the

5.7 Path Smoothing
119
ν
0
ν
1
ν
2
ν
3
ν
4
γ
0
γ
1
ˆ
γ
Figure 5.8:
The dashed path is the result after one iteration of the
Shortcut algorithm in [58]. In this case, the algorithm will stop in the
next iteration because the dash-dotted path is not collision free, resulting
in a path far from the optimal path ˆ
γ. The figure is adopted from [58].
Shortcut procedure alone will in most cases not give satisfactory results;
as shown in Figure 5.8, Shortcut can get stuck at an early stage. To
address this, extra via-points ν
l
and ν
r
are inserted around each via-point
ν
i
, 1 ≤ i ≤ N . The points ν
1
and ν
2
are initially chosen as the midpoints
of (ν
i−1
, ν
i
) and (ν
i
, ν
i+1
), respectively. If the line segment (ν
l
, ν
r
) is not
collision free, then the points are iteratively moved closer to v
i
using a
bisection method. After the extra via-points are inserted, Shortcut is
applied again. The procedure stops when no further improvement, in
terms of the path cost, is obtained. The resulting algorithm, which is
called Adaptive Shortcut, is described in more detail in [58].
As shown in Section 5.8, Adaptive Shortcut performs well in terms
of speed and quality. However, it was found that running the algorithm
many iterations tend to produce paths where the manipulator moves close
to obstacles. This is a natural result of Adaptive Shortcut because it con-
verges to a path that at some places is tangent to C-obstacles. Although
we are looking for short paths, we do not want the manipulator to move
unnecessarily close to obstacles; there must be some clearances between
the manipulator to allow for position and control errors. Therefore it
is important to ensure that Adaptive Shortcut stops before the solution
path is made tangent to some C-obstacle. If we use collision detection al-
gorithms that are capable of solving distance queries, a natural approach
would be to simply add a distance threshold; if the distance between
the manipulator and an obstacle becomes smaller than this threshold,
then Adaptive Shortcut terminates. For manipulation tasks, however,

120
5 Planning of Pick-and-Place Tasks
this appealing solution poses several implementation difficulties: A ma-
nipulation task inevitably involves moving close to obstacles. In fact,
when grasping an object, the distance to the closest “obstacle” is zero.
Furthermore, for the transport phase of the task, the distance between
the task object and the environment is zero at the beginning and the end.
In effect, the approach of using a single threshold for all potential colli-
sions will not work. Instead we must use different thresholds for different
pairs of objects. Furthermore, these thresholds will vary, depending on
which phase of the task we are in (approach, transport, or return).
A much simpler, but less reliable, approach is to monitor the rate
with which the path cost decreases: As the processed path gets closer
and closer to the locally optimal path, the rate of improvement gets
smaller and smaller. So terminating the algorithm before the rate of
improvement gets very small can avoid the problem of passing very close
to obstacles. Using the rate of improvement as a termination criterion
is of course nothing new, as it is used by almost all iterative numerical
methods. However, experiments have shown that at the early stages
Adaptive Shortcut can produce an almost zero improvement, followed by
a much larger improvement in the next iteration. Therefore we suggest
that the termination criterion require the rate of improvement to be small
for at least two successive iterations.
5.7.2
Spline Paths
Postprocessing the solution paths with Adaptive Shortcut as described in
the previous section results in motions that for most purposes are smooth
enough. A piece-wise linear path does, however, have velocity discontinu-
ities at each via-point. These discontinuities can have a negative influence
if the path is used to control a high-speed industrial manipulator: Veloc-
ity discontinuities can lead to induced vibrations and inefficient control
of the manipulator. To the human eye, the discontinuities are most ap-
parent at the transitions, where the manipulator goes from zero to a
finite velocity in an instant. To address these two issues, we would like
to have a path that has continuous velocity profiles for all the joints and
the correct joint velocities at the start and end points.
Assume that we have a collision-free piece-wise linear path with N
via-points and the dimension of the configuration space is D. To get
rid of the velocity discontinuities, we would like to describe each joint
trajectory with a cubic-spline; they have the appealing property of pass-
ing exactly through the interpolation points under C
2
-continuity. Thus,

5.7 Path Smoothing
121
each joint trajectory will consist of N − 1 spline segments, where each
segment is described by four parameters, yielding a total of 4(N − 1)D
parameters to determine. The following constraints will determine the
spline parameters:
• Interpolation of each via-point (2(N − 1)D constraints)
• Continuity of velocity ((N − 2)D constraints)
• Continuity of acceleration ((N − 2)D constraints)
• Start and end velocity (usually) zero (2D constraints)
So, in total we have 4(N − 1)D constraint equations, which is sufficient
to determine the spline path.
It turns out that the resulting spline path in general is not collision-
free, even though the underlying linear path is so. As can be seen in
Figure 5.9, large deviations from the linear path can result in a spline path
that is not collision free. Here we propose a very simple iterative scheme
to resolve this problem: Every time a collision is found on the spline path,
a new via-point is inserted at the corresponding position on the linear
path. This extra via-point will cause the new spline path to follow the
linear path more closely and the risk of collision is reduced. The process
is repeated until the whole spline path is collision free. Experiments
has shown that this simple method performs well except for situations
where the robot move extremely close to object over longer periods; small
spline deviations from the linear path are certain to cause collisions and
therefore many extra via-points are needed to force the spline closer to
the linear path. As the number of via-points increases, so does the cost
for computing the spline curve. This cost could be reduced if we exploit
the tri-diagonal structure of the matrix representing the spline equations.
The current implementation, however, only uses an ordinary solver based
on LU-factorization.
The method proposed here can be used to produce spline paths for al-
most any path planning problem (rigid-body problems could benefit more
from an interpolation scheme based on quaternions, see Section 2.5.2),
but here we will mention some details that are specific to pick-and-place
tasks. As mentioned in Section 5.2, the solution to the task can be seen
as a concatenation of the approach, transport, and return paths. If we
apply the method described in this section to the concatenated path,
the most visible effect is that the manipulator smoothly accelerates and
decelerates at the start and the stop of the task, respectively. However,

122
5 Planning of Pick-and-Place Tasks
ν
0
ν
1
ν
2
ν
3
ν
4
Figure 5.9:
The via-points of a piece-wise linear path can be interpo-
lated with cubic splines to generate a smooth path. In case the spline
causes a collision, extra via-points can be inserted on the corresponding
linear path segment.
with this approach we have no control over the velocities at the transi-
tions from approaching to grasping and from grasping to returning. If we
instead apply the method to the three sub-paths, before concatenating
them, then we have precise control of the transitions. Mostly we want
to have ease-in and ease-out behaviors, such that the robot comes to
a stop when grasping or releasing the object. In such a case the joint
velocities are zero at the transitions. If we are instead are interested
in motions where the object is grasped and moved in a continuous mo-
tion, i.e., the robot does not stop during grasping, then the following
approach can be used: We form the end-effector velocity based on the
approach and departure directions.
5
The magnitude of the end-effector
velocity is a user-determined value. Based on the end-effector velocity
and the manipulator Jacobian at grasp configuration, the corresponding
joint velocities can be computed. Note that this assumes a non-singular
Jacobian. With these non-zero velocity constraints, we can now make the
robot to grasp the object in one sweeping motion. The resulting path will
be C
2
-continuous everywhere except at the transition from approaching
to grasping, where it would be C
1
-continuous.
5
This method assumes these two directions are roughly similar. Thus, it would not
work for a grasp from above, where the hand first move downwards and then lifts the
task object.

5.8 Examples
123
As a final remark; converting a piece-wise linear path to a spline
path assumes that we have a time associated with each via-point. So
far we have not discussed how those time values are chosen. Under the
assumptions that each joint i has the maximum speed σ
i
and it can
reach this speed in negligible amount of time, we simply assign the time
duration for each straight-line path segment so that at least one joint
moves with maximum joint velocity in each segment. If the time for the
first via-point is given, the time values for the subsequent via-points can
be computed. There are of course other, more elaborate techniques for
choosing the time values for the via-points, but we point out that the
spline converting method described in this section is independent of that
choice.
Figures 5.10 (a) and (b) show a comparison between the piece-wise
linear solution and the spline solution for a pick-and-place task. Fig-
ure 5.10 (a) shows the trajectory for joint 4 of the robot arm, while the
Figure 5.10 (b) shows the trajectory for one of the finger joints. The
circles are the via-points of piece-wise linear solution, and the triangles
are the via-points of the spline solution. The triangles that do not co-
incide with a circle are thus extra via-points that were inserted because
the spline solution was not collision free. As expected, the extra via-
points are inserted near the transitions (shown with dotted lines), where
the hand is moving close to the task object. From the curves in Fig-
ure 5.10 it is also seen that the spline solution gives a zero velocity at the
beginning and the end of the task.
5.8
Examples
In this section, the pick-and-place planner is tested on some problems,
ranging from easy to difficult. For each example we present the average
time required for: planning, smoothing, and spline conversion. If not
stated otherwise, the average values are based on 40 trials for each prob-
lem. In the planning time is included the time for grasp planning and for
generating the retract trees.
As mentioned in Section 5.5.1, the planner is able to plan toward
several possible goals at the same time. For the approach phase of the
task, the goals are determined by the ten first grasps delivered by the
grasp generator.
6
These grasps are known to be compatible with the
6
In case no solution is found for these ten first grasps, the planner will backtrack
and ask the grasp generator for more grasps.

124
5 Planning of Pick-and-Place Tasks
0
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
1
−1.5
−1
−0.5
0
0.5
1
1.5
θ [rad]
time
(a) joint 4
0
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
1
0
0.2
0.4
0.6
0.8
1
1.2
1.4
θ [rad]
time
(b) finger joint
Figure 5.10:
Comparison of the piece-wise linear solution and the spline
solution for a pick-and-place task. The triangles show extra via-points
that were inserted because the spline solution was not collision free. The
grasping time and the release time are shown by the dotted lines.

5.8 Examples
125
Figure 5.11:
A simple task where the robot has to put the cylinder on
the table.
initial pose and the final pose(s) of the task object. For the transport
phase, the number of goals is determined by the task and the currently
active approach path. For the return path, the number of goals is equal
to the number of “home configurations” for the arm. In these examples,
the home configuration is equal to the initial configuration of the arm.
The results are presented in Table 5.1. All examples were run on a
1.2 GHz Pentium 3 processor, and PQP was used for collision detection.
Example E1, Standing Cylinder
The first example is very simple
and is used to establish some kind of lower bound on the required planning
time. The task is to move a standing cylinder from the top shelf to the
table, see Figure 5.11. The ten first grasps from the grasp generator
are all around the cylinder axis. The results in Table 5.1 show that the
average planning time is 2.5 seconds, which includes grasp planning and
retract tree generation. In this case, the time needed for grasp planning
is negligible as it is only about 0.1 seconds. However, for difficult tasks
with only a few valid grasps, the time required for grasp planning can be
significantly larger.
Example E2, Fixture Task
In industrial applications, a typical task
is to place a workpiece in some fixture for machining. Figure 5.12 (a)
shows a simple model of a fixture, in which a cylindrical workpiece is to

126
5 Planning of Pick-and-Place Tasks
be placed. For this task, the robot must grasp the workpiece close to
one of the ends; a grasp on the middle of the cylinder would make the
insertion into the fixture impossible. The results for this task is shown in
Table 5.1. Note that the spline conversion results are missing in the table:
The tight fitting between the cylinder and the fixture will cause the spline
path to collide repeatedly with the fixture. Hence, spline conversion was
not used for this task. If desirable, the approach path and the return
path can still be converted to splines with a low cost.
This is an example that shows the usefulness of the retract trees and
the use of multiple goals; without the use of retract trees, the planner
would spend several minutes on inserting the cylinder into the fixture.
Furthermore, for the chosen grasp, the end position in the fixture is reach-
able with six different arm configurations. Some of these configurations
are bad for the task as a straight-line insertion into the fixture is not pos-
sible. This is shown in Figure 5.12 (b); the size of the retract tree (only
one node is visible) indicates that the range of a straight-line motion is
too small to remove the cylinder from the fixture. Figure 5.12 (c) shows a
retract tree for the same grasp, but with another arm configuration. This
arm configuration is clearly better as the retract tree alone can remove
the cylinder from the fixture. With the possibility to use multiple goals,
there is no need to decide which one of the six different arm configura-
tions to choose; the planner will first connect with the retract tree that
is easiest to reach.
Example E3, Long Rod
The following example is considerably more
difficult compared to the previous ones. The robot has to move a long
cylindrical rod from the bookshelf and place it on top of two supports
on the table, see Figure 5.13 (a). Due to the length of the rod and its
awkward initial position, this task requires careful manoeuvering. The
thin pillars of the bookshelf and the surrounding shelves act as an efficient
cage for the rod.
As seen in Table 5.1, this task takes considerably more time to solve.
Most of the time was spent on finding a transport path, i.e., moving the
rod from the bookshelf to the table. When the robot has grasped the rod,
see Figure 5.13 (b), its motion is constrained in every direction. Thus,
the approach of using retract trees has little effect on this problem. In
addition, for this example, the RRT-ConCon approach is not so efficient:
Due to the constrained motion of the rod, it is much harder to add nodes
to the start tree, than to the goal trees, causing the trees to become
unbalanced. An algorithm that tries to balance the trees [79] would be

5.8 Examples
127
(a) Task specification
(b) Small retract tree
(c) Large retract tree
Figure 5.12:
The robot has to insert the cylindrical object into the
fixture on the table. For the chosen grasp, the end position is reachable
with six different arm configurations. Figures (b) and (c) show that some
arm configurations are more suitable than others for this task.
more effective in this example, as it would spend more time on trying to
move the rod out from the bookshelf.
Worth noticing in Figure 5.13 (b) is that the robot is closing the hand
and moving the arm forward at the same time. Hence, there is no abrupt
transition from approaching to grasping. This gradual transition from
approaching to grasping is due to the smoothing algorithm described in

128
5 Planning of Pick-and-Place Tasks
Section 5.7.1.
Example E4, Orientation Constraint
Orientation constraints arise
naturally in tasks that involve manipulating, e.g., liquid filled cups. Here
we see such orientation constraints as equivalent to forcing a body-fixed
direction vector coincide with a world-fixed direction vector. In real-
world applications it is sufficient if the angle between the two direction
vectors is less than some threshold. Thus, an orientation constraint spec-
ification involves two direction vectors and an upper limit for the an-
gle between them. Orientation constraints are represented by the class
OrientationConstr
, which is derived from BinaryConstraint, see Fig-
ure E.2 in Appendix E.
To evaluate the planner’s performance in the presence of an orienta-
tion constraint, it was tested on the problem in Figure 5.14. The task
is to move a cylinder from the top shelf to the table. The motion must
satisfy an orientation constraint where the maximum deviation angle is
1 degree. Note that the cylinder radius is too large for a natural grasp
around the cylinder axis; to increase the span between the fingers, a hand
configuration with a spread angle of 90 degrees must be used. (See Fig-
ure 6.1 in Chapter 6 for an explanation of the spread angle.) Even though
the maximum allowed deviation angle is small, Table 5.1 shows that the
task does not take significantly longer time to solve compared to Exam-
ples E1 and E2. Making the task simpler by increasing the constraint
angle, causes the required planning time to rapidly approach that for a
task without any constraint. A typical trajectory of the cylinder is shown
in Figure 5.14.
Example E5, Camera Constraint
The following example is different
from the others in that it is not a pick-and-place task. The intent is to
test the performance of the RRT-planner on a rather non-trivial task
constraint. Instead of the Barrett hand, there is now a camera mounted
on the Puma end-effector, see Figure 5.15. We now also consider the
degrees of freedom for the mobile base, resulting in a total of nine degrees
of freedom. The task of the mobile robot is to move from one position
to another, while keeping a target object in the camera’s field of view.
This is a rather complex constraint, and it is satisfied only if all of the
following criteria are satisfied:
• The distance from the camera to the target is greater than d
min
.
• The distance from the camera to the target is less than d
max
.

5.8 Examples
129
planning
smoothing
splines
total
E1
Standing Cylinder
2.5
3.0
1.1
6.6
E2
Fixture
4.1
4.7
–
8.8
E3
Long Rod
485.9
15.6
7.0
508.5
E4
Orientation Constraint
17.7
3.0
10.0
30.7
E5
Camera Constraint
40.8
1.8
–
42.6
Table 5.1:
Average planning times for the different examples in this
chapter. Each example was run 40 times, except example E5, which was
run 100 times.
• The center of the target is within the camera cone.
• The line of sight between the camera and the target center is not
occluded.
The camera cone is defined as a circular cone with opening angle θ
cone
and its apex at the camera lens. In Figure 5.15 (b) the camera cone is
shown as a transparent cone with length d
max
. If rotations of the camera
image is undesirable, an additional constraint can easily be added to the
previous list. Without this constraint, the last degree of freedom is of
no importance as it only rotates the camera around its axis. Thus, the
effective dimension of the task is eight degrees of freedom.
In Figure 5.15 (a) the start position and the end position of the robot
is shown. The camera target is the sphere on the table. The task is made
more difficult by the two boxes surrounding the sphere: They will force
the robot to lift the camera high to avoid occlusion. There are also two
stools and a shelf that give rise to narrow passages for the platform.
Because the platform rotation is unbounded, this degree of freedom
has to be modeled as a ring joint, as described in Section 2.5. The
parameters of the constraint are chosen as d
min
= 0.5 m, d
max
= 1.4 m,
and θ
cone
= 5.0
◦
. This problem was solved using the RRT-LocTrees
algorithm in Chapter 4. The averaged computational times for 100 trials
are shown in Table 5.1. Figure 5.15 (b) shows a snapshot from a solution;
it is clearly seen how the box on the table forces a high camera position.

130
5 Planning of Pick-and-Place Tasks
5.9
Chapter Summary
In this chapter we have presented an efficient and flexible planner for
pick-and-place tasks. Just as the planner in Kuffner’s thesis [71], this
planner also uses an RRT-planner to find the approach path, trans-
port path, and return path. An important difference is that the RRT-
planner in this chapter is modified to allow multiple goals and prepro-
cessed configuration-space trees as input. Multiple goals naturally arise
when, e.g., the same grasp can be reached with several arm configura-
tions. Instead of choosing one of these configurations, the planner can
treat them in parallel.
The retract planner concept has shown to be useful to preprocess the
start and the end-configurations of a task. A retract planner uses the in-
verse kinematics to move the end-effector (and the grasped object) along
predefined directions, thus converting a configuration to a tree. This pre-
processing stage helps the planner to find paths out from confined areas,
and it also helps to generate more natural transitions when grasping or
releasing an object.
The grasp generator concept separates the process of grasp planning
from the trajectory planning. In case of a failure, the grasp generator
interface can provide the higher-level planner with enough information
to formulate a different plan. This plan could, for example, involve a
regrasping operation.
The backtracking mechanism in combination with multiple goals make
the planner very robust. The planner will, however, not be efficient for
tasks that require numerous regrasping operations. For such tasks, the
multiple roadmap approach of Sim´eon et al. [134] and Sahbani et al. [128]
would be better choice.
Despite the very simple constraint model used, the examples have
shown that the planner is also capable of solving tasks that involve ori-
entation constraints.
Due to the random nature of the underlying RRT-planner, the result-
ing trajectories are jerky and will often contain unnecessary via-points.
Thus, smoothing is a necessary postprocessing step before the solution
can be used. The examples in this chapter showed that smoothing can
take a significant part of the total time; for the simplest tasks, smoothing
took even longer time than the planning itself. It should be mentioned
though that the maximum step size was much smaller during smooth-
ing than during planning. With more efficient methods to check path
segments, the time needed for smoothing can be reduced.

5.9 Chapter Summary
131
A somewhat unexpected benefit from the smoothing process is that
stiff and “robot like” transitions from approaching to grasping could be
turned into soft and gradual transitions; a great improvement if the goal
is to produce smooth and natural looking motions.
To summarize, this chapter has covered the following topics:
• Multiple Goals
• Retract planners and configuration space trees
• Grasp generators
• Backtracking
• Task constraints
• Smoothing
• Splines

(a)
(b)
(c)
(d)
(e)
(f)
Figure 5.13:
A task involving a long cylindrical rod.

Figure 5.14:
A typical trajectory for a pick-and-place task with an
orientation constraint. The maximum deviation angle is 1
◦
. Note that
the figure only shows the transport phase of the task.
(a)
(b)
Figure 5.15:
(a) The robot has to move from the leftmost position to
the rightmost, while keeping the sphere within the camera’s field of view.
(b) To avoid occlusion, the robot must lift the camera high.

Chapter 6
Grasp Planning for a
Three-Fingered Hand
Automated grasp planning is an important step towards autonomous sys-
tems that are able to manipulate their environment.
1
Formulating a gen-
eral and efficient grasp planning algorithm has shown to be a formidable
task. Therefore, researchers have often simplified the grasp planning
problem in various ways, e.g., by going from 3D to 2D, or by neglecting
the kinematic constraints. However, neglecting the kinematic constraints
have lead to grasp planning algorithms that are difficult to use in prac-
tice: Of what use is an “optimal” grasp if it cannot be reached by the
hand?
In this chapter we present a fast grasp planning algorithm that is de-
signed to be used on a real robot platform. Based on a 2D-contour, the
planner generates an ordered sequence of grasps where the emphasis is
on robust grasps. With robust we here mean that the properties of the
grasp should not change dramatically even in the presence of small per-
turbations in the position or the geometry. The proposed planner fulfills
the grasp generator requirements, as stated in Section 5.3, so it could be
used together with the pick-and-place planner presented in Chapter 5.
The next section will describe some of the related work and Section 6.2
will provide the background and the problem formulation. The details
of the grasp planner is given Section 6.3 followed by a section with ex-
1
There are of course other modes of manipulation, e.g., pushing, but here we
concentrate on manipulation that involves grasping.

136
6 Grasp Planning for a Three-Fingered Hand
amples of planned grasps. The chapter ends with a summary and some
conclusions.
6.1
Related Work
Automated grasp planning is a difficult problem, and its importance for
autonomous robots has led to a rich literature in the field. The grasp
planning problem is often formulated as an optimization problem, but
constraints from many different domains, e.g., kinematic and friction
constraints, make it hard to solve. A common simplification has been
to neglect the hand kinematics and try to find the optimal positions for
a fixed number of contacts. An example of this is the work of Marken-
scoff and Papadimitriou [96], who derived methods for finding optimal
grasps for 2D polygons. However, neglecting hand kinematics has the
disadvantage that the “optimal” grasp might not be reachable by the
hand.
Another often neglected aspect in grasp planning is grasp robustness,
meaning that small changes in the object geometry or hand configuration
should not dramatically change the properties of the grasp. Any grasp
planner that is to be used for real-world tasks must take grasp robustness
into account in order to not fail due to model errors or noisy sensor
data. Nguyen [108] addressed this aspect by deriving independent contact
regions instead of contact points. As long as the contacts are inside
the computed regions, the grasp is stable. Formulating the solution as
a set of regions also increase the probability of finding a grasp that is
kinematically feasible.
Fischer and Hirzinger [43] suggested a randomized grasp planner for
the DLR hand. Random contact point candidates on the surface of the
object were generated using ray-casting. If a set of contact points is found
reachable, a grasp quality measure is computed. The planner is general
and fast, but it can only plan finger-tip grasps.
Miller et al. [102] proposed a grasp planner for the Barrett hand.
Given a decomposition of the object in terms of primitive shapes, a set
of rules was used to generate grasp candidates. Each candidate was
evaluated on the accurate object model using the grasping simulator
GraspIt! [101, 99].
The planner proposed in this chapter is also developed for the Barrett
hand, and it resembles the planner in [43] in that it generates hypotheses
which are tested for feasibility. Both planners are guided by heuristics to

6.2 Problem Description
137
make the search more efficient. A special feature of our planner is that
it explicitly searches for robust grasps. This is done by accumulating the
number of feasible neighbors, where neighbors is defined as samples in
configuration space that are close to the current grasp. Independent of
the work in this chapter, Morales et al. [106, 105] developed a very similar
grasp planner.
2
From a theoretical point of view, the planner presented
here has the advantage of using more detailed kinematic models and
a more explicit robustness test, without necessarily slowing down the
planner. From practical point of view, however, the planner in [106] has
the significant advantage of being tested on a real robot platform.
6.2
Problem Description
The Centre for Autonomous Systems has a custom designed mobile robot,
which is based on a Nomadics XR4000 platform. To be able to grasp
objects, the robot is also equipped with a PUMA 560 arm, a Barrett
hand, cameras and touch sensors. The platform can be seen in Figure 3.6
in Chapter 3.
The Barrett hand, from Barrett Technology, has three fingers and a
large planar palm surface, see Figure 6.1. The two joints on each finger
are coupled and driven by DC-motors inside the hand. Thanks to a clever
clutch mechanism, the outer finger link can continue to close even if the
inner link is blocked by an object. This way, the fingers can wrap around
an object, providing a secure grasp. Additionally, the fingers F
1
and F
2
can rotate symmetrically around the palm. This is called spread motion
and its value is denoted by ϕ, see Figure 6.1. The spread motion angle
can take values between zero and 180 degrees, allowing for a wide range
of gripper configurations. With eight axes and only four motors, the
Barrett hand is clearly underactuated. It will be shown later that the
mechanical couplings of the hand can be utilized to simplify the grasp
planning problem.
For tasks like object recognition, visual servoing and contour extrac-
tion, a CCD-camera is mounted between fingers F
1
and F
2
such that the
view plane is always parallel to the palm surface (see Figure 6.1). Note
that this is the only placement of the camera that both avoids obstruc-
tion of the fingers and occlusion of the camera. For contact detection,
the whole palm surface and parts of the fingers are covered with tactile
sensors.
2
Both planners were presented in October 2002, but at different conferences.

138
6 Grasp Planning for a Three-Fingered Hand
F
1
F
2
ϕ
CCD
thumb
ϕ
Figure 6.1:
The figure shows the Barrett hand with all fingers fully
opened. The crosses denote each finger’s base position in the palm. Note
the CCD-camera mounted between fingers F
1
and F
2
.
The main assumptions in this work are that the outer 2D contour
of an object is extracted, using the camera mounted on the hand. This
contour is of course the projection of the object onto the view plane,
but from here on it will be assumed that the contour is the cross section
of a generalized cylinder, with a straight-line axis and a homogeneous
mass distribution. The cylinder assumption can seem very restrictive,
but thanks to the clutch mechanism of the hand, the proposed grasp
planner will also be able to handle many objects that deviate from this
assumption. However, objects that are tapered towards the view plane

6.3 The Grasp Planner
139
normal, such that the smallest cross section is closest to the hand, will
always be difficult for the hand to grasp. Note that the view plane normal
can have any global orientation, but for most practical cases it will be
either horizontal or vertical.
In the starting position, the camera is looking at the top of a cylinder,
with the palm surface parallel to the cylinder top.
3
Achieving a grasp
with a plane to plane contact between the cylinder top and the palm
will often imply a strong grasp, which is desirable. Therefore, the grasp
planner will keep the palm parallel to cylinder top. Locking the direc-
tion of the view plane normal this way, reduces the degrees of freedom
(DOFs) for the wrist from six to four. Considering the wrist and the
hand together, the problem has a total of eight DOFs.
6.3
The Grasp Planner
The contour given to the grasp planner can either be a polygon or a spline
curve. In the latter case, the spline curve is adaptively approximated by
a polygon to a user-defined degree of accuracy. The resulting polygon is
parameterized in the arc-length parameter s. The advantage of using a
spline curve as input is that the radius of curvature at the contact points
can be used as an early, and cheap, quality indicator for the contact.
Besides, a spline representation is inherent in many contour detecting
algorithms, see, e.g., Blake and Isard [16].
The direction of gravity relative the view plane will be used in the
grasp evaluation. Additional, but not necessary information, is the height
of the cylinder and depth constraints due to support surfaces. It is also
possible to specify parts of the contour that may not be touched because
of, e.g., nearby obstacles or support surfaces.
The next subsection will give an overview of the grasp planner, with
following subsections describing important parts in more detail.
6.3.1
An Overview
Basically the grasp planner searches the configuration space for kinemat-
ically valid grasps until a termination criterion is fulfilled. As mentioned
in Section 6.2 , the problem has eight DOFs, so performing an exhaustive
search will be time consuming. Here we reduce the search space in two
3
For a horizontal camera orientation, the cylinder top actually corresponds to the
side of the object.

140
6 Grasp Planning for a Three-Fingered Hand
1
F
2
F
y
ϕ
x
ϕ
y
x
thumb
s
palm
d
d
d
T
2
1
F
T
F
O
Figure 6.2:
Relevant frames and variables for a grasp hypothesis. The
position of the thumb is specified by the arc-length parameter s. Each
thumb position has a copy of the polygon relative the frame F
T
.
ways: We utilize the special kinematic structure of the hand and we use a
set of heuristic rules that help guiding the search. Each tested configura-
tion is called a grasp hypothesis, and Figure 6.2 shows a grasp hypothesis
together with some of the configuration parameters. For clarity, a dashed
box representing the palm is also drawn.
A grasp hypothesis is formed in several steps, and at each step there
are some requirements that has to be fulfilled. These requirements can
be divided into hard and soft requirements, respectively. If a grasp hy-
pothesis does not pass a step associated with a hard requirement, the
hypothesis is discarded and a new one is generated. If, instead, the
step was associated with a soft requirement, the hypothesis can be kept
as idle. This way we avoid throwing away a possible solution and can
quickly backtrack idle hypotheses if stuck. Hard requirements are typ-

6.3 The Grasp Planner
141
Place thumb on contour at position
s
Transform polygon to thumb frame
Choose
d
T
and
ϕ
Close finger F
1
→ d
1
Close finger F
2
→ d
2
Determine
z-coordinates
Check if grasp has OK neighbors
Evaluate grasp
Figure 6.3:
Pseudo-code describing the steps for forming a grasp hy-
pothesis.
ically kinematic constraints, while soft requirements are heuristic rules
that relate to the expected quality of the grasp. Threshold parameters
are difficult to choose: On the one hand, too low thresholds will generate
many grasps, of which many are bad. On the other hand, with too high
thresholds we risk throwing away the only feasible solutions. With the
distinction between soft and hard requirements we avoid the problem of
choosing various threshold parameters. The most important steps for
forming a grasp hypothesis are shown in Figure 6.3.
It is important that we start with placing the thumb when we form
a grasp hypothesis; placing the thumb has the effect of fixating three
DOFs, thereby achieving a desirable reduction of the search space. After
a valid thumb position has been found, grasp hypotheses can quickly be
generated by choosing combinations of d
T
and ϕ and thereafter closing
fingers F
1
and F
2
. This can be seen as if we let F
1
and F
2
sweep along the
contour of the object. The coupling between F
1
and F
2
through ϕ allows
us to quickly discard hypotheses that are not kinematically valid: If no
valid contact is found when closing F
1
, then there is no need for checking
F
2
too, and the hypothesis can be discarded. This of course implies that
we only allow three-finger grasps. If we represent the polygon contour in
the current thumb frame, see F
T
in Figure 6.2, the computations for the
contact points for F
1
and F
2
become simple and fast.
Depth Constraints
When all fingers have been closed, the only remaining degree of freedom is
a translation in the z-direction for the whole hand. Ideally we would like

142
6 Grasp Planning for a Three-Fingered Hand
to place the palm against the object to achieve a more stable grasp. Due
to depth constraints imposed by, e.g., the height of cylinder and eventual
support surfaces, palm contact might not be possible. In such cases we try
to place the palm as close as possible to the object, without violating any
depth constraints. This determines the value of the last remaining DOF,
the z-translation. The process of determining this translation and the
z-coordinates is accelerated through the use of precomputed trajectories,
described in Section 6.3.4.
Efficient Search
A benefit with the stepwise computation of a grasp hypothesis is that we
postpone the, comparatively, expensive grasp evaluation by having cheap
validation gates (the soft requirements) between each step. As a result,
the last two steps in Figure 6.3 are rarely executed. Experiments have
shown that the last step in Figure 6.3 is seldom executed for grasps that
are not good. This result indicates that the soft requirements are good
at filtering out bad grasp hypotheses.
The resolutions used when stepping through combinations of d
T
and ϕ
are 2 mm and 1
◦
, respectively. Performing a depth-first search, i.e., test-
ing every possible combination of d
T
and ϕ before moving on to the next
thumb position, would not be efficient. Instead a quick and coarse scan
is done over the most promising combinations and then a new thumb
position is generated. When the number of valid thumb positions has
reached an upper limit, we go back to the first thumb position again and
try less likely combinations. Likely combinations of d
T
and ϕ are deter-
mined from global contour characteristics such as size and eccentricity.
The definition and computation of the characteristics will be described
in Section 6.3.2.
Grasp Robustness
An important aspect of the presented grasp planner is that it explicitly
searches for robust grasps. By robust is here meant that the grasp prop-
erties should not change dramatically for perturbations of the geometry
and the hand/wrist configuration. Here we measure the grasp robustness
by sampling s, d
T
and ϕ in a neighborhood of the current hypothesis
and counting the number of kinematically valid neighbors it has, i.e., the
number of valid samples. Hypotheses that accumulate a low count of
valid neighbors are given a low confidence and they are kept idle in case

6.3 The Grasp Planner
143
no better hypotheses are found.
Grasp Quality and Planner Termination
Each thumb position will keep a reference to the best grasp, i.e., the best
combination of d
T
and ϕ at this thumb position. The quality of this grasp
is also the quality of the thumb position. When the quality of the N
g
best thumb positions all exceed a lower threshold, the planner sorts these
thumb positions into an output list. The number of grasps in the output
list, N
g
, is a user-defined parameter. The sorting criterion considers
both the quality (here the same as the strength) and the robustness of
the grasp. Hence, a strong grasp might end up last in the output list if
it has a low robustness.
In terms of optimization, the planning can be seen as if we, at each
thumb position, are optimizing over d
T
and ϕ, followed by an optimiza-
tion over thumb positions, i.e., the parameter s. With the algorithm
described here, the planner will obviously find suboptimal solutions, but
that is not a problem as long as we find grasps that are good enough for
the task at hand. The planner can also terminate directly if it finds a
grasp with quality and robustness near the theoretical upper limits. This
decision can greatly reduce the planning time in some instances, espe-
cially for symmetric objects like cylinders with a circular cross section.
6.3.2
Global Contour Characteristics
So far, the grasp planner only performs an exhaustive search over the
configuration space. To make the grasp planner more efficient, it must
also be able to decide which directions to examine first based on global
contour characteristics. Here we propose to use the size and eccentricity
to characterize the contour.
The size of the object is simply defined as the radius of the smallest
circle enclosing the polygon. This is a standard problem in computational
geometry and there exist algorithms with O(N log N ) complexity, see
Preparata and Shamos [121], where N is the number of vertices in the
polygon.
The eccentricity of the contour is defined as
e =
I
x
+ I
y
+
(I
x
− I
y
)
2
+ 4I
2
xy
I
x
+ I
y
−
(I
x
− I
y
)
2
+ 4I
2
xy
,
(6.1)

144
6 Grasp Planning for a Three-Fingered Hand
where I
x
, I
y
and I
xy
are the second order area moments, defined as
I
x
=
y
2
dA,
I
y
=
x
2
dA,
I
xy
= − xy dA.
(6.2)
Note that in Equation (6.1), the area moments are computed with re-
spect to a frame located at the centroid of the contour. The eccentricity
measure defined by Equation (6.1) has a useful geometric interpretation:
If we replace the contour with an ellipse that has the same principal mo-
ments, then the eccentricity is equal to the squared ratio of the major
axis to the minor axis.
Using Green’s formula and the fact that the contour is a polygon, we
can easily transform the area integrals in Equation (6.2) into the following
sums:
I
x
=
1
12
N
i=1
(x
i
− x
i+1
)(y
i
+ y
i+1
) y
2
i
+ y
2
i+1
,
(6.3)
I
y
=
1
12
N
i=1
(y
i+1
− y
i
)(x
i
+ x
i+1
) x
2
i
+ x
2
i+1
,
(6.4)
I
xy
=
1
24
N
i=1
(x
i+1
y
i
− x
i
y
i+1
) (x
i
+ x
i+1
) (y
i
+ y
i+1
)
+ x
i
y
i
+ x
i+1
y
i+1
,
(6.5)
where we have introduced the convenient notation x
N +1
= x
1
and
y
N +1
= y
1
. The summation formulas are similar to those given in [139],
but they are slightly more effective in terms of the required number of
summations and multiplications. Using the above sums, the moments for
the contour can be quickly computed. The moments are used to compute
the eccentricity and the directions of the principal axes.
Good properties of the size and eccentricity parameters used here are
that they are intuitive, global, and invariant to rotations and translations
of the contour. Their global nature can be used to quickly give the grasp
planner indications of where to look for good grasps. For very eccentric
objects, the planner first tests thumb positions that allow the hand to
wrap around the minor axis of the object, see Section 6.3.3 and Figure 6.8.
The size of the contour determines the order in which the d
T
-values are
tested; for a large contour large values of d
T
are tested first, and vice
versa.

6.3 The Grasp Planner
145
The idea of using global contour characteristics can be applied to
grasp planners for other hands as well.
6.3.3
Choosing Good Thumb Positions
Each valid thumb position will give rise to a high number of possible grasp
hypotheses. With the current resolution, each thumb position result in
5460 possible combinations of d
T
and ϕ. Therefore it would be good if
we, at an early stage, could classify a thumb position as good or bad with
some degree of confidence.
Let the term contact triangle denote the triangle formed by the con-
tact for F
1
, F
2
and the thumb, respectively. A common property for
grasps that can hold heavy objects is that they have the center of gravity
within the contact triangle. This is especially true for vertical grasps,
where the gravity is perpendicular to the view plane. If we place the
thumb and find that the centroid is not in front of it, there is no way we
can get the centroid within the contact triangle. Taking this idea fur-
ther, we can say that we want the centroid to be within the thumb’s field
of view (FOV), which is defined as a conical sector with a user-defined
opening angle. Hence, thumb positions that do not have the centroid in
the thumb’s FOV are put in an idle state.
For very eccentric objects, good grasps will often be those with ϕ ≈ 0
◦
and the hand wrapping around the minor axis of the object. So for
eccentric objects, we can, as a first step, require the normal of the thumb
to be almost perpendicular to the minor principal axis. Those thumb
positions that do not fulfill this requirement are put in an idle state.
For every thumb position, the planner keeps track of the number
of tested and valid grasps, respectively. If the number of valid grasps
remains close to zero even though the number of tested grasps is very
high, the thumb position is put in an idle state. Note that this decision
has to be grounded on the fact that the planner perform coarse scans
over the entire range of d
T
and ϕ values.
Because our strategy is to perform coarse scans over the configuration
space, new thumb positions should always be as far away as possible
from the previous ones. Therefore we let the s-values form the following
sequence:
s
1
= 0,
s
2
=
s
tot
2
,
s
3
=
s
tot
4
,
3s
tot
4
, . . .
where s
tot
is the perimeter of the contour.

146
6 Grasp Planning for a Three-Fingered Hand
To summarize, we have the following rules for the thumb positions:
• The centroid should be within the thumb’s FOV.
• For very eccentric objects, look for grasps that can wrap around
the minor principal axis of the object.
• Thumb positions where the accumulated number of valid grasps
is low (in comparison to the number of tested grasps) should be
avoided.
• New thumb positions are generated to be as far away as possible
from the previous ones.
6.3.4
Use of Precomputed Trajectories
As long as the inner finger link is free to move, the finger will follow a
predetermined trajectory when closing. This implies that interpolating
from precomputed trajectories will almost eliminate the need for solving
kinematical problems. For the grasp planner, a number of trajectories
were found to be useful, see Figure 6.4.
Trajectory 1 is the contact position as the finger closes. Due to the
cylinder assumption, the finger tip tangent will always be vertical at the
contact point. The x-coordinate for the contact point defines the finger
extension, denoted d
T
, d
1
and d
2
for the different fingers. All other
trajectories are expressed as functions of the finger extension. Once the
finger extension is found, all other parameters, including motor encoder
values, are quickly found through linear interpolation.
Trajectory 2 is traced out by the maximum z-coordinate for the finger.
This is useful, for example, in the case of a vertical grasp, where we want
to avoid the finger tips colliding with the support surface, see Figure 6.5.
Trajectory 3 shows how close the cylindrical object can get to the
palm. Trajectories 1, 2 and 3 are all used when determining the z-
coordinates for the contacts. The strategy is to move the palm as close
as possible to the cylinder, without violating any of the geometric con-
straints imposed by the trajectories in Figure 6.4. An example is shown
in Figure 6.5. The small circles indicates points that were determined us-
ing the precomputed trajectories. Without the depth constraint imposed
by the support surface, the planner would have placed the palm against
the cylinder top as that would generate a more stable grasp.
Another use of the precomputed trajectories is shown in Figure 6.6.
In general it is bad to have a grasp where the finger extensions are very

6.3 The Grasp Planner
147
c
1
c
2
z
x