Lab 5: Path Planning

# Challenge Statement:

Find a way around a known map from an unknown start position to an unknown end position without touching any obstacle. All obstacles will be either rectangles or circles (on a plane) and leaving the boundaries of the map counts as touching an obstacle.

# Demo:

1. Place the robot in a position (for the center of the robot) and orientation on the map specified by a TA in inches and degrees respectively.
2. Input a desired goal position for the center of the robot as specified by a TA given in in inches.
3. Start the robot so that it moves from its initial configuration to the goal position where it should come to a halt.
4. If the robot hits an obstacle, the run is over no points will be given even if the robot recovers and reaches the goal.
5. Grading will be based on how close the robot comes to the desired goal position.
6. The orientation of the robot at the goal position does not matter.
7. Three tries will be given in order to improve your score if needed. All tries are independent of each other. Progressively easier start and goal positions can be requested, each for a 10 point penalty and only once per try. A request cannot be made on the first try.

# Map and obstacle list:

All measurements are in inches.

Obstacle Coordinates
Rectangle 1 Diagonal Point 1 : (); Point 2 : ()
Rectangle 2 Diagonal Point 1 : (); Point 2 : ()
Rectangle 3 Diagonal Point 1 : (); Point 2 : ()
Circle 1 Center : (); Radius : 3 inches
Circle 2 Center : (); Radius : 3 inches
Circle 3 Center : (); Radius : 3 inches

# How to:

The method described below is a waypoint based planner. However, please feel free to implement any other planner for this lab.

1. Write a structure that stores all obstacles on the map and a corresponding function that is capable of tesing if a line in world space passes through any one of them.
2. Pick points on the map such that at lest one waypoint is visible from another (a straight line connecting two waypoints does not pass through any obstacle) and by following the wapoints any point on the map can be eventually reached. Remember that the robot has a non zero base area and that the obstacles must be expanded by the "radius" of the robot before picking the waypoints and doing collision detection.
3. Now, given a start and goal position all that is necessary to plan a valid path between them is to do a graph search with the waypoints as nodes. Things that may help are hard coding all wapoints that are visible from each waypoint and having a cost function that picks a point that is both not too far away from the current location and takes you closer to the goal. Note that the start and the goal positions must be included in the search.
4. Given a path composed of straight lines from the start to the goal, add some basic timing information and use the trajectory planner from the previous lab to follow the path. The final orientation of the robot will be in the direction of the vector joining the two points.
5. It is not necessary to use straight lines everywhere and it is possible to hardcode splines that minimize the time taken for the robot to travel between two waypoints (for example - racing turns) and use straight lines for paths that must be computed in real time (start to waypoint and waypoint to goal).

Sample execution result of the algorithm :

 A map with hardcoded waypoints marked in blue and the start and goal positions.

 Note how the shorter but more dangerous path between R1 and R2 by following Start -> WP4 -> Goal is avoided by picking a cost function that weights waypoints closer to the current position more than waypoints closer to the goal.

# Other tips:

1. Pick waypoints that are far enough aways from the obstacles such that both physical dimensions of the robot and error due to dead reckoning are taken into account.
2. Instead of the marker, use a different reference point like the front or back of the robot for the trajectory planner in this lab.
3. It is possible to rotate towards the desired end orientation and then go straight between waypoints without using the trajectory planner.
4. Save the planner. It will be used in lab 9.

# Evaluation:

1. Robot seems to move towards the goal - 20 points
2. Robot comes close to the goal without touching an obstacle and stops (out of 80) :
i) Center of the robot is > 6 inches from the goal - points
ii) Center of the robot is 6 inches from the goal - 80 points