15-494/694 Cognitive Robotics Lab 6:
RRT Path Planner

I. Software Update and Initial Setup

  1. If you are running on your personal laptop you will need to update your copy of the cozmo-tools package. (The REL workstation copies are kept updated by course staff.) To apply the update, assuming you put the cozmo-tools package in /opt, do this: 
    $ cd /opt/cozmo-tools
$ sudo git pull
  2. For this lab you will need a robot, a charger, and a Kindle.
  3. Log in to the workstation.
  4. Make a lab6 directory.

II. RRT Path Planning Demo

cozmo_fsm now includes the beginnings of an RRT path planner. At present it only handles circular obstacles, and it does no path smoothing.
  1. Download the file Lab6.py and run it in simple_cli by typing runfsm('Lab6'). The robot's outline is shown at the starting location; it is modeled by two yellow circles.

  2. Run the demo several more times and observe the variation in the solutions.

  3. Read the Lab6 source code to see how the demo works.

III. Harder Planning Problem

You can construct a harder planning problem by adding more obstacles between the start and the goal, forcing the robot to head down corridors and turn corners. But make sure that the corridors are wide enough for the robot to fit through.

Make an environment that shows off what the path planner can do.

IV. Path Execution

Write code to make the robot execute the path returned by the path planner. The first node is the start node; it can be ignored. For each subsequent node, the robot should first turn to the heading q associated with that node, and then travel forward by the distance between this node and the preceding node. This distance should be equal to the RRT's step size, except possibly at the point where the two trees meet, but don't make any assumptions about this because the distance between path nodes will change in the next section.

The best way to think about this problem is that you are converting a path (a sequence of RRTNode instances) into a navigation plan (a sequence of turn actions and drive actions). You can use an Iterate node to iterate over the navigation plan and perform the steps.

Lay out some obstacles on the table using the charger, a roll of tape, or some Legos. Describe the obstacles to the RRT and have the robot plan and execute a path around them to a goal location.

V. Path Smoothing

The path produced by the RRT-Connect algorithm is both circuitous and jagged. We can smooth the path using the following algorithm:
Let N be the number of nodes initially in the path.

Repeat N times:

Pick two random nodes p1 and p2 from the path such that p2 comes after p1.

Linearly interpolate between positions p1 and p2 in increments of the step size, and at each step, check for a collision.

If no collision is detected, create a shortcut by deleting all the nodes that lie between p1 and p2 on the path and adjusting p2's heading value (q) to reflect a direct segment from p1 to p2.

Study the source code in cozmo_fsm/rrt.py, particularly the collides() method, to understand how to do the collision check.

Display the smoothed path in the path viewer. This will help you debug your code.

Run your path on the robot and see how it peforms.

V. 15-694 Problem

Navigation plans in this lab are sequences of turns-in-place alternating with straight segments. This works but looks somewhat "robotic" and inefficient. Instead we might consider using arc segments so that the robot can change both position and heading at the same time. You previously implemented a DriveArc primitive that could be used for this purpose. DriveArc is now built in to cozmo_fsm.

Modify your path smoothing code to replace the path segment between node i and node j with an arc, as follows. Draw a line through the robot's position at node i that is perpendicular to its heading at node i. Do the same for node j. The intersection of those two lines gives the centerpoint of an arc from i to j. Note that the robot is tangent to the arc at both points. You will have to interpolate along the arc to do collision detection.

Try out your path planner on the robot. How does the motion look? It may not always be advantageous to use an arc instead of sharp turns and straight segments. Experiment a bit and see when you think it is best to use arcs. Explain your reasoning briefly in a writeup you include with your hand-in.

Hand In

Collect all your code, and screenshots of the obstacle courses you constructed, in a zip file.

Hand in your work through AutoLab by Friday March 3.

Dave Touretzky
Last modified: Sun Feb 26 06:48:38 EST 2017