O Rourke Chapter 8 Motion Planning

Transcription

1 UMass Lowell Computer Science Advanced Algorithms Computational Geometry Prof. Karen Daniels Spring, 2010 O Rourke Chapter 8 Motion Planning

2 Chapter 8 Motion Planning Shortest Paths (robot = point) Moving a Disk Translating a Convex Polygon Moving a Ladder Robot Arm Motion Separability

3 Problem Specification s Assume fixed environment of impenetrable objects e.g. Polygons or polyhedra Robot: movable object at initial position e.g. point robot starting at s Goal: : Plan motion to final position (e.g. t) avoiding robot penetration of objects Sliding contact is acceptable Questions: Does there exist a free (collision-avoiding) path? How to construct such a path? How to find shortest such path? t

4 Shortest Paths s Reduce possibilities to finite list! Shortest path segment endpoint vertex of obstacle (consider (consider s,t as point obstacles) Shortest path is subpath of visibility graph of vertices of obstacle polygons visibility graph requires Ω(n 2 ) time t Algorithm: : DIJKSTRA S S ALGORITHM T {s} while t not in T do Find edge e in (G( \ T) ) that augments T to reach a node x whose distance from s is minimum T T + {e}{ Assume: - Polygonal obstacles have total of n vertices - Points s, t are outside obstacles - Points s, t are degenerate polygons - Obstacles are disjoint

5 Moving a Disk Revised Goal: find any path to t not necessarily shortest path Shrink disk to a point Grow obstacles by disk radius Form union of grown obstacles If t, s in same component of plane, there is a free path find it by modifying visibility graph to include circular arcs from grown obstacles s s t t O(n 2 lg n)

6 Why Does it Work? Minkowski Sum: vector sum for point sets A = { a + b a A, b } a a+b b a 4 a 1 a 2 a 3 a 4 +b 3 a 3 +b 2 A b 3 b 2 A Simple Case b 1 a 2 +b 1 Convex/Convex Case

7 Why Does it Work? (continued) ( a 5 + b4 ),( a5 + b3 ) ( a3 + b4 ),( a4 + b4 ) a 4 a a 5 a 5 +b 4 a 4 +b 4 3 a 1 a a a 5 +b 3 3 +b 4 2 a 2 +b 3 b 4 b 3 a 3 +b 2 A b 2 a 3 +b 1 a 2 +b 2 b 1 ( a 3 + b1 ),( a3 + b2 ) ( a2 + b2 ),( a2 + b3 ) Polygonal Nonconvex/Nonconvex Case

8 Minkowski Sum: Some Properties Minkowski Sum: Some Properties Shape is Shape is translationally translationally invariant invariant Commutative Commutative Union formulation Union formulation When A, convex, sum is convex When A, convex, sum is convex ) ( ) ( ) ( t A t A t A + = + = + A A = U U U b A a A b a A b a b a A + = + = + = } { } { } {,

9 Minkowski Sum: Properties (continued) TRANSLATIONAL INTERSECTION ( + t) A iff t ( A ( )) Why?? t a b Consider Simple Case: a=a, b= b + t = a t = a b Note: -b b = b rotated by π

10 Minkowski Sum: Properties (continued) TRANSLATIONAL INTERSECTION ( + t) A iff t ( A ( )) A ( ) - t A

11 Polygon Motion Planning To plan motion for a shape P amidst polygonal obstacles U set of all displacements of P relative to U such that (translated) P intersects U: set of all displacements of P relative to U such that (translated) P does not intersect U: If t, s in same component of plane, there is a free path U t find it by modifying visibility graph to include circular arcs from grown obstacles U U U ( P) ( P) t P s s

12 Minkowski Sum Algorithms Algorithms for Constructing the Minkowski Sum of 2 Polygons A and A [n vertices] [m vertices] Size Running Time Convex Convex O(n+m) O(n+m) Convex NonConvex O(nm) O(nm) NonConvex NonConvex O(n 2 m 2 ) O(n 2 m 2 ) Convex A, NonConvex A, a 4 a 5 a 3 a 3 supports b 3 b 4 a 1 a 2 b 4 b 3 b 2 b 3 supports a 2 a 3 merge edge copies in slope order b 1 Identify vertex/edge support pairs

13 Minkowski Sum Exercises These statements are about Minkowski sums for 2 2D point sets A and : (a) provide a counterexample that shows this is false: A = A ( ) (b) prove this is true (( A) ) = A ( ) -

14 Moving a Ladder Rotation adds degree of freedom - makes the configuration space 3D Two Methods to find free path through configuration space: 1) Cell decomposition 2) Retraction Source: O RourkeO

15 Configuration space if robot and obstacles can have circular arcs (smoothly joined) Source: PhD dissertation by Steven Cy Trac,, U. Miami

16 Moving a Ladder through Polygonal Obstacles (continued) Find Free Path through Configuration Space Cell decomposition method Partition configuration space into finite number of well-behaved cells For a single orientation Cell = connected region in free space of appropriate configuration space Connectivity graph represents cell structure Identify O(n 2 ) critical orientations where combinatorial structure of connectivity graph changes Alignment of ladder with either obstacle edges or 2 obstacle vertices. Form overall connectivity graph Determine a path in the space by finding a path between cells dual graph G Θ reference point Shaded regions comprise free space. No path in G 0 from A to C. disconnect Disconnect for this angle too! obstacles Source: O RourkeO

17 Moving a Ladder (continued) Find Free Path through Configuration Space Retraction method Construct Voronoi diagram of obstacles for (fixed orientation of) ladder L Set of free points x such that, when ladder s reference point is placed at x, L is equidistant from >= 2 obstacle points Distance of point p to ladder L is minimum length of any line segment from p to a point on L. Stack Voronoi diagrams for successive angles to form twisted sheets Perform path planning in network of diagram formed by ribs between Voronoi sheets rib is place where 2 sheets meet Voronoi vertex is equidistant from at least 3 obstacle points. obstacles Moving L so its reference point stays on diagram edges places L as far from nearby obstacles as possible. a Voronoi diagram edge Source: O RourkeO

18 Moving a Ladder (continued) Find Free Path through Configuration Space 2D Time Complexity Authors Date Shwartz, Sharir 1983 O Dunlaing et al Leven, Sharir 1987 Sifrony, Sharir 1987 Vegter 1990 O Rourke 1985b 3D Time Complexity Authors Date Shwartz, Sharir 1984 Ke,, O RourkeO 1987 Canny 1987 Ke,, O RourkeO 1988 Time Complexity O( n 5 ) O( n 2 log nlog* n) O( n 2 log n) O( n 2 log n) O( n 2 ) Ω( n 2 ) Time Complexity O( n 11 ) O( n 6 log n) O( n 5 log n) Ω( n 4 ) Source: O RourkeO Canny (1987): Any motion planning problem in which the robot has d degrees of motion freedom can be solved in O(n d logn) ) time.

19 Robot Arm Motion Planar, multilink arm links L 1, L 2,.., L n, connected at joints J 0, J 1, J 2,.., J n joint J 0 anchored at origin no obstacles arm may self-intersect origin = J 0 J 1 L 1 L 2 tip of arm can tip of arm reach this? L 1 can reach all points on this circle L 2 can reach all points on each such circle centered on a point of L 1 s s circle Reachable region for an n-link n arm is an annulus centered on the origin

20 Robot Arm Motion: Reachability Region Two cases showing reachability region for a 2-link 2 arm is an annulus centered on the origin Source: O RourkeO

21 Robot Arm Motion: Reachability Region Reachability region is independent of order in which links are arranged. justify using parallelogram determined by link vector sum (commutativity of vector addition) so, assume w.l.o.g.. first link is longest Theorem 8.6.3: Reachability region for n-link arm n is origin-centered annulus with outer radius r0 = l i i= 1 and inner radius ri = 0 if longest link lm is at most half the total length of links, and ri = lm li otherwise. i M r l ( l l ) i = l L 4 L 3 L 2 L 1

22 Robot Arm Motion: Finding Configurations Find a single solution Can arm tip reach p? 2-Link Case Intersect circle C 1 of radius l 1 (centered on origin J 0 ) with circle C 2 of radius l 2 (centered on origin p). In general there are 2 solutions (depends on how circles intersect) C 1 l 1 C 2 l 2 p = point to be reached Source: O RourkeO

23 Robot Arm Motion: Finding Configurations Theorem: : Every 3-link problem can be solved by one of these 2-link problems: (l 1 + l 2, l 3 ) [Fig 8.22(a)] (l 1, l 2 + l 3 ) [Fig 8.22(b),(c), Fig 8.23] j 0 = 0 and (l( 2, l 3 ) [Fig 8.22(d)] j 0 = 1st joint angle oundary of annulus represents extreme 2-link 2 configurations for single link of length l 1 +l 2 or l 1 -l 2 Align L with L 1 2 Case 1: R C 0/ Anti-Align L with L 1 2 C does not enclose J 0 Case 2 : R C = 0/ Align L with L 2 3 O C is centered at p and has radius l 3. Solution exists for every j 0! I C encloses J 0 Alternative to Anti-Aligning L with L 1 2 (align L with L 2 3 ) Source: O RourkeO

24 Robot Arm Motion: Finding Configurations Recursive, linear algorithm for n-link reachability: annulus R represents n-1 links of n-link arm with circle C of radius l n centered on p cases of Figure 8.22 apply Case 1: R C 0/ [Fig 8.22(a),(b)] Choose one of (in general) eral) 2 points of intersection Case 2 : R C [Fig 8.22(c),(d)] Choose any point on C (e.g. furthest from J 0 ) Recursively find configuration for A n-1 =(l 1,,l,l n-1 ) Append last link L n to this solution to connect to p Given point p to reach, first determine if p is reachable (via Theorem 8.6.3); if so, find configuration recursively. Source: O RourkeO

25 Robot Arm Motion: n-link Reachability Two Kinks Theorem: : If an n-link arm A can reach a point, it can reach it with at most 2 joints kinked : Only 2 joints among J 1,,J n-1 have nonzero angles. The 2 joints can be chosen to be those at either end m of the median link : : the link L m such that 1 l i is at m i= 1 most half the total link length but l i is more than i= 1 half. Note this does not require reordering links. Source: O RourkeO

26 Separability Examples Separable: movable to infinity without overlapping others u separable using combination of different translation directions separable along some translation directions, but not for direction u unseparable via 2D translation Source: O RourkeO

27 2D Separability via Translation Guibas/Yao 1983: A collection of 2D convex polygons can be separated under these motion conditions (does( not necessarily hold in 3D!): Translation: all motions are translations Unidirectional: all translations in same direction Moved once: each polygon moved only once One-at at-a-time: time: only one polygon is moved at a time Developing this Separating Disjoint Segments: Of subset of segments whose upper endpoint is illuminated from right, r the segment with lowest upper endpoint is completely illuminated and therefore separable towards right. Separating Convex Polygons: Region swept by right boundary of convex shape moving horizontally ly is subset of region swept by line segment between its leftmost highest and lowest points. Separating these segments separates the polygons. O(nlogn) ) time, based on sorting Final Result: : Any set of n 2D convex shapes can be separated via translations s all parallel to any given fixed direction, with each shape moving only once. Moving order can be computed in O(nlogn) ) time.

28 2D Separability Hardness An NP-Hard 2D Separability Problem: Translation: all motions are translations Polygons are moved one-at at-a-timetime Each translation can be in a different direction Each polygon can be moved more than once Reduction from PARTITION Create separability instance from arbitrary PARTITION instance locks of height 1 and widths from PARTITION numbers Q can be moved down and right iff blocks can be packed into left part of orange shape Requires them to be stacked and perfectly packed into rectangle of width = ½ sum of PARTITION block widths Q