Robot Path Planning: An Object-Oriented Approach

1.
2.
3.
Figure 6.4:
Interpolating from a number of precomputed trajectories
minimizes the time spent on solving kinematical problems. Trajectory 1
shows the contact position as the finger closes. Trajectory 2 is traced
out by the maximum z-coordinate for the finger. Trajectory 3 shows how
close a cylindrical object can get to the palm. The minimum clearance
needed by a finger is given by c.
different. In Figure 6.4 we see that the z-coordinate of the contact point
varies with the finger extension (trajectory 1). If the z-coordinates of the
contact points are different, the grasp will apply a torque on the object.
This torque tend to decrease the stability of the grasp so we would like
to keep it to a minimum. The measure we have chosen to use here is
the angle between the contact plane and the z-axis, where the contact
plane is defined by the three contact points (see Figure 6.6). This is a

148
6 Grasp Planning for a Three-Fingered Hand
Figure 6.5:
Illustration of how the precomputed trajectories can be used
to quickly compute grasps that satisfy the depth constraints. The circles
denote points that were determined from the trajectories.
natural measure as it captures the fact that a large grasp, i.e., large finger
extensions, is better at tolerating differences in the contact z-coordinates
than a small grasp.
4
If the contact plane inclination is large, then the
grasp quality evaluation and the grasp robustness test are postponed until
no better grasp is found. Morales et al. [106] also avoid grasps with large
differences between the contact z-coordinates. They used the following
criterion:
(d
1
− d
2
)
2
+ (d
1
− d
T
)
2
+ (d
2
− d
T
)
2
However, as can be seen from trajectory 1 in Figure 6.4, a large differ-
ence in finger extensions does not necessarily imply a large difference in
contact z-coordinates. As a result, the criterion in [106] can be overly
conservative.
In addition to the trajectories shown in Figure 6.4, we also compute
the horizontal distance between the contact position and the maximum
4
This property is due to the larger torque arms for the friction forces that have to
counteract the generated torque

6.3 The Grasp Planner
149
Figure 6.6:
The precomputed trajectories can be used to determine the
contact plane for the grasp. The contact plane is shown with the solid
line.
x-coordinate for the finger boundary, as a function of the finger extension,
see c
1
and c
2
in Figure 6.4. This is the minimum clearance needed for
the finger, which is useful if, for example, the finger is surrounded by the
contour. This is the case for the thumb in the grasp shown in Figure 6.9.
6.3.5
Grasp Quality Evaluation
The grasp planner must be provided with a value function so that a grasp
hypothesis can be given a value reflecting its quality. The grasp planner
and the value function are treated as two separate objects. That opens
the possibility to switch between different value functions depending on,
e.g., the available computing time, desired properties of the planned
grasp, or task information. Currently, the only value function that has
been tested with the planner is the one described in this section.
If we want to emphasize the ability of the grasp to resist gravitational
forces, a natural value function would be the maximum admissible ob-
ject weight. Thus, the value function becomes a maximization problem
where we have constraints on the contact forces. Here it is convenient to
introduce a new coordinate frame, located at the centroid of the object,

150
6 Grasp Planning for a Three-Fingered Hand
with the z-axis in the opposite direction of gravity. The centroid’s loca-
tion in the view plane is, as mentioned above, computed from the object
contour. The z-coordinate of the centroid is estimated using the height
of the cylinder.
Using this new frame, the value function can be written as the fol-
lowing linear programming (LP) problem:
max
x
f
g
f
g
[0, 0, 1, 0, 0, 0]
T
= Gx,
x
i
≥ 0,
{x
i
}
F
1
≤ 1,
{x
i
}
F
2
≤ 1,
{x
i
}
thumb
≤ 1,
where f
g
is the object weight, x is a column vector of length M × N of
generalized contact forces, M is the number of contacts, and N is the
number of base vectors in the contact model. The matrix G, called the
grasp matrix, is a mapping from the contact forces to the forces and
torques exerted by the grasp on the object. Given the contact model and
the position and orientation of each contact, the grasp matrix is straight-
forward to compute. The notation {x
i
}
F
1
is introduced to conveniently
denote all contact forces on finger F
1
. Hence, the last three inequalities
in the LP problem puts an upper bound on the force exerted by each in-
dividual finger. For more details on contact models and the grasp matrix,
see Section 7.1 or the book by Murray et al. [107].
Here, each contact is modeled as a point contact with friction. The
chosen contact model has N = 8 base vectors. Nguyen [108] pointed out
that in the case of planar polygonal contact areas, there is always an
equivalent representation in terms of a finite number of point contacts.
So, even though we only use point contacts, we can still handle distributed
contacts. A value for the coefficient of friction, µ, must be assumed. Since
µ is only used for comparing grasp hypotheses, it is not so important
that it coincides with the real coefficient of friction. Here µ = 0.3 is used.
Note that even though the planner uses a 2D contour with eventual depth
constraints as input, the grasp evaluation is done for 3D grasps. That is,
not only are the contact positions on the 2D curve taken into account,
but also their z-coordinates.

6.4 Examples of Planned Grasps
151
6.4
Examples of Planned Grasps
The proposed algorithm has been tested on a wide range of geometries
with good results. To give an indication of the required planning time
and the type grasps produced by the planner, three examples are pre-
sented here. In all examples, the contour was given as a spline curve,
which was adaptively approximated by a polygon. The smooth contours
in Figures 6.7 to 6.9 indicate that the approximation error is negligible.
For all the examples, only the best grasp is shown, but the output is
actually an ordered set of N
g
distinct (i.e., well separated thumb posi-
tions) grasps. For the examples we used N
g
= 10. The timing results
were obtained using a Sunblade 100 computer. The time needed for the
spline-to-polygon conversion is included in the total planning time.
In the first example, see Figure 6.7, gravity is directed along the view
plane normal. Thus, we are planning a vertical grasp. The height of
the cylinder is so large that no grasp hypothesis violated any depth con-
straints. Most of the good grasps for this geometry had ϕ ≈ 60
◦
, simply
because the admissible object weight reaches its theoretical maximum if
all three contact forces converge at a single point. Although many strong
grasps were found, we can clearly see in Figure 6.7 that the planner also
emphasizes grasp robustness: The planned grasp can tolerate relative
large perturbations without losing its stability. The polygon curve has
174 vertices and the required planning time was 0.38 seconds.
The second example is a vertical grasp on an eccentric ellipse, see
Figure 6.8. Here the strong eccentricity of the object (e = 16) is used to
initially bias the thumb position and the spread angle such that the grasps
wrap around the minor axis of the object. The best grasp is centered over
the centroid and achieves a large contact area between the palm and the
object, thereby making it a very secure grasp. The polygon curve has 60
vertices and the required planning time was 0.24 seconds.
The last example is a horizontal grasp, with gravity directed down-
wards in the view plane. As the intent was to implement this planner on
a (research) service robot, we chose an object more appropriate to that
context, namely an iron. Because the iron is resting on a support plane,
a constraint box is put around its lower parts, which can be seen as the
dash-dot box in Figure 6.9. This is a much harder problem compared to
the previous ones because much of the contour is not accessible. Obvi-
ously one or two fingers
5
must get under the handle of the iron, which
5
Because of the cylinder assumption, the hook grasp with all three fingers on one
side of the handle is not considered.

152
6 Grasp Planning for a Three-Fingered Hand
1
d
2
F
1
F
2
d
ϕ
ϕ
d
T
x
y
Figure 6.7:
The polygon has 174 vertices and required planning time
was 0.38 seconds. Gravity is perpendicular to the view plane.
is not easy considering the minimum clearance needed for the fingers,
see Figure 6.4. Even though the algorithm assumes cylindrical objects,
this is an example where the planner does well even on objects that do
not fulfill this assumption. From Figure 6.9 we can see that the palm
has contact with the object. If the real robot would execute this grasp,
the hand would be moved forward until the tactile sensor in the palm
senses a contact. When closing the fingers, they would, due to the clutch
mechanism, wrap around the handle and secure the object. This is an
additional reason why grasps with palm contacts are preferred by the
planner. Note that the thumb in Figure 6.9 is surrounded by the contour
and that the clearance is really small. Here the precomputed clearance
measures described in Section 6.3.4 was used to ensure that the clearance
was large enough.
The polygon in this example has 153 vertices and the planning time

6.4 Examples of Planned Grasps
153
Figure 6.8:
The polygon has 60 vertices and required planning time
was 0.24 seconds. The palm makes contact with the object and gravity
is perpendicular to the view plane.
F
1
2
F
mg
x
y
Figure 6.9:
The polygon has 153 vertices and required planning time
was 1.3 seconds. The dashed line indicates the palm position and the
dash-dot line is the constraint box. Note that this grasp has palm contact
with the object.

154
6 Grasp Planning for a Three-Fingered Hand
was 1.3 seconds. The much longer planning time for this example is
partly due to much time was used on hypotheses with fingers F
1
and F
2
inside the constraint box. Another reason is that the size heuristic is not
so appropriate in this case: The very large enclosing circle around the
contour suggests that the planner should initially look for grasps with a
large value for d
T
, whereas all the feasible grasps will have a small value
for the thumb extension. Without the constraint on the lower part of
the iron, the size heuristic would instead have accelerated the planner.
However, the resulting grasps would wrap around the lower and upper
parts of the iron, which, from a practical point of view is not so good.
6.5
Chapter Summary
In this chapter we have presented a grasp planner for the three-fingered
Barrett hand. The planner utilizes the kinematic structure of the hand,
together with a set of heuristics to quickly search through the most
promising directions of the configuration space. Even though the plan-
ner takes a 2D contour as input, depth information is taken into account
and grasp evaluation is done in 3D. This makes the proposed planner
something in between a pure 2D planner and a pure 3D planner.
Grasp robustness is taken into consideration by examining the effect
of perturbations on a grasp hypothesis. The value of the grasp is decided
by considering both its strength and robustness.
Three examples showed that the planner is fast and produces effi-
cient grasps. Furthermore, the example in Figure 6.9 showed that the
planner can produce good grasps even for objects that do not fulfill the
cylinder assumption. This is due to the strategy of placing the palm as
close as possible to the object in combination with the clutch mechanism
controlling the finger curl motion.
Although the algorithm is tailored for the Barrett hand, it is argued
that some of the ideas used here can be used also for other robot hands:
Classifying objects with simple invariants can give rise to regions in the
configuration space that have high probability of producing a good grasp
for geometries in the corresponding class. This is similar to choosing
pregrasp shapes for certain types of objects. These regions, which can
be found either through learning or expert knowledge, can be used to
reduce the search space to a manageable size. Furthermore, with inaccu-
rate models and sensor data, grasp robustness must also be considered.
Another idea proposed here is that modularity and flexibility can be pro-

6.5 Chapter Summary
155
moted by separating the planner from the value function.
If we extend the grasp planner such that it also checks if grasps are
reachable and collision free, then it will fulfill the grasp generator concept,
introduced in Section 5.3. However, to be really useful, an additional
level of planning should be added—a view-planner. The view-planner
would be responsible for choosing appropriate views of the task object,
project the object onto the view plane of the virtual camera and send the
resulting contour to the grasp planner. If the object does not fulfill the
cylinder assumption, the grasp quality computed by the grasp planner
could be misleading. A final step could therefore include testing the
planned grasp on the full model of the object. This would be similar to
the approach in [102], where a grasping simulator was used for the final
grasp evaluation.

Chapter 7
Grasp Stability
Evaluation
Robots that autonomously grasp objects and interact with them must
have methods for choosing appropriate grasps. However, deciding about
a good grasp also requires some method for evaluating and comparing
grasps. Furthermore, evaluation can be performed with respect to differ-
ent properties of the grasp. Considering the uncertainties in models and
sensor data, grasps that are robust to positioning and modeling errors
are to be preferred. Another important property of a grasp is its ability
to apply forces to the grasped object: A good grasp should be able to
efficiently counteract external disturbance forces, especially those forces
that are expected to occur during the task that is to be performed.
In this chapter we present a novel approach to grasp evaluation. The
approach is based on the ability of the grasp to resist disturbance forces,
and it leads to a min-max formulation. We also propose an efficient
algorithm for solving this min-max problem. The result of the algorithm
is easily visualized as a surface in the force space. For polyhedral objects,
we give a proof showing that only the vertices of the object need to be
considered. Compared to other approaches to grasp evaluation, the main
benefits are:
• The procedure incorporates the complete object geometry.
• Task information is easily included and actually reduces the com-
putational complexity.

158
7 Grasp Stability Evaluation
• The result is independent of scale and choice of reference frame.
• The result can be visualized in 3D and is easy to interpret.
In the next section we give a brief introduction to grasp analysis. In
Section 7.2 we discuss related work and in Section 7.3 we present the
proposed grasp evaluation procedure. To demonstrate the procedure, we
give some examples in Section 7.4. The chapter ends with a summary
and conclusions.
Parts of the material in this chapter has been presented in [143].
7.1
Grasp Analysis Introduction
In this section we give a brief introduction to grasp analysis and introduce
the notation and concepts that are used in the context of grasp evaluation.
It is assumed that the grasped object is rigid and that the grasp
consists of any number of point contacts with friction. The point contact
assumption might seem limiting but, as was pointed out by Nguyen [108],
any planar polygonal contact can be represented as the convex sum of
point contacts placed at the vertices of the contact polygon. Attached
to the object is a reference frame, to which all contacts and forces are
related.
Each contact will have its own reference frame, with the z-axis point-
ing in the direction of the inward surface normal, see Figure 7.1 (a).
Because of friction being present, the contact force can deviate from the
z-axis. If the contact forces obey the Coulomb friction model, then the
space of all admissible contact forces forms a circular cone with opening
angle 2 tan
−1
(µ), where µ is the coefficient of friction. This cone, called
the friction cone, will impose nonlinear constraints on the contact force
components.
In literature, the circular friction cone is often approximated with
an n-sided pyramid, see Figure 7.1 (b). By doing this, we can write
the contact force as a positive linear combination of the force vectors
spanning the pyramid:
f
=
n
j=1
α
j
f
j
,
α
j
≥ 0.
(7.1)
Note that by choosing the vectors {f
j
} to have unit z-component, the
normal component of the contact force is easily obtained as
n
j=1
α
j
.

7.1 Grasp Analysis Introduction
159
y
z
2 tan
−1
(µ)
(a) Point contact
x
y
z
f
j
(b) Friction cone approximation
Figure 7.1:
For nonslipping contacts that obey the Coulomb friction
model, the contact forces must be inside the friction cone. (a) A side
view of a point contact together with its coordinate system. (b) An
example of a friction cone approximated by a five-sided pyramid.
It is often convenient to concatenate force and torque vectors, F and
T
, into a wrench, defined as W = (F
T
, T
T
)
T
. A wrench is thus a
six-dimensional column vector.
Each force f
j
will result in an object wrench w
j
, which can be com-
puted if the position and the orientation of the contact relative the object
frame is known. Let the w
j
from all contacts be the columns of a 6 × mn
matrix G, where m is the number of contacts. This matrix is called the
grasp matrix. Summing up the contributions from all contacts, the total
wrench exerted by the grasp on the object, W, can be written as
W
= Gx,
x
k
≥ 0,
k = 1, . . . , mn
(7.2)
where x is a vector containing the α
j
for all contacts. See the book by
Murray et al. [107] for more details on how to construct the grasp matrix.
When analyzing a grasp, it is of interest to know the space of wrenches
that can be applied to the object by the grasp. The unit grasp wrench
space (UGWS) is often
1
defined as the space of wrenches that satisfies
Equation (7.2) and
mn
k=1
x
k
= 1. This space is equal to the convex hull
1
Note that by choosing another norm for the contact force vector, we get other def-
initions for the UGWS. Another common, and more natural, definition of the UGWS
is to limit the normal component of each individual contact to one, see Ferrari and
Canny [41]. With this definition, however, the UGWS is much more costly to compute,
and therefore the definition based on the sum of all contact forces is more common.

160
7 Grasp Stability Evaluation
of G, which can be efficiently computed using the Quickhull algorithm,
see Barber et al. [12].
An important class of grasps are those that have force closure. A
grasp has force closure if there exists a solution to Equation (7.2) for
any W. This means that the grasp can counteract any external wrench
acting on the body by adjusting the contact forces properly. If a grasp
has force closure, then the convex hull of G must contain a neighborhood
of the origin [107]. The converse is also true: If the convex hull contains
a neighborhood of the origin, then the grasp has force closure.
7.2
Related Work
Grasp evaluation can be performed with respect to different properties of
the grasp. Which properties are important is determined by the context
and the task. For example, when inserting a pin into a hole, it is impor-
tant that the grasp is compliant to not give rise to large contact forces
due to position and orientation errors. For such a task it is important
that the grasp is not only stable, but also that it has a high manipulabil-
ity measure. Grasp manipulability is the degree to which the fingers can
impart arbitrary motions to the object, see Kerr and Roth [66].
Grasp Robustness
Considering the uncertainties in models and sen-
sor data, grasps that are robust to positioning and modeling errors are
to be preferred. Nguyen [108] addressed this by developing algorithms
for finding maximal independent contact regions for several important
types of grasps; among them, two-finger grasps of polygons. As long as
the contacts are within these regions, the grasp has force closure. Ponce
and Faverjon [120] extended the results of Nguyen to three-finger grasps
of polygons. Thus, if robustness to position and modeling errors are of
concern, a grasp quality measure could be chosen as the length of the
smallest contact region.
Bone and Du [19] derived a measure of robustness to positional errors
for polygon grasps. They consider all combinations of finger displace-
ments and evaluate the maximum torque magnitude the resulting grasp
can resist. The robustness measure is the sum of the relative change in
torque resisting capability for all the displacements.
It is well known that friction helps to make a grasp more stable. In
situations where the coefficient of friction is not know beforehand, we
should favor grasps whose stability is not dependent on a high coefficient

7.2 Related Work
161
of friction. Mantriota [95] defined as quality measure the minimum co-
efficient of friction needed to resist a set of unit disturbance wrenches,
directed along the principal wrench space directions. The resulting grasps
are robust in the sense that they rely as little as possible on friction.
Wrench Space Volumes
Another important property of a grasp is
its ability to apply forces to the grasped object. A good grasp should be
able to efficiently counteract external forces, especially those forces that
are expected to occur during the task that is to be performed. Previous
work focusing on this aspect of a grasp is naturally divided into task in-
dependent quality measures and task directed quality measures. In task
independent grasp evaluation, information about the task is assumed to
be unavailable or ignored, and the resulting grasp quality measure there-
fore reflects, in some way, the overall stability of the grasp. In task
directed grasp evaluation, on the other hand, the suitability of a grasp
with respect to a particular task is evaluated. Both types of quality mea-
sures are often based on the UGWS. The wrench space is six-dimensional
and, intuitively, a good grasp should have a large UGWS whose shape
matches the set of wrenches that are expected to occur for the given task
(the task wrench space).
Kirkpatrick et al. [69] proposed a quality measure which is the radius
of the largest wrench space ball that just fits within the UGWS. This mea-
sure was also used by Ferrari and Canny [41] and by Pollard [119]. This
quality measure is task independent, and it assumes that all directions in
the wrench space are equally important. There are two drawbacks with
this kind of measure: It is not scale invariant and it is not invariant to
translations of the torque origin. The first drawback is easily remedied by
normalizing the torque components of the wrench vector, e.g., with the
inverse of the maximum distance from the torque origin to the surface of
the object [119]. The second drawback is more severe, because, clearly,
a grasp quality measure should not depend on the choice of reference
frame. This issue was addressed by Teichmann [148] , who proposed as
an invariant measure the radius of the largest wrench space ball with
respect to all possible coordinate frames. This approach do however lack
a simple physical interpretation; the choice of coordinate frame for eval-
uating a grasp is dependent on the grasp itself. A much simpler invariant
measure was proposed by Li and Sastry [88]; they suggested to use the
volume of the UGWS as an invariant measure. Note, however, that a
volume-based quality measure can give a nonzero quality to grasps that
do not have force closure, i.e., are unstable in some direction.

162
7 Grasp Stability Evaluation
Much of the problems with quality measures that are based on wrench
space balls arise because we are trying to compare forces with torques,
which does not make sense as they have different units. To avoid such
comparisons, Mirtich and Canny [104] used a decoupled approach, leading
to a two-valued quality measure. The optimal grasp is the one that
lexicographically maximizes both values; they first compute the grasps
that best counteract pure forces, and then select among those grasps the
one which best resists pure torques.
To obtain task directed measures, Li and Sastry [88] suggested the
use of six-dimensional task ellipsoids, whose shape resembles the space of
forces and torques encountered in the task. They defined a task directed
quality measure as the largest scale factor that causes the task ellipsoid
to be embedded in the UGWS. However, as pointed out in [88], “The
process of modeling a task by a task ellipsoid is quite complicated”.
Including the Object Geometry
Using the UGWS alone for the
construction of a quality measure has a drawback that, seemingly, has
not received much attention: The wrench space is constructed from in-
formation about the contacts alone, thus, effects of the complete object
geometry on grasp stability are ignored. A grasp quality measure that
does not take object geometry into account would treat the two grasps
in Figure 7.2 as equal; our intuition, however, tells us that the grasp of
object A should be more stable than that of object B.
Introducing the object wrench space (OWS) concept, Pollard [119],
actually incorporates the complete object geometry into the grasp eval-
uation. The OWS represents the best grasp of the object that can ever
be achieved . An alternative view is that the OWS is the set of wrenches
that can be created by a (unit) distribution of disturbance forces act-
ing anywhere on the surface of the object. Thus, the OWS depends on
the geometry of the object. The OWS concept is closely related to the
idea presented in this chapter, however, the examples presented in [119]
on this subject are limited to two-dimensional polygons with frictionless
point contacts.
In a recent approach, Borst et al. [22] combined the OWS concept
of Pollard [119] with the task ellipsoids of Li and Sastry [88]. If no
task information is given, the best assumption one can make about the
disturbance wrenches is that they are distributed according to the OWS.
Borst et al. [22] therefore choose as task ellipsoid an ellipsoid that tightly
encloses the OWS. As the task ellipsoid is automatically constructed, it
removes one of the strongest objections against using them. It it is also

7.2 Related Work
163
x
y
A
B
Figure 7.2:
The grasp’s stability will depend on which object is grasped,
object A or object B. Most grasp quality measures do, however, only use
the contact information.
worth noting that their method for computing the grasp quality does not
rely on any friction cone approximations.
Visualization
Visualization of a six-dimensional wrench space is of
course impossible and therefore many papers only dealt with two-
dimensional grasping problems. The wrench space of a 2D grasping prob-
lem is three-dimensional and easy to visualize. Miller and Allen [100] sug-
gested several methods for projecting the 6D wrench space into 3D. Using
these projections, important characteristics of the 6D wrench space could
be visualized. The approach presented here can be seen as a natural pro-
jection from the six-dimensional wrench space to the three-dimensional
force space.
Compliant Grasps
The work presented so far have all assumed that
grasp compliance can be neglected. That is also the assumption of the
method proposed in this chapter. For compliant grasps, the grasp stiff-
ness matrix is a useful tool, see, e.g., Howard and Kumar [57]. A com-
pliant grasp is stable if the grasp stiffness matrix is positive definite. It
seems natural to base a quantitative measure of stability on the eigen-
values of the stiffness matrix, but care must be taken as these are not
invariant under change of reference frame, see, e.g., [90, 24].
Bruyninckx et al. [24] derived a grasp quality measure based on the
generalized eigenvalue decomposition of the grasp’s stiffness matrix. The
generalization requires a choice of a metric on the group of rigid body
displacements that enables the identification of twists with wrenches. Lin
et al. [90] derived a frame-invariant quality measure in terms of the prin-
cipal translational and rotational stiffness parameters. By introducing a

164
7 Grasp Stability Evaluation
physically based conversion of rotational stiffness parameters into equiva-
lent translational stiffness, Lin et al. overcame the problem of comparing
translational and rotational stiffness parameters.
7.3
Grasp Evaluation Procedure
As was pointed out in [88], modeling a task using six-dimensional task
ellipsoids is laborious. Furthermore, visualization of the wrench space
is impossible, unless the grasp problem is two-dimensional. We argue
that the explicit use of the torque component is not necessary, thereby
reducing the six-dimensional wrench space into a three-dimensional force
space.
Key Idea
Consider how disturbance wrenches are applied to a grasped
object: In almost all practical cases, a disturbance wrench arises from
a pure force acting on the surface of the object. The resulting torque
component is immediately given by
T
= a × F,
(7.3)
where a is a vector specifying where the force F is applied. Specifying the
grasp matrix G and a in the same coordinate system is key here: Using
the same torque origin assures that the result will be independent of the
choice of frame. According to Equation (7.3), the torque component is
not independent of the applied force and they are always orthogonal to
each other. Based on this observation, the grasp evaluation procedure
described below is proposed.
Consider a unit vector ˆ
e
, representing a fixed direction for the distur-
bance force so that the disturbance force can be written as f ˆ
e
, where f
is a dimensionless scalar. Sweep this disturbance force over the surface of
the object, finding the smallest, positive f that results in a wrench that is
exactly on the border of the UGWS. Let us denote this value by f
⋆
. Note
that when performing this ‘sweeping’ operation, only those points on the
object for which ˆ
e
is inside the friction cone can come into consideration.
We also require f ≥ 0 because we cannot allow tractional disturbance
forces. Repeating this process for all directions of the disturbance force,
we will end up with a closed surface S in force space. Specifying force di-
rections with spherical coordinates ϕ and θ, the surface S is given by the
vectors {f
⋆
ϕθ
ˆ
e
ϕθ
}. The interpretation of this surface is straightforward:

7.3 Grasp Evaluation Procedure
165
If a disturbance force is inside S, a unit grasp will be able to resist the
resulting wrench, no matter where the force is applied.
Min-Max Formulation
The above procedure can be formulated
mathematically as a min-max problem. Let ∂D denote the surface of
the object, and FC
a
the friction cone at the surface point specified by
the vector a. To include forces that cannot be seen as a force acting on
the surface of the object (e.g., gravitational forces), we can include an
offset wrench W
0
. For each direction ˆ
e
, we want to solve the following
problem:
f
⋆
= min
a
max
x
f ∈ R
+
:
−W
0
− f
ˆ
e
a
× ˆ
e
= Gx,
ˆ
e
∈ FC
a
,
a
∈ ∂D,
x
i
≥ 0,
mn
i=1
x
i
= 1 .
(7.4)
The resulting surface S can be seen as the space of admissible distur-
bance forces around W
0
, assuming a unit grasp and that the disturbance
forces are always applied at the worst case surface point. Taking the com-
plete geometry into account, we thereby assume that disturbance forces
can occur anywhere on the object surface. However, as illustrated by
the following example, task information can easily be included by only
choosing those parts of the object surface where disturbance forces are
likely to occur.
Task Directed Evaluation
The usual way to hold a pen when writing
is to place the fingertips close to the tip of the pen, letting its upper part
rest between the thumb and the index finger, see Figure 7.3. Clearly,
this grasp cannot easily resist forces acting on the upper part of the pen.
Using the complete surface of the pen as input to the evaluation procedure
would thus result in a poor overall stability for the grasp. However, when
writing, we know that external forces are only exerted on the tip of the
pen, and the chosen grasp is excellent for balancing those. Accordingly, a
task directed measure, indicating a more stable grasp, could be computed
by using only the tip of the pen as input.
Disturbance Force Friction Cone
Note that upon using only parts
of the object surface, S might not be closed. This can happen if some

166
7 Grasp Stability Evaluation
x
z
y
F
Figure 7.3:
The grasp for holding the pen is good at resisting forces
that are applied to the tip of the pen, but bad at resisting forces at the
upper part. A task directed evaluation of the grasp would only include
only the tip of the pen, as this is where external forces are expected to
occur.
of the force directions ˆ
e
never fall inside a friction cone. The same can
happen if the assumed friction cone for the disturbance force is very
small. However, choosing large enough friction cones for the disturbance
forces, the procedure will cover the force directions relevant to the task.
Thus the procedure depends on some imagined coefficient of friction, that
should be chosen considerably larger than the real one.
Disturbance Force Upper Bound
Since we assume a unit grasp,
there exists a theoretical upper bound for S. It is easily shown that, if
W
0
= 0, S is bounded by a sphere with radius
1 + µ
2
. For a nonzero
offset wrench, this bounding sphere will be distorted and translated.
7.3.1
An Algorithm for Solving the Min-Max Prob-
lem
One immediate objection against the proposed procedure is that it seems
both unnecessary and computationally overwhelming to traverse the en-
tire surface of the object. However, it is shown in Section 7.3.3 that if
the object is a polyhedron, then it is enough to traverse its vertices only.
Therefore, in the following analysis, we will assume polyhedral objects.
The UGWS can be seen as the interior of a set of N
P
hyperplanes in
6D. Each hyperplane has a unit normal n
p
and an offset d
′
p
. The purpose
of the prime will be explained below. For a wrench W to be inside the

7.3 Grasp Evaluation Procedure
167
hull we must have
n
T
p
W
+ d
′
p
≤ 0,
p = 1, . . . , N
P
.
(7.5)
Looking for wrenches that are exactly on the boundary of the wrench
space can be done by looking for intersections with these hyperplanes.
Here we use the Quickhull program [12] to compute the hyperplanes.
Clearly, we must discretize the space disturbance force direc-
tions.
This is most easily done using spherical coordinates:
ˆ
e
=
(cos ϕ sin θ, sin ϕ sin θ, cos θ)
T
, where θ ∈ [0, π] and ϕ ∈ [0, 2π). Assume
that the grasp must resist some default offset wrench W
0
. If, in addition,
a disturbance force is applied at a point a on the object’s surface, then
the following must hold for the grasp to remain stable:
−W
0
− f
ˆ
e
a
× ˆ
e
= Gx, x
i
≥ 0,
mn
i=1
x
i
= 1.
(7.6)
The minus signs in the left hand side of Equation (7.6) are necessary
because the grasp has to exert a wrench that cancels the external wrench
to maintain equilibrium.
With ˆ
e
and a constant, the left hand side of Equation (7.6) describes
a line in wrench space parameterized by f :
W
= −W
0
− f W
⋆
,
(7.7)
where we have introduced the convenient notation
W
⋆
=
ˆ
e
a
× ˆ
e
.
(7.8)
Thus, if −W
0
is inside the hull there exists two intersections with the hull
boundary and the line: One with negative f , which we are not interested
in, and one with positive f , which we are looking for. Now we only
need an effective algorithm for finding this positive f -value for each force
direction.
Assume that the wrench W from Equation (7.7) is exactly on hyper-
plane p. Then we obtain, from Equation (7.5)
−n
T
p
(W
0
+ f W
⋆
) + d
′
p
= 0,
p = 1, . . . , N
P
.
(7.9)
We note that the term -n
T
p
W
0
can be seen as coordinate translation
of the wrench space, changing the plane offsets. Thus, we can introduce
d
p
= d
′
p
− n
T
p
W
0
which simplifies Equation (7.9) to

168
7 Grasp Stability Evaluation
−f n
T
p
W
⋆
+ d
p
= 0,
p = 1, . . . , N
P
.
(7.10)
Since it is assumed that the grasp is able to resist the offset wrench, we
must have d
p
≤ 0 for all hyperplanes. This is a more general condition
than the commonly used d
′
p
< 0, which require the grasp to have force
closure. With only one unknown in Equation (7.10), we can solve directly
for f :
f
p
=
d
p
n
T
p
W
⋆
,
p = 1, . . . , N
P
.
(7.11)
The smallest positive f
p
, taken over all hyperplanes, will correspond to
the largest disturbance force that can be applied to a particular vertex a.
Repeating this procedure for all vertices, for which ˆ
e
is inside the friction
cone, will give the worst case disturbance force. However, computing f
p
according to Equation (7.11) for all hyperplanes and all vertices seems
highly inefficient. The complexity of such an algorithm would be close to
O(N
V
N
P
), where N
V
is the number of vertices on the object. In reality
it would be slightly lower due to ˆ
e
falling outside the friction cone for
many vertices.
7.3.2
Improvement from Sorting the Hyperplanes
One way of reducing the complexity would be to precompute and keep
track of the minimum, positive f that can be achieved from each hyper-
plane. Calling these minimum values f
min,p
, we can sort the hyperplanes
so that f
min,p
is always increasing. This way we are likely to find the
limiting hyperplane earlier and we can stop the loop as soon as the cur-
rent minimum f is smaller than f
min,p
. Using Equations (7.8) and (7.11)
together with ||ˆ
e
|| = 1, we find the following simple lower bound:
f
min,p
=
−d
p
n
2
1p
+ n
2
2p
+ n
2
3p
+ a
max
n
2
4p
+ n
2
5p
+ n
2
6p
,
(7.12)
where a
max
is the maximum vertex distance. Note that the minus sign is
necessary because d
p
is negative.
Noticing that ||a × ˆ
e
|| is constant while looping over the hyperplanes,
this sorting idea can be further refined: With knowledge about ||a × ˆ
e
||
we can obtain a better lower bound than that of Equation (7.12). So
instead of sorting only once, sorting can be done N
B
times where each

7.3 Grasp Evaluation Procedure
169
sorting is associated with an interval for ||a × ˆ
e
||. Thus, we have N
B
buckets, where the hyperplane ordering in each bucket b is determined
by sorting them increasing in f
b
min,p
, given by:
f
b
min,p
=
−d
p
n
2
1p
+ n
2
2p
+ n
2
3p
+
a
max
b
n
2
4p
+ n
2
5p
+ n
2
6p
.
(7.13)
So if (b − 1)a
max
/N
B
≤ ||a × ˆ
e
|| < ba
max
/N
B
, the current cross product
belongs to bucket b. One could find even better values for f
b
min,p
using
the fact that ˆ
e
⊥(a × ˆ
e
), but the resulting optimization problem would
cost too much to make it worthwhile.
In Figure 7.4 it is seen that the proposed sorting scheme has a signifi-
cant influence on the computational time. For sorting, a variation of the
Radix Sort algorithm [34] was used.
2
This algorithm is special in that it
has a linear complexity in the number of elements to be sorted. In this
case, sorting always require four traversals over the data, thus we know
that the time spent on sorting will be exactly proportional to the number
of buckets. As the number of buckets are increased, at some point there
will be no further gain from having the hyperplanes sorted. Any further
increase in the number of buckets will then cause a linear increase of the
total computational time. This is also seen in Figure 7.4.
For optimal effect of the sorting, the number of buckets should be
set adaptively for each problem, depending upon the number of object
vertices and the number of hyperplanes. How this should be done re-
quires more numerical experiments and a deeper analysis of the algo-
rithm. However, experiments so far have shown that choosing N
B
= 30
gives a significant performance increase for most problems.
Assuming that the vertices have a uniform range distribution around
the origin, the limiting, or worst case, vertices will often be those which
generate large torque components. A further optimization is therefore
to sort the vertices, decreasing in ||a × ˆ
e
||, before we loop over them.
To satisfy the assumption about uniform range distribution, the object
frame should be placed at the object’s centroid. The final algorithm is
described in listing 1.
2
In books on algorithms, Radix Sort is described as an algorithm for sorting integer
values. It is, however, due to the IEEE Standard 754 floating point representation,
possible to use it for sorting floating point numbers as well; if they are interpreted as
integers, comparisons between them will still be correct (although some care has to
be taken with the sign bit).

170
7 Grasp Stability Evaluation
0
20
40
60
80
100
35
40
45
50
55
60
65
70
75
80
time  [ms]
N
B
Figure 7.4:
The grasp from Figure 7.5 (a) was evaluated with F
g
= 0
(8 object vertices, 416 hyperplanes, and 578 force directions). The curve
shows required computational time, in milliseconds, as a function of N
B
.
For the special case N
B
= 0, no sorting at all is done.
7.3.3
Disturbance Forces and Object Vertices
An important question is how many points on the surface of the body
we have to consider when moving around the disturbance force. Here
we will give a proof showing that, if the body is a polyhedron, then it is
sufficient to only consider its vertices.
Theorem 1
For a polyhedral body grasped by a unit grasp, the worst
point of attack for a disturbance force will always be a vertex of the body.
Proof:
Without loss of generality, we assume that each face of the poly-
hedral body is a convex polygon; in case of a nonconvex polygon, we can
always decompose it into a finite number of convex polygons. The kth
face is the convex hull of the vertices a
1
, a
2
, . . . , a
N
k
, where N
k
is the
number of vertices.
3
Any position a on the kth face can thus be written
as a convex combination of its vertices:
3
A more strict notation would show that the vertices of each polygon face is a sub-
set of the vertices of the whole body. However, besides from running out of letters,
we believe that additional indices in this case would only lead to a notation that is
harder to read.

7.3 Grasp Evaluation Procedure
171
Algorithm 1
Compute disturbance-force surface
choose object vertices relevant to the task
give the offset wrench → W
0
compute the convex hull of G, → {n, d
′
}
transform the hyperplanes, d
p
= d
′
p
− n
T
p
W
0
if
(d
p
> 0) then
abort {W
0
is not inside the hull}
end if
sort hyperplanes N
B
times, increasing in f
b
min,p
for all
force directions ˆ
e
i
do
select vertices a
j
for which ˆ
e
i
∈ FC
j
sort vertices, descending in ||a
j
× ˆ
e
i
||
f
⋆
i
= +∞
for all
sorted vertices a
j
do
find bucket, b = N
B
||a
j
× ˆ
e
i
||/a
max
for all
sorted hyperplanes n
p
, d
p
do
if
(f
⋆
i
< f
b
min,p
) then
break
end if
f
cand
= d
p
/(n
T
p
W
⋆
)
if
(f
cand
≥ 0 and f
cand
< f
⋆
i
) then
f
⋆
i
= f
cand
end if
end for
end for
end for
a
=
N
k
i=1
β
i
a
i
,
N
k
i=1
β
i
= 1,
β
i
≥ 0.
(7.14)
For a given disturbance force direction ˆ
e
we only have to consider the
subset of faces for which the disturbance force satisfy the friction cone
constraint. In this subset, let us now consider the kth face, so that the
position of the disturbance force can be expressed with Equation (7.14).
Using Equations (7.8), (7.11) , and (7.14), we can write the intersection
between the applied wrench and the pth hyperplane as

172
7 Grasp Stability Evaluation
f
p
=
d
p
b
0p
+ b
1p
β
1
+ · · · + b
ip
β
i
+ · · · + b
N
k
p
β
N
k
,
(7.15)
where b
0p
= n
1p
e
x
+ n
2p
e
y
+ n
3p
e
z
and b
ip
, 1 ≤ i ≤ N
k
, can be computed
from n
4p
, n
5p
, n
6p
, e
x
, e
y
, e
z
and a
1
, . . . , a
N
k
. It is seen that, for each
hyperplane, the minimum positive f
p
is achieved by letting the β
i
with
smallest coefficient become one (note that d
p
is negative for a stable
grasp). This clearly corresponds to one of the vertices of the polygon. So
for each hyperplane, the worst point of attack for the disturbance force
will correspond to a vertex of the polygon. Hence, the overall worst point
of attack will also be a vertex.
Since it is sufficient to consider only the vertices for each polygon
face, we conclude that we only have to consider the vertices of a polyhe-
dral body. Note that this conclusion is independent of the friction cone
opening angle: Narrowing the friction cone will only reduce the num-
ber of polygon faces that has to be considered for each disturbance force
direction.
7.4
Examples
To illustrate the procedure proposed in this chapter, it was tested on two
small problems. For both problems, the coefficient of friction was set to
0.3, and each friction cone was approximated by eight force vectors. The
resolution for the disturbance force directions are set to 10
◦
for both ϕ and
θ, resulting in a total of 578 force directions. This simple discretization
scheme will produce more samples for the force directions near θ = 0
◦
and 180
◦
. A uniform, and therefore more efficient, discretization could be
made by choosing the force directions as the vertices of a geodesic dome.
When evaluating the grasp, the coefficient of friction for the disturbance
force was set to 1.5. Note that the friction cone for a vertex can be
defined somewhat arbitrarily. Here it is defined as the union of the cones
belonging to the planes forming the vertex. An additional cone, in the
direction of the averaged vertex normal is also added.
Adding Gravity
In the first example we will look at the effect of
adding gravity in the evaluation of a grasp. To simplify interpretation,
the grasped object is simply a box and the grasp is symmetrical. The box
has dimensions 2 × 2 × 5. The object frame is placed in middle of the box,
such that the z-axis points along the longer direction of the box. The

7.5 Chapter Summary
173
box is grasped by four frictional point contacts that are symmetrically
placed in the xy-plane. The coordinates of the contacts are (±1, 0, 0)
T
and (0, ±1, 0)
T
, see Figure 7.5 (a). Solving for the convex hull of the
grasp matrix G results in 416 hyperplanes. The grasp was evaluated two
times; first without gravity and then with the gravitational force set to
F
g
= 0.15. Gravity acts along the negative z-axis. Note that F
g
= 0.3
is the upper limit for the unit grasp since it is assumed that µ = 0.3. In
Figure 7.6, two views of S are given for each grasp evaluation. In the
first case, F
g
= 0, the problem is totally symmetric. Hence, the surface
S is symmetric around all axes and the grasp has almost equal strength
in all directions. Breaking the symmetry by adding a small gravitational
force has the major effect of translating S upwards, compare the upper
diagrams in Figure 7.6. This is due to upward directed disturbance forces
having to work against the gravitational force and vice versa. It is also
seen that the total volume enclosed by S has decreased due to the grasp
being preloaded by a nonzero W
0
. As the gravitational force is further
increased, the volume reducing effect quickly becomes dominant. This
behavior simply reflects the loss of stability margins for the grasp as it is
being used more and more for just holding the weight of the object.
Adding Contacts
In this example we want to study the effect of
adding an additional contact to the grasp in the previous example. In the
presence of gravity, the grasp in Figure 7.5 (a) is weak in the downward
direction. To make most use of an additional contact, it should therefore
be placed under the box so that it can work against the gravitational
force. The resulting grasp is shown in Figure 7.5 (b). When compar-
ing the two grasps, the gravitational force was set to 0.1. The result is
shown in Figure 7.7. It is seen that the extra contact expands the surface
S in all directions, except in the positive z-direction. As expected, the
expansion is largest in the negative z-direction. The expansion of S in
the xy-plane is explained by the friction force between the box and the
extra contact: Due to its large torque arm, this friction force efficiently
counteracts the torque generated by a horizontal disturbance applied to
the upper or lower parts of the box.
7.5
Chapter Summary
When comparing task directed and task independent grasp measures to
each other, one most often finds that the former are more complicated

174
7 Grasp Stability Evaluation
x
y
z
(a) Four point contacts
x
y
z
(b) Five point contacts
Figure 7.5:
Illustration of the grasps evaluated in the examples. The
box dimensions are 2 × 2 × 5 and gravity is directed along the negative
z-axis. The coefficient of friction is set to 0.3 for all the contacts and
each friction cone is approximated by eight force vectors. Note that the
friction cones have been plotted on the outside of the object to make
them more visible.
and take longer time to compute. The proposed algorithm breaks this
pattern: Task information is easily included by removing object vertices
that are unlikely to be in contact during the task, thereby reducing the
size of the problem.
One of the main features of the proposed evaluation procedure is that
the torque-component of the wrench space is used implicitly, thereby
reducing the result to a 3D surface in force space. This surface is easily
interpreted and visualized. Furthermore, in the case of a zero offset
wrench, this surface is bounded by a sphere with radius
1 + µ
2
.
An algorithm solving the resulting min-max problem was also pro-
posed. It was shown that introducing a sorting procedure greatly reduced
the complexity of the algorithm. Further improvements would involve
using more uniformly distributed force directions (e.g., the vertices of
a geodesic dome) and downsampling of detailed objects (to reduce the
number of vertices that has to be considered).
Offset wrenches, like gravitational forces, are easily taken into ac-
count. In some cases the offset wrench can even have a stabilizing effect
on the grasp, as when carrying a plate in the open palm; the weight of

7.5 Chapter Summary
175
the plate helps the grasp to resist horizontal disturbance forces.
The surface S can be used as a quality measure as it is, but in many
cases one would prefer to work with a scalar quality measure. How to
derive a scalar quality measure from S is not addressed here, but it is
argued that the proposed procedure provides a sound basis for doing so.
One suggestion is to choose to choose the minimum f
⋆
over all direc-
tions. A force with this magnitude can be resisted by a unit grasp no
matter where it is applied and no matter its direction. The drawback of
such a quality measure is that it could be overly conservative. If S is a
closed surface, one could instead use the volume enclosed by S as a less
conservative measure.
The proposed procedure can be used in grasp planning, for finding
good grasps for robot grippers. It can also be used as a validation gate in
a more reactive manner: If the executed grasp is good enough, the robot
will continue its task. Otherwise the object will be re-grasped.

−0.1
−0.05
0
0.05
0.1
−0.15
−0.1
−0.05
0
0.05
0.1
0.15
F
z
F
x
F
g
= 0
−0.1
−0.05
0
0.05
0.1
−0.15
−0.1
−0.05
0
0.05
0.1
0.15
F
z
F
x
F
g
= 0.15
−0.1
−0.05
0
0.05
0.1
−0.1
−0.05
0
0.05
0.1
F
y
F
x
−0.1
−0.05
0
0.05
0.1
−0.1
−0.05
0
0.05
0.1
F
y
F
x
Figure 7.6:
Illustration of the effect of adding gravity to the grasp
in Figure 7.5 (a). The left-hand figures show two views of the surface
S when gravity is neglected. In the right-hand figures, a gravitational
force of 0.15 is included. Because µ = 0.3, this is also the maximum
gravitational force a unit grasp can withstand. Note that the top of S is
plotted darker to make the views easier to identify and that the scale is
the same in all the figures.

−0.12 −0.06
0
0.06 0.12
−0.25
−0.2
−0.15
−0.1
−0.05
0
0.05
0.1
0.15
F
z
F
x
Four contacts
−0.12 −0.06
0
0.06 0.12
−0.25
−0.2
−0.15
−0.1
−0.05
0
0.05
0.1
0.15
F
z
F
x
Five contacts
−0.12 −0.08 −0.04
0
0.04
0.08
0.12
−0.08
−0.04
0
0.04
0.08
0.12
F
y
F
x
−0.12 −0.08 −0.04
0
0.04
0.08
0.12
−0.08
−0.04
0
0.04
0.08
0.12
F
y
F
x
Figure 7.7:
The left-hand figures show two views of the result for the
grasp with four point contacts in Figure 7.5 (a). The right-hand figures
show the result of adding one extra contact, working against the gravity,
see Figure 7.5 (b). The extra contact causes the surface S to expand
in all directions, except for the positive z-direction. As expected, the
expansion is largest in the negative z-direction. For both grasps, the
gravitational force was 0.1 and the coefficient of friction was 0.3.

Chapter 8
Summary and
Suggestions for Future
Work
This chapter briefly summarizes the key ideas presented in this thesis.
The summary of each chapter ends with suggestions for future work and
research.
Chapter 2
Chapter 2 presented the design of CoPP, an object-oriented framework for
path planning. The framework can be considered as a set of LEGO blocks
that can be used to build path planners; the blocks can be combined in
many different ways and blocks that share the same same interface can
replace each other. The most important aspects of the framework are:
• It makes it easy to make fair comparisons between variations of a
concept or an algorithm.
• It is flexible and easy to extend.
• It is portable.
Future work for this framework would involve adding classes for dy-
namic systems. With this addition, the framework could be used for
studying methods for kinodynamic motion planning as well.

180
8 Summary and Suggestions for Future Work
Chapter 3
A path planning problem involves some moving system, which could be
a single rigid body, a kinematic chain, or a dynamic model of an aircraft.
Chapter 3 presented a class for representing robots with a tree-like kine-
matic structure. This class can be used to model a wide range of robots
and includes features such as
• Customizable inverse kinematics
• Joint couplings
• Hierarchical robot composition
The possibility to compose complex robots from simpler ones has several
benefits. With a complex robot it might not make sense to consider all
degrees of freedom at the same time. With a hierarchical composition it
is easy to extract the sub-robot that involves the degrees of freedom we
are interested in. The resulting modularity also makes it easy to change
various part of a robot. One can for example change the end-effector tool
of a robot from a parallel-jaw gripper to a welding tool.
Note that the CoPP framework does not hinge upon this robot class;
other models of motion are easily added to the framework.
A possible improvement for the robot class would be to allow other
types of joint couplings. Specialized joint couplings could be used to
model slider-crank mechanisms common in, e.g., excavator arms.
Chapter 4
Chapter 4 presented a method for augmenting bidirectional RRT-
planners with local trees. The experimental results in this chapter show
that local trees help to reduce the required planning time for problems
that involve narrow passages. The best effect was seen on problems where
the solution trajectory had to pass a sequence of difficult passages.
For unbalanced problems where either start tree or the goal tree is
stuck, the method is inefficient because too many nodes are added to one
of the trees. A future version of the algorithm should therefore strive
to keep the trees balanced, focusing more on the tree that has difficul-
ties to expand. Future research would also involve more experiments to
determine better heuristics for when and how to grow local trees.

8 Summary and Suggestions for Future Work
181
Chapter 5
Chapter 5 presented a high-level planner for pick-and-place tasks. Such
tasks were divided into three phases: approach, transport, and return. A
bidirectional RRT-planner was used to find motions for each phase. The
contributions of this chapter are:
• The use of multiple goals in case many arm configurations can reach
the same grasp.
• Retract planners that preprocess start and goal configurations into
configuration space trees.
• Grasp generators that serve as useful abstraction of the grasp plan-
ning process.
• A simple model that allows the planner to handle different task
constraints.
This planner has shown to perform well in virtual environments, where
the map is exact and sensors are perfect. The real challenge would be
to implement this planner on a real robot system. This would call for
sensor-based path planning methods for mobile manipulation, an area
where not so much work have been done.
Chapter 6
The last part of the thesis shifted focus towards grasp planning and grasp
stability. Chapter 6 presented a fast grasp planner for a three-fingered
hand. The input to the planner is only a 2D contour, with optional depth
information. Even though the input may be purely two-dimensional, the
grasp planning takes place in 3D. The grasp planner works by quickly
generating grasp hypotheses, which are evaluated in a step wise fashion; a
hypothesis that does not pass an intermediate evaluation step is idle until
no better grasp is found. This way, the most computationally expensive
steps are postponed until the planner has found a really promising grasp
hypothesis.
Considering noisy sensor data and modeling errors, planned grasps
should be robust against positional and geometric errors. The planner
explicitly tests a grasp’s robustness by disturbing the grasp configuration
in a fixed number of directions. The number of valid “neighbors” is used
as a measure of the grasp’s robustness.

182
8 Summary and Suggestions for Future Work
In most cases, the 2D contour that is the input to the planner corre-
spond to the top view of the object. To be more flexible, however, the
grasp planner should be combined with a view planner that plans which
view of the object should be used to extract the 2D contour.
Chapter 7
Chapter 7 presented a novel approach to grasp stability evaluation. The
approach is based on the ability of a grasp to reject external disturbance
forces. The advantages compared to other approaches are:
• Inclusion of the complete object geometry
• Independent of the choice of reference frame
• Easy to do task directed evaluations
The proposed method produces a three-dimensional surface in the
force-space, but grasp planning algorithms need scalar measures to com-
pare grasps. Hence, a subject for future research would be how to derive
a scalar measure from the three-dimensional force surface.

Appendix A
Class Diagram Notation
Throughout this thesis we have used class diagrams to describe designs
and relationships between classes. This appendix describes the class di-
agram notation itself. The notation is based on OMT (Object Modeling
Technique), invented by Rumbaugh et al. [126], but we have added some
conventions to be able to express C++ constructs like template functions.
A class diagram is a graph whose nodes are classes and whose arcs
are relationships between classes; it depicts classes, their structure, and
the static relationships between them. In the OMT notation, a class is
denoted by a box with the class name in bold font at the top. The key
operations of the class are listed in a separate compartment below the
class name. Any member variables are listed in another compartment,
below the operations.
1
See Figure A.1 for two examples.
Abstract and Concrete Classes
An abstract class is a class where
one or more of its operations lack implementation; its purpose is to define
a common interface for derived classes. An operation that has no imple-
mentation is an abstract operation (also known as a pure virtual function
in C++). A class that has no abstract operations is a concrete class.
The OMT notation [126] has no typographical construct to distinguish
between abstract and concrete classes. Here we follow the convention of
Gamma et al. [45] and set the names of abstract classes in italics, see Fig-
ure A.1. We use the same convention for the operations also. Note that
1
Rumbaugh et al. [126] place the member variables above the list of operations.
We do the opposite to stress that it is the operations of a class, and not its member
variables, that are the most important.

184
A Class Diagram Notation