15-494/694 Cognitive Robotics: Lab 3
I. Software 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
- For this lab you will need a robot, a charger, a Kindle, and some
landmark sheets. You will not need light
cubes for this lab, and in fact you do not want Cozmo to see any light
cubes while you're performing this lab.
- Log in to the workstation.
- Make a lab3 directory.
- Download the file Lab3.py and put it in your
lab3 directory. Read the file.
- Put Cozmo on his charger and connect to him; put him in SDK mode.
- Set up the first landmark sheet so that landmarks 2 and 3 are
directly ahead of the robot, at a distane of 160 mm (6.29 inches)
measuring from his face. Fold the sheet at the bottom and use masking
tape to secure it to the table.
- Set up the second landmark sheet perpendicular to the first one
so that landmarks 0 and 1 are running along the robot's left side,
about 160 mm to the left of his midline.
- Open a shell on the workstation, cd to your lab3 directory, and type "simple_cli".
- Do runfsm('Lab3') to begin the lab. Notice that the particles are
initially distributed randomly.
II. Localization Using Just Distance Features
- The particle filter implementation we're using is based on these
lecture slides. You may find it helpful to review them if you want to
understand the code in more detail.
- Lab3 sets up a particle filter that evaluates the particles based
on their predictions about the distances to landmarks. For a
single landmark, if we know only its sensor-reported distance z, then
p(x|z) forms an arc of radius z.
- Place an object (not a light cube!) in front of marker 2 so that
Cozmo can only see marker 3.
- The particle filter viewer accepts keyboard commands. Press "e"
to ask the sensor model to evaluate the particles based on the current
sensor information. Do this several times. What do you see?
- With two landmarks visible we can narrow down our location a bit,
but it helps if the landmarks are widely separated. Landmarks 2 and 3
are not well separated. Unblock landmark 2 so the robot can see both,
and press "e" some more to observe the effect.
- The blue triangle shows Cozmo's location and heading. Cozmo has
no way to determine his heading from distance data alone. So even
though his location estimate converged quickly, he still has no clue
as to his heading. The particles in the display are all at roughly
the correct distance, but they have random headings.
- What does Cozmo need to do to narrow down his heading estimate?
III. Localization Using Distance Plus Motion
- Remove the charger, put Cozmo back at his starting position, quit
Python and run Lab3 again. Press the "e" key a bunch of times. The
particle headings are random.
- The particle viewer uses the w/s keys to move forward and
backward. Drive Cozmo forward and backward and observe what happens
to the particles. Although the particles still cover a broad arc,
they are now all pointing toward the landmark. This is because
particles whose headings were inconsistent with Cozmo's motion earned
low weights, and were eventually replaced by the resampling algorithm.
Now Cozmo's estimated heading, being the weighted average of the
particles, is closer to his true heading.
- The particle viewer uses the a/d keys to turn left and right.
Turn to view the 0/1 landmarks, move toward or away from them, then
turn back to the 2/3 landmarks, and so on. This provides more
information to the sensor model and allows the particle filter to
better discriminate among particles. What do you observe the
particles doing when the robot moves this way?
IV. A Bearing-Based Sensor Model
- Lab3 uses a class called ArucoDistanceSensorModel to weight
particles. It's defined in cozmo_fsm/particle.py. Instead of
distances, we could use choose to use bearings to landmarks.
- Write your own class ArucoBearingSensorModel that uses bearings
instead of distances. While the distance model results in an arc
around each landmark, a bearing model results in a line. How does the
particle filter behave when using bearing features? How big a
difference does it make to have multiple landmarks in view?
V. A Combination Distance and Bearing Sensor Model
- There's no reason we can't combine distance and bearing
information to have the best features of both. Write your own class
ArucoCombinedSensorModel to implement this.
- How does the particle filter behave now?
VI. An Odometry Issue
- Note: this section doesn't involve use of the particle filter;
you can put the landmarks away. The particle filter's motion model
relies on Cozmo's built-in odometry to measure the robot's travel.
But there is a problem with it.
- Cozmo's origin is located on the floor at the center of his front
axle. However, when he turns in place, he rotates about a point
between the front and back axles. Thus, although the position of
Cozmo's center of rotation isn't changing (ignoring any wheel
slippage), the position of his origin does change. To the motion
model, it looks like Cozmo is traveling forward or backward.
- To see this, pick Cozmo up and put him back down, so that
robot.pose.position is close to (0,0). Then do Turn(90).now() and
re-check robot.pose. Try a series of 90 degree turns and check the pose
after each one.
- Write code to estimate the distance R between Cozmo's origin and
his center of rotation. You can do this by making a bunch of turns
and looking at robot.pose after each turn. Collect the data and come
up with a value for R.
- 15-694 only: The motion model works as follows. First it
determines the distance D and turn angle Q that Cozmo has gone through
since the last time the motion model was called. Then it makes the
assumption that Cozmo's trajectory consisted of a turn by Q/2 radians,
then a straight segment of length D, and then another turn by Q/2
radians. When Cozmo turns in place, D should be zero, but it's not.
How should we correct the motion model? Keep in mind that Cozmo is
not limited to either turning (D=0) or driving straight (Q=0); he can
execute arcs as well. Write your own motion model to impelement your
correction idea, and test it.
Collect the following into a zip file:
Hand in your work through AutoLab by Friday Feb. 10.
- Your two Sensor Models.
- The code you wrote to determine R, and the value you came up with.
- If you wrote a new motion model, throw that in too.