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

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

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

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

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

Why Does it Work? Minkowski Sum: vector sum for point sets Minkowski Sum: vector sum for point sets a b a+b Simple Case a3a3 a1a1 a2a2 a4a4 b3b3 b2b2 b1b1 a 2 +b 1 a 3 +b 2 a 4 +b 3 Convex/Convex Case

Why Does it Work? (continued) Polygonal Nonconvex/Nonconvex Case a5a5 a3a3 a1a1 a2a2 a4a4 b4b4 b3b3 b1b1 a 2 +b 2 a 2 +b 3 a 5 +b 4 b2b2 a 5 +b 3 a 3 +b 1 a 3 +b 4 a 4 +b 4 a 3 +b 2

Minkowski Sum: Some Properties Shape is translationally invariant Shape is translationally invariant Commutative Commutative Union formulation Union formulation When A, B convex, sum is convex When A, B convex, sum is convex

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

Minkowski Sum: Properties (continued) TRANSLATIONAL INTERSECTION TRANSLATIONAL INTERSECTION -B B t A B B

Polygon Motion Planning To plan motion for a shape P amidst polygonal obstacles U 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 intersects U: set of all displacements of P relative to U such that (translated) P does not intersect 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 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 find it by modifying visibility graph to include circular arcs from grown obstacles s t stP U U

Minkowski Sum Algorithms Convex A, B NonConvex A, B merge edge copies in slope order in slope order Identify vertex/edge support pairs a5a5 a3a3 a1a1 a2a2 a4a4 b4b4 b3b3 b1b1 a 3 supports b 3 b 4 b2b2 b 3 supports a 2 a 3

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

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

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

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

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 Moving a Ladder (continued) Find Free Path through Configuration Space Source: O’Rourke obstacles Moving L so its reference point stays on diagram edges places L as far from nearby obstacles as possible. a “Voronoi diagram” edge “Voronoi vertex” is equidistant from at least 3 obstacle points.

Moving a Ladder (continued) Find Free Path through Configuration Space 2D Time Complexity AuthorsDateTime Complexity Shwartz, Sharir 1983 O’Dunlaing et al Leven, Sharir1987 Sifrony, Sharir1987 Vegter1990 O’Rourke1985b 3D Time Complexity AuthorsDateTime Complexity Shwartz, Sharir 1984 Ke, O’Rourke1987 Canny1987 Ke, O’Rourke1988 Source: O’Rourke 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.

Robot Arm Motion Planar, multilink arm Planar, multilink arm links L 1, L 2,.., L n, connected at joints J 0, J 1, J 2,.., J n links L 1, L 2,.., L n, connected at joints J 0, J 1, J 2,.., J n joint J 0 anchored at origin joint J 0 anchored at origin no obstacles no obstacles arm may self-intersect arm may self-intersect origin = J 0 J1J1J1J1 L1L1L1L1 L2L2L2L2 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 circle Reachable region for an n-link arm is an annulus centered on the origin tip of arm

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

Robot Arm Motion: Reachability Region Reachability region is independent of order in which links are arranged. Reachability region is independent of order in which links are arranged. justify using parallelogram determined by link vector sum (commutativity of vector addition) justify using parallelogram determined by link vector sum (commutativity of vector addition) so, assume w.l.o.g. first link is longest so, assume w.l.o.g. first link is longest Theorem 8.6.3: Reachability region for n-link arm is origin-centered annulus with outer radius and inner radius if longest link is at most half the total length of links, and otherwise. Theorem 8.6.3: Reachability region for n-link arm is origin-centered annulus with outer radius and inner radius if longest link is at most half the total length of links, and otherwise. L2L2L2L2 L1L1L1L1 L3L3L3L3 L4L4L4L4

Robot Arm Motion: Finding Configurations Source: O’Rourke Find solution Find a single solution Can arm tip reach p? Can arm tip reach p? 2-Link Case 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). 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) In general there are 2 solutions (depends on how circles intersect) p = point to be reached C1C1C1C1 l1l1l1l1 C2C2C2C2 l2l2l2l2

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

Robot Arm Motion: Finding Configurations Source: O’Rourke Recursive, linear algorithm for n-link reachability: 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 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 cases of Figure 8.22 apply [Fig 8.22(a),(b)] Choose one of (in general) 2 points of intersection [Fig 8.22(a),(b)] Choose one of (in general) 2 points of intersection [Fig 8.22(c),(d)] Choose any point on C (e.g. furthest from J 0 ) [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 n-1 ) Recursively find configuration for A n-1 =(l 1,…,l n-1 ) Append last link L n to this solution to connect to p 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.

Robot Arm Motion: n-Link Reachability Source: O’Rourke 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 of the “median link”: the link L m such that is at most half the total link length but is more than half. Note this does not require reordering links.

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

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!): 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 Translation: all motions are translations Unidirectional: all translations in same direction Unidirectional: all translations in same direction Moved once: each polygon moved only once Moved once: each polygon moved only once One-at-a-time: only one polygon is moved at a time One-at-a-time: only one polygon is moved at a time Developing this… Developing this… Separating Disjoint Segments: Separating Disjoint Segments: Of subset of segments whose upper endpoint is illuminated from right, the segment with lowest upper endpoint is completely illuminated and therefore separable towards right. Of subset of segments whose upper endpoint is illuminated from right, the segment with lowest upper endpoint is completely illuminated and therefore separable towards right. Separating Convex Polygons: Separating Convex Polygons: Region swept by right boundary of convex shape moving horizontally is subset of region swept by line segment between its leftmost highest and lowest points. Separating these segments separates the polygons. Region swept by right boundary of convex shape moving horizontally 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 O(nlogn) time, based on sorting Final Result: Any set of n 2D convex shapes can be separated via translations all parallel to any given fixed direction, with each shape moving only once. Moving order can be computed in O(nlogn) time. Final Result: Any set of n 2D convex shapes can be separated via translations all parallel to any given fixed direction, with each shape moving only once. Moving order can be computed in O(nlogn) time.

2D Separability Hardness An NP-Hard 2D Separability Problem: An NP-Hard 2D Separability Problem: Translation: all motions are translations Translation: all motions are translations Polygons are moved one-at-a-time Polygons are moved one-at-a-time Each translation can be in a different direction Each translation can be in a different direction Each polygon can be moved more than once Each polygon can be moved more than once Reduction from PARTITION Reduction from PARTITION Create separability instance from arbitrary PARTITION instance Create separability instance from arbitrary PARTITION instance Blocks of height 1 and widths from PARTITION numbers Blocks 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 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 Requires them to be stacked and perfectly packed into rectangle of width = ½ sum of PARTITION block widths Q