Assignment 6: Motion planning in Configuration Space

Again we'll be looking at the problem of motion planning: given a robot R and an environment (or physical space), a start and end position p_start and p_end, find a path so that the robot can move from start to end, without collisisons.

For this assignment the robot is a rectangle.

Notation: we'll assume that the obstacles consist of n edges. So for us O(n) represents the complexiy of the obstacles (vertices plus edges). We assume that the robot is moving in a large bounding box B that contains all polygons.

We consider the problem in the configuration space, in addition to the physical space: the physical space is the space where the robot is moving. The configuration (or parametric) space is the parametric space where the robot is moving; put differently, it's the set of all positions for the robot.

• For example: when the robot is a rectangle and can only translate, this means that its position at any time is fully specified by two coordinates: (x,y). Think of these as the coordinates of its center. Each position of the robot would correspond to a point in a 2-dimensional configuration space. We can view the physical movement of the robot as a move in the configuration space; in this case the parametric space is also a 2D space.

• If the robot would be able to rotate, then its position would be specified by 3 parameters: (x,y, theta). Each position of the robot would correspond to a point in this 3-dimensional parametric space. The robot would be moving physically in a 2D space, and parametrically in a 3D space.

• The parametric space corresponds to the degrees of freedom (dof) of the robot, and you can imagine its gets pretty complicated with articulated arms and such.

We view the motion of the robot as a path in the physical 2D space, and associate with it a path in its parametric space. Remember that a path in physical space has to be collision-free. We have to extend the concept of obstacles. Obstacles exist in physical space, and we need to generalize them to parametric space. Consider a robot R and a physical obstacle O; the parametric obstacle PO corresponding to the real obstacle O can be defined as follows: PO is the set of all parameters (x,y) which, if the robot were placed at (x,y), it would cause a collision with O. Put differently, a point (x,y) is part of PO if placing the robot R at position (x,y) would cause a collision with obstacle O.

The free configuration space represents all points in parametric space that are not part of parametric obstacles.

The basic steps of generic motion planinng are as follows:

1. For each obstacle O, compute its coresponding parametric obstacle (Minkowski sums of the robot and the obstacke)
2. Compute the union of all parametric obstacles
3. Compute their complement (this represents the free configuration space).
4. Compute a roadmap for the free configuration space, and use it to find paths.

Heuristics: In many practical situations, computing the free configuration space and a complete roadmap is too costly or is not possible. However, this idea of framing motion planning as a search in parametric space is nice and convenient, because it allows for artificlal intelligence heuristics. A common idea in many approaches is to discretize free space and approximate it with a grid. This is what you'll implement for the second part of this assignment.

This assignment: a rectangular robot among polygonal obstacles

1. Part 1: Let's say the robot has pre-set size and start position. Find a collision-free path of the robot from start to end, by performing a search in the 2d-dimensional paramatric search that correspnds to the position of the center of the robot. Below is a generic search procedure:

   a state is a position (x,y) of the robot 
   Q is a queue (priority queue) of  states 
   initially Q contains the start position
   
   while  goal not found
        remove state s from Q
        find all successors of state s (all states where we can move from s)
        for each such successor s':
                 if s' is the final goal state, then we are done;
                 otherwise check that we have not been there (at s'); if we have, skip to next successor
                check whether placing the robot at s' would intersect any obstacle; if not,   put s' on the queue     
     

   To guide the search towards the goal, score each state with a cost function that adds two components: the euclidian distance of that state from the goal state, and the euclidian distance form the start state.

   Approximate the parametric space with a grid. What this entails, for your algorithm, is how you generate the successor states: a state (x,y) can move only to its 4 neighbors on the grid: (x, y+1), (x, y-1), (x-1, y), (x+1, y). You could also allow diagonal moves, that might speed up teh search. Assume the resolution of the space is the resolution of the window (500 by 500?).

   Testing for intersections: your basic interscetion function will be to test whether the robot, when placed at (x,y), would stay completely in free space. One way to do this is to check, for each of its 4 edges, whether it intersects any obstacle edge (you'll need to think of something to detect when robot is completely inside a polygon). Another idea is to pre-process the scene in an occupancy grid: this is a grid that stores, for each pixel in your discretized parrametric space, whether that pixel is free, or occupied (falls inside an obstacle). If you have an occupancy grid, then you can use it to determine whether a state is intersection free, by essentially checking every pixel that the robot occupies to see if it is free or not.

   Keeping track of the path: During the search, whenever a state discovers another state, set parent pointers appropriately. At the end, you'll be able to start from the goal and move through the parent pointers and find the path. The goal of the program woudl be to find and render this path.

2. Part 2: Extend your code to rectangular robots that can translate and rotate among polygonal obstacles. This sounds tricky, but if you implemented part II in a generic way, it comes for free. Here the parametric space is 3 dimensional. It comes down to writing a different function to generate next state; and writig a different function that tests whether a rectangle at (position (x,y) and rotated by alpha stays completely in free-space.

How to turn in: Please check in your code to teh svn folder provided, and I will access them in your svn folder.

Enjoy!