15-494/694 Cognitive Robotics Lab 8:
Cozmo's Clubhouse

I. Software Update, SDK Update, and Initial Setup

  1. 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
  2. We have upgraded all the robots to SDK version 0.11 and Cozmo app version 1.2. If you are running on your personal laptop you will need to update your copy of the SDK as well. Do this:
    $ sudo -H pip3 install --update cozmo
  3. For this lab you will need a robot, a charger, a Kindle, some cubes, and some walls.
  4. Log in to the workstation.
  5. Make a lab8 directory.
  6. Get a wall from the TAs.

II. Drive Through a Visible Doorway

Doorways are 75 mm wide and have an ArUco marker on each side. Write code to make the robot to drive through a doorway. The code should work from any robot starting position where both markers are visible, e.g., the robot might be off to the side and turned at an angle. You will have to execute a trajectory to get the robot lined up with the doorway and then drive through it, because the markers will no longer be visible when the robot gets close to the doorway.

You can find the markers in robot.world.aruco.seen_markers.

Please remind your TAs to take some pictures of you working on this part of the lab. (The folks at Anki are curious about what you're up to.)

III. Recognize a Partial Marker

If the robot sees fewer than two markers, or if the markers aren't the right distance apart to indicate a doorway, then it will need to move its camera to find more markers. This might mean a turn, or perhaps moving backward to get a better view if it knows it's really close to one marker.

One thing that makes a robot look really dumb is turning the wrong way when looking for a marker because it didn't recognize that there was half of a marker visible in the camera image. ArUco marker recognition is all-or-none, but we can be smarter than that. Using the OpenCV routine cv2.goodFeaturesToTrack you can find corner points in the image. A cluster of corner points that are close to each other and regularly spaced is strongly suggestive of a marker. So if you see something like that on one side of the camera image and not on the other side, you should turn in the direction of the cluster.

Run the cozmo_fsm example file CV_GoodFeatures to play with the feature detector. Then write your own code to ensure that Cozmo turns the right way when part of a marker is visible.

What you should end up with is a little program that allows you to start the robot with a full or partial view of a marker. If it sees a marker it should turn to center the marker in the camera image. If it doesn't see a marker, but it sees a cluster of points, it should try turning in that direction to locate a marker, and then center the marker in the camera image. If it guessed wrong about the direction to turn, it should try turning the other way. If it still can't find a marker it should throw a fit; you can use an AnimationNode for that.

IV. Map One Wall

Every doorway has ArUco markers on both sides of the wall, so there are four markers per doorway. All markers are unique. Some walls have one doorway and some have two. In the latter case, the doorways must be far enough apart that their markers don't overlap. No wall is longer than 610 millimeters.

Write code to completely map a wall from an arbitrary starting position. The robot must locate the wall by finding at least one ArUco marker. Then it must travel around until it knows how many doorways are present, and the ids for all the markers. The same code should work for both long and short walls, with either one doorway or two.

Download the file Lab8.py to see how to use the new Particle SLAM feature. If you write code to drive the robot around, the particle filter will build the map for you. All you need to do is look in robot.world.particle_filter.particles[0].landmarks to find particle 0's map.

The landmarks attribute holds a dictionary keyed by the marker number (an integer). Each value is a tuple (mu, phi, sigma), where mu is a column vector giving the estimated x and y coordinates of the landmark on the world map, phi is the estimateed orientation of the landmark in radians, and sigma is the x-y covariance matrix.

After mapping the wall, your code should draw a picture of the wall using matplotlib. Draw rectangles for the wall segments, which will be separated by gaps for the doorways. Draw text objects to show the marker locations.

V. How Similar Are the Maps?

Every particle maintains its own map. These should all converge to similar topologies as the robot makes repeated landmark sightings, but the maps can still differ by slight translations or rotations.

One way to judge the similarity of maps is to measure the distances between pairs of landmarks. Suppose your doorway uses markers 34 and 35. For every particle, you can calculate the distance between markers 34 and 35 on that particle's map. Then you can display a histogram of the values, allowing you to see what the spread looks like. Write code to do this using matplotlib.

VI. 15-694 Only: Run Through the Clubhouse

Cozmo's Clubhouse is a structure composed of multiple walls. The wall with no doors goes on the left, and the open side goes on the right. If the robot starts outside the front of the clubhouse, it can travel all the way through it and exit out the back by passing through three doorways, one in the front wall, one in the middle wall, and one in the back wall. Write code to do this, assuming that the robot starts out at an arbitrary position and heading that is somwhere outside the front of the clubhouse and roughly facing it. You are not allowed to program a fixed trajectory through the clubhouse; you must use vision to locate markers on each wall.

You can write this code as a state machine to locate a door, pass through it, and repeat until you've gone through three doors. If your code has the particle filter running then you will automatically get a partial map of the clubhouse when you're done. Take a screenshot of this map.

Hand In

Hand in all the code you wrote above, plus copies of your matplotlib plots showing the walls you mapped and the marker distance histogram you computed. For 15-694, include the screenshot of your partial map of the clubhouse.

A Note About Path Planning

What we'd really like to be able to do is have the robot plan paths for us that automatically go through doorways to avoid collisions with walls. We already have a path planner. What we need now is a world map that represents the walls, not just the markers. We'll develop that after Spring Break.

Dave Touretzky
Last modified: Wed Mar 8 01:47:37 EST 2017