1 Single Robot Motion Planning Liang-Jun Zhang COMP Sep 22, 2008

2 Motion planning is the ability for an agent to compute its own motions in order to achieve certain goals. All autonomous robots and digital actors should eventually have this ability

3 Piano Mover’s Problem 2D or 3D rigid models

4

5 Types of Robots Rigid robots Articulated robots Manipulator, VideoVideo Humanoid robots

6 Goal of Motion Planning Compute motion strategies, e.g.: –geometric paths –time-parameterized trajectories –sequence of sensor-based motion commands To achieve high-level goals, e.g.: –go from A to B without colliding with obstacles –assemble product P –build map of environment E –find object O

7 PlanMoveSense

8 Basic Problem  Statement: Compute a collision-free path for a rigid or articulated object among static obstacles  Inputs: Geometry of moving object and obstacles Kinematics of moving object (degrees of freedom) Initial and goal configurations (placements)  Output: Continuous collision-free path connecting the initial and goal configurations

9 Types of Path Constraints Local constraints –Collision-free paths Differential constraints –A car cannot move sideways –Have bound curvature Global constraints –Shortest or optimal paths Path Planning Motion Planning

10 Is It Easy? alpha puzzle

11 Outline (Mon & Wed) Path planning for a point robot Configuration space Approximate cell decomposition Sampling-based motion planning

12 Path Planning for a Point Robot g s

13 Visibility Graphs  Introduced in the Shakey project at SRI in the late 60s  Can produce shortest paths in a point robot in 2D g s

14 Simple (Naïve) Algorithm 1.Install all obstacles vertices in VG, plus the start and goal positions 2.For every pair of nodes u, v in VG 3. If segment(u,v) is an obstacle edge then 4. insert (u,v) into VG 5. else 6. for every obstacle edge e 7. if segment(u,v) intersects e 8. then goto 2 9. insert (u,v) into VG 10.Search VG using BFS (any other graph search scheme)

15 Complexity A point robot in 2D using visibility graphs  Simple algorithm: O(n 3 ) time  Rotational sweep: O(n 2 log n)  Optimal algorithm: O(n 2 )  Space: O(n 2 ) g s

16 Motion Planning Framework Motion planning: a search problem in continuous space Discretization Continuous representation Graph search

17 g s Issues With Visibility Graphs  Difficult to extend from point robots to rigid or articulated robots A L-shaped robot

18 Outline (Mon & Wed) Path planning for a point robot Configuration space Approximate cell decomposition Sampling-based motion planning

19 Configuration Space: Tool to Map a Robot to a Point

20 Example: rigid robot in 2-D workspace 3-parameter specification: q = (x, y,  ) with  [0, 2  ). –3-D configuration space robot workspace reference point x y  reference direction

21 Configuration Space (C-Space) The configuration of a moving object is a specification of the position of every point on the object. –Usually a configuration is expressed as a vector of position & orientation parameters: q = (q 1, q 2,…,q n ). Configuration space –C-space –The set of all possible configurations. –A configuration is a point in C-space. q=(q 1, q 2,…,q n ) q1q1q1q1 q2q2q2q2 q3q3q3q3 qnqnqnqn

22 Dimension of C-space The dimension of a configuration space is the minimum number of parameters needed to specify the configuration of the object completely. It is also called the number of degrees of freedom (dofs) of a moving object.

23 Example: Rigid Robot in 3-D Workspace What is the configuration space?

24 Example: rigid robot in 3-D workspace q = ( position, orientation ) = (x, y, z, ???) Number of dofs = 6 Euler angles –  x yz x yz x y z  x yz 1  2  3  4

25 C = S 1 x S 1 θ Φ Topology of C-Space The topology of C is usually not that of a Cartesian space R n. 0 22 22 θ Φ θ Φ

26 Example: Rigid Robot in 3-D Workspace Number of dofs = 6 Topology: R 3 x SO(3)

27 Example: An Articulated Robot Number of dofs = 3 C-space is 3 dimensional

28 An articulated object is a set of rigid bodies connected at the joints. An Articulated Robot Puma 560 Number of dofs = 6

29 Obstacles in C-space Workspace Configuration Space x y Robot Start Goal Free Obstacle C-obstacle A 2D Translating Robot

30 Obstacles in C-space A configuration q is collision-free, or free, if a moving object placed at q does not intersect any obstacles in the workspace. The free space F is the set of free configurations. A configuration space obstacle (C-obstacle) is the set of configurations where the moving object collides with workspace obstacles.

31 Disc in 2-D Workspace workspaceconfiguration space

32 Polygonal Robot Translating in 2-D Workspace workspace configuration space

33 Articulated Robot in 2-D Workspace workspace configuration space

34 Live demo of C-space of a 2-link robot

35 Fundamental Question workspace configuration space Are two given points connected by a path?

36 Problem: Computing C-obstacles Input: –Polygonal moving object translating in 2-D workspace –Polygonal obstacles Output: configuration space obstacles represented as polygons

37 Minkowski Sum A B

38 Exercise A B A  B = ?

39 Minkowski Sum

40 Minkowski Sum

41 Minkowski Sum

42 Minkowski Sum The Minkowski sum of two sets A and B, denoted by A  B, is defined as A  B = { a+b | a  A, b  B } Similarly, the Minkowski difference is defined as A – B = { a–b | a  A, b  B } p q

43 Minkowski Sum of Non-convex Polyhedra

44 Configuration Space Obstacle If P is an obstacle in the workspace and M is a translating object. Then the C-space obstacle corresponding to P is P – M. P - M Obstacle P Robot M C- obstacle Classic result by Lozano-Perez and Wesley 1979

45 Minkowski Sum of Convex Polygons The Minkowski sum of two convex polygons A and B of m and n vertices respectively is a convex polygon A + B of m + n vertices. –The vertices of A + B are the “sums” of vertices of A and B.

46 Complexity of Minkowksi Sum 2D convex polygons: O(n+m) 2D non-convex polygons: O(n 2 m 2 ) –Decompose into convex polygons (e.g., triangles or trapezoids), compute the Minkowski sums, and take the union 3-D convex polyhedra: O(nm) 3-D non-convex polyhedra: O(n 3 m 3 )

47 Complexity of Computing C-obstacles 3D rigid robots with both translational and rotational DOF –6D C-space –Arrangement of non-linear surfaces –High combinatorial complexity Conclusion –Explicit computation of the boundary of C-obstacle is difficult and impractical for robots more than 3 DOFs

48 Wednesday’s Lecture Path planning for a point robot Configuration space Approximate cell decomposition Sampling-based motion planning