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 the RRT 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 Lab5, and run it in simple_cli. It won't do anything yet.

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

5. In the particle viewer, tilt the robot's head up using the "i" key so he can see the cube dead ahead.

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

7. Now you are ready to perform a pilot request. Type the command "tm" to simple_cli.

8. Look in the path viewer and you will see the results of the RRT path planner.

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

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

11. Try this Pilot request in simple_cli:

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

    PilotToPose(destination).now()

III. Examining the Wavefront Path Planner

1. Start a fresh simple_cli and do "show all".

2. Place cube 1 about 8 inches ahead of the robot and make sure he sees it.

3. Now place cube 2 about 4 inches ahead of the robot, so it blocks the view of cube 1.

4. Enter the following in simple_cli:

   PilotToObject(wcube1).now()

5. What do you see in the wavefront viewer?

IV. 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 a pose that is located right in the middle of cube 2. You can do this by writing:

   PilotToPose(cube2.pose).now()

   You should get a "goal collides" error when you make your Pilot request.

3. 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, but a bug is currently preventing this; instead you will get a "max iterations exceeded" error. If not, adjust your parameters as necessary to demonstrate the error. The bug will be fixed over the weekend.

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 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?

IV. SLAM

1. The particle filter uses ArUco markers as landmarks. The markers are used to define walls.

2. Build "Cozmo's shack" following the instructions on this page: Cozmo's shack.

3. Note that walls must be distinct. One wall uses ArUco markers 39-44, and the other uses markers 45-50.

4. Exit simple_cli and restart it. Position Cozmo in front of his shack.

5. 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 cozmo_fsm/wall_defs.py.

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

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

8. 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 DoorPass(46).now().

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

Homework

Problem 1: Success or Failure. You can use a Pilot transition such as =PILOT(GoalCollides)=> to tell if a node like PilotToPose has failed. Write a program that tries to go to a point (300,50). If it succeeds, it should have the robot say something. If an obstacle prevents the path planner from finding a path due to a GoalCollides error it should have the robot 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 or that the walls will be in some specific relationship to each other; make your code more general than that.

Hand In

Hand in your code, and include appropriate images to document your exploration program. These should include both images of the robot and walls, and screenshots showing the world map.