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