Single Robot Motion Planning - IIReview: C-spaceReview: Computing C-obstacleOutlineApproximate Cell Decomposition (ACD)Approximate Cell DecompositionFinding a Path by ACDConnectivity GraphSlide 9Slide 10First Graph Cut AlgorithmSlide 12ACD for Path Non-existenceSlide 14Slide 15Slide 16Five-gear ExampleTwo-gear ExampleMotion Planning FrameworkSummary: Approximate Cell DecompositionSlide 21MotivationProbabilistic Roadmap (PRM)Basic PRM AalgorithmTwo Geometric Primitives in C-spaceQuery ProcessingTwo Tenets of PRM PlanningWhy does it work? IntuitionSlide 29Narrow Passage ProblemDifficultyStrategies to Improve PRMSampling StrategiesPoor Visibility in Narrow PassagesBut how to identify poor visibility regions?Workspace-guided strategiesDilation-based strategiesErrorWeaker CompletenessKinodynamic PlanningRRT for Kinodynamic SystemsMore ExamplesSummary: Sampling-based Motion PlanningSummaryReferences1Single Robot Motion Planning - IILiang-Jun ZhangCOMP790-058Sep 24, 20082Review: C-spaceWorkspaceConfiguration SpacexyRobot InitialGoalFree ObstacleC-obstacleA 2D Translating Robot3Review: Computing C-obstacle•Difficult due to geometric and space complexity•Practical solutions are only available for–Translating rigid robots: Minkowski sum–Robots with no more than 3 DOFs4Outline•Approximate cell decomposition•Sampling-based motion planning5Approximate Cell Decomposition (ACD)•Not compute the free space exactly at once•But compute it incrementally•Relatively easy to implement–[Lozano-Pérez 83]–[Zhu et al. 91]–[Latombe 91]–[Zhang et al. 06]Octree decomposition6full mixedemptyApproximate Cell Decomposition•Full cell•Empty cell •Mixed cell–Mixed–Uncertain•Cell labelling algorithms–[Zhang et al 06]Configuration Space7Finding a Path by ACDGoalInitial8Connectivity GraphGf : Free Connectivity Graph G: Connectivity GraphGf is a subgraph of G9Finding a Path by ACDGoalInitialGf : Free Connectivity Graph10Finding a Path by ACDL: Guiding Path•First Graph Cut Algorithm–Guiding path in connectivity graph G–Only subdivide along this path–Update the graphs G and Gf11First Graph Cut AlgorithmOnly subdivide the cells along LL : Guiding Pathnew Gf12Finding a Path by ACDGf•A channel•Can be used for path smoothing.13ACD for Path Non-existenceC-spaceGoalInitial14Connectivity Graph Guiding PathACD for Path Non-existence15ACD for Path Non-existenceConnectivity graph is not connectedNo path!A sufficient condition for deciding path non-existence16•Live Demo–Gear-2DOF–Gear-3DOF17Five-gear ExampleVideoInitial Goal roadmap in free spaceTotal timing 85s# of total cells 168K18Two-gear Exampleno path!Cells in C-obstacleInitial Goal Roadmap in FVideo3.356s19Motion Planning Framework•Continuous representation•Discretization •Graph search20Summary: Approximate Cell Decomposition •Simple and easy to implement •Efficient and practical for low DOF robots –Inefficient for 5 or more DOFs robot•Resolution-complete–Find a path if there is one–Otherwise, report path non-existence–Up to some resolution of the cell21Outline•Approximate cell decomposition•Sampling-based motion planning22Motivation• Geometric complexity• Space dimensionality23Probabilistic Roadmap (PRM)free spaceqqinitinitqqgoalgoalmilestone[Kavraki, Svetska, Latombe,Overmars, 95]local path24Basic PRM AalgorithmInput: geometry of the moving object & obstaclesOutput: roadmap G = (V, E)1: V   and E  .2: repeat3: q  a configuration sampled uniformly at random from C.4: if CLEAR(q)then5: Add q to V.6: Nq  a set of nodes in V that are close to q.6: for each q’ Nq, in order of increasing d(q,q’)7: if LINK(q’,q)then8: Add an edge between q and q’ to E.25Two Geometric Primitives in C-space•CLEAR(q)Is configuration q collision free or not?•LINK(q, q’) Is the straight-line path between q and q’ collision-free?26Query Processing•Connect qinit and qgoal to the roadmap•Start at qinit and qgoal, perform a random walk, and try to connect with one of the milestones nearby•Try multiple times27Two Tenets of PRM PlanningChecking sampled configurations and connections between samples for collision can be done efficiently.  Hierarchical collision checking[Hierarchical collision checking methods were developed independently from PRM, roughly at the same time]A relatively small number of milestones and local paths are sufficient to capture the connectivity of the free space. Exponential convergence in expansive free space (probabilistic completeness)28Why does it work? Intuition•A small number of milestones almost “cover” the entire free space.29Two Tenets of PRM PlanningChecking sampled configurations and connections between samples for collision can be done efficiently.  Hierarchical collision checking[Hierarchical collision checking methods were developed independently from PRM, roughly at the same time]A relatively small number of milestones and local paths are sufficient to capture the connectivity of the free space. Exponential convergence in expansive free space (probabilistic completeness)30Narrow Passage Problem•Narrow passages are difficult to be sampled due to their small volumes in C-spaceNarrow passageAlpha puzzleqinitqgoal2F3FConfiguration Space1F31Difficulty•Many small connected components32Strategies to Improve PRM•Where to sample new milestones?–Sampling strategy•Which milestones to connect?–Connection strategy•Goal: –Minimize roadmap size to correctly answermotion-planning queries33Sampling Strategies34small visibility setsgood visibilitypoor visibilityPoor Visibility in Narrow Passages•Non-uniform sampling strategies35But how to identify poor visibility regions?• What is the source of information?–Robot and environment geometry •How to exploit it?–Workspace-guided strategies–Dilation-based strategies–Filtering strategies–Adaptive strategies36Identify narrow passages in the workspace and map them into the configuration spaceWorkspace-guided strategies37FODilation-based strategies •During roadmap construction, allow milestones with small penetration•Dilate the free space–[Hsu et al. 98, Saha et al. 05, Cheng et al. 06, Zhang et al. 07]A milestone with small penetration38Error•If a path is returned, the answer is always correct.•If no path is found, the answer may or may not be correct. We hope it is correct with high probability.39Weaker