Piecewise-Linear Motion Planningamidst Static, Moving, or Morphing Obstacles

Bachir El Khadir1, Jean Bernard Lasserre2, Vikas Sindhwani3

Abstract— We propose a novel method for planning shortestlength piecewise-linear motions through complex environmentspunctured with static, moving, or even morphing obstacles.Using a moment optimization approach, we formulate a hier-archy of semidefinite programs that yield increasingly refinedlower bounds converging monotonically to the optimal pathlength. For computational tractability, our global momentoptimization approach motivates an iterative motion plannerthat outperforms competing sampling-based and nonlinearoptimization baselines. Our method natively handles continuoustime constraints without any need for time discretization, andhas the potential to scale better with dimensions compared topopular sampling-based methods.

Index Terms— Motion and Path Planning, Semidefinite Pro-gramming, Convex Optimizaton

I. INTRODUCTION AND PROBLEM STATEMENT

How should robots – viewed as complex systems of artic-ulated rigid bodies – move from a start to a goal configu-ration in an environment cluttered with static and dynamicobstacles? Even without considering dynamic feasibility ofa desired motion, mechanical and sensor limitations, uncer-tainty and feedback, the purely geometric motion planningproblem is known to be computationally hard [26] in its fullgenerality.A. The Optimal Motion Planning ProblemWe follow a similar notation to that of [12] to describe theOptimal Motion Planning (OMP) problem. Let X = Rn bethe configuration space, where n ∈ N. We are interestedin finding the shortest path xxx : [0, T ] → X (where T isa positive constant) that starts at a configuration xxx(0) =xxx0 ∈ X , ends at a configuration xxx(T ) = xxxT ∈ X , andavoids a time-varying obstacle region Xobs(t) ⊆ X at alltimes t ∈ [0, T ]. Here, we assume that the obstacle-freespace Xfree(t) := X \ Xobs(t) is a closed basic semialge-braic set, i.e., that there exists a (multivariate, scalar-valued)polynomial function gk ∈ R[t,xxx] in variables t and xxx suchthat

Xfree(t) := {xxx ∈ Rn | g1(t,xxx) ≥ 0, . . . , gk(t,xxx) ≥ 0}.

Our choice for working with polynomial functions todescribe obstacles stems from two reasons. On the one hand,

1 IBM Watson Research Center, Yorktown Heights, NY 10598, USAbachir@ibm.com

2 Laboratoire d’Analyse et d’Architecture des Systemes (LAAS), Instituteof Mathematics, University of Toulouse, France lasserre@laas.frJ.B. Lasserre was partly funded by the AI Interdisciplinary Institute ANITIthrough the French “Investing for the Future PI3A” program under the Grantagreement ANR-19-PI3A-0004

3 Robotics at Google, New York City, NY 10011, USAsindhwani@google.com

t = 010 t = 2

10 t = 410 t = 6

10 t = 810 t = 10

10

t

Fig. 1: Example of an time-varying obstacle described by thepolynomial inequality g(t,xxx) < 0, with g(t,xxx) := (1 − t)(x21 +x22+x

23+1)+ t

320(320x21x

33+36x22x

33−5(4x21+9x22+4x23−4)3).

The shape of the obstacle changes from a sphere to a heart as timet goes from 0 to 1.

polynomial functions can uniformly approximate any con-tinuous function over compact sets, and hence are powerfulenough for modeling purposes. See figure 1 for an illustrationof an obstacle morphing into a complex shape over time, asdescribed by a degree-7 polynomial, and see [5], [7], [8], [24]for more examples on the use of polynomial functions for thepurposes of modeling 3D geometry. On the other hand, aswe will see in section II, the discovery of recent connectionsbetween algebraic geometry and semidefinite programminghas resulted in powerful tools that are designed specificallyfor tackling optimization problems which are described bypolynomial data.

More formally, the OMP problem described by data

D = (xxx0,xxxT , {g1, . . . , gm}), (1)

where xxx0,xxxT ∈ Rn and g1, . . . , gm ∈ R[t,xxx] is the followingminimization problem,

minxxx:[0,T ]→Rn

∫ T

0

‖xxx(t)‖ dt

s.t. xxx(0) = xxx0 , xxx(T ) = xxxT ,

gk(t,xxx(t)) ≥ 0 ∀t ∈ [0, T ], ∀k ∈ [m],

OMP(D)

where ‖ · ‖ denotes the `2 norm, and [s] denotes the set{1, . . . , s}. The objective term

∫ T0‖xxx(t)‖ dt is the length of

the path xxx(t). A path that satisfies the constraints of OMP(D)is said to be feasible. A path that is feasible and has minimumlength is said to be optimal.B. Background on Motion PlanningWe first set the stage for describing and motivating ourapproach in relation to the vast prior literature [18], [9],[20] on motion planning. Most obviously, OMP(D) can betranscribed into a nonlinear optimization problem by usinga parametric representation of the path together with time-discretization to construct a finite-dimensional optimizationproblem [28], [11], [30]. Because of its non-convexity, theeffectiveness of such an approach depends on having a goodinitial guess and in general no guarantees can be providedthat the process will not return a sub-optimal stationary point.

1

arX

iv:2

010.

0816

7v1

[cs

.RO

] 1

6 O

ct 2

020

Closely related is the body of work on virtual potentialfields [13] where a vector field is designed to pull the robottowards the goal and push it away from obstacles. Unless arestricted class of navigation functions [27] generates the gra-dient flow, these methods are also susceptible to local min-ima. By contrast, sampling-based motion planners [19], [12],[10], pervasively used in robotics, are attractive since theycan at-least offer a guarantee of probabilistic completeness,that is, as the planning time goes to infinity, the probability offinding a solution tends to one. Sampling-based planners relyon a collision checking primitive to construct a data structure,e.g., tree or a graph, that stores a sampling of obstacle-avoiding feasible motions of the robot. In their most commoninstantiations, sampling methods return feasible paths, notnecessarily optimal [12], almost alway also requiring postprocessing to reduce jerkiness.

Even in the time-independent, purely geometric path plan-ning setting, the general problem of finding a feasible path,or correctly reporting that such a path does not exist, hasbeen shown by Reif [26] to be PSPACE-hard. If Xfree issemialgebraic, then its cylindrical cell decomposition [29]allows for a doubly-exponential (in the configuration spacedimension n) solution to the motion planning problem.Canny’s Roadmap [6] gives an improved single-exponentialsolution based on the notion of a roadmap, a network of one-dimensional curves preserving the connectivity of the freespace that can be reached from any configuration. However,despite their completeness guarantee, these techniques areconsidered computationally impractical for all but simple orlow-degree-of-freedom problems.

It should be no surprise that dynamic environments whereobstacles can appear, disappear, move or morph only magnifythe hardness of general motion planning [25], even when theobstacle motion is pre-specifed as a function of time. Manyplanners can be adapted to this setting by simply defining theproblem in a time-augmented state space. Then, the primarycomplication stems from the requirement that time mustalways increase along a path. An alternative is to decouplespace and time planning by first finding a collision-free pathin the absence of moving obstacles, and then determining atime scaling function. In any case, planners for time-varyingproblems may also become prone to failure simply due todiscretization of time.C. Statement of ContributionsIn this paper, we focus on solving the optimal motionplanning problem for piecewise-linear motions. At the out-set, it should be noted that even with this restriction, theproblem remains PSPACE-hard [31]. With this setting, ourcontributions are as follows. First, we introduce a newarsenal of algorithmic and complexity-characterization toolsfrom polynomial optimization and semidefinite programming(SDP) to the motion planning literature. Specifically, forany optimal motion problem OMP(D) described by data Das in (1), and for any number of pieces s, we present ahierarchy of semidefinite programs SDP(r, s;D) indexed bya scalar r. Every level of this hierarchy provides a lowerbound ρ(r, s;D) on the minimum length ρ(s;D) attained by

piecewise-linear paths that are feasible to OMP(D) and haves pieces. Importantly, we provide the asymptotic guaranteethat ρ(r, s;D)→ ρ(s;D) as r →∞. This notion of asymp-totic completeness is analogous to probabilistic completenessin sampling-based methods, in the sense that in the limit ofincreasing computation, we are guaranteed to optimally solvethe problem, or declare that no solution exists.

To remain computationally competitive with practical mo-tion planners, we also derive a sequential SDP-based methodcalled Moment Motion Planner (MMP). Unlike pre-viously proposed planners for dynamic obstacle avoidance,MMP natively handles continuous-time constraints, does notrequire any discretization, and relies on semidefinite pro-grams whose size scales polynomially in configuration spacedimensionality. On several benchmark problems involvingstatic, moving and morphing obstacles in dimension 2, 3,and 4, including a bimanual planar manipulation task, MMPconsistently outperforms RRT and nonlinear programmingbased baselines, while returning smoother paths in compara-ble solve time.

II. MOMENT-BASED APPROACH FOR TIME-VARYINGOPTIMIZATION PROBLEMS

The last few decades have known the emergence of apowerful moment-based approach for solving optimizatonproblems that are described by polynomial data [17]. One ofthe main challenges one faces when applying this momentapproach to the motion planning problem OMP(D) is the factthat solutions (and the constraints on these solutions) varycontinuously with time. For clarity of presentation, we firstignore the complexities arising from this time dependenceand present the basic ideas behind this approach. Then,we present a result from real algebraic geometry on sumof squares representations of univariate polynomial matricesthat will allow us to impose time-varying constraints on time-varying solutions.

Let us recall some standard notation. For any vectorααα ∈ Nn, |ααα| denotes

∑ni=1 αi. We denote by Nnd the set

of vectors ααα ∈ Nn that satisfy |ααα| ≤ d. We denote byR[yyy] the set of (scalar valued) polynomial functions in thevariables y1, . . . , yn. For ααα ∈ Nn, the monomial yα1

1 . . . yαnn

is denote by yyyααα, and the coefficient of a polynomial p ∈ R[yyy]corresponding to the monomial yyyααα is denoted by pααα. Thedegree of the monomial yyyααα is |ααα|, and the degree deg pof a polynomial p ∈ R[yyy] is the maximum degree of itsmonomials. We denote by Rd[yyy] the set of polynomials ofdegree smaller than or equal to d.A. Moment Approach for Polynomial Optimization Prob-

lemsA polynomial optimization problem is a problem of the form

p∗ = minyyy∈Rn

p(yyy)

s.t. hk(yyy) = 0 k ∈ [m1],

s.t. gk(yyy) ≥ 0 k ∈ [m2],

(P)

where p, h1, . . . , hm1, g1, . . . , gm2

∈ R[yyy]. In general, prob-lem (P) is nonconvex and is very challenging to solve. Infact it is NP-hard even when m1 = 0, m2 = 0, and p is

2

a polynomial of degree four (see, e.g., [21]). An approachpioneered in [14] has been to replace the feasible set

K := {yyy ∈ Rn | hk(yyy) = 0 ∀k ∈ [m1], gk(yyy) ≥ 0 ∀k ∈ [m2]}

with the setM(K)+ of nonnegative Borel measures on K oftotal mass equal to one, leading to the optimization problem

minµ∈M(K)+

∫p(yyy) dµ. (2)

It is not hard to see that the optimal value of problem (2) isequal to that of (P). Moreover, problem (2) has a linear ob-jective function and a convex (infinite-dimensional) feasibleset.

We will now explain how to obtain a finite dimen-sional, convex relaxation of (2). The key idea is to view(2) not as an optimization problem over measures µ ∈M(K)+, but as an optimization problem over sequencesof moments {

∫yyyαααdµ}ααα∈Nn of measures µ ∈ M(K)+.

This is possible because the objective function∫p(yyy)dµ =∑

|ααα|≤deg p pppααα∫yyyαααdµ of problem (2) only depend on the

measure µ through its first few moments.Before we move further with the explanation of the

moment approach, we need to introduce some additionalnotation. For any integer r ∈ N, we denote by Mr,n the setof truncated sequences of “pseudo-moments” in n variables,i.e., elements of the form (φααα)ααα∈Nn

r, where φααα ∈ R for every

ααα ∈ Nnr . Note that any measure µ gives rise to an elementof Mr,n, namely,

(∫yyyαααdµ

)ααα∈Nn

r∈ Mr,n, but a general

element of Mr,n might not come from a measure. Forany φ ∈ Mr,n, we introduce the so-called Riesz functionalLφ : Rr[yyy]→ R defined by

q

=∑ααα∈Nn

r

qαααxααα

7→ ∑ααα∈Nn

r

φαααqααα.

The functional Lφ is to “pseudo-moments” what the ex-pectation operator is to genuine moments. For φ ∈ Mr,n

and q ∈ Rr[yyy], we denote by Mφ(q) the localization matrixassociated with q and φ, i.e., the matrix

Mφ(q)ααα,βββ = Lφ(yyyαααyyyβββq(yyy)) ∀ααα,βββ ∈ Nnb(r−deg q)/2c,

whose rows and columns are labeled by elements ofNnb(r−deg q)/2c, where b·c is the floor function.

Now, for an integer r larger than the maximum of thedegrees of the polynomials p, h1, . . . , hm1 , g1, . . . , gm1 , con-sider the moment relaxation of order r of problem (2) givenby

minφ∈Mr,n

Lφ(p)

s.t. Lφ(1) = 1 ; Mφ(1) � 0

Lφ(yyyαhk) = 0 ∀ααα ∈ Nnr−dk , ∀k ∈ [m1],

Mφ(gk) � 0, ∀k ∈ [m2].

(3)

To see that problem (3) is indeed a relaxation of prob-lem (2), take an arbitrary candidate measure µ ∈ M(K)+

with corresponding objective value v :=∫p(yyy)dµ for prob-

lem (2), and let us extract from it the truncated sequence

of moments φ := (∫yyyαααdµ)|ααα|≤r ∈ Mr,n and show that

φ is (i) feasible to problem (3) and (ii) has v as objectivevalue. To show (i), note that Lφ(1) =

∫dµ = 1, and that

the matrix Mφ(1) is positive semidefinite because for allpolynomials q ∈ Rr[yyy], qqqTMφ(1)qqq =

∫q2(yyy) dµ ≥ 0,

where qqq is the vector of coefficients of the polynomial q. Asimilar reasoning shows that φ satisfies all of the remainingconstraints of problem (3). To show (ii), simply observe thatLφ(p) =

∫p(yyy)dµ = v. The constraints Lφ(1) = 1 and

Mφ(1) � 0 do not depend on the data of the problem athand. We refer to them as moment consistency constraints.

In general, it is not always possible to extract an optimalsolution yyy ∈ Rn of (P) from a ”pseudo-moment” solutionφ ∈Mr,n of (3). However, under some conditons that holdgenerically (see, e.g., [22], [23]), there exists an order r forwhich the optimal value of (3) is equal to that of (P), andan optimal solution yyy of (P) can be recovered from φ by alinear algebra routine. For more details related to extractionof solutions from moment relaxations, the interested readeris referred to [17].

For any r ∈ N, problem (3) is an SDP that can be readilysolved by off-the-shelf solvers such that MOSEK [2]. Weremind the reader that an SDP is the problem of optimizing alinear function subject to linear matrix inequalities. SDPs canbe solved to arbitrary accuracy in polynomial time. See [32]for a survey of the theory and applications of this subject.

Remark 1 (Notation for vector-valued variables): In therest of the paper, we will often deal with variables that arevector valued. To lighten our notation, we use R[yyy1, . . . , yyys](resp. Mr,n1+...+ns ) to denote the set of polynomials (resp.truncated sequences of pseudo-moments) in all of the entriesof the vector-valued variables yyy1 ∈ Rn1 , . . . , yyys ∈ Rns .We also write (yyy1, . . . , yyys)

(ααα1,...,αααs) to denote the monomialyyyααα1

1 . . . yyyαααss , where for each i ∈ [s], αααi is an integer vector

of the same size as yyyi.B. Extension to the time-varying setting.In this paper, we are interested in a variation of problem(P) where the inequality constraints are time-varying, i.e., avariation where inequalties are of the form

g(t, yyy) ≥ 0 ∀t ∈ [0, T ],

where g ∈ R[t, yyy]. Such a constraint can be viewed as acontinuum of constraints gt(yyy) ≥ 0 indexed by t ∈ [0, T ],where gt := g(t, ·) ∈ R[yyy]. If we denote the univariatepolynomial matrix t 7→ Mφ(gt) by X(t), then the momentapproach explained above leads to the constraint

X(t) � 0 ∀t ∈ [0, T ]. (4)

The observation that the coefficients of the polynomialmatrix X depend linearly on the elements of φ combinedwith proposition 1 allows us to rewrite constraint (4) as a(nonvarying) semidefinite programming constraint on φ. Thisallows us to circumvent the need for time discretization.

In the statement of proposition below, Sm (resp. Rm×md [t])denotes the set of symmetric matrices of size m whoseentries are elements of R (resp. Rd[t]) for any positiveintegers m and d.

3

Proposition 1 ([4] Univariate matrix Positivstellensatz):Let m and d be positive integers. There exist two(explicit) linear maps λ1 : Sb

d2 +1cm → Rm×md [t] and

λ2 : Sbd2 +1cm → Rm×md [t] such that for X ∈ Rm×md [t],

X(t) � 0 ∀t ∈ [0, T ] if and only if there exist positivesemidefinite matrices Q1 and Q2 of appropriate sizes thatsatisfy the equation X = λ1(Q1) + λ2(Q2).

III. EXACT MOMENT OPTIMIZATION OVERPIECEWISE-LINEAR PATHS

A. Search for Piecewise-Linear PathsWe propose to approximate the shortest path of OMP(D) bypiecewise-linear paths with a fixed number of pieces. Wechoose to work with the family of piecewise linear functionsfor two reasons. First, they can uniformly approximate anypath over the time interval [0, T ] as the number of piecesgrows. Second, fixing a low number pieces often leads tosimpler and smoother paths.

More concretely, we fix a regular subdivision{0, Ts ,

2Ts , . . . , T} of the time interval [0, T ] of size s,

and we parametrize our candidate trajectory xxx(t) as follows:

xxx(t) = uuui + tvvvi ∀t ∈[

(i− 1)T

s,iT

s

), ∀i ∈ [s], (5)

where uuui, vvvi ∈ Rn for i = 1, . . . , s. We rewrite theobjective function and constraints of OMP(D) in this settingdirectly in terms of uuu := (uuu1, . . . ,uuus) and vvv := (vvv1, . . . , vvvs).The objective function in OMP(D) can be expressed asTs

∑si=1 ‖vvvi‖, and the obstacle-avoidance constraints become

gk(t,uuui+tvvvi) ≥ 0 ∀t ∈[

(i− 1)T

s,iT

s

), ∀i ∈ [s], k ∈ [m].

To ensure continuity of the path xxx(t) at the grid point iTs ,for i = 0, . . . , s, we need to impose the additional constrainthi(uuu,vvv) = 0 with

hi(uuu,vvv) := uuui +iT

svvvi −

(uuui+1 +

iT

svvvi+1

),

and the convention that uuu0 = xxx0, vvv0 = 0, uuus+1 = xxxT , andvvvs+1 = 0.

In conclusion, when specialized to piecewise-linear pathsof type (5), problem OMP(D) becomes

ρ(s;D) = minuuui,vvvi∈

T

s

n∑i=1

‖vvvi‖

s.t. hi(uuu,vvv) = 0 ∀i ∈ {0, . . . , s+ 1}

gk(t,uuui + tvvvi) ≥ 0 ∀t ∈[

(i− 1)T

s,iT

s

],

k ∈ [m], i ∈ [s].

LMP(s;D)

LMP(s;D) is a nonlinear, nonconvex optimization prob-lem in the variables (uuu,vvv). The main difficulty comes fromthe global constraints

gk(t,uuui+tvvvi) ≥ 0 , ∀t ∈[

(i− 1)T

s,iT

s

], ∀i ∈ [s]. (6)

B. A hierarchy of SDPs to find the best piecewise-linearpath

For any optimal motion problem OMP(D), and for anynumber of pieces s, we present a hierarchy of semidefi-nite programs SDP(r, s;D) indexed by a scalar r with thefollowing properties: (i) at every level r, the optimal valueof SDP(r, s;D) is a lower bound on that of LMP(s;D),(ii) under a compactness assumption, the the optimal value ofSDP(r, s;D) converges monotonically to that of LMP(s;D),and (iii) under a compactness and uniqueness assumption,the optimal solution of SDP(r, s;D) converges to that ofLMP(s;D).

As a preliminary step, for each piece i ∈ [s], we introducea scalar variable zi that represents the length ‖vvvi‖ of thatpiece. Mathematically, we impose the constraints

hzi (uuu,vvv,zzz) = 0 and zi ≥ 0 i ∈ [s], (7)

where hzi (uuu,vvv,zzz) = z2i −(Ts ‖vvvi‖

)2for i ∈ [s]. We introduce

the auxiliary variable zi in this seemingly complicated way(instead of simply taking zi = ‖vvvi‖) to make the functionsappearing in the objective and constraints of LMP(s;D)polynomial functions.

We are now ready to follow the moment approach pre-sented in section II. We fix a positive integer r, and weconstruct the moment relaxation of order r of problemLMP(s;D). For that, we need to specify the decision vari-ables, objective, and constraints. Our decision variable isa truncated sequence φ ∈ Mr((2n + 1) × s) (that shouldbe viewed as a sequence of “pseudo-moments” in variablesuuu ∈ Rn×s, vvv ∈ Rn×s, and zzz ∈ Rs up to degree r). Intuitively,φ represents a “pseudo-distribution” over candidate paths.Our objective function is

∑si=1 Lφ(zi), and our constraints

are the moment consistency constraints

Lφ(1) = 1 and Mφ(1) � 0, (8)

the continuity constraints

Lφ((uuu,vvv,zzz)αααhi(uuu,vvv)) = 0 (9)

for all ααα ∈ Ns(2n+1)r−1 and i ∈ {0, . . . , s}, the obstacle

avoidance constraints

Mφ(gk(t,uuui + tvvvi)) � 0 ∀t ∈[

(i− 1)T

s,iT

s

](10)

for all k ∈ [m] and i ∈ [s], and the constraints

Lφ((uuu,vvv,zzz)αααhzi (uuu,vvv,zzz)) = 0 and Mφ(zi) � 0 (11)

coming from the definition of zzz in (7) for all ααα ∈ Ns(2n+1)r−2

and i ∈ [s].In conclusion, the moment relaxation of order r of prob-

lem LMP(s;D) is the SDP

ρ(r, s;D) = minφ∈Mr,(2n+1)×s

s∑i=1

Lφ(zi)

s.t. φ satisfies (8) to (11),

SDP(r, s;D)

We emphasize that the objective function of SDP(r, s;D)is linear, and that its constraints are valid SDP constraints.

4

Fig. 2: In red, setup of the path planning problem considered inexample 1. In blue, the shortest piecewise-linear path extracted fromSDP(r, s;D) with s = 2 and r = 6.

Indeed, constraints (8), (9) and (11) are (scalar or matrix)linear inequalities, while the time-varying inequalities in (10)translates to positive semidefinite constraints on the φααα’s andsome additional auxiliary variables in view of Proposition 1.

Theorems 1 and 2 below1 present the main results of thissection. They are related respectively to the optimal valueand optimal solution of SDP(r, s;D).

Theorem 1: Consider the motion planning problemOMP(D) given by data D = (xxx0,xxxT , {g1, . . . , gm}). Thesequence {ρ(r, s)}r∈N of optimal values of SDP(r, s;D) isnondecreasing and is upper bounded by the optimal valueρ(s;D) of LMP(s;D). (In particular, if ρ(r, s;D) = ∞for some r ∈ N, then problem LMP(s;D) is infeasible.)Furthermore, if

gm(t,xxx) = R2 − ‖xxx‖2 ∀xxx ∈ Rn, ∀t ∈ R for some R > 0,(12)

then ρ(r, s;D)→ ρ(s;D) as r →∞.Assumption (12) is needed for technical reasons but isnot restrictive in practive. Indeed, in most motion planningproblems, the configuration space is bounded, in which casewe can append the polynomial g(t,xxx) := R2 − ‖x‖2 to thelist of polynomials in D without loss of generality.

Theorem 2: Consider the motion planning problemOMP(D) given by data D = (xxx0,xxxT , {g1, . . . , gm}).Under assumption (12), for any r ∈ N, the optimalvalue of SDP(r, s;D) is attained by some element φr ∈Mr((2n+ 1)× s). Furthermore, if LMP(s;D) has a uniqueoptimal solution

xxx∗(t) := uuu∗i + tvvv∗i , t ∈[

(i− 1)T

s,iT

s

]i ∈ [s], (13)

then Lφr (uuui)→ uuu∗i and Lφr (uuui)→ vvv∗i as r →∞ for i ∈ [s].C. Detecting optimality of a solution to SDP(r, s;D)The results of theorems 1 and 2 presented in the previoussection are asymptotic. For a given number of pieces s anda given relaxation order r, the optimal value of SDP(r, s;D)provides only a lower bound on that of LMP(s;D). Recov-ering the shortest piecewise-linear path or its correspondinglength requires taking r to infinity in general. The followingproposition shows that, if some conditons that are easilycheckable hold, we can get the same recovery guaranteesfor finite r.

Proposition 2: For integers any integers s and n, if anoptimal solution φ ∈ Mr,(2n+1)×s of SDP(r, s;D) satisfies

1The proofs of these results were ommitted to conserve space. They canbe found in [1]

Lφ(‖uuui‖r) = ‖Lφ(uuui)‖r, Lφ(‖vvvi‖r) = ‖Lφ(vvvi)‖r, andLφ(zri ) = Lφ(zi)

r for i ∈ [s], then the piecewise-linear path

xxx∗(t) := Lφ(uuui)+t Lφ(vvvi) ∀t ∈[

(i− 1)T

s,iT

s

], ∀i ∈ [s],

is optimal for LMP(s;D).

Other than its obvious practical benefit, the result of proposi-tion 2 inspires the iterative approach we present in section IV.

Example 1: Consider the simple instance of OMP(D) indimension n = 2 given by data D = (xxx0,xxxT , {g1 . . . , g5}),where xxx0 = (0,−1)T , xxxT = (0, 1)T , g1(t,xxx) = 1 − x1,g3(t,xxx) = 1 + x1, g3(t,xxx) = 1 − x2, g4(t,xxx) = 1 + x2,g5(t,xxx) = (xxx1 + 1

3 )2 + (xxx2 − 15 )2 − t(xxx1 + 1

3 )3 − ( 12 )2. (See

figure 2 for a plot of this setup.) We search for paths thatare piecewise-linear of the form in (5) with s = 2 pieces.By computing the optimal values of SDP(r, s;D) for r ∈{3, 4, 5, 6}, we obtain the nondecreasing sequence of lowerbounds

r 3 4 5 6SDP(r, s;D) 0.75 1.81 2.09 2.14

on the length of any piecewise-linear path with 2 piecesthat starts in xxx0, ends at xxxT , and avoids the obstacles givenby the polynomials {g1, . . . , g5}. In particular, no such pathhas length smaller than 2.14. We check numerically that forr = 6, the optimal solution φ of SDP(r, s;D) returned bythe solver satisfies the requirement of proposition 2, and weextract from φ the path plotted in figure 2 whose lengthis 2.14.

D. A sparse version of the SDP hierarchy SDP(r, s;D)

In this section we briefly describe how one may reduce thesize of the semidefinite programs SDP(r, s;D) by exploit-ing an inherent sparsity of LMP(s;D). If we partition thedecision variables (uuu,vvv,zzz) of LMP(s;D) as V1 ∪ · · · ∪ Vs,where for each i ∈ [s], Vi := {uuui, vvvi, zi,uuui+1, vvvi+1, zi+1},then each constraint that appear in (9), (10), or (11) involvesonly the variables of exactly one of the Vi’s. Furthermore,the family {V1, . . . , Vs} satisfies the Running IntersectionProperty (RIP), that is,

∀i ∈ [s− 1], ∃k ≤ i, (V1 ∪ . . . ∪ Vi) ∩ Vi+1 ⊂ Vk. (RIP)

Following [33], [15], we replace the single truncatedsequence of “pseudo-moments” φ ∈ Mr,(2n+1)×s in allvariables of V with s truncated sequences φ1, . . . , φs ∈Mr,2n+1, where for each i ∈ [s], φi is a truncated sequenceof “pseudo-moments” in the variables of Vi. Intuitively, φirepresents a “pseudo-distribution” from which the i-th pieceof our candidate piecewise-linear path is sampled. Withoutentering into details beyond the scope of this paper we canprove that theorems 1 and 2 hold if SDP(r, s;D) is replacedwith the SDP

5

MMP RRT NLP

t = 010 t = 3

10 t = 610 t = 9

10

t

Fig. 3: Typical paths obtained for a motion planning problemin 2D. The path is constrained to be in the black box andshould avoid the moving obstacles. Each obstacle is a spherethat moves with constant velocity (depicted by a red arrow).

ρ′(r, s;D) = minφi

s∑i=1

Lφi(zi)

s.t. Lφi(1) = 1, Mφi(1) � 0, i ∈ [s− 1]

Lφi

((Vi, Vi+1)

αααhi(Vi, Vi+1))= 0 ∀ααα ∈ N4n

2r−1, i ∈ [s],

Lφi

((Vi, Vi+1)

αααhzi (Vi))= 0 ∀ααα ∈ N4n

2r−2, i ∈ [s],

Mφi(gk(t,uuui + tvvvi)) � 0 ∀t ∈[(i− 1)T

s,iT

s

],

k ∈ [m] ; i ∈ [s− 1],

SparseSDP(r, s;D)where for each i ∈ {0, . . . , s}, hi is the polynomial functionsuch that hi(Vi, Vi+1) = hi(uuu,vvv), and for each i ∈ [s], hziis the polynomial function such that hzi (Vi) = hzi (uuu,vvv,zzz).The main feature of SparseSDP(r, s;D) when compared toSDP(r, s;D) is that its constraints involve localizing matriceswith pseudo-moments on 2(2n + 1) variables (instead ofs(2n+ 1) variables in SDP(r, s;D)). For more details aboutthe use of sparsity in polynomial optimization problems,interested reader is referred to [16].

IV. MOMENT MOTION PLANNER: AN ITERATIVEOPTIMIZATION PROCEDURE OVER

PIECEWISE-LINEAR PATHS

Algorithm 1 MMP: Moment Motion Planner

1: Input: Data D = ({xxx0,xxxT , {g1, . . . , gm}) order r ofthe moment relaxation, number of iterations N , trade-off constant λ > 0.

2: Initialize φ(0) = (φ(0)1 , . . . , φ

(0)s ) randomly, where for

each i ∈ [s], φ(0)i ∈Mr,2n.

3: for t = 1, . . . , N do4: Let φ(t) be a minimizer of (18) with φ = φ(t) subject

to (14), (15), and (16).5: Return the piecewise-linear path defined by

xxx∗(t) := Lφ(N)i

(uuui) + t Lφ(N)i

(vvvi)

for every t ∈[

(i−1)Ts , iTs

]and every i ∈ [s].

As we have seen in the previous section, the optimalvalues ρ(r, s) of the SDPs in the hieararchy SDP(r, s;D)

are nondecreasing lower bounds on the optimal value ρ(s)of OMP(D). As a downside, a feasible path cannot possiblybe extracted from a solution of one of these SDPs (at order,say, r) unless r is large enough so that ρ(r, s) = ρ(s). Theorder r needed for that to happen is in general prohibitivelylarge.

To adress this issue, we present in algorithm 1 a morepractical motion planner called MMP. MMP is also basedon a moment relaxation, but has two distinctive featureswhen compared to SDP(r, s;D): (i) it produces feasiblepaths already for low orders r (taking r = 2 producedgood results in all of our benchmarks) and (ii) the optimalvalues produced by MMP are not necessarily lower boundson ρ(s). In other terms, MMP trades off some the theoreticalguarantees of SDP(r, s;D) for more efficiency.

MMP is an iterative algorithm. At every iteration, we solvean SDP that is similar in spirit to SDP(r, s;D) with a fewkey differences. First, we drastically decrease the numberof decision variables. We completely discard the variablezzz, and we take inspiration from the sparsity considerationsreviewed in section III-D to partition the remaining variables(uuu,vvv) of LMP(s;D) as W1 ∪ · · · ∪ Ws, where for eachi ∈ [s], Wi := {uuui, vvvi}. Then, we take as decision variablesof our inner SDP s truncated sequences φ := (φ1, . . . , φs),where for each i ∈ [s], φi ∈ Mr,2n is a truncated sequenceof “pseudo-moments” in variables Wi. Intuitively, each φirepresents a “pseudo-distribution” from which the i-th pieceof our candidate piecewise-linear path is sampled. Note thatthe family of sets {W1, . . .Ws} does not satisfy the (RIP)property anymore. This is the main reason why MMP lackssome of the theoretical guarantees of the moment relaxationSDP(r, s;D).

Then, we adapt the constraints of our inner SDP to ournew choice of decision variables. In addition to the classicalmoment-consistency constraints

Lφi(1) = 1 and Mφi

(1) � 0 ∀i ∈ [s], (14)

we impose the the continuity constraints

Lφi((uuui+

iT

svvvi)

ααα) = Lφi+1((uuui+1 +

iT

svvvi+1)ααα) ∀ααα ∈ N2n

r−2

(15)between endpoints of pieces i and i+ 1 for each i ∈ [s], andthe obstacle-avoidance constraints

Mφi(gk(t,uuui + tvvvi)) � 0 ∀t ∈

[(i− 1)T

s,iT

s

](16)

for each obstacle k ∈ [m] and for each piece i ∈ [s].Finally, let us explain our choice of objective function.

Motivated by proposition 2, we would ideally like to takethe objective function of our SDP to be

∑si=1 ‖Lφi

(vvvi)‖ +λJ(φ), where λ > 0 and

J(φ) =

s∑i=1

Lφi(‖uuui‖r)− ‖Lφi

(uuui)‖r

+ Lφi(‖vvvi‖r)− ‖Lφi

(vvvi)‖r(17)

6

nMethods Static Obstacles Dynamic Obstacles

success rate length smoothness solve time success rate length smoothness solve time

2RRT 40% 3.73 0.06 0.02 50% 3.59 0.12 0.06NLP 0% nan nan nan 20% 3.4 0.06 0.06MMP 60% 3.0 0.03 0.43 50 % 2.85 0.03 0.43

3RRT 50% 5.74 0.13 0.12 40% 4.82 0.1 0.23NLP 70% 3.44 0.05 0.12 60% 3.48 0.06 0.17MMP 100% 3.5 0.04 0.47 100% 3.55 0.04 0.47

4RRT 60% 7.67 0.15 1.43 0% nan nan nanNLP 80% 3.99 0.08 0.25 90% 4.34 0.1 0.2MMP 100% 4.11 0.05 0.55 100% 4.1 0.05 0.55

TABLE I: Average success, smoothness, and solve-time comparison of RRT, NLP and MMP (proposed) methods over 10 static anddynamic motion planning problems.

θ

α γ

Initial configuration Goal configuration

RRTt = 1

10 t = 210 t = 3

10 t = 410 t = 5

10 t = 610 t = 7

10 t = 810 t = 9

10 t = 1010

NLPt = 1

10 t = 210 t = 3

10 t = 410 t = 5

10 t = 610 t = 7

10 t = 810 t = 9

10 t = 1010

MMPt = 1

10 t = 210 t = 3

10 t = 410 t = 5

10 t = 610 t = 7

10 t = 810 t = 9

10 t = 1010

Fig. 4: Performance comparison of RRT, NLP, and MMP (proposed) methods on a bimanual planar manipulation task.

The intuition is that, if J(φ) = 0 for some φ = (φ1, . . . , φs)satisfying (15) and (16), then the path

xxx∗(t) := Lφi(uuui)+t Lφi

(vvvi) ∀t ∈[

(i− 1)T

s,iT

s

], ∀i ∈ [s],

is feasible to OMP(D). The constant λ controls the trade-off between minimizing the length of the path and enforcingthat the path is feasible. The issue with objective function(17) is that the function J is nonconvex. As a workaroud,we replace J in (17) with its linearization around a referencepoint φ = (φ1, . . . , φs), leading to the objective function

s∑i=1

‖Lφi(vvvi)‖+ λJ(φ; φ), (18)

where J(φ; φ) is given by

J(φ) +

s∑i=1

Lφi(‖uuui‖r)− (r − 1)Lφi(uuui)‖Lφi(uuui)‖r−1

+ Lφi(‖vvvi‖r)− (r − 1)Lφi

(vvvi)‖Lφi(vvvi)‖r−1

In the t-th iteration of our iterative approach, we takethe optimal solution φ(t−1) obtained from solving the innerSDP at iteration t − 1, and uses that as φ. The elements of

φ(0) are initialized from some random distribution. Gaussianinitializaton seems to work well in practice. Note that theoverall time complexity of algorithm 1 is polynomial in thedimension n, the number of iterations N , and the number ofpieces s.

V. NUMERICAL RESULTS

For animations of motion planning problems and code toreproduce numerical results, please see [1].A. MMP vs NLP vs RRTSetup. In each dimension n ∈ {2, . . . , 4}, we gener-ate 10 motion planning problems where the path is con-strained to live in the unit box B = [−1, 1]n and mustavoid 10 static or dynamic spherical obstacles. More pre-cisely, each motion planning problem is given by dataD = (xxx0,xxxT , {g1, . . . , gm}), where xxx0 = (−1, . . . ,−1) ∈Rn,xxxT = (1, . . . , 1) ∈ Rn, i ∈ [n], gi(xxx) = 1 − xi,gi+n(t,xxx) = 1 + xi for i ∈ [n], g2n+k(t,xxx) = ‖xxx −(ccck + tvvvk)‖2−

(210

)2. The centers ccck are sampled uniformly

at random from B, the velocities vvvk are either identicallyzero in the static case, or sampled uniformly from B in thedynamic case. See figure 3 for an example of this setup indimension n = 2.

Comparison. In table I, we compare our MMP solver

7

(with r = 2, N = 20, and λ = 0.1) against a clas-sical sampling-based technique (RRT) and basic nonlinearprogramming baseline (NLP) implemented with the helpof the KNITRO.jl package [3]. MMP consistently achieveshigher success rates, significantly shorter and smoothertrajectories (the smoothness of a path xxx(t) is given by∫ T

0

(xxx(t)−

∫ T0xxx(s)ds

)2

dt). The solve times are higher butremain highly practical.

θ

α

γ

MMPRRTNLP

Fig. 5: Plot of the paths found by RRT, NLP, and MMP (proposed)for the bimanual planning manipulation task of figure 4. The reddots depict values of the joint angles (α, β, γ) that would makethe two arms collide. The black wireframe depicts the smallestenclosing ellipsoid containing the red dots.B. Bimanual ManipulationAs a proof of concept, we also consider a bimanual ma-nipulation task requiring two two-link arms working col-laboratively to go from an initial to goal configurationwithout colliding (see figure 4). For visualization, we restrictattention to planar manipulation problems involving planningin a configuration space of 3 joint angles. First, we liftthe obstacle set in cartesian space to joint angle space byevaluating all collision configurations on a grid over the 3joint angles. We then fit an enclosing ellipsoid (see figure 5)to obtain a semialgebraic description of the obstacle region inconfiguraton space. RRT finds a feasible path, but producesa jerky path involving an unnecessary recoiling of one ofthe arms. NLP fails to find a feasible path at all. MMP, with3 segments, succeeds in finding the shortest and smoothestpath.

ACKNOWLEDGEMENTS

We thank Ed Schmerling and Brian Ichter for referenceson motion planning with time-varying obstacles, CarolinaParada and Jie Tan for robot mobility research frameworks,and Amirali Ahmadi for guidance on SoS programming.

REFERENCES

[1] https://github.com/bachirelkhadir/PathPlanningSOS.jl.

[2] Introducing the MOSEK optimization suite. 2018. URL https://docs.mosek.com/8.1/intro/index.html.

[3] Knitro.jl. 2020. URL https://github.com/jump-dev/KNITRO.jl.

[4] Amir Ali Ahmadi and Bachir El Khadir. Time-varying semidefiniteprograms. Preprint available at arXiv:1808.03994, 2018.

[5] Amir Ali Ahmadi, Georgina Hall, Ameesh Makadia, and Vikas Sind-hwani. Geometry of 3d environments and sum of squares polynomials.arXiv preprint arXiv:1611.07369, 2016.

[6] John Canny. The complexity of robot motion planning. MIT press,1988.

[7] Fabrizio Dabbene and Didier Henrion. Set approximation viaminimum-volume polynomial sublevel sets. In 2013 European ControlConference (ECC), pages 1114–1119. IEEE, 2013.

[8] Fabrizio Dabbene, Didier Henrion, and Constantino M Lagoa. Simpleapproximations of semialgebraic sets and their applications to control.Automatica, 78:110–118, 2017.

[9] Dan Halperin, Oren Salzman, and Micha Sharir. Algorithmic motionplanning. In Handbook of Discrete and Computational Geometry,pages 1311–1342. Chapman and Hall/CRC, 2017.

[10] Lucas Janson, Brian Ichter, and Marco Pavone. Deterministicsampling-based motion planning: Optimality, complexity, and perfor-mance. The International Journal of Robotics Research, 37(1):46–61,2018.

[11] Mrinal Kalakrishnan, Sachin Chitta, Evangelos Theodorou, Peter Pas-tor, and Stefan Schaal. Stomp: Stochastic trajectory optimization formotion planning. In 2011 IEEE international conference on roboticsand automation, pages 4569–4574. IEEE, 2011.

[12] Sertac Karaman and Emilio Frazzoli. Sampling-based algorithmsfor optimal motion planning. The international journal of roboticsresearch, 30(7):846–894, 2011.

[13] Oussama Khatib. Real-time obstacle avoidance for manipulatorsand mobile robots. In Autonomous robot vehicles, pages 396–404.Springer, 1986.

[14] Jean B Lasserre. Global optimization with polynomials and theproblem of moments. SIAM Journal on optimization, 11(3):796–817,2001.

[15] Jean B Lasserre. Convergent SDP-relaxations in polynomial optimiza-tion with sparsity. SIAM J. Optim., 17:822–843, 2006.

[16] Jean B Lasserre. Moments, Positive Polynomials and Their Applica-tions, volume 1. World Scientific, 2010.

[17] Jean B Lasserre. An Introduction to Polynomial and Semi AlgebraicOptimization. Cambridge University Press, 2015.

[18] Steven M LaValle. Planning algorithms. 1999.[19] Steven M LaValle and James J Kuffner. Rapidly-exploring random

trees: Progress and prospects. Algorithmic and computational robotics:new directions, (5):293–308, 2001.

[20] Kevin M Lynch and Frank C Park. Modern Robotics. CambridgeUniversity Press, 2017.

[21] Katta G Murty and Santosh N Kabadi. Some NP-complete problemsin quadratic and nonlinear programming. Mathematical Programming,39:117–129, 1985.

[22] J. Nie. Certifying convergence of Lasserre’s hierarchy via flattruncation. Mathematical Programming, Ser. A, 146(1-2):485–510,2013.

[23] J. Nie. Optimality conditions and finite convergence of Lasserre’shierarchy. Mathematical Programming, Ser. A, 146(1-2):97–121, 2014.

[24] Edouard Pauwels and Jean B Lasserre. Sorting out typicality withthe inverse moment matrix sos polynomial. In Advances in NeuralInformation Processing Systems, pages 190–198, 2016.

[25] John Reif and Micha Sharir. Motion planning in the presence ofmoving obstacles. Journal of the ACM (JACM), 41(4):764–790, 1994.

[26] John H Reif. Complexity of the mover’s problem and generalizations.In 20th Annual Symposium on Foundations of Computer Science (sfcs1979), pages 421–427. IEEE, 1979.

[27] Elon Rimon and Daniel E Koditschek. Exact robot navigation usingartificial potential functions. Departmental Papers (ESE), page 323,1992.

[28] John Schulman, Yan Duan, Jonathan Ho, Alex Lee, Ibrahim Awwal,Henry Bradlow, Jia Pan, Sachin Patil, Ken Goldberg, and PieterAbbeel. Motion planning with sequential convex optimization andconvex collision checking. The International Journal of RoboticsResearch, 33(9):1251–1270, 2014.

[29] Jacob T Schwartz and Micha Sharir. On the piano movers’ problem:V. the case of a rod moving in three-dimensional space amidst poly-hedral obstacles. Communications on Pure and Applied Mathematics,37(6):815–848, 1984.

[30] Vikas Sindhwani, Rebecca Roelofs, and Mrinal Kalakrishnan. Sequen-tial operator splitting for constrained nonlinear optimal control. In2017 American Control Conference (ACC), pages 4864–4871. IEEE,2017.

[31] Klaus Sutner and Wolfgang Maass. Motion planning among timedependent obstacles. Acta Informatica, 26(1-2):93–122, 1988.

[32] Lieven Vandenberghe and Stephen Boyd. Semidefinite programming.SIAM review, 38(1):49–95, 1996.

[33] H. Waki, S. Kim, M. Kojima, and M. Muramatsu. Sums of squaresand semidefinite program relaxations for polynomial optimizationproblems with structured sparsity. SIAM J. Optim., 17(1):218–242,2006.

8