Computing Singularity-Free Paths on the Configuration Manifold for Closed-Chain

Manipulators

O. Bohigas, L. Ros, M. Manubens, J. Porta

Institut de Robotica i Informatica IndustrialCSIC-UPC

Michael E. HendersonIBM T.J. Watson Research Center

Use local approximations of surface to solve a global problem

Continuation: Cover a surface

Optimization: Minimize a functional on a surface

Surface is feasible set (trust region methods)

Path planning: Find the shortest path between two points on a surface.

Surface is configuration space.

All can be posed as extending a “known” region from the boundary.

Chain manipulator. Closed Chain manipulator.

From Zlatanov, Bonev, Gosselin 2002

http://krafttelerobotics.com/

Open Chain manipulator. Closed Chain manipulator.

Graph is a tree Graph has cycles

Configuration Space

Φ(qIn , qOut)=0

g(qIn ,qOut)>0

If x's are the position of the joints, Φ=0 has algebraic constraints

(xi – xj) . (xi – xj) = Lij

Joint restrictions are not algebraic (rotations). xj = xi + Lij Rj(θ,φ)

Describes all possible positions (configuration space).

2

Configuration Space

Φ(qIn , qOut) = 0

g(qIn ,qOut) > 0

If x's are the position of the joints, Φ=0 has algebraic constraints

(xi – xj) . (xi – xj) = Lij

If not closed Φ can be put in lower triangular form (walk the tree).

2

qIn given, find qOut

http://kmoddl.library.cornell.edu/model.php?m=reuleaux

http://home.roadrunner.com/~trumpetb/loco/

Peaucellier–Lipkin linkage

Fairly simple linkages can have complicated configuration spaces

Steam engines/Watt's Linkage (These are 1 degree of freedom)

Klann:

Jansen:

Strandbeest: Theo Jansen

http://shuisman.com/?p=245

a b

c

How do you change inputs to move from here to there?

Without “problems”/Singularities

(E.g. when b changes can't controlwhich way the platform moves.)

b

Path Planning

Singular configurations

Φ(qIn

,qOut

) = 0

ΦIn

ΔqIn

+ ΦOut

ΔqOut

= 0

||ΔqIn

||2 + ||ΔqOut

||2= 1

ΔqIn

= 0ΔqOut

= 0 q

q

In

Out

RO/II

RI/IO

IIM

“Stuck“

Path Planning:

Given two points in configuration space O and Dfind a path through configuration space connecting them.

“Shortest”/”Lowest energy”/”Obstacle avoiding”

Kinematic/Dynamic.

O

D

Path Planning:

Given two points in configuration space O and Dfind a path through configuration space connecting them

“Shortest”/”Lowest energy”/”Obstacle avoiding”

Kinematic/Dynamic.

Distance is local (metric). Length of a path is not.

If dist(O,x) is known

dist(O,y) can be found

y near x

O

D

y

x

xT M

M

Path which

minimizes f [g].ds

Dijkstra's Algorithm

Find shortest path from O to D

Update the neighbors of O Mark O as “visited”

1

O

D

1

11

1

Find length of all paths

Dijkstra's Algorithm

Pick closest unvisited vertex X Update the unvisited neighbors of X Mark X as visited

X

Stop when D reached

Walk back to O

O

Dvisited region

dist(x,O) = const.

Dijkstra, nearest point to O

Abstract version

O

Dvisited region

dist(x,O) = const.

Dijkstra, nearest point to O

Rapidly-exploring Random Tree Closest point to a random point.

LaSalle

(needs some distanceestimate in ambient space)

RRT: Rapidly Exploring Random Tree

Abstract version

Porta, Jaillet

(needs some distanceestimate in ambient space)

O

Dvisited region

dist(x,O) = const.

Dijkstra, nearest point to O

Randomized, Directed"closest" point to a random point near D

α-RRT (Randomized, directed)

Abstract version

O

D

y

x

xT M

M

d(O,*) on configuration spaceFind a path on a surface Distance defied locally.

O

D

y

x

xT M

M

d(O,*) on configuration spaceFind a path on a surface Distance defied locally.

O

D

O

D

dist(*,O)=1

dist(*,O)=2

dist(*,O)=2

Using continuation to find shortest path.

Inside union know dist(O,*)

Using continuation to find shortest path.

O

D

O

Visited region is the union of spherical neighborhoods.

Need to find a point on the boundary.

δ ( A U B ) = ( δA W!B ) U ( δΒ W !A )

A B

δ ( A U B ) = ( δA W!B ) U ( δΒ W !A )

The boundary of the union of spheres

| x - x | = R

| x - x | > R

2 2

221 1

0 0

x0

x1

R 0

R 1

1 112

0 002

2

2

2

2

B

A

!B

δA

!B

δA !B

U

| x | - 2 x.x > R - | x | | x | - 2 x.x = R - | x |

δA

δ ( A U B ) = ( δA W!B ) U ( δΒ W !A )

The boundary of the union of spheres

| x - x | = R

| x - x | > R

2 2

221 1

0 0

x0

x1

R 0

R 1

1 112

0 002

2

2

2 2

0

2

1 0 1 1

2

0

2

2

B

A

!B

δA

!B

δA !B

U

| x | - 2 x.x > R - | x | | x | - 2 x.x = R - | x |

2 x.( x - x ) < R - R - | x | + | x |

δA

δ ( A U B ) = ( δA W!B ) U ( δΒ W !A )

The boundary of the union of spheres

x0

x1

R 0

R 1

B

A P

δA !B

U

δ ( A U B ) = ( δA W!B ) U ( δΒ W !A )

P = C

U

H01

U

H02

U

H030

U

H04

x0

x1

R 0

R 1

B

A

P0

Finite, convex

The boundary of the union of spheres

x0

x1

R 0

x2p0

p1

p2

p3

On boundary ↔ on sphere and in P

Keep a simple list of x,R,P

Choose one with a vertex of P |v-x|>R

The boundary of the union of spheres

Restricted Laguerre-Voronoi diagram

On a curved configuration space.

Avoiding singularities

F(u)=0

b = 1/μ(u) indicator function μ(u) e.g det(Fu(u))

In (u,b) space singularities get mapped to infinity

b

uu

The pivots lie on an equilateral triangle with side 2.35Each leg is two links, lengths 1 and 1.35Platform is an equilateral triangle with side 1.2

a1

a2

a3

θp

Example: “3-RRR” planar manipulator.

Two angles for each leg, + p and θ

6 Eq. 9 Unk.

a1

a2

a3

θp

2.35

a1 a

2

a3

θ

θ

θ

2.35

2.35

2.35

a1 a

2

a3

2.35

2.35

pa

1 a2

3

θ

a

a1 a

2

a3

p

A projection of configuration space for 3-RRR

θ=0

Singularities

a1 a

2

a3

p

Singular when thethree legs cross at a point

Eight different sets/way legs bend.

a1 a

2

a3

p

Fold when one leg is straight.

Summary

To find a non-singular path from one configuration to another.

Work with (u,b) with b=1/det.

Use a directed version of Dijkstra's algorithm on a surface

Visited region is a union of spherical balls in the tangent space.

Store u, tangent space T, radius R, and polyhedron P

Choose a point on the boundary “nearest” the destination.

Point on sphere, inside polyhedron.

Update polyhedra by subtracting half spaces.

Bohigas, H, Ros, Manubens, Porta

Planning Singularity-free Paths on Closed-Chain Manipulators

To appear, IEEE Transactions on Robotics