1. Completeness of Randomized Kinodynamic Planners with State-based Steering
Stéphane Caron中, Quang-Cuong Pham光, Yoshihiko Nakamura中
中　Nakamura-Takano Laboratory, The University of Tokyo, Japan
光　School of Mechanical and Aerospace Engineering, Nanyang Technological University, Singapore

2. Motivation: VIP-RRT Benchmark
“Kinodynamic Motion Planners based on Velocity Interval Propagation” (RSS 2013)
Benchmark of Kinodynamic Planners
Observations: completeness?
Literature: did not help…

3. Theorem
The motion planning problem is to find a smooth trajectory connecting states 𝑥 𝑖𝑛𝑖𝑡 and 𝑥 𝑔𝑜𝑎𝑙 . Consider a kinodynamic system 𝑆 satisfying our Assumptions 1-3, and a randomized motion planner 𝐾 with state-based steering satisfying our Assumptions 4-6. If there is a solution to the motion planning problem with 𝛿-clearance in control space, then 𝐾 will, with probability one, find such a solution after a finite number of iterations. 𝑲 is thus probabilistically complete.

4. Completeness ?
Completeness: if there are solutions, return one, otherwise fail Probabilistic Completeness: if there are solutions, find one with probability one as the number of iterations goes to infinity
Why proving completeness?

5. Kinodynamic Constraints
Geometric
Dynamic Constraints
Non-Holonomic Equations
Geometric:
Holonomic equations (=)
Self-collisions (≤)
Obstacle avoidance (≤)
Non-holonomic equations:
Rolling without slipping
Conservation of angular momentum
Dynamic constraints:
Equations of motion (=)
Torque limits (≤)
Frictional contact (≤)
ZMP balance (≤)

6. Three Examples Pendulum with torque limits Reeds-Shepp car Humanoid
Geometric: self-collisions
Non-holonomic: not when fully actuated
Dynamic: EoM, torque limits
Reeds-Shepp car
Geometric: obstacles
Non-holonomic: rolling without slipping
Dynamic: none
Humanoid
Geometric: foot contact, self-collisions, obstacles
Non-holonomic: not while surface foot contact
Dynamic: EoM, torque limits, frictional contact, ZMP balance

7. Randomized Motion Planner (RRT, PRM, …)
1) SAMPLE
State Space
2) PARENTS
3) STEER
Obstacle
&c.
Start

8. Steering 𝑥 =𝑓(𝑥, 𝑢) Control Space (𝑢) 𝑓 −1 𝑓 𝑥 State Space (𝑥)
Control-based steering
Interpolate in Control Space
Use Forward Dynamics
Control Space (𝑢)
State-based steering
Interpolate in State Space
Use Inverse Dynamics 𝑡
𝑢(𝑡)
Analytical steering
Exact control-state trajectories are known between pairs of states 𝑓
𝑓 −1
State Space (𝑥) 𝑥 𝑥’

9. Comparison of Steering Approaches
Relies on + -
Control-based
Forward Dynamics
Valid Controls
Non-holonomy
State Constraints
State-based
Inverse Dynamics
Valid Controls?
Analytical
Researchers’ Brains
All constraints
Hard to find!

10. Kinodynamic Constraints
This paper is about…
Kinodynamic Constraints
Steering
Geometric
Dynamic Constraints
Non-Holonomic Equations
Steering
Relies on + -
Control-based
Forward Dynamics
Valid Controls
Non-holonomy
State Constraints
State-based
Inverse Dynamics
Valid Controls?
Analytical
Researchers’ Brains
All constraints
Hard to find!

11. Theorem again
The motion planning problem is to find a smooth trajectory connecting states 𝑥 𝑖𝑛𝑖𝑡 and 𝑥 𝑔𝑜𝑎𝑙 . Consider a kinodynamic system 𝑆 satisfying our Assumptions 1-3, and a randomized motion planner 𝐾 with state-based steering satisfying our Assumptions 4-6. If there is a solution to the motion planning problem with 𝛿-clearance in control space, then 𝐾 will, with probability one, find such a solution after a finite number of iterations. 𝑲 is thus probabilistically complete.

12. State-based Steering = Trajectory Interpolation + Inverse Dynamics
Keywording
State-based Steering = Trajectory Interpolation + Inverse Dynamics
Assumptions 4-6
Assumptions 1-3

13. Inverse Dynamics Assumptions
System
Pendulum Example
𝑥 =𝑓(𝑥, 𝑢)
𝑢= 𝑓 −1 𝑥, 𝑥
The system is fully actuated
The set of admissible controls is compact
The Inverse Dynamics function 𝑓 −1 is Lipschitz in both arguments
Pendulum with torque limits:
𝑀 𝑞 𝑞 + 𝑞 𝐶 𝑞 𝑞 +𝑔 𝑞 =𝑢
Controls:
𝑈 𝑎𝑑𝑚 = 𝑢∈𝑈 𝑢 ≤ 𝜏 𝑚𝑎𝑥 }
Smooth Inverse Dynamics
𝑞 1
𝑞 2

14. Interpolation Assumptions
State Space
Interpolated trajectories are smooth Lipschitz functions in both position and velocity
𝑥= 𝑞 𝑞 and Δ𝑥= Δ𝑞 Δ 𝑞 𝑑
𝜂⋅𝑑 𝑥 𝑥’
Interpolation: Smooth & Local
Interpolated trajectories stay within a neighborhood of their start and goal positions
Acceleration of interpolated trajectories converges to the discrete velocity derivative
𝑞 𝑖𝑛𝑡𝑒𝑟𝑝 − 𝑞 Δ 𝑞 Δ𝑞 Δ𝑥→0 0
Informal alert!

15. Theorem & Proof Sketch
The motion planning problem is to find a smooth trajectory connecting states 𝑥 𝑖𝑛𝑖𝑡 and 𝑥 𝑔𝑜𝑎𝑙 . Consider a kinodynamic system 𝑆 satisfying our Assumptions 1-3, and a randomized motion planner 𝐾 with state-based steering satisfying our Assumptions 4-6. If there is a solution to the motion planning problem with 𝛿-clearance in control space, then 𝐾 will, with probability one, find such a solution after a finite number of iterations. 𝑲 is thus probabilistically complete.
Proof outline:
Bound controls from Eq. of Motion
Decompose into distance and acceleration terms (Lipschitz, Assumptions 5 & 6)
Build an attraction sequence
Conclude as in [LaValle et al. (2001)]

16. On a concluding note
We proved probabilistic completeness for all planners using:
State-based steering (trajectory interpolation + inverse dynamics)
Compact control constraints
System assumptions: “you can do Inverse Dynamics”
Interpolation assumptions: “be smooth & local”

17. Thank you for your attention.

18. Venture there at your own risk!
Extra Slides Section
Venture there at your own risk!

19. Back to the VIP-RRT Benchmark
Acceleration-abusive
Interpolation

20. Assumptions you can check?
Kinodynamic planning:
Hsu et al. (1997) (𝛼, 𝛽)-expansiveness with 𝛼>0 and 𝛽>0
LaValle et al. (2001) existence of an attraction sequence
Karaman et al. (2011, 2013) optimal local planner (2011) or computability of “w-weighted boxes” (2013)
Check?
Half-way
Strong