15-494/694 Cognitive Robotics: Lab 5

I. Software Update and Initial Setup

1. At the beginning of every lab you should update your copy of the cozmo-tools package. Do this:

   $ cd ~/cozmo-tools
   $ git pull
   

2. For this lab you will need a robot, a charger, a Kindle, and some light cubes.
3. Log in to the workstation.
4. Make a lab5 directory.
5. Connect the Kindle to the robot and start simple_cli.

II. Examining Cozmo's Path Planner

1. Download the file Lab5.fsm and read it.

2. Lay out the three cubes so they form a left-to-right line about 5 inches in front of Cozmo. Space the cubes roughly 4 inches from center to center, so the line is 8 inches wide.

3. Compile and run Lab5. The program brings up four viewers: the camera viewer, the worldmap viewer, the particle viewer, and the path viewer.

4. Tilt the robot's head up so he can see the cube dead ahead.

5. 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.

6. Now you are ready to construct a pilot request. Enter the following:

   destination = Pose(300, 0, 0, angle_z=degrees(180))

   PilotToPose(destination).now()

7. You'll note that Cozmo makes a number of gratuitous spins on his way to the destination. This is a bug.

8. If we don't care about the final heading of the robot, we can specify it as NaN.

9. Place the robot back at its starting location, and steer it with the particle viewer so all three cubes are on the map again.

10. Try this Pilot request:

    destination = Pose(300, 0, 0, angle_z=degrees(math.nan))

    PilotToPose(destination).now()

III. Path Planning Failures

1. Place the robot back at its starting location, and steer it with the particle viewer so all three cubes are on the map again.

2. Ask the Pilot to navigate to a new destination that is only 150 mm head of the robot. This should be roughly the location of the middle cube. You should get a "goal collides" error. If not, adjust your parameters as necessary to demonstrate the error.

3. Use Forward(100).now() to drive the robot up to the middle cube. Now ask the Pilot to navigate to the 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.

4. 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 6 inches in front of the robot. Turn the robot as necessary so it knows where all three cubes are. Then ask the Pilot to navigate to the center of the triangle. What happens?

IV. SLAM

1. In the previous section we used the cubes as landmarks, but normally they are not used this way because the robot is expected to pick them up and move them around. Instead we use ArUco markers as landmarks, and paste them onto walls.

2. Set up two walls (provided in the lab) to form a little robot habitat. Note that the walls must be distinct. Wall 1 uses ArUco markers 1-12, and wall 2 uses markers 13-24.

3. Exit simple_cli and restart it.

4. Use the particle viewer to drive the robot, and observe how Cozmo constructs his map in the worldmap viewer. When he sees one ArUco marker he creates a marker object. When he sees two markers on the same wall, he builds the wall. Walls are defined in cozmo_fsm/wall_defs.py.

5. Can you manually drive Cozmo through a doorway using the particle viewer? Try it.

6. In cozmo_fsm/doorpass.py there is code for exploring a walled environment by driving through doors. Point Cozmo at a wall so he can see a couple of ArUco markers. Then do Explore().now() and see how he does.

Homework

V. Habitat Exploration

1. Assume Cozmo starts outside the habitat, but not necessarily facing it.

2. Have him turn until he spots a wall. (Look for a WallObj object in robot.world.world_map.objects.)

3. Once he's found a wall, he knows where the doorways are located. Using the code in cozmo_fsm/doorpass.fsm as an example, write code to take Cozmo through the doorway into the habitat.

4. Write additional code to look for new walls. For each wall added, pick a door and drive through it to explore further. Do not build in any knowledge of the particular habitat you've constructed; he must learn the map by exploration. But he does know which ArUco markers are associated with which walls, and where the doorways appear in those walls.

5. Keep a list of the doors Cozmo has driven though. (Each has a unique ArUco marker.)

6. When Cozmo has driven through every doorway at least once, your program should end.

7. Extension for lab 6: modify your program so that Cozmo can carry a cube through a doorway. You will have to figure out how to hold the cube so that he can still see what he needs to see.

If your code is well-written, it will work for any habitat. We will test it on some new ones.

Hand In

Hand in your code, a photo of the habitat you created, and a screenshot showing the final worldmap after Cozmo has finished his explorations.