Motion Planning =============== Lectures 17 & 18 [Slides 1-4] Probem Definition ----------------- R: The robot. A non-stationary geometric object. Could be a point, a line segment, a polygon, a polyhedron, a set of articulated bodies. S = {P_1, ..., P_m}: A set of obstacles. For our purposes we can consider obstacles stationary robots. q_s: A placement of the robot, the starting configuration. q_f: A placement of the robot, the desired final configuration. Collision: The robot R is in collision with P_1 if R intersected with P_1 is not the empty set. Path: tau(s) : [0,1] -> Q A continuous mapping from the interval [0,1] to element of Q, the set of all possible configurations. A path is collision-free iff for all s \in [0,1], tau(s) is collision-free. A) (Decision) Is there a path for R among S from q_s to q_f? B) (Find-Path) Find a path for R among S from q_s to q_f? C) (Shortest Path) Find the shortest path for R among S from q_s to q_f? Point Robot in the Plane ------------------------ We have already seen motion planning algorithms, or at least major components of it. 1) Visibility graph - solves C) - visibility graph can be computed in O(n^2) (O'Rourke '87) or even in O(n log n + |edges|) (Ghosh & Mount '91) - include q_s and q_f as points for the visibility graph computatoin - Dijkstra's algorithm to find shortest path in O(n^2) [Slide 5] 2) Trapezoidal decomposition - n is the number of vertices in the environment - trapezoidal decomposition can be computed, together with a point location data structure by a randomized incremental algorithm in O(n log n) expected time - breadth first search to find a path in O(n) - O (n log n) [Slides 6-11] Polygonal Robot Translating in the Plane / Configuration Space ---------------------------------------------------------------- For a point robot in 2d the space of representing obstacles and configurations is identical. But not for a polygon, for example. Therefore, we introduce configuration space (as opposed to work space). The generalized coordinates or a robot is a minimal set of parameters to uniquely describe a placement of the robot. The number of coordinates corresponds to the number of degrees of freedom of the robot. If the number of controllable axes is smaller than the number of degrees of freedom, we say that kinematic constraints are imposed on the motion. If more coordinates are needed than are necessary for the task to be performed by the robot, we say that a robot is redundant with respect to the task. For a polygon in the plane the c-space of a robot is R^2 x [0..2Pi). C-space obstacles / Minkowski Sum ----------------------------------- Assumption: the robot is a convex polygon. Let's start with a translating polyon - no rotation, i.e., fixed orientation. C-Obstacle_P := {(x,y) : R(x,y) intersect P =/= 0} [Slide 12-15] C-Obstacles can be computed using the Minkowski Sum, denoted as (+) S_1 (+) S_2 := {p+q : p \in S_1, q \in S_2} Theorem: R is a polygon translating in the plane and P is a polygon. Then: C-Obstacle_P = P (+) (-R(0,0)) Proof: R(x,y) intersects P <=> (x,y) \in P (+) -R(0,0) a) => Assume R(x,y) intersects P; point p=(p_x, p_y) is in the intersection p \in R(x,y) => (p_x - x, p_y - y) \in R(0,0) => -(p_x - x, p_y - y) \in -R(0,0) => (-p_x + x,-p_y + y) \in -R(0,0) => (x,y) - p \in -R(0,0) | +p => (x,y) \in p - R(0,0) => (x,y) \in P (+) - R(0,0) because p \in P b) <= (x,y) \in P (+) -R(0,0) there exist r \in R(0,0) and p \in P such that (x,y) = p-r => r + (x,y) = p => R(x,y) intersects P Properties of Minkowski sum --------------------------- Observation 1: Exterme points of a P (+) R along a given direction are the sum of the extreme points in P and R along the same direction. Observation 2: Let P and R be convex polygons with n and m edges. P (+) R is convex. Theorem: Let P and R be convex polygons with n and m edges. Then P (+) R has at most n+m edges. Proof: Follows from Observation 1. Computation of Minkowski sum / C-space obstacle ----------------------------------------------- Can we construct the Minkowski sum of two convex polygons in linear time? See slides for an illustration of the algorithm. [Slides 16-19] Computation of C-space for a Polygonal Translating Robot -------------------------------------------------------- Using trapezoidal decomposition O(n log n), Minkowski sum O(n), and an algorithm for computing the intersections of C-space obstacles O(n log^2 n), we can compute the C-space for a polygonal, convex robot translating among polygons in O(n log^2 n). Using a randomized incremental algorithm, this can be improved to O(n log n) Adding Rotation --------------- [Slide 20] For a complete motion planner MP the following holds: There exists a path <=> MP finds a path For a resolution complete motion planner MP the following holds: There exists a path with c clearance <=> There exists a resolution c>0 of c-space for which MP will find a path Motion Planning for a Planar Stationary Articulated Robot Arm ------------------------------------------------------------- The robot is rooted in a point. There are n links with lenght l_n. l_0 is the base (not counted in n), the tip of l_n is called end-effector. [Slides 21-35] Reachability ------------ In robotics this is called the workspace of a manipulator. Dexterous workspace refers to the region that can be reached with the end-effector in any orientation. 1-link: circle 2-link: annulus a) l_1 > l_2 b) l_1 < l_2 c) l_1 = l_2 n-link: outer radius sum of all l_i what is the inner radius? Lemma: Reachability is indpenent of order of links. Proof: Commutativity of vector addition. Theorem (Reachability): The reachability region for an n-link arm (n>2) is an annulus centered around its base with outer radius r = sum(over all i) l_i and inner radius 0 if the longest link length l_j <= 1/2 r, and r_i = l_j - sum(i=/=j) l_i otherwise. Constructing a Reach -------------------- In robotics this is called the inverse kinematics problem. Given a point (and an orientation) in the plane (in space), what joint angles position the end-effector at that point (with that orientation)? 1-link: easy, unique solutoin 2-link: 0, 1, 2, infinity solutions (what are they?) Motion Planning for an Arm -------------------------- The general motion planning problem is PSPACE-complete!!! Separability / Disasembly ------------------------- Given a set of polygons in the plane, can each one of them be moved to infinity without colliding with any of the others? Polygons can be a) inseparable b) separable only by translation and rotation together c) separable by translation d) separable by a large number of motions Convex polygons are separable under the following strict conditions: a) only translations allowed b) they all have to be in the same direction c) every polygon can only move once d) only one polygons moves at a time This is not true in 3D! How hard is Separability? ------------------------- Separability is NP-hard. Proof: By reducing the partition problem to separability. Partition: Given a set of integers, can the set be divided into two sets for each of which the sum of integers adds up to the same number? Draw picture with {1,3,3,5,6}.