This assignment continues the topic of motion planning, which is the following: given a robot R and an environment (or physical space), a start and end position pstart and pend, find a path so that the robot can move from start to end, without collisisons.
In the previous assignment you implemented a motion planning approach that computes the visibility graph and then finds path using the visibility graph. This is referred to as combinatorial or exact motion planning because it finds paths without resorting to approximation. The resulting planner was complete (for any instance, it either finds a solution or it correctly reports that no solution exists) and optimal (finds the shortest path).
In this assignment you will experiment with more complex scenarios (rectangle robot with translation and rotation) and approximate planners.
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 parametric (or configuration) space, in addition to the physical space: the physical space is the space where the robot is moving; this can be 1-dimensional, 2-dimensional or, 3-dimensional. The position of the robot in physical space is specified by a set of configurations (or parameters). The configuration (or parametric) space is the set of all possible configurations for the robot. Put differently, the configuration space is is the parametric space where the robot is moving. Any point in the configuration space uniquely identifies a position and configuration of the robot in physical space.
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. That is, a path in physical space cannot intersect an obstacle. The free space is the part of the physical space that's free, ie not occupied by an obstacle.
Extended obstacles in configuration space: Obstacles exist in physical space, and we need to extend them to configuration space. Consider a robot R and a physical obstacle O; the parametric obstacle PO corresponding to obstacle O is defined as follows:
PO is the set of all position (x,y, .....) in parametric space which, if the robot were placed at (x,y,...), it would cause a collision with obstacle O.Put differently, a configuration (x,y,..) is part of PO if placing the robot R at (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 concept of extended obstacle allows us to frame the motion planinng of a general robot in physical space as the motion planning of a point robot moving in parametric space.
Outside these cases, computing completely the free configuration space and a complete roadmap space is too costly or is not possible at all, and planning relies on sampling-based methods.
So you have a very large space to search. What is the simplest heuristic you can use to search the space?
If you thought about gridding, you are right: Discretize the space and approximate it with a uniform grid. The grid is used to sample the free space with a uniform resolution. The grid can be used in several ways:
For example, imagine a 3D configuration space corresponding to a robot translating and rotating in 2D. The goal is to get from a start position (x1,y1,theta1) to an end position (x2,y2,theta2). Assuming the coordinates are in [0,500] we can for e.g. use a grid of resolution 500 x 500 to approximate the coordinates. Cordinates will be truncated to integers. The rotation space [0,360) can be discretized for e.g. in angles of 1 degree. So that total size of this space is 500x500x360 = 90 million. This is a reasonable-sized space, mostly because we constrained the coordinates to be in [0,500]. If the coordinates were larger for e.g. [0,5000] the space would be 100 times larger, or 9 billion points.
Grids tend to not work well in large spaces, simply because uniform resolution that's good enough to capture the boundary of the free space can make the grid prohibitively large.
Approaches that improve grids all rely on the idea that sampling be done with varying resolution that tries to capture the density of the space: high resolution in points of lots of change (border between free and non-free).
The interface of the program is the same as before: allow a pre-set scene of obstacles, a start and end position and a pre-set robot; or allow the user to draw a new scene, robot, and a start and end position; once all these are set, calculate a path from start to end and animate it.
We'll use two scenarios:
And two planners:
Below is the generic search outline:
a state is a configuration (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 (i.e. all states where we can move from s)
for each such successor s':
if s' is the final goal state:
then we are done;
if isFree (s') and cost[s| + cost_of_edge(s,s') < cost[s']:
update cost[s'] = cost[s] + cost_of_edge(s,s')
insert (s', cost[s']) in the queue
Scoring a state: To guide the search towards the goal, score each state s with a cost function that adds two components: the cost of the path from the start state (the "g(s)" function), and an heuristic estimate of the cost to get to the goal state (the "h(s)" function). Setting the h()=0 makes the search behave like Dijkstra. It is known that if h(s) represents an underestimate of the cost to get from s to the end state, then the algorithm is guaranteed to find the optimal solution. Using h()=0 is always possible and the algorithms degenerates to Dijkstra, but using an heuristic h() that's close to the true cost will cause much fewer vertices to be explored. In path planning, the heuristic us usually the Euclidian distance to the goal state (because this correponds to a straightforward path that ignores the obstacles). You can start by setting h() = 0 (Dijkstra), and once it works you can optimize it by adding the heuristic h(). While you are here feel free to read some basics on A*, the most basic algorithm in AI. Here is a demo of A* compared to Dijkstra: youtube.
Generating successors: The succesors of a state s are the set of states where you can move from s. With a uniform grid, this will be the points whose L-infinity (Chebyshev) distance from the current state is 1 (L-infinity metric).
For example, for scenario (A), 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. For scenario (B) you will also add the possibility to stay at the same theta, go to theta+1 or theta-1. Here I assumed that the angle is discretized with a spacing of 1 degree, but this is something that needs to be set by the program.
Collision detection: Your basic intersection function will be to test whether the robot, when placed at configuration (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).
//return true if plaving the (origin of the) robot at state s //results in no intersection with any obstacle bool isFree(state s)There are many flavors you can implement this grid idea. For example you could pre-process the scene in an occupancy grid: this is a grid that stores, for each pixel in your discretized parametric space, whether that pixel is free, or occupied (falls inside an obstacle). In a pre-processing step you run a loop that computes the occupancy grid like so:
for all i = 0 500,
for all j = 0 to 500
for all theta = 0 to 360
if isFree(i,j,theta): occupancyGrid[i][j][theta] = FREE
else occupancyGrid[i][j][theta] = OCCUPIED
If you spend the time to build an occupancy grid, then the search will
be faster because instead of calling the collision detector for any
state, you can simply check that state in the occupancy grid to see if
it is free or not. The con is that you use a lot of time upfront, and
also space.
I mentioned the grid idea just to emphasize that there is a lot of space for creating your own version of this generic code. I am not suggesting that implementing an occupancy grid is a good idea ---- it probably depends. Feel free to improvise.
Keeping track of the path: One way to keep track of the path is via parent pointers: whenever a state discovers another state, set parent pointer. At the end, you'll be able to start from the goal and move through the parent pointers and find the path.
Note: Should we do PRM instead? Let's talk about this in class. The PRM paper Probabilistic roadmaps for path planning in high dimensional configuration spaces. AT a high level, the PRM algorithm generates a roadmap of the free space by sampling the free space. The roadmap consists of a set of nodes (configurations) in free space and edges between these configurations that connect them. The roadmap is rooted at the start node. The algorithm randomly expands the roadmap by adding new edges and nodes until it finds a collision-free path connecting it to the end configuration.
How to turn in: Please check in your code to the svn folder, together with a README that details how to run your code.
Enjoy!!!