High-Dimensional Winding-Augmented Motion Planning with
2D Topological Task Projections and Persistent Homology
Florian T. Pokorny, Danica Kragic, Lydia E. Kavraki, Ken Goldberg
Abstract— Recent progress in motion planning has madeit possible to determine homotopy inequivalent trajectories
between an initial and terminal configuration in a robot
configuration space. Current approaches have however either
assumed the knowledge of differential one-forms related toa skeletonization of the collision space, or have relied ona simplicial representation of the free space. Both of theseapproaches are currently however not yet practical for higher
dimensional configuration spaces. We propose 2D topologicaltask projections (TTPs): mappings from the configuration space
to 2-dimensional spaces where simplicial complex filtrations andpersistent homology can identify topological properties of thehigh-dimensional free configuration space. Our approach only
requires the availability of collision free samples to identify
winding centers that can be used to determine homotopy
inequivalent trajectories. We propose the Winding Augmented
RRT and RRT* (WA-RRT/RRT*) algorithms using which
homotopy inequivalent trajectories can be found. We evaluate
our approach in experiments with configuration spaces ofplanar linkages with 2-10 degrees of freedom. Results indicatethat our approach can reliably identify suitable topologicaltask projections and our proposed WA-RRT and WA-RRT*
algorithms were able to identify a collection of homotopy
inequivalent trajectories in each considered configuration spacedimension.

I. I NTRODUCTION
Over the last two decades, sampling based motion planning approaches have enabled the planning of complex
motions even for robotic systems with a many degrees of
freedom. Algorithms such as Rapidly Exploring Random
Trees (RRT) [13] and Probabilistic Roadmaps (PRM) [12]proceed by incrementally constructing a sampling basedgraph-representation G of the environment using which theconnectivity of the free configuration space Cf can beapproximated as the number of samples increases. Thesealgorithms can determine feasible trajectories between an
initial and a terminal configuration in Cf and recent variants
such as RRT* and RRG [11] can asymptotically determine
a shortest path. While these methods approximate the pathconnectivity of Cf as the number of samples is increased,
they do not capture all topological information about the
space of continuous paths in Cf . More precisely, the graph G
Florian T. Pokorny and Ken Goldberg are with the Department ofComputer Science and Electrical Engineering, University of California,
Berkeley. Ken Goldberg is also with the Department of Industrial
Engineering and Operations Research, University of California, Berkeley.
Danica Kragic is with CAS/CVAP, KTH Royal Institute of Technology
and Lydia Kavraki is with the Department of Computer Science, Rice
University,
ftpokorny@berkeley.edu, dani@kth.se,kavraki@rice.edu, goldberg@berkeley.edu.
Florian T. Pokorny acknowledges support from the Knut and Alice
Wallenberg Foundation. Danica Kragic was supported by the EU grant
FLEXBOT (FP7-ERC-279933).

Π1

Π2

Cf
0.5

0

0

0.5

Fig. 1: Illustration of our approach: The top left part of the figure

illustrates a configuration space Cf with two cylindrical obstacles. Collision
free samples X ⊂ Cf in black are projected along two task-projections
Π1 , Π2 onto the (x, y) and (y, z) coordinates. Our persistent homology
approach determines that each projection carries non-trivial information
about homotopy classes in Cf by computing a persistence diagram for each(red and blue points in the diagram in the bottom right). The projection of
Cf is approximated by a simplicial complex and winding centers lying in
the projection of the collision space are automatically found (red points inthe projections). Our WA-RRT* algorithm is initialized with these winding
centers and determines four homotopy inequivalent trajectories in Cf .

does not capture higher order topological information about
Cf which is contained in the first homology and homotopy
groups of Cf . These algorithms are hence currently not able
to distinguish homotopy classes of trajectories – where two
trajectories are called homotopy inequivalent if they cannot
be continuously deformed into one another (see Fig. 1).
We study the problem of finding a collection of several
homotopy inequivalent trajectories between pairs of points
in the configuration space. Let us highlight two benefits ofbeing able to find such motions: Since continuous trajectory
optimization approaches can deform a given initial trajectory
only within its homotopy class, an initialization of thesealgorithms with several sub-optimal trajectories in distinct
homotopy classes holds promise to avoid local minima.
Furthermore, the ability to reason about homotopy classes,
can be useful, for example to replan a trajectory in a different
homotopy class when a given trajectory becomes infeasible due to changing environment conditions as homotopy

inequivalent trajectories provide a knowledge of alternative
motion classes to the robot. We propose the use of continuous
projections Πi : Cf → R2 , i ∈ {1, . . . , n} to 2-dimensional
spaces for the purpose of determining homotopy inequivalent
trajectories between two points s, t ∈ Cf in a collision
free configuration space Cf . Our approach uses a finite setof samples X ⊂ Cf that are mapped to Πi (X) ⊂ R2
and utilizes persistent homology to test whether nontrivial
topological information about the fundamental group π1 (Cf )is captured by Πi . We use persistent homology to determine‘winding centers’ using which we formulate the topologicalmotion planning problem and we introduce an adaptation of
RRT and RRT*-based motion planners which we call WARRT and WA-RRT* (Winding Augmented - RRT/RRT*).
These algorithms allow incorporating the determined topological information to find homotopy inequivalent trajectories
and we present an experimental evaluation in configuration
spaces of dimension 2-10 using planar linkages.
II. BACKGROUND
Homotopy-Aware Motion Planning: We consider the
problem of determining homotopy inequivalent trajectories
between two points in a configuration space Cf . Early
work on synthesizing homotopy inequivalent trajectories has
focused on two dimensional configuration spaces for planar
robot motion planning. Jenkins [10] decomposed a 2D configuration space using wedge-sectors around a central point
to describe trajectories as words classifying the transitionof a path with respect to these wedges. Grigoriev et al [6]considered planar cuts to construct homotopy inequivalent
trajectories in the plane while [18] suggested a PRM based
approach called Homotopy Preserving Roadmaps in 2D. In[17], homology classes are advocated instead of homotopy
classes to plan homotopy inequivalent trajectories by meansof winding angles in 2D which was later generalized tohigher dimensions [3]. This approach relied on an initialgraph representation of the free configuration space which
was then augmented by a topological signature integratinga differential one-form along a given path. This differential
one-form, capturing information about the topology of thecollision space, was assumed to be given and in 2D corresponded to a winding angle and relied on a representative given winding center in the interior of each obstacle
surrounded by free space. We build on these prior works,
but here only assume the ability to sample collision freepoints in Cf . We furthermore propose the use of continuous
projections to 2 dimensional space Πi : Cf → R2 and a first
method to automatically extract winding points in a sampling
driven manner by using persistent homology.
Topological approaches have inspired the work of [20] utilizing writhe to form a representation for motion planning toefficiently plan twisting motions. This work has however notstudied the generation of homotopy inequivalent trajectories.
Our work [16] utilizes winding angles to plan enveloping
motions of a hand to cage an object, but homotopy classesof motions are not the focus of this work. Our work is related
to these approaches in that we also use winding angles. Our

approach utilizes a winding augmented covering space on
which our RRT/RRT* based planners perform incremental
search resulting in homotopy inequivalent trajectories. A
further difference to our approach is that we only assume theavailability of collision free samples in Cf without analytic
information about the obstacles.
In [15], we used persistent cohomology and represented
the free configuration space of a robot in a sampling basedmanner by means of a filtration of simplicial complexes toplan trajectories in distinct homology classes in dimensions
up to 4. The construction of these simplicial complexes ishowever currently impractical in higher dimensions sincethese methods are based on Delaunay triangulations ofsamples X whose complexity in dimensions higher than 4quickly becomes infeasible for large numbers of samples.
In this work, we also utilize persistent homology, but only
under a projection to 2D task spaces. We thus avoid the‘curse of dimensionality’ of constructing high-dimensional
simplicial approximations of Cf itself. Our approach to use
projections is also inspired by the planner KPIECE [19] thatutilizes projections to lower dimensions to efficiently guidea tree-based motion planner.
Path Homotopy Classes and Homology: In order to
identify whether two trajectories are homotopy inequivalent,
we require topological information about the free configuration space Cf beyond the path-connectivity of Cf . The
fundamental group π1 (Cf , x0 ), whose elements are given byequivalence classes of closed trajectories based at a pointx0 ∈ Cf can distinguish homotopy classes of trajectories
because two trajectories γ1 , γ2 : [0, 1] → Cf between x0 andanother point x1 ∈ Cf yield a closed loop l = γ1 ◦ −γ2 :[0, 1] → Cf following γ1 from x0 to x1 and then γ2 fromx1 to x0 . The loop l corresponds to the identity elementin π1 (Cf , x0 ) precisely if γ1 , γ2 are homotopy equivalent.
When Cf is path-connected, π1 (Cf , x0 ) is independent of thebase point [7]. Typically, the fundamental group has infinite
cardinality and forms a highly complex group that is typically
non-commutative. To avoid the complexity of π1 (Cf ), we
consider a commutative version of π1 (Cf ) which is provided
by the first homology group H1 (Cf ) [17].
Simplicial Complexes and Persistent Homology: Oneof the key problems we address in this paper is the detection of topologically non-trivial information as identified by
π1 (Cf ). In order to detect that π1 (Cf ) is non-trivial basedon collision free samples – and to therefore detect the factthat homotopically distinct trajectories exist – we will utilize
persistent homology [4] which we review now.
Given a sample of points X ⊂ Rd , weScan consider thefamily of union of balls spaces Xr = x∈X {y ∈ Rd :kx − yk 6 r}, for r > 0 as in prior work [15]. For
each r, Xr is homotopy equivalent to the Delaunay-Čechcomplex DCr (X) [1], which is a simplicial complex definedfor the finite set X ⊂ Rd and a radius parameter r whichin this context is called the filtration value. Let us recall
here that a geometric k-simplex σ = [v0 , . . . , vk ] in Rd is aconvex hull of k + 1 ordered affinely independent elements
v0 , . . . , vk ∈ Rd and a convex hull of an ordered subset of

these elements is called a face τ of σ, indicated by τ 6 σ.
We call k the dimension of a k-simplex. A (finite) simplicial
complex K is a non-empty set of simplices such that if σ ∈ Kand τ 6 σ, then τ ∈ K and if σ, σ 0 ∈ K then σ ∩ σ 0 is
empty or an element of K. We write |K| for set of points
in Rd contained in the union of all simplices in K. The set|K| is a topological space with the subspace topology from
Rd . Let D(X) denote the simplicial complex corresponding
to the Delaunay triangulation of X with simplices definedby D(X) = {[v0 , . . . , vk ] : vi ∈ X, ∩ki=0 Vvi 6= ∅ for k ∈{0, 1, . . . , d}}, where Vx denotes the Voronoi cell containing
x. For each k-simplex
Tk σ = [v0 , . . . , vk ] ∈ D(X), definef (σ) = min{r :i=0 Br (vi ) 6= ∅}, where Br (x) ={y ∈ Rd : kx − yk 6 r}. The Delaunay-Čech complex
DCr (X), for r > 0 is the sub-complex of D(X) definedby DCr (X) = f −1 ((−∞, r]). The key point about thisconstruction is that, since DCr (X) is homotopy equivalent to
Xr , we can now compute topological information about Xr
from DCr (X) at all scales r > 0 using persistent homology.
Persistent Homology: Whenever the first homology
group H1 (Xr ) is non-trivial, π1 (Xr ) is also non-trivial, sincehomology yields an Abelianization of the fundamental group[7]. As a result, we can conclude that Xr contains distinct
homotopy classes if the first homology group H1 (Xr ) is
non-trivial (but the converse does not hold in general). Our
approach is to investigate H1 (Xr ) at all radii r > 0 bymeans of the first persistent homology of DCr (X). We omit
the technical details of persistence here, more details canbe found in [4], [15]. Fig. 2 illustrates the output of thepersistence algorithm, called the first persistence diagram.
The diagram displays along the horizontal axis the birthradius rb at which a given holes in Xr are first formed(voids that are fully enclosed by Xr ). Along the verticalaxis, the death ‘filtration value’ rd at which a given holedisappear because it is fully covered by Xr is also recorded(formally, this corresponds to rank changes in the first
persistent homology groups). Since rb 6 rd , all points inthe diagram lie above the diagonal and the number of voids
at radius r (formally the dimension of H1 (Xr )) can be readoff by counting the number of points (rb , rd ) with rb 6 r
and rd > r. In the example, there exists one such point in
the shown dashed quadrant for r = 0.1. A point (rb , rd ) in
the persistence diagram is also referred to as a persistenceinterval. The difference ε = rd − rb is called the persistenceof the topological feature corresponding to (rb , rd ). Points
with large persistence have a large vertical distance to the
diagonal and are noise-robust [5]. Points close to the diagonal
can be considered as topological noise – e.g. small voids thatappear and disappear quickly as r changes. In the example, asingle large void exists in the point-cloud, corresponding to
the red point in the first persistence diagram. Corresponding
to each point in the diagram, the computation of persistencealso returns one or more closed paths of edges (1-simplices)
called a 1-cycle which represents a non-trivial element in
the first homology group H1 (DCr (X)) with coefficients in
Z2 . The 1-cycle is displayed in red in the figure to the left
and surrounds the non-trivial hole which is identified by the

0.5

0.1

0

Fig. 2:

0

0.1

0.5

Persistent homology analysis for a point-cloud X ⊂ R2 where

samples are given by vertices (black dots) of the depicted simplicial
complex. The union of balls-space Xr for r = 0.1 as illustrated by blue
discs is approximated by the gray and black simplicial complex DCr (X).
We use the first persistence diagram shown on the right to identify a radiusparameter r = 0.1 such that Xr contains a single hole (there exists just one
point (x, y) in the diagram with x 6 r and y > r. The hole corresponds
to a computed collection of 1-simplices (a 1-cycle) displayed in red whichsurround the identified hole.

cycle. For more details, see [4], [15].
Winding Numbers: Given a continuous curve γ :[0, 1] → R2 − {0}, where (r(t), θ(t)) ∈ R>0 × R denote
continuous polar coordinates of γ, we define1(θ(1) − θ(0)).
2π
If γ is a closed curve, W (γ) ∈ Z is called the winding
number and measures the total number ot times γ winds
around the origin (with sign). In Cartesian coordinates
γ(t) = (x(t), y(t)) and for differentiableR γ, we can compute
1 ẏ(t)x(t)−ẋ(t)y(t)1
W by the integral formula W (γ) = 2πdtx(t)2 +y(t)20while if γ is a piecewise-linear curve W (γ) can be computedby an explicit formula involving tan−1 [8].
For a point w ∈ R2 and a continuous curve γ : [0, 1] →2
R − {w}, we define the winding around the winding centerw by W (γ, w) = W (γ − w), where γ − w denotes thetranslated curve t 7→ γ(t) − w for t ∈ [0, 1]. For x ∈ R2 ,π1 (R2 − {x}) = Z and W in fact provides an isomorphism,mapping any closed curve in R2 −{x} to an integer. Winding
numbers have been used classically, e.g. in complex analysis(Cauchy’s residue theorem). We will use the following result:Lemma 2.1: Let S = {w1 , . . . , wk } ⊂ R2 . And let α, β :[0, 1] → R2 − S be continuous curves such that α(0) =β(0) and α(1) = β(1). If there exists i ∈ {1, . . . , k} suchthat W (α, wi ) 6= W (β, wi ), then α and β are homotopy
inequivalent.
Proof: We observe that the closed curve γ following
α from α(0) to α(1) and then β from β(1) = α(1)backwards to β(0) = α(0) satisfies W (γ, wi ) = W (α, wi )−W (β, wi ) 6= 0. Therefore γ is a non-trivial element of
π1 (R2 − {wi }) and hence α, β are homotopy inequivalentin R2 − {wi }. Since R2 − S ⊆ R2 − {wi } this implies thatα, β are homotopy inequivalent in R2 − S also.
W (γ) =

III. M ETHODOLOGY
We now formalize the planning problem and propose todetermine a collection of homotopy inequivalent trajectories
between given initial and terminal positions, x, y ∈ Cf by
considering a purely sampling-driven approach. To accommodate high-dimensional spaces Cf , we propose to utilize afinite collection of topological task projections Π1 , . . . , Πk :Cf → R2 to identify non-trivial topological information
about the fundamental group πi (Cf ). We now discuss how
such projections Πi can be defined and identified by meansof persistent homology before focusing on our WindingAugmented RRT and RRT* algorithms.
Topological Task Projection and Winding Centers: We
observe that Lemma 2.1 is useful in order to distinguishhomotopy classes in a general 2D free configuration space
Cf ⊂ R2 if we can identify suitable representative points
S = {w1 , . . . , wk } ⊂ R2 − Cf . In order to generalize thisidea to higher dimensions, we observe:
Lemma 3.1: Let X, Y be topological spaces and let Π :X → Y be a continuous function. Consider two continuous
curves α, β : [0, 1] → X such that α(0) = β(0) and α(1) =β(1). Suppose that the curves Π ◦ α, Π ◦ β : [0, 1] → Y arehomotopy inequivalent in Y . Then α, β must be homotopy
inequivalent in X.
Proof: Suppose there was a homotopy H : [0, 1] ×
[0, 1] → X, H(s, 0) = α(s), H(s, 1) = β(s) for all s ∈[0, 1], then (s, t) 7→ π(H(s, t)) yields a homotopy between
π ◦ α and π ◦ β, which leads to a contradiction.
The above lemma opens a possibility to detect homotopy
inequivalence by means of continuous maps Π : Cf → R2 .
However, this approach is only feasible if Π(Cf ) contains
non-trivial homotopy classes of curves which we can test byconsidering π1 (Π(Cf )):Definition 3.2: For a path-connected topological space Cf ,let Π : Cf → Rn be a continuous map such that π1 (Π(Cf ))is non-trivial. We call Π an n-dimensional topological task
projection of Cf .
Here, we utilize 2-dimensional topological task projections.
Since H1 (Π(Cf )) 6= 0, computed over finite field coefficients
implies that π1 (Π(Cf )) is non-trivial, we use the computationally more amenable first homology group H1 (Cf ) to test
whether a map Π : Cf → R2 is in fact a topological task
projection. While it might be possible to compute H1 (Π(Cf ))when Π(Cf ) is analytically determined, we work under theassumption that we are only able to obtain samples in Cf .
Topological Task Projection Identification: Given acollection of samples X ⊂ Cf and a continuous candidate
map Π : Cf → R2 , we would like to evaluate whether
H1 (Π(Cf )) is non-trivial. We recall the following manifold
reconstruction result of Niyogi:
Theorem 3.3 (Niyogi [14]): Let X = {x1 , . . . , xn } ⊂ Rd
such that X is 2r -dense and let M ⊂ Rd be a compact
Riemannian
q manifold with condition number τ . Then for
any r 6 35 τ , Xr deformation retracts to M . Therefore thehomology of M is isomorphic to the homology of Xr .
In the above, the condition number τ encodes the tameness of M , quantified in local and global curvature conditions. Thus, if we can obtain a sufficiently dense sample
X = {x1 , . . . , xn } ⊂ Cf such that the projected sample
Y = Π(X) satisfies the conditions above, H1 (Π(Cf )) =H1 (Yr ) = H1 (DCr (Y )) for appropriate r and Yr =∪ni=1 Br (Y ). Since τ is however typically not computablein practice, we rely on the identification of noise-robust

0.5

0

0

0.5

Fig. 3: Identification of winding centers with persistent homology. Top:

Collision free samples (in blue) and simplicial approximation of Xr at
r = 0.05 where two holes are identified by two points in the first
persistence diagram shown to the right. Each of the two red points yields
a 1-cycle shown as a red closed curve in the bottom row. The 2-boundary
corresponding to each 1-cycle yield triangles in the interior of the 1-cycle.
The triangles in each 2-boundary with filtration larger than r yield green
approximations of obstacles. While 1-cycles may enclose more than a single
hole, there exists a unique triangle of maximal filtration index/filtration value
for each corresponding 2-boundary. For each 1-cycle this triangle is shadedin dark green in the bottom figures and its barycenter is chosen as a winding
center (one blue point for each figure in the bottom row).

homological information and appropriate radii r > 0 bymeans of the persistence diagram of H1 (DCr (X) for all
r > 0. Persistent homology generators with large persistence
have been proven to provide features that are robust to
noise, see [5]. In summary, to evaluate a candidate projection
Π : Cf → R2 in practice, we compute a sample X ⊂ Cf anddetermine the first persistence diagram of Y = Π(X). Givena threshold ε > 0, we deem the projection to be a topologicaltask projection if there exist points in the persistence diagramwith persistence larger than ε. For example, if we obtainsamples Y ⊂ R2 and a persistence diagram with a single
point with large persistence as in Fig. 2, we empirically deemthe projection a valid topological task projection.
Winding Centers: Given a topological task projection
Π : Cf → R2 , we will require representative winding points
w1 , . . . , wk ∈ R2 such that Π−1 (wi ) lies in the collision
space for each i ∈ {1, . . . , k}. To identify such points, wefirst attempt to identify a fixed filtration value r > 0 such that
only points of large persistence are alive in Yr ' DCr (Y ).
When H1 (Yr ) is n-dimensional, we compute a resulting
basis of 1-cycles c1 , . . . , cn such that [c1 ], . . . [cn ] form abasis of H1 (Xr ). This basis can be extracted from thestandard matrix reduction algorithm for persistent homology,
for example. Since H1 (XR ) = 0 for sufficiently large
R > 0, we can furthermore extract a collection of 2boundaries b1 , . . . , bn , such that ∂bi = ci [15]. Each 2boundary intuitively corresponds to the triangulated surface
interior to each 1-cycle (see Fig 3).

1

xstart

w
xend
−1
−1

1

Fig. 4: Left: Planning problem from xstart to xend with central winding
point w and disc obstacle in blue. Right: Expansive Tree (our WA-RRT*)in Winding Covering Space modulo 2. The z-coordinate corresponds to thewinding angle around w and is taken modulo 2, so that the ends of the spiralare ‘glued together’. The blue path in the covering space jumps from the
top layer of the covering space to the bottom layer and we can distinguishbetween the two red and green homotopy inequivalent trajectories since they
have differing terminal z-coordinate in the winding augmented covering
space. Path length in the winding augmented space is given by the path
length of the resulting projection onto Cf .
By considering the subset of triangles in bi that havefiltration values larger than r, we obtain a geometric representation of the holes in Yr as shown in green in Fig. 3. While
cycles can surround more than a single obstacle as in thecase of the bottom left cycle, each obstacle contains a uniquetriangle of maximal filtration index whose barycentric center
we use as a winding point for the obstacle. This triangle isshaded in dark green in the figure and the resulting winding
points are displayed in blue.
Planning with Winding Augmentation: Let us now
consider the topological motion planning problem. We first
discuss the 2-dimensional case before proposing our algorithm utilizing topological task projections. Given a 2Dconfiguration space Cf ⊂ R2 and a collection of points
w1 , . . . , wk , Bhattacharya et al. [2] consider representing
Cf by a fixed graph representation and to plan trajectories in a type of covering space over the graph in Cf
to plan homotopically distinct trajectories. For each pathγ : [0, 1] → Cf , a point γ(t) = (x(t), y(t)) on the graph isrepresented by an augmented topological coordinate: γ̂(t) =(x(t), y(t), W1 (t), . . . , Wk (t)), where Wl (t) = Wl (γ([0, 1]))
mod mk denotes the winding angle from γ(0) to γ(1)modulo a chosen integer mk > 2.
We observe now that this construction can be generalized
to higher dimensions by using topological task projections.
Furthermore, we observe here that we can utilize this representation not just on a fixed graph, but for any curve in Cf .
We hence propose to instead dynamically explore a winding
augmented space based on task-projections with RRT-based
incremental algorithms:
Projections and Winding Augmentation: Consider aconfiguration space Cf given by a topological space, anda number of k topological task projections Π1 , . . . , Πk :Cf → R2 and corresponding winding centers wi,1 , . . . , wi,ni
for each projection Πi and consider integers mi,j > 2 forPkeach winding center. Denote by M =i=1 ni the total
number of winding centers across all projections. Consider
an initial configuration x0 ∈ Cf and a continuous pathγ : [0, 1] → Cf , and define Wi,j (γ, t) to be the total winding angle W (Πi (γ([0, t]))) of the projected path segment
Πi (γ([0, t])) ⊂ R2 under the projection Πi : Cf → R2 modulo mi,j and with respect to the winding point wi,j ∈ R2 .
We define the augmented winding coordinates of γ(t) ∈ Cf
to be γ̂(t) = (γ(t), W1,1 (γ, t), . . . , Wk,nk (γ, t)) ∈ Cf × Z,
where Z = [0, m1,1 ) × . . . × [0, mk,nk ) ∈ RM . We observe
Lemma 3.4: Let α, β : [0, 1] → Cf be two continuous
paths such that α(0) = β(0) = x0 and α(1) = β(1), but
α̂(1) 6= β̂(1). Then α and β are homotopy inequivalent.
Proof: We have Wi,j (α, 1) 6= Wi,j (β, 1) for some i ∈{1, . . . , k} and j ∈ {1, . . . , ni }. But then, by Lemma 2.1, theprojections Πi (α) and Πi (β) are homotopy inequivalent. By
Lemma 3.1, this implies that α, β are themselves homotopy
inequivalent.
Note that since we consider the winding coordinates modulo
mi,j , our lifted coordinates can only distinguish between
paths winding up to mi,j − 1 times around each corresponding winding point wi,j – however, as a result, the space ofall possible lifting coordinates is bounded and of volume thatcan be controlled by empirically setting the size of mi,j .
We propose to use winding augmented coordinates toperform incremental motion planning, using RRT-based algorithms in dimension 2 and higher. Fig. 4 visualizes anexample application of WA-RRT* an RRT* based algorithm
in the trivial case with a 2-dimensional space Cf and asingle winding point and task projection equal to the identity
map, while Fig. 1 illustrates a 3 dimensional case with two
linear task projections (m1,1 = m2,1 = 2). Note that linear
projections to R2 are not sufficient in all cases, as in thecase of a torus obstacle in R3 and more general non-linear
task projection to Rn , n > 2 might in such cases be ofinterest. For the current work, we will however focus on 2D
projections.
IV. A LGORITHMIC D ETAILS AND I MPLEMENTATION
Given Cf , we denote our winding augmentation by A =(Π1 , . . . , Πk , (w1,1 , m1,1 ), . . . , , (wk,nk , mk,nk )), where Πi :Cf → R2 denotes the i-th topological task projection, wi,j
the j-th winding center for Πi and mi,j denotes the integer
modulo which we consider winding around wi,j . We consider
an initial state q0 ∈ Cf and a goal region Ω ⊂ Cf that wewould like to reach via a path from q0 .
Winding Augmented RRT (WA-RRT): Like the standard
RRT motion planner [13], WA-RRT consists of a main forloop shown in Alg. 1 which incrementally constructs a tree.
However, our tree (V, E) consists of vertices V ⊆ Cf × Z
and edges E ⊆ Cf × Z augmented by winding angles withrespect to all chosen winding centers wi,j in Z. Alg. 1
is similar to the RRT algorithm with goal set Ω and goal
bias PGoalBias , but at each iteration we sample both a state
qrand ∈ Cf , either from the goal set Ω or uniformly fromCf , and an appropriate winding coordinate tuple wrand ∈Z. To sample winding coordinates for a new point qrand ,RandomWindingLayer(q0 , qrand ) determines, for each i, j
the winding λi,j of the straight line segment from Πi (q0 )to Πi (qrand ) with respect to wi,j ∈ R2 and adds a random

Algorithm 1 WA-RRT(q0 , Ω, A = ({Πi }, {wi,j , mi,j }))1: E ← ∅; w0 ← 0 ∈ RM ; x0 ← (q0 , w0 ); V ← {x0 }2: for i = 1 . . . N do
3:
if with some probability PGoalBias then4:
qrand ← RandomElement(Ω)
5:
else
6:
qrand ← RandomState(Cf )7:
end if8:
wrand ← RandomWindingLayer(q0 , qrand , A)∈ Z
9:xrand ← (qrand , wrand )10:
Extend-WA-RRT((V, E), xrand , A)
11: end for

Algorithm 2 Extend-WA-RRT((V, E), xrand , A)
1: xnearest ← Nearest((V, E), xrand , A)
2: xnew ← Steer(xnearest , xrand , A)
3: if CollisionFree(xnearest , xnew ) then4:
V ← V ∪ {xnew }; E ← E ∪ {(xnearest , xnew )}5: end if

integer in [0, mi,j −1] (chosen with equal probability) to each
λi,j before returning the resulting tuple wrand of winding
coordinates modulo mi,j . The purpose of this procedure isto yield random samples xrand = (qrand , wrand ) covering
the winding augmented covering space. In the simplest caseof a single winding coordinate as in Fig. 4 and m1,1 = 2,
the procedure first selects a random sample from Cf andthen chooses a ‘height level’ in {0, 1} that is added modulo
2 to the winding from q0 . The Extend-WA-RRT (Alg. 2)
method is structurally identical to the RRT extension method,
however since we are passing tuples xrand = (qrand , wrand )we need to adapt the function calls to consider points inthe winding augmented covering space and in particular,
the Nearest function. The purpose of Nearest((V, E), xrand )is to return a nearest vertex xnearest ∈ V to xrand =(q rand , wrand ) in the winding augmented space. Since acomputation of geodesic distances in the full winding spaceis challenging between points whose winding difference
exceeds 0.5, we instead return the Cf -nearest neighbor only
within a chart (see also Atlas RRT [9]) defined by theset N (q rand , wrand , A) of vertices that have a winding
difference smaller than 0.5 (i.e. < ±π in polar coordinates) to any of the winding coordinates of q rand , whererand
N (q rand , wrand ) = {(q, w) ∈ V : maxi,j |wi,j − wi,j|<
0.5}.
The distance between xrand = (q rand , wrand ) and any
point x = (q, w) in N (q rand , wrand ) is then given by the
distance of q rand and q in Cf . The nearest neighbor querywhen restricted to N (q rand , wrand ) hence reduces to thestandard nearest neighbor problem in Cf . When no neighborexists within this set, the algorithm proceeds by resamplinga new xrand . For a returned nearest neighbor with winding
differences less than 0.5, Steer returns a new state by locallysteering towards xnew = (qnew , wnew ) as in the standard
RRT Steer method but also returns the winding coordinateswnew ∈ Z by adding the winding angle of the new pathsegment to the winding coordinates of xnearest in eachtopological task projection. CollisionFree(xnearest , xnew ),for xnew = (qnew , wnew ), xnearest = (qnearest , wnearest )1

r = 0.03
r = 0.05

0.5

r = 0.1
r = 0.2

0
100

500

1000

10000

1

r = 0.03
r = 0.05

0.5

r = 0.1
r = 0.2

0
100

500

1000

10000

Fig. 5: Top: We display the mean ratio of identified winding centers (y-axis) under the projection Π1 in C(r) corresponding to persistence intervals
with persistence of at least r/2 as the number of samples (x-axis) and radiusr is changed. Bottom: mean number of false positives under the projection
Π2 as defined by the ratio of times a persistence interval with persistence
larger than r/2 exists under the projection Π2 . Both ratios converge as thenumber of samples increases.

returns True if the path segment from qnearest to qnew iscollision free.
Winding Augmented RRT* (WA-RRT*): WA-RRT* isbased on RRT* and utilizes the WA-RRT main loop, but thencalls a modified Extend method that at the notational levelis identical Extend-RRT* of RRT* (please see Alg. 4 [11]).
Instead of a state in Cf , our xrand however lies in the winding
augmented covering space and we utilize the previously
discussed modifications of CollisionFree, Nearest and Steer.
RRT* further requires a method Near((V, E), xnew , |V |))returning all nearest neighbors within a ball of radius ε(|V |)depending on |V | [11]. We adapted Near to return allvertices of distance less than ε(|V |) and lying within the set
N (xnew ). Recall here ε(|V |) indicates a radius parameterthat asymptotically tends to zero as the number of vertices|V | is increased [11].
V. E XPERIMENTS
Topological Task Projections and Winding Centers
Sample and obstacle size dependence: We investigate
the reliability and parameter dependence of our topologicaltask projection and winding angle detection algorithm. Denote by C(r) = {x = (x1 , . . . , xd ) ∈ [−1, 1]d : x21 + x22 >r2 } ⊂ Rd , B(r) = {x = (x1 , . . . , xd ) ∈ [−1, 1]d : kxk 6
1, x21 + x22 > r2 } ⊂ Rd , the unit cube and ball witha cylindrical hole of radius r along the x1 , x2 axes. We
first considered d = 5, varied r ∈ {0.03, 0.05, 0.1, 0.2}
and sampled between n = 100 and n = 10000 samples
uniformly from C(r). We study the linear projections Π1 , Π2of C(r) onto (x1 , x2 ) by Π1 and onto (x2 , x3 ) by Π2 . Notethat Π1 reveals the cylindrical void, while Π2 can only resultin false positives as the hole is not revealed by Π2 . For 50
trials per parameter setting, Fig.5 displays the mean fractionof times a winding center with persistence larger than 2r wascorrectly identified in the projection of C(r) under Π1 as
the radius r and the sample size is varied. The bottom part
displays the analogous results for Π2 as an approximation to
the false positive rate. As the number of samples increases to10000 we obtain a success rate of one and false positive rateof zero. The mean time to compute the persistence diagram
and winding centers increases from 0.9ms for 100 samples
to 0.36s for 10000 samples.

1

πr = 0.2
r = 0.6

0.5

3

r = 0.8
r = 0.9

0
2

3

4

5

6
0

0

1

r = 0.2
r = 0.6

0.5

r = 0.8
r = 0.9

0
2

3

4

5

−3

6

Fig. 6: Success ratio of recovering a winding coordinate with persistence−3larger than 0.1 by means of random orthogonal linear task projection from
C(r) (top figure) and B(r) (bottom figure) for dimensions 2 to 6 (xaxis). While on average 26% of projections from C(0.9) still revealed thecylindrical hole, none of the 100 trials per setting revealed the hole for B(r)for dimensions larger than 4.

1

Choosing projection candidates: We now investigate
identifying a topological task projection by random linear
orthogonal projections from C(r) and B(r) to R2 . For 10000
samples from C(r), B(r) and in 100 trials per parametersetting respectively, Fig.6 displays the fraction of times apersistence interval with persistence larger than 0.1 was recovered as the dimension d is varied. Since C(r) degenerates
to a thin ring as r tends to one, we can recover the hole evenfor d = 6, while for B(r) many projections do not reveal
the cylindrical hole in higher dimensions (bottom figure),
and we were not able to find a projection with persistence
larger than 0.1 over 100 trials for dimensions larger than 4.
This exemplifies that the probability of determining topological task projections in a randomized manner is highly
configuration-space dependent. In practice, the use of domain
knowledge can be employed to determine the number andtype of projections. Natural projections along the ith andj th joint-angles of a robotic system as well as non-linear
maps to end-effector positions can serve as an alternative
approach to random search for finding an initial candidate
set of projections.

0Planning with multi-joint planar linkages
We evaluate our WA-RRT/RRT* algorithms using simulated planar linkages that are attached to the origin and with
2 6 d 6 10 degrees of freedom. The linkages consist of asegment of length 1.5 followed by a segment of length 1and between 0 and 8 segments of length 0.5. The arm isplaced in an environment with the obstacles displayed bydisks in Fig.7 and arm self-collisions as well as collisions
with the obstacles are disallowed. Joint angles are denoted byθ = (θ1 , . . . , θd ) and θj ∈ (−π, π). From an initial position
with θstart = ( π4 , 0, . . . , 0) to θgoal = (− π4 , 0, . . . , 0), thearm is required to move from a straightened position justleft of the top obstacle to a straightened position just rightof the top obstacle. Fig. 7 illustrates one such arm for d = 6
and 30.000 samples from Cf are visualized.
Homotopy Classes for Planar Linkages: Note that evenfor simple planar linkages, there exists a wealth of distinct
topological task projections and homotopy classes. Firstly,
if we consider a goal region defined by Ω = Π−1 (x),where Π : Cf → R2 denotes the map to the endeffector

0

0

3

−π
−π

0

π

1

Fig. 7: Planar 6DOF robot arm from our family of 2-10 DOF arms
and 30.000 sampled joint-configurations displayed by color coded endpoint positions of the resulting link placements (top left). Bottom left: first
persistence diagram (computed in 0.52s) for these samples reveals threeholes in the (θ1 , θ2 ) projection Π. The projected samples X correspond tovertices of DC0.1 (X) shown in the top right figure. The three large coloredpoints in the persistence diagram correspond to the colored winding centersin the top right. Bottom right: Projections of WA-RRT* trajectories in(θ1 , θ2 )-coordinates for a 2 DOF arm, illustrating different found homotopy
classes. Please see the supplementary video for animations.

position, winding coordinates around any winding point in
the interior of an obstacle determine homotopy inequivalent
trajectories in end-effector space that wind differently aroundthe obstacle, as illustrated in the right part of Fig.9. Similarly,
if self-collisions are allowed and without joint-limits (e.g.
corresponding to a 3D robot with joints stacked along zcoordinate), we can consider the i-th joint angle θi embedded
onto a unit circle centered at the origin and with winding
center given by the origin. The resulting map Πi : Cf →R2 is a topological task projection and trajectories plannedwith Πi allow us to control the number of times the i-thlink rotates around the i-th joint. For animations of someexamples of homotopy inequivalent motions planned with
our approach, please see the supplementary video.
For our main experimental evaluation, we focus on thecomplexity of our approach in various dimensions and choseto investigate the projection Π : Cf → R2 onto (θ1 , θ2 )(Fig.7). For this projection Π, each hole corresponds to oneof the three obstacles in Cf and a path moving above a
corresponding hole in (θ1 , θ2 ) corresponds to rotating thesecond link to the left as we pass the narrow passage createdby the obstacle while passing below moves the second linkto the right as the obstacle is passed. Winding around ahole clockwise corresponds to passing the narrow passage
near the obstacle with the second link angled to the rightand then returning, passing the passage with the second
link angled to the left. Fig.7 displays found examples fromdifferent homotopy classes in the 2DOF case, for higher
DOF, the remaining links fold to avoid the obstacles, but the
topological winding under Π only depends only on (θ1 , θ2 ).
Complexity of WA-RRT: For a maximal RRT stepsize of200, collision tests in steps of 0.1, a linear steering function

100

300

VI. C ONCLUSIONS

200
50
100

0

1

2

3

4

5

6

7

8

0

1

5

10

15

20

27

Fig. 8: Left: We investigated the mean time in seconds (y-axis) to findk homotopy inequivalent solution trajectories (x-axis) with WA-RRT for
mi,j = 2 (left) and the three found winding centers for planar linkages
with 2 to 10 DOF and averaged over 5 trials. While, for d = (2, 3, 4, 5),
all 8 classes were found in (0.36, 0.55, 0.48, 1.97) seconds on average, ford = 6, 8, 10 (black, blue, red) we obtain the performance shown in the left
plot for the chosen maximal step size of 200 and goal bias 0.01. Middle
figure: For mi,j = 3, the winding augmented search space increases involume and there are up to 33 = 27 homotopy classes. The figure displays
the mean time over 5 trials to find k homotopy inequivalent trajectories indimensions d = 2, 3, 4, 5, 6, 8, 10 with plots steepening with the dimension.
Right: A projection of a found 10DOF arm trajectory for mi,j = 3 whichwinds more than once around the rightmost winding point.

60
3
40
0
20
−3
0

0

100

200

−3

0

3

Fig. 9: Left: Typical incremental path length (y-axis) reduction of WA-

RRT* against runtime in seconds (x-axis) illustrating how paths in separate
homotopy classes can converge to distinct path lengths under WA-RRT*.
In this experiment, we considered a 4DOF linkage, ε(1) = 0.005 and ourthree winding centers resulting in 23 = 8 homotopy classes (mi,j = 2).
Right: Our approach can also be applied to obtain homotopy classes underforward-kinematics. A 4DOF planar linkage and a winding center w in R2
corresponding to the cross in the smallest obstacle and a goal set Ω =Π−1 (x) is used. Here, x denotes the blue point to the right of the red crossand Π denotes the end-effector forward kinematics map. Using Π, w andm1,1 = 2, we obtain two solution trajectories of the arm that are homotopy
inequivalent in R2 as visualized by end-effector trajectories in red and blue.

in joint space and with a goal bias of 0.01, we studied themean time to determine the k homotopy classes for the threewinding points for our robot arms for various dimensions
using our C++ implementation of WA-RRT/RRT* on an Intel
i7 laptop with 8GB RAM. Fig. 8 (left) displays the result for
m1,1 = m1,2 = m1,3 = 2 winding layers per winding center(23 = 8 classes). For mi,j = 3, we obtain at most 33 = 27
classes in the middle part of of Fig.8. In this case someclasses are more difficult to find due to the increased size of
the winding augmented space for mi,j = 3.
Convergence of WA-RRT*: WA-RRT* discovers trajectories at the same iteration as WA-RRT for identicalsamples, but additionally optimizes the path incrementally.
Fig.9 displays convergence of the length of various homotopy
classes for a 4DOF planar linkage and with an initial RRT*
neighborhood radius of ε(1) = 0.005.

We have introduced topological task projections (TTPs)
and a persistence based approach to detect winding centers
for high-dimensional homotopy aware motion planning. We
introduced two incremental algorithms, WA-RRT and WARRT* which enable us to determine homotopy inequivalent
trajectories and demonstrated our approach with 2-10 DOF
planar linkages resulting in a novel demonstration of topological motion planning in configuration spaces of dimension
higher than 4. In future work, we intend to investigate theprobabilistic completeness of the proposed algorithms.
R EFERENCES[1] U. Bauer and H. Edelsbrunner, “The Morse theory of Čech andDelaunay filtrations,” in Proc. of the Thirtieth Annual Symp. on Comp.
Geometry, ser. SOCG’14. New York, NY, USA: ACM, 2014.
[2] S. Bhattacharya, R. Ghrist, and V. Kumar, “Persistent homology forpath planning in uncertain environments,” Robotics, IEEE Trans.,
vol. 31, no. 3, pp. 578–590, June 2015.
[3] S. Bhattacharya, D. Lipsky, R. Ghrist, and V. Kumar, “Invariants forhomology classes with application to optimal search and planningproblem in robotics,” Annals of Math. and Artif. Intell. (AMAI), 2013.
[4] H. Edelsbrunner and J. Harer, “Persistent homology-a survey,” Contemporary mathematics, vol. 453, pp. 257–282, 2008.
[5] H. Edelsbrunner and J. L. Harer, Computational topology: an introduction. AMS Bookstore, 2010.
[6] D. Grigoriev and A. Slissenko, “Polytime algorithm for the shortest
path in a homotopy class amidst semi-algebraic obstacles in the plane,”
ISSAC ’98, 1998.
[7] A. Hatcher, Algebraic topology. Cambridge University Press, 2002.
[8] K. Hormann and A. Agathos, “The point in polygon problem for arbitrary polygons,” Computational Geometry. Theory and Applications,vol. 20, no. 3, pp. 131–144, Nov. 2001.
[9] L. G. Jaillet and P. Pleite, “Path planning with loop closure constraints
using an atlas-based rrt,” 2011.
[10] K. D. Jenkins, “The shortest path problem in the plane with obstacles: A graph modeling approach to producing finite search lists ofhomotopy classes,” DTIC Document, Tech. Rep., 1991.
[11] S. Karaman and E. Frazzoli, “Incremental sampling-based algorithms
for optimal motion planning,” in Proceedings of Robotics: Science andSystems, Zaragoza, Spain, June 2010.
[12] L. E. Kavraki, P. Svestka, J.-C. Latombe, and M. H. Overmars, “Probabilistic roadmaps for path planning in high-dimensional configuration
spaces,” IEEE Trans. on Robotics and Automation, vol. 12, no. 4, pp.
566–580, 1996.
[13] S. M. LaValle and S. A. Hutchinson, “Optimal motion planning for
multiple robots having independent goals,” Robotics and Automation,
IEEE Transactions on, vol. 14, no. 6, pp. 912–925, 1998.
[14] P. Niyogi, S. Smale, and S. Weinberger, “Finding the homology ofsubmanifolds with high confidence from random samples,” Discrete
and Comp. Geometry, vol. 39, no. 1-3, pp. 419–441, 2008.
[15] F. T. Pokorny and D. Kragic, “Data-driven topological motion planningwith persistent cohomology,” in Proceedings of Robotics: Science andSystems, Rome, Italy, July 2015.
[16] F. T. Pokorny, J. A. Stork, and D. Kragic, “Grasping objects withholes: A topological approach,” in Proc. of the IEEE International
Conference on Robotics and Automation (ICRA), 2013.
[17] M. L. S. Bhattacharya, V. Kumar, “Search-based path planning withhomotopy class constraints,” in Proc. of The Twenty-Fourth AAAI Conf.
on Artificial Intelligence, 11-15 July 2010.
[18] E. Schmitzberger, J. L. Bouchet, M. Dufaut, D. Wolf, and R. Husson,“Capture of homotopy classes with probabilistic road map,” in Intelligent Robots and Systems, 2002. IEEE/RSJ International Conferenceon, vol. 3, 2002, pp. 2317–2322 vol.3.
[19] I. Sucan and L. E. Kavraki, “A sampling-based tree planner for systems
with complex dynamics,” IEEE Trans. on Robotics, vol. 28, no. 1, pp.
116–131, 2012.
[20] D. Zarubin, V. Ivan, M. Toussaint, T. Komura, and S. Vijayakumar,“Hierarchical motion planning in topological representations,” in Proceedings of Robotics: Science and Systems, Sydney, Australia, July2012.