15-494/694 Cognitive Robotics Lab 6:
RRT Path Planner
I. Software Update and Initial Setup
- 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
- For this lab you will need a robot, a charger, and a Kindle.
- Log in to the workstation.
- 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
- 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
- Run the demo several more times and observe the variation in the
- 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:
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.
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.
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.
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.