Free Space of Rigid Objects:
Caging, Path Non-Existence, and Narrow
Passage Detection
Anastasiia Varava? , J. Frederico Carvalho? , Florian T. Pokorny,
Danica Kragic
KTH Royal Institute of Technology{varava,jfpbdc,fpokorny,dani}@kth.se
Abstract. In this paper, we present an approach towards approximating configuration spaces of 2D and 3D rigid objects. The approximation
can be used to identify caging configurations and establish path nonexistence between given pairs of configurations. We prove correctness andanalyse completeness of our approach. Using dual diagrams of unions ofballs and uniform grids on SO(3), we provide a way to approximate a
6D configuration space of a rigid object. Depending on the desired levelof guaranteed approximation accuracy, the experiments with our single
core implementation show runtime between 5 − 21 s. and 463 − 1558 s.
Finally, we establish a connection between robotic caging and molecularcaging from organic chemistry, and demonstrate that our approach isapplicable to 3D molecular models.
Keywords: Caging · Path non-existence · Computational Geometry

1

Introduction

Understanding the global topological and geometric properties of the free space
is a fundamental problem in several fields. In robotics, it can be applied tomanipulation and motion planning. In computational chemistry and biology, it
may be used to predict how molecules can restrict each other’s mobility, which
is important for sorting molecules, drug delivery, etc. [14, 22].
In robotic manipulation, the mobility of an object may be constrained bymanipulators and/or obstacles. When the object cannot escape arbitrarily far
from an initial configuration, we say that the object is caged. Formally, thismeans that it is located in a compact path-connected component of its free space.
One of the biggest challenges in caging is verification – i.e., designing efficientalgorithms which are able to provide theoretical guarantees that an object in agiven configuration is caged. One approach towards caging verification relies onparticular geometric and topological features of the object under consideration.
This approach can be computationally efficient, but is limited to objects withcertain shape properties, such as the existence of narrow parts or handles.
To deal with objects and obstacles of arbitrary shape, one needs to show thatan object is located in a bounded path-connected component of its free space.
?

The first two authors contributed equally.

2

A. Varava, J. F. Carvalho et al.

Direct reconstruction of a 6D configuration space is computationally expensive,
and to the best of our knowledge no fast and rigorous approach has been proposedso far. In this paper, we address this problem.

Fig. 1: Diagram of our method for different application scenarios. We approximate the collision spaceof an object by choosing a finite set of fixed object’s orientations and considering the corresponding
projections of the collision space to Rn (n ∈ {2, 3}).

We consider caging a special case of the problem of proving path nonexistence between a pair of configurations. To show that two configurations are
disconnected, we construct an approximation of the object’s collision space. Intuitively, we construct a set of projections of a subset of the object’s configuration
space to subspaces corresponding to fixed orientations of the object, see Fig. 1.
By construction, our approximation is a proper subset of the object’s collision
space, which implies that when our algorithm reports that the two configurations are disconnected, then there is no path between them in the real free space.
However, for the same reason, our algorithm is not guaranteed to find all possible
caging configurations, since we do not reconstruct the entire collision space.
In [27], we presented our initial work on this approach without analyzing its
theoretical properties and discussing practical details of its implementation. The
core contributions of the present paper with respect to [27] can be summarized
as follows: (i) we present a way to implement our algorithm using geometricconstructions from [4] and [34], and run experiments in a 2D and 3D workspace;(ii) we provide an accurate bound for the displacement of a 3D object induced
by a given rotation, and discuss how to practically discretize SO(3) using atechnique from [33]; (iii) we generalize our approach to narrow passage detection
and construct δ-clearance configuration space approximations; (iv) we provide
a correctness proof of our algorithm by showing that if in our approximation of
the free space two configurations are disconnected, then they are disconnectedin the actual free space; (v) we prove δ-completeness of the algorithm: if two
configurations are disconnected, we can construct a good enough approximationof the free space to show that these configurations are either disconnected orconnected by a narrow passage; (vi) we establish a connection between robotic
caging and molecular caging in organic chemistry and discuss possible future
research in this direction; as a proof of concept, we run our algorithm on a pairof molecules studied in [14] and obtain the same result as the authors in [14]obtained in a chemical laboratory.

Free Space of Rigid Objects

2

3

Related WorkIn manipulation, caging can be considered as an alternative to a force-closure
grasp [9, 10, 18, 26], as well as an intermediate step on the way towards a formclosure grasp [21]. Unlike classical grasping, caging can be formulated as a purely
geometric problem, and therefore one can derive sufficient conditions for an
object to be caged. To prove that a rigid object is caged, it is enough to prove
this for any subset (part) of the object. This allows one to consider caging a
subset of the object instead of the whole object, and makes caging robust to
noise and uncertainties arising from shape reconstruction and position detection.
The notion of a planar cage was initially introduced by Kuperberg in 1990 [5]as a set of n points lying in the complement of a polygon and preventing it fromescaping arbitrarily far from its initial position. In robotics, it was subsequently
studied in the context of point-based caging in 2D by Rimon and Blake [19], Pipattanasomporn and Sudsang [16], Vahedi and van der Stappen [28], and others.
A similar approach has also been adopted for caging 3D objects. For instance,
Pipattanasomporn and Sudsang [17] proposed an algorithm for computing all
two-finger cages for non-convex polytopes. In [25, 29] the authors present a set
of 2D caging-based algorithms enabling a group of mobile robots to cooperatively
drag a trapped object to the desired goal.
In the above mentioned works fingertips are represented as points or spheres.
Later, more complex shapes of caging tools were taken into account by Pokorny
et al. [18], Stork et al. [23, 24], Varava et al. [26], Makita et al. [9, 10]. In theseworks, sufficient conditions for caging were derived for objects with particularshape features. Makapunyo et al. [12] proposed a heuristic metric for partial
caging based on the length and curvature of escape paths generated by a motion
planner. The authors suggested that configurations that allow only rare escapemotions may be successful in practice.
We address caging as a special case of the path non-existence problem: an
object is caged if there is no path leading it to an unbounded path-connectedcomponent. The problem of proving path non-existence has been addressed byBasch et al. [2] in the context of motion planning, motivated by the fact thatmost modern sampling-based planning algorithms do not guarantee that two
configurations are disconnected, and rely on stopping heuristics in such situations [6]. Basch et al. prove that two configurations are disconnected when theobject is ‘too big’ or ‘too long’ to pass through a ‘gate’ between them. There
are also some related results on approximating configuration spaces of 2D objects. In [31], Zhang et al. use approximate cell decomposition and prove path
non-existence for 2D rigid objects. They decompose a configuration space into
a set of cells and for each cell decide if it lies in the collision space. In [13]McCarthy et al. propose a related approach. There, they randomly sample theconfiguration space of a planar rigid object and reconstruct its approximation as
an alpha complex. They later use it to check the connectivity between pairs ofconfigurations. This approach has been later extended to planar energy-boundedcaging [8].

4

A. Varava, J. F. Carvalho et al.

The problem of explicit construction (either exact or approximate) of configuration spaces has been studied for several decades in the context of motion
planning, and a summary of early results can be found in [30]. In [7] the ideaof slicing along the rotational axis was introduced. To connect two consecutive slices, the authors proposed to use the area swept by the robot rotating
between two consecutive orientation values. In [32], this idea was extended tousing both outer and inner swept areas to construct a subset and a supersetof the collision space of polygonal robots. The outer and inner swept areas are
represented as generalized polygons defined as the union and intersection of all
polygons representing robot’s shape rotating in a certain interval of orientation
values, respectively. Several recent works propose methods for exact computation of configuration spaces of planar objects [3, 15]. In [3], a method towards
exact computation of the boundary of the collision space is proposed. In [15],the authors explicitly compute the free space for complete motion planning.
As we can see, several approaches to representing configuration spaces of
2D objects, both exact and approximate, have been proposed and successfully
implemented in the past. The problem however is more difficult if we consider
a 3D object, as its configuration space is 6-dimensional. In the recent survey on
caging [11], the authors hypothesise that recovering a 6D configuration space
and understanding caged subspaces is computationally infeasible. We present a
provably-correct algorithm for approximating 3D and 6D configuration spaces,
which has a reasonable runtime on a single core of Intel Core i7 processor.
We study path-connectivity of the free space of 2D and 3D rigid objects.
We do this by decomposing the configuration space into a finite set of lower
dimensional slices. Although the idea of slicing is not novel and appears in theliterature as early as in 1983 [7], recent advances in computational geometry andtopology, as well as a significant increase in processing power, have made it easier
to approximate a 6D configuration space on a common laptop. When dealing
with slicing a 6D configuration space, we identify two main challenges: how toquickly compute 3D slices, and how to efficiently discretize the orientation space.
For slice approximation, our method relies on simple geometric primitives —unions of balls, which makes our algorithm easy to implement and generalizable
to 6D configuration spaces. Given a subset of an object, we determine howdensely the set of orientations should be sampled in order to approximate thecollision space of the object, and use dual diagrams of a union of balls [4] toapproximate the free space. This way, we do not need to use generalized polygons,
which makes previous approaches more difficult in 2D and very hard to generalize
to 3D workspaces. For SO(3) discretization, we use the method from [33], which
provides a uniform grid representation of the space. The confluence of thesefactors result in overcoming the dimensionality problem without losing necessary
information about the topology of the configuration space, and achieving bothpractical feasibility and theoretical guarantees at the same time.
Finally, our method does not require precise information about the shape ofobjects and obstacles, and the only requirement is that balls must be locatedstrictly inside them, which makes our approach robust to noisy and incompleteFree Space of Rigid Objects

5

sensor data. The focus of this paper is on proving path non-existence rather
than complete motion planning. In practice, it is often enough to have a roughapproximation of the free space to find its principal connected components, whichis easier to compute than its exact representation.

3

Free Space Decomposition

Since our work is related both to object manipulation and motion planning,
we use the general term ‘object’ without loss of generality when talking about
objects and autonomous rigid robots (e.g., disc robots) moving in n-dimensional
workspaces1
Definition 1. A rigid object is a compact connected non-empty subset of Rn .
A set of obstacles is a compact non-empty subset of Rn .
We want to represent both the object and the obstacles as a set of n−dimensionalballs. Therefore, we do not allow them to have ‘thin parts’. Formally, we assume
that they can be represented as regular sets [20]:Definition 2. A set U is regular if it is equal to the closure of its interior:U = cl(int(U )).
We approximate both the obstacles and the object as unions of balls lying intheir interior, S = {BR1 (X1 ), . . . , BRn (Xn )} and O = {Br1 (Y1 ), . . . , Brm (Ym )}of radii R1 , . . . , Rn and r1 , . . . , rm respectively.
Let C(O) = SE(n) denote the configuration space of the object. We define
its collision space 2 C col (O) as the set of the objects configurations in which theobject penetrates the obstacles:
Definition 3. C col (O) = {c ∈ C | [int c(O)] ∩ [int S] 6= ∅}, where c(O) denotes
the object in a configuration c. The free space C f ree (O) is the complement of thecollision space: C f ree (O) = C(O) − C col (O).
In [27], we suggested that configuration space decomposition might be amore computationally efficient alternative to its direct construction. Namely, we
represent the configuration of an object space as a product C(O) = Rn × SO(n),and consider a finite covering of SO(n)by open sets (this is always possible,Ssince SO(n) is compact): SO(n) = i∈{1,...,s} Ui . We recall the notion of a slice,
introduced in [27]:Definition 4. A slice of the configuration space C(O) of a rigid object, is asubset of C(O) defined as follows: SlU (O) = Rn × U , where U is an subset ofSO(n).
f ree
colWe denote a slice of the collision (free) space by SlU(O) (SlU(O)). Forcoleach slice we construct an approximation aSlU (O) of its collision space in sucha way that our approximation lies inside the real collision space of the slice,
col
col
aSlU
(O) ⊂ SlU(O).
1
2

Throughout the paper n ∈ {2, 3}.
A theoretical analysis of different ways to define the free space can be found in [20].

6

A. Varava, J. F. Carvalho et al.

This way, we approximate the entire collision space by a subset aC col (O):

aC

col

(O) = 


[

col
aSlU
(O) ⊂ C col (O)ii∈{1,...,s}An object’s ε−core. First of all observe that by Def. 3, if a subset O0 of an
object O placed in configuration c ∈ C(O) is in collision, then the entire object O
is in collision. Therefore, the collision space of O is completely contained within
the collision space of O0 . This allows us to make the following observation:
Observation 1 Consider an object O and a set of obstacles S. Let c1 , c2 ∈C f ree (O) be two collision-free configurations of the object. If there is no collisionfree path between these configurations for its subset O0 ⊂ O, then there is nocollision-free path connecting these configurations for O.
Therefore, if some subset O0 of O in configuration c is caged, then the entire
object O in the same configuration c is caged. This means that if we construct
col
col
aSlU
in such a way that for any configuration c ∈ aSlU
there exists a subset00
O of c(O) such that O is in collision, then c(O) is also in collision. In [27] wedefined an ε−core of an object as follows:
Definition 5. The ε-core of an object O is the set Oε comprising the points ofO which lie at a distance3 of at least ε from the boundary of O: Oε = {p ∈O | d(p, ∂O) ≥ ε}.
Now, for an object O and its ε-core Oε , we write
Oφ and Oεφ respectively to mean that their orientation
is fixed at φ ∈ SO(n). So, let C col (Oεφ ) denote the collision space of Oε with a fixed orientation φ. Note thatsince the orientation is fixed, we can identify C col (Oεφ )with a subset of Rn .
In [27], we showed that for an object O, ε > 0
and a fixed orientation φ ∈ SO(n) there exists a nonFig. 2: An ε−core remains
empty neighbourhood U (φ, ε) of φ such that for any inside the object when weθφslightly rotate itθ ∈ U (φ, ε), Oε is contained in O , see Fig. 2.
We approximate the collision space as follows:
we pick an ε > 0 and a set of orientation values {φ1 , . . . , φs } so thatU (φ1 , ε), . . . , U (φs , ε) cover SO(n), and so that for any θ ∈ U (φi , ε) we haveOεφi ⊂ Oθ .
Finally, for each φi ∈ {φ1 , . . . , φs }, we compute collision space slice apcol
φi
col
col
proximations aSlU
(Oεφi ) ×
(φi ,ε) as the collision space of Oε , aSlU (φi ,ε) = C
U (φi , ε) which results in our approximation of the collision space: aCεcol (O) =S
colφi aSlU (φi ,ε) .
Discretization of SO(n): Elements of SO(n) can be seen as parametrizing
rotations in Rn , so for any q ∈ SO(n) we define Rq as the associated rotation.
Let D(Rq ) denote the maximal displacement of any point p ∈ O after applying
Rq , i.e. D(Rq ) = maxp∈O (d(p, Rq (p))), then Oε ⊂ Rq (O) if D(Rq ) < ε.
3

By distance here we mean Euclidean distance in Rn

Free Space of Rigid Objects

7

In [27] we have derived the following upper bound for the displacement of
a two-dimensional object: D(Rq ) ≤ 2| sin(q/2)| · rad(O) assuming that we rotate the object around its geometric center and rad(O) denotes the maximum
distance from it to any point of the object, and q is the rotation angle. In thetwo-dimensional case, discretization of the rotation space is trivial: given an ε wepick q ∈ SO(2) such that D(Rq ) < ε, and compute a set of orientation samples{φ1 = 0, φ2 = 2q, . . . , φs = 2(s − 1)q}, where s = dπ/qe. This gives us a covering {U (φ1 , ε), . . . , U (φs , ε)} of SO(2), where for each i ∈ {1, . . . , s} we define
U (φi , ε) = [φi − q, φi + q]4 .
Let us now discuss the three-dimensional case. Similarly to the previous case,our goal is to cover SO(3) with balls of fixed radius. To parametrize SO(3) we use
unit quaternions. For simplicity, we extend the real and imaginary part notationfrom complex numbers to quaternions, where <q and =q denote the real and“imaginary” parts of the quaternion q. Further, we identify =q = qi i + qj j + qk kwith the vector (qi , qj , qk ) ∈ R3 ; and we write q̄, and |q| to mean the conjugate√<q − =q and the norm q q̄, respectively. We use angular distance [33] to define
the distance between two orientations: ρ(x, y) = arccos(|hx, yi|), where x and y
are two elements of SO(3) represented as unit quaternions which are regarded
as vectors in R4 , and hx, yi is their inner product. A unit quaternion q defines=qa rotation Rq as the rotation of angle θq = 2 cos−1 (<q) around axis wq = |=q|.
This allows one to calculate the displacement of the rotation D(q) = D(Rq ) as:θD(q) = 2 sin( 2q ) = 2 sin(cos−1 (<q)) = 2|=q|
Define the angle distance from a point to a set S ⊆ SO(3) in the usual way asρ(S, x) = miny∈S ρ(y, x). In [33], the authors provide a deterministic sampling
scheme to minimize the dispersion δ(S) = maxx∈SO(3) ρ(S, x). Intuitively, thedispersion of a set δ(S) determines how far a point can be from S, and in thisway it determines how well the set S covers SO(3). Now, assume we are given
a set of samples S ⊆ SO(3) such that δ(S) < δ. Then for any point p ∈ SO(3)denote D(pS̄) = maxq∈S D(pq̄), we want to show that in these conditions, there
exists some small ε such that maxp∈SO(3) D(pS̄) < ε. This would imply that
there exists some δ 0 > δ such that if we take U = {q ∈ SO(3) | ρ(1, q) < δ 0 },then denoting Up = {qp | q ∈ U }, the family {Up }p∈S fully covers SO(3) and forany q ∈ Up satisfies D(q p̄) < ε.
Now, Proposition 1 allows us to establish the relation between the distance
between two quaternions and displacement associated to the rotation between
them.
Proposition 1. Given two unit quaternions p, q, the following equation holds:
D(pq̄) = 2 sin(ρ(p, q)).

(1)

The proof of Proposition 1 can be found in the supplementary material.
This means that if we want to cover SO(3) with patches Ui centered at apoint pi such that D(Rqp̄i ) is smaller than some ε, we can use any deterministic
4This is a cover by closed sets, but given q 0 > q satisfying D(Rq ) < ε we can use
instead U (φi , ε) = (φi − q 0 , φi + q 0 ) which esults on the same graph.

8

A. Varava, J. F. Carvalho et al.

sampling scheme on SO(3) (e.g. [33]) to obtain a set S with dispersion δ(S) <arcsin( 2ε ).
Note that if S ⊆ SO(3) has a dispersion δ(S) < arcsin( 2ε ) then the family ofpatches {U (s, ε) | s ∈ S} forms a cover of SO(3).
Finally, given such a cover of SO(3), recall that we want to approximately
f reereconstruct the full free space of the object C f ree as the union of slices aSlU(s,ε) .
This requires us to test the whether two slices overlap. To make this efficient,
we create a slice adjacency graph, which is simply a graph with vertices S andedges (s, s0 ) if U (s, ε) ∩ U (s0 , ε) 6= ∅.
To compute this graph, we use the fact that U (s, ε) = {p ∈ SO(3)|ρ(s, p) < δ}for some δ, and so if two slices U (s, ε), U (s0 , ε) are adjacent, there must exist somep such that ρ(p, s), ρ(p, s0 ) < δ, which implies ρ(s, s0 ) < ρ(s, p) + ρ(s0 , p) < 2δ.
We use this fact to calculate the slice adjacency graph in Alg. 1.

Algorithm 1: ComputeAdjacentOrientations
1
2
3
4
5
6
7
8
9
10
11
12

input : S /* Set of points in SO(3).
output: G /* Patch adjacency graph.
V ← S;δ ← δ(S);T ← KDT ree(V );E ← {};
for p ∈ V do
P ← T.query(p, dist ≤ 2 sin(δ) + τ ) \ {p} ;P ← L ∪ T.query(−p, dist ≤ 2 sin(δ) + τ );for q ∈ L do
add (p, q) to E;
end
end
return G = (V, E);*/*/

The algorithm starts by setting V = S, as this is the set of vertices in thegraph, and we put these vertices in a KDT ree in order to quickly perform nearest
neighbor queries. Now, to compute the set of edges E, we locate for each p ∈ S
the points at an angle distance smaller than 2δ 5 in line 6. Finally, in line 7 wealso add edges to the points at an angle distance smaller than 2δ of −p, as bothp and −p represent the same orientation.
One of the prerequisites of Alg. 1 is the availability of an estimate of the dispersion of δ(S). In our implementation we used the algorithm in [33] to compute
S where the authors provide a provable upper bound for dispersion. However
because this is an upper bound, and we want a tight estimate of the dispersion,in our implementation we employed a random sampling method to estimate thedispersion. At each step we draw a uniform sample p and compute its two nearest
neighbors q, q 0 . We estimate the displacement at point p to be half the (angle)
distance between q and q 0 . The final estimate is taken to be the maximum overthe estimate for every sample taken.
5Since the KDT ree T uses the Euclidean distance in R4 we employ the formula
kp − qk = 2 sin( ρ(p,q)).
2Free Space of Rigid Objects

9Construction of Slices. In [27], weSderive the following representation for thecollision space of Oεφi : C col (Oεφ ) = i,j (BRj +ri −ε (Xj − GYi )), where G is theorigin chosen as the geometric center of the object, and GYi denotes the vector
from G to Yi . Now, let us discuss how we construct path-connected components
of C f ree (Oεφ ), see Alg. 2.
For this, we first need to approximate thecomplement of C col (Oεφ ), which is represented
as a finite collection of balls. We do this byconstructing S
its dual diagram [4]. A dual
S diagram Dual( Bi ) of a union of balls Bi isa finite collection of balls such that the complement of its interior isScontained in and ishomotopy equivalent to Bi . It is convenient
to approximate C f ree (Oεφ ) as a dual diagramof collision space balls for several reasons.
First, balls are simple geometric primitives
which make intersection checks trivial. Second, the complement of the dual diagram isguaranteed to lie strictly inside the collision Fig. 3: Dual diagram of a set of circles.
space, which provides us with a conservative Red circles with blue centers represent
collision space, the yellow circles apapproximation of the free space. Finally, ho- theproximate the free space. The black edges
motopy equivalence between its complement form the associated weighted Voronoi diand the approximate collision space implies agram.
that our approximation preserves the connectivity of the free space that wewant to approximate. Another advantage of a dual diagram is that it is easy toconstruct.
First, recall that a weighted Voronoi diagram is a special case of a Voronoi
diagram, where instead of Euclidean distance between points a weighted distancefunction is used. In our case, the weighted distance of a point x ∈ Rn from2
Bri (zi ) is equal to dw (x, B
Sri (zi )) = ||x − zi || − ri . Consider a weighted Voronoi
diagram of a set of balls i Bri (zi ). For each vertex yj of the Voronoi diagram,let Bqj (yj ) whose radius qj is equal to the square root of the weighted
Sdistanceof yj to any of the four (three in the two-dimensional case) balls from i Bri (zi )generating yj . Then, take each infinite edge of the Voronoi diagram, and add
an degenerate “infinitely large” ball (a half-space) with center at the infinite
end of this edge, see Fig. 3. After constructing the dual diagram, it is easy tocompute connected components of the slice. Note that there always is one infinite
component corresponding to the outside world, and caging configurations arecontained in bounded components.
The Connectivity Graph. A naive algorithm described in [26] requires us
to first compute all the slices and their connected components, and only afterthat compute the connectivity graph. This strategy leads to suboptimal usage ofmemory — in practice, when one needs to compute around 30000 slices, storing
all their free space representations at the same time is not feasible on a common
laptop. However, we notice that this is not necessary, since the representation of

10

A. Varava, J. F. Carvalho et al.

Algorithm 2: Compute-Slice-Connectivity
1
2
3
4
5
6
7
8
9
10
11

Data: A union of d-dimensional balls C col (Oεφ )Result: A set of connected components aC0 , . . . , aCn
V (C col (Oεφ )) ← Weighted-Voronoi-Diagram(C col (Oεφ ))aC f ree ← Dual-Diagram(V (C col (Oεφ )))aC0 ← Compute-Infinite-Component()i←0
foreach ball ∈ aC f ree do
if Connected-Component(ball) = ∅ theni←i+1
aCi ← Compute-Component(ball)end
end
return {aC0 , . . . , aCn }each slice is needed only to check whether its connected components overlap withthe connected components of adjacent slices. Therefore, in our implementationeach slice is deleted as soon as the connectivity check between it and the slices
adjacent to it is performed.
Let us now discuss how we check whether two connected components overlap. Let G(aCεcol (O)) = (V, E) be a graph approximating the free space. The
vertices of G correspond to the connected components {aC1i , . . . , aCni i } of eachslice, i ∈ {1, . . . , s}, and are denoted by v = (aC, U ), where aC and U are thecorresponding component and orientation interval. Two vertices representing
f ree
f ree
components Cp ⊂ aSlU
and Cq ⊂ aSlU
, i 6= j, are connected by an edge ifijthe object can directly move between them. For that, both the sets Ui , Uj , andaCq , aCp must overlap: Ui ∩ Uj , aCq ∩ aCp 6= ∅. G(aCεcol (O)) approximates thefree space of the object: if there is no path in G(aCεcol (O)) between the vertices
associated to the path components of two configurations c1 , c2 , then they are
disconnected in C f ree (O). Finally, to check whether two connected components
in adjacent slices intersect, we recall that they are just finite unions of balls. Instead of computing all pairwise intersections of balls, we approximate each ballby its bounding box and then use the CGAL implementation of Zomorodian’salgorithm [34], which efficiently computes the intersection of two sequences ofthree-dimensional boxes. Every time it finds an overlapping pair of boxes, wecheck whether the respective balls also intersect.
Remark 1 Recall that in each slice the aC f ree (Oεφi ) are constructed as the dualof the collision space C col (Oεφi ), which entails that aC f ree (Oεφi ) has the sameconnectivity as C f ree (Oεφi ). However, it also entails that any connected component of aC f ree (Oεφi ) partially overlaps with the collision space C col (Oεφi ). This0means that for two connected components Cji , Cji0 of adjacent slices which do0
not overlap, it may occur that the corresponding approximations aCji , aCji0 do
overlap. In this case the resulting graph G(aCεcol (O)) would contain an edge between the corresponding vetices. This effect can be mitigated by verifying whether
the overlap between the approximations occurs within the collision space of both0
slices. This can be done for example by covering the intersection aCji ∩ aCji0

Free Space of Rigid Objects

11with a union of balls and checking if it is contained inside the collision spaceφ
C col (Oεφi ) ∪ C col (Oε i0 ).
Existence of δ-clearance paths. For safety reasons, in path planning applications a path is often required to keep some minimum amount of clearance
to obstacles. The notion of clearance of an escaping path can also be applied
to caging: one can say that an object is partially caged if there exist escaping
paths, but their clearance is small and therefore the object is unlikely to escape.
Consider a superset of our object O, defined as a set of points lying at mostat distance δ from O, and let us call it a δ-offset of the object: O+δ = {p ∈Rn | d(p, O) ≤ δ}. Now observe that two configurations are δ-connected if andonly if there exists a collision-free path connecting these configurations in theconfiguration space of the δ-offset O+δ of the object. This means that two configurations c1 and c2 are not δ-connected in C f ree (O) if and only if they are notpath-connected in C f ree (O+δ ). Therefore, to understand the δ−connectivity of
the free space it is enough to compute path-connected components as previously
described, for the δ-offset O+δ .
One application of this observation is narrow passage detection. One can use
our free space approximation to identify narrow passages: if two configurations
f ree
are disconnected in aCε+δ
(O+δ ), but connected in aCεf ree (O), then they areconnected by a narrow passage of width at most δ. Our approximation then canbe used to localize this passage, so that probabilistic path planning algorithm
can sample in this location.
Furthermore, we can view δ as the level of accuracy of the approximation: if
δ = 0, and our algorithm reports path non-existence, then this result is provably
correct; for δ > 0, we can guarantee non-existence of paths whose clearance does
not exceed δ. This means that depending on the desired level of accuracy one
can construct a coarse approximation of the free space by looking at the pathf reeconnectivity of aCε+δ
(O+δ ). This approximation requires fewer slices, and Sec. 5
demonstrates that this significantly reduces the computation time.

4

Theoretical Properties of Our Approach

In this section, we discuss correctness, completeness and computational complexity of our approach. The proofs of Propositions 2 and 3, as well as the discussion
about computational complexity, can be found in the supplementary material.
First of all, let us show that our algorithm is correct: i.e., if there is no collisionfree path between two configurations in our approximation of the free space,then these configurations are also disconnected in the actual free space.
Proposition 2 (correctness). Consider an object O and a set of obstacles S.
Let c1 , c2 be two collision-free configurations of the object. If c1 and c2 are notpath-connected in G(aCεf ree (O)), then they are not path-connected in C f ree (O).
Now, we show that if two configurations are not path-connected in C f ree (O),we can construct an approximation of C f ree (O) in which these configurations
are either disconnected or connected by a narrow passage.

12

A. Varava, J. F. Carvalho et al.

Proposition 3 (δ-completeness). Let c1 , c2 be two configurations in C f ree (O).
If they are not path-connected in C f ree (O), then for any δ > 0 there exists
ε > 0 such that the corresponding configurations are not path-connected in
G(aCεf ree (O+δ )), where the graph is produced according to the procedure outlined
in Rem. 1.
Computational complexity. The complexity of the first part of the algorithm(slice construction) is O(n2 m2 ), where n and m are the number of balls in
the object’s and the obstacle’s spherical representation; the complexity of thesecond part (connectivity graph construction) is O(sl(b log3 (b) + k)), where sis the number of slices, l is the number of neighbours of each slice, b is the
maximum number of balls in each pair of connected components, and k thenumber of pairwise intersections of the balls. The details can be found in the
supplementary material.

5

Examples and Experiments

We implemented our algorithm for 2D and 3D objects. All experiments were run
on a single core of a computer with Intel Core i7 processor and 16GB GPU.
Different Approximations of the Workspace. In this experiment, we consider how the accuracy of a workspace spherical approximation affects the output
and execution time of the algorithm, see Fig. 4. For our experiments, we generate

Fig. 4: In the left we present a set of 2D obstacles in blue, and an object in green. The numbers
represent the connected components of the free space. In red are three approximations of the obstacles
by sets of balls of radius 15, 10, and 4, respectively. Note that the smaller the radius the more featuresfrom the original configuration space are preserved.

a workspace and an object as polygons, and approximate them with unions ofballs of equal radii lying strictly inside the polygons. Note that the choice of theradius is important: when it is small, we get more balls, which increases the computation time of our algorithm; on the other hand, when the radius is too large,
we lose some important information about the shape of the obstacles, because
narrow parts cannot be approximated by large balls, see Fig. 4. We consider asimple object whose approximation consists of 5 balls. We run our algorithm for
all the 3 approximations of the workspace, and take 5 different values of ε, see
Tab. 1. We can observe that as we increase the ε the computation time decreases.
This happens because we are using fewer slices. However, we can also observethat when the ε is too large, our approximation of the collision space becomes
too small, and we are not able to find one connected component (see the lastcolumn of Tab. 1).

Free Space of Rigid Objects

13εR = 15R = 10R=40.30 · r 2 c. 741 ms 3 c. 1287 ms 4 c. 1354 ms
0.33 · r 2 c. 688 ms 3 c. 1208 ms 4 c. 1363 ms
0.37 · r 2 c. 647 ms 3 c. 1079 ms 4 c. 1287 ms
0.40 · r 2 c. 571 ms 3 c. 986 ms 3 c. 1156 ms
0.43 · r 2 c. 554 ms 3 c. 950 ms 3 c. 1203 ms
Table 1: We report the number of path-connected components we found and the computation time foreach case. When we were using our first approximation of the workspace, we were able to distinguishonly between components 4 and 2 (see Fig. 4), and therefore prove path non-existence between
them. For a more accurate approximation, we were also able to detect component 3. Finally, thethird approximation of the workspace allows us to prove path non-existence between every pair of
the four components.

Different Types of 3D Caging. As we have mentioned in Sec. 2, a number ofapproaches towards 3D caging is based on identifying shape features of objects
and deriving sufficient caging conditions based on that. By contrast, our method
is not restricted to a specific class of shapes, and the aim of this section is toconsider examples of objects of different types and run our algorithm on them.
The examples are depicted on Fig. 5, and Tab. 2 reports execution time fordifferent resolutions of the SO(3) grid.
#slices rings (320) narrow part (480) surrounding (266) gravity (348) molecules (2268)
576
5 s δ = 2.4
6sδ = 5.910 s
δ = 1.2
4 s δ = 1.2
21 s δ = 1.02
4608 49 s δ = 0.7 70 s
δ = 2.4 108 s
δ = 0.4 41 s δ = 0.4 181 s δ = 0.34
36864 540 s δ = 0.0 785 s
δ = 0.0 1073 s
δ = 0.0 463 s δ = 0.0 1558 s
δ = 0.0
Table 2: Results from running 3D experiments in the 4 different scenarios described in Fig. 5 andthe molecular model in Fig. 6 using 3 different resolutions for the SO(3) grid. The number of balls
used to approximate the collision space of each model is indicated in parenthesis next to the model
name. We report the computational time and the value of guaranteed clearance δ for each case .

Fig. 5: Different 3D caging scenarios. From left to right: linking-based caging linking-based
caging [18, 24, 23], narrow part-based caging [27, 35], surrounding-based caging, and caging withgravity [8]. In linking-based and narrow part-based caging the obstacles form a loop (not necessarilyclosed) around a handle or narrow part of the object. In surrounding-based the object is surroundedby obstacles so as not to escape. In caging with gravity, gravity complements the physical obstacles
in restricting the object’s mobility. Here, we set a fixed level of the potential energy that the object
can achieve and consider every configuration with higher potential energy as “forbidden”.

Observe that depending on the desired accuracy of the resulting approximation, different numbers of slices can be used. In our current implementation, we
precompute grids on SO(3) using the algorithm from [33], and in our experiments
we consider 3 different predefined resolutions. However, given a concrete clearance value δ, one can construct a grid on SO(3) with the necessary resolution
based on δ.
As we see, our algorithm can be applied to different classes of shapes. It certainly cannot replace previous shape-based algorithms, as in some applications
global geometric features of objects can be known a priori, in which case it might

14

A. Varava, J. F. Carvalho et al.

be faster and easier to make use of the available information. However, in those
applications where one deals with a wide class of objects without any knowledge about shape features they might have, our algorithm provides a reasonable
alternative to shape-based methods.
Molecular Cages. In organic chemistry, the notion of caging was introduced independently of the concept of [14]. Big molecules or theirensembles can form hollow structureswith a cavity large enough to envelope a smaller molecule. Simple or- Fig. 6: In this example, we consider one big andganic cages are relatively small in size two small molecules to see whether our algorithm
predict their ability to form a cage. Atomic
and therefore can be used for selective cancoordinates and (van der Waals) radii were reencapsulation of tiny molecules based trieved from the Cambridge Crystallographic Data
(CCDC) database. The depicted moleculeson their shape [14]. In particular, this Centre
are mesitylene (left, CCDC 907758), CC1 (midproperty can be used for storage and dle, CCDC 707056), and 4-ethyltoluene (right,
Our algorithm reported that CC1
delivery of small drug molecules in 1449823)[14].
cages mesitylene, but does not cage 4-ethyltoluene.
living systems [22]. By designing the These results coincide with the experimental recaging molecules one is able to control sults reported in [14].
the formation and breakdown of a molecular cage, in such a way as to remain
intact until it reaches a specific target where it will disassemble release the drug
molecule. An example on Fig. 6 shows that in principle, our algorithm can beapplied to molecular models, assuming they can be treated as rigid bodies. In
our future work, we aim to use our algorithm to test a set of potential molecular carriers and ligands to find those pairs which are likely to form cages. Thisprediction can later be used as a hypothesis for further laboratory experiments.
Possible parallel implementation. Our algorithm has a lot of potential forparallelization, and in the future we are planning on taking advantage of it. For
the examples considered above, our current implementation requires between 40
and 60 milliseconds to compute each slice together with its connected components, and the connectivity check for a pair of slices is even faster. The bottleneckof the algorithm is the number of slices. However, most of the computations canbe performed in parallel: indeed, we can first compute the approximation of
the collision space for each orientation s in separate threads, and store them inshared memory. After that, we can again run one process per slice to computethe intersections between connected components of adjacent slices.In the future,
we are planning on implementing a parallel version of our algorithm.

6

Conclusion

In this paper, we provide a computationally feasible and provably-correct algorithm for 3D and 6D configuration space approximation and use it to provecaging and path non-existence. In the future, we will look into how we can makeour algorithm faster and applicable to real-time scenarios. In particular, we will
work on a parallel implementation, and investigate how the number of slice canbe reduced. We are also interested in integrating our narrow passage detection

Free Space of Rigid Objects

15

and path non-existence techniques with probabilistic motion planners. Finally,
we are going to use our approach to find molecular cages.

7

Acknowledgements

The authors are grateful to D. Devaurs, L. Kavraki, and O. Kravchenko for theirinsights into molecular caging. This work has been supported by the Knut andAlice Wallenberg Foundation and Swedish Research Council.

References
1. Barraquand, J., Kavraki, L., Latombe, J.-C., Motwani, R., Li, T.-Y., Raghavan,P.: A random sampling scheme for path planning.: IJRR, 16(6), 759–774 (1997).
2. Basch, J., Guibas, L. J., Hsu, D., Nguyen, A. T.: Disconnection proofs for motion
planning.: IEEE ICRA (2001), 1765–1772.
3. Behar, E., Lien, J.-M.: Mapping the configuration space of polygons using reduced
convolution.: IEEE/RSJ Intelligent Robots and Systems (2013), 1242–1248.
4. Edelsbrunner, H.: Deformable smooth surface design. Discr. and Comp. Geom.
(1999), 21(1), 87–115.
5. Kuperberg, W.: Problems on polytopes and convex sets.: DIMACS Workshop on
polytopes (1990), 584–589.
6. Latombe, J.-C.: Robot Motion Planning. Norwell, MA, USA: Kluwer Academic
Publishers (1991).
7. Lozano-Perez, T.: Spatial planning: A configuration space approach.: IEEE Transactions on computers, 2, 108–120 (1983).
8. Mahler, J., Pokorny, F. T., McCarthy, Z., van der Stappen, A. F., Goldberg, K.:
Energy-bounded caging: Formal definition and 2-D energy lower bound algorithm
based on weighted alpha shapes.: IEEE RA-L, 1(1), 508–515 (2016).
9. Makita, S., Maeda, Y.: 3D multifingered caging: Basic formulation and planning.:
IEEE IROS (2008), 2697–2702.
10. Makita, S., Okita, K., Maeda, Y.: 3D two-fingered caging for two types of objects:
sufficient conditions and planning.: International Journal of Mechatronics and Automation, 3(4), 263–277 (2013).
11. Makita, S., Wan, W.: A survey of robotic caging and its applications. Advanced
Robotics, 31(19-20), 2017.
12. Makapunyo, T., Phoka, T., Pipattanasomporn, P., Niparnan, N., Sudsang, A.: Measurement framework of partial cage quality based on probabilistic motion planning,
IEEE ICRA (2013), 1574–1579.
13. McCarthy, Z., Bretl, T., Hutchinson, S.: Proving path non-existence using sampling
and alpha shapes.: IEEE ICRA (2012), 2563–2569.
14. Mitra, T., Jelfs, K. E., Schmidtmann, M., Ahmed, A., Chong, S. Y., Adams, D.
J., Cooper, A. I.: Molecular shape sorting using molecular organic cages.: Nature
Chemistry, 5, 276 (2013).
15. Milenkovic, V., Sacks, E., Trac, S. Robust complete path planning in the plane.:
Algorithmic Foundations of Robotics X, 37–52 (2013).
16. Pipattanasomporn, P., Sudsang, A.: Two-finger caging of concave polygon.: IEEEICRA (2006), 2137–2142.
17. Pipattanasomporn, P., Sudsang, A.: Two-finger caging of nonconvex polytopes.:
IEEE T-RO, 27(2), 324–333 (2011).
18. Pokorny, F. T., Stork, J. A., Kragic, D.: Grasping objects with holes: A topologicalapproach.: IEEE ICRA (2013), 1100–1107.

16

A. Varava, J. F. Carvalho et al.

19. Rimon, E., Blake, A.: Caging planar bodies by one-parameter two-fingered gripping
systems.: IJRR, 18(3), 299–318 (1999).
20. Rodriguez, A., Mason, M. T.: Path connectivity of the free space.: IEEE T-RO,
28(5), 1177–1180 (2012).
21. Rodriguez, A., Mason, M. T., Ferry, S.: From caging to grasping.: IJRR, 31(7),886–900 (2012).
22. Rother, M., Nussbaumer M. G., Rengglic, K., Bruns N.: Protein cages and synthetic
polymers: a fruitful symbiosis for drug delivery applications, bionanotechnology
and materials science.: Chemical Society Reviews, 45, 6213, (2016).
23. Stork, J. A., Pokorny, F. T., Kragic, D.: Integrated Motion and Clasp Planningwith Virtual Linking.: IEEE/RSJ IROS (2013), 3007–3014.
24. Stork, J. A., Pokorny, F. T., Kragic, D.: A Topology-based Object Representation for Clasping, Latching and Hooking.: IEEE-RAS International Conference on
Humanoid Robots (2013), 138–145.
25. Pereira, G. A. S., Campos, M.F.M., Kumar, V.: Decentralized algorithms for multirobot manipulation via caging.: IJRR 23(7–8), 783–795 (2004).
26. Varava, A., Kragic, D., Pokorny, F. T.: Caging Grasps of Rigid and PartiallyDeformable 3-D Objects With Double Fork and Neck Features. IEEE T-RO, 32(6),1479–1497 (2016).
27. Varava, A., Carvalho, J. F., Pokorny, F. T., Kragic, D.: Caging and Path NonExistence: a Deterministic Sampling-Based Verification Algorithm. ISRR, 2017.
Preprint: https://www.csc.kth.se/~jfpbdc/path_non_ex.pdf
28. Vahedi, M., van der Stappen, A. F.: Caging polygons with two and three fingers.:
The IJRR, 27(11–12) 1308–1324 (2008).
29. Wang Z., Kumar, V.: Object Closure and Manipulation by Multiple Cooperating
Mobile Robots.: IEEE ICRA (2002), 394–399.
30. Wise, K., Bowyer, A.: A survey of global configuration-space mapping techniques
for a single robot in a static environment.: IJRR, 19(8), 762–779 (2000).
31. Zhang, L., Young, J. K., Manocha, D.: Efficient cell labelling and path nonexistence computation using C-obstacle query.: IJRR, 27(11–12), 1246–1257 (2008).
32. Zhu, D., Latombe, J.: New heuristic algorithms for efficient hierarchical path planning.: IEEE T-RO, 7(1), 9–20 (1991).
33. Yershova, A., Jain S., LaValle S. M., Mitchell J. C.: Generating Uniform Incremental Grids on SO(3) Using the Hopf Fibration. IJRR, vol. 29, 801–812 (2009).
34. Zomorodian, A., Edelsbrunner H.: Fast software for box intersections, Proceedingsof the 16th annual symposium on Comp. geom., 129–138 (2000).
35. Liu J., Xin S., Gao Z., Xu K., Tu C., Chen B.: Caging Loops in Shape Embedding
Space: Theory and Computation. ICRA, 2018.