Caging and Path Non-Existence: a Deterministic
Sampling-Based Verification Algorithm
Anastasiia Varava, J. Frederico Carvalho, Florian T. Pokorny, and Danica Kragic

Abstract Caging restricts the mobility of an object without necessarily immobilizing it completely. The object is caged if it cannot move arbitrarily far from its
initial position. Apart from its common applications to grasping and manipulation,
caging can also be considered as a problem dual to motion planning: an object is
caged when it is isolated within a bounded connected component of its configuration space and is disconnected from the rest of the latter. In this paper, we address
the problem of caging and path non-existence verification in 2D and 3D workspaces
by representing a subset of the collision space as a simplicial complex and analyzing the connectivity of its complement. Since configuration spaces of 2D and 3D
rigid objects are three-dimensional and six-dimensional respectively, it is computationally expensive to reconstruct them explicitly. Thus, we represent the object’s
collision space as a union of a finite set of ‘slices’, corresponding to small intervalsof the object’s orientation coordinates. We also discuss possible generalizations of
our approach to higher dimensions.

1 Introduction
A cage is an arrangement of obstacles that restricts the mobility of the object insuch a way that it cannot escape arbitrarily far from its initial position. In robotics,
caging relates to grasping, where it is used either as an alternative to contact-based
grasping or as a pregrasping condition [8, 9, 13, 16, 17, 20]. Apart from that, caging
has applications in multi-agent manipulation [18].
In terms of the object’s configuration space, the object is caged if its configuration
lies in a bounded connected component of its free space. Therefore, a cage and apath can be seen as two opposite concepts: while the existence of a path ensures
A. Varava, J. F. Carvalho, F. T. Pokorny, and D. Kragic
KTH Royal Institute of Technology, Stockholm, Sweden
e-mail: {varava,jfpbdc,fpokorny,dani}@kth.se1

2

Anastasiia Varava, J. Frederico Carvalho, Florian T. Pokorny, and Danica Kragic

reachability, the presence of a cage means that the object’s mobility is restricted tothe current bounded connected component, see Fig. 1.

Fig. 1 The collision space of the object is illustrated in grey, the two connect components of thefree space are denoted by Q1 and Q2 . We see that the configuration c1 is caged as Q1 is bounded.
Having Q1 and Q2 we can also say that any configuration c2 ∈ Q2 cannot be reached from c1 .

The problem of proving path non-existence can be applied to path planning. Mostof the sampling-based planning algorithms perform iterative random sampling andincrementally build paths between configurations. Usually these algorithms are either probabilistically complete [1] or resolution complete [6], but they are not guaranteed to find a path in finite time if one exists. Therefore, one of the practicalquestions is how long the planner should search for a path before concluding that it
does not exists. Instead of using stopping heuristics, one could avoid this problem
by proving path non-existence. The latter is a challenging problem as it requires retrieving information about the connectivity of the entire configuration space rather
than finding a single path which has been addressed in [2, 10, 22].
Essentially, computing the connected components of the free space would solve
two problems: (i) proving or disproving caging and (ii) checking if the object can be
moved from one configuration to another. In this paper, we address both problems
for rigid objects in 2D and 3D workspaces.

2 Related WorkThe notion of a planar cage was initially introduced by Kuperberg in 1990 [5] asa set of n coplanar points lying in the complement of a polygon and preventing itfrom escaping arbitrarily far from its initial position. This problem can be directly
applied to robotics if we represent the fingertips or mobile robots as points, andthe object as a polygon. Point-based caging of polygons and polytopes has received
a lot of attention in the context of grasping and multi-agent manipulation. Rimon
and Blake [14] proposed an algorithm computing a set of caging configurations ofa two-fingered hand for planar non-convex objects. In [11], Pipattanasomporn andSudsang proposed an algorithm reporting all two-finger caging sets for a given concave polygon. Vahedi and van der Stappen extended this result in [21] by proposing
an algorithm that returns all caging placements of a third finger given a polygonal object and a placement of two other fingers. A similar approach has also beenCaging and Path Non-Existence Verification3

adopted for caging 3D objects. Pipattanasomporn and Sudsang [12] proposed analgorithm for computing all two-finger cages for non-convex polytopes. Rodriguez
et al. introduced a notion of a pregrasping cage in their work [15]. Starting from apregrasping cage, a manipulator can move to a form closure grasp without breaking
the cage.
In the above mentioned works fingertips are represented as points or spheres,and more complex shapes of caging tools usually are not taken into account. Alternatively, one can derive sufficient caging conditions based on specific geometric andtopological features of the object and the caging tools. In particular, Stork et al. proposed an approach towards caging objects with ‘holes’ in their works [13, 16, 17].
Subsequently, Varava et al. considered loop-based caging of objects with narrow
parts – so-called ‘necks’ and ‘double forks’ [20]. Sufficient conditions for caging
objects of particular shapes have also been studied in [8, 9].
The advantage of the above mentioned approaches is that they utilize global geometric properties of the objects. Therefore, one does not have to explicitly reconstruct the free space of the object, which is a challenging task due to its high dimensionality. However, these methods require the objects to exhibit particular shapefeatures, which prevents us from applying them to caging objects of arbitrary shape.
A more general approach is to directly analyze the topological properties of theconfiguration space of the object.
In this context, caging is closely related to the problem of proving path nonexistence. Motivated by motion planning applications, in their pioneering work [2]Basch et al. propose an approach towards proving that two configurations are disconnected because the object is ‘too big’ or ‘too long’ to pass through a ‘gate’
between them. The authors apply their algorithm to several simple objects and integrate it with a PRM motion planner. In [22], Zhang et al. use approximate cell decomposition and prove path non-existence. They decompose a configuration space
into a set of cells and for each cell decide if it lies in the collision space. In [10]McCarthy et al. propose a somewhat similar approach. There, they randomly sample the configuration space and reconstruct its approximation as an alpha complex.
They later use it to check the connectivity between pairs of configurations.
In this paper, we also aim to study the connectivity of the free space of the object.
However, unlike [10], we do not construct the collision space directly. Instead we
decompose it into a finite set of lower dimensional ‘slices’. We compute the connected components of the latter and analyze the possible transitions between different slices. This allows us to overcome the dimensionality problem without losing
any necessary information about the topology of the configuration space.

3 An Overview of Our Method
In this section we state the problem we are going to address, introduce the necessary
notation, and provide an intuitive description of our approach while avoiding going
into technical details.

4

Anastasiia Varava, J. Frederico Carvalho, Florian T. Pokorny, and Danica Kragic

3.1 Problem Formulation
Let us start with the necessary definitions. A rigid object O is a compact connected
subset of Rd of the form O = cl(U), where d ∈ {2, 3}, U is an open subset of Rd ,and cl(U) denotes the closure of U. A set of obstacles S is a compact subset of
Rd with the same property: it can be represented as a closure of an open subset
of Rd . Let C = SE(d) denote the configuration space of the object. We define its
collision space as the set of the objects configurations in which the object penetrates the obstacles: C col = {c ∈ C | int(c(O)) ∩ int(S ) 6= 0},/ where c(O) denotes
the object in a configuration c. Note that this definition allows the object to be incontact with the obstacles. The free space C f ree is the complement of the collision
space: C f ree = C − C col . We assume that both the obstacles and the object can beapproximated as unions of balls lying in their interior, S = {BR1 (X1 ), .., BRn (Xn )}and O = {Or1 (Y1 ), .., Orm (Ym )} of radii R1 , .., Rn and r1 , .., rm respectively1 .
Our goal is to provide an algorithm which:• takes the approximations of the object and the obstacles as input;• constructs an approximation of the collision space;• for a given configuration, checks whether it is caged; returns “True” if it is guaranteed to be caged, and “Undefined” otherwise;• for a given pair of configurations, checks whether there is a collision-free path
between them; returns “False” if they are guaranteed to be disconnected, and“Undefined” otherwise.

3.2 Our Approach: The General Idea
To address our problem we compute an approximation of C col , and then analyse
the connectivity of its complement – the free space. Let us think of C as a product
C = Rd × SO(d) where d ∈ {2, 3}.
Note that as long as the object is caged by a subset of the collision space, it isguaranteed to be caged by the entire collision space. Therefore, to solve our problem, it is not necessary to reconstruct the exact collision space C col . Instead, it isenough to approximate it by its sufficiently large subset Cacol ⊂ C col .
Instead of reconstructing C col directly, we work with its projections C col (O φ )onto Rd corresponding to fixed orientation φ of the object in SO(d).
Since the number of all possible orientation values is infinite, we divide C into a
finite number of slices:
Definition 1 A slice of the configuration space C is a subset of C defined as follows:
SlU = Rd ×U,
1 From now on, we will always use this approximation in this paper, and hence we write theapproximations as equalities to simplify the notation.

Caging and Path Non-Existence Verification

5

where U is an open subset of SO(d).
Similarly, we denote a slice of the collision (free) space by SlUcol (SlUf ree ). For eachslice we build an approximation of the collision space SlUcol , so that the union of allof them represents the entire collision space:
C col =[

SlUcolU⊂SO(d)Let us now discuss how we construct the slices. Note that to show that the object
collides with an obstacle it is enough to show that some part of the object is incollision. We define an ε−core of the object as follows:
Definition 2 An ε-core of an object O is a subset Oε of the object such that any
point of Oε lies at least at a distance2 ε from the boundary of O:
Oε = {p ∈ O|d(p, ∂ O) ≥ ε}
Let O φ and Oε denote an object O and its ε-core with a fixed orientation φ ∈SO(d), respectively. Note that the collision space of an object with a fixed rotation
is a subset of Rd .
φFig. 2 An ε-core Oε of the object O φ is depicted in grey; it is contained within the rotated objectOεθ .
φ

In Sec 4, we show that for an object O φ and its ε-core Oε with a fixed orientation
φ we can find an open neighbourhood U(ε, φ ) ⊂ SO(d) of φ such that for anyφθ ∈ U(ε, φ ) the ε-core Oε is fully contained within a slightly rotated object O θ ,see Fig. 2. Since SO(d) is compact, we choose a finite set of rotations {φ1 , .., φs }so that the corresponding family of sets U(ε, φi ) is a finite open cover of SO(d):S
SO(d) = i∈{1,..,s} U(ε, φi ).
φNow note that if Oε ⊂ O θ , then C col (Oε ) ⊂ C col (O θ ). Thus, we can approxicol
mate the collision space of a slice by aSlU(ε,φ) defined as follows:
φ

φ

col
col
col
SlU(ε,φ(Oε ) ×U(ε, φ ),) ≈ aSlU(ε,φ ) = C
φ

and then C col ≈ Cacol =coli∈{1,..,s} aSlU(ε,φi ) .

S

2 By distance here we mean Euclidean distance in Rd

6

Anastasiia Varava, J. Frederico Carvalho, Florian T. Pokorny, and Danica Kragic

Given an object O φ and an ε > 0, we compute the corresponding ε-core Oε .
φ
We then compute an approximation of the collision space for Oε . Recall that theset of obstacles is represented as a set of balls S = {BR1 (X1 ), .., BRn (Xn )} and O ={Or1 (Y1 ), .., Orm (Ym )}. The ε-core can be represented as a set of balls with centres
φat the same points and smaller radii: Oε = {Or1 −ε (Y1 ), .., Orm −ε (Ym )}.
However, to understand the connectivity of the free space it is not enough toconsider different orientation intervals separately. Consider an example, see Fig.3.
Here we have two slices of the free space corresponding to two orientation intervals U1 ∩ U2 6= 0.
/ Both slices have 3 connected components: Q1 , Q2 , Q3 , and
Q01 , Q02 , Q03 respectively. The object can move between the connected components
Qi and Q0j from different slices if (i) the corresponding orientation intervals have anon-empty intersection and (ii) Qi and Q j have non-empty intersection in Rd .
φFig. 3 Two approximations of the collision space corresponding to the non-disjoint orientationintervals U1 and U2 are depicted in grey. The free space is depicted in white. The arrows between
different connected components indicate the possible transitions between slices.

Therefore, the object is contained within a bounded connected component of
the free space if (i) for the corresponding slice SlU of the configuration space it iscontained within a bounded connected component of C f ree , and (ii) there is no path
between slices leading the object to an unbounded connected component of C f ree .

4 Methodology
In this section, we describe the technical details of our approach. We start with thederivation of the possible slice size depending on the size of the chosen ε-core of
the object.

4.1 The Object Core and The Size of The Slices
Consider an ε-core of the object in a given orientation for a fixed ε, and let us fix its
orientation φ ∈ SO(d). We now want to explicitly find an open set U(ε, φ ) ⊂ SO(d)φsuch that for any θ ∈ U(ε, φ ) the ε-core Oε is fully contained within any O θ . We

Caging and Path Non-Existence Verification

7

represent a motion in SO(d) as a rotation matrix R. First, we make the following
observation:
Observation 1 Consider an object O φ and an ε-core Oε inside it. Let us fix theφφorientation φ ∈ SO(d) of Oε , and apply a rotation R to O φ . Then Oε is fully contained inside the rotated object O Rφ if the displacement ||Ry − y||2 of a point y after
applying the rotation is smaller than ε for any point y ∈ O φ .
φ

The proof is trivial, as by construction any point of Oε is separated at least at aφdistance ε from the boundary of O φ . Thus, if Oε is not lying inside of O Rφ , thenthere exists a point y ∈ O φ such that the distance from it to its image Ry in O Rφ is
not smaller than ε, ||Ry − y||2 ≥ ε.
Note that in fact the form of U(ε, φ ) does not depend on φ , and U(ε, Rφ ) can beobtained by rotating the former: U(ε, Rφ ) = RU(ε, φ ) for any φ ∈ SO(d) and any
rotation R.
We now want to estimate the maximum displacement ||Ry−y||2 of a point y ∈ Rd
after applying a rotation R. Let I be the identity matrix. We call R − I a displacement
matrix associated to the rotation R. To estimate ||(R−I)y||2 , we compute the spectral
norm of the displacement matrix:φ||R − I||2 =supy∈R3 , ||y||2 6=0||(R − I)y||2,||y||2

which gives us an upper bound for the displacement:∀y ∈ O φ : ||R − I||2 max (||x||2 ) ≥ ||(R − I)y||2x∈O φ

4.1.1 2D Workspace
Consider a two-dimensional workspace. In this case, the configuration space (andhence the collision space) of the object is a three-dimensional subset of SE(2) =R2 × SO(2), where each configuration is described by a position of the object x ∈ R2
and its orientation φ ∈ SO(2).
In this case rotation matrix can be written ascos(θ ) − sin(θ )Rθ =,sin(θ ) cos(θ )Recall that the spectral norm of R − I is defined as the square root of the maximum eigenvalue of (R − I)T (R − I).
This matrix has a single single eigenvalue of algebraic multiplicity two:λ = (cos(θ ) − 1)2 + sin2 (θ ) = 2 − 2 cos(θ ) = 4 sin2 (θ /2),
which gives us an upper bound for the maximum displacement of y after applying
a rotation Rθ :

8Anastasiia Varava, J. Frederico Carvalho, Florian T. Pokorny, and Danica Kragic||Rθ y − y||2 ≤ 2| sin(θ /2)| · ||y||2

4.1.2 3D Workspace
We represent motions in SO(3) as a composition of the pitch, yaw and roll rotation
matrices:







cos(φ ) 0 sin(φ )cos(σ ) − sin(σ ) 0
1 0
0
1 0  , Rσ =  sin(σ ) cos(σ ) 0 , Rθ = 0 cos(θ ) − sin(θ )
Rφ =  0− sin(φ ) 0 cos(φ )0010 sin(θ ) cos(θ )As before, we want to estimate the displacement incurred by applying a given rotation. Consider a fixed triplet (θ , σ , φ ) we define R(θ ,σ ,φ ) = Rθ · Rσ · Rφ , and once
again compute the norm of the displacement matrix as the square root of the maximum eigenvalue of (R(θ ,σ ,φ ) − I)T (R(θ ,σ ,φ ) − I).
Direct computations show that this matrix has a single non-zero eigenvalue ofalgebraic multiplicity two:
λ = 3 − cos(σ ) cos(φ ) − cos(θ ) cos(φ ) − cos(σ ) cos(θ ) − sin(σ ) sin(φ ) sin(θ )We can simplify this expression by rewriting the products of trigonometric functions as their sums and get:cos(σ − φ ) cos(σ + φ ) cos(σ − θ ) cos(σ + θ ) cos(θ − φ )
−
−
−
−
2
2
2
2
2
cos(θ + φ ) sin(σ + θ − φ ) sin(σ − θ + φ ) sin(σ + θ + φ ) sin(σ − θ − φ )
−
−
+
+
−
2
4
4
4
4

λ = 3−For simplicity, instead of treating our parameters θ , σ , and φ separately, we
consider those triples which lie inside of a ball Br (0) of radius r > 0.
Now for a fixed r > 0 we observe that for any (θ , σ , φ ) ∈ Br (0), each of thearguments σ ±θ , σ ±φ , θ ±φ , σ ±φ ±θ is contained in [−r, r]. Indeed, for examplep|s + t + p| ≤ |s| + |t| + |p| ≤ s2 + t 2 + p2 ≤ r,
and the same follows for any other argument from the list. This means that for
any r in the interval [−π/2, π/2] we have:cos(σ + θ ), cos(σ − θ ), cos(σ + p), cos(σ − φ ), cos(θ + φ ), cos(θ − φ ) ≥ cos(r)sin(σ + φ + θ ), sin(σ − θ − φ ) ≥ sin(−r) = − sin(r)sin(σ + θ − φ ), sin(σ − θ + φ ) ≥ − sin(r),And henceλ ≤ 3 − 3 cos(r) + sin(r)Caging and Path Non-Existence Verification

9

Furthermore, since this is true for every l ∈ [0, r] we getp||R(θ ,σ ,φ ) − I||2 ≤ 3 − 3 cos(r) + sin(r)∀(θ , σ , φ ) ∈ Br (0),and so the maximum displacement of y after applying a rotation R(θ ,σ ,φ ) can be
estimated as follows:p||(R(θ ,σ ,φ ) − I)y||2 ≤ 3 − 3 cos(r) + sin(r) · ||y||2
Fig. 4 illustrates how the displacement grows with the radius r.

Fig. 4 Graph of the maximum displacement estimate for a ball of radius r

4.2 Collision Space Reconstruction
Let us now discuss how we construct the collision space for each slice. Recall
that both the object and the
obstacles are represented as unions of balls, S =
S
S
B
(X
)
and
O
=
i∈{1,..,n} Ri ii∈{1,..,m} Ori (Yi ) of radii R1 , .., Rn and r1 , .., rm respectively. Let the frame of the object be centred in its centroid G, GYi denote the position vector of the center of each ball, i ∈ {1..m}, and T−GYi denote a translation to avector −GYi .
The object represented as a union of balls collides with the obstacles if at least
one of the balls is in collision, so the collision space of the object is a union of thecollision spaces of the balls shifted with respect to the position of the balls centers:
C col (Oε ) =φ[i∈{1..m}

T−GYi (C col (Ori −ε ))10Anastasiia Varava, J. Frederico Carvalho, Florian T. Pokorny, and Danica Kragic

Note also that a ball collides with the obstacles when the distance from the obstacles to its center is not greater than the radius of the ball, so the collision space ofa single ball of radius ri − ε can be expressed as follows:
C col (Ori −ε ) = {x ∈ Rd | d(x, S) ≤ ri − ε} =[BR j +ri −ε (X j )j∈{1..n}So, the collision space of the ε-core can be written as
C col (Oε ) =[φ

T−GYi (BR j +ri −ε (X j ))i∈{1..m}, j∈{1..n}Algorithm 1: Construct-Slice
Data: An ε-core Oε = {Or1 −ε (Y1 ), .., Orm −ε (Ym )}, a set of obstacles
S = {BR1 (X1 ), .., BRn (Xn )}φResult: A union of balls representing the collision space C col (Oε )φ
1 GYi ← Centroid(Oε )col (O φ ) ← 0
2 C/
ε3 foreach i ∈ {1..m}, j ∈ {1..n} doφφ4
C col (Oε ) ← C col (Oε ) ∪ T−GYi (BR j +ri −ε (X j ))5 end
col (O φ )6 return Cεφ

The construction of the collision space of the ε-core of the object is straightforφ
ward, see Alg. 1. Recall now that for a certain U(ε, φ ) the collision space C col (Oε )colθ
is contained within C (O ) for each θ ∈ U(ε, φ ), which gives us an approximaφtion of the slice of C col as C col (Oε ) ×U(ε, φ ).

4.3 Gluing The Slices Together
Once we have computed the collision space for each slice, we proceed by finding
the connected components of the free space of each of them. As a result, we wantto get s a d−dimensional simplicial complexes approximating the free space foreach of the slices. For that, we use the approach proposed by McCarthy et al. in
φ[10]. Since C col (Oε ) is a union of balls, it is convenient to represent it as an alphacomplex – a discrete representation of the space. Alpha complexes are simplicial
complexes which are particularly convenient for approximating unions of balls, as
by the nerve theorem they preserve the homotopy type (and hence the necessary
topological properties) of the corresponding union of balls. They have been used
to approximate configuration spaces, for instance, in [7, 10]. We refer the reader to[3] for a complete and self-contained treatment of alpha complexes and the theory
behind them. To approximate the free space, we compute a regular triangulation,

Caging and Path Non-Existence Verification

11

Algorithm 2: Glue-Slices
Data: A set of slices Cacol = {aSlUcol1 , .., aSlUcols }; their connected components{{C11 , ..,Cn11 }, .., {C1s , ..,Cns s }}Result: A connectivity graph G
1 G ←0/11ss
2 AddVertices(G , {C1 , ..,Cn , ..,C1 , ..,Cn }
s
1
col
col
3 foreach aSlU ∈ Ca
doi
4foreach aSlUcolj ∈ Neighbours(aSlUcoli ) do
5
foreach Cki ∈ Components(aSlUcoli ) do
6
foreach Clj ∈ Components(aSlUcolj ) do
if Cki ∩Clj 6= 0/ then
AddEdge(G , (Cki ,Clj )
end

7
8
9

end

10
11

end

end
end
14 return G
1213which is a convex superset of the computed alpha complex, see [4]. We mark tetrahedra in the regular triangulation as “free” or “in collision”, and compute a set ofconnected components of the free space. We then mark the connected components
as “bounded” and “unbounded”. Note that since both the object and the set of obstacles are bounded, the collision space is also bounded, and therefore the free spacealways has one unbounded connected component. We represent it by artificiallyadding an “infinite” tetrahedron.
Now when we have an approximation of the collision space as a union of slices
Cacol =[aSlUcoli

Ui ,i∈{1,..,s}and an alpha complex representing each slice aSlUcol , we can study the relationship between slices. More precisely, we would like to build a connectivity graph G ,where the vertices represent the connected components {C1i , ..,Cni i } of each slice Ui
for i ∈ {1, ..s}, see Alg. 2.
Two vertices representing components C p ⊂ aSlUcoli and Cq ⊂ aSlUcolj , i 6= j, areconnected by an edge if a direct transition between them is possible. That is, if (i)the sets Ui and U j overlap, Ui ∩U j 6= 0,/ and (ii) the respective projections of the connected components to Rd intersect: PrRd (Cq ) ∩ PrRd (C p ) 6= 0,/ where the projection
operator is defined as PrRd (x, φ ) = x. So, for each slice aSlUcoli , we consider its neighbours – i.e., slices aSlUcolj such that Ui ∩ U j 6= 0,/ and check whether the connectedcomponents of the free space of these pairs of slices (represented as d-dimensional
simplicial complexes) overlap.

12Anastasiia Varava, J. Frederico Carvalho, Florian T. Pokorny, and Danica Kragic

Once the connectivity graph is constructed, we can easily check if there is no path
between two given configurations, and if a given configuration is caged. We run a
depth-first search in G to compute its connected components, and then given two
configurations cs and cg we simply check if the corresponding vertices of G belong
to different connected components. If that is the case, there is no path between cs
and cg .
Algorithm 3: Query-Connectivity
Data: Two configurations, ci and c j ; a set of slices Cacol = {aSlUcol1 , .., aSlUcols };their connected components {{C11 , ..,Cn11 }, .., {C1s , ..,Cns s }}; theconnectivity graph G
Result: Boolean, path existencecolcol
1 aSli ← Slice-Containing-Configuration(ci , {aSlU , .., aSlU })
s
1
col
col
2 aSl j ← Slice-Containing-Configuration(c j , {aSlU , .., aSlU })s
1ii
3 Ci ← Component-Containing-Configuration(ci , aSli , {C1 , ..,Cn })1jj
4 C j ← Component-Containing-Configuration(c j , aSl j , {C1 , ..,Cn1 })5 if Are-Connected(Ci , C j , G ) then6
return Undefined
7 end
8 else9
return False
10 endFor caging, we need to check whether we can reach an unbounded connectedcomponent starting from a given configuration, see Alg. 4. For each connected component of each of the slices, we know if it is bounded or not, each vertex of G has acorresponding mark. To prove caging, we check if the vertex of G corresponding toa certain configuration c is connected to an ”unbounded” vertex.
Algorithm 4: Check-Caging
Data: A configuration ci ; a set of slices Cacol = {aSlUcol1 , .., aSlUcols }; theirconnected components {{C11 , ..,Cn11 }, .., {C1s , ..,Cns s }}; the connectivity
graph G
Result: Boolean, the configuration is cagedcolcol
1 aSli ← Slice-Containing-Configuration(ci , {aSlU , .., aSlU })s
1ii
2 Ci ← Component-Containing-Configuration(ci , aSli , {C1 , ..,Cn })1
3 if Connected-To-Unbounded(C, G ) then4
return Undefined
5 end
6 else7
return True
8 end

Caging and Path Non-Existence Verification

13

5 Paths Construction
In this section, we discuss how our approximation of the configuration space can beused for path planning. Modern path planning algorithms, such as PRM, RRT, andtheir modifications, are guaranteed to find a path if one exists. However, they cannot prove that two configurations are disconnected. On the contrary, our approachis not guaranteed to find a path if one exists, as our approximation of the collision
space is just a subset of the actual collision space. However, we can prove path
non-existence. Thus, path planning algorithms and our method can be considered ascomplementary techniques and can be run in parallel to solve path planning problem more efficiently. Furthermore, our approximation of the collision space can beused to construct path candidates which are not guaranteed to be collision-free, but
can be used as initial approximations and to be further improved using some path
optimization technique, such as CHOMP [19]. This is especially useful in clutteredenvironments, when we need to find a path through a narrow passage. Our approach
can be seen as a continuation of the pioneering work by Basch et al., [2].
Since our approximation of the collision space is not complete, we are limited toa certain class of paths which we can construct. We call them decomposable paths:– collision-free paths that can be decomposed into finitely many motions in SO(d)followed by a motion in Rd . Formally, we define it as follows:
Definition 3 A path γ : [0, 1] → SO(d) is decomposable if there exists a family ofpaths γ1 , γ10 , .., γn , γn0 such that PrRd (Im(γi )) and PrSOd (Im(γi0 )) are constant paths in
Rd and SO(d) respectively3 , and γ = γ1 ∗ γ10 ∗ .. ∗ γn ∗ γn0 , where by γi ∗ γ j we mean acomposition of paths γi and γ j .
However, this class is nevertheless useful as we can show that if there exists
some collision-free path in the free space, our approximation of the configuration
space allows us to construct a decomposable approximation. Namely, instead of
considering the original object, we construct a decomposable path for its ε-core,
and we show that we can always do that providing there is a collision-free path:Lemma 1 If two configurations c and c0 of the original object O are connected,
then there is a decomposable path between the respective configurations of its εcore Oε .

Fig. 5 The arcs (pi−1 , pi and (pi , pi+1 of the projection π are completely contained within a ball
ω of radius ε/2d3 Here Pr (.) and Pr
SO(d) (.) denote the projections to R and SO(d) respectively, and Im(γ)
Rddenotes the image of γ in SO(d).

14Anastasiia Varava, J. Frederico Carvalho, Florian T. Pokorny, and Danica Kragic

Proof. If there exists a decomposable path between c and c0 for the original object O, then the same path is valid for Oε ⊂ O. Otherwise, let us pick any nondecomposable path γ between c and c0 . Note that along this path Oε is always
at least at a distance ε from the obstacles. Let us now explicitly decompose
γ by choosing waypoints c = (p0 , r0 ), . . . , (pn , rn ) = c0 from γ and constructing
γ 0 = {(p0 , r0 ), (p1 , r0 ), (p1 , r1 ), . . . , (pn−1 , rn−1 ), (pn , rn−1 ), (pn , rn )}, where pi are
the coordinates of the Rd component of SE(d) and ri are the coordinates of theSO(d) component. The resulting decomposition will consist of alternating puretranslation and pure rotation paths sending the point (pi , ri ) to (pi+1 , ri ) througha translation-only segment and then to (pi+1 , ri+1 ) through a rotation-only segment.
Assume the waypoints c = (p0 , r0 ), . . . , (pi , ri ) are already chosen. Let p =PrRd (γ) be the projection of γ onto Rd . We choose the next point pi+1 as the second endpoint of the longest arc (pi , y) of the path π which is fully contained within
an ε/2-ball ω centered at pi (or as the Rd -component of c0 , in case the part of πconnecting it to pi lies inside ω), and make sure that pi+1 does not coincide withpi−1 .
Let us show that a pure translation of Oε from (pi , ri ) to (pi+1 , ri ) is collisionfree. Since π is a projection of a collision-free path, O in configuration (pi , ri ) does
not collide with obstacles. Then any point of Oε in (pi , ri ) is separated at least at a
distance ε from the obstacles. Pure translation to (pi+1 , ri ) will move each point ofOε at distance ε/2 < ε, so it will not collide with the obstacles.
Now consider a pure rotation of Oε from (pi+1 , ri ) to (pi+1 , ri+1 ). Assume that
there is some r0 ∈ [ri , ri+1 ] such that Oε in configuration (pi+1 , r0 ) collides withobstacles. Given the continuity of the path between (pi , ri ) and (pi+1 , ri+1 ), there issome configuration (p0 , r0 ) such that p0 ∈ [pi , pi+1 ]. Since pi+1 is chosen in a waythat the arc (pi , pi+1 ) lies inside ω, p0 also lies inside ω. Then the distance between
p0 and pi+1 is less than ε. Therefore, since (p0 , r0 ) is collision-free for the original
object O, (pi+1 , r0 ) is collision-free for Oε , which contradicts our assumption.
Finally, we note that using Lemma 1 we can extend Algorithm 3 to provide anapproximate path between two configurations (p, r) and (p0 , r0 ) if one exists. To
do this, we can have the algorithm output a sequence of pairs (C1 , S1 ), . . . , (Cn , Sn )which comprise the path in the connectivity graph G , where the Ci are the components and the Si . To produce a path we start from the configuration (p, r) ∈ C1 × S1
and use a local planner to find a path to some point (p2 , r) ∈ (C1 ∩C2 ) × S1 and thenwe rotate this configuration to (p2 , r2 ) where r2 ∈ S2 . We proceed in this manner
until we reach (pn , rn ) ∈ Cn × Sn , since by construction (p0 , r0 ) ∈ Cn × Sn , we canuse a local planner to find a path from (pn , rn ) to (p0 , rn ) and finally rotate the final
configuration to (p0 , r0 ).

6 Discussion and Possible Extensions
In this paper, we propose an approach towards proving caging and path nonexistence for rigid objects in 2D and 3D workspaces. We compute an approximation

Caging and Path Non-Existence Verification

15of the collision space of the object, represent it as a collection of lower dimensional
projections, and analyze the connectivity of the free space of the object. We also
discuss how our approach can be generalized to arbitrary configuration spaces.
Our approach can be potentially extended to a broader class of configuration
spaces, for example, in order to prove path non-existence for an articulated robot.
Indeed, as long as the configuration space can be represented as a product C =C1 × C2 , whereC1 ⊂ Rd and C2 is a compact set, we can construct a finite openS
cover C2 = Ui ⊂C2 Ui , and for each slice Sli = C1 × Ui construct collision spaceapproximations using an ε-core of the robot.

References[1] Barraquand, J., Kavraki, L., Latombe, J.-C., Motwani, R., Li, T.-Y., Raghavan,P.: A random sampling scheme for path planning. In: The International Journalof Robotics Research, 16(6), pp. 759774 (1997).
[2] Basch, J., Guibas, L. J., Hsu, D., Nguyen, A. T.: Disconnection proofs for motion planning. In: IEEE International Conference on Robotics and Automation(2001), pp. 1765-1772.
[3] Edelsbrunner, H., Harer, J.: Computational topology: an introduction. American Mathematical Soc., 2010.
[4] Edelsbrunner, H.: Weighted alpha shapes. University of Illinois at UrbanaChampaign, Department of Computer Science, 1992.
[5] Kuperberg, W.: Problems on polytopes and convex sets. In: DIMACS Workshop on polytopes (1990), pp. 584-589.
[6] Latombe, J.-C.: Robot Motion Planning. Norwell, MA, USA: Kluwer Academic Publishers (1991).
[7] Mahler, J., Pokorny, F. T., McCarthy, Z., van der Stappen, A. F., Goldberg,
K.: Energy-bounded caging: Formal definition and 2-D energy lower boundalgorithm based on weighted alpha shapes. In: IEEE Robotics and Automation
Letters, 1(1), pp.508-515 (2016).
[8] Makita, S., Maeda, Y.: 3D multifingered caging: Basic formulation and planning. In: IEEE Intelligent Robots and Systems (2008), pp. 2697–2702.
[9] Makita, S., Okita, K., Maeda, Y.: 3D two-fingered caging for two types of objects: sufficient conditions and planning. In: International Journal of Mechatronics and Automation, 3(4), pp. 263-277 (2013)[10] McCarthy, Z., Bretl, T., Hutchinson, S.: Proving path non-existence using
sampling and alpha shapes. In: IEEE International Conference on Robotics
and Automation (2012), pp. 2563-2569.
[11] Pipattanasomporn, P., Sudsang, A.: Two-finger caging of concave polygon.
In: IEEE International Conference on Robotics and Automation (2006), pp.
2137–2142.
[12] Pipattanasomporn, P., Sudsang, A.: Two-finger caging of nonconvex polytopes. In: IEEE Transactions on Robotics, 27(2), pp. 324-333 (2011).

16

Anastasiia Varava, J. Frederico Carvalho, Florian T. Pokorny, and Danica Kragic[13] Pokorny, F. T., Stork, J. A., Kragic, D.: Grasping objects with holes: A topological approach. In: IEEE International Conference on Robotics and Automation (2013), pp. 1100–1107.
[14] Rimon, E., Blake, A.: Caging planar bodies by one-parameter two-fingered
gripping systems. In: The International Journal of Robotics Research, 18(3),pp. 299–318 (1999).
[15] Rodriguez, A., Mason, M. T., Ferry, S.: From caging to grasping. In: The
International Journal of Robotics Research, 31(7), pp. 886-900 (2012).
[16] Stork, J. A., Pokorny, F. T., Kragic, D.: Integrated Motion and Clasp Planningwith Virtual Linking. In: IEEE/RSJ International Conference on Intelligent
Robots and Systems (2013), pp. 3007-3014.
[17] Stork, J. A., Pokorny, F. T., Kragic, D.: A Topology-based Object Representation for Clasping, Latching and Hooking. In: IEEE-RAS International Conference on Humanoid Robots (2013), pp. 138-145.
[18] Pereira, G. A. S., Campos, M.F.M., Kumar, V.: Decentralized algorithms for
multi-robot manipulation via caging. In: The International Journal of Robotics
Research 23(7-8), pp. 783 – 795 (2004).
[19] Ratliff, N., Zucker, M., Bagnell, J. A., Srinivasa, S.: CHOMP: Gradient optimization techniques for efficient motion planning. In: IEEE International
Conference on Robotics and Automation (2009), pp. 489-494.
[20] Varava, A., Kragic, D., Pokorny, F. T.: Caging Grasps of Rigid and Partially Deformable 3-D Objects With Double Fork and Neck Features. In IEEE
Transactions on Robotics, 32(6), pp. 1479-1497 (2016).
[21] Vahedi, M., van der Stappen, A. F.: Caging polygons with two and three fingers. In: The International Journal of Robotics Research, 27(11-12) pp. 1308–
1324 (2008).
[22] Zhang, L., Young, J. K., Manocha, D.: Efficient cell labelling and path nonexistence computation using C-obstacle query. In: The International Journalof Robotics Research, 27(11-12), pp. 1246-1257 (2008).