Computing Singularity-Free Paths on the Configuration ... E Henderson.pdfComputing Singularity-Free...
Transcript of Computing Singularity-Free Paths on the Configuration ... E Henderson.pdfComputing Singularity-Free...
![Page 1: Computing Singularity-Free Paths on the Configuration ... E Henderson.pdfComputing Singularity-Free Paths on the Configuration Manifold for Closed-Chain Manipulators O. Bohigas, L.](https://reader033.fdocuments.in/reader033/viewer/2022041712/5e48c5de0e20aa111540e4b1/html5/thumbnails/1.jpg)
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
![Page 2: Computing Singularity-Free Paths on the Configuration ... E Henderson.pdfComputing Singularity-Free Paths on the Configuration Manifold for Closed-Chain Manipulators O. Bohigas, L.](https://reader033.fdocuments.in/reader033/viewer/2022041712/5e48c5de0e20aa111540e4b1/html5/thumbnails/2.jpg)
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.
![Page 3: Computing Singularity-Free Paths on the Configuration ... E Henderson.pdfComputing Singularity-Free Paths on the Configuration Manifold for Closed-Chain Manipulators O. Bohigas, L.](https://reader033.fdocuments.in/reader033/viewer/2022041712/5e48c5de0e20aa111540e4b1/html5/thumbnails/3.jpg)
Chain manipulator. Closed Chain manipulator.
From Zlatanov, Bonev, Gosselin 2002
http://krafttelerobotics.com/
![Page 4: Computing Singularity-Free Paths on the Configuration ... E Henderson.pdfComputing Singularity-Free Paths on the Configuration Manifold for Closed-Chain Manipulators O. Bohigas, L.](https://reader033.fdocuments.in/reader033/viewer/2022041712/5e48c5de0e20aa111540e4b1/html5/thumbnails/4.jpg)
Open Chain manipulator. Closed Chain manipulator.
Graph is a tree Graph has cycles
![Page 5: Computing Singularity-Free Paths on the Configuration ... E Henderson.pdfComputing Singularity-Free Paths on the Configuration Manifold for Closed-Chain Manipulators O. Bohigas, L.](https://reader033.fdocuments.in/reader033/viewer/2022041712/5e48c5de0e20aa111540e4b1/html5/thumbnails/5.jpg)
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
![Page 6: Computing Singularity-Free Paths on the Configuration ... E Henderson.pdfComputing Singularity-Free Paths on the Configuration Manifold for Closed-Chain Manipulators O. Bohigas, L.](https://reader033.fdocuments.in/reader033/viewer/2022041712/5e48c5de0e20aa111540e4b1/html5/thumbnails/6.jpg)
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
![Page 7: Computing Singularity-Free Paths on the Configuration ... E Henderson.pdfComputing Singularity-Free Paths on the Configuration Manifold for Closed-Chain Manipulators O. Bohigas, L.](https://reader033.fdocuments.in/reader033/viewer/2022041712/5e48c5de0e20aa111540e4b1/html5/thumbnails/7.jpg)
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
![Page 8: Computing Singularity-Free Paths on the Configuration ... E Henderson.pdfComputing Singularity-Free Paths on the Configuration Manifold for Closed-Chain Manipulators O. Bohigas, L.](https://reader033.fdocuments.in/reader033/viewer/2022041712/5e48c5de0e20aa111540e4b1/html5/thumbnails/8.jpg)
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
![Page 9: Computing Singularity-Free Paths on the Configuration ... E Henderson.pdfComputing Singularity-Free Paths on the Configuration Manifold for Closed-Chain Manipulators O. Bohigas, L.](https://reader033.fdocuments.in/reader033/viewer/2022041712/5e48c5de0e20aa111540e4b1/html5/thumbnails/9.jpg)
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“
![Page 10: Computing Singularity-Free Paths on the Configuration ... E Henderson.pdfComputing Singularity-Free Paths on the Configuration Manifold for Closed-Chain Manipulators O. Bohigas, L.](https://reader033.fdocuments.in/reader033/viewer/2022041712/5e48c5de0e20aa111540e4b1/html5/thumbnails/10.jpg)
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
![Page 11: Computing Singularity-Free Paths on the Configuration ... E Henderson.pdfComputing Singularity-Free Paths on the Configuration Manifold for Closed-Chain Manipulators O. Bohigas, L.](https://reader033.fdocuments.in/reader033/viewer/2022041712/5e48c5de0e20aa111540e4b1/html5/thumbnails/11.jpg)
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
![Page 12: Computing Singularity-Free Paths on the Configuration ... E Henderson.pdfComputing Singularity-Free Paths on the Configuration Manifold for Closed-Chain Manipulators O. Bohigas, L.](https://reader033.fdocuments.in/reader033/viewer/2022041712/5e48c5de0e20aa111540e4b1/html5/thumbnails/12.jpg)
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
![Page 13: Computing Singularity-Free Paths on the Configuration ... E Henderson.pdfComputing Singularity-Free Paths on the Configuration Manifold for Closed-Chain Manipulators O. Bohigas, L.](https://reader033.fdocuments.in/reader033/viewer/2022041712/5e48c5de0e20aa111540e4b1/html5/thumbnails/13.jpg)
Dijkstra's Algorithm
Pick closest unvisited vertex X Update the unvisited neighbors of X Mark X as visited
X
![Page 14: Computing Singularity-Free Paths on the Configuration ... E Henderson.pdfComputing Singularity-Free Paths on the Configuration Manifold for Closed-Chain Manipulators O. Bohigas, L.](https://reader033.fdocuments.in/reader033/viewer/2022041712/5e48c5de0e20aa111540e4b1/html5/thumbnails/14.jpg)
Stop when D reached
![Page 15: Computing Singularity-Free Paths on the Configuration ... E Henderson.pdfComputing Singularity-Free Paths on the Configuration Manifold for Closed-Chain Manipulators O. Bohigas, L.](https://reader033.fdocuments.in/reader033/viewer/2022041712/5e48c5de0e20aa111540e4b1/html5/thumbnails/15.jpg)
Walk back to O
![Page 16: Computing Singularity-Free Paths on the Configuration ... E Henderson.pdfComputing Singularity-Free Paths on the Configuration Manifold for Closed-Chain Manipulators O. Bohigas, L.](https://reader033.fdocuments.in/reader033/viewer/2022041712/5e48c5de0e20aa111540e4b1/html5/thumbnails/16.jpg)
O
Dvisited region
dist(x,O) = const.
Dijkstra, nearest point to O
Abstract version
![Page 17: Computing Singularity-Free Paths on the Configuration ... E Henderson.pdfComputing Singularity-Free Paths on the Configuration Manifold for Closed-Chain Manipulators O. Bohigas, L.](https://reader033.fdocuments.in/reader033/viewer/2022041712/5e48c5de0e20aa111540e4b1/html5/thumbnails/17.jpg)
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
![Page 18: Computing Singularity-Free Paths on the Configuration ... E Henderson.pdfComputing Singularity-Free Paths on the Configuration Manifold for Closed-Chain Manipulators O. Bohigas, L.](https://reader033.fdocuments.in/reader033/viewer/2022041712/5e48c5de0e20aa111540e4b1/html5/thumbnails/18.jpg)
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
![Page 19: Computing Singularity-Free Paths on the Configuration ... E Henderson.pdfComputing Singularity-Free Paths on the Configuration Manifold for Closed-Chain Manipulators O. Bohigas, L.](https://reader033.fdocuments.in/reader033/viewer/2022041712/5e48c5de0e20aa111540e4b1/html5/thumbnails/19.jpg)
O
D
y
x
xT M
M
d(O,*) on configuration spaceFind a path on a surface Distance defied locally.
![Page 20: Computing Singularity-Free Paths on the Configuration ... E Henderson.pdfComputing Singularity-Free Paths on the Configuration Manifold for Closed-Chain Manipulators O. Bohigas, L.](https://reader033.fdocuments.in/reader033/viewer/2022041712/5e48c5de0e20aa111540e4b1/html5/thumbnails/20.jpg)
O
D
y
x
xT M
M
d(O,*) on configuration spaceFind a path on a surface Distance defied locally.
O
D
![Page 21: Computing Singularity-Free Paths on the Configuration ... E Henderson.pdfComputing Singularity-Free Paths on the Configuration Manifold for Closed-Chain Manipulators O. Bohigas, L.](https://reader033.fdocuments.in/reader033/viewer/2022041712/5e48c5de0e20aa111540e4b1/html5/thumbnails/21.jpg)
O
D
dist(*,O)=1
dist(*,O)=2
dist(*,O)=2
Using continuation to find shortest path.
Inside union know dist(O,*)
![Page 22: Computing Singularity-Free Paths on the Configuration ... E Henderson.pdfComputing Singularity-Free Paths on the Configuration Manifold for Closed-Chain Manipulators O. Bohigas, L.](https://reader033.fdocuments.in/reader033/viewer/2022041712/5e48c5de0e20aa111540e4b1/html5/thumbnails/22.jpg)
Using continuation to find shortest path.
O
D
![Page 23: Computing Singularity-Free Paths on the Configuration ... E Henderson.pdfComputing Singularity-Free Paths on the Configuration Manifold for Closed-Chain Manipulators O. Bohigas, L.](https://reader033.fdocuments.in/reader033/viewer/2022041712/5e48c5de0e20aa111540e4b1/html5/thumbnails/23.jpg)
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
![Page 24: Computing Singularity-Free Paths on the Configuration ... E Henderson.pdfComputing Singularity-Free Paths on the Configuration Manifold for Closed-Chain Manipulators O. Bohigas, L.](https://reader033.fdocuments.in/reader033/viewer/2022041712/5e48c5de0e20aa111540e4b1/html5/thumbnails/24.jpg)
δ ( 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
![Page 25: Computing Singularity-Free Paths on the Configuration ... E Henderson.pdfComputing Singularity-Free Paths on the Configuration Manifold for Closed-Chain Manipulators O. Bohigas, L.](https://reader033.fdocuments.in/reader033/viewer/2022041712/5e48c5de0e20aa111540e4b1/html5/thumbnails/25.jpg)
δ ( 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
![Page 26: Computing Singularity-Free Paths on the Configuration ... E Henderson.pdfComputing Singularity-Free Paths on the Configuration Manifold for Closed-Chain Manipulators O. Bohigas, L.](https://reader033.fdocuments.in/reader033/viewer/2022041712/5e48c5de0e20aa111540e4b1/html5/thumbnails/26.jpg)
δ ( 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
![Page 27: Computing Singularity-Free Paths on the Configuration ... E Henderson.pdfComputing Singularity-Free Paths on the Configuration Manifold for Closed-Chain Manipulators O. Bohigas, L.](https://reader033.fdocuments.in/reader033/viewer/2022041712/5e48c5de0e20aa111540e4b1/html5/thumbnails/27.jpg)
δ ( 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
![Page 28: Computing Singularity-Free Paths on the Configuration ... E Henderson.pdfComputing Singularity-Free Paths on the Configuration Manifold for Closed-Chain Manipulators O. Bohigas, L.](https://reader033.fdocuments.in/reader033/viewer/2022041712/5e48c5de0e20aa111540e4b1/html5/thumbnails/28.jpg)
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
![Page 29: Computing Singularity-Free Paths on the Configuration ... E Henderson.pdfComputing Singularity-Free Paths on the Configuration Manifold for Closed-Chain Manipulators O. Bohigas, L.](https://reader033.fdocuments.in/reader033/viewer/2022041712/5e48c5de0e20aa111540e4b1/html5/thumbnails/29.jpg)
On a curved configuration space.
![Page 30: Computing Singularity-Free Paths on the Configuration ... E Henderson.pdfComputing Singularity-Free Paths on the Configuration Manifold for Closed-Chain Manipulators O. Bohigas, L.](https://reader033.fdocuments.in/reader033/viewer/2022041712/5e48c5de0e20aa111540e4b1/html5/thumbnails/30.jpg)
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
![Page 31: Computing Singularity-Free Paths on the Configuration ... E Henderson.pdfComputing Singularity-Free Paths on the Configuration Manifold for Closed-Chain Manipulators O. Bohigas, L.](https://reader033.fdocuments.in/reader033/viewer/2022041712/5e48c5de0e20aa111540e4b1/html5/thumbnails/31.jpg)
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.
![Page 32: Computing Singularity-Free Paths on the Configuration ... E Henderson.pdfComputing Singularity-Free Paths on the Configuration Manifold for Closed-Chain Manipulators O. Bohigas, L.](https://reader033.fdocuments.in/reader033/viewer/2022041712/5e48c5de0e20aa111540e4b1/html5/thumbnails/32.jpg)
Two angles for each leg, + p and θ
6 Eq. 9 Unk.
a1
a2
a3
θp
![Page 33: Computing Singularity-Free Paths on the Configuration ... E Henderson.pdfComputing Singularity-Free Paths on the Configuration Manifold for Closed-Chain Manipulators O. Bohigas, L.](https://reader033.fdocuments.in/reader033/viewer/2022041712/5e48c5de0e20aa111540e4b1/html5/thumbnails/33.jpg)
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
![Page 34: Computing Singularity-Free Paths on the Configuration ... E Henderson.pdfComputing Singularity-Free Paths on the Configuration Manifold for Closed-Chain Manipulators O. Bohigas, L.](https://reader033.fdocuments.in/reader033/viewer/2022041712/5e48c5de0e20aa111540e4b1/html5/thumbnails/34.jpg)
θ=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.
![Page 35: Computing Singularity-Free Paths on the Configuration ... E Henderson.pdfComputing Singularity-Free Paths on the Configuration Manifold for Closed-Chain Manipulators O. Bohigas, L.](https://reader033.fdocuments.in/reader033/viewer/2022041712/5e48c5de0e20aa111540e4b1/html5/thumbnails/35.jpg)
![Page 36: Computing Singularity-Free Paths on the Configuration ... E Henderson.pdfComputing Singularity-Free Paths on the Configuration Manifold for Closed-Chain Manipulators O. Bohigas, L.](https://reader033.fdocuments.in/reader033/viewer/2022041712/5e48c5de0e20aa111540e4b1/html5/thumbnails/36.jpg)
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