Transcript Slides
Randomized KinoDynamic Planning Steven LaValle James Kuffner Randomized KinoDynamic Planning • To determine the sequence of control inputs to drive a robot from an initial state to an end state while obeying physically based dynamic models and avoiding obstacles in the robot’s environment • Approach: tailored form of randomization (RRT’s) specially suited to high dimensional state spaces Problem Formulation • Differential Constraints in State Space Problem Formulation • Handling Obstacles in State Space Problem Formulation • Solution Trajectory Unique Challenges: • State Space has twice the dimension of Configuration; worsens the curse • Momentum considerations cause drift, overshooting, oscillations, which potential fields and probabilistic roadmaps can’t handle RRT based planner • Rapidly exploring Random Trees- State Space • Strategy… – Initialize tree with vertex – Repeatedly, • select state at random in state space • Select its nearest neighbor in the tree • Choose a control and ensuing output that pushes neighbor state towards random state • Create new vertex and add to tree RRT based planner • Rapidly exploring Random Trees • Outcome… RRT based planner • Rapidly exploring Random Trees – PRO: works for high degrees of freedom – PRO: no steering required – PRO: biased toward unexplored space – PRO: probabilistic completeness – CON: no well defined metric RRT based planner • Bidirectionnal planning algorithm • Strategy: – Grow two RRT’s at initial and goal states – At each growth step, check for intersection – Either halt once path exists, or continue to accumulate best paths Spacecraft and Hovercraft tests • Model – State Spacecraft and Hovercraft tests • Model – Control Spacecraft and Hovercraft tests • Model – Metric (Euclidean) – Weight vector is normalized – Dot product represents cosine of angle Spacecraft and Hovercraft tests • Model – Controls consists of a fixed set U in each example – Each set includes a ‘no control’ control – Each control is applied over a fixed timestep • eg. dt = 0.01 sec – Control timestep is independent of RRT timestep Video Spacecraft and Hovercraft tests • Case 1: Planar Translating Body in X-Z plane 4 DOF: x,z,x’,z’ 4 Controls: Spacecraft and Hovercraft tests • Case 2: Planar Body with Rotation – 6 DoF: x,y,θ,x’,y’, θ’ – 3 controls: • Translate forward • Rotate clockwise • Rotate counterCW Spacecraft and Hovercraft tests • Case 2: Planar Body with Rotation – ~ 5 minutes – 13,600 nodes Spacecraft and Hovercraft tests • Case 3: Translating 3-D body – 6 DoF: x,y,z,x’,y’,z’ – 6 controls: • Opposing forces In each of the 3 Principal directions Spacecraft and Hovercraft tests • Case 3: Translating 3-D body – ~1 min – 16,300 nodes Spacecraft and Hovercraft tests • Case 3: Translating 3-D body – ~1 min – 16,300 nodes Spacecraft and Hovercraft tests • Case 4: 3-D body with rotation – Cylindrical satellite object – 12 DoF: x,y,z,Rx,Ry,Rz and derivatives – 5 controls: translate along Cylindirical axis, rotates arbitrarily Simulates satellite docking Spacecraft and Hovercraft tests • Case 4: 3-D body with rotation – ~6 minute – 23,800 nodes Spacecraft and Hovercraft tests • Case 4: 3-D body with rotation – ~6 minute – 23,800 nodes Spacecraft and Hovercraft tests • Case 4: 3-D body with rotation – 12 DoF – 5 controls – Forward, up-down – Clockwise roll Spacecraft and Hovercraft tests • Case 4: 3-D body with rotation – ~11 minute – ??? nodes Broader Applications • Broad applications in Humanoid Robotics – Generalized to many high DoF problems subject to various constraints • Integrated Grasp Planning (Vahrenkamp, Do et al) [6] • Full Body Motion (S Kagami, J Kuffner et al) [7]