Quotient-Space Motion Planning

Let $M$ be a vector space and $C$ be a subspace of $M$. Then the quotient space of $M$ by $C$, denoted by ${\raisebox{1.99997pt}{$M$}\left/\raisebox{-1.99997pt}{$C$}\right.}$ is the space obtained by collapsing all equivalence classes of $C$ in $M$ to zero [3]. Collapsing a space is done by creating an equivalence relation $\sim$ on $M$: for all $x,y\in M$ we have that $x\sim y$ if $x-y\in C$. This relation creates a partition of equivalence classes on the vector space. The set of equivalence classes is called the quotient space.

As an example, consider the vector space $\mathbb{R}^{2}$ and its subspace $\mathbb{R}$. We can partition $\mathbb{R}^{2}$ into equivalence classes of its subspace $\mathbb{R}$ such that two points $x,y\in\mathbb{R}^{2}$ are equivalent if $x-y\in 0\times\mathbb{R}$. We visualize this in Fig. 1, where $\mathbb{R}^{2}$ is first partitioned into the equivalence classes of $\mathbb{R}$ (the vertical lines), and then all lines are collapsed to yield $\mathbb{R}$. The point $q^{2}\in\mathbb{R}^{2}$ and all points on the dashed line are equivalent and therefore collapsed into the single point $q^{1}\in\mathbb{R}$. We identify $\mathbb{R}={\raisebox{1.99997pt}{$\mathbb{R}^{2}$}\left/\raisebox{-1.99997pt}{$\mathbb{R}$}\right.}$ to denote the quotient space.

Quotient spaces generalize to manifolds [17]. Consider $\mathcal{M}=\operatorname{SO}(2)\times\operatorname{SO}(2)$, the configuration space of a 2-dof fixed-base manipulator in the plane. Two points $x,y\in\operatorname{SO}(2)$ are equivalent if $x-y\in 0\times\operatorname{SO}(2)$. The manifold $\mathcal{M}$ can be partitioned into equivalence classes of $\operatorname{SO}(2)$, and then each equivalent class is collapsed to yield $\operatorname{SO}(2)={\raisebox{1.99997pt}{$\mathcal{M}$}\left/\raisebox{-1.99997pt}{$\operatorname{SO}(2)$}\right.}$. This has been visualized in Fig. 2. On the left three different configurations are shown and their position in the configuration space. On the right the quotient space is shown, corresponding to a 1-dof manipulator nested inside the 2-dof manipulator. Two interesting observations can be made in the quotient space: First, the configuration $q_{3}$ is infeasible, regardless of how the second joint is moved. Second, there does not exists a path between $q_{1}$ and $q_{2}$. This can be inferred solely from the 1-dof manipulators configuration space.

This example shows the defining feature of quotient spaces: If a robot is nested inside another robot, then the feasibility of the nested robot is a necessary condition for the feasibility of the other robot¹¹1Zhang and Zhang [4] call this the falseness-preserving property of quotient space decompositions. We will proceed to define how robots can be nested in each other, and we will show that this definition indeed leads to the necessary condition for feasibility.

A robot $\mathcal{R}_{i}$ is nested inside another robot $\mathcal{R}_{i+1}$ if two conditions are fulfilled: First, the configuration space of $\mathcal{R}_{i}$ is a subspace of $\mathcal{R}_{i+1}$, and second, the volume of the body of $\mathcal{R}_{i}$ at each configuration must be a subset of the volume of the body of $\mathcal{R}_{i+1}$.

Let $\mathcal{M}_{i}$ be the configuration space of robot $\mathcal{R}_{i}$, and let $\mathbf{V}_{i}(p)\subset\mathbb{R}^{3}$ be the volume of the body of robot $\mathcal{R}_{i}$ at configuration $p\in\mathcal{M}_{i}$.

whereby the operation $\circ$ is the cartesian product defined as $p\circ q=(p,q)\in\mathcal{M}_{i+1}$.

Given a robot $\mathcal{R}$ and a sequence of nested robots $\mathcal{R}_{1}\subseteq\cdots\subseteq\mathcal{R}_{K}=\mathcal{R}$, the configuration spaces define a decomposition of $\mathcal{M}$ as

From Definition (2) we can infer the key property of the quotient-space decomposition: if a nested robot is infeasible, then so is the original robot.

Let $\mathbf{E}$ denote the environment, the subset of $\mathbb{R}^{3}$ containing obstacles. We say that robot $\mathcal{R}_{i}$ is feasible at configuration $q\in\mathcal{M}_{i}$ if $\mathbf{V}_{i}(q)\cap\mathbf{E}=\emptyset$.

To simplify the algorithmic development, we group each quotient-space with its associated objects into a tuple called the quotient-space roadmap.

Let $\mathcal{M}$ be the $N$-dimensional configuration space of robot $\mathcal{R}$. We consider a nested sequence of $K$ robots $\mathcal{R}_{1}\subset\cdots\subset\mathcal{R}_{K}=\mathcal{R}$, such that $\mathcal{M}$ is decomposed into a sequence of quotient spaces as $\mathcal{M}_{1}\subset\cdots\subset\mathcal{M}_{K}=\mathcal{M}$.

To each quotient space $\mathcal{M}_{k}$ we associate a start configuration $q^{I}_{k}$, a goal configuration $q^{G}_{k}$, a graph $\mathbf{G}_{k}$, a shortest path $\mathbf{p}_{k}$ on $\mathbf{G}_{k}$ between $q^{I}_{k}$ and $q^{G}_{k}$, and a density $V_{k}$ defined on $\mathbf{G}_{k-1}\times\mathbf{C}_{k}$ as

whereby $|\mathbf{G}_{k}|$ are the number of vertices of $\mathbf{G}_{k}$, $L_{k-1}$ is the sum of all edge lengths of $\mathbf{G}_{k-1}$, and $\mu(\mathbf{C}_{k})$ is the $n^{k}$-dimensional measure of $\mathbf{C}_{k}$. The density is used to decide which graph should be grown next.

We group all elements together into the quotient-space roadmap

with $\mathbf{C}_{k}={\raisebox{1.99997pt}{$\mathcal{M}_{k}$}\left/\raisebox{-1.99997pt}{$\mathcal{M}_{k-1}$}\right.}$, $\mathcal{M}_{0}=\emptyset$, $\mathbf{Q}_{0}=\emptyset$.

QMP is an adapted version of the probabilistic roadmap planner (PRM) for quotient space decompositions. We first summarize the workings of PRM, then show how QMP can be built from it.

A simplified version of PRM is depicted in Algorithm 1. While a planner terminate condition (PTC) ²²2A planner terminate condition (PTC) has to be chosen by a user and can be a time limit, an iterations limit, or a desired cost of the resulting path. has not been reached (Line 2), the algorithm grows a graph $\mathbf{G}$ on the configuration space $\mathcal{M}$ of robot $\mathcal{R}$ (Line 3). If there exists a path on the graph between start and goal configuration (Line 4), then this path is returned (Line 5). If the PTC is reached then PRM fails and returns an empty path.

The growing of the graph is depicted in Algorithm 2. A configuration $q_{rand}$ is sampled from the configuration space $\mathcal{M}$ (Line 1). If this configuration is valid (Line 2), then it is added to the graph (Line 3) and the nearest $R$ configurations $Q_{R}$ are searched (Line 4). The graph is extended in a straight line from each $q_{near}\in Q_{R}$ (Line 5) towards $q_{rand}$ until it hits an obstacle (Line 6). The last configuration before the obstacle becomes $q_{new}$, and the edge between $q_{near}$ and $q_{new}$ is added to the graph (Line 7).

QMP is depicted in Algorithm 3. An empty priority queue is constructed (Line 1), and the $k$-th quotient roadmap is initialized (Line 3) and added to the queue (line 4). While no path between start and goal has been found (Line 5), we pop the quotient roadmap with the smallest density from the queue (line 6), grow its graph (line 7) and push it back onto the queue (line 8). Then we check if there exists a path on the current quotient space (line 9); if yes, we construct its solution (line 10), and we continue to the next quotient roadmap. For $k=1$ the algorithm is equivalent to PRM. For $k>1$, multiple quotient spaces are inside the queue, and depending on the density function we pop one quotient space and grow its graph. The algorithm terminates if either the path $\mathbf{p}_{K}$ has been found, or if the PTC is reached, in which case $\mathbf{p}_{K}=\emptyset$ is returned.

The growing of the quotient space graph is depicted in Algorithm 5. Instead of sampling $\mathcal{M}_{k}$ as in the PRM, we sample instead $\mathbf{C}_{k}$ uniformly, and we pick one configuration from the graph $\mathbf{G}_{k-1}$ on $\mathcal{M}_{k-1}$ (Line 1). The SampleGraph samples a uniform vertex from the graph $\mathbf{G}_{k-1}$. Then a random incoming edge is chosen, and a configuration uniformly on the edge is sampled. This is called Random-Vertex-Edge (RVE) sampling [18]. The cartesian product $\circ$ merges the two configuration to yield a configuration on $\mathcal{M}_{k}$. The rest of the algorithm (line 2-7) operates as the PRM algorithm, with two exceptions. First, the R-Nearest-Neighbors method measures distance not by euclidean distance on $\mathcal{M}_{k}$, but by the graph distance on $\mathbf{G}_{k-1}$ plus euclidean distance on $\mathbf{C}_{k}$ (Line 4). Second, the Connect method does not interpolate along a straight line, but interpolates along the edges of the graph $\mathbf{G}_{k-1}$, while interpolating on $\mathbf{C}^{k}$ using a straight line. For each vertex crossed on $\mathbf{G}_{k-1}$ we add another configuration. The Connect method then returns a piece-wise linear (PL) path on $\mathcal{M}_{k}$. For each edge along this PL-path we add one edge to the graph $\mathbf{G}_{k}$ (Line 7).

Interpolating along the graph instead of using a straight line should be seen as a change in the metric on $\mathcal{M}_{k}$. While a standard euclidean metric is agnostic about the graph on $\mathcal{M}_{k-1}$, our graph interpolation metric utilizes the knowledge about $\mathbf{G}_{k-1}$ to improve the metric computation.

Our software uses the Klamp’t [20] physical simulator, and the open motion planning library OMPL [21].

The nesting of robots has to be prespecified as a set of Unified Robot Description Format (URDF) files along with its subspaces. Each subspace is represented by an OMPL space, and our algorithm iterates through them computing the quotient spaces. We currently support the following quotient space computations: $\mathbb{R}^{3}={\raisebox{1.99997pt}{$\operatorname{SE}(3)$}\left/\raisebox{-1.99997pt}{$\operatorname{SO}(3)$}\right.}$, $\mathbb{R}^{2}={\raisebox{1.99997pt}{$\operatorname{SE}(2)$}\left/\raisebox{-1.99997pt}{$\operatorname{SO}(2)$}\right.}$, $\mathbb{R}^{n-m}={\raisebox{1.99997pt}{$\mathbb{R}^{n}$}\left/\raisebox{-1.99997pt}{$\mathbb{R}^{m}$}\right.}$, $\operatorname{SE}(3)={\raisebox{1.99997pt}{$\operatorname{SE}(3)\times\mathbb{R}^{n}$}\left/\raisebox{-1.99997pt}{$\mathbb{R}^{n}$}\right.}$, and $\operatorname{SE}(3)\times\mathbb{R}^{n-m}={\raisebox{1.99997pt}{$\operatorname{SE}(3)\times\mathbb{R}^{n}$}\left/\raisebox{-1.99997pt}{$\operatorname{SE}(3)\times\mathbb{R}^{m}$}\right.}$, with $n,m\in\mathbb{N}$ and $n>m>0$.

Our algorithm terminates after a path has been found on the configuration space, or a timelimit $T$ has been reached.

Solution paths on quotient-spaces do not behave as nicely as one would expect. We show two examples where the solution path is non-simple or spurious.

Keeping the intricacies in mind, we design three heuristics which we found to be beneficial to reduce the runtime of QMP.

The double L-shape is a rigid body consisting of two L-shapes glued together. The environment is a wall with a small square hole in it, as depicted in Fig. 8. This problem was introduced by [13] as an example of a narrow passage planning problem. We decompose the double L-shape as $\mathbb{R}^{3}\subset\operatorname{SE}(3)$, as depicted in Fig. 6. We compared our algorithm QMP with three state of the art algorithms implemented in the OMPL software framework: PRM [25], bidirectional rapidly-exploring random tree (RRTConnect) [26] and the expansive space trees planner (EST) [27]. Fig. 9 shows that only QMP and PRM solved all runs, and that QMP has a median runtime of $2.5$s, compared to $27$s for PRM.

The mechanical snake is a 10-dof articulated body which has three links, connected each by two revolute joints with limits (See Fig. 6). The snake can freely translate and rotate in space. We found the most effective decomposition to be $\mathbb{R}^{3}\subset\operatorname{SE}(3)\times\mathbb{R}^{4}$. QMP has a median runtime of $30$s ($67$s for EST). However, we can see that QMP has two outliers.

The KUKA Ligthweight Robot (LWR) is a 7-dof manipulator. We consider transporting a windshield through a factory simulating a car manufacturing task. Our decomposition is $\mathbb{R}^{5}\subset\mathbb{R}^{7}$. In the experiments, only QMP was able to solve all runs, with a median time of $18$s ($164$s for EST).

Baxter is a two-arm fixed-base robot with a tree kinematic chain having two serial chains of 7 dof for each arm, and having a total of 14 dof. We consider a maintenance task, where Baxter needs to move its arms into small openings of a defect water tank. We decompose Baxter as $\mathbb{R}^{5}\subset\mathbb{R}^{7}\subset\mathbb{R}^{12}\subset\mathbb{R}^{14}$ as depicted in Fig. 7. Only QMP was able to solve all runs, with a median runtime of $4.5$s compared to $94$s for RRTConnect.