CS 326A Motion Planning Non Holonomic Motion Planning Coordination for Multiple Robots Notes for HW 2 n robots R1 Rn with configuration spaces C1 Cn sharing the same workspace Problem Plan coordinated motion so that each robot achieves its own goal configuration Centralized planning Plan the coordinated motion in C1xC2x xCn but very high dimensional space Decoupled planning Plan the motion of each robot ignoring the other robots then coordinate their motions so that no two robots collide Prioritized planning Plan the motion of one robot ignoring the other robots then plan the trajectory of a second robot in its configurationxtime space treating the first robot as a moving obstacle then plan the trajectory of a third robot Coordination Space 2 robots R1 and R2 2 paths i si 0 1 Ci i 1 2 s2 2 D coordination space 1 0 Generalize to n robots n D coordination space 1 s1 Variants of Decoupled Planning 1 Coordinate the n paths in n D coordination space 2 Coordinate paths of R1 and R2 in a 2 D coordination diagram path of R1 R2 then coordinate paths of R1 R2 and R3 s1 in 2 D coordination diagram etc Under Actuated Robots Fewer controls than dimensions in configuration space What is a degree of freedom number of dimensions of C space global or number of controls local How can m controls make it possible to span a Cspace with n m dimensions By exploiting mechanics properties Rolling with no sliding contact friction e g car bicycle roller skate Conservation of angular momentum satellite robot under actuated robot cat Others submarine plane object pushing Why is it useful Fewer actuators less weight Design simplicity Convenience think about driving a car with 3 controls Example Car Like Robot y dx sin dy cos 0 d dt v L tan L dx dt v cos dy dt v sin x Configuration space is 3 dimensional q x y But control space is 2 dimensional v with Example Car Like Robot y dx sin dy cos 0 d dt v L tan L dx dt v cos dy dt v sin x q x y q dq dt dx dt dy dt d dt dx sin dy cos 0 is a particular form of f q q 0 A robot is nonholonomic if its motion is constrained by a non integrable equation of the form f q q 0 Example Car Like Robot y dx sin dy cos 0 d dt v L tan L dx dt v cos dy dt v sin x Lower bounded turning radius How Can This Work Tangent Space Velocity Space y x y dx dy d L x dx dt v cos dy dt v sin d dt v L tan dx dy x y How Can This Work Tangent Space Velocity Space y x y dx dy d L x dx dt v cos dy dt v sin d dt v L tan dx dy x y Lie Bracket Maneuver made of 4 motions X Y Y X t Lie Bracket Maneuver made of 4 motions For example X Going straight X cos sin 0 T Y Turning angle T Y cos sin tan L dx dt v cos dy dt v sin d dt v L tan Lie Bracket Maneuver made of 4 motions For example X X Going straight X cos sin 0 T Y Y Turning angle 2 T X Y t Y cos sin tan L Lie bracket Y X t Lie Bracket X Y dY X dX Y X1 x X1 y X1 dX X2 x X2 y X1 X2 x X2 y X2 X Y Lin X Y the motion constraint is nonholonomic X Y X Y t2 Lie bracket Y X t Tractor Trailer Example 4 D configuration space 2 D control velocity space two independent velocity vectors X and Y U X Y Lin X Y V X U Lin X Y U Nonholonomic Path Planning Approaches Two phase planning path deformation Compute collision free path ignoring nonholonomic constraints Transform this path into a nonholonomic one Efficient but possible only if robot is controllable Need for a good set of maneuvers Direct planning control based sampling Use control based sampling to generate a tree of milestones until one is close enough to the goal deterministic or randomized Robot need not be controllable Applicable to high dimensional c spaces Path Deformation Holonomic path Nonholonomic path Type 1 Maneuver CYL x y x1 y1 0 d q x3 y3 0 dq x0 y0 0 x y dq x2 y2 0 Allows sidewise motion x y 2 tan d 2 1 cos 1 0 When 0 so does d and the cylinder becomes arbitrarily small Type 2 Maneuver Allows pure rotation Combination Coverage of a Path by Cylinders q q y x Path Examples Drawbacks of Two phase Planning Final path can be far from optimal Not applicable to robots that are not locally controllable e g car that can only move forward Reeds and Shepp Paths Reeds and Shepp Paths CC C0 CC C C CS0C C Given any two configurations the shortest RS paths between them is also the shortest path Example of Generated Path Holonomic Nonholonomic Path Optimization Nonholonomic Path Planning Approaches Two phase planning path deformation Compute collision free path ignoring nonholonomic constraints Transform this path into a nonholonomic one Efficient but possible only if robot is controllable Need for a good set of maneuvers Direct planning control based sampling Use control based sampling to generate a tree of milestones until one is close enough to the goal deterministic or randomized Robot need not be controllable Applicable to high dimensional c spaces Control Based Sampling Previous sampling technique Pick each milestone in some region Control based sampling 1 Pick control vector at random or not 2 Integrate equation of motion over short duration picked at random or not 3 If the motion is collision free then the endpoint is the new milestone Tree structured roadmaps Need for endgame regions Example dx dt v cos dy dt v sin d dt v L tan 1 Select a milestone m 2 Pick v and t 3 Integrate motion from m new milestone m Example Indexing array A 3 D grid is placed over the configuration space Each milestone falls into one cell of the grid A maximum number of milestones is allowed in each cell e g 2 or 3 Asymptotic completeness If a path exists the planner is guaranteed to find one if the resolution of the grid is fine enough Computed Paths Computed Paths Car That Can Only Turn Left Tractor trailer max 45o min 22 5o max 45o Application Summary Two planning approaches Path deformation Fast but paths can be far from optimal Restricted to controllable robots Control based sampling Can generate better paths but slower Can be scaled to higher dimensional space using probabilistic sampling techniques next lecture