Decidability in robot manipulation planning

Abstract

Consider the problem of planning collision-free motion of n objects movable through contact with a robot that can autonomously translate in the plane and that can move a maximum of $m \le n$ objects simultaneously. This represents the abstract formulation of a general class of manipulation planning problems that are proven to be decidable in this paper. The tools used for proving decidability of this simplified manipulation planning problem are, in fact, general enough to handle the decidability problem for the wider class of systems characterized by a stratified configuration space. These include, e.g., problems of legged and multi-contact locomotion, bi-manual manipulation. In addition, the approach described does not restrict the dynamics of the manipulation system modeled.

1 Introduction

The problem of planning collision free motion for a free-flying single-body robot in environments populated by static obstacles has been widely studied in the past decades and can be considered today well understood. In this paper we consider a generalization of this basic problem by allowing the presence of movable obstacles, i.e., objects in the environment that the robot can move by “grasping” them, while avoiding collisions with all the obstacles and objects.

The problem of motion planning in the presence of movable obstacles was first addressed in Wilfong (1991), where the decidability is proven for the case of discrete grasps. This problem was further generalized in Alami et al. (1989) to the so-called manipulation planning problem where the movable obstacles are considered as objects to be moved to reach a goal position. In that paper the authors present an algorithm for the case of discrete placements and grasps. This is the formulation briefly described in Chapter 11 of Latombe’s book (1991). Decidability of the problem in the case of continuous grasps and placements was shown in Dacre-Wright et al. (1992) considering one movable object.

While Berg et al. (2010) provides an efficient probabilistically complete algorithm in the case of several movable obstacles, the decidability problem, i.e., the existence of an exact algorithm that decides whether a solution exists in finite time, remained open even in the case of two movable objects as also mentioned in Karagoz et al. (2004).

In this paper we prove that the manipulation planning problem is decidable for a robot that can freely translate in the plane and manipulate up to $m\le n$ objects simultaneously, with n the total number of movable objects. The objects can move only if they are in contact with (“grasped” by) the robot. The proof is based on a cell decomposition of the collision-free contact configuration space, a refinement of the cells through a properly defined projection/lifting procedure, and on the reduction property. This property establishes the equivalence of two types of paths: namely, paths continuously satisfying the contact constraint (hereafter called paths in contact or contact paths) and manipulation paths, along which the objects either translate rigidly with the robot as a single object (transfer paths) or remain in a fixed position while the robot moves freely (transit path). To prove that the reduction property holds for the considered manipulation model we exploit the configuration space structure and investigate the controllability of the system based on the stratified controllability notion Goodwine and Burdick (2001).

The decidability procedure for this simplified case of manipulation planning problem uses methodological tools which are abstract enough to allow handling a more general class of manipulation planning problems.

The result, presented here, lays the basis for answering important questions such as under which conditions motion in contact can be reduced to a manipulation path, how to efficiently construct manipulation graphs related to many different problems (climbing, walking, multi-contact planning), and how to determine the rate of convergence of probabilistic planners for the manipulation of multiple objects. It is worth emphasizing that our main goal is to provide a versatile decision algorithm, and therefore we do not give any specific planning algorithm.

This paper is an extension of our previous work Vendittelli et al. (2015) in which initially we had only proved the decidability for 3 disks. A strongly related approach was later proposed in Deshpande et al. (2016), stating a more general result than the one in Vendittelli et al. (2015). In the present paper, we show that the method in Vendittelli et al. (2015) is general enough to consider an arbitrary number of movable objects with arbitrary shape and discuss the general characteristics of the approach which permits dealing with other classes of planning problems.

The work, presented here, differs from the close contribution in Deshpande et al. (2016) mainly in the approach that relies on (a) the decomposition of the composite configuration space and (b) the definition of a control model for the robotic manipulation system. This approach enjoys several advantages: namely, (i) it leads to easy-to-check conditions for decidability; (ii) it decouples the projection and lifting procedure performed on the unconstrained composite configuration space from the controllability properties of the manipulation system; this separation allows one to decide planning problems also for manipulation systems characterized by non-constant distributions; (iii) it can naturally encode kinematic constraints in the system dynamics; (iv) it considers also the case in which the robot can manipulate a maximum of objects $m < n$, where n is the total number of manipulable objects in the workspace.

Beyond the decidability procedure, this work contributes to the analysis of the manipulation planning problem structure in an accessible way, providing tools for analysing the decidability of robotic manipulation systems.

The paper is organized as follows. The next section formalizes the problem after defining the configuration space and its connectivity through manipulation paths. Section 3 analyses the controllability properties of the manipulation system. Section 4 establishes the conditions under which paths in contact can be reduced to manipulation paths. Section 5 illustrates the main steps for the construction of the manipulation graph. Section 6 opens perspective about the generalization of the results to other manipulation planning problems and finally, Sect. 7 concludes the paper.

2 Problem formulation

Consider the scene in Fig. 1: the “robot” R can translate autonomously in a polygonal (or semi-algebraic) environment populated by fixed obstacles and n objects $O_1, \, O_2, \ldots ,$$O_n$ that R can move by establishing a contact with them. More specifically, the objects $O_1, \, O_2, \ldots , O_n$ translate rigidly with the robot when in contact with it; otherwise, they are considered as fixed obstacles.

Fig. 1

Scenario of the considered manipulation planning problem discussed here

2.1 Configuration space

The configuration spaces of the robot and the objects are defined as:

• ${\mathcal {C}}_R = {\varvec{R}}^2$, the configuration space of the robot;

• ${\mathcal {C}}_{O_i} = {\varvec{R}}^2$ the configuration space of $O_i$, $i \in {1, \ldots , n}$.

The combined configuration space is obtained as ${{\mathcal {C}}}={{\mathcal {C}}}_R \times {{\mathcal {C}}}_{O_1} \times {{\mathcal {C}}}_{O_2} \times \cdots \times {{\mathcal {C}}}_{O_n} = {\varvec{R}}^{2(n+1)}$ and, accordingly, a configuration ${\varvec{q}}\in {{\mathcal {C}}}$ is described by the $(n+1)$-tuple ${\varvec{q}}= ({\varvec{q}}_R, {\varvec{q}}_{O_1}, {\varvec{q}}_{O_2}, \ldots , {\varvec{q}}_{O_n})$, where ${\varvec{q}}_R \in {{\mathcal {C}}}_R$, ${\varvec{q}}_{O_i} \in {{\mathcal {C}}}_{O_i}$, $i \in {1, \ldots , n}$.

The collision-free configuration space ${{\mathcal {C}}}_{\text{ free }}$ is obtained by removing from ${\mathcal {C}}$ the set of inadmissible configurations:

• ${\varvec{q}}_R$ such that the robot is in contact with static obstacles or overlaps with either static or movable obstacles;

• ${\varvec{q}}_{O_i}$, $i \in {1, \ldots , n}$ such that $O_i$ overlaps with the static obstacles, the robot or with ${O_j}$, $j\ne i$. Note that contact between objects and obstacles is allowed.

2.2 Configuration space paths and manipulation paths

Configuration space paths may or may not include contacts. To move the objects, however, the robot must be in contact with the objects. In the (composite and unconstrained) configuration space ${\mathcal {C}}$ any motion in contact is allowed.

Paths of interest in ${{\mathcal {C}}}$ can be categorized according to the two motion modalities:

• robot free motion: this is a path in ${{\mathcal {C}}}$ characterized by the absence of contact between the robot and the objects;

• contact motion: this is a path in ${{\mathcal {C}}}$ constrained by the condition that the robot is in contact with at least one object; along the path the robot position and the positions of the objects relative to the robot can change.

The paths described above might or might not be feasible. It is the dynamics of the manipulation system that defines the characteristics of grasping and, hence, of the feasible robot-object motion.

In the following developments we consider only manipulation by rigid grasp. Therefore, not all the configuration space paths are feasible in our setting. Feasible motions correspond to paths of two types:

• transfer paths along which the robot grasps at least one object and moves rigidly with it (the objects not grasped remain in a fixed position); along these paths the relative configurations between robot and objects in contact do not change.

• transit paths along which the robot moves alone and the objects remain in fixed positions.

A sequence of transit and transfer paths is called a manipulation path.

Fig. 2

Structure of ${{\mathcal {C}}}$ induced by the contact constraints and interconnection of the contact submanifolds through transit and transfer paths. The case illustrated in a corresponds to $n=2$ and $m=2$, while in b it is considered $n=2$ and $m=1$

2.3 Configuration space connectivity through manipulation paths

The maximum number m of objects that the robot can simultaneously manipulate affects the structure of ${{\mathcal {C}}}$ induced by the contact constraints. In this section we illustrate the structure of ${{\mathcal {C}}}$ in terms of the submanifolds defined by the contact constraints and their interconnection through transit and transfer paths.

Figure 2 shows representative configurations in each submanifold for the cases $n=2$ and $m=2$ or $m=1$, respectively illustrated in Fig. 2a, b. The embedding configuration space ${{\mathcal {C}}}$ has dimension 6 and foliates with the position of the movable objects. In particular, leaves of dimension 2 correspond to fixed positions of the two objects. Transit paths belong to one of these leaves. A representative configuration in this manifold is shown at the top of Fig. 2a. Manipulation paths across the leaves may require leaving the manifold.

Configurations on the second row (from top) of Fig. 2a represent the single-contact manifold which has dimension 5. The leaves of interest for the problem of interest have dimension 3 and correspond to fixed positions of the object which is not in contact with the robot. Manipulation paths across the leaves (change of the position of the object not in contact) require leaving the submanifold.

The double-contact submanifold, represented by the configurations on the third row of Fig. 2a, has dimension 4 and foliates with the relative position of the contact points. The leaves of interest have dimension 3 and 2 and correspond respectively to one or both the points of contact being fixed. Manipulation paths across the leaves (change of the contact point) require leaving the submanifold.

Finally, the triple-contact submanifold has dimension 3, it foliates with the position of the contact points and the leaves have dimension 2. Manipulation paths across the leaves require leaving the submanifold.

This last submanifold is not present in the structure of ${{\mathcal {C}}}$ when $m=1$ as illustrated in Fig. 2b. However, our solution requires considering also this submanifold of ${{\mathcal {C}}}$ to prove decidability, as will be illustrated in what follows.

In general, the structure of ${\mathcal {C}}$ depends both on the number n of movable objects and on m, the maximum number of objects that the robot can move at the same time.

As will be illustrated in Sect. 3.3, the “manipulability” properties associated with the above described submanifolds are actually transversal to this geometric structure and depend on the controllability of the underlying manipulation system (see Sect. 3.2).

2.4 The manipulation planning problem

Given the definitions and analysis in the previous sections, we can formulate the following problem.

Manipulation planning problem Assigned an initial configuration ${\varvec{q}}_s \in {{\mathcal {C}}}_{\text{ free }}$ and a goal configuration ${\varvec{q}}_g \in {{\mathcal {C}}}_{\text{ free }}$, find a sequence of transit and transfer paths joining ${\varvec{q}}_s$ to ${\varvec{q}}_g$, if it exists.

To prove that this problem is decidable we adopt the same approach as Dacre-Wright et al. (1992). First we study the problem of reducing configuration space paths belonging to the contact submanifolds represented in Fig. 2 to manipulation paths. Then, we determine a cell decomposition of the contact space. Finally, we construct the manipulation graph whose connected components characterize the existence of solutions to the manipulation problem defined above. In case $m < n$ we remove some nodes and the corresponding arcs from the manipulation graph.

The first part of our approach consists in answering the following question: is it possible to reduce any collision-free configuration space path describing motion of the robot in contact with at least one object to a (finite) sequence of transit and transfer paths? Answering this question requires studying the local controllability of the dynamic system that is possible to associate with the manipulation model. The analysis is described in the following section and makes use of the result by Goodwine and Burdick Goodwine and Burdick (2001) providing sufficient conditions for controllability of kinematic control systems on stratified configuration spaces.

This part of our approach shares with Laumond et al. (1994) the idea of first determining the manifold of the “unconstrained” (i.e., unaware of the manipulation system dynamics) configuration space where the solution path could exist and then prove that the paths in this manifold can be reduced to manipulation, i.e. feasible, paths. In other words, both the approaches rely on the combination of an algebraic/geometric method to find a solution path and on the local controllability of the, possibly constrained, system under study. The core components related to the specificity of the problem at hand, however, are the projection/lifting procedure of Sect. 5 and the proof of Theorem 1, together allowing to establish decidability of the manipulation planning problem.

3 Controllability of the manipulation system

To answer the first part of the manipulation planning problem, we define here the simple kinematics describing the manipulation system underlying the planning problem under consideration. This system has a stratified configuration space and we use the result in Goodwine and Burdick (2001) to establish its local controllability.

3.1 Controllability definitions

This section recalls the controllability definitions of interest to prove decidability of the manipulation problems of interest. Given an open set $V \subseteq M$, where M is the manifold describing the state space, let $R^V(x_0, T)$ be the set of states $x_f$ such that there exists $u: [0, T] \rightarrow {{\mathcal {U}}}$ that steers the control system from $x(0) = x_0$ to $x(T) = x_f$ and satisfies $x(t) \in V$ for $0 \le t \le T$, where ${\mathcal {U}}$ is the set of admissible control inputs. Define the set of states reachable up to time T as

$$\begin{aligned} R^V(x_0, \le T) = \bigcup _{0 < \tau \le T} {{R^V(x_0, \tau )}}. \end{aligned}$$

(1)

Definition 1

(Small Time Local Controllability) A smooth analytic system is small time locally controllable (or STLC) if $R^V(x_0, \le T)$ contains a neighborhood of $x_0$ for all neighborhoods V of $x_0$ and $T > 0$.

A more suited notion of controllability for proving decidability is the so-called local–local controllability (LLC) introduced in Haynes and Hermes (1970).

Definition 2

(Local–Local Controllability) A smooth analytic system is local–local controllable (or LLC) from $x_0$ if $\forall \epsilon> 0 \,\exists \,\delta (\epsilon ) > 0$ such that for all admissible states $x, || x - x_0|| < \delta (\epsilon )$ there exists admissible control $u: [0, T] \rightarrow {{\mathcal {U}}}$, producing the state trajectory $x(t), \, t \in [0,T]$ with

$$\begin{aligned} x(t_1)=x_0, \,\, x(t_2)=x \quad \hbox {and}\quad ||x(t)-x_0||<\epsilon , \quad \forall t \in [t_1, t_2].\quad \end{aligned}$$

(2)

LLC requires that the trajectory to reach points in the neighborhood of a given state $x_0$ is local. The time, however, is not specified (or bounded) in advance. For systems with bounded inputs STLC implies LLC Celikovsky and Nijmeijer (1997), while for driftless systems (which is the case considered in this paper) the two properties are equivalent.

3.2 Controllability on stratified configuration spaces

The definition of STLC above is generalized in Goodwine and Burdick (2001) to include the case of stratified systems. We briefly recall here the main definitions and properties of stratified configuration spaces and the stratified controllability property that we prove to hold in our case.

Stratified configuration manifold (Definition 2.2 in Goodwine and Burdick 2001) Let M be a manifold (possibly with boundary), and n functions $\varPsi _i$: $M \mapsto {\mathbb {R}}$, $i=1,\ldots ,n$ be such that the level sets $S_i = \varPsi _i^{-1}(0) \subset M$ are regular submanifolds of M, for each i, and the intersection of any number of the level sets, $S_{i_1i_2\ldots i_m} = \varPsi _{i_1}^{-1}(0) \cap \varPsi _{i_2}^{-1}(0) \cap \ldots \varPsi _{i_m}^{-1}(0)$, $m \le n$ , is also a regular submanifold of M. Then M and the functions $\varPsi _{i}$, define a stratified configuration space. The level sets $S_i,\ i=1,\ldots ,n$, and their intersections $S_I$, where I is an arbitrary multi-index of length $m\le n$, are referred to as the strata of the configuration manifold.

The driftless systems defined on stratified configuration manifolds are described on each stratum, or on strata intersections, by equations of motion characterized by smooth vector fields and the only discontinuities present in the equations of motion are due to transitions on and off the strata or their intersections.

Stratified controllability (Definition 3.2 inGoodwine and Burdick 2001) A stratified system is stratified controllable in the stratum $S_I$ from $x_0 \in S_I$ if $R^V(x_0, \le T)$ contains a neighborhood of $x_0$ in $S_I$ for all neighborhoods $V \subseteq S_I$ of $x_0$ and $T > 0$, where $R^V(x_0, \le T)$ is defined by Eq. (1) with $V \subseteq S_I$.

Stratified controllability (Proposition 4.4 inGoodwine and Burdick 2001) If there exists a nested sequence of submanifolds at the configuration $x_0$

$$\begin{aligned} x_0 \in S_p \subset S_{p-1} \subset \cdots \subset S_1 \subset S_0 = M \end{aligned}$$

where the subscript is the codimension of the submanifold, such that the associated involutive distributions satisfy

and each has constant rank for some neighborhood $V_j \subset S_j$, of $x_0$, then the system is stratified controllable from $x_0$ in M.

Stated differently, if the closure of the involutive distributions associated to each submanifold in the nested sequence intersect transversely then the system can flow in any direction in M. Intuitively, this controllability concept allows to prove that any path in contact in the composite configuration space of the robot and the movable objects can be reduced to a sequence of transit and transfer paths, i.e., a path in the stratified configuration space of the manipulation system.

3.3 Stratified controllability of the manipulation system

To use the stratified controllability concept for proving that a contact path can be transformed into a manipulation path, we consider ambient manifolds and stratifications defined by the contact submanifolds. In particular, there exist $n_p= \left( \begin{array}{c} n \\ p \end{array} \right)$, $p = \{1, \ldots , m\}$, ambient submanifolds $M_{n_p}$ given by the combined configuration space of the robot and the p objects in contact. The dimension of each ambient submanifold is equal to $\mathrm{dim}(M_{n_p})= 2 + 2 p$ and it contains $\left( \begin{array}{c} p \\ p-i \end{array} \right)$ submanifolds of codimension $p-i$, $i = \{0, \ldots , p-1\}$ defined by all the possible contact combinations of the p objects with the robot. Note that these submanifolds are leaves of the combined configuration space ${\mathcal {C}}$ of the robot and all the n objects.

The stratified controllability property is easily verified to hold on each of these ambient manifolds, while it is not verified on submanifolds of ${\mathcal {C}}$ of dimension greater than $2 + 2 p$. Hence, the considered manipulation system is stratified controllable on all the leaves of ${\mathcal {C}}$ defined by the submanifolds $M_{n_p}$. How to build the nested sequence of submanifolds providing stratified controllability is the subject of what follows.

Denote by ${\varvec{x}}= (x_R, y_R, x_{O_{c_1}}, y_{O_{c_1}}, \ldots , x_{O_{c_p}}, y_{O_{c_p}})^T$, a configuration of the manipulation system formed by the robot and p movable objects. Set the maximum number of objects that the robot can move at the same time equal to p. The equations of motion on each stratum are determined by considering that R can only translate in the plane and the objects can be moved when in contact with R with a stable grasp. The control system underlying this manipulation model can be written in the form

$$\begin{aligned} {\dot{{\varvec{x}}}} = g_1^{S_i} u_1 + g_2^{S_i} u_2, \end{aligned}$$

(3)

where $u_1$, $u_2$ are the robot cartesian velocities considered as the system inputs and $g_1^{S_i}$, $g_2^{S_i}$ are the input vector fields that have a different expression on each substratum. In particular, in $S_0 ={{\mathcal {C}}}$ we have

$$\begin{aligned} g_1^{{{\mathcal {S}}}_0} = (1,\,0,\,\mathbf{0})^T, \quad g_2^{{{\mathcal {S}}}_0} = (0,\,1,\,\mathbf{0})^T, \end{aligned}$$

where the first two components of the vector fields determine the motion of the robot and are invariant on all the strata. In $S_0$ they describe the free motion of the robot alone on a leaf of ${\mathcal {C}}$ that depends on the position of the objects. The remaining components have been grouped in vectors of zeros of dimension $p-2$.

On the single-contact manifolds ${{\mathcal {C}}}_{1}$ the input vector fields have the same first two components, the $(i+2)$-th entry of $g_1^{{{\mathcal {C}}}_{1}}$ and the $(i+3)$-th entry of $g_2^{{{\mathcal {C}}}_{1}}$, $i \in {1,\ldots ,p}$, equal to 1, the remaining components equal to zero. Flowing along these vector fields amounts to moving the object in contact while staying on a leaf that depends on the position of the objects that are not touched by the robot. Since all the single-contact manifolds have codimension 1, $S_1$ will be equal to either one of them in the sequence of nested submanifolds. This control will however implicitly assume that the position of the other objects will remain constant, i.e., the system is flowing on a leaf of the single contact manifold. Hence, if the system is stratified controllable, it will be only possible to prove that any path in contact on a leaf of $S_1$ can be reduced to a manipulation path. In fact, we need this local controllability to guarantee that a collision free path in continuous contact can be reduced to a manipulation path that is contained in a neighborhood of the original contact path. Complete decidability study requires the analysis of the manipulation graph connectivity.

The vector fields describing the motion on the subsequent strata can be defined iteratively by setting the components corresponding to the coordinates of the objects in contact equal to 1 and all the remaining components equal to zero. As noted above, the first two components of each vector field do not change across the strata and only one stratum for each codimension will be considered in the sequence of nested submanifolds. Analogously to the single contact case, this implies that the reduction property, or the possibility to reduce a collision-free path to a manipulation path, is only valid on a leaf of each substratum $S_j$, $j \in {1, \ldots , m}$. Note that, in our setting, a substratum $S_j$ collects all the contact submanifolds of codimension j defined by the contact of the robot and j of the n movable obstacles.

As an example, assuming that $n=m=2$, considering the unique ambient submanifold $M_{n_p}$ with $n=p=2$, on the single-contact manifolds ${{\mathcal {C}}}_{1}$, composing the stratum $S_1$, the input vector fields in (3) have the expressions

$$\begin{aligned} g_1^{S_1} = (1,\,0,\,1,\,0,\,0,\,0)^T, \quad g_2^{S_1} = (0,\,1,\,0,\,1,\,0,\,0)^T \end{aligned}$$

or

$$\begin{aligned} g_1^{S_1} = (1,\,0,\,0,\,0,\,1,\,0)^T, \quad g_2^{S_1} = (0,\,1,\,0,\,0,\,0,\,1)^T. \end{aligned}$$

On the double-contact manifold $S_2 = {{\mathcal {C}}}_{2}$, in case of contact with the objects $O_1$ and $O_2$, it is

$$\begin{aligned} g_1^{S_2} = (1,\,0,\,1,\,0,\,1,\,0)^T, \quad g_2^{S_2} = (0,\,1,\,0,\,1,\,0,\,1)^T. \end{aligned}$$

On this stratum the objects move with the robot without changing the points of contact.

It is easy to verify that the stratified controllability proposition holds by choosing as involutive distributions

where $g_1^{S_1}$ and $g_2^{S_1}$ can have either one of the expressions provided above.

Figure 3 illustrates the stratification of the configuration space induced by the contact constraints in the case $n=m=2$. By virtue of the controllability property described above, any continuous path in contact with the robot in $S_2$ can be reduced to a manipulation path.

Considering then as ambient manifold one of the two submanifolds $M_{n_p}$ with $n=2$ and $p=1$ defined by the object in contact with the robot. The preceding construction can be adapted to prove stratified controllability on each of these submanifolds that are leaves of the complete configuration space defined by the robot and the two movable objects. Hence, a path in contact in each leaf of $S_1$ can be reduced to a manipulation path that possibly goes through strata of lower codimension.

Until here we are not considering obstacles, hence the existence of a manipulation path depends locally on the controllability property. We take the hypothesis that the free configuration space of the robot without considering the objects is an open (same as the hypothesis H in Dacre-Wright et al. 1992). Then, any collision-free path in contact can be reduced to a collision-free manipulation path. Intuitively, if the system is controllable, the “maneuvers” involved in the manipulation path can be made as small as desired so as the manipulation path remains in the neighbourhood of the contact path.

Fig. 3

Stratification of the configuration space induced by the robot-objects contact constraints in the case $n=m=2$. Note how the relevant contact submanifolds needed to prove controllability are determined by the contact configurations between the robot and the objects

The following statement summarizes and generalizes the above arguments.

Proposition 1

Consider the general case $n\ge m$ and the ambient manifold $M_{n_m}$. Let $S_0$ define, as before, the foliation of $M_{n_m}$ where only the robot moves and the objects are in a fixed position. Sort the m objects as $i_1,\ldots ,i_m$ and consider the sequence of nested submanifolds

$$\begin{aligned} S_m\subset S_{m-1}\subset \cdots \subset S_1\subset S_0 \end{aligned}$$

where $S_{j}$ indicates the submanifold characterized by the robot in contact with the objects $i_1,\ldots ,i_j$ and the remaining objects $i_{j+1},\ldots ,i_{m}$ in a fixed position. Then, if $x_0\in S_{m}$, the system is stratified controllable from $x_0$ in $M_{n_m}$.

Proof

The idea is to apply the result of (Goodwine and Burdick 2001, Proposition 4.4). Let us set again

$$\begin{aligned} g_1^{{{\mathcal {S}}}_0} = (1,\,0,\,\mathbf{0})^T, \quad g_2^{{{\mathcal {S}}}_0} = (0,\,1,\,\mathbf{0})^T, \end{aligned}$$

with . For $j=1,\ldots ,m$, let us denote by ${\mathbf {e}}_j^1\in {\mathbb {R}}^{2+2m}$ the canonical vector with 1 in the $(2j+1)$-th entry and 0 in all other entries and by ${\mathbf {e}}_j^2\in {\mathbb {R}}^{2+2m}$ the canonical vector with 1 in the $(2j+2)$-th entry and 0 in all other entries, i.e.

$$\begin{aligned} \begin{array}{c} {\mathbf {e}}_j^1=(\underbrace{0,0,\ldots ,0}_{2j},1,\underbrace{0,0,\ldots ,0}_{2m-2j+1})^T\\ {\mathbf {e}}_j^2=(\underbrace{0,0,\ldots ,0}_{2j+1},1,\underbrace{0,0,\ldots ,0}_{2m-2j})^T \end{array} \end{aligned}$$

The input vector fields associated to the sequence of submanifold $S_j$ and the involutive distributions can be computed recursively as follows:

$$\begin{aligned} g_1^{{{\mathcal {S}}}_j} =g_1^{{{\mathcal {S}}}_{j-1}}+{\mathbf {e}}_{i_j}^{1}, \quad g_2^{{{\mathcal {S}}}_j} = g_2^{{{\mathcal {S}}}_{j-1}}+{\mathbf {e}}_{i_j}^{2}, \end{aligned}$$

and It is then clear that the property

holds true, this providing stratified controllability. $\square$

The stratified controllability property will be used to prove decidability by showing that if a solution exists it will either lie on a leaf of the free configuration space defined by a fixed position of the objects, i.e., the solution does not imply manipulation of the objects, or it passes through the submanifolds defined by the contact constraints between the robot and the objects. On each leaf of these submanifolds the reduction property holds, then a manipulation path exists if start and goal configurations can be connected through collision-free transit paths and paths in contact.

4 Reduction property

The aim of this section is to show that any collision-free path in contact on a leaf of one of the $m-1$ strata or on the stratum $S_m$ can be reduced to a sequence of admissible maneuvers, i.e., a collision-free manipulation path.

Consider then a finite length, collision-free path ${{\mathcal {P}}} \in S_j$ (more specifically, on a leaf of $S_j$ if $j=1,\ldots ,m-1$, or on $S_m$) from a given start configuration $x_\mathrm{start}\in {\mathbb {R}}^{2+2j}$, $j=1,\ldots ,m$ to a final configuration $x_\mathrm{goal}\in {\mathbb {R}}^{2+2j}$. Since ${{\mathcal {P}}}$ is admissible, all the configurations along the path are such that the robot is only in contact with the objects, and each object is in contact with the robot only. Assume no contact between objects or with the obstacles occurs (some comments on how to move beyond such assumption are provided further on).

The following Proposition 1 provides a sufficient condition for reducibility of a path in contact.

Theorem 1

Any path in contact ${\mathcal {P}}$ on any leaf of a stratum $S_j$, $j=0,\ldots ,m$ such that along ${\mathcal {P}}$ no object/object nor object/obstacle contact occurs, can be reduced to a manipulation path.

Proof

Consider a configuration x along the path ${\mathcal {P}}$. Determine first the set ${{\mathcal {N}}}_R(x)$ of configurations reachable from x by the robot through collision-free paths without considering the objects. ${{\mathcal {N}}}_R(x)$ is clearly an open neighbour of the projection $x_R$ of x on $R^2$ and includes all the robot transit paths that are not in collision with static obstacles.

Consider then the set ${{\mathcal {N}}}_{1,i}(x)$, $i=1,\ldots ,j$ of configurations reachable from x by the robot through collision-free paths in contact in $S_1$ without considering the $j-1$ objects that are not in contact with the robot. Each ${{\mathcal {N}}}_{1,i}(x)$, $i=1,\ldots ,j$ is an open neighbourhood of the projection $x_{1,i}$ of the configuration x on the $S_{1,i}$ stratum reachable by paths in contact with the i-th object, including transfer paths, that do not touch obstacles. Lift each ${{\mathcal {N}}}_{1,i}(x)$ to $R^{2+2j}$, by considering all the robot and object configurations in ${{\mathcal {N}}}_{1,i}(x)$ that do not overlap, i.e., remove the contact constraint while preserving admissibility. This is an open neighbourhood of x reachable by all the transit paths that are not in contact with obstacles and do not overlap with the i-th object and by all the paths in contact with the i-th object, including transfer paths, that do not touch obstacles.

Iterate the process by determining the set ${{\mathcal {N}}}_{k,i}(x)$, $i=1 ,\ldots ,\left( {\begin{array}{c}j\\ k\end{array}}\right)$ of configurations reachable from x through collision-free paths in contact in $S_{k,i}$ without considering the $j-k$ objects that are not in contact with the robot. Each ${{\mathcal {N}}}_{k,i}(x)$, $i=1 ,\ldots ,\left( {\begin{array}{c}j\\ k\end{array}}\right)$ is a neighbour of the projection $x_{k,i}$ of the configuration x on the $S_{k,i}$ stratum. Lift each ${{\mathcal {N}}}_{k,i}(x)$ to $R^{2+2j}$ with the procedure illustrated before. This is an open neighbourhood of x reachable by all the transit paths that are not in contact with obstacles and do not overlap with the k objects and by all the paths in contact with $i \in \{1,\ldots ,k\}$ objects, including transfer paths, that do not touch obstacles.

The process terminates on $S_j$ leading to the determination of the set ${{\mathcal {N}}}_{j}(x)$, of configurations reachable from x by the robot through collision-free paths in contact in $S_j$. The set ${{\mathcal {N}}}_{j}(x)$ is an open neighbourhood of x by construction. Lift ${{\mathcal {N}}}_{j}(x)$ to $R^{2+2j}$. This is an open neighbourhood of x reachable by all the transit paths that are not in contact with obstacles and do not overlap with the j objects and by all the paths in contact with the $i \in \{1,\ldots ,j\}$ object, including transfer paths, that do not touch obstacles.

Let us define now a polishing operation on the lifted sequence of neighbourhoods. Such operation corresponds to progressively eliminate from the lift of ${{\mathcal {N}}}_R(x)$ to $R^{2+2j}$ the closure of the set of configurations in $R^{2+2j}$ that have been found to be not admissible during the lifting of ${{\mathcal {N}}}_{k,i}(x)$, $k=1,\ldots ,j$, $i=1,\ldots ,\left( {\begin{array}{c}j\\ k\end{array}}\right)$ to $R^{2+2j}$. The polished lifting of ${{\mathcal {N}}}_R(x)$ to $R^{2+2j}$ is then an open neighbourhood of x reachable by robot transit configurations that are not in contact with obstacles and do not overlap with any object and by admissible paths in contact, including transfer paths.

The intersection of this neighbourhood of $R^{2+2j}$ with all the strata $S_{k,i}$, $k=0,\ldots ,j$, generates the neighbourhoods $\tilde{{\mathcal {N}}}_{k,i}(x)$, $k=0,\ldots ,j$, $i=1,\ldots ,\left( {\begin{array}{c}j\\ k\end{array}}\right)$ Goodwine and Burdick (2001).

Determine then the disk of maximum radius $\epsilon _x$ contained in the smallest neighbourhood belonging to the family $\tilde{{\mathcal {N}}}_{k,i}(x)$, $k=0,\ldots ,j$, $i=1,\ldots ,\left( {\begin{array}{c}j\\ k\end{array}}\right)$ then lift it to the ${R}^{2+2j}$ space where it is a ball ${{\mathcal {B}}}_{\epsilon _x}(x)$ with radius $\epsilon$ centered in x. Being ${\mathcal {P}}$ a compact set and $\epsilon _x$ a continuous function of x, there exists $\epsilon > 0$ such that $\epsilon = \min _{\{x \in {{\mathcal {P}}}\}}\epsilon _x$. By virtue of the stratified controllability of Proposition 1 and using continuity of ${\mathcal {P}}$, there exists $\delta =\delta (\epsilon )>0$ such that for each configuration x along the path there always exists a configuration $x^\prime$ on the path such that $||x - x^\prime || < \delta (\epsilon )$ and the manipulation path from x to $x^\prime$ is always contained in ${{\mathcal {B}}}_{\epsilon }(x)$.

The set of all the balls ${{\mathcal {B}}}_\delta (x)$, $x \in {{\mathcal {P}}}$, constitutes a covering of ${\mathcal {P}}$. Since ${\mathcal {P}}$ is compact, it is possible to get a finite sequence of configurations $(x_i)_{1\le i \le k}$ (with $x_1=x_\mathrm{start},\;\;x_k=x_\mathrm{goal}$), such that the balls ${{\mathcal {B}}}_\delta (x_i)$ cover ${\mathcal {P}}$.

To complete the proof, consider a point $y_{i,i+1} \in {{\mathcal {P}}}$ at the intersection ${{\mathcal {B}}}_\delta (x_i) \cap {{\mathcal {B}}}_\delta (x_{i+1})$. Between $x_i$ and $y_{i,i+1}$ (respectively $x_{i+1}$ and $y_{i,i+1}$) there is an admissible manipulation path that does not escape ${{\mathcal {B}}}_{\epsilon }(x_i)$ (respectively ${{\mathcal {B}}}_{\epsilon }(x_{i+1})$).

Then there is an admissible path between $x_i$ and $x_{i+1}$ that does not escape ${{\mathcal {B}}}_\epsilon (x_i) \cup {{\mathcal {B}}}_\epsilon (x_{i+1})$; this path is then collision-free. The sequence $(x_i)_{1\le i \le k}$ is finite and we can conclude that there exists a collision-free admissible path between $x_\mathrm{start}$ and $x_\mathrm{goal}$. $\square$

If the objects are allowed to touch the obstacles (a realistic case), and hence it is not possible to define an open neighbour around some or all the configuration of a path in contact ${\mathcal {P}}$ on a stratum $S_j$, $j=0,\ldots ,m$, then it is still possible to prove that a sufficient condition is that for any configuration x along the path there exist an open set in $\in {\mathbb {R}}^{2+2j}$ containing x and a segment of the path from x with non-zero length.

Finally, if the objects can also touch each other along segments of the path, then in the reduction procedure their motion should be handled as the motion of a single object.

The proof in these last cases presents complex technicalities that are not of practical use for real robotic manipulation systems for which the control-based tools adopted to define the decision procedure are more relevant and easy to use, e. g., in defining the properties that a robotic manipulation system should have for decidability purposes.

5 Building the manipulation graph

The remaining key issue involve building a geometric data structure that accounts for the obstacles presence and ultimately for the decidability of the manipulation problem.

5.1 Cell decomposition and refinement

We propose here an extension of the manipulation graph as it has been introduced in Dacre-Wright et al. (1992) for the case of a single movable object, i.e., $n=m=1$. In that case the admissible (i.e., not in collision with static obstacles nor overlapping the object to move) contact configurations between the robot and the object were represented by the class GRASP. The nodes of the manipulation graph were then given by the connected components of GRASP where the controllability, and hence reduction, property is easily shown to hold.

In the case of n movable objects and a maximum of $m \le n$ objects movable at the same time by the robot, it is necessary to introduce $n_p$ classes $\textit{GRASP}_{\varPhi _{n_p}}$, $p=\{1,\ldots ,m\}$, and to build the manipulation graph over the connected components of these classes (see Fig. 4 for a graphical illustration).

Each class $\textit{GRASP}_{\varPhi _{n_p}}$ represents the configurations in ${{\mathcal {C}}}_\mathrm{free}$ such that the robot is in contact with the p objects corresponding to one of the $n_p$ combinations also defining the ambient manifolds in Sect. 3.3 within which the stratified controllability holds.

Fig. 4

Example of GRASP classes with 2 objects: $\textit{GRASP}_i$, $i=1,2$ represents all the configurations in $\mathcal {C}_{\textit{free}}$ such that the robot is in contact with the object $O_i$

We define, hence, $\varPhi _{n_p}$ as the string of length p consisting in one of the $n_p$ combinations of p from the n objects. The position of the objects not included in $\varPhi _{n_p}$ can change within the class. To distinguish the different $n_p$ combinations we introduce the notation $\varPhi _{n_{p,i}}$, $i=\{1, \ldots , n_p\}$. When only the length of the string $\varPhi _{n_p}$ is relevant the index i is not specified and we do not make a distinction between the $n_p$ combinations.

The reduction property shown in the previous section does not apply on the whole connected components of $\textit{GRASP}_{\varPhi _{n_p}}$ but inside each leaf of the foliation of $\textit{GRASP}_{\varPhi _{n_p}}$ that keeps constant the position of the obstacles that are not in contact with the robot: any path inside these leaves can be reduced to a sequence of transit and transfer paths. These are, for example, leaves of dimension 3 in the single contact manifolds schematically represented in Fig. 2a for the case $n=m=2$.

The key questions are then: (i) how to determine the connected components of each $\textit{GRASP}_{\varPhi _{n_p}}$, and (ii) how to build a manipulation graph on which it is possible to decide for the existence of a manipulation path.

To answer the first question consider that each class $\textit{GRASP}_{\varPhi _{n_p}}$ is by definition a contact submanifold of ${{\mathcal {C}}}_\mathrm{free}$ of dimension $2(1+n) - p$, $p=\{1,\ldots ,m\}$. If there exists a cell decomposition of the $2(1+n)$-dimensional space ${{\mathcal {C}}}_\mathrm{free}$, then this cell decomposition induces, by retraction on its boundary, a cell decomposition of the $(2(1+n) - p)$-dimensional contact submanifolds (up to some potential singularities whose analysis goes beyond the scope of the present paper, see Schwartz 1983). Then, such a cell decomposition leads to a straightforward characterization of the connected components of each $\textit{GRASP}_{\varPhi _{n_p}}$. The first question is then reduced to the existence of an algorithm that provides a cell decomposition of ${{\mathcal {C}}}_\mathrm{free}$. A cylindrical decomposition can be used to this aim, as proposed in Schwartz (1983).

Building the manipulation graph is the second issue to be addressed. In particular, we need a suitable adjacency relationship between the cells of the classes $\textit{GRASP}_{\varPhi _{n_p}}$. Note that each cell in the class $\textit{GRASP}_{\varPhi _{n_p}}$ includes configurations that can be joined by elementary collision-free paths. These elementary paths, however, consist in the coordinated motion of robot and objects, including those which are not in contact. The configuration space is unconstrained by definition, that is, it has been constructed without considering that to change the position of an object it is necessary to move in contact with the robot. Hence, only the elementary paths that remain in the same leaf of a connected component of $\textit{GRASP}_{\varPhi _{n_p}}$ are guaranteed to be reducible to collision-free manipulation paths by retracting the cell decomposition of ${{\mathcal {C}}}_\mathrm{free}$ on its boundaries, as outlined in Fig. 5.

Fig. 5

Example of GRASP classes with 2 objects: the contact space is decomposed by retraction on the boundary

Then, we need to refine the cell decomposition of the connected components of each $\textit{GRASP}_{\varPhi _{n_p}}$ by considering their projections along the directions of the foliations generated by: (i) transit paths (the robot moves alone), (ii) transfer paths of type p (the p objects are moved by the robot while the remaining $n-p$ do not move).

Fig. 6

Example of $\textit{GRASP}$ classes with 2 objects: foliation in $\textit{GRASP}_i$ is obtained through transfer paths of type i with $i=1,2$ (so that the object $O_{3-i}$ does not move), and the cells of $\textit{GRASP}_i$ are projected onto $\textit{GRASP}_1\cap \textit{GRASP}_2$ along the directions of the respective foliations

Note first that the projection of a given cell $C_1$ onto a cell $C_2$ induces a decomposition of $C_2$ into several cells. The projection of a $\textit{GRASP}_{\varPhi _{n_{p,i}}}$ cell decomposition along the direction of its foliations onto a $\textit{GRASP}_{\varPhi _{n_{p+1}}}$ (as in Fig. 6) gives rise to a decomposition of this last class into multiple cells. This decomposition is further refined by projecting the cell decomposition of each $\textit{GRASP}_{\varPhi _{n_{p,j}}}$ such that the length of the string $\varPhi _{n_{p,i}} \cup \varPhi _{n_{p,j}}$ is equal to $p+1$ onto $\textit{GRASP}_{\varPhi _{n_{p+1}}}$. The initial cell decomposition of classes $\textit{GRASP}_{\varPhi _{n_{p,i}}}$ and $\textit{GRASP}_{\varPhi _{n_{p,j}}}$ can then be refined by “lifting” all cells in $\textit{GRASP}_{\varPhi _{n_{p+1}}}$ along the foliations of $\textit{GRASP}_{\varPhi _{n_{p,i}}}$ and $\textit{GRASP}_{\varPhi _{n_{p,j}}}$. Such lifting procedure, as displayed in Fig. 7, yields an additional and finer cell decomposition in the classes $\textit{GRASP}_{\varPhi _{n_{p,i}}}$ and $\textit{GRASP}_{\varPhi _{n_{p,j}}}$ that is inherited from the cell decomposition at their boundary interface. The class $\textit{GRASP}_{\varPhi _{n_{p+1}}}$ is then decomposed in elementary cells of which some are at the basis of two cylinders containing respectively cells of $\textit{GRASP}_{\varPhi _{n_{p,i}}}$ and $\textit{GRASP}_{\varPhi _{n_{p,j}}}$.

The cell decomposition of $\textit{GRASP}_{\varPhi _{n_{p+1}}}$ may however need to be further refined. The complete cell refinement is obtained by incrementally projecting cells of $\textit{GRASP}_{\varPhi _{n_{p}}}$ on cells of $\textit{GRASP}_{\varPhi _{n_{p+1}}}$, $p=1, \ldots , m-1$, along the foliations induced by transfer paths of type $n_p$. Then each cell of $\textit{GRASP}_{\varPhi _{n_{m-i}}}$ is lifted to the class $\textit{GRASP}_{\varPhi _{n_{m-i-1}}}$, $i=0, \ldots , m-2$. The cells generated by this refinement procedure and belonging to at least one cylinder constitute the nodes of the manipulation graph.

Fig. 7

Example of $\textit{GRASP}$ classes with 2 objects: the decomposition is further refined by lifting the cells of $\textit{GRASP}_1\cap \textit{GRASP}_2$ along the directions of foliations. These cells are the bases of two cylinders containing cells of $\textit{GRASP}_1$ and $\textit{GRASP}_2$, respectively

Fig. 8

Example of $\textit{GRASP}$ classes with 2 objects: two cells in $\textit{GRASP}_i$, $i=1,2$, are adjacent (by transfer paths) if and only if they have a common frontier and they belong to a same cylinder

5.2 Adjacency

In order to move from one node of the manipulation graph to another, the corresponding cells must be adjacent, in the sense that a manipulation path connecting them must exist and be feasible. We then introduce the following adjacency relation: two cells in a class $\textit{GRASP}_{\varPhi _{n_{p}}}$ are adjacent by transfer paths if and only if they have a common frontier and they belong to at least one same cylinder.

If $\textit{GRASP}_{\varPhi _{n_{p}}}$ belongs to a contact manifold of minimum dimension, i.e., $p=m$, and it is $n=m$ then adjacency is given by the existence of a common frontier between two cells. In fact, in this submanifold, any path in contact is equivalent to a manipulation path due to the stratified controllability property that, in this case, holds on ${\mathcal {C}}$.

We can then introduce the following recursive definition of adjacency by transfer paths (see also Fig. 8):

Two cells belonging to two different classes $\textit{GRASP}_{\varPhi _{n_i}}$ and $\textit{GRASP}_{\varPhi _{n_j}}$ are adjacent by transfer paths if and only if their projections on $\textit{GRASP}_{\varPhi _{n_i} \cup \varPhi _{n_j}}$ along the respective foliations, induced by transfer paths, intersect cells of the class $\textit{GRASP}_{\varPhi _{n_i} \cup \varPhi _{n_j}}$ which are adjacent by transfer paths. Note that the recursion terminates if $m=n$ because in the $\textit{GRASP}_{\varPhi _{n}}$ class the adjacency is given by the existence of a common frontier between cells.

Finally, consider the adjacency by transit paths, i.e., paths along which the robot is not in contact with any object. The main idea is the same as before. It is simpler because we have to consider only the foliation induced by transit paths in the robot free space. The leaves of such foliation are 2-dimensional. We consider the cell decomposition of each $\textit{GRASP}_{\varPhi _{n_p}}$ after the previously described cell refinement. We add an edge between two cells $c_1$ and $c_2$ belonging respectively to $\textit{GRASP}_{\varPhi _{n_i}}$ and $\textit{GRASP}_{\varPhi _{n_j}}$ if and only if the projection of either one of the two cells onto the other along the foliation by transit paths is not empty.¹

5.3 Decidability

Wrapping up, the manipulation graph nodes are cells of the $S_j$ strata refined through the projection and lifting process described before; adjacency in the contact space is provided by transfer paths between the refined cells; adjacency in the robot free space is provided by transit paths between the refined cells.

To prove decidability the graph is always constructed by considering $m = n$. In case $m < n$, the nodes corresponding to classes $\textit{GRASP}_{\varPhi _{n_p}}$, $p > m$, are removed from the graph together with all adjacency relations between cells that are based on the adjacency of $\textit{GRASP}_{\varPhi _{n_p}}$ cells. This will, of course, change the manipulation graph connectivity and problems requiring the simultaneous manipulation of all the movable objects will be correctly reported to be not solvable.²

The cell decomposition and the adjacency relationships provided in Sects. 5.1 and 5.2 , in a similar way to the cases treated in Dacre-Wright et al. (1992) and Siméon et al. (2004), allow us to establish the decidability of the problem as summarized in the following statement.

Theorem 2

There exists a manipulation path between two configurations in the free space if and only if these configurations retract on two cells belonging to the same connected component of the manipulation graph.

Fig. 9

Example of a manipulation problem: the robot R can move up to the three objects a, b, and c (left) at the same time to achieve a manipulation task. The collision free path connecting two configurations of $\textit{GRASP}_{a}$ and including the autonomous motion of b (right) is an admissible path within $\textit{GRASP}_{a}$ but not necessarily a manipulation path

Fig. 10

To connect an initial and a desired configuration of $\textit{GRASP}_{a}$ it is first necessary to establish a contact with b (left), take b to a convenient collision free configuration, i.e., move within $\textit{GRASP}_{ab}$, and then reach the goal. Along this paths motion in contact is allowed

5.4 Example: $n=m=3$

Consider the case of $n=3$ movable objects and a robot that can move up to $m=3$ objects together to achieve the planning task. The scenario is sketched in Fig. 9(left).

In this case we have the following classes: $\textit{GRASP}_{i}$ with $i=\{a,b,c\}$, $\textit{GRASP}_{ab}$, $\textit{GRASP}_{ac}$, $\textit{GRASP}_{bc}$, $\textit{GRASP}_{abc}$, where the index denotes the object in contact with the robot.

The retraction of the cylindrical decomposition of the 8-dimensional configuration space on its boundary induces a cell decomposition of the (8-p)-dimensional ($p=1, \ldots , 3$) contact submanifolds and this decomposition characterizes the connected components of each of the above listed classes $\textit{GRASP}_{\varPhi _{n_p}}$, where $\varPhi _{n_p} = \{a,b, c, ab, ac, bc, abc \}$. To refine the cell decomposition of each $\textit{GRASP}_{\varPhi _{n_p}}$ proceed as follows:

• 1. merge the projections of $\textit{GRASP}_{a}$ cell decomposition along the direction of its foliations (corresponding to constant positions of the objects b and c) onto $\textit{GRASP}_{ab}$; this gives rise to a decomposition of this last class into many cells; these cells can be reached by collision free paths in contact with a that belong to the leaves of $\textit{GRASP}_{a}$; it is not guaranteed however that all the configurations in these cells can be reached by collision free paths in contact with b; this is guaranteed by a cell refinement illustrated in the next step;

• 2. repeat the analogous projection for $\textit{GRASP}_{b}$ onto $\textit{GRASP}_{ab}$; this further refines the decomposition of this last class after the projection of the first step; the cells in $\textit{GRASP}_{ab}$ that appear to be at the basis of two cylinders containing cells of $\textit{GRASP}_{a}$ and $\textit{GRASP}_{b}$ can be reached by collision free paths respectively in contact with a and b; any path within these cells belonging to a same leaf (i.e., the object c remains in a same position) can be reduced to a collision free manipulation path; Fig. 10 illustrates through an example the adjacency by transfer path in class $\textit{GRASP}_{a}$; to guarantee that any path within a cell in this class is collision free it is however necessary to refine the decomposition as illustrated below;

• 3. execute the steps analogous to 1. and 2. to refine the decomposition of $\textit{GRASP}_{ac}$ and $\textit{GRASP}_{bc}$;

• 4. merge sequentially the projections of $\textit{GRASP}_{ab}$, $\textit{GRASP}_{ac}$ and $\textit{GRASP}_{bc}$ along their respective foliations onto $\textit{GRASP}_{abc}$; this generates a decomposition of this last class into many elementary cells;

• 5. lift each elementary cell in $\textit{GRASP}_{abc}$ to $\textit{GRASP}_{ab}$, $\textit{GRASP}_{ac}$ and $\textit{GRASP}_{bc}$ along their respective foliations; each elementary cell in $\textit{GRASP}_{abc}$ is at the basis of two cylinders containing cells of either $\textit{GRASP}_{ab}$ and $\textit{GRASP}_{ac}$ respectively or $\textit{GRASP}_{ab}$ and $\textit{GRASP}_{bc}$ or $\textit{GRASP}_{ac}$ and $\textit{GRASP}_{bc}$; these cells are nodes of the manipulation graph;

• 6. lift each elementary cell in $\textit{GRASP}_{ab}$, $\textit{GRASP}_{ac}$ and $\textit{GRASP}_{bc}$, obtained through refinement in the previous step, to $\textit{GRASP}_{a}$, $\textit{GRASP}_{b}$ and $\textit{GRASP}_{c}$ along their respective foliations; the cells in $\textit{GRASP}_{a}$, $\textit{GRASP}_{b}$ and $\textit{GRASP}_{c}$ resulting from this refinement and belonging to at least one cylinder are nodes of the manipulation graph; the cell refinement is then completed.

6 Generality of the approach

To avoid formal complications, we have illustrated the decision process by making reference to a specific manipulation system. The approach, however, is general enough to be applied to any manipulation system dynamics, any shape and any number of robots and objects. To support this generality claim, consider first the following synthetic description of the decision procedure.

• 1. Verify stratified controllability of the manipulation system.

• 2. If the system is stratified controllable, build the manipulation graph as described in Sect. 5:

  • (a) based on a cylindrical decomposition of ${{\mathcal {C}}}_\mathrm{free}$, determine the connected components of each class $\textit{GRASP}_{\varPhi _{n_p}}$, $p = \{1,\ldots , m\}$, $m=n$;

  • (b) refine the cell decomposition of each $\textit{GRASP}_{\varPhi _{n_p}}$ through appropriate projections and lifting operations;

  • (c) connect cells within each class $\textit{GRASP}_{\varPhi _{n_p}}$ and between classes $\textit{GRASP}_{\varPhi _{n_i}}$ and $\textit{GRASP}_{\varPhi _{n_j}}$ which are adjacent by transfer paths;

  • (d) connect cells which are adjacent by transit paths;

  • (e) if $m < n$, remove the nodes corresponding to classes $\textit{GRASP}_{\varPhi _{n_p}}$, $p > m$, together with all arcs corresponding to adjacency relations between cells that are based on the adjacency of $\textit{GRASP}_{\varPhi _{n_p}}$ cells.

• 3. Search the manipulation graph for a solution; return failure if it does not exists.

Note first that the use of a cylindrical decomposition as proposed in Schwartz (1983) to determine the connected components of the composite free configuration space allows robot, obstacles, objects and environment of any shape, provided that they posses a semi-algebraic geometry. The chosen decomposition also allows to decompose the free configurations for multiple robots and objects. On the other hand, the stratified controllability test in Goodwine and Burdick (2001) can be repeatedly applied to multiple nested sequences of strata. If the top stratum in each sequence is different (as would be the case of multiple robots), then the test determines controllability for the union of the top strata. In addition, the test can be used with robots of any kinematic architecture. In principle, also the nature of the contact could be included in testing the controllability. In this case, however, we need to define other kinds of adjacencies in addition to adjacency by transfer and transit paths characterising the dynamics of a rigid grasp. Hence, we argue that it is possible to build the manipulation graph for obstacles of any shape, in 2D and 3D workspaces, with multiple robots, possibly multi-articulated.

7 Conclusion

We have proposed in this paper a decision procedure for the problem of planning the motion of systems with stratified configuration space. The main contribution is the development of the decision algorithm, and no specific path planner is considered in the paper. Manipulation, climbing, walking, legged or multi-contact locomotion planning fall in the class of motion planning problems that may be handled with the presented approach.

The decision process relies on the cylindrical decomposition of the composite (i.e., robot and movable objects) free configuration space and the construction of a manipulation graph whose nodes are cells in the composite free configuration space. Cell refinement and adjacency is determined through a projection/lifting process on leaves of the contact configuration space. Within each cell in these submanifolds, paths in contact can be reduced to manipulation paths remaining in the free configuration space due to the stratified controllability of the manipulation system.

Although illustrated for a particular manipulation problem, the tools and approach adopted are general enough to include a much wider class of problems as discussed in Sect. 6.