Understanding the global topological and geometric properties of the free space is important for both robotic manipulation and motion planning.
In manipulation, the mobility of an object may be constrained by manipulators and/or obstacles. Here, it is crucial to understand the object’s free space to quantify how far it can move from its initial configuration. When the object cannot escape arbitrarily far, we in particular say that the object is caged. Formally, this means that it is located in a compact path-connected component of its free space. Caging can be applied to robotic grasping and multi-agent manipulation.
One of the biggest challenges in caging is verification – i.e., designing efficient algorithms providing theoretical guarantees that a given configuration is a caging configuration. To prove that an object is located in a bounded path-connected component of its free space requires knowledge about the entire configuration space, which is high dimensional even in the case of rigid objects (three-dimensional when the workspace has only two dimensions). This makes direct reconstruction of the free space computationally expensive. Another approach towards verifying caging relies on particular geometric and topological features of the object under consideration. These algorithms can be computationally efficient, but they are limited to particular classes of objects’ shapes.
In contrast, the only assumption we make about the shape of the object is that it should not have ‘thin parts’, see Sec.III for details. We also address the problem of proving path non-existence between a pair of given configurations. To prove that there is no path, one has to show that two configurations belong to different path-connected components.
Apart from the purely topological property of being located in a certain path-connected component, it is also important to quantitatively describe the amount of freedom the object possesses in a certain configuration. Indeed, assume that our task is to cage an object and move it to a certain position. The more mobility the object has within the cage, the less precision we can achieve trying to put it into the goal position. Therefore, we would prefer those caging configurations where the object’s mobility is more restricted. To distinguish between different cages, we compute the volume of path-connected components.
To show that two configurations are disconnected, we construct an approximation of the object’s collision space. Intuitively, our approximation is a set of projections of a subset of the object to planes with fixed orientations, see Fig.1. By construction, our approximation is a proper subset of the real collision space, which means that if our algorithm reports that the two configurations are disconnected, then there is no path between them in the real free space. However, for the same reason our algorithm is not guaranteed to find all the possible caging configurations, since we do not reconstruct the entire collision space.
In , we presented the general idea of our approach. While in this paper, we present an elaborate theoretical framework, prove the correctness of our method, implement the algorithm for two-dimensional workspaces, and report the experimental results. The core contributions of the present paper with respect to  can be summarized as follows: (i) we provide a correctness proof of our algorithm (i.e., we show that if in our approximation of the free space two configurations are disconnected, then they are disconnected in the real free space). (ii) We provide algorithms to compute lower bounds for volume of path-connected components and width of narrow passages. (iii) We provide implementation details and run experiments in a two-dimensional workspace. (iv) We prove that if two configurations are disconnected, we can construct a good enough approximation of the free space to show that these configurations are either disconnected or connected by a narrow passage.
Ii Motivation and Related Work
In grasping, caging can be considered as an alternative to a classical grasp [9, 10, 14, 20], as well as an intermediate step on the way towards a firm grasp . Unlike classical grasping, caging can be formulated as a purely geometric problem, and therefore one can derive sufficient conditions for an object to be caged. Apart from that, caging deals with global geometric features of the object, which may in applications be more robust to noise than local geometry. To prove that a rigid object is caged it is enough to prove this for any subset (part) of the object. This allows a large enough subset of the object lying strictly inside the real object to be considered. This makes caging robust to noise and uncertainties appearing in the process of shape reconstruction and position detection. The notion of a planar cage was initially introduced by Kuperberg in 1990  as a set of coplanar points lying in the complement of a polygon and preventing it from escaping arbitrarily far from its initial position. In tobotics, it was subsequently studied in the context of point-based caging in 2D by Rimon and Blake , Pipattanasomporn and Sudsang , Vahedi and van der Stappen , and others. A similar approach has also been adopted for caging 3D objects. For instance, Pipattanasomporn and Sudsang  proposed an algorithm for computing all two-finger cages for non-convex polytopes.
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. [14, 17, 18], Varava , Makita at al. [9, 10]. In these works, sufficient conditions for caging were derived for objects with particular shape features.
In this paper, we address caging as a special case of 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. 
in the context of motion planning, motivated by the fact most of the modern sampling-based planning algorithms do not guarantee that two configurations are disconnected, and rely on stopping heuristics in such situations. Basch et al. prove that two configurations are disconnected when the object is ‘too big’ or ‘too long’ to pass through a ‘gate’ between them. In , Zhang et al. use approximate cell decomposition and prove path non-existence. They decompose a configuration space into a set of cells and for each cell decide if it lies in the collision space. In  McCarthy et al. propose a somewhat similar approach. There, they randomly sample the configuration space and reconstruct its approximation as an alpha complex. They later use it to check the connectivity between pairs of configurations.
In this paper, we also aim to study path-connectivity of the free space of the object. Unlike , we do not construct the collision space directly. Instead we decompose it into a finite set of lower dimensional ‘slices’. This allows us to overcome the dimensionality problem without losing any necessary information about the topology of the configuration space.
Iii Definitions, Notation, and Overview
Let us now provide the necessary definition and formulate our problem. Since our work is related both to object manipulation and motion planning, we use the general term ‘object’ without loss of generality when talking about both objects and autonomous rigid robots (e.g., disc robots) moving in two-dimensional workspaces.
Iii-a Definitions and Notation
A rigid object is a compact connected non-empty subset of . A set of obstacles is a compact non-empty subset of .
We would like to limit ourselves to a large class of sufficiently ‘good’ shapes: namely, we want both the object and the obstacles to be representable as a set of dimensional balls. Therefore, we do not allow them to have ‘thin parts’. To formalize this notion, we recall the definition of a regular set:
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 .
We make the following assumption:
Both the object and the obstacles are regular sets.
We approximate both the obstacles and the object as unions of balls lying in their interior, and of radii and respectively.
Let denote the configuration space of the object. We define its collision space as the set of the objects configurations in which the object penetrates the obstacles:
, 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.
Two configurations and are called path-connected if there exists a continuous collision-free path between them: , . If two configurations are path-connected, they belong to the same path-connected component.
Iii-B An Overview of Our Approach
To compute path-connected component of the free space, we decompose the free space into a set of two-dimensional slices. The key idea of this approach has been discussed in our paper . Now we provide the implementation details and use our approach to compute geometric characteristics of the free space introduced in the next section.
In , we suggested that configuration space decomposition might be a more computationally efficient alternative to its direct construction. Namely, we represent the configuration space as a product , and consider a finite covering of :
Note that this is always possible, since is compact.
We recall the notion of a slice, introduced in :
A slice of the configuration space is a subset of defined as follows:
where is an subset of .
We denote a slice of the collision (free) space by (). 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 its subset :
In Sec. V, we recall how we construct these approximations and provide details of our approach.
Iv Free Space and its Properties
Let us now discuss how we can quantitatively estimate the ‘quality’ of path-connected components and paths between different parts of the free space. We start with the volume of a component, which can be interesting for object manipulation. Then, we consider the width of narrow passages of the free space, which can be applied to motion planning.
Iv-a The Volume of Path-Components
In robotic manipulation, it is useful to distinguish between big and small connected components, see Fig. 2. Recall that the free space is a subset of . There is a natural parametrization of as a one-dimensional manifold by the angle and therefore we can represent an element of by .
Now, note that since is a Lie group , each point gives rise to a diffeomorphism . Concretely, given a point in , this function has the form:
which is a diffeomorphism of to itself. Furthermore, from this we can obtain the volume form of as the pullback of a volume element along :
Therefore, choosing we can obtain the volume of a subset of as:
Thus, for a set of the form in , we get .
Iv-B The Width of Narrow Passages
Motion planning becomes more challenging in presence of narrow passages. In particular, although our definition of collision space allows the object to be in contact with the obstacles, in practice we might want to avoid such situations. Furthermore, since we deal with approximations of objects and obstacles, the notion of robustness of a path is useful. Therefore, another interesting geometric characteristic is the width of narrow passages of the free space. See Fig. 3.
Let denote the signed distance from the point to the set , defined as if and if .
We say that two configurations are connected if there is a clearance path between them, or, more formally, we define connectivity as follows:
Two collision-free configurations of are connected, if there is a collision-free path between them such that the signed distance from any point of the object to the obstacles along this path is not less than :
Note that connectivity is a generalization of path-connectivity: two configurations are path-connected if and only if they are connected.
In the next section, we discuss how we check connectivity of the free space.
V Free Space Decomposition
V-a 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:
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 the original object.
If some subset of in the configuration is caged, then the entire object in the same configuration is also caged.
From the above observation it follows 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  we defined an core of an object as follows:
An -core of an object is a subset of the object such that any point of lies at least at a distance111By distance here we mean Euclidean distance in from the boundary of :
Let us denote an object and its -core by and respectively when their orientation is fixed. Let denote the collision space of the core with a fixed orientation . Note that since the orientation is fixed, is a subset of .
In , we showed that for an object and its -core with a fixed orientation there always exists a non-empty neighbourhood of such that for any the -core is fully contained within a slightly rotated object , see Fig. 4.
So, we construct the collision space approximation as follows: we pick an and a sufficient set of orientation values so that form a covering of such that for any we have . This gives us a decomposition of the entire configuration space:
For each , we compute collision space slice approximations as the collision space of :
Let denote the maximal displacement of any point after rotating it to an angle around the object’s geometric center:
where is the rotation operator. Then is fully contained inside a rotated object if . In , we have shown that
assuming that we rotate the object around its geometric center and denotes the biggest distance from it to any point of the object. Note that monotonically decreases with .
So, given an we pick such that , and compute a set of orientation samples . This gives us a covering of of the form
Given and , we approximate the collision space by
V-C Construction of Slices
In , 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 fromto .
For simplicity, in our implementation we assume that the obstacles and the object are approximated by sets balls of equal radii: , .
Now, let us discuss how we construct path-connected components of . Since is a collection of balls, we represent it as an alpha complex.
An alpha complex representing a union of balls is a simplicial complex with vertices which lies strictly inside . By the nerve theorem, an alpha complex is homotopy equivalent to the union of balls it approximates .
Assume we have a finite set of points . Let us continuously increase the radius of the balls centered at these points, and consider a nested family of unions of balls. Correspondingly, we get a nested family of alpha complexes, , where is the Delaunay triangulation of . Since is finite, and any is its subcomplex, the family of nested subcomplexes is also finite.
Here by we denote a triangular representation of the three-dimensional path-connected component of a slice. These approximations are constructed as connected components of the complement of the alpha-complex representing the collision space of a slice.
Delaunay-Triangulation() computes the Delaunay triangulation based on the centers of the balls representing the collision space.
Compute-Infinite-Component() computes an artificially created connected component representing the unbounded connected component of the free space. It contains those exterior triangles which can be reached from the boundary of the Delaunay triangulation.
Get-Exterior-Triangles returns a list of faces of the Delaunay triangulation lying outside of the alpha-complex , which approximates the collision space represented as a union of balls of radius .
V-D The Connectivity Graph
Once we have computed connected components of each slice, we can analyze the connectivity between slices. A detailed description of this procedure can be found in , and we briefly recall it here. 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, the sets and must overlap: , and the respective triangular approximations of path-connected components must intersect: .
approximates the free space of the object: if two configurations are disconnected in , then they are disconnected in . Moreover, we later show that if two configurations are not path-connected in , then they are not connected in for any .
V-E The Width of the Narrow Passages
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:
A offset of an object is defined as
where denotes Euclidean distance in .
Now observe that by definition, two configurations are connected if and only if there exists a collision-free path connecting these configurations in the configuration space of the -offset of the object. This means that two configurations and are not connected in if and only if they are not path-connected in .
Therefore, to understand the connectivity of the free space it is enough to compute path-connected components as described in Alg. 1 for the -offset .
We can also slightly modify Alg. 1 to find potential narrow passages for each . In this case, instead of constructing a Delaunay triangulation, we can compute a nested family of alpha-complexes of the centers of the collision balls for all positive alpha. As we mentioned before, this family is always finite. Increasing the parameter corresponds to increasing the radius of the object under consideration, i.e., the family of alpha complexes approximates a nested family of collision space approximations of offsets of the object for all positive . As we increase , the topology of the free space of the slice will change, and the narrow passages will disappear. The corresponding values of reflect the width of the passages.
Vi Theoretical Guarantees of Our Approach
In this section, we discuss correctness, completeness and computational complexity of our approach.
First of all, let us show that our algorithm is correct: i.e., if there is no collision-free path between two configurations in our approximation of the free space, then these configurations are also disconnected in the actual free space.
Consider an object and a set of obstacles . Let be two collision-free configurations of the object. If and are not path-connected in , then they are not path-connected in .
Recall that the approximation of the free space is constructed as follows:
Let us first recall that by construction for any we have , and therefore .
Therefore, , which together with (1) implies that
We now want to show that if there is no path between two vertices and in , then there is no path between connected components of corresponding to them. It is enough to show that if two vertices corresponding to adjacent slices are not connected by an edge, then they represent two components which are disconnected in the union of these adjacent slices.
Consider two adjacent slices and , and two path-connected components and . Let and respectively be their triangular representations.
Let and be two vertices of corresponding to these components: and . Let us show that if there is no edge between and , then there is no path between the corresponding components and in . Indeed, since an alpha complex representing the collision space of a slice is a subset of its actual collision space, the complement of the alpha complex is a superset of the actual free space of the slice. So, and . Now, if there is no edge between and , then . This implies that , and therefore and are two disjoint components of .
We would like to show that if two configurations are not path-connected in , we can always construct an approximation of in which these configurations are either disconnected or connected by a narrow passage.
Let be two configurations in . If they are not path-connected in , then for any and there exists such that the corresponding configurations are not connected in the approximated free space .
First, observe that if two configurations and are not path-connected in , then for any they are not connected in . We want to show that they are not connected in for a particular choice of . Let be such that
where is the displacement function.
Since and are not path-connected there exists a collision configuration in any path between them, so, . Therefore, for the same configuration we have . Let be such that , and let . We get
Vi-C Computational Complexity
Let us now estimate the computational complexity of our approach. Let and be the number of balls in the obstacles and the object approximations, respectively, and let be the number of slices we need. Let us focus on the basic version of our algorithm when we are not interested in connectivity of the space, and limit ourselves to path-connectivity. Our algorithm has two major steps: first, we compute approximations of the slices and their path-connected component, and then we construct the connectivity graph. For each slice, we compute a Delaunay triangulation of the union balls, pick , and compute the path-connected components of the corresponding alpha-complex. The computation of path-connected components is linear on the number of faces of the complex, and therefore the overall complexity of this step is dominated by the computation of Delaunay triangulation, which is .
Since we need to compute slices, the overall complexity of the first step of the algorithm is .
Once the slices are computed, we proceed to the next step – the connectivity graph construction. Assume that each slice has at most path-connected components. We create vertices of the connectivity graph by iterating through all the components of each slice, which gives us at most iterations. Then for each pair of overlapping slices we check whether their path-connected components intersect. Assume that each slice has at most faces in its alpha-complex representation. Given our partition of , each slice has 2 neighbours, so the overall complexity of the edges construction is at most . Note that this is a conservative estimation. In our current implementation, we check whether two components from different slices intersect by iterating over all the triangles in their triangulations, which makes the complexity of this procedure quadratic on the number of triangles. As a consequence, this step takes the majority of the overall computation time of the algorithm. However, this can be significantly improved using, for instance, quadtrees.
Finally, we compute connected components of the connectivity graph, The complexity of this procedure is , since each slice has 2 neighbours, and hence each vertex of the connectivity graph has at most adjacent vertices.
This gives us the overall complexity . In practice, the number of connected components per slice is small unless the original space has a lot of path-connected components. So, the most computationally expensive parts are slices construction – , and the connectivity graph edges construction – . Note that both these parts can be parallelized. Each slice approximation can be computed separately, because the slices do not depend on each other. For each pair of adjacent slices we can compute the intersections between their connected components independently.
In Sec. V we said that the partition of is constructed in such a way that the distance between two consecutive orientation samples should be chosen such that the maximal displacement does not exceed the chosen . Note that should be less than the radius of the smallest ball in the object’s representation in order to preserve the shape of the object. On the other hand, Fig. 5 shows that the number grows significantly when we decrease the . Finally, the possible choice of depends on the shape of the object: if the shape is easy to approximate with a small number of large balls, then we can choose a large without losing any crucial information about the shape. In contrast, if the object has a lot of thin parts, it has to be approximated with a larger number of smaller balls. In this case we have to choose a smaller , which increases the necessary number of slices.
Vii Experiments and Results
We consider a simple environment, see Fig. 6. 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 thin parts cannot be approximated by large balls, see Fig. 7.
We use CGAL library to compute Delaunay triangulation and alpha complexes. Our experiments were run on an Intel Core i7 laptop with 12 Gb RAM.
We consider a simple object whose approximation consists of 5 balls. We run our algorithm for all the 3 approximations of the workspace, and take 5 different values of , see Table I. 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 the table).
|2 c.; 741 ms||3 c.; 1287 ms||4 c.; 1354 ms|
|2 c.; 688 ms||3 c.; 1208 ms||4 c.; 1363 ms|
|2 c.; 647 ms||3 c.; 1079 ms||4 c.; 1287 ms|
|2 c.; 571 ms||3 c.; 986 ms||3 c.; 1156 ms|
|2 c.; 554 ms||3 c.; 950 ms||3 c.; 1203 ms|
When we were using our first approximation of the workspace, we were able to distinguish only between components 4 and 2 (see Fig. 7), 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. The accuracy of a workspace approximation depends on the task: for instance, if the only thing we need to know is whether the object can escape arbitrarily far from the obstacles, then it is enough to use a rough approximation.
|Component 1||Component 2||Component 3|
We have also computed an estimation of the volume of the 3 bounded components, see Table II. For that, we used the most accurate approximation of the workspace, and 5 different values of . We can see that when we increase , the size of the components increases, because larger values of provide more conservative (larger) approximations of the free space. For the last 2 values of components 1 and 2 merge, and hence the volume of component 2 increases significantly.
In this paper, we propose an approach towards proving caging and path non-existence for rigid objects in 2D workspaces. We compute an approximation of the collision space of the object, represent it as a collection of lower dimensional projections, and analyze path-connectivity of the free space of the object. Apart from that, we estimate the volume of path-connected components and the width of narrow passages. We perform an experimental evaluation of our approach and show that our algorithm is correct and complete.
-  Barraquand, J., Kavraki, L., Latombe, J.-C., Motwani, R., Li, T.-Y., Raghavan, P.: A random sampling scheme for path planning. In: The International Journal of Robotics Research, 16(6), 759–774 (1997).
-  Basch, J., Guibas, L. J., Hsu, D., Nguyen, A. T.: Disconnection proofs for motion planning. In: IEEE International Conference on Robotics and Automation (2001), 1765-1772.
-  Bullo, F., Lewis, A. D.: Geometric Control of Mechanical Systems. Springer, 2005.
-  Edelsbrunner, H., Harer, J.: Computational topology: an introduction. American Mathematical Soc., 2010.
-  Guibas, L.J., Stolfi, J.: Primitives for the manipulation of general subdivisions and the computation of Voronoi diagrams. In: ACM Trans. Graphics, 4 (1985), 74-123.
-  Kuperberg, W.: Problems on polytopes and convex sets. In: DIMACS Workshop on polytopes (1990), 584-589.
-  Latombe, J.-C.: Robot Motion Planning. Norwell, MA, USA: Kluwer Academic Publishers (1991).
-  Mahler, J., Pokorny, F. T., McCarthy, Z., van der Stappen, A. F., Goldberg, K.: Energy-bounded caging: Formal definition and 2-D energy lower bound algorithm based on weighted alpha shapes. In: IEEE Robotics and Automation Letters, 1(1), 508-515 (2016).
-  Makita, S., Maeda, Y.: 3D multifingered caging: Basic formulation and planning. In: IEEE Intelligent Robots and Systems (2008), 2697–2702.
-  Makita, S., Okita, K., Maeda, Y.: 3D two-fingered caging for two types of objects: sufficient conditions and planning. In: International Journal of Mechatronics and Automation, 3(4), 263-277 (2013)
-  McCarthy, Z., Bretl, T., Hutchinson, S.: Proving path non-existence using sampling and alpha shapes. In: IEEE International Conference on Robotics and Automation (2012), 2563-2569.
-  Pipattanasomporn, P., Sudsang, A.: Two-finger caging of concave polygon. In: IEEE International Conference on Robotics and Automation (2006), 2137–2142.
-  Pipattanasomporn, P., Sudsang, A.: Two-finger caging of nonconvex polytopes. In: IEEE Transactions on Robotics, 27(2), 324-333 (2011).
-  Pokorny, F. T., Stork, J. A., Kragic, D.: Grasping objects with holes: A topological approach. In: IEEE International Conference on Robotics and Automation (2013), 1100–1107.
-  Rimon, E., Blake, A.: Caging planar bodies by one-parameter two-fingered gripping systems. In: The International Journal of Robotics Research, 18(3), 299–318 (1999).
-  Rodriguez, A., Mason, M. T., Ferry, S.: From caging to grasping. In: The International Journal of Robotics Research, 31(7), 886-900 (2012).
-  Stork, J. A., Pokorny, F. T., Kragic, D.: Integrated Motion and Clasp Planning with Virtual Linking. In: IEEE/RSJ International Conference on Intelligent Robots and Systems (2013), 3007-3014.
-  Stork, J. A., Pokorny, F. T., Kragic, D.: A Topology-based Object Representation for Clasping, Latching and Hooking. In: IEEE-RAS International Conference on Humanoid Robots (2013), 138-145.
-  Pereira, G. A. S., Campos, M.F.M., Kumar, V.: Decentralized algorithms for multi-robot manipulation via caging. In: The International Journal of Robotics Research 23(7-8), 783 – 795 (2004).
-  Varava, A., Kragic, D., Pokorny, F. T.: Caging Grasps of Rigid and Partially Deformable 3-D Objects With Double Fork and Neck Features. In IEEE Transactions on Robotics, 32(6), 1479-1497 (2016).
-  Varava, A., Carvalho, J. F., Pokorny, F. T., Kragic, D.: Caging and Path Non-Existence: a Deterministic Sampling-Based Verification Algorithm. International Symposium on Robotics Research, 2017 (accepted, to appear). Preprint: http://www.csc.kth.se/~jfpbdc/path_non_ex.pdf
-  Vahedi, M., van der Stappen, A. F.: Caging polygons with two and three fingers. In: The International Journal of Robotics Research, 27(11-12) 1308–1324 (2008).
-  Zhang, L., Young, J. K., Manocha, D.: Efficient cell labelling and path non-existence computation using C-obstacle query. In: The International Journal of Robotics Research, 27(11-12), 1246-1257 (2008).