15-494/694 Cognitive Robotics: Lab 5
I. Software Update and Initial Setup
- At the beginning of every lab you should
update your copy of the cozmo-tools package. Do this:
$ cd ~/cozmo-tools
$ git pull
- For this lab you will need a robot, a charger, a Kindle, and some
- Log in to the workstation.
- Make a lab5 directory.
- Connect the Kindle to the robot and start simple_cli.
II. Examining the RRT Path Planner
You can do this portion of the lab with a partner. Cozmo-tools
includes an RRT path planner that is used by the Pilot to navigate
around obstacles to reach a specific pose. In this portion of the lab
we will investigate the path planner's behavior.
- Download the file Lab5.fsm and read it.
- Lay out the three cubes so they form a left-to-right line about 5
inches in front of Cozmo, with the middle cube being cube 2 (the
anglepoise lamp). Space the cubes roughly 4 inches from center to
center, so the line is 8 inches wide. Make sure there is plenty of
room around the cubes because the robot is going to drive around and
- Compile Lab5, and run it in simple_cli. It won't do anything yet.
- Type "show all" to simple_cli. This brings up five viewers: the
camera viewer, the worldmap viewer, the particle viewer, the path
viewer, and the wavefront viewer.
- In the particle viewer, tilt the robot's head up using the "i"
key so he can see the cube dead ahead.
- In the particle viewer, use the "a" and "d" keys to turn the robot so he sees the
other two cubes and adds them to his world map.
- Now you are ready to perform a pilot request. Type the command "tm" to simple_cli.
- Look in the path viewer and you will see the results of the RRT path planner.
- If we don't care about the final heading of the robot, we can specify it as NaN.
- Place the robot back at its starting location, and steer it with
the particle viewer so all three cubes are on the world map
- Try this Pilot request in simple_cli:
destination = Pose(300, 0, 0, angle_z=degrees(math.nan))
III. Examining the Wavefront Path Planner
When planning routes to distant objects, cozmo-tools uses a wavefront
path planner to find a route, as this is faster than relying on an RRT
when there are many obstructions. Then it uses the RRT functions
to shorten the path and verify that there are no collisions. The
PilotToObject class uses the wavefront path planner.
- Start a fresh simple_cli and do "show all".
- Place cube 1 about 8 inches ahead of the robot and make sure he sees it.
- Now place cube 2 about 4 inches ahead of the robot, so it blocks the viw of cube 1.
- Enter the following in simple_cli:
- What do you see in the wavefront viewer?
IV. Path Planning Failures
- Place the robot back at its starting location, and steer it with the
particle viewer so all three cubes are on the map again.
- Ask the Pilot to navigate to a a pose that is located right in
the middle of cube 2. You can do this by writing:
You should get a "goal collides" error when you make your Pilot
- Use something like
Forward(150).now() to drive the
robot right up to the middle cube. Now use PilotToPose to
navigate to a point 300 mm ahead. You should get a "start
collides" error because the robot's starting state is in
collision. If not, adjust your parameters as necessary to
demonstrate the error.
- Bring the robot back to its starting position. Arrange the
three cubes at the vertices of an equilateral triangle 160 mm on a
side (measured from cube center to cube center), with the middle
of the triangle roughly 200 mm in front of the robot, and the apex
of the triangle closest to and directly in front of the robot (the
other two cubes being farther away). Use the particle viewer to
turn the robot as necessary so it knows where all three cubes are.
Use "show objects" to get the coordinates of the cubes, and "show
pose" to get the robot's position. Then ask the Pilot to navigate
to the center of the triangle, with a heading of Angle(nan). What
happens? Does it work every time?
- The particle filter uses ArUco markers as landmarks. The markers
are used to define walls.
- Build "Cozmo's shack" following the instructions on this page:
- Note that walls must be distinct. One wall uses ArUco markers
39-44, and the other uses markers 45-50.
- Exit simple_cli and restart it. Position Cozmo in front of his shack.
- Use the particle viewer to drive the robot, and observe how
Cozmo constructs his map in the worldmap viewer. When he sees at
least two ArUco markers he builds a wall. Walls are defined in
- Each particle encodes an estimate of the map. Type "show
particles" to see the state of the particle filter. Then try
"show particle 0", "show particle 50", and "show particle 120" (or
any other numbers you like) and compare their estimates of the
- Can you manually drive Cozmo through a doorway using the
particle viewer? Try it.
- In cozmo_fsm/doorpass.fsm there is a DoorPass node that will
drive Cozmo through a doorway. Doorways are numbered by the AruCo
marker above them, so the doorways available to you are numbered
40 and 46. Line up Cozmo so he sees wall 45 and do
- Once Cozmo has seen the wall, you can drive him with the
particle viewer to a position and orientation from which it would
be difficult to pass through the doorway. Then, when you use
DoorPass, see how well he does.
You can do this assignment either individually or in teams of two (but
no more than two) people.
Problem 1: Success or Failure. You can use a Pilot
transition such as
tell if a node like PilotToPose has failed. Write a program
that tries to go to a point (300,50). If an
obstacle prevents the path planner from finding a path
due to a GoalCollides error it should complain (via a Say node)
that it can't go there.
Problem 2: Exploration. Write a program that explores an
environment by driving through every doorway it can. It should keep a
list of doorways visited so it doesn't repeat itself. If it doesn't
currently see any un-visited doorways, it should turn by some amount
and look again. If you build a structure with two adjacent walls,
your program should find a wall, drive through its doorway, notice the
adjacent wall, and drive through that doorway. But don't assume that there
will always be exactly two doorways; make your code more general than that.
Hand in your code, and appropriate images to document what you did for Problems 1 and 2.