6.141: robotics systems and science lecture 9...
TRANSCRIPT
![Page 1: 6.141: Robotics systems and science Lecture 9 ...courses.csail.mit.edu/6.141/spring2011/pub/lectures/Lec09-MotionPlanning-II.pdf1. Find a point in S’s C-obstacle (robot placement](https://reader034.vdocument.in/reader034/viewer/2022050421/5f90cd63a045237b722a9edd/html5/thumbnails/1.jpg)
6.141: Robotics systems and science Lecture 9: Configuration Space and Motion Planning
Lecture Notes Prepared by Daniela Rus EECS/MIT
Spring 2011 Figures by Nancy Amato, Rodney Brooks, Vijay Kumar
Reading: Chapter 3, and Craig: Robotics
http://courses.csail.mit.edu/6.141/!Challenge: Build a Shelter on Mars!
![Page 2: 6.141: Robotics systems and science Lecture 9 ...courses.csail.mit.edu/6.141/spring2011/pub/lectures/Lec09-MotionPlanning-II.pdf1. Find a point in S’s C-obstacle (robot placement](https://reader034.vdocument.in/reader034/viewer/2022050421/5f90cd63a045237b722a9edd/html5/thumbnails/2.jpg)
Announcements
Next Lab Reports: each team member talks about the technical piece he/she executed
Sign up for MIT@150 Symposium: Computation and the Transformation of Nearly Everything
![Page 3: 6.141: Robotics systems and science Lecture 9 ...courses.csail.mit.edu/6.141/spring2011/pub/lectures/Lec09-MotionPlanning-II.pdf1. Find a point in S’s C-obstacle (robot placement](https://reader034.vdocument.in/reader034/viewer/2022050421/5f90cd63a045237b722a9edd/html5/thumbnails/3.jpg)
Control architectures: reactive, behavior, deliberative
Visibility Graphs for Motion Planning Started Configuration Space
During the last module we saw
![Page 4: 6.141: Robotics systems and science Lecture 9 ...courses.csail.mit.edu/6.141/spring2011/pub/lectures/Lec09-MotionPlanning-II.pdf1. Find a point in S’s C-obstacle (robot placement](https://reader034.vdocument.in/reader034/viewer/2022050421/5f90cd63a045237b722a9edd/html5/thumbnails/4.jpg)
Today
Understand c-space Motion planning with grids Probabilistic motion planning
![Page 5: 6.141: Robotics systems and science Lecture 9 ...courses.csail.mit.edu/6.141/spring2011/pub/lectures/Lec09-MotionPlanning-II.pdf1. Find a point in S’s C-obstacle (robot placement](https://reader034.vdocument.in/reader034/viewer/2022050421/5f90cd63a045237b722a9edd/html5/thumbnails/5.jpg)
Transforming to C-Space
Higher dimension
Simpler problem
![Page 6: 6.141: Robotics systems and science Lecture 9 ...courses.csail.mit.edu/6.141/spring2011/pub/lectures/Lec09-MotionPlanning-II.pdf1. Find a point in S’s C-obstacle (robot placement](https://reader034.vdocument.in/reader034/viewer/2022050421/5f90cd63a045237b722a9edd/html5/thumbnails/6.jpg)
C-space Overview C-obst
C-obst
C-obst
C-obst
C-obst
C-Space
6D C-space (x,y,z,pitch,roll,yaw)
3D C-space (x,y,z) 3D C-space
(α,β,γ) α β γ
• robot maps to a point in higher dimensional space • parameter for each degree of freedom (dof) of robot • C-space = set of all robot placements • C-obstacle = infeasible robot placements
2n-D C-space (φ1, ψ1, φ2, ψ2, . . . , φ n, ψ n)
![Page 7: 6.141: Robotics systems and science Lecture 9 ...courses.csail.mit.edu/6.141/spring2011/pub/lectures/Lec09-MotionPlanning-II.pdf1. Find a point in S’s C-obstacle (robot placement](https://reader034.vdocument.in/reader034/viewer/2022050421/5f90cd63a045237b722a9edd/html5/thumbnails/7.jpg)
C-obstacle Example
![Page 8: 6.141: Robotics systems and science Lecture 9 ...courses.csail.mit.edu/6.141/spring2011/pub/lectures/Lec09-MotionPlanning-II.pdf1. Find a point in S’s C-obstacle (robot placement](https://reader034.vdocument.in/reader034/viewer/2022050421/5f90cd63a045237b722a9edd/html5/thumbnails/8.jpg)
Transforming to C-Space
![Page 9: 6.141: Robotics systems and science Lecture 9 ...courses.csail.mit.edu/6.141/spring2011/pub/lectures/Lec09-MotionPlanning-II.pdf1. Find a point in S’s C-obstacle (robot placement](https://reader034.vdocument.in/reader034/viewer/2022050421/5f90cd63a045237b722a9edd/html5/thumbnails/9.jpg)
C-obstacle for fixed robot orientation
Robot
![Page 10: 6.141: Robotics systems and science Lecture 9 ...courses.csail.mit.edu/6.141/spring2011/pub/lectures/Lec09-MotionPlanning-II.pdf1. Find a point in S’s C-obstacle (robot placement](https://reader034.vdocument.in/reader034/viewer/2022050421/5f90cd63a045237b722a9edd/html5/thumbnails/10.jpg)
What if the robot can rotate?
![Page 11: 6.141: Robotics systems and science Lecture 9 ...courses.csail.mit.edu/6.141/spring2011/pub/lectures/Lec09-MotionPlanning-II.pdf1. Find a point in S’s C-obstacle (robot placement](https://reader034.vdocument.in/reader034/viewer/2022050421/5f90cd63a045237b722a9edd/html5/thumbnails/11.jpg)
What if the robot can rotate?
![Page 12: 6.141: Robotics systems and science Lecture 9 ...courses.csail.mit.edu/6.141/spring2011/pub/lectures/Lec09-MotionPlanning-II.pdf1. Find a point in S’s C-obstacle (robot placement](https://reader034.vdocument.in/reader034/viewer/2022050421/5f90cd63a045237b722a9edd/html5/thumbnails/12.jpg)
How do we compute C-space
Identify dimensions Compute all c-obstacles
![Page 13: 6.141: Robotics systems and science Lecture 9 ...courses.csail.mit.edu/6.141/spring2011/pub/lectures/Lec09-MotionPlanning-II.pdf1. Find a point in S’s C-obstacle (robot placement](https://reader034.vdocument.in/reader034/viewer/2022050421/5f90cd63a045237b722a9edd/html5/thumbnails/13.jpg)
How do we compute c-obstacles?
Step 1: Reflect Robot
![Page 14: 6.141: Robotics systems and science Lecture 9 ...courses.csail.mit.edu/6.141/spring2011/pub/lectures/Lec09-MotionPlanning-II.pdf1. Find a point in S’s C-obstacle (robot placement](https://reader034.vdocument.in/reader034/viewer/2022050421/5f90cd63a045237b722a9edd/html5/thumbnails/14.jpg)
C-space Algorithm
Step 2: Minkowski sum with reflected robot
![Page 15: 6.141: Robotics systems and science Lecture 9 ...courses.csail.mit.edu/6.141/spring2011/pub/lectures/Lec09-MotionPlanning-II.pdf1. Find a point in S’s C-obstacle (robot placement](https://reader034.vdocument.in/reader034/viewer/2022050421/5f90cd63a045237b722a9edd/html5/thumbnails/15.jpg)
C-space Algorithm
Step 2: Vert ( - Robot) + Vert (Obstacle)
![Page 16: 6.141: Robotics systems and science Lecture 9 ...courses.csail.mit.edu/6.141/spring2011/pub/lectures/Lec09-MotionPlanning-II.pdf1. Find a point in S’s C-obstacle (robot placement](https://reader034.vdocument.in/reader034/viewer/2022050421/5f90cd63a045237b722a9edd/html5/thumbnails/16.jpg)
C-space Algorithm
Step 3: ConvexHull (Vert ( - Robot) + Vert (Obstacle))
![Page 17: 6.141: Robotics systems and science Lecture 9 ...courses.csail.mit.edu/6.141/spring2011/pub/lectures/Lec09-MotionPlanning-II.pdf1. Find a point in S’s C-obstacle (robot placement](https://reader034.vdocument.in/reader034/viewer/2022050421/5f90cd63a045237b722a9edd/html5/thumbnails/17.jpg)
Convex Hull Algorithm
![Page 18: 6.141: Robotics systems and science Lecture 9 ...courses.csail.mit.edu/6.141/spring2011/pub/lectures/Lec09-MotionPlanning-II.pdf1. Find a point in S’s C-obstacle (robot placement](https://reader034.vdocument.in/reader034/viewer/2022050421/5f90cd63a045237b722a9edd/html5/thumbnails/18.jpg)
How do we compute convex hulls?
![Page 19: 6.141: Robotics systems and science Lecture 9 ...courses.csail.mit.edu/6.141/spring2011/pub/lectures/Lec09-MotionPlanning-II.pdf1. Find a point in S’s C-obstacle (robot placement](https://reader034.vdocument.in/reader034/viewer/2022050421/5f90cd63a045237b722a9edd/html5/thumbnails/19.jpg)
Convex Hull Algorithm
A
B
C D E
![Page 20: 6.141: Robotics systems and science Lecture 9 ...courses.csail.mit.edu/6.141/spring2011/pub/lectures/Lec09-MotionPlanning-II.pdf1. Find a point in S’s C-obstacle (robot placement](https://reader034.vdocument.in/reader034/viewer/2022050421/5f90cd63a045237b722a9edd/html5/thumbnails/20.jpg)
Convex Hull Algorithm
A
B
C D E
![Page 21: 6.141: Robotics systems and science Lecture 9 ...courses.csail.mit.edu/6.141/spring2011/pub/lectures/Lec09-MotionPlanning-II.pdf1. Find a point in S’s C-obstacle (robot placement](https://reader034.vdocument.in/reader034/viewer/2022050421/5f90cd63a045237b722a9edd/html5/thumbnails/21.jpg)
C-obstacle with Rotations simple 2D workspace obstacle => complicated 3D C-obstacle
Figure from Latombe’91
![Page 22: 6.141: Robotics systems and science Lecture 9 ...courses.csail.mit.edu/6.141/spring2011/pub/lectures/Lec09-MotionPlanning-II.pdf1. Find a point in S’s C-obstacle (robot placement](https://reader034.vdocument.in/reader034/viewer/2022050421/5f90cd63a045237b722a9edd/html5/thumbnails/22.jpg)
Motion Planning Algorithm (1) Compute c-obstacle for each obstacle (Reflect points, Minkowsky sums, convex hull) (2) Find path from start to goal for point robot
The robots DOF dictate (1) The method for (2) differentiates among motion
planning algorithms
![Page 23: 6.141: Robotics systems and science Lecture 9 ...courses.csail.mit.edu/6.141/spring2011/pub/lectures/Lec09-MotionPlanning-II.pdf1. Find a point in S’s C-obstacle (robot placement](https://reader034.vdocument.in/reader034/viewer/2022050421/5f90cd63a045237b722a9edd/html5/thumbnails/23.jpg)
Motion Planning Summary
robot
obst
obst
obst
obst
x y
θ
C-obst
C-obst C-obst
C-obst
robot Path is swept volume Path is 1D curve
Workspace
C-space
![Page 24: 6.141: Robotics systems and science Lecture 9 ...courses.csail.mit.edu/6.141/spring2011/pub/lectures/Lec09-MotionPlanning-II.pdf1. Find a point in S’s C-obstacle (robot placement](https://reader034.vdocument.in/reader034/viewer/2022050421/5f90cd63a045237b722a9edd/html5/thumbnails/24.jpg)
How do we find the path? Recall Visibility Graphs
In 2D the V-graph method finds the shortest path from S to G What about 3D?
Figure from Latombe’91
![Page 25: 6.141: Robotics systems and science Lecture 9 ...courses.csail.mit.edu/6.141/spring2011/pub/lectures/Lec09-MotionPlanning-II.pdf1. Find a point in S’s C-obstacle (robot placement](https://reader034.vdocument.in/reader034/viewer/2022050421/5f90cd63a045237b722a9edd/html5/thumbnails/25.jpg)
Most motion planning problems are PSPACE-hard [Reif 79, Hopcroft et al. 84 & 86] The best deterministic algorithm known has running time that is exponential in the dimension of the robot’s C-space [Canny 86] • C-space has high dimension - 6D for rigid body in 3-space • simple obstacles have complex C-obstacles impractical to compute explicit representation of freespace for high dof robots
So … attention has turned to approximation and randomized algorithms which • trade full completeness of the planner • for a major gain in efficiency
How hard is this to compute? The Complexity of Motion Plannin
![Page 26: 6.141: Robotics systems and science Lecture 9 ...courses.csail.mit.edu/6.141/spring2011/pub/lectures/Lec09-MotionPlanning-II.pdf1. Find a point in S’s C-obstacle (robot placement](https://reader034.vdocument.in/reader034/viewer/2022050421/5f90cd63a045237b722a9edd/html5/thumbnails/26.jpg)
Exact Cell Decomposition for finding path
![Page 27: 6.141: Robotics systems and science Lecture 9 ...courses.csail.mit.edu/6.141/spring2011/pub/lectures/Lec09-MotionPlanning-II.pdf1. Find a point in S’s C-obstacle (robot placement](https://reader034.vdocument.in/reader034/viewer/2022050421/5f90cd63a045237b722a9edd/html5/thumbnails/27.jpg)
Searching the Convex Cells for finding path
Build graph Search for path
![Page 28: 6.141: Robotics systems and science Lecture 9 ...courses.csail.mit.edu/6.141/spring2011/pub/lectures/Lec09-MotionPlanning-II.pdf1. Find a point in S’s C-obstacle (robot placement](https://reader034.vdocument.in/reader034/viewer/2022050421/5f90cd63a045237b722a9edd/html5/thumbnails/28.jpg)
Approximate Cell Decomposition
![Page 29: 6.141: Robotics systems and science Lecture 9 ...courses.csail.mit.edu/6.141/spring2011/pub/lectures/Lec09-MotionPlanning-II.pdf1. Find a point in S’s C-obstacle (robot placement](https://reader034.vdocument.in/reader034/viewer/2022050421/5f90cd63a045237b722a9edd/html5/thumbnails/29.jpg)
Cell Connectivity Graph
![Page 30: 6.141: Robotics systems and science Lecture 9 ...courses.csail.mit.edu/6.141/spring2011/pub/lectures/Lec09-MotionPlanning-II.pdf1. Find a point in S’s C-obstacle (robot placement](https://reader034.vdocument.in/reader034/viewer/2022050421/5f90cd63a045237b722a9edd/html5/thumbnails/30.jpg)
Probabilistic Road Maps (PRM) for finding paths [Kavraki at al 96]
1. Connect start and goal to roadmap
Query processing start
goal
C-obst
C-obst
C-obst
C-obst
Roadmap Construction (Pre-processing)
2. Connect pairs of nodes to form roadmap - simple, deterministic local planner (e.g., straightline) - discard paths that are invalid
1. Randomly generate robot configurations (nodes) - discard nodes that are invalid
C-obst
C-space
2. Find path in roadmap between start and goal - regenerate plans for edges in roadmap
Primitives Required: 1. Method for Sampling points in C-Space 2. Method for `validating’ points in C-Space
![Page 31: 6.141: Robotics systems and science Lecture 9 ...courses.csail.mit.edu/6.141/spring2011/pub/lectures/Lec09-MotionPlanning-II.pdf1. Find a point in S’s C-obstacle (robot placement](https://reader034.vdocument.in/reader034/viewer/2022050421/5f90cd63a045237b722a9edd/html5/thumbnails/31.jpg)
More PRMS PRMs: Pros
1. PRMs are probabilistically complete 2. PRMs apply easily to high-dimensional C-space 3. PRMs support fast queries w/ enough preprocessing
Many success stories where PRMs solve previously unsolved problems
C-obst
C-obst
C-obst
C-obst
C-obst
start
goal
![Page 32: 6.141: Robotics systems and science Lecture 9 ...courses.csail.mit.edu/6.141/spring2011/pub/lectures/Lec09-MotionPlanning-II.pdf1. Find a point in S’s C-obstacle (robot placement](https://reader034.vdocument.in/reader034/viewer/2022050421/5f90cd63a045237b722a9edd/html5/thumbnails/32.jpg)
More PRMS PRMs: Pros
1. PRMs are probabilistically complete 2. PRMs apply easily to high-dimensional C-space 3. PRMs support fast queries w/ enough preprocessing
Many success stories where PRMs solve previously unsolved problems
C-obst
C-obst
C-obst
C-obst
C-obst
start
goal
PRMs: Cons
1. PRMs don’t work as well for some problems: – unlikely to sample nodes in narrow passages – hard to sample/connect nodes on constraint surfaces
start
goal
C-obst
C-obst
C-obst
C-obst
![Page 33: 6.141: Robotics systems and science Lecture 9 ...courses.csail.mit.edu/6.141/spring2011/pub/lectures/Lec09-MotionPlanning-II.pdf1. Find a point in S’s C-obstacle (robot placement](https://reader034.vdocument.in/reader034/viewer/2022050421/5f90cd63a045237b722a9edd/html5/thumbnails/33.jpg)
Sampling Around Obstacles [Amato et al 98]
start
goal
C-obst
C-obst
C-obst
C-obst
To Navigate Narrow Passages we must sample in them • most PRM nodes are where planning is easy (not needed)
PRM Roadmap
start
goal
C-obst
C-obst
C-obst
C-obst
Idea: Can we sample nodes near C-obstacle surfaces? • we cannot explicitly construct the C-obstacles... • we do have models of the (workspace) obstacles...
OBPRM Roadmap
![Page 34: 6.141: Robotics systems and science Lecture 9 ...courses.csail.mit.edu/6.141/spring2011/pub/lectures/Lec09-MotionPlanning-II.pdf1. Find a point in S’s C-obstacle (robot placement](https://reader034.vdocument.in/reader034/viewer/2022050421/5f90cd63a045237b722a9edd/html5/thumbnails/34.jpg)
OBPRM: Finding points on C-obstacles
1
3
2
4 5
Basic Idea (for workspace obstacle S) 1. Find a point in S’s C-obstacle (robot placement colliding with S) 2. Select a random direction in C-space 3. Find a free point in that direction 4. Find boundary point between them using binary search (collision checks)
Note: we can use more sophisticated approaches to try to cover C-obstacle
C-obst
![Page 35: 6.141: Robotics systems and science Lecture 9 ...courses.csail.mit.edu/6.141/spring2011/pub/lectures/Lec09-MotionPlanning-II.pdf1. Find a point in S’s C-obstacle (robot placement](https://reader034.vdocument.in/reader034/viewer/2022050421/5f90cd63a045237b722a9edd/html5/thumbnails/35.jpg)
Repairing Paths [Amato et al] Even with the best sampling methods, roadmaps may not contain valid solution paths • may lack points in narrow passages • may contain approximate paths that are nearly valid
C-obstacle
C-obstacle approximate
path
![Page 36: 6.141: Robotics systems and science Lecture 9 ...courses.csail.mit.edu/6.141/spring2011/pub/lectures/Lec09-MotionPlanning-II.pdf1. Find a point in S’s C-obstacle (robot placement](https://reader034.vdocument.in/reader034/viewer/2022050421/5f90cd63a045237b722a9edd/html5/thumbnails/36.jpg)
Repairing Paths [Amato et al]
1. Create initial roadmap 2. Extract approximate path P 3. Repair P (push to C-free)
Focus search around P Use OBPRM-like techniques
Even with the best sampling methods, roadmaps may not contain valid solution paths • may lack points in narrow passages • may contain approximate paths that are nearly valid
Repairing/Improving Approximate Paths C-obstacle
C-obstacle approximate
path
repaired path
![Page 37: 6.141: Robotics systems and science Lecture 9 ...courses.csail.mit.edu/6.141/spring2011/pub/lectures/Lec09-MotionPlanning-II.pdf1. Find a point in S’s C-obstacle (robot placement](https://reader034.vdocument.in/reader034/viewer/2022050421/5f90cd63a045237b722a9edd/html5/thumbnails/37.jpg)
Algorithm Summary Compute c-space for each obstacle Compute graph representation Find path from start to goal
S
G
V-graph complete; gives optimal shortest path in 2d What about 3d? What else can we optimize?
![Page 38: 6.141: Robotics systems and science Lecture 9 ...courses.csail.mit.edu/6.141/spring2011/pub/lectures/Lec09-MotionPlanning-II.pdf1. Find a point in S’s C-obstacle (robot placement](https://reader034.vdocument.in/reader034/viewer/2022050421/5f90cd63a045237b722a9edd/html5/thumbnails/38.jpg)
Piano Movers’ Problem
Donald et al 93