15-494/694 Cognitive Robotics Lab 8:
Cozmo's Clubhouse
I. Software Update, SDK Update, and Initial Setup
- 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
- 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
- For this lab you will need a robot, a charger, a Kindle, some
cubes, and some walls.
- Log in to the workstation.
- Make a lab8 directory.
- 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.
|