Towards faster roadmap algorithms for smooth and unbounded

Towards faster roadmap algorithmsfor smooth and unbounded real algebraic sets

Remi Prebet, PolSys, LIP6, Sorbonne Universite JNCF 2021, March 2nd 2021

joint work with Mohab Safey El Din and Eric Schost

1

Motion planning problem

Semi-algebraic sets

S =M⋃i=1

{x ∈ Rn | fi(x) = 0 ∧ gi(x) > 0

}with (fi, gi) ⊂ R[x1, . . . , xn] finite

Motion planning problem

Data:

1. S ⊂ Rn semi-algebraic set

2. x,y ∈ S

Question:

Are x and y connected in S?

[LaValle, 2006]

2

First approach

Cylindrical Algebraic Decomposition (CAD) [Collins, 1973]

V ⊂ Rn alg. set of dimension d and defined by s polynomials of degree 6 D

Authors Complexities Assumptions

[Schwartz & Sharir, 1983] (sD)O(1)n

A CAD adapted to a cylinder

[Basu & Pollack & Roy, 2009]

Goal: only describe the connectedness of S

Lower bound of complexity[Thom & Milnor, 1964]∣∣∣Connected components of S

∣∣∣ ≤ (sD)O(n)

3

First approach

Cylindrical Algebraic Decomposition (CAD) [Collins, 1973]

V ⊂ Rn alg. set of dimension d and defined by s polynomials of degree 6 D

Authors Complexities Assumptions


A CAD adapted to a cylinder

[Basu & Pollack & Roy, 2009]

Goal: only describe the connectedness of S

Lower bound of complexity[Thom & Milnor, 1964]∣∣∣Connected components of S

∣∣∣ ≤ (sD)O(n)

3

Roadmaps

[Canny, 1988] Compute R ⊂ S one-dimensional, sharing its connectivity

Roadmap of a semi-algebraic set S

It is a semi-algebraic curve R ⊂ S, containing P finite and such that

for all connected components C of S: C ∩R is non-empty and connected

Proposition

x,y ∈P are connected in S ⇐⇒ they are in R

Interest?

Arbitrary dimension =⇒ Dimension 1

4

Complexities

S ⊂ Rn semi alg. set of dimension d and defined by s polynomials of degree 6 D

Connectivity result [Canny, 1988]

If V is bounded, W (π2, V ) ∪ F has dimension d− 1

and satisfies the Roadmap property.

Author·s Complexity Assumptions


[Canny, 1993] (sD)O(n2)

[Basu & Pollack & Roy, 1999] sd+1DO(n2)

[Safey El Din & Schost, 2011] (nD)O(n√

n) Smooth, bounded algebraic sets

[Basu & Roy & Safey El Din

& Schost, 2014](nD)O(n

√n) Algebraic sets

[Basu & Roy, 2014] (nD)O(n log2 n) Algebraic sets

[Safey El Din & Schost, 2017] O((nD)6n log2 d

)Smooth, bounded algebraic sets

5

Complexities







[Canny, 1993] (sD)O(n2)










5

Complexities







[Canny, 1993] (sD)O(n2)










5

Complexities


Connectivity result [Safey El Din & Schost, 2011]

If V is bounded, W (πi, V ) ∪ Fi has dimension max(i− 1, d− i+ 1)

and satisfies the Roadmap property



[Canny, 1993] (sD)O(n2)


[Safey El Din & Schost, 2011] (nD)O(n√n) Smooth, bounded algebraic sets







5

Complexities







[Canny, 1993] (sD)O(n2)









5

Complexities







[Canny, 1993] (sD)O(n2)









5

Complexities







[Canny, 1993] (sD)O(n2)









5

Complexities







[Canny, 1993] (sD)O(n2)









Results based on a theorem in the bounded case

5

Complexities







[Canny, 1993] (sD)O(n2)










Remove the boundedness

assumption is a costly step

5

Complexities







[Canny, 1993] (sD)O(n2)










Remove the boundedness

assumption is a costly step

Necessity of a new theorem

in the unbounded case!

5

A challenging robotic application

Matrix M associated to a puma-type robot with a non-zero offset in the wrist

(v3 + v2)(1− v2v3) 0 A(v) d3A(v) a2(v23 + 1)(v22 − 1)− a3A(v) 2d3(v3 + v2)(v2v3 − 1)

0 v23 + 1 0 2a2v3 0 (a3 − a2)v23 + a2 + 2a30 1 0 0 0 2a30 0 1 0 0 0

v4 1− v24 0 d4(1− v24) −2d4v4 0

(v24 − 1)v5 4v4v5 (1− v25)(v24 + 1) (1− v25)(v24 − 1)d5 + 4d4v4v5 2d5v4(1− v25) + 2d4v5(1− v24) −2d5v5(v24 + 1)

where A(v) = (v23 − 1)(v22 − 1)− 4v2v3

A puma 560 [Unimation, 1984]

Fix arbitrary (a2, a3, d3, d4, d5) ∈ (Q∗+)5

a2, a3: distances d3, d4, d5: offsets

v2, v3, v4, v5: half-angle tangents of rotations

Cuspidality?

Is there γ : [0, 1]→ R4 such that

γ(0) = v and γ(1) = v′

∀t ∈ [0, 1], detM(γ(t)) 6= 0?

Goal

Compute the number of connected components

of S ={v ∈ R4 | det(M(v)) 6= 0

}

6

Contributions

Computations - first contribution

• Computed a roadmap for the analysis of the kinematic singularities of a

puma-type robot

• This problem was considered as difficult using the current techniques

Roadmap algorithms can be used for solving robotic problems

New theoretical result - second contribution

• Stated and proved a new connectivity result for constructing roadmaps

• Holds in the case of unbounded real algebraic sets

• Assumptions hold generically

• An algorithm can be easily designed from it

We can obtain efficient algorithms for unbounded real algebraic sets

7

Cuspidality of a PUMA robot

Reduction

Consider S ={x ∈ Rn | f(x) 6= 0

}Assumption 1: S is bounded. [Canny, 1988]

For r > 0 large enough,

RoadMap(S ∩B(0, r)

)= RoadMap(S)

Assumption 2: S is an algebraic set [Canny, 1993]

For ε > 0 small enough,

Roadmap({f ≥ ε} ∩B(0, r)

)Roadmap

({f 6= 0} ∩B(0, r)

) ⋃Roadmap

({f ≤ −ε} ∩B(0, r)

)

Boundaries

Sufficient to compute the intersection of S ∩B(0, r) with the roadmaps of

S+ε = V(f − ε), S+

ε,r = V(f − ε, ||x||2 − r), S+r = V(||x||2 − r)

and S−ε = V(f + ε), S−ε,r = V(f + ε, ||x||2 − r), S−r = V(||x||2 − r).

8

Canny’s strategy

Projection through:

π2 : (x1, . . . , xn) 7→ (x1, x2)

W (π2, V ) critical locus of π2.

Intersects all the

connected components of V

Theorem [Canny, 1988]

W (π2, V )⋃F has dimension dim(V )− 1


Roadmap property

∀C connected component,

C ∩R is non-empty and connected

9

Canny’s strategy

Projection through:

π2 : (x1, . . . , xn) 7→ (x1, x2)


Intersects all the





Roadmap property



9

Canny’s strategy

Projection through:

π2 : (x1, . . . , xn) 7→ (x1, x2)


Intersects all the





Roadmap property



9

Canny’s strategy

Projection through:

π2 : (x1, . . . , xn) 7→ (x1, x2)


Intersects all the





Roadmap property



9

Canny’s strategy

Morse theory

“Scan” W (π2, V ) at the critical values

of π1

• We repair the connectivity failures

with critical fibers

• We repeat the process at

every critical value




Roadmap property



9

Canny’s strategy

Morse theory


of π1








Roadmap property



9

Canny’s strategy

Morse theory


of π1








Roadmap property



9

Canny’s strategy

Morse theory


of π1








Roadmap property



9

Canny’s strategy

Morse theory


of π1








Roadmap property



9

Canny’s strategy

Morse theory


of π1








Roadmap property



9

Canny’s strategy

Morse theory


of π1








Roadmap property



9

Canny’s strategy

Morse theory


of π1








Roadmap property



9

Canny’s strategy

Morse theory


of π1








Roadmap property



9

Canny’s strategy

W (π2, V ) polar variety

F critical fibers

Assumptions:

1. W (π2, V ) has dimension 1

2. F has dimension dim(V )− 1




Roadmap property



9

Canny’s strategy


F critical fibers

Assumptions:






Roadmap property



9

Canny’s strategy


F critical fibers

Assumptions:






Roadmap property



9

Canny’s strategy


F critical fibers

Assumptions:


2. F has dimension dim(V )− 1Genericity




Roadmap property



9

Computation of critical loci

Critical points

x critical point of πi on V ⇐⇒{x ∈ reg(V ) | πi(TxV ) 6= Ci

}= W ◦(πi, V )

An effective characterisation

x critical point of πi on V Ji = Jac(h, [xi+1, . . . , xn]) where h ∈ I(V ) ⊂ R[x1, . . . , xn]

{x ∈ V | rank Ji(x) < c

}All c-minors of Ji(x) vanish at x

(Lemma) c=n−dim(V )

Determinantal ideal

Torus of revolution axis directed by the vector −→x +−→z

Splitting in two sets =⇒ Degree reduction

10


Critical points


}= W ◦(πi, V )





(Lemma) c=n−dim(V ) Determinantal ideal



10


Critical points


}= W ◦(πi, V )








10


Critical points


}= W ◦(πi, V )






Two kinds of critical points

x critical point of πi on V x ∈W2 (polar variety)

TxW2 ⊂ TxV is normal to Im(π1)

TxW2 is normal to Im(π1) or

TxW2 is normal to Im(π2) ⊃ Im(π1)


10

First contribution: application to a puma-type robot

Matrix M associated to a puma-type robot with a non-zero offset in the wrist

(v3 + v2)(1− v2v3) 0 A(v) d3A(v) a2(v23 + 1)(v22 − 1)− a3A(v) 2d3(v3 + v2)(v2v3 − 1)

0 v23 + 1 0 2a2v3 0 (a3 − a2)v23 + a2 + 2a30 1 0 0 0 2a30 0 1 0 0 0

v4 1− v24 0 d4(1− v24) −2d4v4 0

(v24 − 1)v5 4v4v5 (1− v25)(v24 + 1) (1− v25)(v24 − 1)d5 + 4d4v4v5 2d5v4(1− v25) + 2d4v5(1− v24) −2d5v5(v24 + 1)

S ={v ∈ R4 | det(M(v)) 6= 0

}

A puma 560 [Unimation, 1984]

First step

Max. degree without splitting: 1858 Max. time

Locus Max. degree msolve Maple

Critical points 934 3.4 min 63 min

Critical curves 220 77 min 280 min

Recursive step over 96 fibers

Data are for one fiber Max. time

Locus Max. degree msolve Maple

Critical points 38 2 s 3 s

Critical curves 21 3 s 40 s

Roadmap

Degree: 8168

Time: 3h22 (msolve)/ 20h (Maple)

11

A new connectivity result for

unbounded real algebraic sets

Second contribution: new connectivity result

Projection on 2 coordinates

π2 : Cn → C2

(x1, . . . ,xn) 7→ (x1,x2)

• W (π2, V ) polar variety

• F2 = π−11 (π1(K)) critical fibers

• K = critical points of π1 on W (π2, V )


If V is bounded, W (π2, V ) ∪ F2 has dimension d− 1


12


Projection on i coordinates

πi : Cn → Ci

(x1, . . . ,xn) 7→ (x1, . . . ,xi)

• W (πi, V ) polar variety

• Fi = π−1i−1(πi−1(K)) critical fibers

• K = critical points of π1 on W (πi, V )




No critical points!

12


Non-negative proper polynomial map

ϕi : Cn −→ Ci

x 7→ (ψ1(x), . . . , ψi(x))

• W (ϕi, V ) generalized polar variety

• Fi = ϕi−1(ϕi−1(K)) critical fibers.

• K = critical points of ϕ1 on W (ϕi, V )

Connectivity result [P. & Safey El Din & Schost, 2021]

If V is bounded, W (ϕi, V ) ∪ Fi has dimension max(i− 1, d− i+ 1)


Critical point!

12



ϕi : Cn −→ Ci

x 7→ (ψ1(x), . . . , ψi(x))




“Graded” roadmap property RM(x):

For all connected components C of V ∩ Rn ∩ϕ−11

((−∞, x]

)C ∩ (Fi ∪W (ϕi, V )) is non-empty and connected

12



ϕi : Cn −→ Ci

x 7→ (ψ1(x), . . . , ψi(x))






((−∞, x]


Morse theory

Two disjoint cases:

x ∈ ϕ−11 (K) or not

Sard’s lemma

ϕ−11 (K) is finite

12



ϕi : Cn −→ Ci

x 7→ (ψ1(x), . . . , ψi(x))






((−∞, x]


Thom’s isotopy Lemma

12



ϕi : Cn −→ Ci

x 7→ (ψ1(x), . . . , ψi(x))






((−∞, x]


Algebraic Puiseux Series

12



ϕi : Cn −→ Ci

x 7→ (ψ1(x), . . . , ψi(x))






((−∞, x]


12



ϕi : Cn −→ Ci

x 7→ (ψ1(x), . . . , ψi(x))






((−∞, x]


12



ϕi : Cn −→ Ci

x 7→ (ψ1(x), . . . , ψi(x))




Roadmap property RM:

For all connected components C of V

C ∩ (Fi ∪W (ϕi, V )) is non-empty and connected

12

How to use it

Assumptions to satisfy in the new result

(P) ϕ1 is a proper map bounded from below

(D) dimWi = i− 1 and dimFi = n− d+ 1

A prototype

Take ϕ as:

ϕ =

(n∑

i=1

(xi − ai)2 , bT2−→x , . . . , bT

n−→x)

where ai ∈ R, bi ∈ Rn

Then choose a generic (a, b2, . . . , bn) ∈ Rn2

Semi-algebraic sets

A strategy to tackle unbounded semi-algebraic sets:

f ∈ R[x1, . . . , xn]

u new variable

f 6= 0 −→ uf = 1

f ≥ 0 −→ f = u2

f ≤ 0 −→ f = −u2

13

How to use it




A prototype

Take ϕ as:

ϕ =

(n∑

i=1

(xi − ai)2 , bT2−→x , . . . , bT

n−→x)



Semi-algebraic sets


f ∈ R[x1, . . . , xn]

u new variable

f 6= 0 −→ uf = 1

f ≥ 0 −→ f = u2

f ≤ 0 −→ f = −u2

13

How to use it




A prototype

Take ϕ as:

ϕ =

(n∑

i=1

(xi − ai)2 , bT2−→x , . . . , bT

n−→x)



Semi-algebraic sets


f ∈ R[x1, . . . , xn]

u new variable

f 6= 0 −→ uf = 1

f ≥ 0 −→ f = u2

f ≤ 0 −→ f = −u2

13

How to use it




A prototype

Take ϕ as:

ϕ =

(n∑

i=1

(xi − ai)2 , bT2−→x , . . . , bT

n−→x)



Semi-algebraic sets


f ∈ R[x1, . . . , xn]

u new variable

f 6= 0 −→ uf = 1

f ≥ 0 −→ f = u2

f ≤ 0 −→ f = −u2

13

How to use it




A prototype

Take ϕ as:

ϕ =

(n∑

i=1

(xi − ai)2 , bT2−→x , . . . , bT

n−→x)



Semi-algebraic sets


f ∈ R[x1, . . . , xn]

u new variable

f 6= 0 −→ uf = 1

f ≥ 0 −→ f = u2

f ≤ 0 −→ f = −u2

13

Algorithm

Considering an algebraic set V ⊂ Cn with dimension d

(V, d) d-roadmap of V

i = 2 Bounded!

(W2, 1)⋃

(F2, d− 1) (d− 1)-roadmap of V

Theorem

F2 = ϕ−11 (ϕ1(K))

with K finite

Complexity?

RoadmapBounded(F2) = O((nD)6n log2 d

)degree(F2)≤ D

+ Computation of W2 and F2 << O((nD)6n log2 d

)Complexity conjecture = O

((nD)6n log2 d

)

14

Algorithm



i = 2

(W2, 1)⋃


[Safey El Din and Schost, 2017]

(W2, 1)⋃

(RF2 , 1) 1-roadmap of V

Theorem

RoadmapBounded

Complexity?


)degree(F2)≤ D



((nD)6n log2 d

)

14

Algorithm



i = 2

(W2, 1)⋃


[Safey El Din and Schost, 2017]

(W2, 1)⋃

(RF2 , 1) 1-roadmap of V

Theorem

RoadmapBounded

Complexity?


)degree(F2)≤ D



((nD)6n log2 d

)14

Conclusion

Summary

Compute a roadmap for the analysis of the kinematic

singularities of a puma-type robot

New result in the unbounded case

Dimension assumptions hold generically

An algorithm can be developed

Future work

Applications:

◦ Implement an algorithm for describing the geometry of roadmaps

◦ Describe the geometry of the singularity set of a puma-type robot

(joint work with P.Wenger)

◦ Analysis of the kinematic singularities of a puma-type robot

Theoretical aspects:

◦ Develop an algorithm for constructing roadmaps of smooth

unbounded algebraic sets

◦ Generalize the result to smooth unbounded semi-algebraic sets

Credits: figures have been made using TEXgraph and Maple

Thank you for your attention!

15

Conclusion

Summary

Compute a roadmap for the analysis of the kinematic

singularities of a puma-type robot

New result in the unbounded case

Dimension assumptions hold generically

An algorithm can be developed

Future work

Applications:

◦ Implement an algorithm for describing the geometry of roadmaps

◦ Describe the geometry of the singularity set of a puma-type robot

(joint work with P.Wenger)

◦ Analysis of the kinematic singularities of a puma-type robot

Theoretical aspects:

◦ Develop an algorithm for constructing roadmaps of smooth

unbounded algebraic sets

◦ Generalize the result to smooth unbounded semi-algebraic sets

Credits: figures have been made using TEXgraph and Maple

Thank you for your attention! 15

Documents

Towards faster roadmap algorithms for smooth and unbounded