A Motion Planning Algorithm in a lollipop graph

TL;DR

This paper explores the topological complexity of robot motion planning on tracks modeled by graphs, providing explicit algorithms for collision-free movement, especially on a lollipop graph.

Contribution

It introduces a method to compute the topological complexity of configuration spaces and presents an explicit collision-free motion planning algorithm for two robots on a lollipop graph.

Findings

Calculated topological complexity for various graph tracks

Developed an explicit collision-free motion planning algorithm for two robots

Demonstrated the algorithm on a lollipop graph

Abstract

This paper is concerned with problems relevant to motion planning in robotics. Configuration spaces are of practical relevance in designing safe control schemes for robots moving on a track. The topological complexity of a configuration space is an integer which can be thought of as the minimum number of continuous instructions required to describe how to move robots between any initial configuration to any final one without collisions. We calculate this number for various examples of robots moving in different tracks represented by graphs. We present and implement an explicit algorithm for two robots to move autonomously and without collisions on a lollipop track.

Equations

$$I$$

$$S^1$$

$$D$$

$$X\times Y=\{(x,y)|x\in X,y\in Y\}.$$

$$A=\{(r,\theta )|\theta \in [0,2\pi ],r=\frac{\pi }{6}+|\sin (\theta +\frac{\pi }{6}{)}^5|\}$$

$$B=\{(r,\theta )|\theta \in [0,\pi ],r=3\cos (\theta )\}$$

$$X=C^n(\Gamma )=\underset{n\text{ times}}{\underbrace{\Gamma \times \Gamma \times \dots \times \Gamma }}-{\Delta }_n={\Gamma }^n-{\Delta }_n$$

$${\Delta }_n=\{(x_1,x_2,\dots ,x_n)\in {\Gamma }^n|\exists j\ne i,x_i=x_j\}.$$

$$PX=\{\alpha\ |\ \alpha\ \mbox{is a path in $X$}\}.$$

$$ev:PX\to X\times X$$

$$\alpha \mapsto (\alpha (0),\alpha (1))$$

$$s:X\times X\to PX$$

$$(a,b)\mapsto {\alpha }_{a,b}$$

$$s:S^1\times S^1\to P{S}^1$$

$$(a,b)\mapsto {\alpha }_{shortest}$$

$$U=\{(a,b)\in S^1\times S^1|a\text{mbox}isnotantipodaltob\}\text{mbox}and$$

$$V=\{(a,b)\in S^1\times S^1|a\ne b\}.$$

$$\mathcal{A}=\{(a,b)\in S^1\times S^1|a\text{mbox}isantipodaltob\}$$

$$X=C^2(L)=L\times L-\Delta$$

$$TC(G)=\left\{\begin{array}[]{cl}1&\textrm{if }b_{1}(G)=0\\ 2&\textrm{if }b_{1}(G)=1\\ 3&\textrm{if }b_{1}(G)>1\end{array}\right.$$

$$U_2=\{(x_i,x_f)\in {\hat{S}}_j\times {\hat{S}}_j|x_i\text{mbox}isantipodalto{x}_f,\text{mbox}forj=1,2,3\}$$

$$\begin{cases*}A_{i}=B_{f}<{1\over 2}\mbox{ or }A_{f}=B_{i}>{1\over 2}&if $j=1$\\ A_{i}=B_{f}\mbox{ and }A_{f}=B_{i}&if $j=2$\\ A_{i}=B_{f}>{1\over 2}\mbox{ or }A_{f}=B_{i}<{1\over 2}&if $j=3$\end{cases*}$$

Full text

H.C. is supported in part by the Simons Foundation.

1. Introduction

We study the navigation problem for two robots to move autonomously and without collisions on a lollipop-shaped track.

This paper surveys results concerning the motion planning problem for robots moving on graphs and includes a new explicit algorithm for the case of two robots on a lollipop graph. This algorithm is optimal in the sense that the motion planning is performed with the minimal number of instabilities.

First we introduce the configuration space of distinct robots on a graph. Configuration spaces in mathematics were introduced in the sixties by Fadell and Neuwirth [1] and first used in robotics in the eighties [9, 8]. More recently, configuration spaces of robots moving on graphs have been studied by Farber [4] and Ghrist [6] amongst others.

The configuration space for two robots moving on a graph is the space of all feasible combined positions of the robots. The motion planning problem deals with assigning paths between initial and final configurations. We are interested in algorithms that assign outputs (paths) to inputs (initial and final configurations) in a continuous way. These algorithms are rare in real world situations since most algorithms will have discontinuities. Farber introduced the notion of topological complexity of configuration spaces which measures the discontinuities in algorithms for robot navigation [3]. The topological complexity of a space is invariant under homotopy.

Our approach consists on presenting an explicit construction of the configuration space for the case of two robots moving along a lollipop track and then building a deformation retract of it that we call the skeleton. We calculate the topological complexity of the skeleton of the configuration space and exhibit an algorithm with the minimal number of instructions. Finally, we translate back our instructions to the physical space where the robots move. We provide a detailed algorithm with concrete instructions for two robots to move on a lollipop track between any initial and final positions. We implement this algorithm as well and show a simulation for several cases.

We organize the paper in the following manner. In Section 2 we introduce some basic notions and notations. Section 3 introduces the basic ideas on continuous motion planning. Section 4 presents the topological complexity and its basic properties. Section 5 provides a detailed study for the case of two robots moving on a circle track. We construct the configuration space, calculate its topological complexity and present an algorithm for this case that will set the basis for our main example. Section 6 concerns the characterization of our main example: two robots moving on a lollipop track. We construct the configuration space as before and give an explicit algorithm for this case. We also implement and show a simulation for this case.

This paper is the result of an undergraduate research project at Wilbur Wright College supervised by Professor Hellen Colman.

2. Preliminaries

In this section we briefly review some basic definitions and establish notations that we will use in this paper. From now on, the interval, circle and disk will be denoted as $I,S^{1}$ and $D$ respectively:

[TABLE]

2.1. Product Topological Space

Given two topological spaces $X$ and $Y$, consider the cartesian product:

[TABLE]

The product space is the set $X\times Y$ endowed with the product topology.

Example 2.1**.**

Let $A$ and $B$ be the following two subspaces of $\mathbb{R}^{2}$ in polar coordinates:

[TABLE]

[TABLE]

The product space shown in figure 2 is the set $A\times B=\{(x,y)\ |\ x\in A,\ y\in B\}$ endowed with the product topology.

2.2. Homeomorphism and Homotopy

A continuous function $f:X\to Y$ is a homeomorphism if and only if $f^{-1}$ is also continuous and $f$ is a bijection.

Let $f$ and $g$ be two continuous maps between spaces $X$ and $Y$. Let $H:X\times I\to Y$ be a continuous map such that for each $x\in X$, $H(x,0)=f(x)$ and $H(x,1)=g(x)$, then $f$ and $g$ are homotopic and $H$ is a homotopy between $f$ and $g$. We denote $f\simeq g$. Two spaces $X$ and $Y$ have the same type of homotopy if there exist two functions $f:X\to Y$ and $g:Y\to X$ such that $f\circ g\simeq id_{Y}$ and $g\circ f\simeq id_{X}$.

Informally, we can say that one space has the same homotopy type as another if we can deform one space into the other by compression, stretching and without tearing the space nor gluing any two parts of it. For instance, the space $A\times B$ in figure 2 has the same homotopy type as the torus $T=S^{1}\times S^{1}$ in figure 3.

If two spaces are homeomorphic then they have the same type of homotopy. The homotopy however does not imply homeomorphism.

A topological space $X$ is contractible if $X$ has the same type of homotopy as a point. In other words, there exists a deformation as described before from the space $X$ into a point. For instance, a disk $D$ is contractible whereas a circle $S^{1}$ is not.

3. Motion Planning Algorithms

In order to study the problem of planning the movement of robots in a certain space, we need to introduce certain notions and an associated space to aid with the statement as well as the solution of the problem.

3.1. Physical and Configuration Space

The physical space $\Gamma$ is the track where the robots move. The combined positions of all robots at any given moment defines a state of the system. The space of all possible states is the configuration space. Each state in the configuration space is a point that represents a unique configuration of the robots’ locations in the physical space. The physical space is a special case of a configuration space where there is only one robot.

The configuration space $X$ of $n$ robots moving on the physical space $\Gamma$ without collisions is:

[TABLE]

where $\Delta_{n}$ is the diagonal. The diagonal represents the set of pairs where the robots are co-located and is defined as:

[TABLE]

For all spaces $\Gamma$, we have that $C^{1}(\Gamma)=\Gamma$.

3.2. Motion Planning

A path $\alpha$ in $X$ is a continuous function $\alpha:I\to X$, where $I$ is the interval [0,1]. The path space $PX$ is the set of all paths in the space $X$,

[TABLE]

The evaluation function $ev$ is a map that takes in a path $\alpha$ in $X$ and returns the initial and final points of the path.

[TABLE]

[TABLE]

A section $s$ of the evaluation function is a function that takes in a pair of points in the space $X$ and gives out a path between them. That is,

[TABLE]

[TABLE]

where $\alpha_{a,b}$ is an instruction to move the robots from the starting point $a$ to the ending point $b$ following that specific path. A Motion Planning Algorithm (MPA) is such a section.

3.3. Continuity of Motion Planning

Motion planning algorithms may or may not be continuous. In a continuous motion planning algorithm, the small changes on the path are continuously dependent on the small changes that occur at the initial or final positions. In other words, the instruction to go from $a$ to $b$ will depend continuously on the points $a$ and $b$.

The continuity of the MPA implies the stability of the robot behavior. That is, the errors due to the imprecisions or uncertainties on the initial and final positions would still result in a nearby path.

We are interested in motion planning algorithms that assign instructions in a continuous way. Unfortunately, we’ll see later that these are very rare.

Let us consider for instance, one robot moving on a circle, and we will try to construct a section that would define a MPA. This robot can move between its initial and final position in different ways: clockwise, counter clockwise, following the shortest path, following the longest path, etc.

Set the instruction to be “follow the shortest path”, and the section would be defined as:

[TABLE]

[TABLE]

We observe that this section is continuous in all pairs of positions except at the antipodal pairs, and so, this section is not continuous.

Figure 4 shows the initial (filled triangle icon) and final position (empty triangle icon) of a robot. We see that a small variation in the final position results in a huge variation in the output path.

Now, let us try a different instruction, “go counterclockwise” for instance. Here also, there exists a state where the section is not continuous; that is the state $(a,a)$ for any $a$ in $S^{1}$. In other words where the initial and final positions coincide, the section presents a discontinuity (see figure 5).

For both instructions, the section presented discontinuities. In fact, we will see in the next that section that it is impossible to find a continuous section for this particular example.

4. Topological Complexity

Farber proved that a continuous MPA on a configuration space exists only if the space is contractible.

Theorem 4.1**.**

[3]** A globally defined continuous motion planning algorithm in $X$ exists if and only if the configuration space $X$ is contractible.

Since in the previous example the robot was moving on a circle $S^{1}$ which is also the configuration space in this case, and since a circle $S^{1}$ is not contractible, we have that there exists no continuous MPA for one robot moving on a circle.

We can however decompose the cartesian product of the configuration space into subdomains, with each having a continuous instruction.

Definition 4.2**.**

[3] The topological complexity $TC(X)$ of a connected space $X$ is defined as the minimum integer $k$ such that the cartesian product $X\times X$ can be covered by $k$ open subsets $U_{1},U_{2},...,U_{k}$, such that for any $i=1,2,...,k$ there exists a continuous section $s_{i}:U_{i}\to PX$ with $ev\circ s_{i}=i:U_{i}\hookrightarrow PX$.

The sets $U_{1},U_{2},...,U_{k}$ will be the called the domains of continuity. If no such number $k$ exists, the $TC(X)$ will be set to be $\infty$. We observe that if the space $X$ is contractible, then $TC(X)=1$.

Example 4.3**.**

One robot moving on an interval $I$. In this case, we have that the configuration space is $X=C^{1}(I)=I$ and $TC(I)=1$ with domain of continuity $U=I\times I$. Since $I$ is contractible, there exists a continuous MPA where the instruction would be for example “go in straight line towards the final position”.

Example 4.4**.**

One robot moving on a circle $S^{1}$. Since there is just one robot, the configuration space is $X=C^{1}(S^{1})=S^{1}$.

Since $S^{1}$ is not contractible, $TC(S^{1})>1$. We have seen previously two examples of MPAs for one robot on $S^{1}$ where both had discontinuities. Now we can combine the two subsets where each instruction is continuous so that the union would cover the cartesian product $S^{1}\times S^{1}$.

Then, the domains of continuity will be:

[TABLE]

[TABLE]

Therefore, $TC(X)=2$ since we exhibit an explicit covering by domains of continuity with exactly two sets.

From the MPA standpoint, this decomposition into $k$ subspaces can be interpreted as having a set of $k$ continuous instructions, where each operates in its corresponding domain of continuity. For this example, the resulting two instructions in the MPA would be as follows: If the initial and final positions are not antipodal then go following the shortest path. Otherwise, go counterclockwise.

Farber also proved that the topological complexity $TC(X)$ of a space $X$ is invariant under homotopy.

Theorem 4.5**.**

[3]** If $X\simeq Y$ then $TC(X)=TC(Y)$.

Computing the topological complexity of a configuration space is an essential step to derive the motion planning algorithm in the corresponding physical space since it provides the minimum number of instructions of the MPA. The examples seen so far have relatively simple configuration spaces, but as we increase the number of robots and the physical space gets more complex, so does the configuration space. Finding the discontinuities directly while in the configuration space is not trivial, and the above theorem is crucial to reduce the overall complexity by using the homotopy not only to calculate the topological complexity of a simpler space, but also to construct the actual algorithm.

5. Two robots moving on a graph

We consider the case of two distinct robots $A$ and $B$ moving on a graph $\Gamma$. The problem to solve is finding a continuous and collision-free motion planning algorithm with the least number of instructions needed to move both robots A and B from their initial positions to their final positions.

5.1. Two robots moving on an interval $I$

Let $A$ and $B$ be two robots moving on $\Gamma=I$.

The configuration space in this case is $X=C^{2}(I)=I\times I-\Delta$. The physical space and the configuration space are shown in figure 8. Each pair of positions of the two robots in the physical space defines a state in the configuration space. Figure 8 shows a concrete pair of positions in the physical space and its corresponding state in the configuration space.

We observe that the configuration space in this case is not connected because of the removal of the diagonal. Even though there are some states where there is a path between the initial and final positions of the two robots (see figure 9), the problem of finding a path between any initial and final configuration has no solution (figure 10). In other words, there is no MPA for this case.

This impossibility to connect two states in the configuration space by a path has its counterpart in the physical space: two robots cannot swap positions within the interval.

5.2. Two robots moving on a circle $S^{1}$

Now, let’s consider two robots $A$ and $B$ moving on $\Gamma=S^{1}$. The configuration space in this case is given by $X=C^{2}(S^{1})=S^{1}\times S^{1}-\Delta\ =\ T-\Delta.$ This space is homeomorphic to a cylinder $X=C^{2}(S^{1})=S^{1}\times I$.

A very useful way to represent this space would be using the flat torus representation as shown in figure 11. We also shown in this figure the initial and final positions of two robots in the physical space and their corresponding states in the configuration space.

By working in the configuration space, the complexity of the problem is reduced from trying to find two paths to move two distinct robots from their initial positions to their respective final positions into finding one combined path to move the initial state of the two robots to the final state in the configuration space.

5.2.1. Topological Complexity of the Configuration Space

As we have seen before, the configuration space is $X=C^{2}(S^{1})=S^{1}\times I$. Since the configuration space is homotopy equivalent to a circle, we have that $TC(X)=TC(S^{1}\times I)=TC(S^{1})=2$ by theorem 4.5.

This gives us the minimum number of instructions needed for a continuous MPA for two robots moving on a circle $S^{1}$. Knowing how to transition back and forth between the physical space and the configuration space, our objective is first to find two instructions that define the algorithm to move from the initial state to the final state in the the configuration space, and then translate them back to the physical space.

We will take advantage of the construction of an explicit homotopy between the configuration space and the circle to build our algorithm based on the known algorithm for the circle given in the example 4.4.

5.2.2. MPA for two robots moving on $S^{1}$

Recall that the configuration space $X$ is path-connected and is homotopy equivalent to a circle. For the purpose of the homotopy, we will choose a special circle to deform the configuration space into: the antipodal circle.

Definition 5.1**.**

The antipodal circle $\mathcal{A}\subset C^{2}(S^{1})$ is the set of pairs of points on the circle such that they are antipodal to each other. We write:

[TABLE]

The antipodal circle lays halfway of the diagonal $\Delta=\{(a,b)\in S^{1}\times S^{1}\ |\ a=b\}$, and thus, it splits the configuration space into two equal and symmetric subspaces. In the flat torus representation (figure 12), the antipodal circle $\mathcal{A}$ will be the set of points in $X$ lying on the line $b=a-\frac{1}{2}$.

Consider the homotopy $H:X\times I\to X$ given by the projection into $\mathcal{A}$ following horizontal lines. Recall that each state $x=(A,B)$ represents the collective positions of robots $A$ and $B$ in the circle.

Let ${x_{i}}$ and $x_{f}$ be the initial and final states of the robots. We call the initial antipodal state $x_{i}^{\prime}\>=\>H_{1}(x_{i})$ the image of the initial state in the antipodal circle $\mathcal{A}$ by the homotopy. Likewise, we call the final antipodal state $x_{f}^{\prime}\>=\>H_{1}(x_{f})$ the projection per homotopy of the final state in $\mathcal{A}$ (see figure 13).

The idea of our algorithm will be to move the initial and final states $x_{i}$ and $x_{f}$ to their antipodal states $x_{i}^{\prime}$ and $x_{f}^{\prime}$ respectively, and then apply in the circle $\mathcal{A}$ the known algorithm (example 4.4) to move from $x_{i}^{\prime}$ to $x_{f}^{\prime}$ (see figure 14).

We will also define certain distinguished positions in the configuration space that will correspond to the action of interchaging robots in the physical space.

Definition 5.2**.**

Two states $x_{i}=(A_{i},B_{i})$ and $x_{f}=(A_{f},B_{f})$ are swapped states if $A_{i}=B_{f}$ and $A_{f}=B_{i}$.

Swapped states correspond to points in the configuration space that are symmetric with respect to the diagonal (see figure 16).

Remark 5.3**.**

Observe that the swapped states that are in the antipodal circle $\mathcal{A}$ correspond exactly to the antipodal points in that circle.

The following is the description of our algorithm in the configuration space $X$.

1. (1)

Preliminary step: Move initial state $x_{i}$ to its antipodal state $x_{i}^{\prime}$ on $\mathcal{A}$ following the path $H_{t}(x_{i})$. 2. (2)

Main step: While on $\mathcal{A}$, if the two antipodal states $x_{i}^{\prime}$ and $x_{f}^{\prime}$ are swapped states, then move the initial antipodal state $x_{i}^{\prime}$ counterclockwise towards the final antipodal state $x_{f}^{\prime}$. Otherwise, move the initial antipodal state $x_{i}^{\prime}$ following the shortest path on $\mathcal{A}$ towards the final antipodal state $x_{f}^{\prime}$. 3. (3)

Final step: Move the final antipodal state $x_{f}^{\prime}$ back to the final state $x_{f}$ following the reverse path to $H_{t}(x_{f})$.

When transitioning back to the physical space, we obtain the following MPA to move two robots $A$ and $B$ in a circle:

1. (1)

Preliminary step: Move robot $A$ away from robot $B$ until robot $A$ reaches the antipodal position of the robot $B$. Robot $B$ remains stationary. 2. (2)

Main step: If the final position of the robot $B$ is antipodal to its initial position, then move both robots in counterclockwise direction until robot $B$ reaches its final destination. Otherwise, move both robots in the same direction following the shortest path for robot $B$ to its final position, until robot $B$ reaches its final position. 3. (3)

Final step: Move robot $A$ following the shortest path to its final position, until it reaches its final position. Robot $B$ remains stationary.

Note that the preliminary and final steps are common steps in both instructions, the main step is where we deal with the discontinuity of the MPA.

The domains of continuity will be determined by the regions described before. Let $V_{1}=\{((a,b),(a^{\prime},b^{\prime}))\in X\times X|\;b\mbox{ is antipodal to }b^{\prime}\}$ and $V_{2}$ the set of all other pairs of points in the configuration space. Consider $U_{1}$ a small neighborhood of $V_{1}$ and $U_{2}=V_{2}$. Then $\{U_{1},U_{2}\}$ is a covering of $X\times X$ by domains of continuity.

5.3. Running the proposed MPAs

Example 5.4**.**

Initial and final positions of the second robot are not antipodals.

Example 5.5**.**

Initial and final positions of the second robot are antipodals.

In the next section, we’ll expand this construction of the algorithm to derive a MPA for two robots moving on a lollipop-shape track.

6. Main example: Lollipop graph $\Gamma=L$

We focus our efforts now on the construction of a MPA for the case of two robots moving on a track consisting of a circle with an interval attached.

We consider the class $G_{n,g}$ of connected graphs on $n$ vertices with fixed girth $g$ of which the generalized lollipop graphs, $C_{n,g}$, will be the ones obtained by appending a single $g$ cycle to a pendant vertex of a path on $n-g$ vertices [2]. See figure 25.

Definition 6.1**.**

A lollipop graph $L$ will be a generalized lollipop graph with two vertices and girth 1, $L=C_{2,1}$. See figure 26.

6.1. Configuration space

The configuration space $X$ is given by:

[TABLE]

where $L$ is a lollipop graph.

Figure 27 depicts $X$ and its flat representation. Recall that in the configuration space, each state represents the combined positions of the two robots in $L$, where the $x$-axis represents the positions of the first robot $A$, and the $y$-axis represents the positions of the second robot $B$ as in figure 28.

6.1.1. Homotopy deformation

Ghrist proved in [7] that the configuration space of any graph with $k$ vertices of degree greater than two, deformation retracts to a subcomplex of dimension at most $k$ . We have then that our configuration space is homotopy equivalent to a 1-dimensional space. Our motion planning algorithm will be derived while working in the configuration space, as we explained in the previous examples. The execution of the algorithm, however, will run in the physical space. We will chose a 1-dimensional space in the configuration space that has a clear meaning when projected back into the physical space. Following the same approach as before, this 1-dimensional space, that we will call the skeleton $S$, will lay halfway from the diagonal representing the collision line if the two robots are both in the circle or in the interval. This graph will be completed with the addition of other segments as in figure 29.

The homotopy $H$ deforming the configuration space $X$ into the skeleton $S$ is shown in figure 30. This homotopy will be useful to transit in and out of the skeleton in the configuration space $X$.

The skeleton $S$ is homeomorphic to a chain of three circles with two added intervals as shown in figure 31. By contracting the intervals and the lower part of each circle, we can see that the skeleton is homotopy equivalent to a wedge of three circles $S^{1}\vee S^{1}\vee S^{1}$ as shown in figure 32.

6.2. Topological Complexity

In order to exhibit a set of instructions for the motion planning algorithm, we will consider the number of these continuous instructions given by the topological complexity of the configuration space, $TC(X)$. Farber calculated the topological complexity of all graphs based on their first Betti number.

In topological graph theory, the first Betti number of a graph $G$ with $n$ vertices, $m$ edges and $k$ connected components is given by $b_{1}(G)=m-n+k$.

Proposition 6.2**.**

[5]** Let $G$ be a graph, then

[TABLE]

Theorem 6.3**.**

The topological complexity of the configuration space of two robots moving on a lollipop graph is three, $TC(C^{2}(L))=3.$

Proof.

We have shown that $C^{2}(L)\simeq S^{1}\vee S^{1}\vee S^{1}$. By proposition 4.5, we know that $TC(C^{2}(L))=TC(S^{1}\vee S^{1}\vee S^{1})$. Since the first Betti number of a wedge of three circles is $b_{1}=3-1+1=3$, we have that $TC(S^{1}\vee S^{1}\vee S^{1})=3$ by proposition 6.2. Therefore, $TC(C^{2}(L))=3$. ∎

We conclude that for two robots moving on a lollipop graph, any MPA would require at least three continuous instructions.

6.3. Motion Planning Algorithm

We proved previously that $TC(X)=3$ if $X=C^{2}(L)$, which defines the minimum number of continuous sections in $X\times X$. Recall that although the set of instructions of the algorithm will be derived in the configuration space, its execution will be run in the physical space.

If the state corresponding to the two robots’ positions is on the skeleton $S$, we know that in the physical space the robots are one half unit apart from each other. Recall that the distance between two robots in $L$ is the minimal length of the paths in $L$ joining them. See figure 33 for examples of distances between robots.

Definition 6.4**.**

Two robots are in generalized antipodal positions in $L$ if the distance between them is equal to half unit. See figure 34.

Observe that the skeleton $S=\{(a,b)\in L\times L|a\mbox{ is generalized antipodal to }b\}$ corresponds to the configurations in which the robots are in generalized antipodal position.

Consider the homotopy $H:X\times I\to X$ given by the projection into the skeleton $S$ following the traces of the homotopy as in figure 30.

Let ${x_{i}}$ and $x_{f}$ be the initial and final states of the robots. We call the initial generalized antipodal state $x_{i}^{\prime}\>=\>H_{1}(x_{i})$ the image of the initial state in the skeleton $S$ by the homotopy. Likewise, we call the final generalized antipodal state $x_{f}^{\prime}\>=\>H_{1}(x_{f})$ the projection per homotopy of the final state in $S$.

As before, the idea of our algorithm will be to move the initial and final states $x_{i}$ and $x_{f}$ to their generalized antipodal states $x_{i}^{\prime}$ and $x_{f}^{\prime}$ respectively, and then apply in the skeleton $S$ a new algorithm to move from $x_{i}^{\prime}$ to $x_{f}^{\prime}$ that we will explain next.

6.3.1. Moving the states within the skeleton

Recall that the skeleton $S$ is the graph shown in figure 35. Let $V$ be the set of vertices of the graph $S$. We denote $\tilde{V}$ the extended set of vertices given by $\tilde{V}=V\cup I_{1}\cup I_{2}$.

We will decompose the Cartesian product $S\times S$ in three regions. The first region $U_{1}$ will consist of the pairs of points in $S$, initial and final, such that each is an extended vertex, $U_{1}=\tilde{V}\times\tilde{V}$. The instruction here will be to go shortest path. If traversing a circle, choose always the counterclockwise direction.

The second region is the set of points that are antipodal to each other in any circle but none of them is a vertex,

[TABLE]

where $\hat{S}_{j}$ is the circle $S_{j}$ minus the vertices. The instruction in this region is to go counterclockwise in $S_{j}$ from the initial to the final state.

The third region is the rest of the Cartesian product, $U_{3}=(S\times S)\setminus(U_{1}\cup U_{2})$. The instruction in this case is to go following the shortest path and choosing the counterclockwise direction whenever traversing any circle.

6.3.2. Algorithm in the configuration space

Let $V_{1}$ be a small open neighborhood of $U_{1}$, $V_{2}$ an open neighborhood of $U_{2}$ and $V_{3}=U_{3}$.

1. (1)

Preliminary step: Move initial state $x_{i}$ to its generalized antipodal state $x_{i}^{\prime}$ on $S$ following the path $H_{t}(x_{i})$. 2. (2)

Main step: While on $S$, if the two generalized antipodal states $x_{i}^{\prime}$ and $x_{f}^{\prime}$ are in $V_{1}$, then go to $x_{f}^{\prime}$ following shortest path. When crossing any circle $S_{j}\subset S$ go in counterclockwise direction (see figure 36). If $x_{i}^{\prime}$ and $x_{f}^{\prime}$ are in $V_{2}$, then move $x_{i}^{\prime}$ in counterclockwise direction until it reaches $x_{f}^{\prime}$. Otherwise, move $x_{i}^{\prime}$ to $x_{f}^{\prime}$ following shortest path and in counterclockwise direction whenever crossing any circle $S_{j}\subset S$. 3. (3)

Final step: Move the final antipodal state $x_{f}^{\prime}$ back to the final state $x_{f}$ following the reverse path of $H_{t}(x_{f})$.

6.3.3. Algorithm in the physical space $L$

Our aim is to identify in the physical space the positions corresponding to the different regions described in the configuration space.

First we define certain distinguished positions in the configuration space that will correspond to the generalization of the idea of interchanging robots in the physical space.

Definition 6.5**.**

Two states $x_{i}=(A_{i},B_{i})$ and $x_{f}=(A_{f},B_{f})$ in $S_{j}\subset S$ are generalized swapped states if

[TABLE]

The generalized swapped states depicted in figure 37 correspond in the physical space to the positions depicted in figure 38.

Remark 6.6**.**

Observe that if initial and final states are generalized swapped states, then the initial and final positions of at least one robot are antipodal in the circle of the lollipop $L$.

We define the extended interval $\tilde{I}$ in $L$ as the interval $I$ together with the pole $P$ in $S^{1}$ as shown in figure 39, $\tilde{I}=I\cup\{P\}$.

Definition 6.7**.**

We say that two robots in generalized antipodal position are in generalized vertex position if they both are in the extended interval $\tilde{I}$.

For robots in generalized vertex position, the initial and final positions of the two robots are either in the same or reverse order.

Definition 6.8**.**

Two pairs of positions $(A_{i},B_{i})$ and $(A_{f},B_{f})$ in $\tilde{I}$ are said to be in the same order if there exists a collision-free translation motion from $A_{i}$ to $A_{f}$ and $B_{i}$ to $B_{f}$ within the interval. If no such translation motion exists, then the two pairs of positions are said to be in reverse order.

Remark 6.9**.**

We observe that initial and final states in generalized vertex position are in the same order if the initial and final positions of the first robot are both in the same half of the interval edge of the lollipop.

The instructions of the motion planning algorithm in $L$ will be as follows:

1. (1)

Preliminary step: If the distance between the two robots is more than half unit (figure 40), then move the robot that is closer to the beginning of the interval towards the other robot until both robots are at generalized antipodal position. The other robot remains stationary. Otherwise (figure 41), move both robots away from each other (and stay in the circle if moving in the circle) until both robots reach generalized antipodal position.

The ratio between the speeds at which the robots are moving is given by the slopes of the traces of the homotopy in figure 30. Repeat this procedure to move the final positions to a generalized antipodal position as well.

2. (2)

Main step:

• •

If the initial and final generalized antipodal positions are in generalized vertex position, then move the two robots to their final positions following shortest path if they are in the same order, or go around the circle in counterclockwise direction towards the final positions if they are in reverse order.

• •

If the initial and final generalized antipodal positions are generalized swapped positions, then move whichever robot is in the circle counterclockwise to its final destination while remaining in the circle. The other robot moves accordingly so that to stay at half unit distance away from the other.

• •

Otherwise, for all remaining configurations, move both robots following shortest paths, and counterclockwise whenever in the circle, until they reach their final positions. 3. (3)

Final step: Move both robots to their final positions following shortest paths. Recall that the ratio of the speeds at which the two robots will be moving in this case is given by the slopes of the traces of the homotopy in figure 30.

6.4. Running the algorithm in the physical space $L$

We will show now some sample cases for each of the domains of continuity.

1. (1)

Domain of continuity $V_{1}$: Extended Interval

2. (2)

Domain of continuity $V_{2}$: Generalized swapped positions.

3. (3)

Domain of continuity $V_{3}$

7. Algorithm implementation

A demonstration video for both algorithms implemented in MATLAB is available online at https://figshare.com/articles/Demonstration_video_for_collision-free_motion_planning_algorithms_in_topological_robotics/8019383 (or via the shortened web link: https://bit.ly/demoVideo_MPAs)