Sampling-Based Motion Planning on Manifold Sequences

06/03/2020 ∙ by Peter Englert, et al. ∙ 0

We address the problem of planning robot motions in constrained configuration spaces where the constraints change throughout the motion. The problem is formulated as a sequence of intersecting manifolds, which the robot needs to traverse in order to solve the task. We specify a class of sequential motion planning problems that fulfill a particular property of the change in the free configuration space when transitioning between manifolds. For this problem class, the algorithm Sequential Manifold Planning (SMP*) is developed that searches for optimal intersection points between manifolds by using RRT* in an inner loop with a novel steering strategy. We provide a theoretical analysis regarding SMP*s probabilistic completeness and asymptotic optimality. Further, we evaluate its planning performance on various multi-robot object transportation tasks.



There are no comments yet.


page 2

page 11

This week in AI

Get the week's most popular data science and artificial intelligence research sent straight to your inbox every Saturday.

I Introduction

Sampling-based motion planning (SBMP) considers the problem of finding a collision-free path from a start configuration to a goal configuration. Algorithms like probabilistic roadmaps [18] or rapidly exploring random trees [27] are able to plan such motions and provide theoretical guarantees regarding probabilistic completeness. Optimal SBMP algorithms like RRT [17] additionally minimize a cost function and achieve asymptotic optimality. However, many tasks in robotics have a more complex structure that requires incorporating additional objectives like subgoals or constraints into the planning problem. Here, we propose a problem formulation for sequential motion planning that represents a task as a sequence of manifolds. Each manifold describes a subgoal or constraint that the robot needs to fulfill in order to solve the overall task. Current motion planning methods are difficult to directly apply to such types of problems because they require splitting the overall task into multiple planning problems that fit into the SBMP structure. A disadvantage of this approach is that a goal in the configuration space needs to be specified for each subtask. This is a separate problem that is difficult to solve since the selected subgoal will affect the future subtasks. State-of-the-art trajectory optimization methods [41, 33, 31] can handle task descriptions via costs and constraints. However, they strongly depend on the path initialization and suffer from poor local optima for long-horizon tasks.

Fig. 1: 3D Point on Geometric Constraints – The surfaces visualize the level sets of the three constraints. The task is to move from the start point (red dot) towards the goal point (blue dot) while fulfilling the constraints. The line shows a solution that fulfills these constraints and the magenta points are the found intersection vertices.

Here, we propose an algorithm that solves motion planning problems for a given sequence of manifolds. The algorithm searches for an optimal path that starts at an initial configuration, traverses the manifold sequence, and converges when the final manifold is reached. We solve this problem by growing a single tree over the manifold sequence. This tree consists of multiple subtrees that originate at the intersections between pairs of manifolds. We propose a novel steering strategy for RRT that guides the robot towards these manifold intersections. After an intersection is reached, a new subtree is initialized with the found intersection points and their costs. This approach allows us to do dynamic programming over optimal intersection points and scales well to long horizon tasks since a new subtree is initialized for every manifold. The algorithm is applicable to problems specified by a property regarding how the free configuration space changes across subtasks called intersection point-independence (see Section IV). The probabilistic completeness of the algorithm is proven in Section VII.

A running example in this paper is the task of using a robot arm to transport a mug from one table to another while keeping the orientation of the mug upright. This task involves multiple phases and constraints that we describe informally as follows. First, the arm moves to pick up the mug. In this subtask, the arm can move freely in space and only needs to avoid collisions with obstacles. The second subtask is to grasp the mug. The third subtask is for the arm to transfer the mug towards the goal keeping the mug upright at all times. The final subtask is to place and release the mug. This requires the base of the mug to be on the table for the gripper to successfully release the mug. We demonstrate our method in Section VI on transportation tasks that involve multiple robots.

The main contributions of this work are a novel problem formulation of sequential motion planning on manifolds and an algorithm to solve such problems for a certain class of planning problems. The proposed algorithm uses existing optimal sampling-based and constrained-based planning methods to solve sequential planning problems where the constraints change throughout the motion.

Ii Related Work

Ii-a Sampling-Based Motion Planning

Sampling-based motion planning (SBMP) is a randomized approach to path planning that builds a tree or graph in the robot configuration space. A PRM (probabilistic roadmap) is a randomized path planner that builds a graph in the free configuration space that can be used for multiple queries [2, 18, 30]. Kavraki et al. [18] describe the method as a two-step procedure. First, a roadmap is built by sampling collision-free nodes and edges. Second, a path is found from a start to a goal state by using a graph search algorithm. This technique is multi-query, as it does not necessarily encode a specific start or end state, and thus can be reused for many planning problems with the same system. On the other hand, tree methods such as RRTs (rapidly-exploring random trees) are generally single-query, taking a specific start state from which a tree of feasible states is grown toward a goal state or region [27, 26]. An advantage is that they can directly encode and respect kinodynamic and nonholonomic constraints. Many extensions to RRT exist, such as bidirectional trees, goal biasing, and region biasing [25]. There also exist optimal variants RRT and PRM, which find paths that minimize a cost function and guarantee asymptotic optimality by using a rewiring procedure [17]. All these techniques consider the problem of planning without motion constraints; that is, they plan in the free configuration space. Here, we use RRT in the inner loop of our algorithm where the goals are defined in terms of equality constraints instead of a goal configuration.

Ii-B Constrained Sampling-Based Motion Planning

Planning with constraints is an important problem in robotics since it allows for the description of a wider range of tasks compared to the classical SBMP problem. An in-depth review of constrained SBMP can be found in [21]. Constrained SBMP algorithms [37, 4, 13, 19, 22, 38, 9] extend SBMP to constrained configuration spaces, which are of smaller dimension than the full configuration space and usually cannot be sampled directly. An important aspect of these methods is the generation of samples that fulfill the constraints.

One family of approaches are projection-based strategies [37, 4, 38] that first sample a configuration from the ambient configuration space and then project it using an iterative gradient descent strategy to a nearby configuration that satisfies the constraint. Kaiser et al. [16] consider the problem of finding a robot configuration that satisfies multiple constraints. They propose a solution where one constraint is set as the primary constraint on which a configuration is projected. Afterwards, the configuration is projected on remaining constraints while fulfilling the primary constraint. Berenson et al. [4] proposed the algorithm CBiRRT2 (constrained bidirectional RRT) that uses projections to find configurations that fulfill constraints. The constraints are described by task space regions, which are a representation of pose constraints. Their method can also be used for multiple constraints over a single path. However, their approach requires each constraint’s active domain to be defined prior to planning, or configurations are simply projected to the nearest manifold rather than respecting a sequential order. Our approach is perhaps closest to the CBiRRT2 algorithm. A main difference is that our method assumes a different problem formulation where the task is given in terms of a sequence of manifolds. The intersections between manifolds describe subgoals that the robot should reach. This problem formulation allows us to define a steering strategy that guides the robot towards the subgoals.

An alternative sampling strategy is to approximate the constraint surface by a set of local models and use this approximation throughout the planning problem for sampling or steering operations [13, 14, 19, 39, 37, 7]. AtlasRRT [14] builds an approximation of the constraint that consists of local charts defined in the tangent space of the manifold. This representation is used to generate samples that are close to the constraint. Similarly, Tangent Bundle RRT [19, 39] builds a bidirectional RRT by sampling a point on a tangent plane, extending this point to produce a new point, and if it exceeds a certain distance threshold from the center of the plane, projecting it on the manifold to create a new tangent plane. Bordalba et al. [7] address the problem of constrained kinodynamic planning by considering the fact that simulating ODEs will produce drift errors. They also propose an atlas-based method to address this problem by incorporating the creation of an atlas as the state space parameterization into the construction of a bidirectional RRT.

Kingston et al. [22] presented the implicit manifold configuration space (IMACS) framework that decouples two parts of a geometrically constrained motion planning problem: the motion planning algorithm and the method for constraint adherence. With this approach, IMACS acts as a representative layer between the configuration space and the planner that presents the constrained space to the planner. Many of the previously described techniques fit into this framework. They present examples with both projection-based and approximation-based methods for constraint adherence in combination with various motion planning algorithms.

Here, we propose a method that builds on these constrained SBMP methods and extends them towards sequential tasks where the active constraints change during the motion.

Ii-C Sequential Motion Planning

One approach to plan sequential motions is task and motion planning (TAMP), which requires semantic reasoning on selecting and ordering actions to complete a higher-level task [15, 11, 24, 40, 10, 3, 8]. In general, TAMP is more difficult to solve due to scalability issues and the more complicated problem definition compared to SBMP. We assume that the sequence of tasks is given and focus on optimizing over the transition points between two constraints. Though we do not address task planning in this work, it is an interesting future direction we plan to consider.

Manipulation graphs are one technique to plan sequential manipulation tasks for robots. Simeon et al. [35] introduces a framework that consists of graphs representing the continuous space of all feasible motions that the robot can perform, which can be categorized into transit and transfer modes. A reduction property is defined that allows the graph to be represented by separate components that occur at the intersection of the two modes. Visibility-based PRMs [34] are used to represent the graphs and capture the closed-chain systems. Mirabel et al. [29] present an algorithmic implementation of manipulation graphs for object manipulations where the constraints are also defined as level sets of equality constraints. They also present a graph builder that builds the constraint graph automatically. Hauser et al. [12] proposed the multi-modal motion planning algorithm Random-MMP that can plan motions over multiple mode switches that describe changes in the planning domain (e.g., contacts). Their planner builds a tree in a hybrid state that consists of the continuous robot configuration and a discrete mode. In each step of the algorithm, the tree is extended by randomly sampling mode switches and querying a SBMP to find a corresponding path. Informed expansion strategies like utility tables are used to incorporate prior knowledge of a task in order to improve the performance of the planner. Vega et al. [42] presented the Orbital Bellman trees (OBT) algorithm, which addresses the manipulation planning problem in a similar way. They introduce the notion of an orbit, which is the set of reachable configurations of a mode, and assume that they can sample configurations in an orbit as well as on its boundary. This functionality is used to generate random geometric graphs of configurations belonging to an orbit. During planning, such graphs are built for unexplored orbits and A is performed to connect the points of an orbit to its neighboring orbits. The paper also contains a factored variant that uses domain knowledge to reduce the number of graphs that need to be grown. Our work is similar to these approaches. We also consider changes in the configuration space due to picking or placing objects. To contrast, we do not assume direct sampling of modes or switches is possible; rather, our algorithm is designed to find the mode-switching configurations during exploration toward manifold intersections. Further, we differ by specifying our problem formulation over a sequence of given manifolds and by applying this in the domain of intersection point independent problems, which we specify for sequential planning problems and which are solvable by growing a single tree.

Trajectory optimization [43, 33, 41, 31, 5] is another approach to solve sequential motion planning problems. There, an optimization problem over a trajectory is defined that minimizes costs subject to constraints. This formulation allows to describe complex behavior. However, they suffer from poor local minima and the need to specify costs and constraints activity for concrete time points. In complex tasks, it is difficult to specify how long a specific part of a motion takes in advance. Our approach only requires the sequential order of tasks and does not make any assumption of specific time points or durations of subtasks.

Iii Preliminaries

We begin with a brief background on differential geometry (see [6] for a rigorous treatment). An important idea in differential geometry is the concept of a manifold. A manifold is a surface which can be well-approximated locally using an open set of a Euclidean space. In general, manifolds are represented using a collection of local regions called charts and a continuous map associated with each chart such that the charts can be continuously deformed to an open subset of an Euclidean space. An alternate representation of manifolds, which is useful from a computational perspective, is to express them as zero level sets of functions defined on an Euclidean space. Such a representation of a manifold is called an implicit representation of the manifold. For example, a unit sphere embedded in can be represented implicitly as

. An implicit manifold is said to be smooth if the implicit function associated with it is smooth. The set of all tangent vectors at a point on a manifold is a vector space called the

tangent space of the manifold at that point. The null space of the Jacobian of the implicit function at a point is isomorphic to the tangent space of the corresponding manifold at that point. Since the tangent spaces of a manifold are vector spaces, we can equip them with an inner product structure which enables the computation of the length of curves traced on the manifold. A manifold endowed with an inner product structure is called a Riemannian manifold [28]. In this paper, we only consider smooth Riemannian manifolds.

Iv Problem Formulation and Application Domain

We consider kinematic motion planning problems in a configuration space . A configuration describes the state of one or multiple robots with degrees of freedom in total. We represent a manifold as an equality constraint where The set of robot configurations that are on a manifold is given by

We define a projection operator Project that takes a robot configuration and a manifold as inputs and maps the configuration to a nearby configuration on the manifold . We use an iterative optimization method, similar to [22, 4, 36], that iterates until a fixed point on the manifold is reached, which is checked by the condition . The matrix is the pseudo-inverse of the constraint Jacobian

We are interested in solving tasks that are defined by a sequence of such manifolds

and an initial configuration that is on the first manifold. The goal is to find a path from that traverses the manifold sequence and reaches a configuration on the goal manifold . A path on the -th manifold is defined as and is a cost function of a path where is the set of all non-trivial paths. We assume access to a collision check routine that returns if the path between and is collision-free, otherwise. In the following problem formulation, we consider the scenario where the free configuration space changes during the task (e.g., when picking or placing objects). We define an operator on the free configuration space , which describes how changes as manifold intersections are traversed. takes as input the path on the previous manifold and its associated , which we denote as . outputs an updated free configuration space that accounts for the geometric changes due to transitioning to a new manifold.

Returning to our illustrative example, we can now describe one of its constraints more precisely. A simple grasp constraint can be described with where is the forward kinematics function of the robot end effector point and is the grasp location on the mug. This constraint is fulfilled for multiple robot configurations , which correspond to different grasp locations that will affect the free configuration space for the subsequent tasks. For example, if a mug was grasped from the handle, it will have a different free space during the transport phase than if it was grasped by its opening.

Iv-a Problem Formulation

We formulate an optimization problem over a set of paths that minimizes the sum of path costs under the constraints of traversing and of being collision-free. The sequential manifold planning problem is


The second constraint ensures continuity such that the endpoint of a path is the start point of the next path . The third constraint captures the change in the collision-free space defined by the operator . The last two constraints ensure that the path is collision free and on the corresponding manifolds. The endpoint of must be on the goal manifold , which denotes the successful completion of the task.

An advantage of this formulation is that it is not necessary to specify a target configuration in . A wider range of goals (e.g., in the robot end effector space) can be described in the form of manifolds. Another advantage is that it is possible to describe complex sequential tasks in a single planning problem, not requiring the specification of subgoal configurations. The algorithm that is proposed in Section V searches for a path that solves this optimization problem for the problem class described in the next section.

Iv-B Intersection Point Independent Problems

We now use the problem formulation in (1) to describe robotic manipulation planning problems in which the manifolds describe subtasks that the robot needs to complete (e.g., picking up objects). The solution is an end-to-end path across multiple task constraints. In manipulation tasks, it is common that the geometry changes throughout the motion due to picking up or placing objects. Here, we make the assumption that these changes only occur at the transition between two manifolds. For example, changing the grasp of a mug will only occur at points during the task when it is picked up or placed.

For a certain class of sequential manifold planning problems, the following property holds: For each manifold intersection , the free space output by is set-equivalent for every possible path . In other words, the precise action taken to move the configuration from one constraint to the next does not affect the feasible planning space for the subsequent subtask. When this property holds for all intersections, we call the problem intersection point independent. The condition for this class of problems is


where denotes set-equivalence. In this work, we focus on the intersection point independent class of motion planning problems, which encompass a wide range of common problems. The more general intersection point dependent problems are a more difficult class of problems to solve because they require the handling of foliated manifolds [20] (e.g., every grasp leads to a different manifold). Our illustrative example is one such problem, since the grasp on the handle and the grasp from the top of the mug result in different free spaces. These problems require further work to be efficiently solvable and we plan to address them in future work.

V Sequential Manifold Planning

Fig. 2: Initializing a new subtree (steps 3033 of Algorithm 1).

In this section, we present the algorithm SMP that solves the sequential planning problem described in Section IV. The algorithm searches for an optimal solution to the constrained optimization problem in Equation (1), which is a sequence of paths , where each is a collision-free path on the corresponding manifold . The steps of SMP are outlined in Algorithm 1. The input to the algorithm is a sequence of manifolds and an initial configuration on the first manifold.

The overall problem is divided into subproblems. In each subproblem, a tree is grown from a set of initial nodes towards the intersection of the current and next manifold . In the inner loop of the algorithm (lines 424), the steps are similar to RRT and the rewiring step in the RRT_EXTEND routine (Algorithm 2) is equivalent to the one in RRT [17]. The extend operation uses a distance function between two nearby configurations and a function Cost that stores the path costs from the root of the tree to a node . The main modifications with respect to RRT are in the extension and projection steps. Instead of targeting a single goal configuration as in SBMP, we propose novel steering strategies that steer towards the intersection between manifolds.

In steps 512, a new target point in the configuration space is sampled and its nearest neighbor in the tree is computed. Next, one of the following two steering strategies is selected to find a direction in which to extend the tree

  1. extends the tree from towards while staying on the current manifold .

  2. extends the tree from towards the intersection of the current and next manifold .

Similar to the goal bias in RRT, a parameter

specifies the probability of selecting the SteerConstraint step. Both steering steps are described in detail in Section 

V-A and V-B. Afterwards, a new point is computed by taking a step in direction from with the maximum step size . Before the point is added to the tree, it is projected onto a manifold (steps 1317). Depending on the distance to the intersection of the manifolds, which is measured with , the point is projected either on the manifold or on the intersection manifold . The threshold for this condition is sampled uniformly between and , where the parameter decides the closeness required by a point around to be projected onto . Although a formal definition of is given in Section VII, for simplicity, while implementing our strategy we choose a positive number for . This randomization is necessary in order to achieve probabilistic completeness (see Section VII). Afterward, the RRT_EXTEND routine described in Algorithm 2 is called with the projected point. This routine checks the point for collision with the current free configuration space , performs rewiring steps, and eventually adds the point to the tree. The point is also added to a set of intersection nodes if it is also on the next manifold . In order to avoid duplicate intersection nodes, we ensure a minimum distance between two nodes in (step 20). After each time the inner loop of SMP is completed, a new tree is initialized in steps 3033 with the intersection nodes in and their costs so far. In order to keep the tree structure, we add a synthetic root node as parent node for all intersection nodes (see Figure 2). Convergence of the algorithm can be defined in various ways. We typically set an upper limit to the number of intersection nodes or provide a time limit for the inner loop. After reaching the last manifold, the algorithm returns the path with the lowest cost that reached the goal manifold . In the following sections, the steering strategies SteerPoint and SteerConstraint are derived.

2:for i=1 to n do
4:     while TimeRemaining() do
7:         if  then
8:               // Equation (7)
9:         else
10:               // Equation (5)
11:         end if
13:         if  then
15:         else
17:         end if
18:         if RRT_EXTEND then
19:              if  and
22:              end if
23:         end if
24:     end while
25:     // return path with lowest costs when is reached
26:     if  then
27:         return
28:     end if
29:     // initialize next tree with the intersection nodes
31:     for  do
33:     end for
34:end for
Algorithm 1 SMP
1:if  then
4:   ;
5:   for  do
6:      if  and
7: then
8:         ;
9:      end if
10:   end for
12:   for  do
13:      if  and
14: then
16:         ;
17:      end if
18:   end for
19:   return True
21:   return False
22:end if
Algorithm 2 RRT_EXTEND


In this extension step, the robot is at and should step towards the target configuration while staying on the manifold . We formulate this problem as a constrained optimization problem of finding a curve that


This problem is hard to solve due to the nonlinear constraints. Since the steering operations are called many times in the inner loop of the algorithm, we choose a simple curve representation and only compute an approximate solution to this problem. We parameterize the curve as a straight line with length .


Further, we apply a first-order Taylor expansion of the manifold constraint The problem is reduced to finding a direction that

which has the optimal solution


where contains the singular vectors that span the right nullspace of [32]. We normalize later in the algorithm, and thus do not include the constraint in the reduced optimization problem. The new configuration will be on the tangent space of the manifold, so that only few projection steps will be necessary before it can be added to the tree.


This steering step extends the tree from towards the intersection of the current and next manifold , which can be expressed as the optimization problem


The difference to problem (3) is that the loss is now specified in terms of the distance to the next manifold . This cost pulls the robot towards the manifold intersection. Again, we approximate the curve with a line (Equation (4)) and apply a first-order Taylor expansion to the nonlinear terms, which results in the simplified problem

A solution can be obtained by solving the linear system


where are the Lagrange variables. The solution is in the same direction as the steepest descent direction of the loss projected onto the tangent space of the current manifold.

Vi Experiments

In the following experiments, we solve kinematic motion planning problems where the cost function measures path length. We compare the proposed method SMP (Algorithm 1) with two alternative methods:

  • Greedy SMP – Algorithm 1 with the modification that only the node with the lowest cost in is selected to initialize the next tree (steps 3033 of Algorithm 1).

  • RRT+IK – This is a two-step procedure. First, a point on the manifold intersection is generated with inverse kinematics by randomly sampling a point in and projecting it onto the manifold intersection. Next, RRT is applied to compute a path towards this node. This procedure is repeated until the goal manifold is reached.

We executed all method with the same upper time limit and compare the quality of found paths in terms of the given objective.

Vi-a 3D Point on Geometric Constraints

We demonstrate the performance of the proposed algorithm on a 3D point that needs to traverse three constraints defined by geometric primitives. The initial robot state is and the sequence of manifolds is

  1. Paraboloid:

  2. Cylinder:

  3. Paraboloid:

  4. Goal:

    with the goal configuration .

The problem is visualized in Figure 1 where is drawn as red point and as blue point. The intersection nodes are shown as magenta points and a solution path from SMP is visualized as a line. The configuration space is limited to in all three dimensions and the time limit for all methods is 10 s. The parameters of the algorithms are , , , , and .

In Figure (a)a, the path costs of SMP, Greedy SMP and RRT+IK are compared for various parameter values. This parameter specifies the minimum distance between two intersection points, which influences the number of intersection points created (step 20 in Algorithm 1). The results show that paths with a lower lead to lower costs. SMP reaches the overall best performance with . For larger values, the performance of SMP is very similar to the Greedy SMP strategy since only a single intersection point is used.

Figure (b)b visualizes the cost over the phase of the path where parameterizes the path on the first manifold , on and on . The results indicate that the Greedy SMP strategy finds paths that reach the intersections of and with lower costs. However, SMP finds the path that achieves the overall task of reaching the goal manifold with the lowest cost.

Fig. 3: Path costs over variations of the parameter (left) and over the phase of the motion

(right). The graphs show the mean and unit standard deviation over 10 trials. In (a), the costs are decreasing for lower values of

. For larger values the performance of SMP is very similar to the Greedy SMP strategy. In (b), the greedy strategy finds shorter path to the first two manifold intersections while the overall best solutions are found by SMP.

Vi-B Multi-Robot Object Transport Tasks

In this experiment, we demonstrate SMP on various object transportation tasks. The overall task objective is to transport an object from an initial to a goal location. We consider three variations of this task:

  • Task A: A single robot arm mounted on a table with degrees of freedom. The task is to transport an object from an initial location on the table to a target location. This task is described by manifolds.

  • Task B: This task consists of two robot arms and a mobile base consisting of degrees of freedom. The task is defined such that the first robot arm picks the object and places it on the mobile base. Then, the mobile base brings it to the second robot arm that picks it up and places it on the table. This procedure is described with manifolds.

  • Task C: In this task, four robots are used that need to transport two objects between two tables. Three arms are mounted on the tables and another arm with a tray is mounted on a mobile base. Besides transporting the objects, the orientation of the two objects needs to be kept upright during the whole motion. This task is described with manifolds and the configuration space has degrees of freedom.

The three tasks are visualized in Figure 4 where the target locations are shown as green areas. Three types of constraints are used to describe these tasks. Picking up an object is defined with the constraint where is the location of the object and is the forward kinematics function to a point on the robot end effector. The handover of an object between two robots is described by where is the forward kinematics function to the end effector of the first robot and is that of the second robot. The orientation constraint is given by an alignment constraint where is a unit vector attached to the robot end effector that should be aligned with the vector to point upwards. These constraints are sufficient to describe the multi-robot transportation tasks.

The costs of the SMP, Greedy SMP and RRT+IK strategies are reported in Figure 6. The time limit for all strategies is 60 s. The parameters of the algorithms are , , , , and . The solution for Task C is visualized in Figure 5. SMP outperforms the baseline strategies on all tasks. The RRT+IK strategy was not able to solve Tasks B or C. The resulting motions are also shown in a video in the supplementary material.

(a) Task A
(b) Task B
(c) Task C
Fig. 4: Object transport tasks where the goal is to transport the red and blue object to the green target location.
Fig. 5: Visualization of a solution path for Task C.
Task A Task B Task C
Greedy SMP
Fig. 6: Results of object transport experiment. We report the mean and one unit standard deviation over 10 runs.

Vii Probabilistic completeness

In this section, we prove the probabilistic completeness of our algorithm. We start by assuming that there exists a path with clearance connecting the goal manifold with the start configuration embedded on the sequence of manifolds under consideration. Let be the total length of the path, computed based on the pullback metric [28] of the manifolds due to its embedding in . Let be the minimum over the reach [1] of all manifolds in the sequence and the manifolds resulting from the pairwise intersection of adjacent manifolds in the sequence. Informally, the reach of a manifold is the size of an envelope around the manifold such that any point within the envelope and the manifold has an unique projection on the manifold. For the analysis presented here, we pick the steering parameter such that . We use the notation to denote the set , where is the minimum distance of the point to the manifold. Now, if we define

then for the sake of analysis we assume that . If , then we define a sequence of points

on , such that and , where . Without loss of generality, we assume that for every with there exists a non-negative integer such that and . In other words, there exist some points at the beginning of such that they belong to and the rest of the points on the manifold belong exclusively to . For ease of analysis, the sequence of points on is chosen such that , where and are the distances between the points according to the metrics on the manifold and ambient space respectively. We use to denote a ball of radius around under the standard Euclidean norm on . We denote the tree that is grown with RRT as .

We prove the probabilistic completeness of our strategy in two parts. The first part proves the probabilistic completeness of RRT on a single manifold. In the second part, we prove that with probability one, the tree grown on a manifold can be expanded onto the next manifold as the number of samples tends to infinity. For the first part, as suggested in [22, Section 5.3], since [23, Lemma 1] can be shown to hold for the single manifold case, probabilistic completeness of RRT/RRT on a manifold can be easily proven using [23, Theorem 1]. We now focus on proving the second part that shows the probabilistic completeness of our strategy. We start by proving Lemma 1 which enables us to prove that a tree grown with RRT on a manifold can be extended to the next manifold.

Lemma 1

Suppose that has reached and contains a vertex such that . If a random sample is drawn such that , then the straight path between and the nearest neighbor of in lies entirely in .

By definition , then using the triangle inequality and some algebraic manipulation similar to that used in the proof of [23, Lemma 1], we can show that

which leads to and . Again, by the triangle inequality, we can show that . As the sample is taken from within the reach of there exists a unique nearest point of on [1]. In other words, the operation is well defined. Therefore, as

the straight path between and lies entirely in .

Note that the above lemma is an extension of [23, Lemma 1]. The next theorem will prove that with probability one SMP will yield a path as the number of samples goes to infinity. Since we are only concerned about the transition of from one manifold to the next, we focus on the iterations in SMP after reaches a neighborhood of . We refer to such an iteration as a boundary iteration.

Theorem 1

The probability that SMP fails to reach the final manifold from an initial configuration after boundary iterations is bounded from above by , for some positive real numbers and .

Let contain a vertex of . Let be the probability that in a boundary iteration a vertex contained in is added to . From Lemma 1, if we obtain a sample , then can reach . The value can be computed as a product of the probabilities of two events: 1) a sample is drawn from , and 2) is extended to include . The probability that a sample is drawn from is given by , where and are the volumes of and respectively. From the proof of Lemma 1, we infer that the line joining and is collision free. Thus will be augmented with a new vertex contained in if line 9 and 14 in Algorithm 1 are executed. The probability of execution of line 9 and 14 is , which results in

Further, as is very close to and , thus can be picked such that . For SMP to reach from the initial point, the boundary iteration should successfully extend for at least times (there are intersections between the sequential manifolds). The process can be viewed as Bernoulli trials with success probability . Let denote the number of successes in trials, then , which using the fact that , can be upper bounded as


as ,


applying yields,


Through further algebraic simplifications, we can show that


As the failure probability of SMP exponentially goes to zero as , SMP is probabilistic complete.

Viii Conclusion

In this work, we introduced a novel problem formulation for sequential motion planning problems that is based on a task representation as a sequence of intersecting manifolds. Further, we described a problem class where the change in free configuration space is intersection point independent and proposed an algorithm to solve problems in this class. The algorithm uses RRT with a steering strategy that finds intersection points of manifolds. We proved that the algorithm is probabilistic complete and demonstrated it on multi-robot transportation tasks. Future work includes the combination of this work with constrained trajectory optimization techniques [41, 31] that can use the same problem description. The solution of the proposed method could be used as initialization for a trajectory optimization method. We will also explore a more interlinked connection where trajectory optimization is used in the steering strategy, and approaches to task and motion planning where the sequence of manifolds is not given in advance.


This material is based upon work supported by the National Science Foundation Graduate Research Fellowship Program under Grant No. DGE-1842487. Any opinions, findings, and conclusions or recommendations expressed in this material are those of the author(s) and do not necessarily reflect the views of the National Science Foundation. This work was supported in part by the Office of Naval Research (ONR) under grant N000141512550.


  • [1] E. Aamari, J. Kim, F. Chazal, B. Michel, A. Rinaldo, L. Wasserman, et al. (2019) Estimating the reach of a manifold. Electronic journal of statistics 13 (1), pp. 1359–1399. Cited by: §VII, §VII.
  • [2] N. M. Amato and Y. Wu (1996) A randomized roadmap method for path and manipulation planning. In Proceedings of the International Conference on Robotics and Automation, Cited by: §II-A.
  • [3] J. Barry, L. P. Kaelbling, and T. Lozano-Pérez (2013) A hierarchical approach to manipulation with diverse actions. In International Conference on Robotics and Automation, Cited by: §II-C.
  • [4] D. Berenson, S. Srinivasa, and J. Kuffner (2011) Task space regions: a framework for pose-constrained manipulation planning. The International Journal of Robotics Research 30 (12), pp. 1435–1460. Cited by: §II-B, §II-B, §IV.
  • [5] R. Bonalli, A. Cauligi, A. Bylard, T. Lew, and M. Pavone (2019) Trajectory optimization on manifolds: a theoretically-guaranteed embedded sequential convex programming approach. In Proceedings of Robotics: Science and Systems, Cited by: §II-C.
  • [6] W. M. Boothby (1986) An introduction to differentiable manifolds and Riemannian geometry; 2nd ed.. Pure Appl. Math., Academic Press, Orlando, FL. Cited by: §III.
  • [7] R. Bordalba, L. Ros, and J. M. Porta (2018) Randomized kinodynamic planning for constrained systems. In Proceedings of the International Conference on Robotics and Automation, Cited by: §II-B.
  • [8] S. Cambon, R. Alami, and F. Gravot (2009) A hybrid approach to intricate motion, manipulation and task planning. The International Journal of Robotics Research 28 (1), pp. 104–126. Cited by: §II-C.
  • [9] J. Cortés and T. Siméon (2004) Sampling-based motion planning under kinematic loop-closure constraints. In Algorithmic Foundations of Robotics VI, pp. 75–90. Cited by: §II-B.
  • [10] N. T. Dantam, S. Chaudhuri, and L. E. Kavraki (2018)

    The task-motion kit: an open source, general-purpose task and motion-planning framework

    IEEE Robotics & Automation Magazine 25 (3), pp. 61–70. Cited by: §II-C.
  • [11] N. T. Dantam, Z. K. Kingston, S. Chaudhuri, and L. E. Kavraki (2016) Incremental task and motion planning: a constraint-based approach. In Proceedings of Robotics: Science and Systems, Cited by: §II-C.
  • [12] K. Hauser and V. Ng-Thow-Hing (2011) Randomized multi-modal motion planning for a humanoid robot manipulation task. The International Journal of Robotics Research 30 (6), pp. 678–698. Cited by: §II-C.
  • [13] L. Jaillet and J. M. Porta (2013) Asymptotically-optimal path planning on manifolds. In Proceedings of Robotics: Science and Systems, Cited by: §II-B, §II-B.
  • [14] L. Jaillet and J. M. Porta (2013) Path planning under kinematic constraints by rapidly exploring manifolds. IEEE Transactions on Robotics 29 (1), pp. 105–117. Cited by: §II-B.
  • [15] L. P. Kaelbling and T. Lozano-Pérez (2013) Integrated task and motion planning in belief space. The International Journal of Robotics Research 32 (9-10), pp. 1194–1227. Cited by: §II-C.
  • [16] P. Kaiser, D. Berenson, N. Vahrenkamp, T. Asfour, R. Dillmann, and S. Srinivasa (2012) Constellation - An algorithm for finding robot configurations that satisfy multiple constraints. In Proceedings of the International Conference on Robotics and Automation, Cited by: §II-B.
  • [17] S. Karaman and E. Frazzoli (2011) Sampling-based algorithms for optimal motion planning. International Journal of Robotics Research 30 (), pp. 846–894. Cited by: §I, §II-A, §V.
  • [18] L. Kavraki and J-C. Latombe (1994) Randomized preprocessing of configuration for fast path planning. In Proceedings of the International Conference on Robotics and Automation, Cited by: §I, §II-A.
  • [19] B. Kim, T. T. Um, C. Suh, and F. C. Park (2016) Tangent bundle rrt: a randomized algorithm for constrained motion planning. Robotica 34 (1), pp. 202–225. Cited by: §II-B, §II-B.
  • [20] J. Kim, I. Ko, and F. C. Park (2014) Randomized path planning on foliated configuration spaces. In International Conference on Ubiquitous Robots and Ambient Intelligence, Cited by: §IV-B.
  • [21] Z. Kingston, M. Moll, and L. E. Kavraki (2018) Sampling-based methods for motion planning with constraints. Annual review of control, robotics, and autonomous systems 1, pp. 159–185. Cited by: §II-B.
  • [22] Z. Kingston, M. Moll, and L. E. Kavraki (2019) Exploring implicit spaces for constrained sampling-based planning. The International Journal of Robotics Research 38 (10-11), pp. 1151–1178. Cited by: §II-B, §II-B, §IV, §VII.
  • [23] M. Kleinbort, K. Solovey, Z. Littlefield, K. E. Bekris, and D. Halperin (2019) Probabilistic completeness of rrt for geometric and kinodynamic planning with forward propagation. IEEE Robotics and Automation Letters 4 (2). Cited by: §VII, §VII, §VII.
  • [24] G. Konidaris, L. P. Kaelbling, and T. Lozano-Perez (2018) From skills to symbols: learning symbolic representations for abstract high-level planning.

    Journal of Artificial Intelligence Research

    61, pp. 215–289.
    Cited by: §II-C.
  • [25] J. J. Kuffner Jr and S. M. LaValle (2000) RRT-connect: an efficient approach to single-query path planning. In Proceedings of the International Conference on Intelligent Robots and Systems, Cited by: §II-A.
  • [26] S. M. LaValle and J. J. Kuffner (2001) Algorithmic and computational robotics: new directions. pp. 293–308. Cited by: §II-A.
  • [27] S. M. LaValle (1998) Rapidly-exploring random trees: a new tool for path planning. Technical report Technical Report TR 98-11, Computer Science Department, Iowa State University. Cited by: §I, §II-A.
  • [28] J. M. Lee (2006) Riemannian manifolds: an introduction to curvature. Springer Science & Business Media. Cited by: §III, §VII.
  • [29] J. Mirabel, S. Tonneau, P. Fernbach, A. Seppälä, M. Campana, N. Mansard, and F. Lamiraux (2016) HPP: a new software for constrained motion planning. In Proceedings of the International Conference on Intelligent Robots and Systems, Cited by: §II-C.
  • [30] M.H. Overmars (1992) A random approach to motion planning. Technical report Technical Report RUU-CS-92-32, Department of Computer Science, Utrecht University. Cited by: §II-A.
  • [31] N. Ratliff, M. Toussaint, and S. Schaal (2015) Understanding the geometry of workspace obstacles in motion optimization. In Proceedings of the International Conference on Robotics and Automation, Cited by: §I, §II-C, §VIII.
  • [32] N. Ratliff (2014) Multivariate calculus II: the geometry of smooth maps. Lecture notes: Mathematics for Intelligent Systems series. Cited by: §V-A.
  • [33] J. Schulman, J. Ho, A. X. Lee, I. Awwal, H. Bradlow, and P. Abbeel (2013) Finding locally optimal, collision-free trajectories with sequential convex optimization.. In Robotics: Science and Systems, Cited by: §I, §II-C.
  • [34] T. Siméon, J. Laumond, and C. Nissoux (2000) Visibility-based probabilistic roadmaps for motion planning. Advanced Robotics 14 (6), pp. 477–493. Cited by: §II-C.
  • [35] T. Siméon, J. Laumond, J. Cortés, and A. Sahbani (2004) Manipulation planning with probabilistic roadmaps. The International Journal of Robotics Research 23 (7-8), pp. 729–746. Cited by: §II-C.
  • [36] M. Stilman (2007) Task constrained motion planning in robot joint space. In Proceedings of the International Conference on Intelligent Robots and Systems, Cited by: §IV.
  • [37] M. Stilman (2010) Global manipulation planning in robot joint space with task constraints. IEEE Transactions on Robotics 26 (3), pp. 576–584. Cited by: §II-B, §II-B, §II-B.
  • [38] I. A. Şucan and S. Chitta (2012) Motion planning with constraints using configuration space approximations. In Proceedings of the International Conference on Intelligent Robots and Systems, Cited by: §II-B, §II-B.
  • [39] C. Suh, T. T. Um, B. Kim, H. Noh, M. Kim, and F. C. Park (2011) Tangent space rrt: a randomized planning algorithm on constraint manifolds. In Proceedings of the International Conference on Robotics and Automation, Cited by: §II-B.
  • [40] M. Toussaint, K. Allen, K. A. Smith, and J. B. Tenenbaum (2018) Differentiable physics and stable modes for tool-use and manipulation planning.. In Proceedings of Robotics: Science and Systems, Cited by: §II-C.
  • [41] M. Toussaint (2017) A tutorial on Newton methods for constrained trajectory optimization and relations to SLAM, Gaussian Process smoothing, optimal control, and probabilistic inference. In Geometric and Numerical Foundations of Movements, Cited by: §I, §II-C, §VIII.
  • [42] W. Vega-Brown and N. Roy (2016) Asymptotically optimal planning under piecewise-analytic constraints. In Workshop on the Algorithmic Foundations of Robotics, Cited by: §II-C.
  • [43] O. von Stryk and R. Bulirsch (1992) Direct and Indirect Methods for Trajectory Optimization. Annals of Operations Research 37 (1), pp. 357–373. Cited by: §II-C.