Manipula0on Algorithms Mo0on Planning. Mo#on Planning I. Katharina Muelling (NREC, Carnegie Mellon University) 1

Transcription

1 Manipula0on Algorithms Mo0on Planning Mo#on Planning I Katharina Muelling (NREC, Carnegie Mellon University) 1

2 Configura0on Space Obstacles Star Algorithm Convex robot, transla#on C obs : convex polygon Determine the order of these edges! β 2 α 3 β 2 α 2 α 1 β 1 α A 2 O β 3 β 3 α 1 β 1 α 3 β 4 β 4 Key observa0on: every edge of C obs is a translated edge from either A or O

3 Configura0on Space Obstacles Star Algorithm Convex robot, transla#on C obs : convex polygon Solid representa0on of C obs Idea: Create convex obstacles by intersec#ng half- planes Two types of contacts: O A Vertex of A and Edge of O A Edge of A and Vertex of O

4 Configura0on Space Obstacles Star Algorithm Convex robot, transla#on C obs : convex polygon Solid representa0on of C obs Idea: Create convex obstacles by intersec#ng half- planes Contact occurs when n and v are perpendicular n v = 0 Func#on of kinema#cs f(q) <=0 { } H = (x t, y t ) C n v(x t, y t ) 0 A Analysis of contacts: n v

5 Configura0on Space Obstacle What happens when the robot translates and rotates in the plane? π θ π/2 y x 0 From Howie Choset Need to consider orienta#on, i.e., need to consider all possible contact interac#ons between all pairs of object and robot features! Problem: Exponen0al run 0mes! Katharina Muelling (NREC, Carnegie Mellon University) 5

6 Real Time Configura0on Space Transforms for Obstacle Avoidance Newman and Branicky, 1991 Objec0ve: Fast C obs construc#on for manipulator with revolute joints Mo0va0on: C obs construc#on is exponen#al in #me. Not suitable for real #me planning. Katharina Muelling (NREC, Carnegie Mellon University) 6

7 Real Time Configura0on Space Transforms for Obstacle Avoidance Newman and Branicky, 1991 Approach: Define transforma#on primi#ves that allow to transforma#on workspace obstacles to c- Space obstacles Algorithm: lookup + analy#cal transforma#ons to quickly generate the c- space for complex environments from simple primi#ves Katharina Muelling (NREC, Carnegie Mellon University) 7

8 Real Time Configura0on Space Transforms for Obstacle Avoidance Newman and Branicky, 1991 General Transforma0on Proper0es: Set Union Property: C obs (B 1 B 2 ) C obs (B 1 ) C obs (B 2 ) n Relevance: C obs (E) = B E = i B i i=1 i=1 Can decompose complex objects into sets for which we have efficient transforma#ons Set Containment Property: If B 1 B 2 : A physical object that is fully C enclosed by a larger one has obs (B 1 ) = C obs (B 1 ) C obs (B 2 ) a transform that is fully contained C obs (B 1 ) C obs (B 2 ) in the transform of the larger one Only need transforma#ons of boundaries n Katharina Muelling (NREC, Carnegie Mellon University) 8

9 Real Time Configura0on Space Transforms for Obstacle Avoidance Newman and Branicky, 1991 General Transforma0on Primi0ves: Point Primi#ves Line Primi#ves Circle Primi#ves Katharina Muelling (NREC, Carnegie Mellon University) 9

10 Real Time Configura0on Space Transforms for Obstacle Avoidance Newman and Branicky, 1991 General Transforma0on Primi0ves: Point Primi#ves Katharina Muelling (NREC, Carnegie Mellon University) 10

11 Real Time Configura0on Space Transforms for Obstacle Avoidance Newman and Branicky, 1991 Advantages: Can quickly generate complex configura#on space obstacle from precomputed and stored primi#ves. Disadvantages: A lot of work! Only approxima#on. Robot arm dependent. Katharina Muelling (NREC, Carnegie Mellon University) 11

12 A Simple Mo0on Planning Algorithm for General Robot Manipula0on Thomas Lozano- Perez, AAAI 1986 Objec0ve Simple and efficient mo#on planning algorithm for general manipulator moving in a cluiered environment What is it about: Configura#on space representa#on that is suitable for for mo#on planning (easy to implement). Katharina Muelling (NREC, Carnegie Mellon University) 12

13 A Simple Mo0on Planning Algorithm for General Robot Manipula0on Thomas Lozano- Perez, AAAI 1986 Mo0va0on Exis#ng mo#on planning algorithms are either: Not simple enough to implement Not general enough Approach: Discre#ze joint angles and look at ranges of joint angle values Katharina Muelling (NREC, Carnegie Mellon University) 13

14 A Simple Mo0on Planning Algorithm for General Robot Manipula0on Thomas Lozano- Perez, AAAI 1986 Key Idea: Slice Projec#ons q C obs = R n Configura#on for manipulator with n degrees of freedom Approximate C obs by an union of n- 1 dimensional slices π j (q) = ( q 1,..., q j 1, q j+1,..., q ) n Π [aj,b j ](C obs ) = { π j (q) q C obs, q j [a j, b j ]} Slice projec#on of C obs for values of q j in the range of a j - b j Katharina Muelling (NREC, Carnegie Mellon University) 14

15 A Simple Mo0on Planning Algorithm for General Robot Manipula0on Thomas Lozano- Perez, AAAI 1986 Example: Key Idea: Slice Projec#ons a=0 b = π 4 q [a, b] Katharina Muelling (NREC, Carnegie Mellon University) 15

16 A Simple Mo0on Planning Algorithm for General Robot Manipula0on Thomas Lozano- Perez, AAAI 1986 Example: Key Idea: Slice Projec#ons Katharina Muelling (NREC, Carnegie Mellon University) 16

17 A Simple Mo0on Planning Algorithm for General Robot Manipula0on Thomas Lozano- Perez, AAAI 1986 Example: Key Idea: Slice Projec#ons Katharina Muelling (NREC, Carnegie Mellon University) 17

18 A Simple Mo0on Planning Algorithm for General Robot Manipula0on Thomas Lozano- Perez, AAAI 1986 Key Idea: Slice Projec#ons Make image of data structure Katharina Muelling (NREC, Carnegie Mellon University) 18

19 A Simple Mo0on Planning Algorithm for General Robot Manipula0on Thomas Lozano- Perez, AAAI 1986 Algorithm to create C- Space approxima0on: For i=1 to i=n: Compute C- Space(i): Ignore links > i Find range of legal values range(q i ) by rota#ng joint i given the current joint ranges C- Space(i- 1) If i<n: sample range(q i ) at specified resolu#on θ 1 Katharina Muelling (NREC, Carnegie Mellon University) 19

20 A Simple Mo0on Planning Algorithm for General Robot Manipula0on Thomas Lozano- Perez, AAAI 1986 Algorithm to create C- Space approxima0on: For i=1 to i=n: Compute C- Space(i): Ignore links > i Find range of legal values range(q i ) by rota#ng joint i given the current joint ranges C- Space(i- 1) If i<n: sample range(q i ) at specified resolu#on θ 2 Katharina Muelling (NREC, Carnegie Mellon University) 20

21 A Simple Mo0on Planning Algorithm for General Robot Manipula0on Thomas Lozano- Perez, AAAI 1986 How to compute range(q i ): Assume: k<i are fixed and safe Two types of collision: Edge link with vertex object Vertex link with edge object θ 2 Katharina Muelling (NREC, Carnegie Mellon University) 21

22 A Simple Mo0on Planning Algorithm for General Robot Manipula0on Thomas Lozano- Perez, AAAI 1986 How to compute range(q i ): 1. Determine if vertex and infinite support line of edge intersect n v(q i )+ d = 0 Katharina Muelling (NREC, Carnegie Mellon University) 22

23 A Simple Mo0on Planning Algorithm for General Robot Manipula0on Thomas Lozano- Perez, AAAI 1986 How to compute range(q i ): 1. Determine if vertex and infinite support line of edge intersect 2. In edge constraint: Does it indeed intersect with edge 3. Orienta#on constraint: Ensure polygon edges do not intersect object at contact point Katharina Muelling (NREC, Carnegie Mellon University) 23

24 A Simple Mo0on Planning Algorithm for General Robot Manipula0on Thomas Lozano- Perez, AAAI 1986 How to compute range(q i ): 1. Determine if vertex and infinite support line of edge intersect 2. In edge constraint: compute the coordinates 3. Orienta#on constraint: Ensure polygon edges do not intersect object at contact point 4. Reachability constraint: Can it reach point without collision? Katharina Muelling (NREC, Carnegie Mellon University) 24

25 A Simple Mo0on Planning Algorithm for General Robot Manipula0on Thomas Lozano- Perez, AAAI 1986 How to compute range(q i ): 1. Determine if vertex and infinite support line of edge intersect 2. In edge constraint: compute the coordinates 3. Orienta#on constraint: Ensure polygon edges do not intersect object at contact point 4. Reachability constraint: Can it reach point without collision? Katharina Muelling (NREC, Carnegie Mellon University) 25

26 A Simple Mo0on Planning Algorithm for General Robot Manipula0on Thomas Lozano- Perez, AAAI 1986 The effect of ranges of joint angles: Idea: Grow the shape of the links to accommodate the range of the joint angles! Katharina Muelling (NREC, Carnegie Mellon University) 26

27 A Simple Mo0on Planning Algorithm for General Robot Manipula0on Thomas Lozano- Perez, AAAI 1986 Free- Space Representa0on: Regions: set of adjacent slices with overlapping ranges Build region graph to search for path between points in different regions Katharina Muelling (NREC, Carnegie Mellon University) 27

28 A Simple Mo0on Planning Algorithm for General Robot Manipula0on Thomas Lozano- Perez, AAAI 1986 Advantages: Simpler and more efficient than complete C- space construc#on. Disadvantages: Approxima#on: may miss legal paths Rapid grow in execu#on #me and memory with growing DOF Katharina Muelling (NREC, Carnegie Mellon University) 28

29 Mo0on Planning Katharina Muelling (NREC, Carnegie Mellon University) 29

30 Mo0on Planning Let W = R m being the work space, O W the set of obstacles, A(q) the robot in configura#on q C C obs = { q C A(q) O } C free = C \ C obs Mo0on planning: find a con#nuous path With τ :[0,1] C free τ (0) = q I, τ (1) = q G Planning with robot being point in C- Space LaValle, Planning Algorihtms Katharina Muelling (NREC, Carnegie Mellon University) 30

31 Mo0on Planning Overview of all mo#on planning we will handle: Combinatorial Planning Sampling based Planning Poten#al Func#ons Jacobian Transpose Decision Theory Katharina Muelling (NREC, Carnegie Mellon University) 31

32 Mo0on Planning Con0nuous terrain needs to be discre0zed for path planning! Two general approaches: Combinatorial Planning Explicit characteriza#on of C free Capture connec#vity of C free into graph and do graph search to find solu#ons Ooen complete Sampling based planning Sample in C- space Use collision detec#on to probe for collisions with obstacles Probabilis#c complete Katharina Muelling (NREC, Carnegie Mellon University) 32

33 Completeness Complete: A mo#on planner always returns a solu#on if there exist one and returns a failure otherwise in bounded #me. Probabilis0c Complete: The probability tends to zero as the number of sampled points increases. No #me bound. Katharina Muelling (NREC, Carnegie Mellon University) 33

34 Road maps Road map R, is a union of 1- D curves s.t. for all q I C free q G C free there exist a path between q I and q G if: and 1) Accessibility: There is a path to q I C free to a q' 2) Departability: There is a path from a q G C free to a Rq'' R 3) Connec#vity: There exist a path in R between q and q Building R: Nodes: q C free (or its boundary) Edge between nodes: if there is a free path between the two nodes Path Planning: 1) Connect q I and q G to road map to q and q 2) Find path from q and q in R Katharina Muelling (NREC, Carnegie Mellon University) 34

35 Combinatorial Planning Main combinatorial planning techniques: Visibility graphs Voronoi diagrams Exact cell decomposi#on Approximate cell decomposi#on Result: Graph, ooen road map Require: Explicit C obs representa#on Katharina Muelling (NREC, Carnegie Mellon University) 35

36 Visibility graph Idea: Construct a path as polygonal line connec#ng q I and q G through ver#ces of C obs Why? Any collision- free path can be transformed into a piece- wise linear path that bends only at the obstacle ver#ces Shortest Path roadmaps Katharina Muelling (NREC, Carnegie Mellon University) 36

37 A graph defined by: Visibility graph Nodes: q I,q G, obstacle vertex Edges: either obstacle edge or edge between nodes that does not intersect with any obstacle Run#me: O(n 2 log n) Shortest Path roadmaps Katharina Muelling (NREC, Carnegie Mellon University) 37

38 Voronoi Diagram Idea: Divide the space into regions closest to a par#cular site Katharina Muelling (NREC, Carnegie Mellon University) 38

39 Generalized Voronoi Diagram Idea: Set of points that have maximal clearance from all nearest objects goal Katharina Muelling (NREC, Carnegie Mellon University) 39 start Figure: Howie Choset

40 Generalized Voronoi Diagram Idea: Set of points that have maximal clearance from all nearest objects Voronoi diagram V(C free ) = { q C free near(q) >1} where near(q) = { p B free d(p, q) = min(p, q) } Boundary of C free Distance func#on One nearest point p p p q Two nearest points The set of configura#ons with more than one nearest base point p Katharina Muelling (NREC, Carnegie Mellon University)

41 Generalized Voronoi Diagram Idea: Set of points that have maximal clearance from all nearest objects For polygonal C obs consists of n lines and parabolic segments: Naïve: O(n4) Best: O(n log n) Pros: maximize clearance good when uncertainty involved (what is always the case) Cons: Airac#on to open space Subop#mal paths Real #me for low- dimensional C- space goal start Figure: Howie Choset Katharina Muelling (NREC, Carnegie Mellon University) 41

42 Cell Decomposi0on Idea: Decompose C free into regions (cells) and create connec#vity graph that represents adjacencies Two types of approach: Exact Cell Decomposi0on: Union of cells corresponds to C free Approximate Cell Decomposi0on: Decomposes C free into cells with pre- defined shape; union of cells typically only approximates C free space Katharina Muelling (NREC, Carnegie Mellon University) 42

43 Exact Cell Decomposi0on Example: Trapezoid Decomposi#on Each cell is either triangle or trapezoid and is formed by: 1) Draw ver#cal line segments by shoo#ng rays upward and downward from each polygonal vertex 2) Each cell corresponds to one node in connec#vity graph 3) Nodes are connected if the corresponding cells share a common boundary Katharina Muelling (NREC, Carnegie Mellon University) 43

44 Exact Cell Decomposi0on Example: Trapezoid Decomposi#on Each cell is either triangle or trapezoid and is formed by: 1) Draw ver#cal line segments by shoo#ng rays upward and downward from each polygonal vertex 2) Each cell corresponds to one node in connec#vity graph 3) Nodes are connected if the corresponding cells share a common boundary Katharina Muelling (NREC, Carnegie Mellon University) 44

45 Exact Cell Decomposi0on Example: Trapezoid Decomposi#on Each cell is either triangle or trapezoid and is formed by: 1) Draw ver#cal line segments by shoo#ng rays upward and downward from each polygonal vertex 2) Each cell corresponds to one node in connec#vity graph 3) Nodes are connected if the corresponding cells share a common boundary Nodes: Cells Edges: between all adjacent cells Run #me best: O(n log n) N number of cells Katharina Muelling (NREC, Carnegie Mellon University) 45

46 Approximate Cell Decomposi0on Exact cell decomposi#on can be inefficient for complex objects and problems Idea: Use cells with the same simple predefined shape, e.g., rectangle, square Hierarchical space decomposi#on: Quadtree or Octree decomposi#on The union of those non- overlapping cells is contained in C free Katharina Muelling (NREC, Carnegie Mellon University) 46

47 Approximate Cell Decomposi0on Example: Quadtree decomposi#on full mixed empty Pros: - Itera#on of repe##ve and simple computa#on - Simple to implement - Can be made complete Katharina Muelling (NREC, Carnegie Mellon University) 47

48 Combinatorial Planning Summary Pros Cons Completeness Become intractable with increasing C- space dimensionality and/or object complexity In finite #me: Report failure if no solu#on exist Report solu#on, otherwise Combinatorial explosion in represen#ng robot, obstacles and C obs, especially with manipulators How can we do beier? Katharina Muelling (NREC, Carnegie Mellon University) 48

49 Sampling Based Planning Sampling based planning algorithms: Probabilis#c Road Maps (PRM) Rapidly Exploring Random Trees Derivates of above More efficient in most prac0cal problems but weaker guarantees: Probabilis#c Complete: The probability tends to 0 that no solu#on exist if not found yet Main planning approaches found in manipula#on! Katharina Muelling (NREC, Carnegie Mellon University) 49

50 Discrete Mo0on Planning Given a graph, how do we search? Search Algorithms: Breadth- First Search Depth- First Search Dijkstra s algorithm Greedy best- first Search A* D* Katharina Muelling (NREC, Carnegie Mellon University) 50

51 Summary Combinatorial Planning Algorithms Visibility Graphs Generalized Voronoi Diagrams Exact Cell Decomposi#on Approximate Cell Decomposi#on Concept Explicit representa#on of C obs Captures connec#vity of C free in graph and finds path with graph search Sampling Based Planning Sample in C- Space Use collision detec#on to probe for collisions with obstacles Pros and Cons Pros Cons Cons complete Becomes intractable with increasing C- Space and object/robot complexity Completeness Complete Probabilis#c Complete Weaker guarantees Katharina Muelling (NREC, Carnegie Mellon University) 51

52 Concepts: Completeness Probabilis#c Complete Roadmap Summary Katharina Muelling (NREC, Carnegie Mellon University) 52