Free Space of Rigid Objects: Caging, Path Non-Existence, and Narrow Passage Detection

In this work we propose algorithms to explicitly construct a conservative estimate of the configuration spaces of rigid objects in 2D and 3D. Our approach is able to detect compact path components and narrow passages in configuration space which are important for applications in robotic manipulation and path planning. Moreover, as we demonstrate, they are also applicable to identification of molecular cages in chemistry. Our algorithms are based on a decomposition of the resulting 3 and 6 dimensional configuration spaces into slices corresponding to a finite sample of fixed orientations in configuration space. We utilize dual diagrams of unions of balls and uniform grids of orientations to approximate the configuration space. We carry out experiments to evaluate the computational efficiency on a set of objects with different geometric features thus demonstrating that our approach is applicable to different object shapes. We investigate the performance of our algorithm by computing increasingly fine-grained approximations of the object's configuration space.


Introduction
A basic question one may ask about a rigid object is to what extent it can be moved relative to other rigidly fixed objects in the environment. In robotic manipulation, this has lead to the notion of a cage. An object is considered caged by rigid fixtures, or a robotic manipulator, if it cannot be moved arbitrarily far from its initial configuration. In terms of the configuration space of the rigid object, this is equivalent to being able to answer whether an object is located in a configuration contained in a bounded path component of its configuration space. Similarly, in fields such as chemistry and biology, this notion of a cage is a useful basic concept for predicting how molecules can restrict each other's mobility which has important applications to drug delivery and related problems (Mitra et al. 2013;Rother et al. 2016).
The main challenge in caging verification is that the configuration space of a rigid object in 3D is in general a 6 dimensional subset of SE(3). For this reason, explicit reconstruction of the configuration space in terms of higherdimensional analogues of discretization techniques such as voxel-grids and triangulations has been considered a computationally infeasible approach to this problem (Makita and Wan 2017). Past work has instead focused on analyzing caging configurations either for 2D objects only McCarthy et al. (2012), or for specific 3D objects with special geometric properties, such as handles and narrow parts Varava et al. 2016), which allow one to avoid modelling the configuration space directly.
We study caging as a special case of proving path non-existence 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 slices of the object's collision space to subspaces corresponding to fixed orientations of the object, see Fig. 1. We then compute the free space approximation as the complement to the collision space of each slice. By construction, our collision space approximation is a proper subset of the real collision space, which implies that when our algorithm reports that the two configurations are not path-connected, 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.
The key contribution of the work we present here is to show that it is possible to compute explicit approximations of configuration spaces of generic rigid bodies in 3D relatively efficiently, while maintaining provability guarantees with respect to reasoning about caging and, more generally, path existence. Our technique for constructing such an approximation is based on the following key insights: • Representation: We utilize a union-of-balls based object and obstacles representation that allows one to utilize dual diagrams to approximate the free space in a provably correct manner. • Slicing: We use an approximate configuration space decomposition based on locally fixed orientations. Figure 1. Diagram of our method. We approximate the collision space of an object by choosing a finite set of fixed object's orientations and considering the corresponding slices of the collision space to R n (n ∈ {2, 3}). From the collision space slices we compute approximations of the free space slices. Finally, we analyze the connectivity between neighboring slices to get an approximation of the connected components of the entire free space.
• Object shrinking: We use a subset of the object (its ε−core), which makes it possible to construct a slicedbased approximation of its configuration space. • Parallelization: Our approach allows for parallel slice computation, leveraging modern CPU architectures.
The presented work constitutes an extended version of our initial work at WAFR'18 (Varava et al. 2018). It features an revised introduction and algorithms description, as well as extended experimental results, a parallelized version of the presented algorithm and its evaluation on 3D models of real objects.

Related Work
In manipulation, caging can be considered as an alternative to a force-closure grasp (Makita and Maeda 2008;Makita et al. 2013;Pokorny et al. 2013;Varava et al. 2016), as well as an intermediate step on the way towards a form-closure grasp . 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 (1990) as a set of n points lying in the complement of a polygon and preventing it from escaping 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 (1999); Pipattanasomporn and Sudsang (2006); Vahedi and van der Stappen (2008), and others. A similar approach has also been adopted for caging 3D objects. For instance, Pipattanasomporn and Sudsang (2011) proposed an algorithm for computing all two-finger cages for non-convex polytopes. Pereira et al. (2004) and Wang and Kumar (2002) present a set of 2D cagingbased 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. (2013); Stork et al. (2013); Varava et al. (2016); Makita and Maeda (2008); Makita et al. (2013). In these works, sufficient conditions for caging were derived for objects with particular shape features. Makapunyo et al. (2013) 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 escape motions may be used as cages in practice.
We address caging as a special case of the path nonexistence problem: an object is caged if there is no path leading it to an unbounded path-connected component. The problem of proving path non-existence has been addressed by Basch et al. (2001) in the context of motion planning, motivated by the fact that most modern sampling-based planning algorithms do not guarantee that two configurations are disconnected, and rely on stopping heuristics in such situations (Latombe 1991). Basch et al. (2001) provide an algorithm to prove that two configurations are disconnected when the object 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. Zhang et al. (2008) 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. McCarthy et al. (2012) propose a related approach. There, they randomly sample the configuration space of a planar rigid object and reconstruct its approximation as an alpha complex. They later use it to check the connectivity between pairs of configurations. This approach has been later extended to planar energy-bounded caging by Mahler et al. (2016).
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 the survey by Wise and Bowyer (2000). Lozano-Perez (1983) introduced the idea of slicing along the rotational axis. To connect two consecutive slices, the authors proposed to use the area swept by the robot rotating between two consecutive orientation values. Zhu and Latombe (1991) extended this idea and used both outer and inner swept areas to construct a subset and a superset of 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 (Behar and Lien 2013;Milenkovic et al. 2013). Behar and Lien (2013) proposed a method towards exact computation of the boundary of the collision space. Milenkovic et al. (2013) explicitly compute the free space for complete motion planning.
Thus, several approaches to representing configuration spaces of 2D objects, both exact and approximate, have been proposed and successfully implemented in the past. The problem is however more difficult if we consider a 3D object, as its configuration space is 6-dimensional. In the recent survey on caging by Makita and Wan (2017), the authors hypothesise that recovering a 6D configuration space and understanding caged subspaces is computationally infeasible. To the best of our knowledge, our paper presents the first practical and provably-correct method to approximate a 6D configuration space.
Our approximation is computed by decomposing the configuration space into a finite set of lower dimensional slices. Although the idea of slicing is not novel and was introduced by Lozano-Perez (1983), recent advances in computational geometry and topology, as well as a significant increase in processing power, have made it possible to approximate a 6D configuration space on a common laptop. We identify two main challenges to slicing a 6D configuration space of a rigid object: how to quickly compute 3D slices of the free space, and how to efficiently discretize the orientation space. For slice approximation, our method relies on the fact that the collision space associated to a rigid object with a fixed orientation and an obstacle represented as a union of balls is itself a union of balls. Then we use the dual diagram to a union of balls presented by Edelsbrunner (1999) as an approximation of the free space of the slice. 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. To discretize SO(3), we use the method by Yershova et al. (2009), which provides a uniform grid representation of the space. The confluence of these factors results in overcoming the dimensionality problem without losing necessary information about the topology of the configuration space, and achieving both practical feasibility and theoretical guarantees at the same time.
Finally, our method does not require precise information about the shape of objects and obstacles, and the only requirement is that balls must be located strictly inside them, which makes our approach more robust to noisy and incomplete sensor data.
We implemented our algorithm to approximate 3D and 6D configuration spaces and verify that it has in practice a reasonable runtime on a single core of Intel Core i7 processor. We provide a parallel implementation which makes use of modern parallel CPU architectures and investigate the effect of parallelization on the runtime of the algorithm.

Objects and obstacles
For the sake of generality, in this paper we use the terms 'object' for objects and autonomous rigid robots (e.g., disc robots) moving in n-dimensional workspaces, where n ∈ {2, 3}. Similarly, we use the term 'obstacle' for everything that restricts mobility of the object -e.g., manipulators, walls, other rigidly fixed objects, etc.
When formally defining an object and a set of obstacles, we make a few mild assumptions to define a large class of shapes and include most of the regular objects in the real world. Since we want to represent both the object and the obstacles as a set of n−dimensional balls, we do not allow them to have 'thin parts'. Formally, we assume that they can be represented as regular sets : Definition 1. A set U is regular if it is equal to the closure of its interior: U = cl(int(U )).
In the above definition, the interior of U is the largest open set contained in U , and the closure of int(U ) is the smallest closed set containing int(U ). In this paper, we assume that both the object and the obstacles are regular sets. Now, we define an object and a set of obstacles as follows: Definition 2. A rigid object is a regular compact connected non-empty subset of R n . A set of obstacles is a regular compact non-empty subset of R n .
We approximate both the obstacles, SS and the object, O as unions of balls which lie in their interior, that is, . . , R n and r 1 , . . . , r m respectively.
Let C(O) = SE(n) denote the configuration space of the object. We define its collision space * C col (O) as the set of the objects configurations in which the object penetrates the obstacles: Note that this definition allows the object to be in contact with the obstacles. To compute path-connected components of the free space, we decompose the free space into a set of n-dimensional slices.

Slice-based representation of the C-space
In our previous work (Varava et al. 2017), we suggested that configuration space decomposition may be a more computationally efficient alternative to its direct construction. We represent the configuration space of an object as a product C(O) = R n × SO(n), and consider a finite covering of SO(n) by open sets (this is always possible, since SO(n) is compact): SO(n) = i∈{1,...,s} U i . We recall the notion of a slice (Varava et al. 2017): Definition 5. A slice of the configuration space C(O) of a rigid object, is a subset of C(O) defined as follows: We denote a slice of the collision (free) space by Sl col U (O) (Sl f ree U (O), respectively). For each slice, we construct an approximation aSl col U (O) of its collision space in such a way that our approximation lies inside the real collision space of the slice, aSl col U (O) ⊂ Sl col U (O). This way, we approximate the entire collision space by a subset aC col (O): Now, we discuss how to construct slice approximations.
3.3 An ε−core of the object Therefore, if some subset O 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 aSl col U in such a way that for any configuration c ∈ aSl col U there exists a subset O of c(O) such that O is in collision, then c(O) is also in collision. In our previous work (Varava et al. (2017)) we defined an ε−core of an object as follows:  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 that since the orientation is fixed, we can identify C col (O φ ε ) with a subset of R n .
In Varava et al. (2017), we showed that for an object O, ε > 0 and a fixed orientation φ ∈ SO(n) there exists a non-empty neighbourhood U (φ, ε) of φ such that for any θ ∈ U (φ, ε), O φ ε is contained in O θ , see Fig. 2. In Sec. 5, we address the problem of representing and discretizing the space of orientations SO(n), and show how it is related to the notion of ε−core.

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.
Definition 7. We say that two configurations are δconnected if and only if there exists a collision-free path of clearance at least ε connecting these configurations.
Consider a superset of our object O, defined as a set of points lying at most at distance δ from O, and let us call it a δ-offset of the object: Equivalently to Def. 7, we can say that two configurations c 1 and Consider now the following modification of our algorithm. If we enlarge the ε−core by a δ−offset, then our rotated object will not be guaranteed to contain it anymore, but the distance between any point of the enlarged core that is located outside of the rotated object and the object itself will not exceed δ. This means that if for this enlarged core our algorithm reports that two configurations are disconnected, then they either are disconnected in reality, or can be connected by a path of clearance at most δ. Let us denote the resulting space approximation by aC f ree ε,δ (O). One application of this observation is narrow passage detection. One can use our free space approximation to identify narrow passages as follows. If two configurations are disconnected in aC f ree ε,δ (O), but connected in aC f ree ε (O), then they are connected by a narrow passage with clearance at most δ. Our approximation then can be 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: assume we want to use a coarse discretization of the orientation space, and therefore the distance between adjacent orientations is large. This will require a larger ε, in which case some important information about the object's shape might not be captured by the ε−core. This might lead to a very conservative approximation of the free space. If now we consider aC f ree ε,δ (O), we might get more caging configurations. These results will be considered with respect to the used parameter δ: if two configurations are disconnected in aC f ree ε,δ (O), then the maximum possible clearance of a path connecting them in reality is at most δ. † By distance here we mean Euclidean distance in R n This leads us to a tradeoff between the desired accuracy of our approximation expressed in terms of clearance δ, and the number of slices that we are willing to compute. The fewer slices we use, the larger δ we will need to consider.

Discretization of SO(n)
Elements of SO(n) can be seen as parametrizing rotations in R n , and for any q ∈ SO(n) we define R q as the associated rotation. The notion of displacement of a point after applying a rotation helps us to understand how the size of ε−core is related to the discretization of SO(n): Our goal now is to derive upper bounds for maximum displacement of any point in the object to make sure that the ε−core always remains inside the object when it is being rotated between different orientations belonging to the same slice, and how to efficiently discretize the orientation space.

Displacement in 2D
In our previous work (Varava et al. (2017)), we derived the following upper bound for the displacement of a twodimensional object: assuming that we rotate the object around its geometric center, rad(O) denotes the maximum distance from it to any point of the object, and q is the rotation angle.
In the two-dimensional case, discretization of the rotation space is simple: given a desired number of slices, we obtain the displacement D(R q ) induced by rotation between two neighboring orientations, and compute a set of orientation samples {φ 1 = 0, φ 2 = 2q, . . . , φ s = 2(s − 1)q}, where s = π/q . Then, we choose the ε > D(R q ). This gives us a covering

Displacement in 3D
We 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 notation from complex numbers to quaternions, where q and q denote the real and "imaginary" parts of the quaternion q. Further, we identify q = q i i + q j j + q k k with the vector (q i , q j , q k ) ∈ R 3 ; and we writeq, and |q| to mean the conjugate q − q and the norm √ qq, respectively. A unit quaternion q defines a rotation R q as the rotation of angle θ q = 2 cos −1 ( q) around axis w q = q | q| . This allows one to calculate the displacement of the rotation D(q) = D(R q ) as: We use the angular distance (Yershova et al. (2009)) to define the distance between two orientations: where x and y are two elements of SO(3) represented as unit quaternions which are regarded as vectors in R 4 , and x, y is their inner product. We define the angle distance from a point to a set S ⊆ SO (3)  Intuitively, the dispersion of a set ∆(S) determines how far a point can be from S, and in this way 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) = max q∈S D(pq), we want to show that in these conditions, there exists some small ε such that max p∈SO (3) (3) and for any q ∈ U p satisfies D(qp) < ε. 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: (1) The proof of Proposition 1 can be found in appendix. This means that if we want to cover SO(3) with patches U i centered at a point p i such that D(p iq ) for any q ∈ U i is smaller than some ε, we can use any deterministic sampling scheme on SO(3) (e.g. (Yershova et al. 2009)) to obtain a set S with dispersion ∆(S) < arcsin( ε 2 rad(O) ). Finally, by considering patches of the form U (s, ε) = {p ∈ SO(3)|ρ(s, p) < ∆}, we obtain the corollary: Given such a cover of SO (3), recall that we want to approximately reconstruct the full free space of the object C f ree as the union of slices aSl f ree U (s,ε) . This requires us to test whether the orientation components of two slices overlap. To make this efficient, we create a slice adjacency graph, which is simply a graph with vertices S and edges (s, To compute the graph adjacency, we note that if two slices U (s, ε), U (s , ε) overlap, there must exist some p ∈ SO(3) such that ρ(p, s), ρ(p, s ) < ∆, which implies ρ(s, s ) < ρ(s, p) + ρ(s , p) < 2∆. This is leveraged in Alg. 1.

Algorithm 1: ComputeAdjacentOrientations
input : S -a set of points in SO (3). ∆ -dispersion of S. output: G -a patch adjacency graph.
The algorithm starts by setting V = S, as this is the set of vertices in the graph, 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∆ § in line 5. Finally, in line 6 we also add edges to the points at an angle distance smaller than 2∆ of −p, as both p and −p represent the same orientation.

Different resolutions and dispersion estimation
Algorithm 2: Approximating the dispersion of a set of points S ⊂ SO(3) input : S -Points in SO (3).
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 designed by Yershova et al. (2009) to compute S where the authors provide a provable upper bound for dispersion.
However, since this is an upper bound, and we want a tighter estimate of the dispersion to reduce the number of slices, in our implementation we employed a random sampling method to estimate the dispersion.
Alg. 2 summarizes the procedure by which we approximate the dispersion. On each iteration of the main loop (lines 3-6) it retrieves an element of SO(3) which is locally the furthest from S this is done by drawing a random sample followed by gradient ascent in line 4. In line 5 the nearest element to w of S is retrieved (since the distance to this element will be the distance from w to S), and finally in 6 we use the fact that w − v = 2 sin( ρ(w,v) 2 ) to update the current value of the dispersion.

The choice of ε and SO(3) discretization in practice
Depending on the chosen discretization of the orientation space and its dispersion, the ε value can be chosen such that it is greater than the estimated displacement: To discretize SO (3), we compute 3 different grids corresponding to different resolution levels using the software provided by Yershova et al. (2009). The first grid contains 576 vertices, and the respective dispersion estimate is 0.23. The second grid consists of 4608 vertices and its estimated dispersion is 0.10, and the third grid has 36864 vertices and a disperion estimate of 0.05. Fig. 3 illustrates how much an object rotates between two neighboring orientations in each of these grids. In our experiments, we analyze how the performance of the algorithm is affected by the choice of a grid.

Free Space Approximation
Let us now discuss how we connect the slices to construct an approximation of the entire free space, see Alg. 3.
Let G(aC col ε (O)) = (V, E) be a graph approximating the free space. The vertices of G correspond to the connected components {aC i 1 , . . . , aC i ni } of each slice, i ∈ {1, . . . , s}, and are denoted by v = (aC, U ), where aC and U are the corresponding component and orientation interval. Two vertices representing components C p ⊂ aSl f ree Ui and C q ⊂ aSl f ree Uj , i = j, are connected by an edge if the object can directly move between them. For that, both the sets U i , U j , and aC q , aC p must overlap: U i ∩ U j , aC q ∩ aC p = ∅. G(aC col ε (O)) approximates the free 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 c 1 , c 2 , then they are disconnected in C f ree (O).
We start by choosing one orientation and run a breadthfirst search over the orientation grid Q. When reaching a particular orientation, we construct the slice corresponding to it. In line 14, we compute the free space of a slice as explained in Alg. 4. In line 18, we check which connected components of adjacent slices overlap, and add edges between them, see Alg. 5.
In order to query connectivity, the slices approximations should be preserved. In our current implementation, each slice is deleted as soon as the connectivity check between it and the slices adjacent to it is performed, in order to optimize memory usage. In this case, one can save the slice approximation together with the resulting graph to disk, for later use by a querying algorithm. § Since the KDT ree T uses the Euclidean distance in R 4 we employ the ). Figure 3. In this figure we show an approximation of a drill as a collection of balls around its medial axis. This representation is visualized at two distinct orientations which are adjacent in the graph over SO (3). The labels underneath each subfigure show how many vertices the corresponding graph has. As expected the larger the grid, the denser the sampling of SO(3) and the closer the two objects are to each other. On one extreme we observe that the overlap between the object in the first grid is very limited and therefore the obtained connectivity graph should not be taken to be very informative. On the other extreme, in the third grid, we see that the overlap between adjacent orientations is almost complete.
Algorithm 3: ComputeConnectivityGraph input : O obst , O obj -spherical representations of the obstacles and the object. δ -clearance parameter Q -a grid over SO(3). output: G -a connectivity graph of the free space.

Construction of Slices
Now, let us discuss how we approximate path-connected components of the free space of each slice, see Alg. 4. Given a set of obstacles, an object, and a particular orientation of its ε−core, we start by computing the collision space of the slice.
In Varava et al. (2017), we derive the following representation for the collision space of O φ ε : where G is the origin chosen as the geometric center of the object, and GY i denotes the vector from G to Y i . Indeed, 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 the collision spaces of the balls shifted with respect to the position of the balls centers: Now, each 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 of a single ball of radius r i − ε can be written as: Algorithm 4: ComputeSlice input : O obst , O ε -spherical representations of the obstacles and the ε−core. q -orientation. output: aSl f ree -A set of connected components of the slice corresponding to q At the next step, we approximate the free space of the slice. For this, we approximate the complement of the collision space by constructing the dual diagram (Edelsbrunner (1999)) to the set of balls representing the collision space. A dual diagram Dual( B i ) of a union of balls B i is a finite collection of balls such that the complement of its interior is contained in and is homotopy equivalent to B i . It is convenient to approximate C f ree (O φ ε ) as a dual diagram of collision space balls for several reasons. First, balls are simple geometric primitives which make intersection checks trivial. Second, the complement of the dual diagram is guaranteed to lie strictly inside the collision space, which provides us with a conservative approximation of the free space. Finally, homotopy equivalence between its complement and the approximate collision space implies that our approximation preserves the connectivity of the free space that we want to approximate. Another advantage of a dual diagram is that it is easy to construct.
A weighted Voronoi diagram is a special case of a Voronoi diagram, where instead of Euclidean distance between points, a special distance function is used. In our case, the weighted distance of a point x ∈ R n from B ri (z i ) is equal to d w (x, B ri (z i )) = ||x − z i || − r 2 i . To construct a dual diagram of a union of balls i B ri (z i ), we first construct their weighted Voronoi diagram, see Fig. 4. For each vertex y j of the weighted Voronoi diagram, let B qj (y j ) whose radius q j is equal to the square root of the weighted distance of y j to any of the four (three in the two-dimensional case) balls from i B ri (z i ) generating y j . 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. The entire process of dual diagram construction can be seen on Fig. 5.
After constructing the dual diagram, we find pairs of overlapping balls in it and run depth-first search to identify connected components. It is important to note that each dual diagram always has exactly one unbounded connected component, representing the outside world.
Finally, let us discuss how to understand whether free space approximations of neighboring slices overlap. In Alg. 5, to check whether two connected components in adjacent slices intersect (line 8), we recall that they are just finite unions of balls. Instead of computing all pairwise intersections of balls, we approximate each ball by its bounding box and then use the CGAL implementation of Zomorodian's algorithm (Zomorodian and Edelsbrunner (2000)), which efficiently computes the intersection of two sequences of three-dimensional boxes. Every time it finds an overlapping pair of boxes, we check whether the respective balls also intersect.

Algorithm 5: AddEdges
input : aSl f ree Ui , aSl f ree Uj -free space approximations of two adjacent slices. output: E ij -a set of edges between the connected components of aSl f ree Ui and aSl f ree Uj . Remark 1. Recall that in each slice the aC f ree (O φi ε ) are constructed as the dual of the collision space C col (O φi ε ), which entails that aC f ree (O φi ε ) has the same connectivity 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 ε ). This means that for two connected components C i j , C i j of adjacent slices which do not overlap, it may occur that the corresponding approximations aC i j , aC i j 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 both slices. This can be done for example by covering the intersection aC i j ∩ aC i j with a union of balls and checking if it is contained inside the collision space

Parallel implementation
To increase the performance of our algorithm, we designed and implemented a parallelized version that uses multiple threads to compute different slices simultaniously. Alg. 6 describes the process.
Recall that in the single-thread version of the algorithm (see Alg. 3), we performed breadth-first search (BFS) over the orientation grid Q. For each orientation, we computed the corresponding slice of the free space and its overlap with neighboring slices. The slice was deleted when all its neighbours were constructed and the connectivity check between them was performed.
A naive approach to parallelization would have a main thread call child threads on to process each new orientation in the queue, however since the orientations are enqueued in sequence it would increase the chances that different Figure 5. From left to right, the elements of the dual diagram approximating the free space. We start with building a Voronoi diagram (visualized in black); the second figure shows a set of finite orthogonal balls, corresponding to the regular edges of the diagram; the third figure visualizes the "infinite" orthogonal balls corresponding to the infinite edges of the diagram; finally, the last figure depicts full diagram. Red circles with blue centers represent the collision space, the yellow circles approximate the dual diagram.
threads try to compete for computing the same slice data, necessitating threads to wait for each other to finish. Instead we opted for a simple scheme where each thread performs its own BFS from a different point in the tree taking care to lock access to resources every time shared data is accessed.
Alg. 6 describes the procedure followed by each thread. It assumes that all threads share an access to the array of slices and the connectivity graph, as well as the book-keeping data, i.e. an array of mutexes, and an array containing the status of each slice. The status array (S) is used to decide what (if anything) should be computed about a given slice at a given moment, whereas the mutex array (M) is used so two threads don't try to alter the same slice at the same time.
The main loop (lines 4-33) proceeds as follows: it pops the first element of the queue q cur and locks its corresponding mutex. If it cannot obtain ownership of the mutex, it waits until this is freed by the other thread. It then verifies the status of the current orientation (lines 7 and 11), if the slice associated to the orientation has yet to be computed it does so (line 8) and if its edges have not yet been processed it goes on to process the edges to its neighboring slices (lines 13-31). To compute the edges between the slices corresponding to the current orientation q cur and an adjacent orientation q adj the thread needs to also lock the mutex associated to q adj (line 14) if it does so successfully then it proceeds to check the connection between q cur and q adj , otherwise it checks the next adjacent orientation. When it has tried to calculate the edges to all the orientations adjacent to q cur it verifies if all of them were successfully calculated (line 28), if so, the current orientation is marked as fully processed and the slice is deleted. Note also that the edges between Slice[q cur ] and Slice[q adj ] only get computed in line 22 if Slice[q adj ] has not been fully processed itself, as that would mean that the existing edges would have been processed in a previous step.
This procedure guarantees the absence of data corruption since in order to change the data associated to slices or the edges between them, the corresponding mutexes need to be locked first. Furthermore the algorithm is guaranteed to be lock-free given that: • If two threads try to lock the same q cur , the second one has to wait for the first one to finish before it can proceed. • If there is an edge (u, v) of the orientation graph and one thread has q cur = v and a second thread has q cur = u, then given that both instances only try to lock the mutex of the corresponding to the other thread's orientation, they will not cause a deadlock.
Note also that each thread is guaranteed to terminate since orientations only get enqueued if their status is unseen (S[q adj ] = 0 in line 16) and this status is revoked immediately afterwards (line 18) before it is enqueued. Furthermore since this occurs while the mutex associated to the corresponding slice is locked, no other thread is able to enqueue the same orientation. This guarantees that each orientation is only enqueued once.
Alg. 6 is called by a threadpool as shown in Alg. 7. The algorithm ComputeConnectivityGraphParallel works by setting up the data that must be shared between the several instances of ConnectivityGraphThread, and calling it starting from different orientations in Q. Once all slices have been constructed the algorithm proceeds by checking that they have all been completely processed, and otherwise checks the intersections with their remaining neighbors. This step is required in case the situation arises where two threads are processing neighboring orientations simultaneously. In this case they may fail to lock the other's mutex and therefore ignore the edges that may exist between their corresponding vertices. However since this failure implies that neither orientation is marked as fully processed, the corresponding slices are not freed in line 30 of Alg. 6, and are therefore available to be further processed in line 18 of Alg. 7.
The function SelectQuaternion is used to select the next quaternion which has not been seen before. In our implementation we use a simple scheme by drawing a random number r and choosing the first i > r so that S[i] = 0.

Theoretical Properties of Our Approach
In this section, we discuss correctness, completeness and computational complexity of our approach.

Correctness
First let us show that our algorithm is correct: i.e., if there is no collision-free path between two configurations in our approximation of the free space, then these configurations are also disconnected in the actual free space.
Consider an object O and a set of obstacles SS. Consider two collision-free configurations of the object. If they are not path-connected i -an initial orientation. shared: M -an array containing a mutex per slice.
Slices -an array of slices (one per orientation).
G -a connectivity graph of the free space Proof. Recall that the approximation of the free space is constructed as follows: Now, recall that by definition (Dual(C col (O φi ε ))) c ⊂ C col (O φi ε ) (Edelsbrunner (1999)), and that we choose ε and U (φ i , ε) so that for any we have: We now want to show that if there is no path between two vertices v = (aC, U ) and v = (aC , U ) in G(aC f ree ε ), then there is no path between connected components of aC f ree ε (O) corresponding to them. It is enough to show that if two vertices corresponding to adjacent slices are not connected by an edge, then they represent two components which are disconnected in Sl f ree Consider two adjacent slices Sl U (φi,ε) and Sl U (φj ,ε) , and two path-connected components C 1 ⊂ aSl f ree U (φi,ε) and C 2 ⊂ aSl f ree U (φj ,ε) . Let aC 1 and aC 2 be their respective representations as unions of balls.

δ−Completeness
Now, we show that if two configurations are not pathconnected 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.
Proposition 3. δ-completeness. Let c 1 , c 2 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.
In proving this proposition we make used of the notion of the signed distance between two sets: Where ∂SS denotes the boundary of SS. Note that d s (A, B) is not necessarily the same asd s (B, A) so we instead consider d s (A, B) = min(d s (A, B),d s (B, A)).
Recall now that we want to prove that for a pair of configurations c 1 , c 2 which are not path-connected in C f ree (O), then for any δ > 0 there exists some ε > 0 so that they are not path-connected in G(aC f ree ε (O +δ )) for. Therefore, we start by noting that since c 1 and c 2 are not path-connected there exists a collision configuration c in any path between them, which implies d s (c(O), SS) < 0. Thus, for the same configuration c we have d s (c(O +δ ), SS) < −δ.
To see that this will result in path non-existence, we take an arbitrary ε > 0 and consider the collision space aC col ε (O +δ ). Further, we let c = (p, φ) ∈ R 3 × SO(3), and for any φ i such that φ ∈ U (φ i , ε), we define c i = (p, φ i ). Finally, we restrict ourselves to the case where ε < δ and define δ = δ − ε, then we get: Where the term ε 1 + δ rad (O) corresponds to the maximum displacement of the object offset O +δ associated to a rotation of O with maximum displacement ε. Which implies that, as long as we choose ε such that then c is in collision and the proof will be concluded. To see that this inequality has a solution with 0 < ε < δ we simply note that with ε = 0 it is reduced to −δ < 0. And the roots of the equation in terms of δ are given by: which are both positive. Finally call the smallest of the two rootsε and the inequality holds as long as ε ∈ (0, min{ε, δ)}, concluding the proof.

Computational complexity
Let us now discuss the total computational complexity of the algorithm. Let s be the number of slices, and let n and m be the number of balls in the object's and the obstacle's spherical representation, respectively. We have pre-computed the grids over SO(3) corresponding to different dispersion values, and therefore we are only interested in the complexity of the connectivity graph construction. For each slice, we execute two computationally expensive procedures: we compute a weighted Voronoi diagram of the collision space, which allows us to extract the balls representation of the free space, and then for each slice we compute its intersections with adjacent slices. In practice, each orientation in Q has around 20 adjacent orientation values, so each slice has around 20 neighbours ¶ .
In CGAL representation, the regular triangulation contains the corresponding weighted Voronoi diagram. Note that a weighted Voronoi diagram can be constructed by other means using for example the algorithm from (Aurenhammer et al. (1984)). The complexity of this step is O(n 2 m 2 ). The computation of the connected components of each slice is linear on the number of balls in the dual diagram, which makes the overall complexity of this step O(n 2 m 2 ).
The complexity of finding the intersections between two connected components belonging to different slices O(b log 3 (b) + k) in the worst-case Zomorodian and Edelsbrunner (2000), where b is the number of balls in both connected components, and k the output complexity, i.e., the number of pairwise intersections of the balls.
The complexity of the final stage of the algorithmcomputing connected components of the connectivity graph -is linear on the number of vertices, and can be expressed as O(s c), where c is the average number of connected components per slice (a small number in practice).

Examples and Experiments
In this section, we test our algorithm on 2D and 3D objects. In the first experiment, we investigate how the quality Figure 6. 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 features from the original configuration space are preserved. Note that the first approximation significantly simplifies the shape, and has only one narrow passage; the second approximation preserves the shape better and has two narrow passages; the third approximation preserves all the important shape features of the obstacles.
of the spherical approximation of the workspace affects the output of our algorithm. In the next experiment, we test our algorithm on a set of 3D objects representing different classes of geometric shapes. In our final experiment we investigate how the performance of our parallel implementation changes with respect to different numbers of threads.

Object and obstacle approximation
A key requirement of our method is the approximation of both objects and obstacles as unions of balls. This presents a challenge especially when dealing with 3D objects. For 2D experiments we were able to obtain good results using a simple grid for the centers of the balls, and choosing the radius of the balls at various levels. This resulted in the approximations of the obstacle seen in Fig. 6 and the results in Tab. 1 which will be analyzed in the next section. Simply put this example shows that using this approach, the smaller the radius of the balls used in the approximation the higher detail we are able to capture. However this method can be detrimental to approximate the object, because we use an ε-core, where ε depends on the dispersion of the grid over SO(n) and the radius of the object and may therefore exceed the necessary ball radius to approximate all the required details.
A more suitable approach to approximate the object is to compute the medial axis of the object using for example the method from (Dey et al. 2006) which is able to construct it from a pointcloud of the object. Alternatively one can use the method from (Dey et al. 2002) which is able to retrieve a skelleton of the object (i.e. a 1-dimensional subset of the medial axis). In our experiments we used a watertight mesh of the drill (see Fig 8) to extract its skelleton axis and obtained an approximation of the drill by sampling ball centers in the skelleton with the largest radius that did not contain any point in the mesh.
Finally, for the mug and the bugtrap (also Fig. 8) we traced a slice of the object and manually fit the medial axis of the slice, which we used to create a surface of revolution where the ball centers were placed (bugtrap and body of the mug).

2D scenario: 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. 6. This experiment was performed on a single thread of Intel Core i7 processor.  Table 1. We report the number of path-connected components we found and the computation time for each case. When we were using our first approximation of the workspace, we were able to distinguish only between components 4 and 2 (see Fig. 6), and therefore prove path non-existence between them. For a more accurate approximation, we were also able to detect component 3. Finally, the third approximation of the workspace allows us to prove path non-existence between every pair of the four components.
For our experiments, we generate a workspace and an object as polygons, and approximate them with unions of balls of equal radii lying strictly inside the polygons. Note that the choice of the radius 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. 6.
We consider a simple 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 observe that 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 last column of Tab. 1).
In our opinion, the accuracy of a workspace approximation should depend on the application scenario as well as on the amount of avalibale computational resources.

3D scenario: Different Types of 3D Caging
As we have mentioned in Sec. 2, a number of approaches 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 to consider examples of objects of different types and run our algorithm on them. The examples are depicted on Fig. 7, and Tab. 2  2018)), surrounding-based caging.In linking-based and narrow part-based caging the obstacles form a loop (not necessarily closed) around a handle or narrow part of the object. In surrounding-based the object is surrounded by obstacles so as not to escape.
reports execution time for different resolutions of the SO (3) grid.
In all cases (Tab. 2), the object was shown to be caged when using the third grid. In the case of the narrow part example the algorithm was not able to discern a cage using the first grid. In the rings example it was not able to find the cage with either the second or third grids.
Results that are based on low-resolution grids can in certain cases be inaccurate for two reasons. First, the they have higher dispersion value, which implies that we need to use a larger value of ε. This results in a smaller ε−core of the object which can escape from a cage even if the entire object cannot. This effect can be mitigated by considering a sufficient clearance δ. Thus, 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 by Yershova et al. (2009), and in our experiments we consider 3 different predefined resolutions. Using a different SO(3)discretization strategy and given a concrete clearance value δ, one can potentially construct a grid on SO(3) with the necessary resolution based on δ.
Another reason for inaccuracies in the low-resolution grid is the fact that the distance between neighboring orientations is so large that adjacent slices might have drastic differences in the topology of their free space. This may lead to adding edges between their connected components that would not be connected if we considered intermediate orientations as well in the case of a more fine grid.
As we see, our algorithm can be applied to different classes of shapes. It certainly cannot replace previous shapebased algorithms, as in some applications global geometric features of objects can be known a priori, in which case it might 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.

Parallel implementation
Our parallel implementation allows one to run experiments on more complex models of real world objects. In this section, we consider a few objects representing different geometric features, see Fig. 8. These examples are inspired by manipulation and motion planning applications. The first two models, a mug and a drill, are caged by a Schunk dexterous hand. The mug and the drill illustrate linkingbased and narrow part-based caging types, respectively. The third example is a bugtrap and a small cylindrical object. The bugtrap environment is a well-known path planning benchmark created by Parasol MP Group, CS Dept, Texas A & M University. The bugtrap does not cage the object, but contains a narrow passage. This example corresponds to the "surrounding" type of restricting the object's mobility.
First, we observe how the performance of our algorithm changes with respect to the number of slices we consider. Table 3 reports the results. The respective objects are depicted on Fig. 8. We report the computational time needed for different resolutions of the orientation grid, as well as the δ value passed to the algorithm. The different δ values correspond to mantaining a constant value of ε across the different grids. In all of the examples, the algorithm was able to detect compact connected components, thus indicating that the objects are caged with clearance δ. In the case of a bugtrap, we were able to detect a narrow passage whose width is at most 0.2. However, when running the same example with clearance parameter δ = 0, the algorithm returned a space approximation with a single connected component, thus indicating that there are no caging configurations. Thus, while the object is not caged by the bugtrap, its free space contains a narrow passage as expected. Now, we analyze how the performance of the parallelized version of our algorithm changes with respect to the number of threads we use. Table 4 presents the results of this experiment. We run our algorithm using from one to eight threads on three different object models and report the respective computational time. In all examples we used 4608 slices (second grid) and clearance δ = 0. We observe a drastic increase of performance up to four threads, and a slight increase when using four to eight threads (see Fig. 9).

Discussion and Future Work
In this paper, we provide a computationally feasible and provably-correct algorithm for 3D and 6D configuration space approximation and use it to prove caging and path nonexistence. We analyze theoretical properties of the algorithm such as correctness and complexity. We provide a parallel implementation of the algorithm, which allows us to run it on complex object models. In the future, we see several main directions of research that would build upon these results. #slices rings (320) narrow part (480) surrounding (266) 576 5 s δ = 3.153 8 s δ = 5.041 9 s δ = 1.390 4 608 45 s δ = 0.958 78 s δ = 1.532 95 s δ = 0.422 36 864 485 s δ = 0.000 674 s δ = 0.000 875 s δ = 0.000 Table 2. Results from running 3D experiments on the objects shown on Fig. 7 using 3 different resolutions for the SO(3) grid with the non-parallelized algorithm. 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 clearance δ used to test whether there is a narrow passage of that clearance for each case. The varying clearance corresponds to using the same value for epsilon on each grid.  Table 3. Results from running 3D experiments with objects shown on in Fig. 8 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.   Fig. 7 and Fig. 8, and a grid over SO(3) comprising 4608 nodes.

Molecular Cages
In organic chemistry, the notion of caging was introduced independently of the concept of Mitra et al. (2013). Big molecules or their ensembles can form hollow structures with a cavity large enough to envelope a smaller molecule. Simple organic cages are relatively small in size and therefore can be used for selective encapsulation of tiny molecules based on their shape Mitra et al. (2013). In particular, this property can be used for storage and delivery of small drug molecules in living systems Rother et al. (2016). By designing the caging molecules one is able to control 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 releasing the drug molecule. An example on Fig. 10 shows that in principle, our algorithm can be applied to molecular models, assuming they can be treated as rigid bodies. In this example, we consider two molecules to see whether our algorithm can predict their ability to form a cage. Atomic coordinates and van der Waals radii were retrieved from the Cambridge Crystallographic Data Centre (CCDC) database. The depicted molecules are mesitylene (left, CCDC 907758) and CC1 (right, CCDC 707056) (Mitra et al. (2013)). Our algorithm reported that this pair forms a cage, as experimentally determined in Mitra et al. (2013). 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. This prediction can later be used as a hypothesis for further laboratory experiments.

Integration with Path Planners
Another possible direction of future work is integration of our approach with sampling-based path planning algorithms. Since the problems of path planning and path non-existence verification are dual to each other, we plan to design a framework where they will be addressed simultaneously: a path planner will attempt to construct a collision-free path as usual, while configuration space approximation will be constructed independently. If there is no path, our configuration space approximation can be used to rigorously demonstrate this instead of relying on heuristical stopping criteria for the planner. Furthermore, we can leverage our approximation to guide sampling, which can be particularly beneficial in the presence of narrow passages. Namely, having an approximation of the narrow regions of the free space, the planner can sample configurations from it. Mahler et al. (2016Mahler et al. ( , 2018 defined the concept of energybounded caging, when obstacles and external forces (such as gravity, or forces directly applied to the object) complement each other in restricting the object's mobility. The authors proposed an approach towards synthesizing energy-bounded cages in 2D.

Energy-Bounded Caging
To directly extend their method to 3D workspaces one would need to represent the configuration space as a 6D simplicial complex, which is expensive in terms of both required memory and execution time (Mahler et al. (2016)). Apart from that, the computation time in their case is dominated by sampling the collision space, and the authors suggest that in the 3D case the necessary number of samples might be even higher.
Analogously to the works by Mahler et al. (2016Mahler et al. ( , 2018), we can model external forces as potential functions and assign potential values to each ball in our approximation of the free space. In each slice, we consider balls with high potential values as "forbidden" parts of the free space, and compute connected components of the rest. In the future, we plan to investigate this direction.