Project 4: Planning

Overview

In this project, you will implement a sampling-based motion planning system. You will construct roadmaps for different environments and planning problems. You will implement the Lazy A* algorithm to search this graph efficiently, and implement a postprocessor to locally improve the path on the graph. By the end of this project, you will have an integrated system that combines all the components you’ve previously developed in this course!

This project is designed for a group of 2-3 students, but may also be completed individually. Each group should write their own writeup and code. If you need to change your group, please let the course staff know on Ed.

The relevant lectures are:

• Lecture 10: Introduction to Planning
• Lecture 11: Heuristic Search
• Lecture 12: Sampling-Based Motion Planning
• Lecture 13: Lazy Search and Planning for Vehicles

Getting Started

The MuSHR dependencies are the same as when Project 3 was released. To pull Project 4 from our starter repository, follow these instructions (assuming you’ve already set the upstream remote previously).

$ cd ~/mushr_ws/src/mushr478
$ git pull upstream main
$ # Install new binary dependencies
$ rosdep install --from-paths . --ignore-src -y -r
$ cd ..
$ catkin build
$ source ~/mushr_ws/devel/setup.bash

If the build succeeds and you can run roscd planning, you’re ready to start! Please post on Ed if you have issues at this point.

Code Overview

The first step of motion planning is to define the problem space (src/planning/problems.py). This project only considers PlanarProblems like R2Problem and SE2Problem. The R2Problem configuration space only considers x- and y- position, while the SE2Problem also includes orientation. The PlanarProblem class implements shared functionality, such as collision-checking. The specific problems implement their own heuristic and steering function to connect two configurations. After defining these classes, the rest of your planning algorithm can abstract away these particulars of the configuration space. (To solve a new type of problem, just implement a corresponding problem class.)

The next step of sampling-based motion planning is to construct a roadmap by sampling configurations. Sampler classes include HaltonSampler, LatticeSampler, and RandomSampler (src/planning/samplers.py). You will fill in HaltonSampler to generate samples using the Halton pseudorandom sampler. Then, you will complete the Roadmap class (src/planning/roadmap.py) to finish constructing roadmaps. To search the roadmap, you will implement (Lazy) A* and path shortcutting (src/planning/search.py).

The Roadmap class contains many useful methods and fields. Three fields are of particular importance: graph, vertices, and weighted_edges. Roadmap.graph is a NetworkX graph, either undirected or directed depending on the problem type.¹ The starter code already handles interacting with this object. Nodes in the NetworkX graph have integer labels. These are indices into Roadmap.vertices, a NumPy array of configurations corresponding to each node in the graph. Roadmap.weighted_edges is a NumPy array of edges and edge weights; each row (u, v, w) describes an edge where u and v are the same integer-labeled nodes and w is the length of the edge between them.

The PlannerROS class in src/control/planner_ros.py provides a ROS interface to your planning algorithms. It adds the current robot state (estimated with your particle filter from Project 2) and the desired goal to the roadmap as the start and goal nodes. Then, it invokes Lazy A* and shortcutting to compute a path. Finally, this path is sent to a path tracking controller (your MPC algorithm from Project 3).

Q1. Roadmap Construction

Q1.1: Complete the HaltonSampler class in samplers.py. The HaltonSampler maintains a separate generator (with a separate base) for each dimension of the configuration space. The compute_sample method returns a deterministic sample between 0 and 1 for a given index and base, which is later scaled by the sample method to match the extents of the configuration space. This blog post on Halton sequences might be a useful reference for implementing compute_sample.

After completing Q1.1, expect to pass all the test suites in python3 test/samplers.py.

You can visualize the sampled configurations with the following command. Your plot should match the reference plot below.

$ python3 scripts/roadmap --num-vertices 100 --lazy

Q1.2: Implement PlanarProblem.check_state_validity in problems.py. Remember to vectorize! This method will be used to collision-check batches of many states (states that have been sampled as potential vertices or interpolated states along an edge), so it needs to be efficient. Your implementation should ensure that configurations are within the extents of the problem space (PlanarProblem.extents), and that the x and y components are collision-free (i.e. that they lie completely within PlanarProblem.permissible_region). For simplicity, we will assume that the robot is a point, so you only need to check one entry of the permissible region for each state.

You can verify your implementation on the provided test suites by running python3 test/problems.py.

The following command will now sample until there are 100 collision-free vertices. Your plot should match the reference plot below.

$ python3 scripts/roadmap --text-map test/share/map1.txt --num-vertices 100 --lazy

Q1.3: Implement Roadmap.check_weighted_edges_validity in roadmap.py. This method is called by the Roadmap constructor to prevent edges in collision from being added to the graph.

You can verify your implementation on the provided test suite by running python3 test/roadmap.py.

The following command samples 25 collision-free vertices and collision-checks edges that have length less than the connection radius of 3. Your plot should match the reference plot below.

$ python3 scripts/roadmap -m test/share/map1.txt -n 25 -r 3.0 --show-edges

Q2. Searching the Roadmap

The A* algorithm expands nodes in order of their f-value \(f(n) = g(n) + h(n)\), where \(g(n)\) is the estimated cost-to-come to the node \(n\) and \(h(n)\) is the estimated cost-to-go from the node \(n\) to the goal.

Q2.1: Implement the main A* logic in search.py. For each neighbor, compute the cost-to-come via the current node (cost-to-come for the current node, plus the length of the edge to the neighbor). Insert a QueueEntry with this information into the priority queue. Be sure to compute the f-value of the node (cost-to-come plus heuristic) so the priority queue is sorted correctly. Then, implement extract_path to follow the parent pointers from the goal to the start; this function is called by astar after the goal has been expanded to finally return the sequence of nodes.

You can verify your implementation on the provided test suites by running python3 test/astar.py. Your implementation should pass the test_astar_r2 and test_astar_se2 suites.

You should now be able to run an A* planner from the start to the goal! The following command specifies that it is an R2Problem, planning from the start of (1, 1) to the goal of (7, 8). Your plot should match the reference plot below.

$ python3 scripts/run_search -m test/share/map1.txt -n 25 -r 3.0 --show-edges r2 -s 1 1 -g 7 8

One of the main computational bottlenecks in motion planning algorithms is checking whether an edge is collision-free. This requires checking collisions between the robot and the environment for every intermediate state along an edge. Rather than checking all edges when constructing the roadmap, Lazy A* only collision-checks an edge when attempting to choose the edge during search.

Q2.2 Implement Lazy A* in search.py. The Roadmap class decides whether to collision-check edges on construction based on Roadmap.lazy. You will use this same field to decide when edges need to be lazily evaluated by A*. When a new node is expanded, the edge from its parent to that node must be collision-checked (with Roadmap.check_edge_validity) to determine whether search can continue. If it is in collision, the entry cannot be used and is discarded. If it is collision-free, the algorithm can proceed as before.

Your implementation should pass the final test_lazy_astar test suite in test/astar.py.

You should now be able to run Lazy A*! Your plot should match the reference plot below. Note that some edges are in collision.

$ python3 scripts/run_search -m test/share/map1.txt -n 25 -r 3.0 --lazy --show-edges r2 -s 1 1 -g 7 8

Q2.3 Implement path shortcutting in search.py. A* returns the shortest path on the graph, which is not necessarily the shortest path in the environment because not all vertices are directly connected by edges. Randomly select two indices to try and connect. Be sure to verify that an edge connecting the two vertices would be collision-free, and that connecting the two indices would actually be shorter.

You can verify your implementation on the provided test suite by running python3 test/shortcut.py.

Your plot for the following command may not match the reference plot exactly. Note that the shortcut uses edges that did not exist in the graph.

$ python3 scripts/run_search -m test/share/map1.txt -n 25 -r 3.0 --lazy --shortcut --show-edges r2 -s 1 1 -g 7 8

Q3. MuSHR Planning

So far, we’ve been solving R2Problems. But since the MuSHR cars have an orientation, the problems we need to solve are SE2Problems. The Dubins path connects two configurations, with a maximum specified curvature². Since the Dubins path is directed, the graph will be directed. As a result, more vertices are generally needed for the graph to be connected.

Your plot should match the reference plot below. Note that some edges are in collision.

$ python3 scripts/run_search -m test/share/map1.txt -n 40 -r 4 --lazy --show-edges se2 -s 1 1 0 -g 7 8 45 -c 3

Shortcutting should reduce the path length further.

$ python3 scripts/run_search -m test/share/map1.txt -n 40 -r 4 --lazy --shortcut --show-edges se2 -s 1 1 0 -g 7 8 45 -c 3

Q4. Putting It All Together

First, we’ll need to create roadmaps for our test environments. The default parameters in the launch files create a roadmap with 200 vertices, a connection radius of 10 meters, and a curvature of 1 meter^-1. These roadmaps are cached under ~/.ros/graph_cache with: the name of the map, the type of problem (r2 or se2), the name of the sampler, the number of vertices, the connection radius, and the curvature (if applicable). If you want to regenerate that roadmap, just delete the corresponding file.

planner_sim.launch will start your particle filter, controller, planner (including roadmap generation), and RViz. The roadmap’s vertices will be rendered in RViz as scattered arrows. Edges take longer to visualize, but will eventually appear as curves between them. You will not be able to plan until the roadmap has been generated (i.e., until the vertices appear in RViz). Creating a roadmap for the maze_0 map with the following parameters takes about 10 seconds.

$ roslaunch planning planner_sim.launch map:='$(find cse478)/maps/maze_0.yaml' num_vertices:=1000 connection_radius:=10 curvature:=1

To start planning to a new goal, use the “2D Nav Goal” tool in the RViz toolbar. Similar to the “2D Pose Estimate” tool, click on the position you want to specify and drag in the desired direction. Once the plan is generated, you should see the path visualized as you did for the pre-programmed paths in Project 3. The planner and controller use the particle filter estimate as the robot’s current state to plan/control from.

Your planner may sometimes fail to find a path. That may be expected if there aren’t enough vertices in the graph along the shortest path. In that case, just set a new goal (or set nearby goals, which can be connected directly to the start).

Simulated Driving in the Gates Center

As with maze_0, our machines take about 10 seconds to construct a roadmap for cse2_2. Your MuSHR car should be able to plan to faraway configurations in this much larger map.

$ roslaunch planning planner_sim.launch map:='$(find cse478)/maps/cse2_2.yaml' num_vertices:=1500 connection_radius:=10 curvature:=1 initial_x:=16 initial_y:=28

You may want to continue to tune the roadmap parameters (number of vertices, connection radius, and curvature) beyond our initial suggestions here. You may also want to retune your MPC controller or particle filter.

For roadmaps that take more time to generate, make_roadmap.launch avoids starting all the other components of your MuSHR system. The constructed roadmap is cached, so you can run planner_sim.launch afterwards without having to wait.

$ roslaunch planning make_roadmap.launch map:='$(find cse478)/maps/cse2_2.yaml' num_vertices:=5000 connection_radius:=10 curvature:=1 initial_x:=16 initial_y:=28

Deliverables

Answer the following writeup questions in planning/writeup/README.md. All figures should be saved in the planning/writeup directory.

1. Include the A* shortest path figure on map2.txt between (252, 115) and (350, 350), with 600 vertices and a connection radius of 100.

2. Holding the number of vertices constant at 600, vary the connection radius when computing the shortest path on map2.txt. For each experiment, report the radius, path length, and planning time. Explain any variation. (You may find a table useful for summarizing your findings.)

3. Holding the connection radius constant at 100, vary the number of vertices when computing the shortest path on map2.txt. For each experiment, report the number of vertices, path length, and planning time. Explain any variation. (You may find a table useful for summarizing your findings.)

4. Use the previous experiments to choose a number of vertices and connection radius for map2.txt. Compute the shortest path with A* and Lazy A*. Report the path length, planning time, and edges evaluated. Explain any variation.

5. Compare the time spent on planning and shortcutting on map1.txt. Describe qualitatively how the paths differ.

6. Holding the number of vertices and connection radius constant at 40 and 4 respectively, vary the curvature at 3, 4.5, 9, and 15 when computing the shortest path on map1.txt. Include plots of the computed paths for each curvature. Describe qualitatively how the paths differ, and quantitatively compare the path lengths.

7. The minimum and maximum steering angles used by MPC are -0.34 and 0.34 radians respectively. Knowing that the distance between the axles is 0.33 meters, what is the maximum curvature for the MuSHR car? (Hint: think about the kinematic car model.)

8. Include an RViz screenshot of the MuSHR car tracking a path in maze_0, as well as the parameters you used to construct the roadmap.

9. Include an RViz screenshot of the MuSHR car tracking a path in cse2_2, as well as the parameters you used to construct the roadmap.

10. If you retuned either the particle filter or MPC, describe why you thought it was necessary and how you chose the new parameters.

Submission

As with previous projects, you will use Git to submit your project (code and writeup).

From your mushr478 directory, add all of the writeup files and changed code files. Commit those changes, then create a Git tag called submit-proj4. Pushing the tag is how we’ll know when your project is ready for grading.

$ cd ~/mushr_ws/src/mushr478/
$ git diff                             # See all your changes
$ git add .                            # Add all the changed files
$ git commit -m 'Finished Project 4!'  # Or write a more descriptive commit message
$ git push                             # Push changes to your GitLab repository
$ git tag submit-proj4                 # Create a tag named submit-proj4
$ git push --tags                      # Push the submit-proj4 tag to GitLab