Lab 6: Path Planning II Solution

The goals of this lab are:




Use shortest path information from Dijkstra to generate a discrete path



Turn a discrete path into a list of waypoints a robot can follow



Showcase higher-order planning by having Sparki plan and execute a collision-free path



You need:




Sparki



Starter code that combines Lab 3 (IK) and Lab 5 (Path Planning).



Overview




Dijkstra's algorithm takes a graph with N nodes as an input and returns the cost of reaching every single vertex from a user-specified source vertex. Using this information, we can find a path of vertices connecting a start location to a goal location. In order to make use of this information, we have to convert the vertices on this path into coordinates we can travel to. Recall that with inverse kinematics we can turn a desired goal pose into the wheel rotations that will enable the robot to travel there! Therefore, combining these two ideas will allow us to use path planning on a graph to enable a robot to traverse a physical, continuous space.




Instructions




Each group must develop their own software implementation and turn in a single report that contains:




The code (1 .ino file)



One individual lab report for each team member (1 file per team member, named properly, in PDF format). Your PDF write-up should be individual, and should be answering each question. Please put the number of each question next to your answers, rather than turning in your answers as an essay



A video of your code running on the Sparki (1 file). If your video file is bigger than 50mb, please submit a link to google drive or some other cloud service of your choice.









You are encouraged to engage with your lab partners for collaborative problem-solving, but are expected to turn in your own write-ups. If your group does not finish the implementation by the end of the class, you may continue this lab on your own time as a complete group.




Part 1: Path Planning




Modify the get_travel_cost function to incorporate the world_map into its cost computation, incurring a high cost if a given cell/vertex is occupied (indicated by a value of 0). (Currently, it only checks to see if the source and dest vertices are neighbors.)



Study the example code that uses run_dijkstra and reconstruct_path functions to get an array of vertices along a path from source to destination.



Part 2: Path Following




Create a state machine in your loop() function based on the following properties: (Your state machine can have more states if desired, but this should be a good start!)



Its “STATE_START” state will compute a path from the robot’s current vertex



(determined using its odometry data) to a predetermined goal vertex.




Make sure to track the current goal vertex in case it changes later! (Relatedly, once you make this update, make sure to set goal_changed to FALSE)
Make sure to check whether the generated path reaches back to the source vertex – if it doesn’t, you shouldn’t transition into another state!



(If the robot has no viable path to the goal, it won’t have a real path to follow.)




Its “STATE_HAS_PATH” state will assign a goal pose (x,y,θ) for the robot to move toward



Make sure that the selected goal pose corresponds to the next vertex in the path.



(-1 indicates ‘end-of-path’ in the output of reconstruct_path)




(The goal θ should point the robot in the direction of the next waypoint!)




If there are no more goal poses to assign, transition to a terminal state.



If the goal vertex has changed (indicated by goal_changed), return to



STATE_START.




Its “STATE_SEEKING_POSE” state will use the Inverse Kinematics functions to navigate to the assigned goal pose



Once Sparki is at the destination pose you should stop the robot and transition to a different state



Test that your implementation works by adding obstacles to world_map and by setting new goal vertices.



(Keep in mind that unless you change the pose_x, pose_y, and pose_theta variables in setup(), the robot assumes its pose is (0,0,0) when it is powered on.)







Part 3: Lab Report




Provide an illustration of the state machine you’ve implemented. Be sure to include all transition criteria!



How could you modify this code to gracefully handle unforeseen objects in Sparki’s path?



What effect does increasing DISTANCE_MARGIN and HEADING_MARGIN have on your path planning?



Provide an example on the 4x4 grid where increasing DISTANCE_MARGIN and HEADING_MARGIN could cause trouble. (Hint: consider the presence of obstacles)



Provide an example on the 4x4 grid where increasing DISTANCE_MARGIN and HEADING_MARGIN could be advantageous.



What are the names of everyone in your lab group?



Roughly how much time did you spend programming this lab?



(Optional, confidential, not for credit) A brief description of any problems (either technical or collaborative) encountered while performing this lab (e.g., issues with the clarity of instructions, clarity of documentation, lab colleague’s behavior, etc.)