Free space of rigid objects: caging,path non-existence,and narrow passage detection

Abstract

In this work, we propose algorithms to explicitly construct a conservative estimate of the configuration spaces of rigid objects in two and three dimensions. 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 three- and six-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. Furthermore, 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. A multithreaded implementation of our approach is shown to result in significant speed improvements.

Keywords

caging path non-existence computational geometry

1. 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 in 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 three dimensions is, in general, a 6D subset of $SE (3)$ . For this reason, explicit reconstruction of the configuration space in terms of higher-dimensional analogs 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 (Pokorny et al., 2013; Varava et al., 2016), which allow one to avoid modeling 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 corresponding to subspaces of fixed orientations of the object, see Figure 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.

Fig. 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 \in {2, 3}$ ). From the collision space slices we compute approximations of the free space slices. Finally, we analyze the connectivity between neighboring slices to obtain an approximation of the connected components of the entire free 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 three dimensions 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.

Object shrinking: We use a subset of the object (its $ε$ -core), which makes it possible to construct a sliced-based 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 a 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.

2. 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 (Rodriguez et al., 2012). 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 two dimensions 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) presented 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. (2013), Stork et al. (2013), Varava et al. (2016), and 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 non-existence 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) provided 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) used 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) proposed a related approach. There, they randomly sampled the configuration space of a planar rigid object and reconstructed its approximation as an alpha complex. They later used 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 6D. In the recent survey on caging by Makita and Wan (2017), the authors hypothesized that recovering a 6D configuration space and understanding caged subspaces is computationally infeasible. To the best of the authors’ knowledge, we present 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. In this way, we do not need to use generalized polygons, which makes previous approaches more difficult in two dimensions 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.

3. Definitions and notation

3.1. Objects and obstacles

For the sake of generality, in this article we use the terms “object” for objects and autonomous rigid robots (e.g., disk robots) moving in $n$ -dimensional workspaces, where $n \in {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 (Rodriguez and Mason, 2012).

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 article, 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, $S$ and the object, $O$ as unions of balls which lie in their interior, that is, $S = {B_{R_{1}} (X_{1}), \dots, B_{R_{n}} (X_{n})}$ and $O = {B_{r_{1}} (Y_{1}), \dots, B_{r_{m}} (Y_{m})}$ with radii $R_{1}, \dots, R_{n}$ and $r_{1}, \dots, 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.

Definition 3. We have $C^{col} (O) = {c \in C | [int c (O)] \cap [int S] \neq \emptyset}$ , where $c (O)$ denotes the object in a configuration $c$ . The free space $C^{free} (O)$ is the complement of the collision space: $C^{free} (O) = C (O) - C^{col} (O)$ .

Note that this definition allows the object to be in contact with the obstacles.

Definition 4. Two configurations $c_{1}$ and $c_{2}$ are called path-connected if there exists a continuous collision-free path $γ : [0, 1] \to C^{free} (O)$ between them: $γ (0) = c_{1}$ , $γ (1) = c_{2}$ . Two configurations are path-connected if and only if they belong to the same path-connected component.

To compute path-connected components of the free space, we decompose the free space into a set of $n$ -dimensional slices.

3.2. Slice-based representation of the configuration 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} \times 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 \in {1, \dots, 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: $S l_{U} (O) = R^{n} \times U$ , where $U$ is an subset of $SO (n)$ .

We denote a slice of the collision (free) space by ${Sl}_{U}^{col} (O)$ ( ${Sl}_{U}^{free} (O)$ , respectively). For each slice, we construct an approximation ${aSl}_{U}^{col} (O)$ of its collision space in such a way that our approximation lies inside the real collision space of the slice, ${aSl}_{U}^{col} (O) \subset {Sl}_{U}^{col} (O)$ .

In this way, we approximate the entire collision space by a subset $a C^{col} (O)$ :

a C^{col} (O) = (⋃_{i \in {1, \dots, s}} {aSl}_{U_{i}}^{col} (O)) \subset C^{col} (O)

Now, we discuss how to construct slice approximations.

3.3. An $ε$ -core of the object

First observe that by Definition 3, if a subset $O'$ of an object $O$ placed in configuration $c \in 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 $O'$ . This allows us to make the following observation.

Observation 1. Consider an object $O$ and a set of obstacles $S$ . Let $c_{1}, c_{2} \in C^{free} (O)$ be two collision-free configurations of the object. If there is no collision-free path between these configurations for its subset $O' \subset O$ , then there is no collision-free path connecting these configurations for $O$ .

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}_{U}^{col}$ in such a way that for any configuration $c \in {aSl}_{U}^{col}$ 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.

Definition 6. The $ε$ -core of an object $O$ is the set $O_{ε}$ comprising the points of $O$ which lie at a distance ² of at least $ε$ from the boundary of $O$ : $O_{ε} = {p \in O | d (p, \partial O) \geq ε}$ .

Now, for an object $O$ and its $ε$ -core $O_{ε}$ , we write $O^{ϕ}$ and $O_{ε}^{ϕ}$ , respectively, to mean that their orientation is fixed at $ϕ \in SO (n)$ . Thus, 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 $ϕ \in SO (n)$ there exists a non-empty neighborhood $U (ϕ, ε)$ of $ϕ$ such that for any $θ \in U (ϕ, ε)$ , $O_{ε}^{ϕ}$ is contained in $O^{θ}$ , see Figure 2.

Fig. 2.

An $ε$ -core remains inside the object when we slightly rotate it.

In Section 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.

4. 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: $O_{+ δ} = {p \in R^{n} | d (p, O) \leq δ}$ . Equivalently to Definition 7, we can say that two configurations $c_{1}$ and $c_{2}$ are $δ$ -connected in $C^{free} (O)$ if and only if they are path-connected in $C^{free} (O_{+ δ})$ .

Consider now the following modification of our algorithm. If we enlarge the $ε$ -core by a $δ$ -offset, then our rotated object will no longer be guaranteed to contain it, 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 $a C_{ε, δ}^{free} (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 $a C_{ε, δ}^{free} (O)$ , but connected in $a C_{ε}^{free} (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 $a C_{ε, δ}^{free} (O)$ , we might get more caging configurations. These results will be considered with respect to the used parameter $δ$ : if two configurations are disconnected in $a C_{ε, δ}^{free} (O)$ , then the maximum possible clearance of a path connecting them in reality is at most $δ$ .

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.

5. Discretization of $SO (n)$

Elements of $SO (n)$ can be seen as parameterizing rotations in $R^{n}$ , and for any $q \in 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)$ .

Definition 8. Let $D (R_{q})$ denote the maximal displacement of any point $p \in O$ after applying $R_{q}$ , i.e., $D (R_{q}) = max_{p \in O} (d (p, R_{q} (p)))$ , then $O_{ε} \subset R_{q} (O)$ if $D (R_{q}) < ε$ .

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.

5.1. Displacement in two dimensions

In our previous work (Varava et al., 2017), we derived the following upper bound for the displacement of a 2D object:

D (R_{q}) \leq 2 | \sin (q / 2) | \cdot rad (O)

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 2D 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} = 2 q, \dots, ϕ_{s} = 2 (s - 1) q}$ , where $s = ⌈ π / q ⌉$ . Then, we choose the $ε > D (R_{q})$ . This gives us a covering ${U (ϕ_{1}, ε), \dots, U (ϕ_{s}, ε)}$ of $SO (2)$ , where for each $i \in {1, \dots, s}$ we define $U (ϕ_{i}, ε) = [ϕ_{i} - q, ϕ_{i} + q]$ .³.

5.2. Displacement in three dimensions

We now discuss the 3D case. Similarly to the previous case, our goal is to cover $SO (3)$ with balls of fixed radius. To parameterize $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}) \in R^{3}$ ; and we write $\bar{q}$ , and $| q |$ to mean the conjugate $ℜ q - ℑ q$ and the norm $\sqrt{q \bar{q}}$ , respectively.

A unit quaternion $q$ defines a rotation $R_{q}$ as the rotation of angle $θ_{q} = 2 \cos^{- 1} (ℜ q)$ around axis $w_{q} = \frac{ℑ q}{| ℑ q |}$ . This allows one to calculate the displacement of the rotation $D (q) = D (R_{q})$ as

\frac{D (q)}{rad (O)} = 2 \sin (\frac{θ_{q}}{2}) = 2 \sin (\cos^{- 1} (ℜ q)) = 2 | ℑ q |

We use the angular distance (Yershova et al., 2009) to define the distance between two orientations:

ρ (x, y) = \arccos (| 〈 x, y 〉 |)

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 \subseteq SO (3)$ in the usual way as

ρ (S, x) = min_{y \in S} ρ (y, x)

Yershova et al. (2009) provided a deterministic sampling scheme to minimize the dispersion

Δ (S) = max_{x \in SO (3)} ρ (S, x)

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 \subseteq SO (3)$ such that $Δ (S) < Δ$ . Then for any point $p \in SO (3)$ denote $D (p \bar{S}) = max_{q \in S} D (p \bar{q})$ , we want to show that in these conditions, there exists some small $ε$ such that $max_{p \in SO (3)} D (p \bar{S}) < ε$ . This would imply that there exists some $Δ' > Δ$ such that if we take $U = {q \in SO (3) | ρ (1, q) < Δ'}$ , then denoting $U_{p} = {qp | q \in U}$ , the family ${U_{p}}_{p \in S}$ fully covers $SO (3)$ and for any $q \in U_{p}$ satisfies $D (q \bar{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 (p \bar{q}) = 2 \sin (ρ (p, q)) rad (O)

(1)

The proof of Proposition 1 can be found in the 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_{i} \bar{q})$ for any $q \in 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 (\frac{ε}{2 rad (O)})$ . Finally, by considering patches of the form $U (s, ε) = {p \in SO (3) | ρ (s, p) < Δ}$ , we obtain the following corollary.

Corollary 1. If $S \subseteq SO (3)$ has a dispersion $Δ (S) < \arcsin (\frac{ε}{2 rad (O)})$ then the family of patches ${U (s, ε) | s \in S}$ forms a cover of $SO (3)$ .

Given such a cover of $SO (3)$ , recall that we want to approximately reconstruct the full free space of the object $C^{free}$ as the union of slices ${aSl}_{U (s, ε)}^{free}$ . 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, s')$ if $U (s, ε) \cap U (s', ε) \neq \emptyset$ .

To compute the graph adjacency, we note that if two slices $U (s, ε), U (s', ε)$ overlap, there must exist some $p \in SO (3)$ such that $ρ (p, s), ρ (p, s') < Δ$ , which implies $ρ (s, s') < ρ (s, p) + ρ (s', p) < 2 Δ$ . This is leveraged in Algorithm 1.

Algorithm 1: ComputeAdjacentOrientations
input: $S$ — a set of points in $SO (3)$ .
$Δ$ — dispersion of $S$ .
output: $G$ — a patch adjacency graph.
1 $V \leftarrow S$
2 $T \leftarrow KDTree (V)$
3 $E \leftarrow {}$
4 for $p \in V$ do
5 $P \leftarrow T . query (p, dist \leq 2 \sin (Δ)) \ {p}$
6 $P \leftarrow P \cup T . query (- p, dist \leq 2 \sin (Δ))$
7 for $q \in L$ do
8 add $(p, q)$ to $E$
9 end
10 end
11 return $G = (V, E)$

The algorithm starts by setting $V = S$ , as this is the set of vertices in the graph, and we put these vertices in a $KDTree$ in order to quickly perform nearest-neighbor queries. Now, to compute the set of edges $E$ , we locate for each $p \in S$ the points at an angle distance smaller than $2 Δ$ in line 5.4.⁴ 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.

5.3. Different resolutions and dispersion estimation

One of the prerequisites of Algorithm 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.

Algorithm 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 (\frac{ρ (w, v)}{2})$ to update the current value of the dispersion.

Algorithm 2: Approximating the dispersion of a set of points $S \subset SO (3)$
input: $S$ — Points in $SO (3)$ .
output: $Δ$ — Approximate dispersion.
1 $Δ \leftarrow 0$
2 $T \leftarrow KDTree (V)$
3 for $i \in {1, \dots, N}$ do
4 $v \leftarrow \arg max_{w \in SO (3)} ρ (S, w)$
5 $w \leftarrow T . query (v)$
6 $Δ \leftarrow max (Δ, 2 \sin^{- 1} (\frac{∥ w - v ∥}{2})$ )
7 end
8 return $Δ$

5.4. 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:

ε > 2 \sin (\frac{Δ (S)}{2}) rad (O)

To discretize $SO (3)$ , we compute three different grids corresponding to different resolution levels using the software provided by Yershova et al. (2009). The size of the discretizations as well as the associated dispersion in both radians and degrees are shown in Table 1. The dispersion value of 0.23 for grid 1, for example, indicates that given any orientation in $SO (3)$ , it can be transformed into one point on the grid via a rotation of angle, at most $0.23$ rad (which is equivalent to $13.1 °$ ). Similarly for grids 2 and 3, rotations of angles at most $5.7 °$ and $2.8 °$ would suffice.

Table 1.

Dispersion of the grids used to discretize $SO (3)$ , as computed by Algorithm 2.

Grid	Number of vertices	Dispersion (rad)	Dispersion (°)
1	576	0.23	13.1
2	4,608	0.10	5.7
3	36,864	0.05	2.8

Figure 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.

Fig. 3.

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.

6. Free space approximation

Let us now discuss how we connect the slices to construct an approximation of the entire free space, see Algorithm 3.

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.
1 $O_{ε} \leftarrow ComputeEpsilonCore (O_{obj}, δ, Q)$
2 $MarkedOrientations \leftarrow \emptyset$
3 $Queue \leftarrow \emptyset$
4 $q \leftarrow Q [0]$
5 $MarkedOrientations . add (q)$
6 $Slices [q] \leftarrow ComputeSlice (O_{obst}, O_{ε}, q)$
7 $Queue . add (q)$
8 while $Queue \neq \emptyset$ do
9 $q_{cur} \leftarrow Queue . dequeue ()$
10 $CurrentSlice \leftarrow Slices [q_{cur}]$
11 for $q_{adj} \in Q . adjacentOrientations (q_{cur})$ do
12 if $q_{cur} \notin MarkedOrientations$ then
13 if $Slices [q_{adj}] = \emptyset$ then
14 $Slices [q_{adj}] \leftarrow ComputeSlice (O_{obst}, O_{obj}, q)$
15 $Queue . enqueue (q_{cur})$
16 $MarkedOrientations . add (q_{cur})$
17 end
18 $AddEdges (Slices [q_{cur}], Slices [q_{adj}])$
19 end
20 end
21 $Slices [q_{cur}] . reset ()$
22 end
23 return $G$

Let $G (a C_{ε}^{col} (O)) = (V, E)$ be a graph approximating the free space. The vertices of $G$ correspond to the connected components ${{aC}_{1}^{i}, \dots, {aC}_{n_{i}}^{i}}$ of each slice, $i \in {1, \dots, 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} \subset {aSl}_{U_{i}}^{free}$ and $C_{q} \subset {aSl}_{U_{j}}^{free}$ , $i \neq j$ , are connected by an edge if the object can directly move between them. For that, both the sets $U_{i}, U_{j}$ , and $a C_{q}, a C_{p}$ must overlap: $U_{i} \cap U_{j}, a C_{q} \cap a C_{p} \neq \emptyset$ . Here $G (a C_{ε}^{col} (O))$ approximates the free space of the object: if there is no path in $G (a C_{ε}^{col} (O))$ between the vertices associated to the path components of two configurations $c_{1}, c_{2}$ , then they are disconnected in $C^{free} (O)$ .

We start by choosing one orientation and run a breadth-first search (BFS) 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 Algorithm 4. In line 18, we check which connected components of adjacent slices overlap, and add edges between them, see Algorithm 5.

Algorithm 4: ComputeSlice
input: $O_{obst}, O_{ε}$ — spherical representations of the obstacles and the $ε$ -core.
$q$ — orientation.
output: $aS l^{free}$ — A set of connected components of the slice corresponding to $q$
1 $C^{col} (O_{ε}) \leftarrow$ Collision-Space( $O_{obst}$ , $O_{obj}$ , $q$ )
2 $V (C^{col} (O_{ε})) \leftarrow$ Weighted-Voronoi-Diagram $(C^{col} (O_{ε}))$
3 $a C^{free} \leftarrow$ Dual-Diagram( $V (C^{col} (O_{ε}))$ )
4 $a C_{0} \leftarrow$ Compute-Infinite-Component()
5 $i \leftarrow 0$
6 foreach $ball \in a C^{free}$ do
7 ifConnected-Component( $ball$ ) $= \emptyset$ then
8 $i \leftarrow i + 1$
9 $a C_{i} \leftarrow$ Compute-Component( $ball$ )
10 end
11 end
12 return $aS l^{free} = {a C_{0}, \dots, a C_{n}}$

Algorithm 5: AddEdges
input: ${aSl}_{U_{i}}^{free}, {aSl}_{U_{j}}^{free}$ — free space approximations of two adjacent slices.
output: $E_{ij}$ — a set of edges between the connected components of ${aSl}_{U_{i}}^{free}$ and ${aSl}_{U_{j}}^{free}$ .
1 $E_{i, j} \leftarrow \emptyset$
2 for $a C_{i} \in ConnectedComponents ({aSl}_{U_{i}}^{free})$ do
3 for $a C_{j} \in ConnectedComponents ({aSl}_{U_{j}}^{free})$ do
4 if $a C_{i} . isInfinite ()$ AND $a C_{j} . isInfinite ()$ then
5 $E_{i, j} . addEdge (v (a C_{i}, U_{i}), v (a C_{j}, U_{i}))$
6 end
7 else
8 if $DoIntersect (a C_{i}, a C_{j})$ then
9 $E_{i, j} . addEdge (v (a C_{i}, U_{i}), v (a C_{j}, U_{i}))$
10 end
11 end
12 end
13 end
14 return $E_{i, j}$

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.

6.1. Construction of slices

Now, let us discuss how we approximate path-connected components of the free space of each slice, see Algorithm 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 derived the following representation for the collision space of $O_{ε}^{ϕ}$ :

C^{col} (O_{ε}^{ϕ}) = ⋃_{i, j} (B_{R_{j} + r_{i} - ε} (X_{j} - \bar{G Y_{i}}))

where $G$ is the origin chosen as the geometric center of the object, and $\bar{G Y_{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:

C^{col} (O_{ε}^{ϕ}) = ⋃_{i \in {1 \dots m}} C^{col} (B_{r_{i} - ε} (Y_{i})) - \bar{G Y_{i}}

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

{x \in R^{d} | d (x, S) \leq r_{i} - ε} = ⋃_{j \in {1 . . n}} B_{R_{j} + r_{i} - ε} (X_{j})

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^{free} (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 (Voronoi, 1907). In our case, the weighted distance of a point $x \in R^{n}$ from $B_{r_{i}} (z_{i})$ is equal to $d_{w} (x, B_{r_{i}} (z_{i})) = ∥ x - z_{i} ∥^{2} - r_{i}^{2}$ . When $r_{i} = 0$ , this distance is equal to the square of the Euclidean distance. A weighted Voronoi cell associated to a ball $B_{r_{i}} (z_{i})$ is a set of all points in $R^{n}$ whose weighted distances to the ball $B_{r_{i}} (z_{i})$ are smaller than or equal to the weighted distance to any other ball $B_{r_{j}} (z_{j}) \in ⋃_{i} B_{r_{i}} (z_{i})$ . A cell is called $n - k + 1$ -dimensional if it is equidistant from $k$ balls with respect to the weighted distance. For example, in the case of $R^{2}$ , a Voronoi vertex, or a zero-dimensional cell, is equidistant from three balls; a Voronoi edge, or a one-dimensional cell, is a set of points that are equidistant from two balls, etc. A weighted Voronoi diagram is a collection of weighted Voronoi cells.

To construct a dual diagram of a union of balls $⋃_{i} B_{r_{i}} (z_{i})$ , we first construct their weighted Voronoi diagram, see Figure 4. For each vertex $y_{j}$ of the weighted Voronoi diagram, let $B_{q_{j}} (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 2D case) balls from $⋃_{i} B_{r_{i}} (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 Figure 5.

Fig. 4.

Green circles and yellow halfspaces (“infinite circles”) together represent a dual diagram of the red circles. Green circles are centered at the vertices of the Voronoi diagram; yellow halfspaces correspond to infinite edges of the diagram (dashed lines).

Fig. 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.

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 Algorithm 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.

Remark 1. Recall that in each slice the $a C^{free} (O_{ε}^{ϕ_{i}})$ are constructed as the dual of the collision space $C^{col} (O_{ε}^{ϕ_{i}})$ , which entails that $a C^{free} (O_{ε}^{ϕ_{i}})$ has the same connectivity as $C^{free} (O_{ε}^{ϕ_{i}})$ . However, it also entails that any connected component of $a C^{free} (O_{ε}^{ϕ_{i}})$ partially overlaps with the collision space $C^{col} (O_{ε}^{ϕ_{i}})$ . This means that for two connected components $C_{j}^{i}, C_{j'}^{i'}$ of adjacent slices which do not overlap, it may occur that the corresponding approximations ${aC}_{j}^{i}, {aC}_{j'}^{i'}$ do overlap. In this case the resulting graph $G (a C_{ε}^{col} (O))$ would contain an edge between the corresponding vertices. 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}_{j}^{i} \cap {aC}_{j'}^{i'}$ with a union of balls and checking if it is contained inside the collision space $C^{col} (O_{ε}^{ϕ_{i}}) \cup C^{col} (O_{ε}^{ϕ_{i'}})$ .

7. Parallel implementation

To increase the performance of our algorithm, we designed and implemented a parallelized version that uses multiple threads to compute different slices simultaneously. Algorithm 6 describes the process.

Algorithm 6: ConnectivityGraphThread
input: $O_{obst}$ , $O_{obj}$ — spherical representations of the obstacles and the object.
$δ$ — clearance parameter.
$Q$ — a graph over $SO (3)$ .
$i$ — an initial orientation.
shared: $M$ — an array containing a mutex per slice.
$Slices$ — an array of slices (one per orientation).
$S$ — an integer array indicating slice status (0:unseen, 1:seen, 2:fully processed).
$G$ — a connectivity graph of the free space
1 $O_{ε} \leftarrow ComputeEpsilonCore (O_{obj}, δ, Q)$
2 $Queue \leftarrow \emptyset$
3 $Queue . enqueue (i)$
4 while $Queue \neq \emptyset$ do
5 $q_{cur} \leftarrow Queue . dequeue ()$
6 $Lock (M [q_{cur}])$
7 if $S [q_{cur}] = 0$ then
8 $Slices [q_{cur}] \leftarrow ComputeSlice (O_{obst}, O_{ε}, q_{cur})$
9 $S [q_{cur}] \leftarrow 1$
10 end
11 if $S [q_{cur}] = 1$ then
12 $n \leftarrow Size (Q . adjacentOrientations (q_{cur}))$
13 for $q_{adj} \in Q . adjacentOrientations (q_{cur})$ do
14 $lock \leftarrow TryLock (M [q_{adj}])$
15 if $lock . successful ()$ then
16 if $S [q_{adj}] = 0$ then
17 $Slices [q_{adj}] \leftarrow ComputeSlice (O_{obst}, O_{ε}, q_{adj})$
18 $S [q_{adj}] \leftarrow 1$
19 $Queue . enqueue (q_{adj})$
20 end
21 if $S [q_{adj}] = 1$ then
22 $AddEdges (Slices [q_{cur}], Slices [q_{adj}])$
23 end
24 $n \leftarrow n - 1$
25 $Unlock (M [q_{adj}])$
26 end
27 end
28 if $n = 0$ then
29 $S [q_{cur}] \leftarrow 2$
30 $Slices [q_{cur}] . reset ()$
31 end
32 end
33 $Unlock (M [q_{cur}])$
34 end
35 $NotifyParent ()$

Recall that in the single-thread version of the algorithm (see Algorithm 3), we performed 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 neighbors were constructed and the connectivity check between them was performed.

A nave 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 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.

Algorithm 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 do not 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 whether 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 has to wait for the first 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.

Algorithm 6 is called by a threadpool as shown in Algorithm 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 Algorithm 6, and are therefore available to be further processed in line 18 of Algorithm 7.

Algorithm 7: ComputeConnectivityGraphParallel
input: $O_{obst}$ , $O_{obj}$ — spherical representations of the obstacles and the object
$δ$ — clearance parameter.
$Q$ — a graph over $SO (3)$
output: $G$ — a connectivity graph of the free space
1 shared $G \leftarrow (\emptyset, \emptyset)$
2 shared $M \leftarrow mutexArray [\| Q \|]$
3 shared $Slices \leftarrow sliceArray [\| Q \|]$
4 shared $S \leftarrow intArray [\| Q \|]$
5 while $\exists i : S [i] = 0$ do
6 $q \leftarrow SelectQuaternion (S)$
7 $LaunchThread$ $ConnectivityGraphThread (O_{obst}, O_{obj},$
8 $NThreads \leftarrow NThreads - 1$
9 if $NThreads = 0$ then
10 $WaitForAnyChild ()$
11 end
12 end
13 $WaitForAllChildren ()$
14 for $q_{cur} \leftarrow Q$ do
15 if $S [q_{cur}] = 1$ then
16 for $q_{adj} \in Q . adjacentOrientations (q_{cur})$ do
17 if $S [q_{adj}] = 1$ then
18 $AddEdges (Slices [q_{cur}], Slices [q_{adj}])$
19 end
20 end
21 $Slices [q_{cur}] . reset ()$
22 end
23 end
24 return $G$

The function $SelectQuaternion$ is used to select the next quaternion which has not been seen previously. 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$ .

8. Theoretical properties of our approach

In this section, we discuss the correctness, completeness, and computational complexity of our approach.

8.1. 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.

Proposition 2 (Correctness). Consider an object $O$ and a set of obstacles $S$ . Consider two collision-free configurations of the object. If they are not path-connected in $G (a C_{ε}^{free} (O))$ , then they are not path-connected in $C^{free} (O)$ .

Proof. Recall that the approximation of the free space is constructed as follows:

a C_{ε}^{free} (O) = ⋃_{i = 1}^{s} {aSl}_{U (ϕ_{i}, ε)}^{free}

where

{aSl}_{U (ϕ_{i}, ε)}^{free} = Dual (C^{col} (O_{ε}^{ϕ_{i}})) \times U (ϕ_{i}, ε)

(2)

Now, recall that by definition $(Dual (C^{col} (O_{ε}^{ϕ_{i}})))^{c} \subset C^{col} (O_{ε}^{ϕ_{i}})$ (Edelsbrunner, 1999), and that we choose $ε$ and $U (ϕ_{i}, ε)$ so that for any $ϕ \in U (ϕ_{i}, ε)$ , $C^{col} (O_{ε}^{ϕ_{i}}) \subset C^{col} (O^{ϕ})$ . This implies that $(Dual (C^{col} (O_{ε}^{ϕ_{i}})))^{c} \subseteq C^{col} (O^{ϕ})$ for any $ϕ \in U (ϕ_{i}, ε)$ , and conversely that $C^{free} (O^{ϕ}) \subset Dual (C^{col} (O_{ε}^{ϕ_{i}}))$ . Finally, since ${Sl}_{U (ϕ_{i}, ε)}^{free} = ⋃_{ϕ} C^{free} (O^{ϕ}) \times {ϕ}$ , we have

{Sl}_{U (ϕ_{i}, ε)}^{free} \subseteq {aSl}_{U (ϕ_{i}, ε)}^{free}

(3)

We now want to show that if there is no path between two vertices $v = (aC, U)$ and $v' = (aC', U')$ in $G (a C_{ε}^{free})$ , then there is no path between connected components of $a C_{ε}^{free} (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 that are disconnected in ${Sl}_{U}^{free} \cup {Sl}_{U'}^{free}$ .

Consider two adjacent slices $S l_{U (ϕ_{i}, ε)}$ and $S l_{U (ϕ_{j}, ε)}$ , and two path-connected components $C_{1} \subset {aSl}_{U (ϕ_{i}, ε)}^{free}$ and $C_{2} \subset {aSl}_{U (ϕ_{j}, ε)}^{free}$ . Let $a C_{1}$ and $a C_{2}$ be their respective representations as unions of balls.

Let $v_{1}$ and $v_{2}$ be the vertices of $G (a C_{ε}^{free} (O))$ corresponding to these components: $v_{1} = (a C_{1}, U (ϕ_{i}, ε))$ and $v_{2} = (a C_{2}, U (ϕ_{j}, ε))$ . By construction, these are adjacent slices, therefore $U (ϕ_{i}, ε) \cap U (ϕ_{j}, ε) \neq \emptyset$ and since there is no edge between $v_{1}$ and $v_{2}$ , we get $a C_{1} \cap a C_{2} = \emptyset$ . However, by construction $C_{1} \subseteq a C_{1} \times U (ϕ_{i}, ε)$ and $C_{2} \subseteq a C_{2} \times U (ϕ_{j}, ε)$ , therefore, we obtain

C_{1} \cap C_{2} \subseteq (a C_{1} \times U (ϕ_{i}, ε)) \cap (a C_{2} \times U (ϕ_{j}, ε)) = \emptyset

and so $C_{1}$ and $C_{2}$ are disjoint in the union of the corresponding slices, which concludes the proof. □

8.2. $δ$ -Completeness

Now, we show that if two configurations are not path-connected in $C^{free} (O)$ , we can construct an approximation of $C^{free} (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^{free} (O)$ . If they are not path-connected in $C^{free} (O)$ , then for any $δ > 0$ there exists $ε > 0$ such that the corresponding configurations are not path-connected in $G (a C_{ε}^{free} (O_{+ δ}))$ , where the graph is produced according to the procedure outlined in Remark 1.

In proving this proposition we make used of the notion of the signed distance between two sets:

\bar{d_{s}} (O, S) = {\begin{matrix} {min}_{p \in O} d (p, S) & if O \cap S \neq \emptyset \\ - {max}_{p \in O \cap S} d (p, \partial S) & otherwise \end{matrix}

where $\partial S$ denotes the boundary of $S$ . Note that $\bar{d_{s}} (A, B)$ is not necessarily the same as $\bar{d_{s}} (B, A)$ so we instead consider $d_{s} (A, B) = min (\bar{d_{s}} (A, B), \bar{d_{s}} (B, A))$ .

Proof. Recall Remark 1 that there is an edge between vertices $(a C_{1}, ϕ_{1})$ , $(a C_{2}, ϕ_{2})$ only if $U (ϕ_{1}, ε)$ overlaps with $U (ϕ_{2}, ε)$ and $C_{1}$ overlaps with $C_{2}$ where $C_{i} = a C_{i} \cap C^{free} (O_{ε}^{ϕ_{i}})$ for $i = 1, 2$ , i.e., the components of the actual free space of $O_{ε}^{ϕ_{i}}$ ( $i = 1, 2$ ) corresponding to the approximations $a C_{1}$ and $a C_{2}$ .

Recall now that we want to prove that for a pair of configurations $c_{1}, c_{2}$ which are not path-connected in $C^{free} (O)$ , then for any $δ > 0$ there exists some $ε > 0$ so that they are not path-connected in $G (a C_{ε}^{free} (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), S) < 0$ . Thus, for the same configuration $c$ we have $d_{s} (c (O_{+ δ}), S) < - δ$ .

To see that this will result in path non-existence, we take an arbitrary $ε > 0$ and consider the collision space $a C_{ε}^{col} (O_{+ δ})$ . Further, we let $c = (p, ϕ) \in R^{3} \times SO (3)$ , and for any $ϕ_{i}$ such that $ϕ \in U (ϕ_{i}, ε)$ , we define $c_{i} = (p, ϕ_{i})$ . Finally, we restrict ourselves to the case where $ε < δ$ and define $δ' = δ - ε$ , then we obtain

d_{s} (c_{i} (O_{+ δ'}), S) \leq d (c (O_{+ δ'}), c_{i} (O_{+ δ'})) + d_{s} (c (O_{+ δ'}), S)

\leq ε (1 + \frac{δ'}{rad (O)}) - δ'

where the term $ε (1 + \frac{δ'}{rad (O)})$ corresponds to the maximum displacement of the object offset $O_{+ δ}$ associated to a rotation of $O$ with maximum displacement $ε$ . This implies that, as long as we choose $ε$ such that

ε (1 + \frac{δ - ε}{rad (O)}) - (δ - ε) < 0

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$ . In addition, the roots of the equation in terms of $δ$ are given by

ε = \frac{rad (O)}{2} (2 + \frac{δ}{rad (O)} \pm \sqrt{{(2 + \frac{δ}{rad (O)})}^{2} - \frac{4 δ}{rad (O)}})

which are both positive. Finally, call the smallest of the two roots $\tilde{ε}$ and the inequality holds as long as $ε \in (0, min {\tilde{ε}, δ}) ?$ , concluding the proof.

8.3. 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$ neighbors.⁵

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 and Edelsbrunner (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 \overset{3}{\log} (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 algorithm, computing 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).

9. Examples and Experiments

In this section, we test our algorithm on 2D and 3D objects. In the first experiment, we investigate how the quality 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.

9.1. 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 Figure 6 and the results listed in Table 2, which are 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.

Fig. 6.

On 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. 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.

Table 2.

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 Figure 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.

$ε$	$R = 15$		$R = 10$		$R = 4$
$0.30 \cdot r$	2 c.	741 ms	3 c.	1,287 ms	4 c.	1,354 ms
$0.33 \cdot r$	2 c.	688 ms	3 c.	1,208 ms	4 c.	1,363 ms
$0.37 \cdot r$	2 c.	647 ms	3 c.	1,079 ms	4 c.	1,287 ms
$0.40 \cdot r$	2 c.	571 ms	3 c.	986 ms	3 c.	1,156 ms
$0.43 \cdot r$	2 c.	554 ms	3 c.	950 ms	3 c.	1,203 ms

A more suitable approach to approximate the object is to compute the medial axis of the object using for example the method from Dey and Sun (2006), which is able to construct it from a pointcloud of the object. Alternatively, one can use the method from Dey and Zhao (2002), which is able to retrieve a skeleton of the object (i.e., a one-dimensional subset of the medial axis). In our experiments, we used a watertight mesh of the drill (see Figure 8) to extract its skeleton axis and obtained an approximation of the drill by sampling ball centers in the skeleton with the largest radius that did not contain any point in the mesh.

Finally, for the mug and the bugtrap (also Figure 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).

9.2. 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 Figure 6. This experiment was performed on a single thread of Intel Core i7 processor.

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 Figure 6.

We consider a simple object whose approximation consists of five balls. We run our algorithm for all the three approximations of the workspace, and take five different values of $ε$ , see Table 2. 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 Table 2).

In our opinion, the accuracy of a workspace approximation should depend on the application scenario as well as on the amount of available computational resources.

9.3. 3D scenario: different types of 3D caging

As we have mentioned in Section 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 in Figure 7, and Table 3 reports execution times for different resolutions of the $SO (3)$ grid. In all cases we show the value for the radius of the object for comparison, which is computed as the distance between the geometric center of the object and the furthest point from the center in the object.

Fig. 7.

Different 3D caging scenarios. From left to right: linking-based caging (Pokorny et al., 2013; Stork et al., 2013), narrow part-based caging (Liu et al., 2018; Varava et al., 2017), 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.

Table 3.

Results from running 3D experiments on the objects shown in Figure 7 using three 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 parentheses 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.

	Rings (320)		Narrow part (480)		Surrounding (266)
Slices	radius $= 9.071$		radius $= 14.500$		radius=4.000
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$

In all cases (Table 3), 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, 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 three 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 shape-based 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.

9.4. 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 Figure 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 linking-based 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.

Fig. 8

Caging real-world objects with a three-finger Schunk hand. First row, from left to right: a mug (linking-based caging), its representations as a union of balls, a drill (narrow part-based caging), a bugtrap (caging by surrounding). Second row: the respective spherical representations of the objects. Third row: a spherical representation of the Schunk hand in the caging configurations used above; an object passing through the bugtrap.

First, we observe how the performance of our algorithm changes with respect to the number of slices we consider. Table 4 reports the results. The respective objects are depicted in Figure 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 maintaining 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.

Table 4.

Results from running 3D experiments with objects shown in Figure 8 using three different resolutions for the $SO (3)$ grid. The number of balls used to approximate the collision space of each model is indicated in parentheses next to the model name.

	Mug (9198 balls)		Drill (2,646 balls)		Bugtrap (2,079 balls)
Slices	radius $= 0.117$		radius $= 0.124$		radius $= 1.600$
576	36 s	$δ = 0.041$	12 s	$δ = 0.044$	169 s	$δ = 0.756$
4,608	230 s	$δ = 0.012$	110 s	$δ = 0.013$	1,095 s	$δ = 0.369$
36,864	1 765 s	$δ = 0.000$	784 s	$δ = 0.000$	8,263 s	$δ = 0.200$

Now, we analyze how the performance of the parallelized version of our algorithm changes with respect to the number of threads we use. Table 5 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 4,608 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 Figure 9).

Table 5.

Results from running the experiments on the ring, and narrow part toy examples, as well as on the drill objects, with one to eight threads. These examples stem from using our parallel algorithm (Algorithm 7) with the objects and obstacles depicted in Figures 7 and 8, and a grid over $SO (3)$ comprising 4,608 nodes.

Threads	Ring	Narrow part	Drill
1	95 s	79 s	769 s
2	48 s	40 s	390 s
3	33 s	28 s	270 s
4	26 s	22 s	210 s
5	25 s	20 s	196 s
6	24 s	19 s	184 s
7	22 s	18 s	176 s
8	22 s	18 s	170 s

Fig. 9

Relative timing for calculating the configuration space of objects with different numbers of threads. The vertical axis measures time in fraction of time taken by computing with a single thread. The horizontal axis indicates the number of threads. The data corresponds to that presented in Table 5.

10. Discussion and future work

In this article, we have provided a computationally feasible and provably correct algorithm for 3D and 6D configuration space approximation and used it to prove caging and path non-existence. We have analyzed the theoretical properties of the algorithm such as correctness and complexity. We have provided 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.

10.1. 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 in Figure 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.

Fig. 10.

The small molecule (a guest) can be caged by the big molecule (the host).

10.2. 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.

10.3. Energy-bounded caging

Mahler et al. (2016, 2018) defined the concept of energy-bounded 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 two dimensions.

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. (2016, 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.

Footnotes

Appendix

Recall the formulation of Proposition 1.

Given two unit quaternions $p, q$ , the following equation holds:

D (R_{p \bar{q}}) = 2 \sin (ρ (p, q)) rad (O)

Proof. For notational simplicity we divide both sides of the equation by $rad (O)$ and define $\bar{D} (R_{q}) = \frac{D (R_{q})}{rad (O)}$ , which reduces the problem to proving

\bar{D} (R_{p \bar{q}}) = 2 \sin (ρ (p, q))

We proceed by reducing both sides of the equation to the same formula, starting with the left-hand side. To do this, we once again point out that we can identify quaternions with vectors in $R^{4}$ , and that a unit quaternion $q = \cos (\frac{θ_{q}}{2}) + \sin (\frac{θ_{q}}{2}) (q_{x} i + q_{y} j + q_{z} k)$ is associated to a 3D rotation of an angle of $θ_{q}$ around the axis $(q_{x}, q_{y}, q_{z})$ which we will denote by $w_{q}$ :

\begin{matrix} \bar{D} (R_{p \bar{q}}) = 2 | ℑ p \bar{q} | \\ = 2 ∥ \cos (\frac{θ_{q}}{2}) \sin (\frac{θ_{p}}{2}) w_{p} - \cos (\frac{θ_{p}}{2}) \sin (\frac{θ_{q}}{2}) w_{q} \\ - \sin (\frac{θ_{p}}{2}) \sin (\frac{θ_{q}}{2}) w_{p} \times w_{q} ∥ \\ = 2 \sqrt{∥ \cos (\frac{θ_{q}}{2}) \sin (\frac{θ_{p}}{2}) w_{p} - \cos (\frac{θ_{p}}{2}) \sin (\frac{θ_{q}}{2}) w_{q} ∥^{2}} \\ \bar{+ ∥ \sin (\frac{θ_{p}}{2}) \sin (\frac{θ_{q}}{2}) w_{p} \times w_{q} ∥^{2}} \end{matrix}

where the last equality is due to the fact that $w_{p} \times w_{q}$ is perpendicular to both $w_{p}$ and $w_{q}$ , and is therefore a consequence of the Pythagorean theorem. Now recall that $∥ w_{p} \times w_{q} ∥ = \sin (ω_{p, q})$ where $ω_{p, q}$ is the angle between $w_{p}, w_{q}$ and that $〈 w_{p}, w_{q} 〉 = \cos (ω_{p, q})$ , since $∥ w_{p} ∥ = ∥ w_{q} ∥ = 1$ . Recall also that $\sin^{2} (θ) = 1 - \cos^{2} (θ)$ , whence we obtain

\begin{matrix} \bar{D} (R_{p \bar{q}}) \\ = 2 \sqrt{∥ \cos (\frac{θ_{q}}{2}) \sin (\frac{θ_{p}}{2}) w_{p} - \cos (\frac{θ_{p}}{2}) \sin (\frac{θ_{q}}{2}) w_{q} ∥^{2}} \\ \bar{+ \sin^{2} (\frac{θ_{p}}{2}) \sin^{2} (\frac{θ_{q}}{2}) (1 - {〈 w_{p}, w_{q} 〉}^{2})} \end{matrix}

Furthermore, let ${\tilde{w}}_{p} = \frac{w_{q} - 〈 w_{p}, w_{q} 〉 w_{p}}{∥ w_{q} - 〈 w_{p}, w_{q} 〉 w_{p} ∥}$ be the component of $w_{q}$ that is perpendicular to $w_{p}$ , then we can rewrite $\cos (\frac{θ_{p}}{2}) \sin (\frac{θ_{q}}{2}) w_{q}$ as

\begin{matrix} \cos (\frac{θ_{p}}{2}) \sin (\frac{θ_{q}}{2}) w_{q} \\ = \cos (\frac{θ_{p}}{2}) \sin (\frac{θ_{q}}{2}) (〈 w_{p}, w_{q} 〉 w_{p} + 〈 {\tilde{w}}_{p}, w_{q} 〉 {\tilde{w}}_{p}) \end{matrix}

Substituting this into the formula, and using the Pythagorean theorem to separate the $w_{p}$ and ${\tilde{w}}_{p}$ components, we can proceed with

\begin{matrix} \bar{D} (R_{p \bar{q}}) \\ = 2 \sqrt{∥ (\cos (\frac{θ_{q}}{2}) \sin (\frac{θ_{p}}{2}) - \cos (\frac{θ_{p}}{2}) \sin (\frac{θ_{q}}{2}) 〈 w_{p}, w_{q} 〉) w_{p} ∥^{2}} \\ \bar{+ ∥ \cos (\frac{θ_{p}}{2}) \sin (\frac{θ_{q}}{2}) 〈 w_{q}, {\tilde{w}}_{p} 〉 {\tilde{w}}_{p} ∥^{2}} \\ \bar{+ \sin^{2} (\frac{θ_{p}}{2}) \sin^{2} (\frac{θ_{q}}{2}) (1 - {〈 w_{p}, w_{q} 〉}^{2})} \end{matrix}

Now we note that $∥ w_{q} ∥, ∥ w_{p} ∥, ∥ {\tilde{w}}_{p} ∥ = 1$ , and therefore $∥ 〈 w_{p}, w_{q} 〉 w_{p} + 〈 w_{q}, {\tilde{w}}_{p} 〉 {\tilde{w}}_{p} ∥^{2} = 1$ which by the Pythagorean theorem gives $〈 w_{q}, {\tilde{w}}_{p} 〉^{2} = 1 - 〈 w_{p}, w_{q} 〉^{2}$ , which we can substitute once again:

\begin{matrix} \bar{D} (R_{p \bar{q}}) \\ = 2 \sqrt{{(\cos (\frac{θ_{q}}{2}) \sin (\frac{θ_{p}}{2}) - \cos (\frac{θ_{p}}{2}) \sin (\frac{θ_{q}}{2}) 〈 w_{p}, w_{q} 〉)}^{2}} \\ \bar{+ \cos^{2} (\frac{θ_{p}}{2}) \sin^{2} (\frac{θ_{q}}{2}) (1 - {〈 w_{p}, w_{q} 〉}^{2})} \\ \bar{+ \sin^{2} (\frac{θ_{p}}{2}) \sin^{2} (\frac{θ_{q}}{2}) (1 - {〈 w_{p}, w_{q} 〉}^{2})} \end{matrix}

Now we want to deal only with a combination of tangents, therefore we divide the term inside the square root by $\cos^{2} (\frac{θ_{p}}{2}) \cos^{2} (\frac{θ_{q}}{2})$ yielding

\begin{matrix} \bar{D} (R_{p \bar{q}}) = 2 | \cos (\frac{θ_{p}}{2}) \cos (\frac{θ_{q}}{2}) | \\ \sqrt{{(\tan (\frac{θ_{p}}{2}) - \tan (\frac{θ_{q}}{2}) 〈 w_{p}, w_{q} 〉)}^{2}} \\ \bar{+ \tan^{2} (\frac{θ_{q}}{2}) (1 - {〈 w_{p}, w_{q} 〉}^{2})} \\ \bar{+ \tan^{2} (\frac{θ_{p}}{2}) \tan^{2} (\frac{θ_{q}}{2}) (1 - {〈 w_{p}, w_{q} 〉}^{2})} \end{matrix}

Now, expanding the squares and multiplying into all the terms under the square root sign, as well as eliminating terms that cancel out, results in

\begin{matrix} \bar{D} (R_{p \bar{q}}) = 2 | \cos (\frac{θ_{p}}{2}) \cos (\frac{θ_{q}}{2}) | \\ \sqrt{\tan^{2} (\frac{θ_{p}}{2}) - 2 \tan (\frac{θ_{p}}{2}) \tan (\frac{θ_{q}}{2}) 〈 w_{p}, w_{q} 〉} \\ \bar{+ \tan^{2} (\frac{θ_{q}}{2}) + \tan^{2} (\frac{θ_{p}}{2}) \tan^{2} (\frac{θ_{q}}{2})} \\ \bar{- \tan^{2} (\frac{θ_{p}}{2}) \tan^{2} (\frac{θ_{q}}{2}) {〈 w_{p}, w_{q} 〉}^{2}} \end{matrix}

By introducing an extra $1 - 1$ into the square root, we can use these terms to complete products in order to simplify the equation.

\begin{matrix} \bar{D} (R_{p \bar{q}}) = 2 | \cos (\frac{θ_{p}}{2}) \cos (\frac{θ_{q}}{2}) | \\ \sqrt{1 + \tan^{2} (\frac{θ_{p}}{2}) + \tan^{2} (\frac{θ_{q}}{2}) + \tan^{2} (\frac{θ_{p}}{2}) \tan^{2} (\frac{θ_{q}}{2})} \\ \bar{- {(1 + \tan (\frac{θ_{p}}{2}) \tan (\frac{θ_{q}}{2}) 〈 w_{p}, w_{q} 〉)}^{2}} \\ = 2 | \cos (\frac{θ_{p}}{2}) \cos (\frac{θ_{q}}{2}) | \sqrt{(1 + \tan^{2} (\frac{θ_{p}}{2})) (1 + \tan^{2} (\frac{θ_{q}}{2}))} \\ \bar{- {(1 + \tan (\frac{θ_{p}}{2}) \tan (\frac{θ_{q}}{2}) 〈 w_{p}, w_{q} 〉)}^{2}} \end{matrix}

Finally, recall that $1 + \tan^{2} (θ) = \frac{1}{\cos^{2} (θ)}$ , which gives us

\begin{matrix} \bar{D} (R_{p \bar{q}}) = 2 \sqrt{1} \\ \bar{- \cos^{2} (\frac{θ_{p}}{2}) \cos^{2} (\frac{θ_{q}}{2}) (1 + \tan (\frac{θ_{p}}{2}) \tan (\frac{θ_{q}}{2}) 〈 w_{p}, w_{q} 〉)^{2}} \end{matrix}

Now we begin to explore the right-hand side of the equation, by noting that when $\sin (θ) > 0$ then $\sin (θ) = | \sin (θ) | = \sqrt{\sin^{2} (θ)} = \sqrt{1 - \cos^{2} (θ)}$ . Furthermore, we note that $\cos^{- 1}$ maps $[- 1, 1]$ to $[0, π]$ and particularly it maps $[0, 1]$ to $[0, π / 2]$ where the sine function is positive, therefore, we obtain

\begin{matrix} 2 \sin (ρ (p, q)) = 2 \sin (\cos^{- 1} (| 〈 p, q 〉 |)) \\ = 2 \sqrt{1 - \cos^{2} (\cos^{- 1} (| 〈 p, q 〉 |))} \\ = 2 \sqrt{1 - {〈 p, q 〉}^{2}} \\ = 2 \sqrt{1 - {(\cos (\frac{θ_{p}}{2}) \cos (\frac{θ_{q}}{2}) + \sin (\frac{θ_{p}}{2}) \sin (\frac{θ_{q}}{2}) 〈 w_{p}, w_{q} 〉)}^{2}} \end{matrix}

and, finally, we obtain the same formula as previously:

\begin{matrix} 2 \sin (ρ (p, q)) = 2 \sqrt{1} \\ \bar{- \cos^{2} (\frac{θ_{p}}{2}) \cos^{2} (\frac{θ_{q}}{2}) (1 + \tan (\frac{θ_{p}}{2}) \tan (\frac{θ_{q}}{2}) 〈 w_{p}, w_{q} 〉)^{2}} \end{matrix}

hence concluding the proof of Proposition 1.

Acknowledgements

The authors are grateful to D. Devaurs, L. Kavraki, and O. Kravchenko for their insights into molecular caging.

Funding

This work has been supported by the Knut and Alice Wallenberg Foundation and Swedish Research Council.

Notes

ORCID iDs

Anastasiia Varava

J Frederico Carvalho

References

Aurenhammer

Edelsbrunner

(1984) An optimal algorithm for constructing the weighted Voronoi diagram in the plane. Pattern Recognition 17(2): 251–257.

Barraquand

Kavraki

Latombe

Motwani

Raghavan

(1997) A random sampling scheme for path planning. The International Journal of Robotics Research 16(6): 759–774.

Basch

Guibas

Hsu

Nguyen

(2001) Disconnection proofs for motion planning. In: IEEE International Conference on Robotics and Automation, vol. 2, pp. 1765–1772.

Behar

Lien

(2013) Mapping the configuration space of polygons using reduced convolution In: IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 1242–1248.

Dey

Zhao

(2012) Approximate medial axis as a voronoi subcomplex. In: Proceedings of the seventh ACM symposium on Solid modeling and applications, pp. 356–366.

Dey

Sun

(2006) Normal and feature approximations from noisy point clouds. In: International Conference on Foundations of Software Technology and Theoretical Computer Science, pp. 21–32.

Edelsbrunner

(1999) Deformable smooth surface design. Discrete and Computational Geometry 21(1): 87–115.

Guibas

Stolfi

(1985) Primitives for the manipulation of general subdivisions and the computation of Voronoi. ACM Transactions on Graphics 4(2): 74–123.

Kuperberg

(1990) Problems on polytopes and convex sets In: DIMACS Workshop on polytopes, pp. 584–589.

10.

Latombe

(1991) Robot Motion Planning. ( Springer International Series in Engineering and Computer Science, vol. 124). New York: Springer.

11.

Liu

Xin

Gaol

Chen

(2018) Caging loops in shape embedding space: theory and computation. In: IEEE International Conference on Robotics and Automation (ICRA), pp. 1–5.

12.

Lozano-Perez

(1990) Spatial planning: A configuration space approach. In: Autonomous Robot Vehicles, pp. 259–271. New York: Springer.

13.

Mahler

Pokorny

McCarthy

van der Stappen

Goldberg

(2016) Energy-bounded caging: Formal definition and 2-D energy lower bound algorithm based on weighted alpha shapes. IEEE Robotics and Automation Letters 1(1): 508–515.

14.

Mahler

Pokorny

Niyaz

Goldberg

(2018) Synthesis of energy-bounded planar caging grasps using persistent homology. IEEE Transactions on Automation Science and Engineering 15(3): 908–918.

15.

Makita

Maeda

(2008) 3D multifingered caging: Basic formulation and planning. In: 2008 IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 2697–2702.

16.

Makita

Okita

Maeda

(2013) 3D two-fingered caging for two types of objects: sufficient conditions and planning. International Journal of Mechatronics and Automation 3(4): 263–277.

17.

Makita

Wan

(2017) A survey of robotic caging and its applications. Advanced Robotics 31(19–20): 1071–1085.

18.

Makapunyo

Phoka

Pipattanasomporn

Niparnan

Sudsang

(2013) Measurement framework of partial cage quality based on probabilistic motion planning. In: IEEE International Conference on Robotics and Automation, pp. 1574–1579.

19.

McCarthy

Bretl

Hutchinson

(2012) Proving path non-existence using sampling and alpha shapes. In: IEEE International Conference on Robotics and Automation, pp. 2563–2569.

20.

Milenkovic

Sacks

Trac

(2013) Robust complete path planning in the plane. In: Algorithmic Foundations of Robotics X, pp. 37–52. Berlin: Springer.

21.

Mitra

Jelfs

Schmidtmann

, et al. (2013) Molecular shape sorting using molecular organic cages Nature Chemistry 5(4): 276.

22.

Pereira

Campos

Kumar

(2004) Decentralized algorithms for multi-robot manipulation via caging. The International Journal of Robotics Research 23(7–8): 783–795.

23.

Pipattanasomporn

Sudsang

(2006) Two-finger caging of concave polygon. In: Proceedings IEEE International Conference on Robotics and Automation, pp. 2137–2142.

24.

Pipattanasomporn

Sudsang

(2011) Two-finger caging of nonconvex polytopes. IEEE Transactions on Robotics 27(2): 324–333.

25.

Pokorny

Stork

Kragic

(2013) Grasping objects with holes: A topological approach. In: 2013 IEEE International Conference on Robotics and Automation, pp. 1100–1107.

26.

Rimon

Blake

(1999) Caging planar bodies by one-parameter two-fingered gripping systems. International Journal of Robotics Research 18(3): 299–318.

27.

Rodriguez

Mason

(2012) Path connectivity of the free space. IEEE Transactions on Robotics 28(5): 1177–1180.

28.

Rodriguez

Mason

Ferry

(2012) From caging to grasping. The International Journal of Robotics Research 31(7): 886–900.

29.

Rother

Nussbaumer

Renggli

Bruns

(2016) Protein cages and synthetic polymers: A fruitful symbiosis for drug delivery applications, bionanotechnology and materials science. Chemical Society Reviews 45(22): 6213–6249.

30.

Stork

Pokorny

Kragic

(2013) Integrated motion and clasp planning with virtual linking. In: 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 3007–3014.

31.

Vahedi

van der Stappen

(2008). Caging polygons with two and three fingers. The International Journal of Robotics Research 27(11–12): 1308–1324.

32.

Varava

Carvalho

Pokorny

Kragic

(2017) Caging and path non-existence: a deterministic sampling-based verification algorithm. In: International Symposium on Robotics Research

33.

Varava

Carvalho

Pokorny

Kragic

(2018) Free space of rigid objects: Caging, path non-existence, and narrow passage detection. In: Workshop on Algorithmic Foundations of Robotics.

34.

Varava

Kragic

Pokorny

(2016) Caging grasps of rigid and partially deformable 3-D objects with double fork and neck features. IEEE Transactions on Robotics 32(6): 1479–1497.

35.

Voronoi

(1907) Nouvelles applications des parametres continusa la théorie des formes quadratiques. Premier mémoire. Sur quelques propriétés des formes quadratiques positives parfaites. Journal für die reine und angewandte Mathematik 133: 97–178.

36.

Wang

Kumar

(2002) Object closure and manipulation by multiple cooperating mobile robots. In: Proceedings IEEE International Conference on Robotics and Automation, pp. 394–399.

37.

Wise

Bowyer

(2000) A survey of global configuration-space mapping techniques for a single robot in a static environment. The International Journal of Robotics Research 19(8): 762–779.

38.

Yershova

Jain

Lavalle

Mitchell

(2010) Generating uniform incremental grids on SO(3) using the Hopf fibration. The International Journal of Robotics Research 29(7): 801–812.

39.

Zhang

Kim

Manocha

(2008) Efficient cell labelling and path non-existence computation using C-obstacle query. The International Journal of Robotics Research 27(11–12): 1246–1257.

40.

Zhu

Latombe

(1991) New heuristic algorithms for efficient hierarchical path planning. IEEE Transactions on Robotics and Automation 7(1): 9–20.

41.

Zomorodian

Edelsbrunner

(2000) Fast software for box intersections. International Journal of Computational Geometry and Applications 12(1–2): 143–172.

Free space of rigid objects: caging,path non-existence,and narrow passage detection

Abstract

Keywords

1. Introduction

2. Related work

3. Definitions and notation

3.1. Objects and obstacles

3.2. Slice-based representation of the configuration space

3.3. An ε -core of the object

4. Existence of δ -clearance paths

5. Discretization of SO ( n )

5.1. Displacement in two dimensions

5.2. Displacement in three dimensions

5.3. Different resolutions and dispersion estimation

5.4. The choice of ε and SO ( 3 ) discretization in practice

6. Free space approximation

6.1. Construction of slices

7. Parallel implementation

8. Theoretical properties of our approach

8.1. Correctness

8.2. δ -Completeness

8.3. Computational complexity

9. Examples and Experiments

9.1. Object and obstacle approximation

9.2. 2D scenario: different approximations of the workspace

9.3. 3D scenario: different types of 3D caging

9.4. Parallel implementation

10. Discussion and future work

10.1. Molecular cages

10.2. Integration with path planners

10.3. Energy-bounded caging

Footnotes

Appendix

Acknowledgements

Funding

Notes

ORCID iDs

References

3.3. An $ε$ -core of the object

4. Existence of $δ$ -clearance paths

5. Discretization of $SO (n)$

5.4. The choice of $ε$ and $SO (3)$ discretization in practice

8.2. $δ$ -Completeness