![Page 1: Planning Sequences of Motion Primitives for humanoid robotsgamma.cs.unc.edu/PROBOT/Lamiraux.pdf · 2011. 5. 24. · Planning Sequences of Motion Primitives for humanoid robots Florent](https://reader035.vdocuments.us/reader035/viewer/2022070219/612ff9411ecc51586943cb9b/html5/thumbnails/1.jpg)
Planning Sequences of Motion Primitivesfor humanoid robots
Florent Lamiraux, Sebastien Dalibard, Alireza Nakhaei and J.-P.Laumond
LAAS-CNRS
May 9, 2011
Florent Lamiraux (LAAS) Planning Sequences of Motion Primitives May 9, 2011 1 / 36
![Page 2: Planning Sequences of Motion Primitives for humanoid robotsgamma.cs.unc.edu/PROBOT/Lamiraux.pdf · 2011. 5. 24. · Planning Sequences of Motion Primitives for humanoid robots Florent](https://reader035.vdocuments.us/reader035/viewer/2022070219/612ff9411ecc51586943cb9b/html5/thumbnails/2.jpg)
Objective
Plan motions for humanoid robot with manipulation of simpleobjects like:
I doors,I windows,I drawers.
Execute these motions using sensor feedback control.
Florent Lamiraux (LAAS) Planning Sequences of Motion Primitives May 9, 2011 2 / 36
![Page 3: Planning Sequences of Motion Primitives for humanoid robotsgamma.cs.unc.edu/PROBOT/Lamiraux.pdf · 2011. 5. 24. · Planning Sequences of Motion Primitives for humanoid robots Florent](https://reader035.vdocuments.us/reader035/viewer/2022070219/612ff9411ecc51586943cb9b/html5/thumbnails/3.jpg)
Objective
Plan motions for humanoid robot with manipulation of simpleobjects like:
I doors,I windows,I drawers.
Execute these motions using sensor feedback control.
Florent Lamiraux (LAAS) Planning Sequences of Motion Primitives May 9, 2011 2 / 36
![Page 4: Planning Sequences of Motion Primitives for humanoid robotsgamma.cs.unc.edu/PROBOT/Lamiraux.pdf · 2011. 5. 24. · Planning Sequences of Motion Primitives for humanoid robots Florent](https://reader035.vdocuments.us/reader035/viewer/2022070219/612ff9411ecc51586943cb9b/html5/thumbnails/4.jpg)
Approach
Reduce complexity of manipulation planning:I “documented object”
Plan motions as sequences of motion primitives
Florent Lamiraux (LAAS) Planning Sequences of Motion Primitives May 9, 2011 3 / 36
![Page 5: Planning Sequences of Motion Primitives for humanoid robotsgamma.cs.unc.edu/PROBOT/Lamiraux.pdf · 2011. 5. 24. · Planning Sequences of Motion Primitives for humanoid robots Florent](https://reader035.vdocuments.us/reader035/viewer/2022070219/612ff9411ecc51586943cb9b/html5/thumbnails/5.jpg)
Motion primitive
DefinitionI Motion produced by a controller.
ExamplesI walking along a curve,I walking on foot prints,I reaching with a hand.
Florent Lamiraux (LAAS) Planning Sequences of Motion Primitives May 9, 2011 4 / 36
![Page 6: Planning Sequences of Motion Primitives for humanoid robotsgamma.cs.unc.edu/PROBOT/Lamiraux.pdf · 2011. 5. 24. · Planning Sequences of Motion Primitives for humanoid robots Florent](https://reader035.vdocuments.us/reader035/viewer/2022070219/612ff9411ecc51586943cb9b/html5/thumbnails/6.jpg)
Documented objects
Manipulating some objects requires some knowledgeI opening a door requires
F to grasp the handle,F to turn the handle,F to pull the handle along a circular path,F to cross the doorway,F to grasp the other handle and release the first one,F to pull the handle along a circular path,F to turn the handle.
We put this knowlegde into the object thus defining the notion ofI documented object.
For each object, motion primitives manipulating the object can beprecomputed and inserted in a global roadmap.
Florent Lamiraux (LAAS) Planning Sequences of Motion Primitives May 9, 2011 5 / 36
![Page 7: Planning Sequences of Motion Primitives for humanoid robotsgamma.cs.unc.edu/PROBOT/Lamiraux.pdf · 2011. 5. 24. · Planning Sequences of Motion Primitives for humanoid robots Florent](https://reader035.vdocuments.us/reader035/viewer/2022070219/612ff9411ecc51586943cb9b/html5/thumbnails/7.jpg)
Documented objects
Manipulating some objects requires some knowledgeI opening a door requires
F to grasp the handle,F to turn the handle,F to pull the handle along a circular path,F to cross the doorway,F to grasp the other handle and release the first one,F to pull the handle along a circular path,F to turn the handle.
We put this knowlegde into the object thus defining the notion ofI documented object.
For each object, motion primitives manipulating the object can beprecomputed and inserted in a global roadmap.
Florent Lamiraux (LAAS) Planning Sequences of Motion Primitives May 9, 2011 5 / 36
![Page 8: Planning Sequences of Motion Primitives for humanoid robotsgamma.cs.unc.edu/PROBOT/Lamiraux.pdf · 2011. 5. 24. · Planning Sequences of Motion Primitives for humanoid robots Florent](https://reader035.vdocuments.us/reader035/viewer/2022070219/612ff9411ecc51586943cb9b/html5/thumbnails/8.jpg)
Documented objects
Manipulating some objects requires some knowledgeI opening a door requires
F to grasp the handle,F to turn the handle,F to pull the handle along a circular path,F to cross the doorway,F to grasp the other handle and release the first one,F to pull the handle along a circular path,F to turn the handle.
We put this knowlegde into the object thus defining the notion ofI documented object.
For each object, motion primitives manipulating the object can beprecomputed and inserted in a global roadmap.
Florent Lamiraux (LAAS) Planning Sequences of Motion Primitives May 9, 2011 5 / 36
![Page 9: Planning Sequences of Motion Primitives for humanoid robotsgamma.cs.unc.edu/PROBOT/Lamiraux.pdf · 2011. 5. 24. · Planning Sequences of Motion Primitives for humanoid robots Florent](https://reader035.vdocuments.us/reader035/viewer/2022070219/612ff9411ecc51586943cb9b/html5/thumbnails/9.jpg)
Documented objects
Manipulating some objects requires some knowledgeI opening a door requires
F to grasp the handle,F to turn the handle,F to pull the handle along a circular path,F to cross the doorway,F to grasp the other handle and release the first one,F to pull the handle along a circular path,F to turn the handle.
We put this knowlegde into the object thus defining the notion ofI documented object.
For each object, motion primitives manipulating the object can beprecomputed and inserted in a global roadmap.
Florent Lamiraux (LAAS) Planning Sequences of Motion Primitives May 9, 2011 5 / 36
![Page 10: Planning Sequences of Motion Primitives for humanoid robotsgamma.cs.unc.edu/PROBOT/Lamiraux.pdf · 2011. 5. 24. · Planning Sequences of Motion Primitives for humanoid robots Florent](https://reader035.vdocuments.us/reader035/viewer/2022070219/612ff9411ecc51586943cb9b/html5/thumbnails/10.jpg)
Documented objects
Manipulating some objects requires some knowledgeI opening a door requires
F to grasp the handle,F to turn the handle,F to pull the handle along a circular path,F to cross the doorway,F to grasp the other handle and release the first one,F to pull the handle along a circular path,F to turn the handle.
We put this knowlegde into the object thus defining the notion ofI documented object.
For each object, motion primitives manipulating the object can beprecomputed and inserted in a global roadmap.
Florent Lamiraux (LAAS) Planning Sequences of Motion Primitives May 9, 2011 5 / 36
![Page 11: Planning Sequences of Motion Primitives for humanoid robotsgamma.cs.unc.edu/PROBOT/Lamiraux.pdf · 2011. 5. 24. · Planning Sequences of Motion Primitives for humanoid robots Florent](https://reader035.vdocuments.us/reader035/viewer/2022070219/612ff9411ecc51586943cb9b/html5/thumbnails/11.jpg)
Documented objects
Manipulating some objects requires some knowledgeI opening a door requires
F to grasp the handle,F to turn the handle,F to pull the handle along a circular path,F to cross the doorway,F to grasp the other handle and release the first one,F to pull the handle along a circular path,F to turn the handle.
We put this knowlegde into the object thus defining the notion ofI documented object.
For each object, motion primitives manipulating the object can beprecomputed and inserted in a global roadmap.
Florent Lamiraux (LAAS) Planning Sequences of Motion Primitives May 9, 2011 5 / 36
![Page 12: Planning Sequences of Motion Primitives for humanoid robotsgamma.cs.unc.edu/PROBOT/Lamiraux.pdf · 2011. 5. 24. · Planning Sequences of Motion Primitives for humanoid robots Florent](https://reader035.vdocuments.us/reader035/viewer/2022070219/612ff9411ecc51586943cb9b/html5/thumbnails/12.jpg)
Documented objects
Manipulating some objects requires some knowledgeI opening a door requires
F to grasp the handle,F to turn the handle,F to pull the handle along a circular path,F to cross the doorway,F to grasp the other handle and release the first one,F to pull the handle along a circular path,F to turn the handle.
We put this knowlegde into the object thus defining the notion ofI documented object.
For each object, motion primitives manipulating the object can beprecomputed and inserted in a global roadmap.
Florent Lamiraux (LAAS) Planning Sequences of Motion Primitives May 9, 2011 5 / 36
![Page 13: Planning Sequences of Motion Primitives for humanoid robotsgamma.cs.unc.edu/PROBOT/Lamiraux.pdf · 2011. 5. 24. · Planning Sequences of Motion Primitives for humanoid robots Florent](https://reader035.vdocuments.us/reader035/viewer/2022070219/612ff9411ecc51586943cb9b/html5/thumbnails/13.jpg)
Documented objects
Manipulating some objects requires some knowledgeI opening a door requires
F to grasp the handle,F to turn the handle,F to pull the handle along a circular path,F to cross the doorway,F to grasp the other handle and release the first one,F to pull the handle along a circular path,F to turn the handle.
We put this knowlegde into the object thus defining the notion ofI documented object.
For each object, motion primitives manipulating the object can beprecomputed and inserted in a global roadmap.
Florent Lamiraux (LAAS) Planning Sequences of Motion Primitives May 9, 2011 5 / 36
![Page 14: Planning Sequences of Motion Primitives for humanoid robotsgamma.cs.unc.edu/PROBOT/Lamiraux.pdf · 2011. 5. 24. · Planning Sequences of Motion Primitives for humanoid robots Florent](https://reader035.vdocuments.us/reader035/viewer/2022070219/612ff9411ecc51586943cb9b/html5/thumbnails/14.jpg)
Documented objects
Manipulating some objects requires some knowledgeI opening a door requires
F to grasp the handle,F to turn the handle,F to pull the handle along a circular path,F to cross the doorway,F to grasp the other handle and release the first one,F to pull the handle along a circular path,F to turn the handle.
We put this knowlegde into the object thus defining the notion ofI documented object.
For each object, motion primitives manipulating the object can beprecomputed and inserted in a global roadmap.
Florent Lamiraux (LAAS) Planning Sequences of Motion Primitives May 9, 2011 5 / 36
![Page 15: Planning Sequences of Motion Primitives for humanoid robotsgamma.cs.unc.edu/PROBOT/Lamiraux.pdf · 2011. 5. 24. · Planning Sequences of Motion Primitives for humanoid robots Florent](https://reader035.vdocuments.us/reader035/viewer/2022070219/612ff9411ecc51586943cb9b/html5/thumbnails/15.jpg)
Documented objects
Manipulating some objects requires some knowledgeI opening a door requires
F to grasp the handle,F to turn the handle,F to pull the handle along a circular path,F to cross the doorway,F to grasp the other handle and release the first one,F to pull the handle along a circular path,F to turn the handle.
We put this knowlegde into the object thus defining the notion ofI documented object.
For each object, motion primitives manipulating the object can beprecomputed and inserted in a global roadmap.
Florent Lamiraux (LAAS) Planning Sequences of Motion Primitives May 9, 2011 5 / 36
![Page 16: Planning Sequences of Motion Primitives for humanoid robotsgamma.cs.unc.edu/PROBOT/Lamiraux.pdf · 2011. 5. 24. · Planning Sequences of Motion Primitives for humanoid robots Florent](https://reader035.vdocuments.us/reader035/viewer/2022070219/612ff9411ecc51586943cb9b/html5/thumbnails/16.jpg)
Example: going through a door
Ideally, the knowledge is stored into the object as a sequence oftime-varying tasks.Our implementation:
I system: robot bounding box + door (3+1 dof),I algorithm: plan a sequence of motions satisfying successive
constraints:1 no constraint,2 left hand is close to the handle,3 previous constraint + right hand close to other handle,4 right hand close to other handle,5 no constraint
I generate step sequence along box path,I build a whole-body motion defined by
F step sequence,F grasping constraints.
Florent Lamiraux (LAAS) Planning Sequences of Motion Primitives May 9, 2011 6 / 36
![Page 17: Planning Sequences of Motion Primitives for humanoid robotsgamma.cs.unc.edu/PROBOT/Lamiraux.pdf · 2011. 5. 24. · Planning Sequences of Motion Primitives for humanoid robots Florent](https://reader035.vdocuments.us/reader035/viewer/2022070219/612ff9411ecc51586943cb9b/html5/thumbnails/17.jpg)
Example: going through a door
Ideally, the knowledge is stored into the object as a sequence oftime-varying tasks.Our implementation:
I system: robot bounding box + door (3+1 dof),I algorithm: plan a sequence of motions satisfying successive
constraints:1 no constraint,2 left hand is close to the handle,3 previous constraint + right hand close to other handle,4 right hand close to other handle,5 no constraint
I generate step sequence along box path,I build a whole-body motion defined by
F step sequence,F grasping constraints.
Florent Lamiraux (LAAS) Planning Sequences of Motion Primitives May 9, 2011 6 / 36
![Page 18: Planning Sequences of Motion Primitives for humanoid robotsgamma.cs.unc.edu/PROBOT/Lamiraux.pdf · 2011. 5. 24. · Planning Sequences of Motion Primitives for humanoid robots Florent](https://reader035.vdocuments.us/reader035/viewer/2022070219/612ff9411ecc51586943cb9b/html5/thumbnails/18.jpg)
Example: going through a door
Ideally, the knowledge is stored into the object as a sequence oftime-varying tasks.Our implementation:
I system: robot bounding box + door (3+1 dof),I algorithm: plan a sequence of motions satisfying successive
constraints:1 no constraint,2 left hand is close to the handle,3 previous constraint + right hand close to other handle,4 right hand close to other handle,5 no constraint
I generate step sequence along box path,I build a whole-body motion defined by
F step sequence,F grasping constraints.
Florent Lamiraux (LAAS) Planning Sequences of Motion Primitives May 9, 2011 6 / 36
![Page 19: Planning Sequences of Motion Primitives for humanoid robotsgamma.cs.unc.edu/PROBOT/Lamiraux.pdf · 2011. 5. 24. · Planning Sequences of Motion Primitives for humanoid robots Florent](https://reader035.vdocuments.us/reader035/viewer/2022070219/612ff9411ecc51586943cb9b/html5/thumbnails/19.jpg)
Example: going through a door
Ideally, the knowledge is stored into the object as a sequence oftime-varying tasks.Our implementation:
I system: robot bounding box + door (3+1 dof),I algorithm: plan a sequence of motions satisfying successive
constraints:1 no constraint,2 left hand is close to the handle,3 previous constraint + right hand close to other handle,4 right hand close to other handle,5 no constraint
I generate step sequence along box path,I build a whole-body motion defined by
F step sequence,F grasping constraints.
Florent Lamiraux (LAAS) Planning Sequences of Motion Primitives May 9, 2011 6 / 36
![Page 20: Planning Sequences of Motion Primitives for humanoid robotsgamma.cs.unc.edu/PROBOT/Lamiraux.pdf · 2011. 5. 24. · Planning Sequences of Motion Primitives for humanoid robots Florent](https://reader035.vdocuments.us/reader035/viewer/2022070219/612ff9411ecc51586943cb9b/html5/thumbnails/20.jpg)
Example: going through a door
Ideally, the knowledge is stored into the object as a sequence oftime-varying tasks.Our implementation:
I system: robot bounding box + door (3+1 dof),I algorithm: plan a sequence of motions satisfying successive
constraints:1 no constraint,2 left hand is close to the handle,3 previous constraint + right hand close to other handle,4 right hand close to other handle,5 no constraint
I generate step sequence along box path,I build a whole-body motion defined by
F step sequence,F grasping constraints.
Florent Lamiraux (LAAS) Planning Sequences of Motion Primitives May 9, 2011 6 / 36
![Page 21: Planning Sequences of Motion Primitives for humanoid robotsgamma.cs.unc.edu/PROBOT/Lamiraux.pdf · 2011. 5. 24. · Planning Sequences of Motion Primitives for humanoid robots Florent](https://reader035.vdocuments.us/reader035/viewer/2022070219/612ff9411ecc51586943cb9b/html5/thumbnails/21.jpg)
Motion Planning constraintDefinition: sub-manifold or region of the configuration space.Sampling: projection function from configuration space to domainsatisfying the constraint.
projection
I projection functions move the bounding box in order to put doorhandles into reaching area of robot hands.
Florent Lamiraux (LAAS) Planning Sequences of Motion Primitives May 9, 2011 7 / 36
![Page 22: Planning Sequences of Motion Primitives for humanoid robotsgamma.cs.unc.edu/PROBOT/Lamiraux.pdf · 2011. 5. 24. · Planning Sequences of Motion Primitives for humanoid robots Florent](https://reader035.vdocuments.us/reader035/viewer/2022070219/612ff9411ecc51586943cb9b/html5/thumbnails/22.jpg)
Motion Planning constraintDefinition: sub-manifold or region of the configuration space.Sampling: projection function from configuration space to domainsatisfying the constraint.
projection
I projection functions move the bounding box in order to put doorhandles into reaching area of robot hands.
Florent Lamiraux (LAAS) Planning Sequences of Motion Primitives May 9, 2011 7 / 36
![Page 23: Planning Sequences of Motion Primitives for humanoid robotsgamma.cs.unc.edu/PROBOT/Lamiraux.pdf · 2011. 5. 24. · Planning Sequences of Motion Primitives for humanoid robots Florent](https://reader035.vdocuments.us/reader035/viewer/2022070219/612ff9411ecc51586943cb9b/html5/thumbnails/23.jpg)
Motion Planning constraintDefinition: sub-manifold or region of the configuration space.Sampling: projection function from configuration space to domainsatisfying the constraint.
projection
I projection functions move the bounding box in order to put doorhandles into reaching area of robot hands.
Florent Lamiraux (LAAS) Planning Sequences of Motion Primitives May 9, 2011 7 / 36
![Page 24: Planning Sequences of Motion Primitives for humanoid robotsgamma.cs.unc.edu/PROBOT/Lamiraux.pdf · 2011. 5. 24. · Planning Sequences of Motion Primitives for humanoid robots Florent](https://reader035.vdocuments.us/reader035/viewer/2022070219/612ff9411ecc51586943cb9b/html5/thumbnails/24.jpg)
Going through a door: constraint transition graph
Awayfrom Door
Hold Doorwith Right
Hand
Hold Doorwith Both
Hands
Hold Doorwith Left
Hand
Florent Lamiraux (LAAS) Planning Sequences of Motion Primitives May 9, 2011 8 / 36
![Page 25: Planning Sequences of Motion Primitives for humanoid robotsgamma.cs.unc.edu/PROBOT/Lamiraux.pdf · 2011. 5. 24. · Planning Sequences of Motion Primitives for humanoid robots Florent](https://reader035.vdocuments.us/reader035/viewer/2022070219/612ff9411ecc51586943cb9b/html5/thumbnails/25.jpg)
Going through a door: algorithm
Configuration space:SE(2)× [αmin, αmax ]× {free, left hand, right hand,both hands}Classical RRT algorithm with dedicated methods
I steering method: connect two configurations ifF states are adjacent in constraint graph,F motion of object is consistent,F enforce weaker constraint,
I distance functionF return ∞ when two configurations cannot be connected,
I random configuration shooter: sample at intersections of constraintmanifolds.
Florent Lamiraux (LAAS) Planning Sequences of Motion Primitives May 9, 2011 9 / 36
![Page 26: Planning Sequences of Motion Primitives for humanoid robotsgamma.cs.unc.edu/PROBOT/Lamiraux.pdf · 2011. 5. 24. · Planning Sequences of Motion Primitives for humanoid robots Florent](https://reader035.vdocuments.us/reader035/viewer/2022070219/612ff9411ecc51586943cb9b/html5/thumbnails/26.jpg)
Going through a door: algorithm
Configuration space:SE(2)× [αmin, αmax ]× {free, left hand, right hand,both hands}Classical RRT algorithm with dedicated methods
I steering method: connect two configurations ifF states are adjacent in constraint graph,F motion of object is consistent,F enforce weaker constraint,
I distance functionF return ∞ when two configurations cannot be connected,
I random configuration shooter: sample at intersections of constraintmanifolds.
Florent Lamiraux (LAAS) Planning Sequences of Motion Primitives May 9, 2011 9 / 36
![Page 27: Planning Sequences of Motion Primitives for humanoid robotsgamma.cs.unc.edu/PROBOT/Lamiraux.pdf · 2011. 5. 24. · Planning Sequences of Motion Primitives for humanoid robots Florent](https://reader035.vdocuments.us/reader035/viewer/2022070219/612ff9411ecc51586943cb9b/html5/thumbnails/27.jpg)
Going through a door: algorithm
Configuration space:SE(2)× [αmin, αmax ]× {free, left hand, right hand,both hands}Classical RRT algorithm with dedicated methods
I steering method: connect two configurations ifF states are adjacent in constraint graph,F motion of object is consistent,F enforce weaker constraint,
I distance functionF return ∞ when two configurations cannot be connected,
I random configuration shooter: sample at intersections of constraintmanifolds.
Florent Lamiraux (LAAS) Planning Sequences of Motion Primitives May 9, 2011 9 / 36
![Page 28: Planning Sequences of Motion Primitives for humanoid robotsgamma.cs.unc.edu/PROBOT/Lamiraux.pdf · 2011. 5. 24. · Planning Sequences of Motion Primitives for humanoid robots Florent](https://reader035.vdocuments.us/reader035/viewer/2022070219/612ff9411ecc51586943cb9b/html5/thumbnails/28.jpg)
Going through a door: algorithm
Configuration space:SE(2)× [αmin, αmax ]× {free, left hand, right hand,both hands}Classical RRT algorithm with dedicated methods
I steering method: connect two configurations ifF states are adjacent in constraint graph,F motion of object is consistent,F enforce weaker constraint,
I distance functionF return ∞ when two configurations cannot be connected,
I random configuration shooter: sample at intersections of constraintmanifolds.
Florent Lamiraux (LAAS) Planning Sequences of Motion Primitives May 9, 2011 9 / 36
![Page 29: Planning Sequences of Motion Primitives for humanoid robotsgamma.cs.unc.edu/PROBOT/Lamiraux.pdf · 2011. 5. 24. · Planning Sequences of Motion Primitives for humanoid robots Florent](https://reader035.vdocuments.us/reader035/viewer/2022070219/612ff9411ecc51586943cb9b/html5/thumbnails/29.jpg)
Going through a door: algorithm
Configuration space:SE(2)× [αmin, αmax ]× {free, left hand, right hand,both hands}Classical RRT algorithm with dedicated methods
I steering method: connect two configurations ifF states are adjacent in constraint graph,F motion of object is consistent,F enforce weaker constraint,
I distance functionF return ∞ when two configurations cannot be connected,
I random configuration shooter: sample at intersections of constraintmanifolds.
Florent Lamiraux (LAAS) Planning Sequences of Motion Primitives May 9, 2011 9 / 36
![Page 30: Planning Sequences of Motion Primitives for humanoid robotsgamma.cs.unc.edu/PROBOT/Lamiraux.pdf · 2011. 5. 24. · Planning Sequences of Motion Primitives for humanoid robots Florent](https://reader035.vdocuments.us/reader035/viewer/2022070219/612ff9411ecc51586943cb9b/html5/thumbnails/30.jpg)
Going through a door: algorithm
Configuration space:SE(2)× [αmin, αmax ]× {free, left hand, right hand,both hands}Classical RRT algorithm with dedicated methods
I steering method: connect two configurations ifF states are adjacent in constraint graph,F motion of object is consistent,F enforce weaker constraint,
I distance functionF return ∞ when two configurations cannot be connected,
I random configuration shooter: sample at intersections of constraintmanifolds.
Florent Lamiraux (LAAS) Planning Sequences of Motion Primitives May 9, 2011 9 / 36
![Page 31: Planning Sequences of Motion Primitives for humanoid robotsgamma.cs.unc.edu/PROBOT/Lamiraux.pdf · 2011. 5. 24. · Planning Sequences of Motion Primitives for humanoid robots Florent](https://reader035.vdocuments.us/reader035/viewer/2022070219/612ff9411ecc51586943cb9b/html5/thumbnails/31.jpg)
Going through a door: algorithm
Configuration space:SE(2)× [αmin, αmax ]× {free, left hand, right hand,both hands}Classical RRT algorithm with dedicated methods
I steering method: connect two configurations ifF states are adjacent in constraint graph,F motion of object is consistent,F enforce weaker constraint,
I distance functionF return ∞ when two configurations cannot be connected,
I random configuration shooter: sample at intersections of constraintmanifolds.
Florent Lamiraux (LAAS) Planning Sequences of Motion Primitives May 9, 2011 9 / 36
![Page 32: Planning Sequences of Motion Primitives for humanoid robotsgamma.cs.unc.edu/PROBOT/Lamiraux.pdf · 2011. 5. 24. · Planning Sequences of Motion Primitives for humanoid robots Florent](https://reader035.vdocuments.us/reader035/viewer/2022070219/612ff9411ecc51586943cb9b/html5/thumbnails/32.jpg)
Going through a door: algorithm
Configuration space:SE(2)× [αmin, αmax ]× {free, left hand, right hand,both hands}Classical RRT algorithm with dedicated methods
I steering method: connect two configurations ifF states are adjacent in constraint graph,F motion of object is consistent,F enforce weaker constraint,
I distance functionF return ∞ when two configurations cannot be connected,
I random configuration shooter: sample at intersections of constraintmanifolds.
Florent Lamiraux (LAAS) Planning Sequences of Motion Primitives May 9, 2011 9 / 36
![Page 33: Planning Sequences of Motion Primitives for humanoid robotsgamma.cs.unc.edu/PROBOT/Lamiraux.pdf · 2011. 5. 24. · Planning Sequences of Motion Primitives for humanoid robots Florent](https://reader035.vdocuments.us/reader035/viewer/2022070219/612ff9411ecc51586943cb9b/html5/thumbnails/33.jpg)
Going through a door: algorithm
right hand
qinit
qgoal
left hand
C
Florent Lamiraux (LAAS) Planning Sequences of Motion Primitives May 9, 2011 10 / 36
![Page 34: Planning Sequences of Motion Primitives for humanoid robotsgamma.cs.unc.edu/PROBOT/Lamiraux.pdf · 2011. 5. 24. · Planning Sequences of Motion Primitives for humanoid robots Florent](https://reader035.vdocuments.us/reader035/viewer/2022070219/612ff9411ecc51586943cb9b/html5/thumbnails/34.jpg)
Going through a door: algorithm
right hand
qinit
qgoal
left hand
C
1. pick a random node
Florent Lamiraux (LAAS) Planning Sequences of Motion Primitives May 9, 2011 11 / 36
![Page 35: Planning Sequences of Motion Primitives for humanoid robotsgamma.cs.unc.edu/PROBOT/Lamiraux.pdf · 2011. 5. 24. · Planning Sequences of Motion Primitives for humanoid robots Florent](https://reader035.vdocuments.us/reader035/viewer/2022070219/612ff9411ecc51586943cb9b/html5/thumbnails/35.jpg)
Going through a door: algorithm
right hand
qinit
qgoal
left hand
C
1. pick a random node2. randomly sample a config about the
node with the same object config
Florent Lamiraux (LAAS) Planning Sequences of Motion Primitives May 9, 2011 12 / 36
![Page 36: Planning Sequences of Motion Primitives for humanoid robotsgamma.cs.unc.edu/PROBOT/Lamiraux.pdf · 2011. 5. 24. · Planning Sequences of Motion Primitives for humanoid robots Florent](https://reader035.vdocuments.us/reader035/viewer/2022070219/612ff9411ecc51586943cb9b/html5/thumbnails/36.jpg)
Going through a door: algorithm
right hand
qinit
qgoal
left hand
C
1. pick a random node2. randomly sample a config about the
node with the same object config3. expand each connected component
Florent Lamiraux (LAAS) Planning Sequences of Motion Primitives May 9, 2011 13 / 36
![Page 37: Planning Sequences of Motion Primitives for humanoid robotsgamma.cs.unc.edu/PROBOT/Lamiraux.pdf · 2011. 5. 24. · Planning Sequences of Motion Primitives for humanoid robots Florent](https://reader035.vdocuments.us/reader035/viewer/2022070219/612ff9411ecc51586943cb9b/html5/thumbnails/37.jpg)
Going through a door: algorithm
right hand
qinit
qgoal
left hand
C
Florent Lamiraux (LAAS) Planning Sequences of Motion Primitives May 9, 2011 14 / 36
![Page 38: Planning Sequences of Motion Primitives for humanoid robotsgamma.cs.unc.edu/PROBOT/Lamiraux.pdf · 2011. 5. 24. · Planning Sequences of Motion Primitives for humanoid robots Florent](https://reader035.vdocuments.us/reader035/viewer/2022070219/612ff9411ecc51586943cb9b/html5/thumbnails/38.jpg)
Going through a door: algorithm
right hand
qinit
qgoal
left hand
C
Florent Lamiraux (LAAS) Planning Sequences of Motion Primitives May 9, 2011 15 / 36
![Page 39: Planning Sequences of Motion Primitives for humanoid robotsgamma.cs.unc.edu/PROBOT/Lamiraux.pdf · 2011. 5. 24. · Planning Sequences of Motion Primitives for humanoid robots Florent](https://reader035.vdocuments.us/reader035/viewer/2022070219/612ff9411ecc51586943cb9b/html5/thumbnails/39.jpg)
Going through a door: algorithm
right hand
qinit
qgoal
left hand
C
Florent Lamiraux (LAAS) Planning Sequences of Motion Primitives May 9, 2011 16 / 36
![Page 40: Planning Sequences of Motion Primitives for humanoid robotsgamma.cs.unc.edu/PROBOT/Lamiraux.pdf · 2011. 5. 24. · Planning Sequences of Motion Primitives for humanoid robots Florent](https://reader035.vdocuments.us/reader035/viewer/2022070219/612ff9411ecc51586943cb9b/html5/thumbnails/40.jpg)
Going through a door: algorithm
right hand
qinit
qgoal
left hand
C
Florent Lamiraux (LAAS) Planning Sequences of Motion Primitives May 9, 2011 17 / 36
![Page 41: Planning Sequences of Motion Primitives for humanoid robotsgamma.cs.unc.edu/PROBOT/Lamiraux.pdf · 2011. 5. 24. · Planning Sequences of Motion Primitives for humanoid robots Florent](https://reader035.vdocuments.us/reader035/viewer/2022070219/612ff9411ecc51586943cb9b/html5/thumbnails/41.jpg)
Going through a door: algorithm
right hand
qinit
qgoal
left hand
C
Florent Lamiraux (LAAS) Planning Sequences of Motion Primitives May 9, 2011 18 / 36
![Page 42: Planning Sequences of Motion Primitives for humanoid robotsgamma.cs.unc.edu/PROBOT/Lamiraux.pdf · 2011. 5. 24. · Planning Sequences of Motion Primitives for humanoid robots Florent](https://reader035.vdocuments.us/reader035/viewer/2022070219/612ff9411ecc51586943cb9b/html5/thumbnails/42.jpg)
Going through a door: algorithm
right hand
qinit
qgoal
left hand
C
Florent Lamiraux (LAAS) Planning Sequences of Motion Primitives May 9, 2011 19 / 36
![Page 43: Planning Sequences of Motion Primitives for humanoid robotsgamma.cs.unc.edu/PROBOT/Lamiraux.pdf · 2011. 5. 24. · Planning Sequences of Motion Primitives for humanoid robots Florent](https://reader035.vdocuments.us/reader035/viewer/2022070219/612ff9411ecc51586943cb9b/html5/thumbnails/43.jpg)
Going through a door: algorithm
right hand
qinit
qgoal
left hand
C
Florent Lamiraux (LAAS) Planning Sequences of Motion Primitives May 9, 2011 20 / 36
![Page 44: Planning Sequences of Motion Primitives for humanoid robotsgamma.cs.unc.edu/PROBOT/Lamiraux.pdf · 2011. 5. 24. · Planning Sequences of Motion Primitives for humanoid robots Florent](https://reader035.vdocuments.us/reader035/viewer/2022070219/612ff9411ecc51586943cb9b/html5/thumbnails/44.jpg)
Going through a door: motion planning results
Florent Lamiraux (LAAS) Planning Sequences of Motion Primitives May 9, 2011 21 / 36
![Page 45: Planning Sequences of Motion Primitives for humanoid robotsgamma.cs.unc.edu/PROBOT/Lamiraux.pdf · 2011. 5. 24. · Planning Sequences of Motion Primitives for humanoid robots Florent](https://reader035.vdocuments.us/reader035/viewer/2022070219/612ff9411ecc51586943cb9b/html5/thumbnails/45.jpg)
Going through a door: whole-body animation
Motion of the bounding box is converted into time parameterizedI step sequence,I motions of end effectors
ResolutionI step sequence→ COM: preview control,I feet and upper body: inverse kinematics.
Florent Lamiraux (LAAS) Planning Sequences of Motion Primitives May 9, 2011 22 / 36
![Page 46: Planning Sequences of Motion Primitives for humanoid robotsgamma.cs.unc.edu/PROBOT/Lamiraux.pdf · 2011. 5. 24. · Planning Sequences of Motion Primitives for humanoid robots Florent](https://reader035.vdocuments.us/reader035/viewer/2022070219/612ff9411ecc51586943cb9b/html5/thumbnails/46.jpg)
Going through a door: whole-body animation
Motion of the bounding box is converted into time parameterizedI step sequence,I motions of end effectors
ResolutionI step sequence→ COM: preview control,I feet and upper body: inverse kinematics.
Florent Lamiraux (LAAS) Planning Sequences of Motion Primitives May 9, 2011 22 / 36
![Page 47: Planning Sequences of Motion Primitives for humanoid robotsgamma.cs.unc.edu/PROBOT/Lamiraux.pdf · 2011. 5. 24. · Planning Sequences of Motion Primitives for humanoid robots Florent](https://reader035.vdocuments.us/reader035/viewer/2022070219/612ff9411ecc51586943cb9b/html5/thumbnails/47.jpg)
Going through a door: animation
Florent Lamiraux (LAAS) Planning Sequences of Motion Primitives May 9, 2011 23 / 36
![Page 48: Planning Sequences of Motion Primitives for humanoid robotsgamma.cs.unc.edu/PROBOT/Lamiraux.pdf · 2011. 5. 24. · Planning Sequences of Motion Primitives for humanoid robots Florent](https://reader035.vdocuments.us/reader035/viewer/2022070219/612ff9411ecc51586943cb9b/html5/thumbnails/48.jpg)
From motion of bounding box to motion primitives
ObjectiveI transform result of path planning into sequence of controllers.
Florent Lamiraux (LAAS) Planning Sequences of Motion Primitives May 9, 2011 24 / 36
![Page 49: Planning Sequences of Motion Primitives for humanoid robotsgamma.cs.unc.edu/PROBOT/Lamiraux.pdf · 2011. 5. 24. · Planning Sequences of Motion Primitives for humanoid robots Florent](https://reader035.vdocuments.us/reader035/viewer/2022070219/612ff9411ecc51586943cb9b/html5/thumbnails/49.jpg)
Hierarchical task based control framework
Task1 Function of configuration to be
controlled to 0,2 Jacobian of the function.
Stack of tasksI tasks in decreasing order of priority,I compute velocity by cascade of
pseudo inverses. [Siciliano, Slotine1991]
Florent Lamiraux (LAAS) Planning Sequences of Motion Primitives May 9, 2011 25 / 36
![Page 50: Planning Sequences of Motion Primitives for humanoid robotsgamma.cs.unc.edu/PROBOT/Lamiraux.pdf · 2011. 5. 24. · Planning Sequences of Motion Primitives for humanoid robots Florent](https://reader035.vdocuments.us/reader035/viewer/2022070219/612ff9411ecc51586943cb9b/html5/thumbnails/50.jpg)
Hierarchical task based control framework
Task1 Function of configuration to be
controlled to 0,2 Jacobian of the function.
Stack of tasksI tasks in decreasing order of priority,I compute velocity by cascade of
pseudo inverses. [Siciliano, Slotine1991]
Florent Lamiraux (LAAS) Planning Sequences of Motion Primitives May 9, 2011 25 / 36
![Page 51: Planning Sequences of Motion Primitives for humanoid robotsgamma.cs.unc.edu/PROBOT/Lamiraux.pdf · 2011. 5. 24. · Planning Sequences of Motion Primitives for humanoid robots Florent](https://reader035.vdocuments.us/reader035/viewer/2022070219/612ff9411ecc51586943cb9b/html5/thumbnails/51.jpg)
Stack of tasks: collaboration with JRL -Tsukuba Japan
Tmvm
q1 = −λ1J+1 v1 → v1 = −λ1v1
q
v1J1
Jm
T1
Florent Lamiraux (LAAS) Planning Sequences of Motion Primitives May 9, 2011 26 / 36
![Page 52: Planning Sequences of Motion Primitives for humanoid robotsgamma.cs.unc.edu/PROBOT/Lamiraux.pdf · 2011. 5. 24. · Planning Sequences of Motion Primitives for humanoid robots Florent](https://reader035.vdocuments.us/reader035/viewer/2022070219/612ff9411ecc51586943cb9b/html5/thumbnails/52.jpg)
Stack of tasks
Tmvm
q1 = −λ1J+1 v1 → v1 = −λ1v1
q
v1J1
Jm
T1
qi = qi−1 − λi(JiPi−1)+(vi−Ji qi−1)
Florent Lamiraux (LAAS) Planning Sequences of Motion Primitives May 9, 2011 27 / 36
![Page 53: Planning Sequences of Motion Primitives for humanoid robotsgamma.cs.unc.edu/PROBOT/Lamiraux.pdf · 2011. 5. 24. · Planning Sequences of Motion Primitives for humanoid robots Florent](https://reader035.vdocuments.us/reader035/viewer/2022070219/612ff9411ecc51586943cb9b/html5/thumbnails/53.jpg)
Stack of tasks
Tmvm
qi = qi−1 − λi(JiPi−1)+(vi−Ji qi−1)
q1 = −λ1J+1 v1 → v1 = −λ1v1
Pi = Pi−1 − (JiPi−1)+JiPi−1
q
v1J1
Jm
T1
Florent Lamiraux (LAAS) Planning Sequences of Motion Primitives May 9, 2011 28 / 36
![Page 54: Planning Sequences of Motion Primitives for humanoid robotsgamma.cs.unc.edu/PROBOT/Lamiraux.pdf · 2011. 5. 24. · Planning Sequences of Motion Primitives for humanoid robots Florent](https://reader035.vdocuments.us/reader035/viewer/2022070219/612ff9411ecc51586943cb9b/html5/thumbnails/54.jpg)
Stack of tasks
Tmvm
q = qm
q1 = −λ1J+1 v1 → v1 = −λ1v1
qi = qi−1 − λi(JiPi−1)+(vi−Ji qi−1)
Pi = Pi−1 − (JiPi−1)+JiPi−1
q
v1J1
Jm
T1
Florent Lamiraux (LAAS) Planning Sequences of Motion Primitives May 9, 2011 29 / 36
![Page 55: Planning Sequences of Motion Primitives for humanoid robotsgamma.cs.unc.edu/PROBOT/Lamiraux.pdf · 2011. 5. 24. · Planning Sequences of Motion Primitives for humanoid robots Florent](https://reader035.vdocuments.us/reader035/viewer/2022070219/612ff9411ecc51586943cb9b/html5/thumbnails/55.jpg)
Stack of tasks
Tlf
q qTrf
Tcom
Trh
robot
Florent Lamiraux (LAAS) Planning Sequences of Motion Primitives May 9, 2011 30 / 36
![Page 56: Planning Sequences of Motion Primitives for humanoid robotsgamma.cs.unc.edu/PROBOT/Lamiraux.pdf · 2011. 5. 24. · Planning Sequences of Motion Primitives for humanoid robots Florent](https://reader035.vdocuments.us/reader035/viewer/2022070219/612ff9411ecc51586943cb9b/html5/thumbnails/56.jpg)
Stack of tasks
forward
kinematics
Tlf
q qTrf
Tcom
Jrhvrh
vrfJlfvlf
Trh
refrh
robotJcomvcom
Jrf
refcom
reflfrefrf
Florent Lamiraux (LAAS) Planning Sequences of Motion Primitives May 9, 2011 31 / 36
![Page 57: Planning Sequences of Motion Primitives for humanoid robotsgamma.cs.unc.edu/PROBOT/Lamiraux.pdf · 2011. 5. 24. · Planning Sequences of Motion Primitives for humanoid robots Florent](https://reader035.vdocuments.us/reader035/viewer/2022070219/612ff9411ecc51586943cb9b/html5/thumbnails/57.jpg)
Stack of tasks
forward
kinematics
Tlf
q qTrf
Tcom
Jrhvrh
vrfJlfvlf
refcom(t)refrf (t)
reflf (t)
Trh
refrh
walkpattern
generatorsequence
robot
footstep
Jcomvcom
Jrf
Florent Lamiraux (LAAS) Planning Sequences of Motion Primitives May 9, 2011 32 / 36
![Page 58: Planning Sequences of Motion Primitives for humanoid robotsgamma.cs.unc.edu/PROBOT/Lamiraux.pdf · 2011. 5. 24. · Planning Sequences of Motion Primitives for humanoid robots Florent](https://reader035.vdocuments.us/reader035/viewer/2022070219/612ff9411ecc51586943cb9b/html5/thumbnails/58.jpg)
Stack of tasks
forward
kinematics
Tlf
q qTrf
Tcom
Jrhvrh
vrfJlfvlf
refcom(t)refrf (t)
reflf (t)
Trh
refrh
walkpattern
generatorsequence
robot
footstep
Jcomvcom
Jrf
servoingvisual
Florent Lamiraux (LAAS) Planning Sequences of Motion Primitives May 9, 2011 33 / 36
![Page 59: Planning Sequences of Motion Primitives for humanoid robotsgamma.cs.unc.edu/PROBOT/Lamiraux.pdf · 2011. 5. 24. · Planning Sequences of Motion Primitives for humanoid robots Florent](https://reader035.vdocuments.us/reader035/viewer/2022070219/612ff9411ecc51586943cb9b/html5/thumbnails/59.jpg)
Motion plan
right hand: SE(3)
left hand: SE(3)
bounding box: R2 × S1
tt0 t1 t2 t3
gaze task: R3
Florent Lamiraux (LAAS) Planning Sequences of Motion Primitives May 9, 2011 34 / 36
![Page 60: Planning Sequences of Motion Primitives for humanoid robotsgamma.cs.unc.edu/PROBOT/Lamiraux.pdf · 2011. 5. 24. · Planning Sequences of Motion Primitives for humanoid robots Florent](https://reader035.vdocuments.us/reader035/viewer/2022070219/612ff9411ecc51586943cb9b/html5/thumbnails/60.jpg)
Preview
Grasping a ball while walking, N. Mansard, O. Stasse, JRL -Tsukuba 2007.
Florent Lamiraux (LAAS) Planning Sequences of Motion Primitives May 9, 2011 35 / 36
![Page 61: Planning Sequences of Motion Primitives for humanoid robotsgamma.cs.unc.edu/PROBOT/Lamiraux.pdf · 2011. 5. 24. · Planning Sequences of Motion Primitives for humanoid robots Florent](https://reader035.vdocuments.us/reader035/viewer/2022070219/612ff9411ecc51586943cb9b/html5/thumbnails/61.jpg)
Conclusion
Let us do it on the physical robot
Florent Lamiraux (LAAS) Planning Sequences of Motion Primitives May 9, 2011 36 / 36