To appear in the IEEE/RSJ International Conference on Intelligent Robots and Systems, 2013.
This copy is for personal use only. c 2013 IEEE/RSJ
Prints will be available at http://ieeexplore.ieee.org shortly.

Integrated Motion and Clasp Planning with Virtual Linking
Johannes A. Stork

Florian T. Pokorny

Danica Kragic

Abstract— In this work, we address the problem of simultaneous clasp and motion planning on unknown objects withholes. Clasping an object enables a rich set of activities suchas dragging, toting, pulling and hauling which can be applied
to both soft and rigid objects. To this end, we define avirtual linking measure which characterizes the spacial relation
between the robot hand and object. The measure utilizes a setof closed curves arising from an approximately shortest basisof the object’s first homology group. We define task spaces toperform collision-free motion planing with respect to multiple
prioritized objectives using a sampling-based planing method.
The approach is tested in simulation using different robot hands
and various real-world objects.

I. I NTRODUCTION
Robots need to be able to interact with both known andpreviously unseen objects. This problem has been studied
extensively which has resulted in state of the art methods forgrasp synthesis and planning. In addition to simply fixing
an object in the end effector, we would however also liketo equip robots with a richer repertoire of grasps affording
both in-hand manipulation and additional activities such as
dragging, toting, pulling and hauling of soft and rigid objects.
These activities may require more force and grip than apoint contact-based precision grasp provides and a grasps
involving the complete robot hand may be required instead.
To address this, we present work on simultaneous grasp
and motion planning, where we concentrate in particularon the clasping of unknown objects. A clasp is a type ofenveloping grasp that affords pushing, dragging and alike. In
our work, it is formalized using the Gauss linking integral for
closed curves. Clasp planning is performed simultaneouslywith motion planning using a Task Space Rapidly-exploring
Random Tree (TS-RRT) approach. Our approach uses task
space control, sampling-based search and a topological representation of objects. Furthermore, we rely solely on a roughobject reconstruction from point clouds and a representationthat builds on a definition of virtual linking.
The main contribution of our work is to address the
problem of clasp and motion planning in an integrated
manner. In addition, the approach can be used on previously
unseen objects without the need for an exact mesh model.
Our approach defines and utilizes multiple task objectives
to control the motion of the robot. These objectives havedifferent priorities and we show how to design a controller
taking different and altering priorities into consideration.
This work was supported by the Swedish Foundation for Strategic Research and the EU grants FLEXBOT (FP7-ERC-279933) and
TOPOSYS (FP7-ICT-318493). The authors are with the Computer
Vision and Active Perception Lab, Centre for Autonomous Systems, CSC, KTH Royal Institute of Technology, Stockholm, Sweden,{jastork,fpokorny,dani}@kth.se.

Fig. 1. A few examples of our approach for clasp planning with various
robotic hands. Here, the topological feature describes the purse’s handle.

II. R ELATED W ORK AND C ONTRIBUTIONS
Rapidly-exploring Random Trees (RRTs) are commonlyused for motion planning [1]–[3], manipulation planning[4]–[6] and the planning of grasping motions [7]–[9]. For
complex robots, planning of collision-free trajectories is a
PSPACE-hard problem in general [10]. The constraints originate mainly from configuration space (C-space) complexityand from representing the collision-free region, Cfree . To
avoid a time consuming explicit representation of Cfree , [11]–[13] and others have proposed sampling-based algorithms
which implicitly cover Cfree , namely RRTs [13].
In [4], a novel kind of goal bias is introduced by adopting
workspace heuristic functions implicitly defining the C-space
goal region. A workspace distance is used for selection of
expansion nodes which are then randomly extended. Manipulation involving inverse kinematics and complex objects isaddressed in [5]. There, the search is directed towards thegoal in a subspace of the manipulator’s C-space. Workspacebias is realized using transpose Jacobian control to produce
locally optimal expansions. Similarly to our approach, they
iteratively apply control until a collision occurs in order toreach a goal. We also consider goal bias in a space different
from the original C-space; however, we do not work with asubspace of the C-space.
RRT-based planning of grasping motions commonly relies
on a set of pre-defined grasp poses that are computed
offline [7]–[9]. Thus, these approaches consider the problem
in two separate phases and could analogously be transferred
to clasping. However, we are interested in an integrated
approach for clasp and motion planning similar to the worksof [14], [15]. For grasping, such an integrated approachis presented in [16] for single and dual arm problems.
During random exploration of the robot arm’s C-space,
the Grasp-RRT algorithm computes approach movements

towards heuristically selected points on the object followed
by grasp scoring. The idea is similar to ours, although theGrasp-RRT plans contact-level grasps. The main differenceis that Grasp-RRT considers objects of known geometry anduses grasp quality evaluation while our approach uses a roughobject reconstruction from point clouds and virtual linking.
Both approaches integrate motion planning and do not rely
on pre-calculated hand poses. The Grasp-RRT searches onlyin the robot arm’s C-space while our approach searches
in a task space and considers both the arm and the handsimultaneously.
While the approaches of [4], [5] consider subspaces of
the C-space to realize goal bias, the concept of task spaces
from feedback control design has been proposed to reduce
planning space dimensionality [17]. The Task Space RRT(TS-RRT) samples and builds the tree only in the task spaceand entirely avoids non-trivial constraints in C-space. The
use of a TS-RRT is demonstrated by planning for the end
effector position of a 1500 DOF robot arm in [17] and byplanning legged locomotion over rough terrain in [18]. The
hybrid motion planning approach of [19] uses TS-RRT andadapts the C-space trajectory to avoid obstacles. In [20], thetask space path is modified and secondary motion objectives
in the form of cost functions are employed. For our approach,we add another extension procedure to the TS-RRT and also
use secondary objectives. However, our secondary objectives
lie in the task space and we additionally use altering taskpriorities.
For many object interaction tasks, objects do not need
to be completely immobilized in the hand. Instead, caging
grasps may be enough. Thus, our work focuses on thegeneration of clasping grasps that fall into the categoryof caging grasps [21]–[24] to form a chain consisting of
the object and the robot hand. The notion of stretchingand squeezing cages was introduced for the analysis of
caging mobile rigid bodies by [25] and was later extended
to more than two fingers [26]. Caging configurations can beconsidered as a waypoint towards immobilizing grasps [26]and as a method to deal with uncertainty [27].
Applications in manipulation planning related to caging
grasps are proposed by [28]. They consider motion planningfor tasks such as opening and closing doors, drawers, etc.
Exploring an initial set of caging grasps using a RRT,
they ensure caging at each expansion by a random motion
escaping test. In our approach, we plan caging configurations
along with robot motions that could be used by a system suchas the one mentioned above. To show that the resulting clasps
are secure, we also perform random movements trying toseparate the robot from the object in a rigid body simulation.
In our recent work [29], we use winding numbers to
describe how much a robot hand is wrapped around a given
point. However, this representation is merely 2-dimensionaland requires a projection onto a plane. This projection results
in a loss of the 3-dimensional relationship. In this work,
we will instead use the notion of the Gauss linking integral
described in Sec. III-A which can be used to characterize thelinking of curves in three dimensions.

III. P RELIMINARIES
We now formalize the concept of clasping using two
closed non-intersecting curves and explain how it is integrated with motion planning.
A. Gauss Linking Integrals
Given two closed non-intersecting curves γ1 , γ2 in R3 ,their linking number Lk (γ1 , γ2 ) ∈ Z provides an invariant
which describes a spacial relation between the two curves.
If it is non-zero, the two curves cannot be separated without
breaking the loops. If γ1 , γ2 : [0, 1] → R3 are smooth closed
curves, Lk can be computed using the Gauss linking integral:
Lk (γ1 , γ2 ) =
Z 1Z 1γ1 (s) − γ2 (t)100, γ (s) × γ2 (t) ds dt.
4π 0 0kγ1 (s) − γ2 (t)k3 1(1)

The above integral also leads to a formula for the case ofpiecewise linear curves [30] and has also been used when
γ1 , γ2 are not closed [31], [32]. However, in the latter casethe resulting real valued quantity is then not a topological(isotopy) invariant anymore.
B. Shortest Loops and Homology
Informally, the first homology group H1 (U ) [33] of aclosed surface U ⊂ R3 describes equivalence classes ofclosed curves (1-cycles) up to curves that form the boundaryof parts (2-cycles) of U . H1 (U ) yields an Abelian versionof the well-known first fundamental group π1 (U ) which describes equivalence classes of closed curves up to continuous
deformations. Importantly, H1 (U ) is a topological quantity
which stays invariant under certain continuous deformations(homotopies) of U itself. When computed over a finite field,
H1 (U ) is a vector space. In our work, we will not work
directly with a surface U , but with a simplicial complex
approximation Kd of U for which H1 (Kd ) is also welldefined. We are interested in a basis for H1 (Kd ) corresponding to curves which are approximately of shortest lengthand use the software ShortLoop [34], [35] to approximately
determine such curves given Kd .
C. Task Space Planner Outline
To avoid high-dimensionality in motion planning, we facilitate a low-dimensional approach commonly used in feedback control design that has been brought to RRT planning as
Task Space RRT (TS-RRT) [17]. A sampling-based search intask spaces is often appropriate since, for systems with manydegrees of freedom, no closed-form solution exists to map
task spaces to C-space. Most importantly, TS-RRT operates
on the task space and combines a projection from C-space tothe task space with Moore–Penrose pseudoinverse Jacobian
local control. Usual RRTs instead search the C-space directly,
possibly using different types of goal bias [4], [5].
We search for a collision-free trajectory and secure clasping grasps simultaneously using a TS-RRT framework withmultiple tasks objectives of altering priorities. Two tasks
form a joint task space searched by the TS-RRT, while aClasp Controller

Point Cloud

Object Representation• Simplical Complex• Shortest Loops
Heuristics
Initialconf. q0 ∈ Q

Hand Representation• Finger and Thumb

Loops S
Collision Model K)

Motion and Clasp Planning

Point Cloud

Finger Curves C
Palm Center Point

Clasp Controller
Task Space X
Task Space Control• 2D Finger Winding• Jacobian Transpose• Approach Vector• Workspace Plane• Collision Potential
Clasp configuration {qi } for the robot hand

Object Representation• Simplical Complex• Shortest Loops
Robot
Initialconf. q0 ∈ Q

Loops S
Clasping Targets G
Collision Model Kd

Hand Representation• Finger and Thumb• Virtual Chord
Finger Curves C
Orientation vfwd , vswd

Motion and Clasp Planning
TS-RRT Exploration Task Space X
Task Space Control• Random Extension • Hand Center• Task Priorities• Alignment
TS-EXTEND• Distance-based Gain
• Connect Goal
• Virtual Linking
TS-CONNECT-GOAL
Collision-free trajectory q1 , q2 , . . . , qn
Clasp configuration qn for the robot arm and handFig. 2. Comparison of the clasp control approach of [29] (left) and the motion and clasp planning proposed in this work (right). Both approaches relyon the same object representation. While the clasp controller considers winding and uses an approach vector heuristic to generate a clasping pose of adisembodied robot hand, the system presented here is based on virtual linking and results in a complete motion plan for a robot arm and hand.

third task realizes a permanent robot pose bias. No explicit
C-space target is defined—instead, we allow a pre-definedgoal tolerance in the task space to implicitly define goal
regions, which are mainly defined in terms of the conceptof virtual linking.
A conceptual outline of our planning approach is depicted
in Fig. 2 alongside the clasp controller used in our previous
work [29]. Common features are the use of shortest loops
and finger curve representations. Instead of the 2-dimensional
winding number representation used in our earlier work,
virtual linking is used to control hand joints towards a
clasping configuration. Our current work does not require
collision potentials since collision checking is intrinsic to
RRT approaches. The clasp controller returns a set of secure
clasp poses while the motion and clasp planner results ina collision-free trajectory of a robot arm and hand together
with a final clasping pose.
IV. M ETHODOLOGY
We now describe details of our Task Space RRT approachand the representations of object and robot hand.
A. Object Representation
Given a point cloud of an object, we obtain a Delaunay
triangulation, D, and proceeded to refine it by eliminating
triangles containing edges of length d or larger. This leads
to a simplicial complex, Kd , approximating the object. A
basis for H1 (Kd ) consisting of approximately shortest loops
can then be computed with a polynomial time algorithm [35]using the ShortLoop software [34]. Details of the process andits parameters can be found in our previous work [29]. We
denote the resulting set of piecewise linear loops by S ={γ1 , γ2 , . . . , γm }. Each γ ∈ S represents a basic topologicaland geometric attribute corresponding to a hole in the object.
Note that Kd is a rough approximation of the object whichis possibly perceived as a noisy point cloud. Therefore Kd
may contain interior triangles such as in the thin handles ofthe objects shown in Fig. 3.
A clasping target on a loop γ ∈ S consists of a pointp ∈ γ and its approximated tangent p0 . The tangent p0 caneasily be determined from two points on γ close to p.

Fig. 3. Left to right: real size objects used for experiments and the numberof detected loops: Purse (1), Bag (1), Travel Bag (1), Lawn Mower (2),Chair (7). Bottom row: collision model Kd and selected loops used as
object representation in the experiments.

The complete object representation, S, Kd , {Gγ | γ ∈S} , consists of a set of approximated shortest loops, S,of the simplicial complex, Kd , which is used as a collisionmodel and of sets of clasping targets Gγ = pi , p0i | i =1, 2, . . . , kγ where the pi are sampled on γ. Examples forpairs of γ ∈ S and Kd can be inspected in Fig. 3. It canbe seen that a path γ does not necessarily lie exactly on theoriginal surface since the refined triangulation is based on
point-samples and provides only a rough description of thereal object. Additionally, γ may run along the interior edgesof Kd .
B. End Effector Representation
For a given robot hand with l fingers, we select nonclosed piecewise linear curves, C = {α1 , α2 , . . . αl−1 },running from the fingertips through the joints to the tip of arespective opposable “thumb”. For our algorithm to apply, it
is necessary to identify at least one pair of opposable fingers.
For most current robot hands such αi are easily identified.
Fig. 4 outlines two curves for the Schunk SDH hand and four
finger curves for each the iCub, DLR 1 and Armar III hand(red). A hand’s orientation is described by orthogonal unit
vectors vfwd , vswd pointing forward (blue) and sidewards(green), respectively. The vectors are rigidly attached to thehand’s tool center point and allow to describe angles between
the hand pose and any given frame. Fig. 4 depicts the initialhand pre-shapes used in our planning approach.

D. Exploration

Fig. 4. From left to right: the Armar III, DLR 1, iCub and Schunk SDH
hands in clasping pre-shape. Orientation vectors are depicted in blue andgreen. Finger curves (red) run from the fingertips to the tip of the “thumb”.
A virtual chord (orange) completes each finger curve to a virtual loop.

C. Virtual Linking
For sampling-based search, the description of the goalstates—in this case caging grasps—is essential. Utilizing theclosed loops γ ∈ S that describe potentially claspable partsof an object and the finger curves α ∈ C that model thefingers’ shape, it is plausible to consider the Gauss linking
integral Lk (γ, α) to estimate the linking of a loop γ and apair of fingers even if α is not closed. The larger the value of
Lk (γ, α) , the more linked one could consider the fingers
and the loop. However, the Gauss linking integral, when applied to a non-closed curve α neglects the fingertip distances
and two curves with absolute linking close to 1 can still be
untangled in certain cases, even if one of the two curves isa closed loop. Additionally the value of Lk (γ, α) does notnecessarily increase monotonically when the open curve α isincreasingly closed which constitutes an unpleasant property
from the control point of view.
For this reason, we propose a novel linking measurecompliant with the representations stated above and based on
the Gauss linking integral. Connecting the first and the last
node of a non-closed finger curve α ∈ C with a virtual cord
results in a virtual finger loop α̂. Fig. 4 exemplifies the ideaof virtual cords (orange) for different robot hands. If a virtual
finger loop α̂ and a loop γ ∈ S have Lk (γ, α̂) ≥ 1, theloop γ “runs through” the hand. Closing the hand at this state
could result in a caging configuration but the closing processof the hand might be obstructed (see Fig. 8). However,reducing the length of the virtual cord to a minimum byclosing the opposing fingers will cage the closed loop γ. The
following definitions specify this notion mathematically.
Let |α̂| denote the length of a virtual finger loop α̂ and δα
the Euclidean distance of the first and last node of the finger
curve α, measuring the length of the virtual cord connecting
the finger tip and the tip of the “thumb”. We define the virtual
linking of the closed curve γ and non-closed curve α by
αVLk (γ, α) = 1 − |δα̂|Lk (γ, α̂).
(2)The virtual linking of α and γ assumes the value of 0 as longas the loop does not “run through” the fingers. Otherwise it
describes the “closing” of the finger curve taking the value
Lk(γ, α) when the tips of the finger and the “thumb” meet.
If the virtual linking value is continuously increased until thefingertips meet, or until the fingers are obstructed, the handis closed around the object. This makes the virtual linking asuitable projection for task space control.

Alg. 1 describes the basic planning procedure of our TSRRT framework. Starting at a C-space pose qinit ∈ Q,
a tree T in the task space X is build. To maintain thecorrespondence between a C-space configuration q and its
task space mapping x, they are always jointly appended tothe tree. In each iteration, either an undirected random exploration towards xrand ∈ X or goal directed iterative control
is performed. By using task space control and samplingbased C-space collision checking, the tree is ensured tocontain only reachable and collision-free configurations. The
search terminates in the procedure TS-CONNECT-GOAL
when a task space goal has been reached up to a pre-definedtolerance. Thereafter, fingers are closed until contact occurs
and the solution sequence is post-processed in configuration
space using a randomized path pruning technique also usedby [16] to produce a smooth C-space trajectory.
Algorithm 1 BUILD-TS-RRT qinit , γ, Gγ

Require: qinit ∈ Q, γ loop, Gγ positions on loop with tangents
1: xinit ← TS-PROJECTION(qinit )2: T .init(qinit , xinit )3: for k = 1 to K do
4:
if with some probability P then5:
xrand ← RANDOM-TS-SAMPLE() ∈ X
6:TS-EXTEND(T , xrand , γ, Gγ )
7:
else

8:
for all p, p0 ∈ Gγ do
9:xgoal ← (p1 , p2 , p3 , 1.0)T
10:TS-CONNECT-GOAL(T , xgoal , γ, Gγ )11:
end for12:
end if
13: end for

Two different procedures are applied to extend the task
space tree T . TS-EXTEND [17] in Alg. 2 selects the L2 nearest neighbor xnear in task space as a starting point.
Instead of connecting C-space positions, local task spacecontrol at qnear is used to generate a new C-space position
qnew . If the path from qnear to qnew is collision-free, thetree is extended. The goal bias is realized by the procedure
TS-CONNECT-GOAL in Alg. 3, which is proposed here as
the TS-RRT equivalent of a similar function in JT-RRT [5].
Stepwise control is iterated until the task space goal x isreached up to a pre-defined tolerance. TS-CONNECT-GOAL
ends when a collision appears or when the task space distanceto the goal is small enough. In the latter case, the last node,
qnew , and its ancestors define the solution sequence. A node
in T is only selected once for extension towards a certain
goal which has been omitted in Alg. 3 for clarity.
E. Control
The process of clasping an object loop using both one’s
arm and hand conceptually consists of the actions (a) moving
the hand towards some segment of the loop, (b) turning theopened hand towards the loop segment and (c) closing thehand while maintaining the loop segment centered between
the actuated fingers. While the first two actions need to beAlgorithm 2 TS-EXTEND(T , x, γ, G)
Require: x ∈ X , γ loop, G positions on loop with tangents
1: (qnear , xnear ) ← TS-NEAREST-NEIGHBOR(T , x)2: qnew ← TS-CONTROL(qnear , xnear , x, γ, G)
3: xnew ← TS-PROJECTION(qnew )4: if COLLISION-FREE(qnear , qnew ) then5:
T .append(qnear , qnew , xnew )6: end if

Algorithm 3 TS-CONNECT-GOAL(T , x, γ, G)
Require: x ∈ X , γ loop, G positions on loop with tangent
1: (qnear , xnear ) ← TS-NEAREST-NEIGHBOR(T , x)
2: loop3:
qnew ← TS-CONTROL(qnear , xnear , x, γ, G)
4:xnew ← TS-PROJECTION(qnew )5:
if COLLISION-FREE(qnear , qnew ) then6:
T .append(qnear , qnew , xnew )7:
if TS-TOLERANCE(x, xnew ) then8:
TERMINATE-SEARCH(qnew )9:
end if10:(qnear , xnear ) ← (qnew , xnew )
11:
else
12:
return13:
end if
14: end loop

coordinated to position the loop segment between the fingers,
the last action has to be performed only when it is alreadywithin reach of the fingers. Still, the hand’s position needs
to be adjusted while the fingers close to clasp. This is mostimportant for asymmetric hands where the center point of
the fingers shifts notably when closing. Using a TS-RRT,
we can describe each of these three actions by its own task
space and mediate them using priorities and gain functions.
To capture the change of hand shape during different
stages of the closing, action (a) is described by the center
point of the axis-aligned bounding box (AABB) of all finger
curves. The task space projection Πpos : Q → Xpos = R3
is defined by Πpos (q) = pAABB . Action (b) is describedby a task space, Xalign , that considers alignment of theunit vectors vfwd and vswd with the nearest clasping target
pnear , p0near ∈ Gγ :0near |
Πalign : q 7→ |vvswd ·p
,
(3)
·t
fwd

near

pnear −pAABB
where tnear = kp
describes the normalized direcnear −pAABB ktion vector pointing from the hand towards the nearest clasping target. The task space projection satisfies Πalign (q) =T(1, 1) if and only if the hand is pointing towards the target
section of the loop and is aligned with the loop’s local
tangent. The action of closing the fingers around the loop,(c), is described by the average virtual linking of all finger
curves in task space Xvlink :P
1(4)
Πvlink : q 7→ |C|α∈C VLk (γ, α) .

TS-EXTEND can be considered as a randomized search fora good starting configuration from which iterated stepwise
local control can be applied to reach the nearest claspingtarget. Consequently, every extension step should try to alignthe end effector with the nearest clasping target. This implies
that the TS-RRT should only search X = Xpos × Xvlink
randomly, but the goals in Xalign should not be random. A
controller that governs the high-dimensional joint C-space ofarm and hand joints, Q, is described in Alg. 4. The task space
goals cpos and cvlink are extracted from dimensions 1 to 3
and 4 of the given target x ∈ X while calign is always set toT(1, 1) . The use of a sigmoid-shaped gain function results
in a smooth change from controlling position with alignment
as a secondary task to prioritizing hand shape control. To
limit conflicts and to focus control from different task spaces
to either the arm or the hand joints, gain matrices Mhand
and Marm are used. The values are defined such that either
all control for the hand or the arm is diminished and thevariable influence of the joints depending on their kinematic
chain position is accounted for. This controller essentiallyperforms a reaching and then closing motion and is actuallycapable of clasping an unobstructed clasping goal point byitself.
Algorithm 4 TS-CONTROL q0 , x0 , x1 , γ, Gγ

Require: q0 ∈ Q, x0 , x1 ∈ X , γ loop, Gγ positions on loop with
tangents
T
1: cpos ← (x1 )1 , (x1 )2 , (x1 )3
2: upos ← cpos − Πpos (q0 )
3: Jpos ← COMPUTE-JACOBIAN Πpos (.) , q0
T
4: calign ← (1, 1)
5: ualign ← calign − Πalign (q0 , Gγ )
6: Jalign ← COMPUTE-JACOBIAN Πalign (., Gγ ) , q0
7: cvlink ← (x1 )4

8: uvlink ← cvlink − Πvlink (q0 , γ)
9: Jvlink ← COMPUTE-JACOBIANΠ vlink (., γ) , q0++
10: q̇arm ← J+
pos upos + I − Jpos Jpos Jalign ualign11: q̇hand ← J+
vlink uvlink
12: α ←GAIN kupos k
13: q̇ ← αMarm q̇arm +

14:




(1 − α) Mhand q̇hand + I − J+Jq̇armvlink vlink

15: return q0 + LIMIT-STEP-SIZE(∆t q̇)V. E XPERIMENTAL E VALUATION
We consider a KUKA KR5 R850 arm equipped witheither an Armar III, DLR 1, iCub or Schunk SDH hand.
The number of joints for the hands is 10 (Armar III), 12(DLR 1), 15 (iCub), 6 (Schunk SDH) and 6 for the arm.
Unless stated otherwise, the goal tolerance in the task space
only considers virtual linking with a threshold of 0.85–0.65
for the DLR 1 and Schunk SDH hands, 0.8–0.65 for the
Armar III and 0.75–0.65 for the iCub hand—depending on
the shape of the object. Hand representations and finger
curves are depicted in Fig. 4. The five real-sized experimentobjects are displayed in Fig. 3 along with their representation
created from 2000 points sampled on the object’s visible
surface. From the detected shortest loops, only the loop
marked in red is used for clasping. The numbers of equally
spaced clasping targets on the used loops are 9 (Purse), 23(Bag), 17 (Chair), 24 (Lawn Mower) and 10 (Travel Bag).

1.8
1.6
1.4
Max

The TS-RRT is programmed to randomly expand for 20,
50, 100 or 200 cycles before entering the loop in Alg. 1 tospeedup the search—depending on the size, complexity andpose of the object. The value P is set to 0.1. The Simox[36] software is used for robot simulation and object meshmodels are taken from [37], [38].

1.2
1

A. Varying Object Poses

iCub
Armar III
DLR 1
SDH

0.8
0.6

To be viable, a motion and clasp planning approach needs
to handle different robot hands, objects and object poses.
Therefore, in this experiment, we position each of the objects
in three different poses for each robot hand. The number of
pre-expansions, n ∈ {20, 50, 100, 200}, is selected as low
as possible so that a clasping goal is reached within thefirst few attempts to connect. The stability of the resulting
clasping poses is tested in rigid body simulation by trying toseparate the robot and object with 10 separate sequences of
500 random translations and rotations. Each transformation
is pursued until collision. A subset of the 60 final poses
is shown in Fig. 5, a comprehensive chart of all figures
can be found at http://www.csc.kth.se/˜jastork/
iros2013/. Each of the resulting configurations passed thestability test and therefore provides a secure clasping pose.
In all cases, the goal was found within 52 cycles after
the initial phase, while in 37 of the 60 cases the searchterminated less then 10 cycles after the initial phase. The
comparably low amount of cycles for a RRT approach showsthat the proposed controller is suitable for clasping. The threeposes of the Bag and the first two poses of the Travel Bag
turned out to be the simplest clasping problems. In bothcases, many kinematically easily reachable clasping goals
are placed in the robot’s workspace, so that n = 20 and n =50 pre-expansions are sufficient. The Purse and the Chair
object are more difficult to clasp. For the Purse, the loop
is small compared to the DLR 1 and Schunk SDH hands.
Additionally, the number of 9 clasping goals is comparably
small and about half of them are obstructed by the object
shape. All hands but the iCub hand require at least n = 100
pre-expansions to find a secure clasp. While the DLR 1
hand could quickly find clasps on the Chair object withn = 20 and n = 50, all the other hands needed n = 50 ton = 200 pre-expansions. The large fingers of the DLR 1 hand
can easily link the loop without being precisely positioned.
Despite its shape, the Lawn Mower is difficult to clasp. Thiscan be explained by the fact that only a few clasping goals at
the middle of the handle are easily reachable with a tangentperpendicular pose.
The distribution of the minimal and maximal Gauss linking integrals for the finger curves of the individual final poses
is depicted in Fig. 6. In all cases, the maximal Gauss linking
is larger than 0.5 while most values are higher than 0.8. The
minimal Gauss linking ranges from ca. 0.5 to 1.5. The largest
spread of values occurs for the DLR 1 hand. The results showthat a large range of Gauss linking integrals for finger andobject curves corresponds to secure clasps.

0.2 0.4 0.6 0.8

1 1.2 1.4 1.6 1.8
Min

Fig. 6. Evaluation of the absolute values of the Gauss linking integral
between object loops and robot hand for each successful clasping pose. For
each final configuration, the minimal value for the hand’s control curves isplotted against the maximal value.

B. Grasping an Obstructed Loop on a Chair
The advantage of planning is that local obstructions canbe bypassed to reach a goal. To show that our approach iscapable of clasping loops that cannot directly be reachedfrom the initial position, the robot arm is placed over thechair’s seat and a loop under the seat is chosen to be clasped(see Fig. 7). After 606 cycles, the search terminates. The
search tree nodes are displayed in red showing the grasp
center point. Blue markers are used for the solution nodes.
The search tree spans from the initial position to the open
space right of the robot, eventually connecting to a claspinggoal. Poses at 20%, 40%, 60% and 80% of the optimized
solution shown in green are displayed in the right of Fig. 7.
C. The Influence of Virtual Linking Tolerance
In our approach, a pre-defined task space goal tolerance
is used to implicitly define the goal regions. Instead ofincreasing virtual linking, the TS-RRT could be used to only
find configurations with virtual linking larger than 0 and
execute automatic closing of the robot hand to grasp a loop
segment. Here, we show the impact of the goal tolerance forthe minimally accepted virtual linking. Different thresholds
imply that different goal regions are considered. Fig. 8 shows
possible results if a virtual linking of 0.5, 0.65, 0.70 or 0.75
is required to terminate the search. As can been seen clearly,
larger values lead to increasingly tighter clasps. While for 0.5
the clasp might be obstructed, a value of 0.75 makes sure thata high virtual linking can be reached without any collision.
Therefore it is clear that this tolerance has to be adjusted
and that a low tolerance increasingly guarantees better final
linking.

Fig. 8. From left to right: possible final configurations for increasing
values of accepted minimal virtual linking (0.5, 0.65, 0.70, 0.75) for searchtermination. Higher values of virtual linking correspond to more secure
clasps.

Fig. 5. A subset of the final poses generated for four hands and five objects is displayed. The small figures show details of the clasping pose. Each of
the depicted poses is a secure clasping pose in the sense that it passes our pulling and turning test.

Fig. 7. Our approach can clasp loops that are not directly reachable from the robot’s initial configuration. Here, the RRT spans from the initial configuration
to the free space in front of the chair, visualized by the grasp center point in red. The solution’s 30 nodes are shown in blue and the optimized solution isshown in green. The search terminated after 606 cycles and with 248 nodes in the tree.

[7] E. Drumwright and V. Ng-Thow-Hing, “Toward interactive reachingin static environments for humanoid robots,” in IEEE/RSJ IROS, 2006.

Fig. 9. Left to right: a representative grasp generated by the Grasp-RRT
approach, a rare Grasp-RRT grasp remotely similar to a caging grasp anda representative clasping configuration as it is achieved by our approach.

D. Comparison to Grasp-RRTIn this experiment, we investigate whether a dedicatedclasp planner is a necessity to generate clasping configurations. For comparison, the Grasp-RRT approach of [16] isused several times on Purse.
Most of the time, a grasp as displayed on the left of Fig. 9
is generated. The hand does not envelop the handle-part of
Purse and has approached the object from the side or frombelow. In less then 5% of the experiments a grasp somewhereon the handle-part is generated and those grasps rarely
envelop the handle-part. Compared to that, our approachreliably produces clasps as shown on the right of Fig. 9 every
time. Our approach, which uses global topological object
information and which generates caging rather than precision
grasps, hence provides a complementary strategy for grasp
synthesis.
VI. CONCLUSIONS
We have presented an integrated approach to clasp andmotion planning based on the novel concept of virtual linking
which applies to objects with holes. A clasp is a grasp that
falls into the category of caging grasps and which extends
the repertoire of a robot to actions that do not necessarily
immobilize the manipulated object. We have defined multiple
task spaces and employed a priority based control techniquetogether with sampling-based motion planning. We havedemonstrated the viability of our approach in simulation
using several robot hands and objects. In future, we plan
to evaluate our approach on a real robot and would liketo deduce currently predefined parameters using a further
analysis of the objects. Additionally, we would like toincorporate task constraints to select optimal handle-partsand clasping positions.
R EFERENCES[1] S. M. LaValle and J. J. Kuffner, “Randomized kinodynamic planning,”
The International Journal of Robotics Research, 2001.
[2] J. Kuffner, K. Nishiwaki, S. Kagami, M. Inaba, and H. Inoue, “Motion
planning for humanoid robots,” Robotics Research, 2005.
[3] J. Kim and J. P. Ostrowski, “Motion planning a aerial robot using
rapidly-exploring random trees with dynamic constraints,” in IEEE
ICRA, 2003.
[4] D. Bertram, J. Kuffner, R. Dillmann, and T. Asfour, “An integrated
approach to inverse kinematics and path planning for redundant
manipulators,” in IEEE ICRA, 2006.
[5] M. Vande Weghe, D. Ferguson, and S. S. Srinivasa, “Randomized path
planning for redundant manipulators without inverse kinematics,” inIEEE-RAS Humanoids, 2007.
[6] J. J. Kuffner Jr and S. M. LaValle, “RRT-connect: An efficientapproach to single-query path planning,” in IEEE ICRA, 2000.

[8] D. Berenson, S. S. Srinivasa, D. Ferguson, A. Collet, and J. J. Kuffner,“Manipulation planning with workspace goal regions,” in IEEE ICRA,
2009.
[9] N. Vahrenkamp, D. Berenson, T. Asfour, J. Kuffner, and R. Dillmann, “Humanoid motion planning for dual-arm manipulation andre-grasping tasks,” in IEEE/RSJ IROS, 2009.
[10] J. H. Reif, “Complexity of the mover’s problem and generalizations
extended abstract,” in IEEE Conference on Foundations of Computer
Science, 1979.
[11] L. Kavraki and J.-C. Latombe, “Randomized preprocessing of configuration space for path planning: Articulated robots,” in IEEE/RSJ
IROS. IEEE, 1994.
[12] L. E. Kavraki, P. Svestka, J.-C. Latombe, and M. H. Overmars, “Probabilistic roadmaps for path planning in high-dimensional configuration
spaces,” Robotics and Automation, IEEE Transactions on, 1996.
[13] S. M. LaValle, “Rapidly-Exploring Random Trees A New Tool for
Path Planning,” 1998.
[14] T. Siméon, J.-P. Laumond, J. Cortés, and A. Sahbani, “Manipulation
planning with probabilistic roadmaps,” The International Journal of
Robotics Research, vol. 23, no. 7-8, pp. 729–746, 2004.
[15] K. Hauser and J.-C. Latombe, “Integrating task and PRM motion
planning: Dealing with many infeasible motion planning queries,” inWorkshop at ICAPS, 2009.
[16] N. Vahrenkamp, T. Asfour, and R. Dillmann, “Simultaneous Grasp
and Motion Planning: Humanoid Robot ARMAR-III,” Robotics &Automation Magazine, 2012.
[17] A. Shkolnik and R. Tedrake, “Path planning in 1000+ dimensions
using a task-space Voronoi bias,” in IEEE ICRA, 2009.
[18] A. Shkolnik, M. Levashov, I. R. Manchester, and R. Tedrake, “Bounding on rough terrain with the LittleDog robot,” The International
Journal of Robotics Research, 2011.
[19] M. Behnisch, R. Haschke, and M. Gienger, “Task space motion
planning using reactive control,” in IEEE/RSJ IROS, 2010.
[20] M. Behnisch, R. Haschke, H. Ritter, and M. Gienger, “Deformabletrees-exploiting local obstacle avoidance,” in IEEE-RAS Humanoids,
2011.
[21] E. Rimon and A. Blake, “Caging 2D bodies by 1-parameter twofingered gripping systems,” in IEEE ICRA, 1996.
[22] C. Davidson and A. Blake, “Caging planar objects with a three-finger
one-parameter gripper,” in IEEE ICRA, 1998.
[23] P. Pipattanasomporn and A. Sudsang, “Two-finger caging of concave
polygon,” in IEEE ICRA, 2006.
[24] S. Makita and Y. Maeda, “3D multifingered caging: Basic formulationand planning,” in IEEE/RSJ IROS, 2008.
[25] A. Rodriguez and M. Mason, “Two finger caging: squeezing andstretching,” Algorithmic Foundation of Robotics VIII, 2009.
[26] A. Rodriguez, M. Mason, and S. Ferry, “From caging to grasping,”
The International Journal of Robotics Research, 2012.
[27] W. Wan, R. Fukui, M. Shimosaka, T. Sato, and Y. Kuniyoshi, “Grasping by caging: A promising tool to deal with uncertainty,” in IEEE
ICRA, 2012.
[28] R. Diankov, S. Srinivasa, D. Ferguson, and J. Kuffner, “Manipulation
planning with caging grasps.” in IEEE-RAS Humanoids, 2008.
[29] F. T. Pokorny, J. A. Stork, and D. Kragic, “Grasping objects withholes: A topological approach ,” in IEEE ICRA, 2013.
[30] K. Klenin and J. Langowski, “Computation of writhe in modeling ofsupercoiled DNA,” Biopolymers, 2000.
[31] E. L. Ho and T. Komura, “Character Motion Synthesis by Topology
Coordinates,” Comput. Graph. Forum, 2009.
[32] D. Zarubin, V. Ivan, M. Toussaint, T. Komura, and S. Vijayakumar,“Hierarchical Motion Planning in Topological Representations,” in
RSS, 2012.
[33] A. Hatcher, Algebraic topology. Cambridge University Press, 2002.
[34] O. Busaryev, T. Dey, J. Sun, and Y. Wang, “ShortLoop Software for
Computing Loops in a Shortest Homology Basis,” Software, 2010.
[35] T. Dey, J. Sun, and Y. Wang, “Approximating loops in a shortest
homology basis from point data,” in ACM SoCG, 2010.
[36] N. Vahrenkamp, M. Kröhnert, S. Ulbrich, T. Asfour, G. Metta, R. Dillmann, and G. Sandini, “Simox: A Robotics Toolbox for Simulation,
Motion and Grasp Planning,” Intelligent Autonomous Systems, 2013.
[37] Trimble 3D Warehouse, http://sketchup.google.com/3dwarehouse/.
[38] Archive3d.net, http://www.archive3d.net/.