motion planning cs 6160, spring 2010 by gene peterson 5/4/2010
TRANSCRIPT
![Page 1: Motion Planning CS 6160, Spring 2010 By Gene Peterson 5/4/2010](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649cd65503460f9499d44e/html5/thumbnails/1.jpg)
Motion PlanningCS 6160, Spring 2010
By Gene Peterson
5/4/2010
![Page 2: Motion Planning CS 6160, Spring 2010 By Gene Peterson 5/4/2010](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649cd65503460f9499d44e/html5/thumbnails/2.jpg)
What is Motion Planning?
• Given a set of obstacles, a start position, and a destination position, find a path from start to destination that doesn’t go through any obstacles
• This project seeks shortest path• Many variations on the basic problem
![Page 3: Motion Planning CS 6160, Spring 2010 By Gene Peterson 5/4/2010](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649cd65503460f9499d44e/html5/thumbnails/3.jpg)
![Page 4: Motion Planning CS 6160, Spring 2010 By Gene Peterson 5/4/2010](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649cd65503460f9499d44e/html5/thumbnails/4.jpg)
Program
• Two parts: Course builder and simulator• Course Builder: made in C# using XNA and
Windows forms• User-friendly interface• Simulator: approximately 2000 lines of code• OpenGL rendering
![Page 5: Motion Planning CS 6160, Spring 2010 By Gene Peterson 5/4/2010](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649cd65503460f9499d44e/html5/thumbnails/5.jpg)
Potential Project Goals
• Point object path finding• Polygonal object finding• Robot rotation• Real-time calculation• Non-convex obstacles• Moving obstacles• Curves• No global vision• 3D problem• Path weighting• Group travel
![Page 6: Motion Planning CS 6160, Spring 2010 By Gene Peterson 5/4/2010](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649cd65503460f9499d44e/html5/thumbnails/6.jpg)
Procedure
• Define input: obstacles, “robot”, starting position, and destination
• Create a visibility graph• Find the shortest path from start to
destination in the graph
![Page 7: Motion Planning CS 6160, Spring 2010 By Gene Peterson 5/4/2010](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649cd65503460f9499d44e/html5/thumbnails/7.jpg)
Required Geometry
• Point• Vector• Line• Line Segment• Ray• Poly Line• Polygon
![Page 8: Motion Planning CS 6160, Spring 2010 By Gene Peterson 5/4/2010](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649cd65503460f9499d44e/html5/thumbnails/8.jpg)
Defining a Polygon
• Ordered set vertices• Edges implicitly defined by adjacency in the
vertex list• May be clockwise or counter-clockwise wound
![Page 9: Motion Planning CS 6160, Spring 2010 By Gene Peterson 5/4/2010](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649cd65503460f9499d44e/html5/thumbnails/9.jpg)
Open or Closed Polygons?Open Closed
Does it matter?
![Page 10: Motion Planning CS 6160, Spring 2010 By Gene Peterson 5/4/2010](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649cd65503460f9499d44e/html5/thumbnails/10.jpg)
Open vs Closed Polygon
No Intersection
Intersection
![Page 11: Motion Planning CS 6160, Spring 2010 By Gene Peterson 5/4/2010](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649cd65503460f9499d44e/html5/thumbnails/11.jpg)
Open vs Closed Polygons
• Cannot achieve shortest possible path with closed polygons
• Always can get slightly closer
• Open polygons cannot get any closer to the polygon without penetrating it
![Page 12: Motion Planning CS 6160, Spring 2010 By Gene Peterson 5/4/2010](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649cd65503460f9499d44e/html5/thumbnails/12.jpg)
Course Builder
• Create 2D polygons and lay them out as a course
• Saved to file, loaded by simulator
![Page 13: Motion Planning CS 6160, Spring 2010 By Gene Peterson 5/4/2010](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649cd65503460f9499d44e/html5/thumbnails/13.jpg)
Convex Hull Algorithm #1
• Trivial O(n^3) algorithm• Every pair of vertices is checked to see if it can
be an edge of the convex set
![Page 14: Motion Planning CS 6160, Spring 2010 By Gene Peterson 5/4/2010](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649cd65503460f9499d44e/html5/thumbnails/14.jpg)
Convex Hull Algorithm #2
• Gift wrapping technique• O(n*h)• n: # of vertices• h: # of vertices in the convex hull
![Page 15: Motion Planning CS 6160, Spring 2010 By Gene Peterson 5/4/2010](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649cd65503460f9499d44e/html5/thumbnails/15.jpg)
Convex Hull Algorithm #3
• Graham-Schmidt algorithm• O(n log n)• n: # of vertices• Sorts the points• Didn’t attempt algorithm #4: mix of gift
wrapping with Graham-Schmidt
![Page 16: Motion Planning CS 6160, Spring 2010 By Gene Peterson 5/4/2010](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649cd65503460f9499d44e/html5/thumbnails/16.jpg)
Polygonal Robot
• Instead of a point robot, use a polygon• By calculating the Configuration Space of the
polygonal robot, the robot can be represented by a point
• C-Space is the areas that the point robot cannot move
![Page 17: Motion Planning CS 6160, Spring 2010 By Gene Peterson 5/4/2010](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649cd65503460f9499d44e/html5/thumbnails/17.jpg)
Configuration Space
• Calculate C-Space version of each obstacle using Minkowski sum of the polygon and the robot
![Page 18: Motion Planning CS 6160, Spring 2010 By Gene Peterson 5/4/2010](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649cd65503460f9499d44e/html5/thumbnails/18.jpg)
Minkowski Sum Algorithm #1
• Trivial algorithm - O(n^3)• For each vertex a in polygon A
For each vertex b in polygon BAdd point (a+b) to P
• Minkowski sum is the convex hull of points P
![Page 19: Motion Planning CS 6160, Spring 2010 By Gene Peterson 5/4/2010](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649cd65503460f9499d44e/html5/thumbnails/19.jpg)
Minkowski Sum Algorithm #2
• Sweep method – O(n + m)• n: # of vertices in polygon A• m: # of vertices in polygon B• Sorts the points in each polygon in counter-
clockwise order• Next point is the point with smallest angle
![Page 20: Motion Planning CS 6160, Spring 2010 By Gene Peterson 5/4/2010](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649cd65503460f9499d44e/html5/thumbnails/20.jpg)
Visibility Graph
• Graph vertices represent vertices of the C-Space obstacles and the start and destination points
• Edges represent possible paths• If a line segment from any vertex in the graph to any
other vertex in the graph doesn’t penetrate an obstacle, there’s an edge between those two vertices
• Edge weights represent distance between points• Shortest path in the graph is shortest path through
the obstacles
![Page 21: Motion Planning CS 6160, Spring 2010 By Gene Peterson 5/4/2010](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649cd65503460f9499d44e/html5/thumbnails/21.jpg)
Graph
![Page 22: Motion Planning CS 6160, Spring 2010 By Gene Peterson 5/4/2010](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649cd65503460f9499d44e/html5/thumbnails/22.jpg)
Line Segment-Polygon Intersection
![Page 23: Motion Planning CS 6160, Spring 2010 By Gene Peterson 5/4/2010](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649cd65503460f9499d44e/html5/thumbnails/23.jpg)
Visibility Graph Algorithm #1
• O(n^3)• Create every possible line segment from pairs
of vertices in the graph• Any line segment that doesn’t penetrate an
obstacle gets added as an edge in the graph
![Page 24: Motion Planning CS 6160, Spring 2010 By Gene Peterson 5/4/2010](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649cd65503460f9499d44e/html5/thumbnails/24.jpg)
Visibility Graph Algorithm #2
• O(n^2 log n)• Sweep algorithm• Uses the fact that some edges will obscure
other edges from being visible• Requires a line segment search tree data
structure
![Page 25: Motion Planning CS 6160, Spring 2010 By Gene Peterson 5/4/2010](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649cd65503460f9499d44e/html5/thumbnails/25.jpg)
Ray-Line Segment AVG Tree
• Balanced binary search tree• Given a ray, find the line segment in the tree
that it will intersect first
![Page 26: Motion Planning CS 6160, Spring 2010 By Gene Peterson 5/4/2010](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649cd65503460f9499d44e/html5/thumbnails/26.jpg)
Shortest Path
• Now that we have an obstacle course and a visibility graph for it, we can calculate the shortest path using graph algorithms
![Page 27: Motion Planning CS 6160, Spring 2010 By Gene Peterson 5/4/2010](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649cd65503460f9499d44e/html5/thumbnails/27.jpg)
Dijkstra’s Algorithm
• Breadth first search with weights• Only non-negative edges• Requires priority queue
![Page 28: Motion Planning CS 6160, Spring 2010 By Gene Peterson 5/4/2010](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649cd65503460f9499d44e/html5/thumbnails/28.jpg)
Path Finding in Video Games
• Generally discretised (grid based)• 2D path finding useful in many 3D games• Speed important (1/60th of a second/frame)• Accuracy less important• Much of the environment is static• Precomputation often available
![Page 29: Motion Planning CS 6160, Spring 2010 By Gene Peterson 5/4/2010](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649cd65503460f9499d44e/html5/thumbnails/29.jpg)
Precomputation
• Precalculate visibility graph of C-Space obstacles only (don’t include any start or end point)
• To create the full visibility graph, insert start and destination points (calculating visibility for each polygon vertex for them)
• Use a copy of precalculated visibility graph so it can be reused
• Needs to be recomputed as often as the environment changes
![Page 30: Motion Planning CS 6160, Spring 2010 By Gene Peterson 5/4/2010](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649cd65503460f9499d44e/html5/thumbnails/30.jpg)
Statistics
• TO FILL IN WHEN I HAVE THE FINAL STATISTICS AVAILABLE
![Page 31: Motion Planning CS 6160, Spring 2010 By Gene Peterson 5/4/2010](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649cd65503460f9499d44e/html5/thumbnails/31.jpg)
More Speed?
• Multi-threading• CUDA• Performance optimizations (length squared,
etc)• Minimizing geometry where possible• Sacrifice accuracy• Amortize path finding cost over time
![Page 32: Motion Planning CS 6160, Spring 2010 By Gene Peterson 5/4/2010](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649cd65503460f9499d44e/html5/thumbnails/32.jpg)
Non-Convex Polygons
![Page 33: Motion Planning CS 6160, Spring 2010 By Gene Peterson 5/4/2010](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649cd65503460f9499d44e/html5/thumbnails/33.jpg)
Robot Rotation
• Unlike translation, rotating the robot changes the C-Space
• Even if rotation angles are discretized, greatly enlarges the graph size, and thus the computation time
• Gave up on this problem to pursue other aspects of motion planning
![Page 34: Motion Planning CS 6160, Spring 2010 By Gene Peterson 5/4/2010](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649cd65503460f9499d44e/html5/thumbnails/34.jpg)
Goals Revisited
• Point object path finding• Polygonal object finding• Robot rotation• Real-time calculation• Non-convex obstacles• Moving obstacles• Curves• No global vision• 3D problem• Path weighting• Group travel
Too computationally expensive for real-time
Not as useful in video games, slow
Didn’t get to it
Beyond the scope of this project
AI Problem
AI Problem
![Page 35: Motion Planning CS 6160, Spring 2010 By Gene Peterson 5/4/2010](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649cd65503460f9499d44e/html5/thumbnails/35.jpg)
Further Exploration
• Rotation• Better precomputation• Non-convex performance• Sacrificing accuracy for performance• Regional partitioning
![Page 36: Motion Planning CS 6160, Spring 2010 By Gene Peterson 5/4/2010](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649cd65503460f9499d44e/html5/thumbnails/36.jpg)
Implicit Line Equation from 2 Points
• Given two points on the line, calculate a, b, c
Pick any a and b that make this true
Use any point (x,y) on the line
* Any multiple k ≠ 0 applied to (a, b, c) will result in the same line
![Page 37: Motion Planning CS 6160, Spring 2010 By Gene Peterson 5/4/2010](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649cd65503460f9499d44e/html5/thumbnails/37.jpg)
Half Space (2D)
• Input: Line and a Point• Output: Real (+,-,0)• Geometric Interpretation:
0 means the point is on the line+ means on one side of the line- means on the other side
• Plug point into implicit line equation• Result is the half space that contains the point
(+,-,0)
![Page 38: Motion Planning CS 6160, Spring 2010 By Gene Peterson 5/4/2010](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649cd65503460f9499d44e/html5/thumbnails/38.jpg)
Half Space (2D) cont.
+
-
![Page 39: Motion Planning CS 6160, Spring 2010 By Gene Peterson 5/4/2010](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649cd65503460f9499d44e/html5/thumbnails/39.jpg)
Line-Line Intersection (2D)
• Line 0:• Line 1: • Solve for (x,y): two equations, two unknowns• Three cases:
- No intersection (parallel lines)- Always intersect (same line)- One intersection
![Page 40: Motion Planning CS 6160, Spring 2010 By Gene Peterson 5/4/2010](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649cd65503460f9499d44e/html5/thumbnails/40.jpg)
Line-Line Intersection (2D) cont.
• No intersection (parallel lines)
• Always intersect (same line)
• Otherwise, solve for (x,y)• Constant time
Slope of the two lines are equal:
For k ≠ 0:
But not the same line:
![Page 41: Motion Planning CS 6160, Spring 2010 By Gene Peterson 5/4/2010](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649cd65503460f9499d44e/html5/thumbnails/41.jpg)
Line Segment-Line Segment Intersection
• Find intersection point using line-line intersection (if it exists, if it doesn’t the line segments don’t intersect)
• Create a ray for each line segment• Ray ranges from 0 to 1 along the line segment• Calculate t of the intersection point for each
line segment• If both t’s are in [0,1], line segments intersect
![Page 42: Motion Planning CS 6160, Spring 2010 By Gene Peterson 5/4/2010](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649cd65503460f9499d44e/html5/thumbnails/42.jpg)
Point-Line Intersection
• Calculate the halfspace the point lies in• If halfspace equals 0, point is on the line• Floating point error may play into this• Use an epsilon, point is near close enough to
being on the line
![Page 43: Motion Planning CS 6160, Spring 2010 By Gene Peterson 5/4/2010](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649cd65503460f9499d44e/html5/thumbnails/43.jpg)
Point-Polygon Intersection
• Calculate the half space the point lies in for each edge of the polygon
• If the polygon is clockwise wound, the point must always be in - half space
• If the polygon is counter-clockwise wound, the point must always be in + half space
• O(# of vertices in the polygon)
![Page 44: Motion Planning CS 6160, Spring 2010 By Gene Peterson 5/4/2010](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649cd65503460f9499d44e/html5/thumbnails/44.jpg)
+
+
+
+ -
-
-
-
ClockwiseWinding
+
++
+-
-
-
-
Counter-ClockwiseWinding
![Page 45: Motion Planning CS 6160, Spring 2010 By Gene Peterson 5/4/2010](https://reader035.vdocuments.site/reader035/viewer/2022062515/56649cd65503460f9499d44e/html5/thumbnails/45.jpg)
Polygon-Polygon Intersection
• Possibly intersecting or fully contained• Check each line segment in polygon A for
intersection with each line segment in polygon B
• Check for containment: pick a point from A and see if it’s in B (and do same check to see if B is in A)