Andreas Orthey

Vladimir Ivan

Maximilien Naveau

Yiming Yang

Olivier Stasse

Sethu Vijayakumar

Homotopic particle motion planning for humanoid robotics

Exploiting structure is essential to an understanding of motion planning. Here, we exploit the topology of the environment to discover connected components. Inside a connected component, instead of planning one trajectory in configuration space, motion planning can be seen as optimizing a set of homotopically equivalent particle trajectories. In this paper, we will concentrate on the problem of motion planning for a humanoid robot. Our contributions are: i) finding the homotopy classes of a single footstep trajectory in an environment, ii) finding a single footstep trajectory in a single homotopy class formulated as a convex optimization problem, and iii) finding a feasible upper body trajectory given a footstep trajectory, formulated as a set of convex optimization problems. This view provides us with important insights into the difficulty of motion planning, and -under some assumptions -allows us to provide the number of local minima of a given motion planning problem. We demonstrate our approach on a real humanoid platform with 36-dof in a highly restricted environment.

I. INTRODUCTION

Motion is one of the fundamental building blocks of intelligent behavior, and a deep understanding would enable robots to achieve autonomous tasks in eldery care, deep space exploration, and nuclear waste removal.

We study here the motion planning problem A = {R, C, q I , q G , E} [START_REF] Lavalle | Planning Algorithms[END_REF] with R be a robotic system, C the configuration space of R, q I ∈ C the initial configuration, q G ∈ C the goal configuration and E the environment.

The motion planning problem was shown to be NP-hard [START_REF] Reif | Complexity of the mover's problem and generalizations[END_REF], and for humanoid robots, computational time can become several hours in a narrow environment [START_REF] Orthey | Motion Planning and Irreducible Trajectories[END_REF], [START_REF] Khoury | Optimal Motion Planning for Humanoid Robots[END_REF], [START_REF] Escande | Planning contact points for humanoid robots[END_REF]. We argue that the main problem is the reliance on random sampling techniques [START_REF] Lavalle | Rapidly-Exploring Random Trees: Progress and Prospects[END_REF]: if the subset of feasible configuration gets arbitrarily small, the convergence rate of random sampling gets arbitrarily high [START_REF] Kavraki | Randomized query processing in robot path planning[END_REF]. While random sampling is excellent for solving the problem in general, we argue here that to design truly efficient algorithms, we need to study, understand and exploit the underlying structure of the motion planning problem.

Here, we concentrate on investigating and exploiting the environment structure by extracting homotopy classes. A homotopy class is a set of functions, which can be continuously {aorthey,mnaveau,ostasse} at laas.fr 2 are with the University of Edinburgh, Edinburgh, UK {vivan,yiming.yang,sethu.vijayakumar} at ed.ac.uk

f 3 f 2 f 1 x I x G
Fig. 1. Left: Two functions are in the same homotopy class, if they can be continuously deformed into each other, while fixing their end points. f 1 would be not homotopically equivalent to f 2 , while f 3 would be. Right: In 3d, we conduct homotopic motion planning for a set of particles, particles with homotopically equivalent space curves.

deformed into each other, as depicted in Fig. 1. For each homotopy class, we consider the trajectories of a set of particles {τ k } η k=0 on the robot body moving through R 3 on space curves of the form τ k : [0, 1] → R 3 . We assume here that all particle trajectories are homotopically equivalent. Motion planning can then be conducted in the environment by first finding a single particle trajectory, and then finding all particles on the robot body by restricting them to belong to the same homotopy class as the single particle trajectory.

Towards this goal, we decompose the open space of the environment into smaller volumes and analyze their covering to compute homotopy classes of robot particles moving through open space. We argue here that performing motion planning locally in one homotopy class ensures continuity, which is a requirement for optimization based planners. This decomposition of motion planning is called homotopic motion planning [START_REF] Bhattacharya | Topological constraints in search-based robot path planning[END_REF].

Our contributions are a single footstep trajectory as a convex optimization problem, and Section IV formulates the upper body optimization as a set of convex optimization problems under convex inequality constraints from the environment. The reader is refered to consult Fig. 2 for a technical overview.

II. ENVIRONMENT HOMOTOPY DECOMPOSITION

In this section, we describe how we compute the free space of a given environment, and its connectivity. We start by reasoning about surfaces on which a foot contact is possible, which we call walkable surfaces. For each walkable surface, we compute its free space stack, a set of boxes on top of the surface in which the swept volume of the robot R necessarily has to lie. We further represent the connectivity of surfaces by a graph structure. This graph structure then enables us identify homotopy classes.

We will consider a decomposition of the environment into a set of objects as

E = O 1 • • • O α (1)
with O i being a bounded convex polytope

O i = {x ∈ R 3 |a (i)T j x ≤ b (i) j , a (i) j 2 = 1, j ∈ [1, α i ]} (2)
We make here the assumption that every object O i is a convex polytope. If an object is not a convex polytope, we decompose it into convex subobjects [START_REF] Lien | Approximate convex decomposition of polyhedra and its applications[END_REF], such that we can operate w.l.o.g. on convex polytopes.

For every object O i , we define the p-th surface element as

S p i = {x ∈ R 3 |a (i)T p x = b (i) p , a (i)T j x ≤ b (i) j , j = 1, • • • , p -1, p + 1, • • • , α i } (3)
with a (i) p the surface normal, and b

(i)
p the distance to the origin. Definition 1 (Walkable Surface). A surface element S p i is called walkable, if 1) the slope of S p i is smaller than the maximum slope R θ the robot can stand on

a (i) p -v g ≤ (2 -2 cos(R θ )) (4)
with v g = (0, 0, 1) T 2) the foot of radius R FR is fully contained inside S p i , meaning the following convex problem is feasible (based on the maximum inscribed circle problem [START_REF] Boyd | Convex Optimization[END_REF])

maximize x∈R n ,r∈R R subject to a T i x + Ra T i a i ≤ b i , i = {1, • • • , p -1, p + 1, • • • , α i } a T p x = b p R ≥ R FR ( 5
)
whereby r is the radius of the circle, x the center, a i is the orthogonal projection onto the hyperplane of a p , i.e. We now add a notion of connectivity:

a i = a i -(a T i a p )a p . Visualized in Fig. 3. R x B p i (0, δ) B p i (δ, 2δ) B p i (2δ, 3δ) S p i
Definition 2 (Connectivity). Two walkable surfaces S i , S j are called connected, iff d(S i , S j ) < R Step , with R Step being the maximum step size of the robot R This connectivity gives rise to a graph structure G E , which contains walkable surfaces a nodes, and an edge between two connected surfaces.

To approximate the free space, we stack cuboids on top of each walkable surface. A cuboid of height δ and with distance ∆ L to S p i is defined as B p i (∆ L , ∆ L + δ) See Fig. 3. The stack of cuboids on S p i will be denoted by

B p i = {B p i,k } β k=1 B p i,k = B p i (kδ, (k + 1)δ)} (6)
β is choosen such that β >

RH U δ
with R HU the maximum height of the robot. For each B p i,k , we apply a clipping algorithm [START_REF] Vatti | A Generic Solution to Polygon Clipping[END_REF] to decompose it into smaller convex cuboids.

Additionally we define the intersection element between two stacks of cuboids B i , B j as

I ij = B i ∩ B j . S 0 S 1 • • • S R-1 S R τ (t 0 , t 1 ) τ (t 1 , t 2 ) • • • τ (t R-1 , t R ) τ (t R , t R+1 )
Fig. 4. A function τ has support on a walkable surface S i at the time

[t i , t i+1 ].
Now, given two configurations q I , q G ∈ C of the robot, we compute the right foot position as x I = T (q I ), x G = T (q G ) by using a forward kinematics operator T . Given x I , x G , we define S I = argmin To summarize, in this section we preprocessed the environment E, to decompose it into More formally, we will consider the functional space of space curves as

• A set of N w walkable surfaces S 1 , • • • , S Nw • A set of N w stack of cuboids B 1 , • • • , B Nw • A set of N i connector elements I 1 , • • • , I Ni • The environment graph G E ,
Ω(x I , x G , H) = C 1 ([0, 1], R 3 ) (7)
under the constraints that for all τ ∈ Ω(x I , x G , H) we have τ (0) = x I , τ (1) = x G , and that a segment τ (t) for t ∈ [t i , t i+1 ] has support on a walkable surface S i , as depicted in Fig. 4. In between support, we assume that the function is not supported, i.e. the foot can freely move through space, under the restriction that the non-support movement is smaller than the maximum stepsize.

We represent the functional space Ω(x I , x G , H) by a linear combination of basis functions [START_REF] Boyd | Convex Optimization[END_REF], based on the Stone-Weierstrass theorem. The Stone-Weierstrass theorem [START_REF] Rudin | Functional analysis[END_REF] guarantees that every continuous function from C([0, 1], R 3 ) can be approximated arbitrarily close by a polynomial function, i.e. we have that for all t ∈ [0, 1] we have

Theorem 1 (Stone-Weierstrass). Let τ ∈ C 1 ([0, 1], R 3 ). For every > 0 there exists a polynomial p(t) = ∞ i=0 w i t i such G E
τ (t) - ∞ i=0 w i t i < (8)
Conceptually, we represent a continuous function by a linear combination of an infinite set of basis functions. We will make the assumption that higher-order terms are negligible such that we choose a finite K 0, and use

p(t) = K-1 i=0 w i t i .
We will denote

F = {x 0 , • • • , x K-1 } ∈ R K×D ,
c(τ ) (9)
subject to

τ (0) = x I , τ (1) = x G (10) ∀S i ∈ {S 0 , • • • , S R }, t ∈ [t i , t i+1 ] : A i τ (t) ≤ b i ( 11
)
∀t ∈ [0, 1] :

τ (t) -τ (t + ∆t) ≤ R Step ( 12
)
∀t ∈ [0, 1] :

τ (t) -W T F (t) ≤ (13)
whereby we have the following parameters

(τ ) = 1 t=0 τ (t) -τ (t + ∆t) 2 2 dt
The given convex problem describes the set of all trajectories restricted to one homotopy class. A valid footstep at t can be modeled as a convex inequality constraint:

∀t ∈ [0, 1] : A i τ (t) ≤ b i -R F diag(A T i A i ) (14)
whereby A i = {a 0 , • • • , a Mi } contains the normals of the polytope associated to the walkable surface S i , A i = {a 0 , • • • , a Mi } with a j = a j -(a T j a p )a p , and R F being the radius of the foot. Compare to [START_REF] Escande | Planning contact points for humanoid robots[END_REF].

Fig. 5 shows the result of our convex optimization problem for an environment with 2 homotopy classes. A more complex version with 4 homotopy classes is shown in Fig. 6. The final planning results are depicted in Table I, all generated by using the splitting conic solver (SCS) [START_REF] O'donoghue | Operator splitting for conic optimization via homogeneous self-dual embedding[END_REF] inside cvxpy [START_REF] Diamond | CVXPY: A Python-embedded modeling language for convex optimization[END_REF].

IV. UPPER BODY OPTIMIZATION

We have showed so far how to optimize a single footstep trajectory τ 0 ∈ Ω(x I , x G , H) in one homotopy class of the environment via a convex optimization problem. Now we assume that τ 0 is fixed. Our goal is to find a set of particle trajectories in the same homotopy class as τ 0 , belonging to the swept volume of R, such that those particles are feasible in E. To put if differently, instead of searching for a single configuration space trajectory, we are searching for a set of mutually constrained particle trajectories in the environment. This section describes one possible way to constrain those particle trajectories to lie in the same homotopy class as τ 0 . Please consult also Fig. 2 for an overview.

Each particle of the swept volume moves along a space curve in R 3 . Let Υ = {{τ l k , τ r k }} η k=0 be the set of space curves of 2(η + 1) particles, with τ {l,r} k ∈ C 1 ([0, 1], R 3 ), and τ l k represents the left outer hull of the swept volume of the robot at height kδ, and τ r k the right hull. If we take a cross-section of the swept volume, then τ k is represented by a point at height kδ, as depicted by the red dots in Fig. 7. To achieve this, we apply three constraints on the functional space Υ 1) τ k (t) ∈ P (t), the plane orthogonal to the foot trajectory τ 0 at instance t (cmp. Fig. 8)

P (t) = {x ∈ R 3 |a T P (t)x = b P (t)} (15)
with a P (t) = τ 0 (t) τ0(t) and b P (t) = a T (t)τ 0 (t). 2) τ k (t) has to be feasible in E, i.e. if τ 0 is supported on S p i at t, then

τ k (t) ∈ B p i (kδ, (k + 1)δ) (16)
3) At instance t, all particles resemble a cross-section X k of the robot

τ k ∈ X k ( 17
)
The first two constraints are a linear equality and a convex inequality, respectively. The third constraint however is nonconvex. To obtain X k , we sample the configuration space C and compute cross-sections. A cross-section of a configuration is defined as its swept volume on the plane in movement direction, i.e. at t, the volume of q ∈ C is projected onto P (t), as depicted in Fig. 7. As a simplification, we use only irreducible configurations of the robot [START_REF] Orthey | Motion Planning and Irreducible Trajectories[END_REF]. An irreducible configuration is a configuration which has a minimal swept volume. Basically, we sample {q 1 , • • • , q σ } ∈ C, apply a crosssection operator φ : C → X × X and compute the crosssections X = {{x l,1 , x r,1 }, • • • , {x l,N , x r,N }}. x l stands for the left points of the swept volume, and x r for the right points and we note that x r = A l x l .

We now have to find a feasible cross-section for each plane. As a simplification, we consider the cross-sections only at intersections I r of the environment, since those intersections represent the narrow passages. We note that we have only convex boxes in-between intersections, and so we assume that we can linearly interpolate two intersection points.

Our algorithm proceeds in the following manner: we compute the feasibility of N cross-sections by solving N convex optimization problems

Θ i 1 , • • • , Θ i N for all intersections i ∈ [1, V ] in I 1 , • • • , I V . A feasible path is then a sequence λ 1 , • • • , λ V of feasible cross-sections Θ 1 λ1 , • • • , Θ V λ V with Θ i λj < ∞.
We can represent this as a solution matrix

Λ =    Θ 1 1 • • • Θ V 1 . . . . . . . . . Θ 1 N • • • Θ V N    (18)
whereby we have that Θ i j solves the problem of feasibility of a cross-section X j on an intersection element I i . Let t i be such that τ (t i ) ∈ I i . Then Θ i j becomes

Θ i j = minimize {τ0,••• ,τη}∈Υ c(τ 0 , • • • , τ η ) (19)
subject to ∀k ∈ [0, η] : Left: The cross-section space for a humanoid robot. Each line represents a certain height above a walkable surface. The cross-section of the robot intersects each height at two points, which we call x L and x R for left and right, respectively. We assume that every cross-section gives rise to only two points, i.e. we ignore configurations, where this is not the case. Overlaid (white line segments) are the constraints by the wall environment, which impose a convex box inequality on the cross-sections.Right: the final experiment, with the humanoid robot HRP-2 walking through a narrow environment.

τ k (t i ), A L τ k (t i ) ∈ P (t r ) (20) τ k (t i ), A L τ k (t i ) ∈ I i (kδ, (k + 1)δ) (21) τ k (t i ), A L τ k (t i ) = X j (22)
Fig. 8 represents the complete algorithmic output: from an environment, we compute walkable surfaces, we compute a footstep trajectory, we compute planes orthogonal to the footstep, we solve a set of convex optimization problems at each intersection, and we compute a final set of particle trajectories in the environment.

Finally, our main point here is that we have investigated the structure of the planning problem. Given our assumptions, we can compute the number of local minima of our planning problem as

L = R i=1 N Vi (23)
with R the number of homotopy classes, N the number of cross-sections, and V i the number of intersections inside the i-th homotopy class. For the wall environment in Fig. 8 we have N = 144,V = 2,R = 1 and so L = 20736.

While our work is preliminary and non-complete, we want to stress the fact that knowing the number of local minima is important for understanding the inherent complexity of motion planning.

V. EXPERIMENTS

We implemented the algorithms in python, and used cvxpy [START_REF] Diamond | CVXPY: A Python-embedded modeling language for convex optimization[END_REF] to compute solutions to the local minima. The source code to reproduced the experiments is available at https://github.com/orthez/mpp-path-planner For experimental verification, we have chosen the wall environment depicted in Fig. 8, which contains 145 objects.

The following parameters were used: β = 40, δ = 0.05, such that βδ = 2.0 > R H with R H = 1.539m the maximum height of HRP-2 [START_REF] Kaneko | Humanoid robot HRP-2[END_REF]. For Problem [START_REF] Lien | Approximate convex decomposition of polyhedra and its applications[END_REF], we used = 0.02, D = 1000, K = 2000, and we used a minimum number of D i = 15 samples for each walkable surface S i .

The environment decomposition took 3m20s, and our algorithm computed N = 144 cross-section configurations. We employ a greedy version of our algorithm, which computes Fig. 9. Comparison of running time on 10 instances for RRT (red) (adapted from [START_REF] Orthey | Motion Planning and Irreducible Trajectories[END_REF]) and for our particle planning (PP) algorithm (blue). For PP, the time depends on the discretization all local minima for the first intersection, and then checks if the next intersection can be solved by the solution to the first intersection. For each intersection, we computed all minima and found out that 11/144 have been feasible (7.64%).

The computation took 12m15s (averaged over 10 runs). All together we have a total computation time of 15m35s. We compared the results of all runs with the results of a rapidly exploring random tree [START_REF] Lavalle | Rapidly-Exploring Random Trees: Progress and Prospects[END_REF], operating on the irreducible configuration space [START_REF] Orthey | Motion Planning and Irreducible Trajectories[END_REF]. The results in Fig. 9 show that we have a lower variance while performing better at the given sampling resolution.

To move the robot in the real world, we add small footsteps along the path, one every 0.1m. Given footsteps and trajectory of the upper body, we use a dynamical solver to compute zero-moment point trajectories for the robot. We have used those results to verify the motion in the dynamical simulator OpenHRP [START_REF] Kanehiro | OpenHRP: Open architecture humanoid robotics platform[END_REF], and executed it on the humanoid robot HRP-2 [START_REF] Kaneko | Humanoid robot HRP-2[END_REF]. The video can be found here http://homepages.laas.fr/aorthey/videos/wall-homotopy.mp4 VI. RELATED WORK Bhattacharya et al. [START_REF] Bhattacharya | Topological constraints in search-based robot path planning[END_REF] compute homotopy classes in the environment, and use them as a constraint for graph-based search. Our work is complementary in the sense that we investigate how to formulate planning in one homotopy class as a set of convex optimal problems, while their work investigates how to compute the homotopy classes in the first place.

The technique presented in [START_REF] Brock | Decomposition-based Motion Planning: A Framework for Real-time Motion Planning in High-dimensional Configuration Spaces[END_REF] estimates a single homotopy class by growing random spheres. Our approach tries to be more systematic in that we reason about contact surfaces, and restrict the free space by the robots geometry. Also, we consider planning inside a homotopy class not as a potential field controller, but as a global optimization procedure.

The work by [START_REF] Perrin | Weakly collisionfree paths for continuous humanoid footstep planning[END_REF] consider sweeping a spherical object to find weakly collision free footstep positions. Our work is similar for footsteps but precomputes homotopy classes to identify high-level minima.

[20] identifies narrow passage in the environment, and computes important waypoint configurations inside those narrow passages. This idea inspired our computation of connector elements, elements which connect two contact surfaces.

Ivan et al. [START_REF] Ivan | Topology-based representations for motion planning and generalization in dynamic environments with interactions[END_REF] introduce different topological representations to easier solve certain subproblems of motion planning. Our work is complementary, in that we would be able to analyze which representation to use given a certain problem.

The authors of [START_REF] Deits | Footstep Planning on Uneven Terrain with Mixed-Integer Convex Optimization[END_REF] discover convex regions of footsteps in an environment, and employ mixed-integer programming to find a solution. Our work explores how adding more structure in form of connectivity can help to discover the homotopy classes, and formulate the resulting problem as a set of convex optimization problems.

Farber [START_REF] Farber | Topological complexity of motion planning[END_REF] introduced the topological complexity of a configuration space. Our work can be seen as a practical means of identifying the covering of the workspace volume and thereby its topological complexity. Our optimization algorithms are then one proposal to find paths inside of a given covering.

This work is fundamentally based on the work by [START_REF] Orthey | Motion Planning and Irreducible Trajectories[END_REF], who introduced irreducible configuration for humanoid robots. Our work is complementary in that we are restricting our motions to the space of irreducible configuration while exploiting environment structure.

VII. CONCLUSION

We decomposed the general motion planning problem into a set of homotopic motion planning problems, with the goal of developing more efficient algorithms for the homotopic motion planning problem.

We presented three results: I) how to identify homotopy classes in an environment, based on walkable surfaces, surfaces on which a robot can make a foot contact. II) how to find a single contact trajectory inside a given homotopy class, formulated as a single convex optimization problem, and III) how to find a feasible upper body trajectory by solving a set of convex optimization problems.

Regarding future work, we currently work on incorporating our particle planning into a local motion planning algorithm to produce a dynamical feasible motion. We also would like to investigate when a surface is walkable, depending on physical properties like density, geometry, maximum pressure, and slippage. Finally, we would like to investigate the complexity properties of homotopic particle motion planning.

Fig. 3 .

 3 Fig. 3. Left: a polytope (light gray), a surface element (dark gray) and an inscribed circle with radius R and center x. Right: a set of cuboids B p i on top of one surface element S p i (dark gray)

  S k ∈S d(x I , S k ) to be the initial surface, and S G = argmin S k ∈S d(x G , S k ) to be the goal surface. Given S I , S G , we compute H = {H 1 , • • • , H R }, the set of R simple connected paths on the environment graph G E . We call H ∈ H a homotopy, and we will write the connection of walkable surfaces as H : S 0 → • • • → S R H . As a note, the complexity of finding all connected paths in a graph with V vertices is O(|V |!) [12].

  describing the connectivity between walkable surfaces • A set of homotopies H for given x I , x G III. CONVEX OPTIMIZATION OF FOOTPATH HOMOTOPIES Given H : S 0 → • • • → S R H , our goal is to find a sliding footstep trajectory supported on the surfaces S 0 , • • • , S R H .

Fig. 5 .

 5 Fig. 5. Two homotopy classes of footsteps, and two solutions, obtained by solving one convex optimization program in each homotopy class. Also we show the environment graph G E for this particular example, which represents connectivity between walkable surfaces.

Fig. 6 .

 6 Fig. 6. Four homotopy classes in the environment: Our algorithm finds the homotopy classes via graph search, and then solves one convex optimization problem in each class to find the global optimal solution trajectory.

Fig. 7 .

 7 Fig. 7.

Fig. 8 .

 8 Fig. 8. From Left to Right, Top to Bottom: 1) all polytopes in the wall environment 2) the extracted walkable surfaces from our algorithm, 3) the footstep trajectory in the single homotopy class, computed by solving the convex optimization problem (9), 4) the vertical planes along the footstep trajectory, 5) the intersection of boxes on each walkable surface to create the connection elements 6) the final plan of workspace trajectories, all homotopically equivalent and representing a configuration space trajectory.

  Fig.2. The conceptual overview about this paper. Top: in section II we decompose the environment E into walkable surfaces S, intersections I and intersections I. From the start contact x I , the goal contact x G and the connectivity graph G E we compute the homotopy classes H on S. Middle: Given the homotopy classes H, the start contact x I and the goal contact x G , we compute a sliding footstep trajectory τ 0 , supported on H. Bottom: We compute upper-body particle trajectories Υ from a given footstep trajectory τ

		E	S	B	I
	Sec. II
		x I , x G	H	G E
	Sec. III	H	τ 0	x I , x G
		τ 0	P	Υ	X
	Sec. IV
			I		C
					• Identification of the homotopy classes in a given environ-
					ment for a sliding footstep and the approximation of the
					free space
					• Formulation of the problem of finding a sliding contact
					trajectory in one homotopy class as a convex optimization
					problem
					• Formulation of the problem of finding a set of particle
					trajectories on the robot body, which are constrainted by
					the contact trajectory, as a set of convex optimization
					problems
					Sec. II describes how we we decompose an environment
					into walkable surfaces, homotopy classes and the free space
					inside of one homotopy class. Sec. III formulates optimizing

0 , planes P , cuboids B, and from cross-sections X generated by sampling robot configurations C.

TABLE I RESULTS

 I FOR PLANNING PATHS IN THE TWO SCENARIOS SHOWN IN FIG. 5 AND FIG. 6. R IS THE NUMBER OF HOMOTOPIES, T W IS THE TIME TO EXTRACT WALKABLE SURFACES FROM THE ENVIRONMENT, T G THE TIME TO COMPUTE THE CONNECTIVITY BETWEEN SURFACES, T P THE PLANNING TIME OF SOLVING R CONVEX OPTIMIZATION PROBLEMS, AND T IS THE ACCUMULATED TIME OF ALL STAGES TOGETHER (AVERAGED OVER 10 RUNS, ROUNDED). Step the maximum step size of the robot • > 0 approximation constant to circumvent numerical instabilities • K number of basis functions • c(τ ) is a convex objective function on τ , for example the shortest path as c

	Environment	R Homotopies	T W (s) T G (s) T P (s) T (s)
	Stepping 1 (Fig. 5)	2	0.27	1.92	6.53	8.73
	Stepping 2 (Fig. 6)	4	0.26	1.68	20.34 22.28

• R

* This research has received funding from the European Union Seventh Framework Programme (FP7/2007 -2013) under grant agreement n o 611909, KoroiBot.