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, 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 behind them.

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. In the particle viewer, tilt the robot's head up using the "i" key 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. Look in the path viewer and you will see the results of the RRT path planner.

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 world map 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 can check it by looking at cube2.pose or typing "show objects" in simple_cli. You should get a "goal collides" error when you make your Pilot request. If not, adjust your parameters as necessary to demonstrate the error.

3. Use Forward(150).now() to drive the robot 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.

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 200 mm in front of the robot, and the apex of the triangle pointing at the robot. 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. 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 37-42, and wall 2 uses markers 43-48.

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 an ArUco marker he builds a wall. Walls are defined in cozmo_fsm/wall_defs.py.

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

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

7. 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 38 and 44. Line up Cozmo so he sees wall 43 and do DoorPass(44).now().

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

V. OpenCV

1. In cozmo_fsm/examples there are several OpenCV demo programs. Try playing with CV_Contour, showing it various objects (including the cubes and walls) and adjusting the threshold parameter.

2. Run the CV_GoodFeatures demo and show it an AruCo marker. What does it pick as features? How reliably can it track these features as you move the marker?

Homework

Problem 1: 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.

Problem 2: Shape matching. OpenCV provides functionality for judging the similarity of two contours (shapes). We can use this to make Cozmo recognize hand-drawn markers.

• Read the OpenCV tutorial on shape matching.

• Run the match_shapes.py program, giving it the files star-a.jpg, star-b.jpg, and star-c.png.

• Make up your own contour for a shape like the four-pointed star in the example.

• Use shape matching to have Cozmo detect your shape when it's pasted to the wall. Create a display in OpenCV to show your results.

Hand In

Hand in your code, and appropriate images to document what you did.