Improved Shortest Path Algorithms by Dynamic Graph Decomposition
Graph-based Path Planning - Aalto
Transcript of Graph-based Path Planning - Aalto
![Page 1: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/1.jpg)
Graph-based Path PlanningGraphs to the rescue!!
Dr. Kshitij Tiwari
Dept. of Electrical Engineering and [email protected]
March 31, 2020
![Page 2: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/2.jpg)
Graph-based Path PlanningK.Tiwari
2/75
OverviewBiographyMotivationAboutGraph-based Path Planners
ScenarioGraphsBest-first Search Methods
Dijkstra AlgorithmA* AlgorithmD* Algorithm
Sampling Based MethodsRRTPRM
SummaryCliff HangerReadings
![Page 3: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/3.jpg)
Graph-based Path PlanningK.Tiwari
3/75
Biography
I Bachelors: Electronics &Comm. Engg. (HKU,2013)
I Masters: ArtificialIntelligence (UoE,2014)
I Ph.D.: Robotics(JAIST,2018)
I Postdoc: Brain-inspiredRobotics (Aalto,Present)
Let’s get to business!!!
![Page 4: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/4.jpg)
Graph-based Path PlanningK.Tiwari
4/75
Motivation
I Definition of a robot X
I Robot positioning (Localization) X
I Path planning . . .
![Page 5: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/5.jpg)
Graph-based Path PlanningK.Tiwari
5/75
Motivation (cont.)
Figure: Robotic arm. Figure: Autonomous cars.
All mobile robots need to plan paths.
![Page 6: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/6.jpg)
Graph-based Path PlanningK.Tiwari
6/75
About
I Introductory lecture for graph-based path planning approaches
I Includes several visual propsI Pop Quizzes!! to be discussed on Slack/Email
I For self-learning and are NOT graded
![Page 7: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/7.jpg)
Graph-based Path PlanningK.Tiwari
7/75
Graph-based Path Planners
Figure: Dog and the food scenario.
![Page 8: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/8.jpg)
Graph-based Path PlanningK.Tiwari
8/75
Graph-based Path Planners (cont.)
Figure: Obstructions to direct path to food.
![Page 9: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/9.jpg)
Graph-based Path PlanningK.Tiwari
9/75
Graph-based Path Planners (cont.)
I Direct path blocked by obstacles.I Need to find path to goal.
Figure: Grrrr . . . I will heck you up hooman.
Need a managable representation of search area !!!
![Page 10: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/10.jpg)
Graph-based Path PlanningK.Tiwari
10/75
Graph-based Path Planners (cont.)
I Discretizing the search space.I Pixelation using pixels.I Tesselation using Square/Hexagon/Triangles/Circles. X
I Representing grid as a graph.
I Finding paths on graphs.
![Page 11: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/11.jpg)
Graph-based Path PlanningK.Tiwari
11/75
Graph-based Path Planners (cont.)
Figure: Simplifying search area⇒ Grid World.
![Page 12: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/12.jpg)
Graph-based Path PlanningK.Tiwari
12/75
Graph-based Path Planners (cont.)
Graph descriptors:I Nodes: The locations represented by circles.
I Edges: The lines connecting the nodes.
I Path: The collection of edges and nodes which define pathsfrom start to goal.
Let’s look at basic types of graphs.
![Page 13: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/13.jpg)
Graph-based Path PlanningK.Tiwari
13/75
Graph-based Path Planners (cont.)
Figure: Fully connected graphical representation with bi-directional edges.
![Page 14: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/14.jpg)
Graph-based Path PlanningK.Tiwari
14/75
Graph-based Path Planners (cont.)
Figure: Weighted graphical representation.
![Page 15: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/15.jpg)
Graph-based Path PlanningK.Tiwari
15/75
Graph-based Path Planners (cont.)
Figure: Directed (Acyclic) graphical representation.
![Page 16: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/16.jpg)
Graph-based Path PlanningK.Tiwari
16/75
Graph-based Path Planners (cont.)
Remarks:I Graph-based approaches deal with graphs only.I They do not care about:
I Indoor/outdoorI Walls/doors/windows
I They only care about:I ObstaclesI Free space
I Graph→ Path. Process graph to find pathI Edges are abstract concepts.
I Explain which nodes are reachable but NOT how.
![Page 17: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/17.jpg)
Graph-based Path PlanningK.Tiwari
17/75
Graph-based Path Planners (cont.)
Figure: Graphical representations of scenario.
![Page 18: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/18.jpg)
Graph-based Path PlanningK.Tiwari
18/75
Graph-based Path Planners (cont.)
Path finding in graphs can be done via:1. Best-first Search Methods
I Dijkstra:I prioritizes exploration over low-cost paths.
I A*I bias search towards goal using heuristics.
I D*I using heuristics and tackle dynamic obstacles.
2. Sampling Based MethodsI RRT
I Single-query Sampling based search.I PRM
I Multi-query Sampling based search.
Let’s dive deeper!!
![Page 19: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/19.jpg)
Graph-based Path PlanningK.Tiwari
19/75
Best-first Search Methods
Dijkstra Algorithm
I Calculates shortest path.I Chosen root node to every other node in graph.
I Needs weighted graphs.
Figure: Recap: Weighted graphical representation.
![Page 20: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/20.jpg)
Graph-based Path PlanningK.Tiwari
20/75
Best-first Search Methods (cont.)
Figure: Step 1. Select a node and mark with star symbol. Mark costs ofselected node as 0 and others as∞.
![Page 21: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/21.jpg)
Graph-based Path PlanningK.Tiwari
21/75
Best-first Search Methods (cont.)
Figure: Step 2. Associate cost for neighbors of selected node.
![Page 22: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/22.jpg)
Graph-based Path PlanningK.Tiwari
22/75
Best-first Search Methods (cont.)
Figure: Step 3. Choose the cheapest node and repeat while ignoringvisited nodes. Pick any in case of identical cost.
![Page 23: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/23.jpg)
Graph-based Path PlanningK.Tiwari
23/75
Best-first Search Methods (cont.)
Pop-Quiz (PQ1)!!
I What is the shortest path for doggo based on this graph?I What is the net cost?
![Page 24: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/24.jpg)
Graph-based Path PlanningK.Tiwari
24/75
Best-first Search Methods (cont.)
Dijkstra(StartNode,G)
1: Input:I StartNode : node representing start positionI G : entire graph of state space
2: Output:I ShortestPath : list of node that form shortest path from
start to goal
![Page 25: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/25.jpg)
Graph-based Path PlanningK.Tiwari
25/75
Best-first Search Methods (cont.)
3: V isited← StartNode;Unvisited← StartNode \G4: do5: CurrNode← arg min∀u∈Unvisited DIST(V isited[−1], u) .
Cheapest node as current node6: NN ← Neighbors(CurrNode,G) . Extract 1-hop neighbors7: ∀n ∈ NN ← minCost(n), Cost(CurrNode, n). Update Costs8: V isited← ∪ CurrNode . Mark current node as visited9: while Unvisited! = NULL . Repeat until all nodes visited
![Page 26: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/26.jpg)
Graph-based Path PlanningK.Tiwari
26/75
Best-first Search Methods (cont.)
Figure: Showcasing the progress of Dijkstra. N.B.: Lot of wasteful steps.Image c©: Subhrajit Bhattachary
![Page 27: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/27.jpg)
Graph-based Path PlanningK.Tiwari
27/75
Best-first Search Methods
A* Algorithm
Overview:I Dijkstra wastes a lot of steps
I Expansions not always towards the goalI Some method to bias the expansion of needs required
I Use heuristics to bias the expansion
This heuristic expansion is then called A* algorithm.
![Page 28: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/28.jpg)
Graph-based Path PlanningK.Tiwari
28/75
Best-first Search Methods (cont.)
Components for A* based planning:I Definition of valid actions.
I Definition of grid resolution.
I Definition of cost associated with movement.
![Page 29: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/29.jpg)
Graph-based Path PlanningK.Tiwari
29/75
Best-first Search Methods (cont.)
Figure: Valid actions from a grid cell. No diagonal movements for simplicity.
![Page 30: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/30.jpg)
Graph-based Path PlanningK.Tiwari
30/75
Best-first Search Methods (cont.)
Figure: Arbitrary grid resolution.
![Page 31: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/31.jpg)
Graph-based Path PlanningK.Tiwari
31/75
Best-first Search Methods (cont.)
The path cost encompasses two kinds of costs:
1. Transition Cost:The cost incurred when moving from one grid to its immediateneighbor.
2. Net Estimated Cost:This cost is an estimate (heuristic) of the overall path cost to beincurred from start to goal.
![Page 32: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/32.jpg)
Graph-based Path PlanningK.Tiwari
32/75
Best-first Search Methods (cont.)
Let us look at a worked example: Helping the dog get to its food.Notational Convention:
I Bottom Left: Transition cost from start to current grid
I Bottom Right: Heuristic cost from current node to goal usingManhattan distance
![Page 33: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/33.jpg)
Graph-based Path PlanningK.Tiwari
33/75
Best-first Search Methods (cont.)
Figure: Step 1. 2 cells have same cost. Pick LEFT.
![Page 34: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/34.jpg)
Graph-based Path PlanningK.Tiwari
34/75
Best-first Search Methods (cont.)
Figure: Step 2. Action UP has lowest cost.
![Page 35: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/35.jpg)
Graph-based Path PlanningK.Tiwari
35/75
Best-first Search Methods (cont.)
Figure: Step 3. Tied low-cost so, continue with previous action:UP
![Page 36: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/36.jpg)
Graph-based Path PlanningK.Tiwari
36/75
Best-first Search Methods (cont.)
Figure: Step 4. Action:LEFT
![Page 37: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/37.jpg)
Graph-based Path PlanningK.Tiwari
37/75
Best-first Search Methods (cont.)
Pop-Quiz (PQ2)!!
I How many steps finally?I Final cost?I Final path?I Type of graph?
![Page 38: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/38.jpg)
Graph-based Path PlanningK.Tiwari
38/75
Best-first Search Methods (cont.)
Need to find the shortest path to goal autonomously !!!
I Use A* algorithm.I Additionally, define 2 kinds of nodes:
I Open Node:consists on nodes that have been visited but not expanded(meaning that sucessors have not been explored yet). This is thelist of pending tasks.
I Close Node:consists on nodes that have been visited and expanded(sucessors have been explored already and included in the openlist, if this was the case).
![Page 39: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/39.jpg)
Graph-based Path PlanningK.Tiwari
39/75
Best-first Search Methods (cont.)A*(StartNode,GoalNode)
1: Input:I StartNode : node representing start positionI GoalNode : node representing goal position
2: Output:I ShortestPath : list of node that form shortest path from
start to goal3: OpenList = [];ClosedList = [];ChildNodes = []4: OpenList← StartNode . Store Start Loc5:
6: do7: CurrNode← arg min∀n∈OpenList Cost(StartNode, n)8: OpenList← CurrNode . Remove from OpenList9: ClosedList← CurrNode
![Page 40: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/40.jpg)
Graph-based Path PlanningK.Tiwari
40/75
Best-first Search Methods (cont.)
10: if CurrNode == GoalNode then11: print("Arrived at Goal")12: break . Terminate13: end if14: ChildNodes← GenChild(CurrNode)15: for ∀c ∈ ChildNodes do16: if c ∈ ClosedList then17: pass
18: end if19: c.f, c.t, c.e← Cost(StartNode, c,GoalNode)20: if c ∈ OpenList then21: if c.t > on.t∀on ∈ OpenList then22: pass
23: end if
![Page 41: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/41.jpg)
Graph-based Path PlanningK.Tiwari
41/75
Best-first Search Methods (cont.)
24: end if25: OpenList← c . Store child26: end for27:
28: while OpenList! = NULL29: Return ShortestPath(StartNode,GoalNode, ClosedList)
![Page 42: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/42.jpg)
Graph-based Path PlanningK.Tiwari
42/75
Best-first Search Methods (cont.)
Cost(StartNode, CurrNode,GoalNode, StepCost = 1)
1: Input:I StartNode : node representing start positionI CurrNode : node representing current positionI GoalNode : node representing goal positionI StepCost : fixed cost of transitioning to immediate neighbors
(children)2: Output:
I f : sum of transition and net estimated costs from currentnode
I t : transition costI e : net estimation cost
![Page 43: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/43.jpg)
Graph-based Path PlanningK.Tiwari
43/75
Best-first Search Methods (cont.)
3: t = DIST(CurrNode, StartNode) + StepCost4: e = DIST(CurrNode,GoalNode)5: f = t + e6: Return{f, t, e}
![Page 44: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/44.jpg)
Graph-based Path PlanningK.Tiwari
44/75
Best-first Search Methods (cont.)
GenChild(CurrNode)
1: Input:I CurrNode : node representing current position
2: Output:I ChildNodes : all adjacent (valid) nodes of current node
3: ChildNodes← isNeighbor(CurrNode) . Get all children
![Page 45: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/45.jpg)
Graph-based Path PlanningK.Tiwari
45/75
Best-first Search Methods (cont.)
Figure: Showcasing the progress of A*. Image c©: Subhrajit Bhattachary
![Page 46: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/46.jpg)
Graph-based Path PlanningK.Tiwari
46/75
Best-first Search Methods (cont.)
Pop-Quiz (PQ3)!!In Romeo and Juliet, Shakespeare said “What’s in a name?”. If thereis nothing, is A* just an arbitrary name?
![Page 47: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/47.jpg)
Graph-based Path PlanningK.Tiwari
47/75
Best-first Search Methods
D* Algorithm
Figure: Dog and the food but with dynamic obstacles also present.
![Page 48: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/48.jpg)
Graph-based Path PlanningK.Tiwari
48/75
Best-first Search Methods (cont.)
I Presence of dynamic obstacles makes planning challenging.I A* re-planning is costly.
I Solution: D* Search Algorithm
![Page 49: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/49.jpg)
Graph-based Path PlanningK.Tiwari
49/75
Best-first Search Methods (cont.)D*(StartNode,GoalNode)
1: Input:I StartNode : node representing start positionI GoalNode : node representing goal position
2: Output:I ShortestPath : list of node that form shortest path from
start to goal3: OpenList = [];ClosedList = [];ChildNodes = []4: OpenList← StartNode . Store Start Loc5:
6: do7: CurrNode← arg min∀n∈OpenList Cost(StartNode, n)8: OpenList CurrNode . Remove from OpenList9: ClosedList← CurrNode
![Page 50: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/50.jpg)
Graph-based Path PlanningK.Tiwari
50/75
Best-first Search Methods (cont.)10: if CurrNode == GoalNode then11: print("Arrived at Goal")12: break . Terminate13: end if14: ChildNodes← GenChild(CurrNode)15: for ∀c ∈ ChildNodes do16: . . . . Same as A*17: end for18: if EnvChange() then . If environment changed19: ChildNodes← GenChild(CurrNode) . Revise nodes20: UpdateCost(ChildNodes) . Update affected children21: end if22: while OpenList! = NULL23: Return ShortestPath(StartNode,GoalNode, ClosedList)
![Page 51: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/51.jpg)
Graph-based Path PlanningK.Tiwari
51/75
Best-first Search Methods (cont.)
Remarks:I D* is quite similar to A* but . . .
I Allows changes in environmentsI Recalculation only for affected nodes
![Page 52: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/52.jpg)
Graph-based Path PlanningK.Tiwari
52/75
Best-first Search Methods (cont.)
Pop-Quiz (PQ4)!!Is D* just an arbitrary name?
![Page 53: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/53.jpg)
Graph-based Path PlanningK.Tiwari
53/75
Best-first Search Methods
Pop-Quiz (PQ5)!!
I If you lookup the literature, you will find the mention ofConfiguration Spaces and Work Spaces. Can youexplain the difference(s) between them?
I How do they relate to the doggo example?
![Page 54: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/54.jpg)
Graph-based Path PlanningK.Tiwari
54/75
Sampling Based Methods
RRT
I Search-space size is a bottleneck for A* and D*.I E.g.:
I Dog navigation from 2D to UAV navigation in 3D.I 7DOF robot arm navigation in ND.
I Solution:I Uniform Grid→ random samplingI Rapidly-exploring Random Trees a.k.a. RRT
![Page 55: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/55.jpg)
Graph-based Path PlanningK.Tiwari
55/75
Sampling Based Methods (cont.)
RRT Jargon:
I Root: Just like the root of tree (start node).
I Goal: Goal node which must be reached eventually.
I Rand: Randomly generated/sampled node in space to expandtree.
I Next: Closest node from tree to the Rand node in its direction.
![Page 56: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/56.jpg)
Graph-based Path PlanningK.Tiwari
56/75
Sampling Based Methods (cont.)
Figure: RRT setup for dog-food scenario. Root node shown in blue andGoal node shown in green.
![Page 57: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/57.jpg)
Graph-based Path PlanningK.Tiwari
57/75
Sampling Based Methods (cont.)
Figure: Step 1. Generate random sample and find the next/nearest nodefrom free.
![Page 58: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/58.jpg)
Graph-based Path PlanningK.Tiwari
58/75
Sampling Based Methods (cont.)
Figure: Step 2. Generate new random sample and repeat Step 1 withnearest neighbor.
![Page 59: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/59.jpg)
Graph-based Path PlanningK.Tiwari
59/75
Sampling Based Methods (cont.)
Figure: Step 3. Generate new random sample and ignore if obstructed.
![Page 60: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/60.jpg)
Graph-based Path PlanningK.Tiwari
60/75
Sampling Based Methods (cont.)
Figure: Step N . Final tree (arrival at Goal).
![Page 61: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/61.jpg)
Graph-based Path PlanningK.Tiwari
61/75
Sampling Based Methods (cont.)
RRT(StartNode,GoalNode,NumV ert, StepSize)
1: Input:I StartNode : node representing start position (root)I GoalNode : node representing goal positionI NumV ert : number of vertices in treeI StepSize : incremental distance for tree expansion
2: Output:I G : RRT Graph
![Page 62: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/62.jpg)
Graph-based Path PlanningK.Tiwari
62/75
Sampling Based Methods (cont.)
3: G← StartNode4: for v = 1, . . . , NumV ert do5: Rand← GetRandVert() . Get random config6: Next← GetNearestVert(Rand,G,StepSize) . Get Nearest
vertex7: if Next ∈ FreeSpace then8: G← Next . Move if free9: end if
10: end for
![Page 63: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/63.jpg)
Graph-based Path PlanningK.Tiwari
63/75
Sampling Based Methods (cont.)
Figure: Showcasing the progress of RRT. Image c©: Steven LaValle
![Page 64: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/64.jpg)
Graph-based Path PlanningK.Tiwari
64/75
Sampling Based Methods (cont.)
Remarks:I RRT is single-query
I Rooted at start location of dog.I Dog moves→ renew tree from new root.I Goal moves→ renew tree.I Multiple goals→ handle one start-goal configuration at a time.I Valid only for one-time querying.I Static obstacles only.
![Page 65: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/65.jpg)
Graph-based Path PlanningK.Tiwari
65/75
Sampling Based Methods
PRMOverview:
I Stands for Probabilistic Roadmaps.I are multi-query in nature.
I Roadmaps “rooted” to environment configurations.I Independent of location of dog.
I Randomly samples points in state space and connects peers.I Static environments only.
![Page 66: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/66.jpg)
Graph-based Path PlanningK.Tiwari
66/75
Sampling Based Methods (cont.)
Figure: PRM setup. Roadmap around static obstacles irrespective of dogand food location.
![Page 67: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/67.jpg)
Graph-based Path PlanningK.Tiwari
67/75
Sampling Based Methods (cont.)
Figure: Connect start and goal nodes to roadmap.
![Page 68: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/68.jpg)
Graph-based Path PlanningK.Tiwari
68/75
Sampling Based Methods (cont.)
Figure: Find shortest, obstacle-free to food.
![Page 69: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/69.jpg)
Graph-based Path PlanningK.Tiwari
69/75
Sampling Based Methods (cont.)
PRM(StartNode,GoalNode, k,NumV ert)
1: Input:I StartNode : node representing start position (root)I GoalNode : node representing goal positionI k : number of nearest neighborsI NumV ert : number of vertices in tree
2: Output:I R : Roadmap
![Page 70: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/70.jpg)
Graph-based Path PlanningK.Tiwari
70/75
Sampling Based Methods (cont.)
3: R← StartNode4: while |R| < NumV ert do . Comp. constraint5: Rand← GetRandVert() . Get random config6: if Rand ∈ FreeSpace then7: R← Rand . Store if free8: end if9: end while
10: for v = 1, . . . , |R| do11: kNN ← GetNeighbors(k,R[v]) . Get k-neighbors12: for ∀nn ∈ kNN do13: if (v, nn)@E and ∆(v, nn) 6= NULL then14: R← nn . Store if valid and novel15: end if16: end for
![Page 71: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/71.jpg)
Graph-based Path PlanningK.Tiwari
71/75
Sampling Based Methods (cont.)
17: end for
Pop-Quiz (PQ6)!!What is “probabilistic” about PRMs?
![Page 72: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/72.jpg)
Graph-based Path PlanningK.Tiwari
72/75
Summary
I Introduction to Graph-searchI Exposure to Best-first search approaches
I DijkstraI A*I D*
I Exposure to Sampling-based approachesI RRTI PRM
I Worked examples and pseudo-codesI Animations as visual props to ease understandingI Several other variants not covered herewithI Several pop quizzes for open discussion
![Page 73: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/73.jpg)
Graph-based Path PlanningK.Tiwari
73/75
Cliff Hanger
Ever seen a dog performgraph-search?
Pop-Quiz (PQ7)!!How does one represent the dog-sniffing behavior?
![Page 74: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/74.jpg)
Graph-based Path PlanningK.Tiwari
74/75
Books
![Page 75: Graph-based Path Planning - Aalto](https://reader033.fdocuments.net/reader033/viewer/2022050313/626f6f1cbea13976417ad486/html5/thumbnails/75.jpg)
Graph-based Path PlanningK.Tiwari
75/75
Thank You!!