SLIDE 1 Kinodynamic Motion Planners based
- n Velocity Interval Propagation
- S. Caron, Y. Nakamura, Q.-C. Pham
The University of Tokyo RSJ 2013 – IS5 on Humanoid Robots
SLIDE 2 Outline
◮ Reminder on Randomized Planning ◮ Admissible Velocity Propagation algorithm ◮ Preliminary experiments ◮ Towards humanoid robots...
SLIDE 3 Kinodynamic planning
◮ Non-holonomic constraint:
¨ q = f(q, ˙ q, τ)
◮ Torque constraints: for every joint i,
|τi| ≤ τ max
i
SLIDE 4 Randomized motion planning
◮ Major algorithms:
– Probabilistic Roadmap (PRM) – Rapidly-expanding Random Tree (RRT)
◮ Pro: probabilistic completeness guarantee
(established for kinematic planning)
◮ Con: curse of dimensionality
SLIDE 5
x
init goal
SLIDE 6
x x'
init goal
SLIDE 7 x x' steer(x, x')
init goal
SLIDE 10 Requirements
◮ Steering function
steer(x, x′): reachable state closer to x′
◮ Antecedent search:
finding nodes to steer from In kinematic planning:
◮ steering: geometric interpolation ◮ antecedent: neighborhoods for a metric σ(x, x′)
What about kinodynamic planning?
SLIDE 11 Steering
◮ Forward dynamics based (non-humanoid)
[LaValle, 1998, Hsu et al., 2002]
◮ Optimal steering (non-humanoid)
[Karaman and Frazzoli, 2011]
◮ Inverse dynamics based [Kuffner et al., 2002]
SLIDE 12 Steering with inverse dynamics?
◮ Previous approach:
– interpolate a trajectory – apply some dynamics filter [Kuffner et al., 2002]
◮ Our approach:
– interpolate a path – propagate reachable-velocity intervals [Pham et al., 2013]
SLIDE 13 Admissible Velocity Propagation
◮ AVP algorithm: extension of the Time-Optimal Path
Tracking algorithm [Bobrow et al., 1985]
◮ Input:
– path P ⊂ Cfree – interval of admissible velocities [v init
min, v init max] ◮ Output:
– is the path traversable? – interval of reachable velocities [v end
min , v end max]
SLIDE 14 Planner integration
◮ Each node stores a state x and a velocity interval
[vmin, vmax]
◮ Extension: interpolate a path, propagate admissible
velocities
Admissible velocity profile Velocity Configuration space Path
v
max
v
min
v
max
v
min
qinit qend
SLIDE 15 Space × time decoupling
x x'
init goal
Space
s s .
Time
Random data Paths Trajectory Velocity intervals
SLIDE 16 Properties
◮ Initial path unchanged → collision checking ◮ Applies to second-order non-holonomic constraints:
ZMP balance, torque limits, ...
Figure: Screenshot from [Pham and Nakamura, 2012]
SLIDE 17 Preliminary experiments
Double-inverted pendulum:
◮ Link: length l = 0.2 m ◮ Link mass m = 1 kg ◮ Statically-stable planning: |τ1| > 15.6 N.m ◮ Torque limits: |τ1| ≤ 8 N.m ∧ |τ2| ≤ 4 N.m
SLIDE 18
Simulation results
SLIDE 19 Towards Humanoids
◮ Extension to under-actuated systems: decoupling
vector fields [Bullo and Lynch, 2001]
◮ Identifying actuator limits ◮ . . .
SLIDE 20 To be continued...
◮ Randomized kinodynamic planning for humanoids? ◮ Importance of steering and antecedent selection ◮ Our approach steering: path tracking with velocity
interval propagation
SLIDE 21
Thanks for your attention!
SLIDE 22
Bobrow, J. E., Dubowsky, S., and Gibson, J. (1985). Time-optimal control of robotic manipulators along specified paths. The International Journal of Robotics Research, 4(3):3–17. Bullo, F. and Lynch, K. M. (2001). Kinematic controllability for decoupled trajectory planning in underactuated mechanical systems. Robotics and Automation, IEEE Transactions on, 17(4):402–412. Hsu, D., Kindel, R., Latombe, J.-C., and Rock, S. (2002). Randomized kinodynamic motion planning with moving obstacles. The International Journal of Robotics Research, 21(3):233–255.
SLIDE 23
Karaman, S. and Frazzoli, E. (2011). Sampling-based algorithms for optimal motion planning. The International Journal of Robotics Research, 30(7):846–894. Kuffner, J. J., Kagami, S., Nishiwaki, K., Inaba, M., and Inoue, H. (2002). Dynamically-stable motion planning for humanoid robots. Autonomous Robots, 12(1):105–118. LaValle, S. M. (1998). Rapidly-exploring random trees a new tool for path planning.
SLIDE 24
Pham, Q.-C., Caron, S., and Nakamura, Y. (2013). Kinodynamic planning in the configuration space via velocity interval propagation. Robotics: Science and System. Pham, Q.-C. and Nakamura, Y. (2012). Time-optimal path parameterization for critically dynamic motions of humanoid robots. In IEEE-RAS International Conference on Humanoid Robots.
