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

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 2D and 3D. Our approach is able to detect compact path components and narrow passages in configuration space which are important for applications in robotic manipulation and path planning. Moreover, as we demonstrate, they are also applicable to identification of molecular cages in chemistry. Our algorithms are based on a decomposition of the resulting 3 and 6 dimensional configuration spaces into slices corresponding to a finite sample of fixed orientations in configuration space. We utilize dual diagrams of unions of balls and uniform grids of orientations to approximate the configuration space. We carry out experiments to evaluate the computational efficiency on a set of objects with different geometric features thus demonstrating that our approach is applicable to different object shapes. We investigate the performance of our algorithm by computing increasingly fine-grained approximations of the object’s configuration space. A multithreaded implementation of our approach is shown to result in significant speed improvements.

grasping, path planning for manipulators, manipulation planning
\corrauth

Anastasiia Varava

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 to drug delivery and related problems (Mitra et al., 2013; Rother et al., 2016).

The main challenge in caging verification is that the configuration space of a rigid object in 3D is in general a 6 dimensional subset of SE(3). For this reason, explicit reconstruction of the configuration space in terms of higher-dimensional analogues of discretization techniques such as voxel-grids and triangulations has been considered a computationally infeasible approach to this problem (Makita and Wan, 2017). Past work has instead focused on analyzing caging configurations either for 2D objects only McCarthy et al. (2012), or for specific 3D objects with special geometric properties, such as handles and narrow parts (Pokorny et al., 2013; Varava et al., 2016), which allow one to avoid modelling the configuration space directly.

Figure 1: Diagram of our method. We approximate the collision space of an object by choosing a finite set of fixed object’s orientations and considering the corresponding slices of the collision space to (). From the collision space slices we compute approximations of the free space slices. Finally, we analyze the connectivity between neighboring slices to get an approximation of the connected components of the entire free space.

We study caging as a special case of proving path non-existence between a pair of configurations. To show that two configurations are disconnected, we construct an approximation of the object’s collision space. Intuitively, we construct a set of slices of the object’s collision space to subspaces corresponding to fixed orientations of the object, see Fig. 1. We then compute the free space approximation as the complement to the collision space of each slice. By construction, our collision space approximation is a proper subset of the real collision space, which implies that when our algorithm reports that the two configurations are not path-connected, then there is no path between them in the real free space. However, for the same reason, our algorithm is not guaranteed to find all possible caging configurations, since we do not reconstruct the entire collision space.

The key contribution of the work we present here is to show that it is possible to compute explicit approximations of configuration spaces of generic rigid bodies in 3D relatively efficiently, while maintaining provability guarantees with respect to reasoning about caging and, more generally, path existence. Our technique for constructing such an approximation is based on the following key insights:

  • Representation: We utilize a union-of-balls based object and obstacles representation that allows one to utilize dual diagrams to approximate the free space in a provably correct manner.

  • Slicing: We use an approximate configuration space decomposition based on locally fixed orientations.

  • 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 an revised introduction and algorithms description, as well as extended experimental results, a parallelized version of the presented algorithm and its evaluation on 3D models of real objects.

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 points lying in the complement of a polygon and preventing it from escaping arbitrarily far from its initial position. In robotics, it was subsequently studied in the context of point-based caging in 2D by Rimon and Blake (1999); Pipattanasomporn and Sudsang (2006); Vahedi and van der Stappen (2008), and others. A similar approach has also been adopted for caging 3D objects. For instance, Pipattanasomporn and Sudsang (2011) proposed an algorithm for computing all two-finger cages for non-convex polytopes. Pereira et al. (2004) and Wang and Kumar (2002) present a set of 2D 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); 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) provide an algorithm to prove that two configurations are disconnected when the object is ‘too big’ or ‘too long’ to pass through a ‘gate’ between them. There are also some related results on approximating configuration spaces of 2D objects. Zhang et al. (2008) use approximate cell decomposition and prove path non-existence for 2D rigid objects. They decompose a configuration space into a set of cells and for each cell decide if it lies in the collision space. McCarthy et al. (2012) propose a related approach. There, they randomly sample the configuration space of a planar rigid object and reconstruct its approximation as an alpha complex. They later use it to check the connectivity between pairs of configurations. This approach has been later extended to planar energy-bounded caging by Mahler et al. (2016).

The problem of explicit construction (either exact or approximate) of configuration spaces has been studied for several decades in the context of motion planning, and a summary of early results can be found in the survey by Wise and Bowyer (2000). Lozano-Perez (1983) introduced the idea of slicing along the rotational axis. To connect two consecutive slices, the authors proposed to use the area swept by the robot rotating between two consecutive orientation values. Zhu and Latombe (1991) extended this idea and used both outer and inner swept areas to construct a subset and a superset of the collision space of polygonal robots. The outer and inner swept areas are represented as generalized polygons defined as the union and intersection of all polygons representing robot’s shape rotating in a certain interval of orientation values, respectively. Several recent works propose methods for exact computation of configuration spaces of planar objects (Behar and Lien, 2013; Milenkovic et al., 2013). Behar and Lien (2013) proposed a method towards exact computation of the boundary of the collision space. Milenkovic et al. (2013) explicitly compute the free space for complete motion planning.

Thus, several approaches to representing configuration spaces of 2D objects, both exact and approximate, have been proposed and successfully implemented in the past. The problem is however more difficult if we consider a 3D object, as its configuration space is 6-dimensional. In the recent survey on caging by Makita and Wan (2017), the authors hypothesise that recovering a 6D configuration space and understanding caged subspaces is computationally infeasible. To the best of our knowledge, our paper presents the first practical and provably-correct method to approximate a 6D configuration space.

Our approximation is computed by decomposing the configuration space into a finite set of lower dimensional slices. Although the idea of slicing is not novel and was introduced by Lozano-Perez (1983), recent advances in computational geometry and topology, as well as a significant increase in processing power, have made it possible to approximate a 6D configuration space on a common laptop. We identify two main challenges to slicing a 6D configuration space of a rigid object: how to quickly compute 3D slices of the free space, and how to efficiently discretize the orientation space. For slice approximation, our method relies on the fact that the collision space associated to a rigid object with a fixed orientation and an obstacle represented as a union of balls is itself a union of balls. Then we use the dual diagram to a union of balls presented by Edelsbrunner (1999) as an approximation of the free space of the slice. This way, we do not need to use generalized polygons, which makes previous approaches more difficult in 2D and very hard to generalize to 3D workspaces. To discretize , 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 paper we use the terms ‘object’ for objects and autonomous rigid robots (e.g., disc robots) moving in -dimensional workspaces, where . 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 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 is regular if it is equal to the closure of its interior: .

In the above definition, the interior of is the largest open set contained in , and the closure of is the smallest closed set containing . In this paper, we assume that both the object and the obstacles are regular sets. Now, we define an object and a set of obstacles as follows:

Definition 2.

A rigid object is a regular compact connected non-empty subset of . A set of obstacles is a regular compact non-empty subset of .

We approximate both the obstacles, and the object, as unions of balls which lie in their interior, that is, and with radii and respectively.

Let denote the configuration space of the object. We define its collision space1 as the set of the objects configurations in which the object penetrates the obstacles:

Definition 3.

, where denotes the object in a configuration . The free space is the complement of the collision space: .

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

Definition 4.

Two configurations and are called path-connected if there exists a continuous collision-free path between them: , . 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 -dimensional slices.

3.2 Slice-based representation of the C-space

In our previous work (Varava et al., 2017), we suggested that configuration space decomposition may be a more computationally efficient alternative to its direct construction. We represent the configuration space of an object as a product , and consider a finite covering of by open sets (this is always possible, since is compact): . We recall the notion of a slice (Varava et al., 2017):

Definition 5.

A slice of the configuration space of a rigid object, is a subset of defined as follows: , where is an subset of .

We denote a slice of the collision (free) space by (, respectively). For each slice, we construct an approximation of its collision space in such a way that our approximation lies inside the real collision space of the slice, .

This way, we approximate the entire collision space by a subset :

Now, we discuss how to construct slice approximations.

3.3 An core of the object

First of all observe that by Def. 3, if a subset of an object placed in configuration is in collision, then the entire object is in collision. Therefore, the collision space of is completely contained within the collision space of . This allows us to make the following observation:

Observation 1.

Consider an object and a set of obstacles . Let be two collision-free configurations of the object. If there is no collision-free path between these configurations for its subset , then there is no collision-free path connecting these configurations for .

Therefore, if some subset of in configuration is caged, then the entire object in the same configuration is caged. This means that if we construct in such a way that for any configuration there exists a subset of such that is in collision, then 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 is the set comprising the points of which lie at a distance2 of at least from the boundary of : .

Figure 2: An core remains inside the object when we slightly rotate it

Now, for an object and its -core , we write and respectively to mean that their orientation is fixed at . So, let denote the collision space of with a fixed orientation . Note that since the orientation is fixed, we can identify with a subset of .

In Varava et al. (2017), we showed that for an object , and a fixed orientation there exists a non-empty neighbourhood of such that for any , is contained in , see Fig. 2.

In Sec. 5, we address the problem of representing and discretizing the space of orientations , 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 , defined as a set of points lying at most at distance from , and let us call it a -offset of the object: . Equivalently to Def. 7, we can say that two configurations and are -connected in if and only if they are path-connected in .

Consider now the following modification of our algorithm. If we enlarge the core by a offset, then our rotated object will not be guaranteed to contain it anymore, but the distance between any point of the enlarged core that is located outside of the rotated object and the object itself will not exceed . This means that if for this enlarged core our algorithm reports that two configurations are disconnected, then they either are disconnected in reality, or can be connected by a path of clearance at most . Let us denote the resulting space approximation by .

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 , but connected in , 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 , we might get more caging configurations. These results will be considered with respect to the used parameter : if two configurations are disconnected in , 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

Elements of can be seen as parametrizing rotations in , and for any we define 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 :

Definition 8.

Let denote the maximal displacement of any point after applying , i.e. , then if .

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 2D

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

assuming that we rotate the object around its geometric center, denotes the maximum distance from it to any point of the object, and is the rotation angle.

In the two-dimensional case, discretization of the rotation space is simple: given a desired number of slices, we obtain the displacement induced by rotation between two neighboring orientations, and compute a set of orientation samples , where . Then, we choose the . This gives us a covering of , where for each we define .3.

5.2 Displacement in 3D

We now discuss the three-dimensional case. Similarly to the previous case, our goal is to cover with balls of fixed radius. To parametrize we use unit quaternions. For simplicity, we extend the real and imaginary part notation from complex numbers to quaternions, where and denote the real and “imaginary” parts of the quaternion . Further, we identify with the vector ; and we write , and to mean the conjugate and the norm , respectively.

A unit quaternion defines a rotation as the rotation of angle around axis . This allows one to calculate the displacement of the rotation as:

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

where and are two elements of represented as unit quaternions which are regarded as vectors in , and is their inner product. We define the angle distance from a point to a set in the usual way as

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

Intuitively, the dispersion of a set determines how far a point can be from , and in this way it determines how well the set covers . Now, assume we are given a set of samples such that . Then for any point denote , we want to show that in these conditions, there exists some small such that . This would imply that there exists some such that if we take , then denoting , the family fully covers and for any satisfies .

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 , the following equation holds:

(1)

The proof of Proposition 1 can be found in appendix.

This means that if we want to cover with patches centered at a point such that for any is smaller than some , we can use any deterministic sampling scheme on (e.g. (Yershova et al., 2009)) to obtain a set with dispersion . Finally, by considering patches of the form , we obtain the corollary:

Corollary 1.

If has a dispersion then the family of patches forms a cover of .

Given such a cover of , recall that we want to approximately reconstruct the full free space of the object as the union of slices . 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 and edges if .

To compute the graph adjacency, we note that if two slices overlap, there must exist some such that , which implies . This is leveraged in Alg. 1.

input :  — a set of points in .
 — dispersion of .
output :  —- a patch adjacency graph.
1
2
3
4 for  do
5      
6      
7       for  do
8            add to
9            
10       end for
11      
12 end for
13return
Algorithm 1 ComputeAdjacentOrientations

The algorithm starts by setting , as this is the set of vertices in the graph, and we put these vertices in a in order to quickly perform nearest neighbor queries. Now, to compute the set of edges , we locate for each the points at an angle distance smaller than 4 in line 1. Finally, in line 1 we also add edges to the points at an angle distance smaller than of , as both and represent the same orientation.

5.3 Different resolutions and dispersion estimation

Figure 3: In this figure we show an approximation of a drill as a collection of balls around its medial axis. This representation is visualized at two distinct orientations which are adjacent in the graph over . The labels underneath each subfigure show how many vertices the corresponding graph has. As expected the larger the grid, the denser the sampling of 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.
input :  — Points in .
output :  —- Approximate dispersion.
1
2
3 for   do
4      
5      
6      
7      
8 end for
return
Algorithm 2 Approximating the dispersion of a set of points

One of the prerequisites of Alg. 1 is the availability of an estimate of the dispersion of . In our implementation we used the algorithm designed by Yershova et al. (2009) to compute where the authors provide a provable upper bound for dispersion.

However, since this is an upper bound, and we want a tighter estimate of the dispersion to reduce the number of slices, in our implementation we employed a random sampling method to estimate the dispersion.

Alg. 2 summarizes the procedure by which we approximate the dispersion. On each iteration of the main loop (lines 2-2) it retrieves an element of which is locally the furthest from this is done by drawing a random sample followed by gradient ascent in line 2. In line 2 the nearest element to of is retrieved (since the distance to this element will be the distance from to ), and finally in 2 we use the fact that to update the current value of the dispersion.

5.4 The choice of and discretization in practice

Depending on the chosen discretization of the orientation space and its dispersion, the value can be chosen such that it is greater than the estimated displacement:

To discretize , we compute 3 different grids corresponding to different resolution levels using the software provided by Yershova et al. (2009). The first grid contains 576 vertices, and the respective dispersion estimate is 0.23. The second grid consists of 4608 vertices and its estimated dispersion is 0.10, and the third grid has 36864 vertices and a disperion estimate of 0.05. Fig. 3 illustrates how much an object rotates between two neighboring orientations in each of these grids. In our experiments, we analyze how the performance of the algorithm is affected by the choice of a grid.

6 Free Space Approximation

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

Let be a graph approximating the free space. The vertices of correspond to the connected components of each slice, , and are denoted by , where and are the corresponding component and orientation interval. Two vertices representing components and , , are connected by an edge if the object can directly move between them. For that, both the sets , and must overlap: . approximates the free space of the object: if there is no path in between the vertices associated to the path components of two configurations , then they are disconnected in .

We start by choosing one orientation and run a breadth-first search over the orientation grid . When reaching a particular orientation, we construct the slice corresponding to it. In line 3, we compute the free space of a slice as explained in Alg. 4. In line 3, we check which connected components of adjacent slices overlap, and add edges between them, see Alg. 5.

input : , —- spherical representations of the obstacles and the object.
—- clearance parameter
 — a grid over
output :  —- a connectivity graph of the free space.
1
2
3
4
5
6
7
8 while  do
9      
10      
11       for  do
12             if  then
13                   if  then
14                        
15                        
16                        
17                        
18                   end if
19                  
20                  
21             end if
22            
23       end for
24      
25      
26 end while
27return
Algorithm 3 ComputeConnectivityGraph

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 Alg. 4. Given a set of obstacles, an object, and a particular orientation of its core, we start by computing the collision space of the slice.

In Varava et al. (2017), we derive the following representation for the collision space of :

where is the origin chosen as the geometric center of the object, and denotes the vector from to .

Indeed, the object represented as a union of balls collides with the obstacles if at least one of the balls is in collision, so the collision space of the object is a union of the collision spaces of the balls shifted with respect to the position of the balls centers:

Now, each ball collides with the obstacles when the distance from the obstacles to its center is not greater than the radius of the ball, so the collision space of a single ball of radius can be written as:

input :   — spherical representations of the obstacles and the core.
—- orientation.
output :   — A set of connected components of the slice corresponding to
1 Collision-Space(, , )
2 Weighted-Voronoi-Diagram
3 Dual-Diagram()
4 Compute-Infinite-Component()
5
6 foreach   do
7       if Connected-Component()  then
8            
9             Compute-Component()
10       end if
11      
12 end foreach
return
Algorithm 4 ComputeSlice
Figure 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).

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.

Figure 5: From left to right, the elements of the dual diagram approximating the free space. We start with building a Voronoi diagram (visualized in black); the second figure shows a set of finite orthogonal balls, corresponding to the regular edges of the diagram; the third figure visualizes the “infinite” orthogonal balls corresponding to the infinite edges of the diagram; finally, the last figure depicts full diagram. Red circles with blue centers represent the collision space, the yellow circles approximate the dual diagram.

A dual diagram of a union of balls is a finite collection of balls such that the complement of its interior is contained in and is homotopy equivalent to . It is convenient to approximate as a dual diagram of collision space balls for several reasons. First, balls are simple geometric primitives which make intersection checks trivial. Second, the complement of the dual diagram is guaranteed to lie strictly inside the collision space, which provides us with a conservative approximation of the free space. Finally, homotopy equivalence between its complement and the approximate collision space implies that our approximation preserves the connectivity of the free space that we want to approximate. Another advantage of a dual diagram is that it is easy to construct.

A weighted Voronoi diagram is a special case of a Voronoi diagram, where instead of Euclidean distance between points, a special distance function is used. In our case, the weighted distance of a point from is equal to .

To construct a dual diagram of a union of balls , we first construct their weighted Voronoi diagram, see Fig. 4. For each vertex of the weighted Voronoi diagram, let whose radius is equal to the square root of the weighted distance of to any of the four (three in the two-dimensional case) balls from generating . Then, take each infinite edge of the Voronoi diagram, and add an degenerate “infinitely large” ball (a half-space) with center at the infinite end of this edge. The entire process of dual diagram construction can be seen on Fig. 5.

After constructing the dual diagram, we find pairs of overlapping balls in it and run depth-first search to identify connected components. It is important to note that each dual diagram always has exactly one unbounded connected component, representing the outside world.

Finally, let us discuss how to understand whether free space approximations of neighboring slices overlap. In Alg. 5, to check whether two connected components in adjacent slices intersect (line 5), 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.

input :  —- free space approximations of two adjacent slices.
output :  — a set of edges between the connected components of and .
1
2 for  do
3       for  do
4             if  AND  then
5                  
6                  
7             end if
8            else
9                   if   then
10                        
11                        
12                   end if
13                  
14             end if
15            
16       end for
17      
18 end for
return
Algorithm 5 AddEdges
Remark 1.

Recall that in each slice the are constructed as the dual of the collision space , which entails that has the same connectivity as . However, it also entails that any connected component of partially overlaps with the collision space . This means that for two connected components of adjacent slices which do not overlap, it may occur that the corresponding approximations do overlap. In this case the resulting graph would contain an edge between the corresponding vetices. This effect can be mitigated by verifying whether the overlap between the approximations occurs within the collision space of both slices. This can be done for example by covering the intersection with a union of balls and checking if it is contained inside the collision space .

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 simultaniously. Alg. 6 describes the process.

Recall that in the single-thread version of the algorithm (see Alg. 3), we performed breadth-first search (BFS) over the orientation grid . For each orientation, we computed the corresponding slice of the free space and its overlap with neighboring slices. The slice was deleted when all its neighbours were constructed and the connectivity check between them was performed.

A naive approach to parallelization would have a main thread call child threads on to process each new orientation in the queue, however since the orientations are enqueued in sequence it would increase the chances that different 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.

input :  , —- spherical representations of the obstacles and the object.
—- clearance parameter.
 — a graph over .
—- an initial orientation.
shared :  —- an array containing a mutex per slice.
—- an array of slices (one per orientation).
—- an integer array indicating slice status (0:unseen, 1:seen, 2:fully processed).
—- a connectivity graph of the free space
1
2
3
4 while  do
5      
6      
7       if   then
8            
9            
10            
11       end if
12      if   then
13            
14             for  do
15                  
16                   if  then
17                         if  then
18                              
19                              
20                              
21                         end if
22                        if  then
23                              
24                              
25                         end if
26                        
27                        
28                   end if
29                  
30             end for
31            if   then
32                  
33                  
34                  
35             end if
36            
37       end if
38      
39 end while
Algorithm 6 ConnectivityGraphThread

Alg. 6 describes the procedure followed by each thread. It assumes that all threads share an access to the array of slices and the connectivity graph, as well as the book-keeping data, i.e. an array of mutexes, and an array containing the status of each slice. The status array () is used to decide what (if anything) should be computed about a given slice at a given moment, whereas the mutex array () is used so two threads don’t try to alter the same slice at the same time.

The main loop (lines 6-6) proceeds as follows: it pops the first element of the queue 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 6 and 6), if the slice associated to the orientation has yet to be computed it does so (line 6) and if its edges have not yet been processed it goes on to process the edges to its neighboring slices (lines 6-6). To compute the edges between the slices corresponding to the current orientation and an adjacent orientation the thread needs to also lock the mutex associated to (line 6) if it does so successfully then it proceeds to check the connection between and , otherwise it checks the next adjacent orientation. When it has tried to calculate the edges to all the orientations adjacent to it verifies if all of them were successfully calculated (line 6), if so, the current orientation is marked as fully processed and the slice is deleted. Note also that the edges between and only get computed in line 6 if 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 , the second one has to wait for the first one to finish before it can proceed.

  • If there is an edge of the orientation graph and one thread has and a second thread has , 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 ( in line 6) and this status is revoked immediately afterwards (line 6) before it is enqueued. Furthermore since this occurs while the mutex associated to the corresponding slice is locked, no other thread is able to enqueue the same orientation. This guarantees that each orientation is only enqueued once.

Alg. 6 is called by a threadpool as shown in Alg. 7. The algorithm ComputeConnectivityGraphParallel works by setting up the data that must be shared between the several instances of ConnectivityGraphThread, and calling it starting from different orientations in . 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 6 of Alg. 6, and are therefore available to be further processed in line 7 of Alg. 7.

input : , —- spherical representations of the obstacles and the object
—- clearance parameter.
 — a graph over
output :  —- a connectivity graph of the free space
1 shared
2 shared
3 shared
4 shared
5 while   do
6      
7      
8      
9       if   then
10            
11            
12       end if
13      
14 end while
15
16 for   do
17       if   then
18             for  do
19                   if   then
20                        
21                        
22                   end if
23                  
24             end for
25            
26            
27       end if
28      
29 end for
return
Algorithm 7 ComputeConnectivityGraphParallel

The function is used to select the next quaternion which has not been seen before. In our implementation we use a simple scheme by drawing a random number and choosing the first so that .

8 Theoretical Properties of Our Approach

In this section, we discuss 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 and a set of obstacles . Consider two collision-free configurations of the object. If they are not path-connected in , then they are not path-connected in .

Proof.

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

where

(2)

Now, recall that by definition  (Edelsbrunner (1999)), and that we choose and so that for any , . This implies that for any , and conversely that . Finally, since , we have:

(3)

We now want to show that if there is no path between two vertices and