CSE 571 - Homework 3: Motion Planning

Due by: Wednesday, Dec 16, 2015 (11:59 PM)

Summary:

The key goal of this project is to get an understanding of motion planning methods including A-star, Weighted A-star and RRTs. You will be implementing:

• A-star (with weighting) and RRT for a 2D motion planning in a grid world
• RRT for a non-holonomic system (car)
• Optionally, an RRT-star for the 2D motion planning problem. This will be extra-credit

Get the code:

The code for this homework is accessible here: HW3-code.zip.

Get to know the files:

Here are the descriptions of the files that you'll find in the zip. You may end up not using every single file. Some are utilities for other files, and you don't really need to bother with them. Some have useful utilities, so you won't have to reinvent the wheel. Most have fuller descriptions in the files themselves.

Things to implement:

astar.m        		-- A-star motion planner for 2D grid world
rrt.m 			-- RRT motion planner for 2D grid world
nonholonomicrrt.m 	-- RRT motion planner for a non-holonomic system (car)
distance.m 		-- Computes distance between two "car" poses. Used in nonholonomicrrt.m

Optional Extra-credit:

rrtstar.m		-- RRT-Star motion planner for 2D grid world

Tools:

You will find these files useful for the 2D grid world planners:

runtest.m		-- Run script for the 2D grid world - for all planners
collcheckstline.m	-- Collision checks states on a line between two 2D points for the current map
checkLimits.m		-- Checks if the 2D point robot is within map limits
bresenham.m		-- Implements Bresenham's line algorithm. You do not have to use this directly
map1.txt		-- Simple environment for testing the 2D planners
map3.txt		-- Bigger environment for testing the 2D planners

You will find these functions useful for the non-holonomic RRT. All these (except the first two) are defined inside nonholonomicrrt.m:

nonholrrtdata.mat		-- Map and Robot data for the non-holonomic system
angdiff.m			-- Computes angle differences. Returns values in -180 to +180 degrees
simulate_carBot			-- Simulate the non-holonomic robot forward with given state & control
checkCollision_carBot		-- Collision checker for the car bot
checkLimitViolation_carBot	-- Checks for limit violations
plotCarBot			-- Plot the car bot at a given pose
plotcircle			-- Plots a circle with given center and radius

Requirements:

Let us look at the two scenarios in more detail:

2D Motion planning for a point robot:

For the 2D planning problem, you are given two worlds. "map1" is a small grid world (9x9) while "map2" is a bigger world (473 x 436). In both these worlds, free space has a value of 0 and obstacles are given a value of 1. The variable "envmap" gives you access to the environment.

The state of the robot is just the (x,y) coordinates. Note that A-star operates on a discrete grid where the states are always integers. For RRT style planners, we assume that the state is continuous. Note that MATLAB indexes matrices as (row,col) which correspond to (y,x). Remember to round off your state before indexing any matrix in MATLAB.

You need to implement the following for the 2D world:

• A-star: You will be implementing the weighted version of A-star where the heuristic is weighted by a factor of "epsilon". Setting epsilon to 1 gives unweighted A-star. You can find more details on A-star here. You will edit the file "astar.m" for this part of the assignment.
  • Use a 8-connected neighbourhood structure so that diagonal actions are also allowed. Each action has a cost of 1
  • Use the Euclidean distance from the goal as the heuristic function
  • Try out different values of epsilon to see how the behavior changes. You will need to display the set of "closed" and "open" states as the algorithm progresses.
  Please read the comments/todo sections in the file astar.m before starting your implementation. On MATLAB, you should get the following result for running A-star on "map3" with epsilon = 1.05:

  runtest('map2.txt',[148,321],[202,106],100,'astar',1.05); --->
  Using epsilon = 1.050000 
  Number of closed states: 2391 (2390 if you don't close goal state before exiting) 
  Final cost of the path: 215.000000 
  

  EDIT: It was pointed out to me that the Euclidean heuristic is inadmissible with a cost of 1.0 for each action. For diagonally separated neighbouring states:

  True cost between [1,1] -> [2,2] = 1.0
  Euclidean Heuristic returns h = sqrt(1^2 + 1^2) = 1.4142
  

  So, the heuristic is greater than the true cost, which makes it inadmissible. To rectify this, you can use the following cost for each action:

  Cost of action (dx,dy) = sqrt(dx^2 + dy^2)
  

  The Euclidean heuristic will be admissible with this cost. Running this on "map2", we get the following result (Check figure below):

  runtest('map2.txt',[148,321],[202,106],100,'astar',1.05); --->
  Using epsilon = 1.050000 
  Number of closed states: 15115 (15114 if you don't close goal state before exiting)
  Final cost of the path: 252.279221 
  

• RRT: You will be implementing a Rapidly-Exploring Random Tree (RRT) for the same 2D world. You can find more details on the RRT implementation in the lecture slides. RRTs (ours atleast) have a single parameter to choose - the length by which to grow the tree in each iteration. By default this is 10 for the HW. You will edit the file "rrt.m" for this part of the assignment.
  • Try biasing the sampling to pick the goal with 1% probability
  • You can extend the graph along the straight line from nearest graph state to the chosen random sample. The function "collcheckstline" can help you for this.
  • The parameter "deltastep" sets the length of expansion (in terms of number of cells on the map). Play with different values to see how the RRT changes.
  • You can end the graph generation once you reach a state that can be rounded off to get the goal state
  • The cost of the path can be the euclidean distance between the nodes on the path (again, "collcheckstline" can help you here)
  • You are expected to display the RRT graph intermittently using the provided figures
  Please read the comments/todo sections in the file rrt.m before starting your implementation. On MATLAB, you should get a result that looks like this when you run your RRT with deltastep = 10. Your results need not match these values.

  runtest('map2.txt',[148,321],[202,106],100,'rrt',10);
  Deltastep = 10.000000 
  Final cost of the path: 348.788889 
  

• RRT-Star: Optionally, you can implement RRT-Star for extra-credit. RRT-Star rewires the tree structure as a new node is added to the tree, thereby ensuring the optimality of the generated path. The implementation should be similar to RRT with additional bookkeeping to enable you to rewire the graph. You can find more details on RRT-Star here. You will be editing the file "rrtstar.m" for this part of the assignment. You can run RRT-Star by using the command "runtest('map2.txt',[148,321],[202,106],100,'rrtstar',10);"

Motion planning for a non-holonomic system:

For this problem, we have a car-like system with non-holonomic constraints (i.e the system cannot instantaneously move to any state from any other state). The state of the car is [x,y,theta] where (x,y) are the co-ordinates of the center of the car and "theta" is the heading. The controls that the system can apply are (v,omega) where "v" is the linear velocity of the car and "omega" the steering angle. Note that the car can move both forward & backward as well as turn right and left.

Unlike the previous problem where we extended the graph by moving along the straight line to the random state, we will be sampling controls to generate possible motion "rollouts" from the graph.

Given a randomly sampled state, we will find the closest graph node. We then simulate forward a sequence of randomly sampled controls from this graph node and find the one that gets us close to the randomly sampled state. The end-point of this "best" rollout is the next state we add to the graph. The code for this problem is in the function nonholonomicrrt.m which you will have to edit. A few points to note:

• You will have to come up with a way to sample states (5% biasing towards goal helps significantly).
• You need to implement the function "distance" which measures the distance between two car poses. This function has to be used for all distance computations for this problem. Be wary of angular distances!
• You are provided with a function for checking limit violations and collisions.
• You are also provided with a function that forward simulates controls given the current state.
• As before, you can set the length of the tree extension using the parameter "deltastep". By default, this is 5 (which leads to simulating the motion for 5 timesteps in the future with fixed controls)
• You will have to sample controls to generate the forward motion. A reasonable sampling scheme is to sample uniformly between [-maxlinearvel, +maxlinearvel] and [-maxsteerangl, +maxsteerangl], though other sampling schemes can also be used.
• Your system has to run till it gets "near" the goal. A reasonable termination criterion is to have a (x,y) distance of 7.5 cells & angular distance of 2.5 degrees. EDIT: You can check to see if any intermediate states on your path (rather than just the final state) are close to the goal.
• As before, you are expected to show the graph as it is being grown

More details and pseudocode for this are in the file nonholonomicrrt.m and this slide deck. There is a single world file "nonholrrtdata.mat" for this problem. Testing the system with a deltastep of 5 gives the following result (which you need not match):

nonholonomicrrt('nonholrrtdata.mat', [100,40,pi], [150,350,0], 100, 5)
State: (150.666226, 356.398499, 6.265385) is near goal (150.000000, 350.000000, 0.000000) 

Hints:

• Please read the comments/TODO sections on the various files before you start implementing the planners. They go into significant detail on the implementation and should answer most of your questions.
• (x,y) = (col,row) in MATLAB
• Always check states for violations before you check for collisions as querying collision status for states out of limits can throw errors.
• Test out the 2D planners on the smaller "map1" before moving to the more complex "map2". Try different start and goal points to see how the system behaves.
• Your system will be tested on different maps and start/end points for grading.

Get to work:

Don't wait till the last minute for this one, folks. Make sure you have a working MATLAB environment as soon as possible.