15-494/694 Cognitive Robotics: Lab 3
I. Software Update and Initial Setup
- At the beginning of every lab you should
update your copy of the cozmo-tools package. Do this:
$ cd ~/cozmo-tools
$ 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 and 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 distance of 160 mm (6.29 inches)
measuring from his front axle. 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.
![](coz-and-markers2.jpg)
- Open a shell on the workstation, cd to your lab3 directory, and type "simple_cli".
- Do runfsm('Lab3') to begin the lab.
- Type "show particle_viewer" to bring up the particle viewer, and
notice that the particles are initially distributed randomly.
- Type "show pose" to see the robot's initial position estimate.
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 (a soda can, or your hand, but 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 "z"
to randomize the particles, then press "e" to ask the sensor model to
evaluate the particles based on the current sensor information. Then
press "r" to resample based on the particle weights. 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 that well separated, but they're good enough. Unblock
landmark 2 so the robot can see both, and press "e" and "r" some more
to observe the effect.
- The yellow triangle shows Cozmo's location and heading, and the
blue triangle shows the variance in his heading estimate.. 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. (We're
pretending that the robot can see in all directions, so knowing a
landmark's distance doesn't tell us anything about its bearing.)
- What action does Cozmo need to take to narrow down his heading estimate?
![](coz-and-markers.jpg)
III. Localization Using Distance Plus Motion
- Remove the charger, put Cozmo back at his starting position,
Cover landmark 2 again. Randomize the particles, and press the
"r" 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.
- Uncover landmark 2 so Cozmo can see both landmarks 2 and 3.
What effect does the availability of a second landmark have on
Cozmo's localization?
- 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.py uses a class called ArucoDistanceSensorModel to weight
particles. It's defined in cozmo_fsm/particle.py. Take a look at it.
Instead of distances, we could use choose to use bearings to
landmarks.
- Create a variant program Lab3a.py that uses
ArucoBearingSensorModel instead. When only a single landmark is
visible, the distance model distributes particles in an arc around the
landmark, but the bearing model provides no position constraint. It
simply forces all the particles to point toward the landmark. How big
a difference does it make to have multiple landmarks in view? Let
Cozmo see both landmarks 2 and 3, and hold down the "r" key for a
while.
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 another variant
program Lab3b.py that uses
ArucoCombinedSensorModel.
- How does the particle filter behave now?
- Drive the robot around using the wasd keys but keep it facing
away from the landmarks so it cannot correct for odometry error.
What do you observe about the particle cloud?
VI. An Odometry Problem
- 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
somewhere between the front and back axles. Thus, although the
position of Cozmo's center of rotation isn't changing when he turns
(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 a program CenterOfRotation to estimate the distance R
between Cozmo's origin and his center of rotation, by making a bunch
of 90 degree turns under program control and looking at robot.pose
after each turn. Your program should collect this data and come up
with a value for R. This diagram
should help. Note that even though you told the robot to turn by 90 degrees,
that doesn't mean it did exactly that. You need to measure the actual change
in heading, which odometry is providing, to know how far it really turned.
- It may interest you to know that the motion model used by
cozmo_fsm takes into account the fact that the robot's center of rotation
is offset from the origin, and makes appropriate corrections.
VII. More Programming Problems
You may do these problems either on your own, or with one partner.
Teams should not be larger than 2 persons. If you work in a team, put
both partners' names in the writeup you include with your hand in.
- Write a FindClosestCube node that computes the distance from the
robot to each cube and finds the cube that is closest. It should
broadcast that cube using a DataEvent. If the robot doesn't see any
cubes, the node should post a failure event. Write another node
that can receive the DataEvent and make Cozmo turn to face the
closest cube. Then put everything together in a state machine
program called TurnToClosest.fsm. If the FindClosestCube node posts
a failure instead of posting a data event, make the robot say
something to complain and then return to looking for cubes.
- TwoTags challenge: the cozmo-tools particle filter treats
landmarks as points, so if the robot sees only a single ArUco tag it
cannot fully localize itself because it lacks heading information.
If it travels a bit and then spots another tag, the particle cloud
should then collapse to a single tight cluster. Construct an
illustration of this in a program called TwoTags.fsm by configuring
some physical landmarks and writing code to look at one landmark,
turn left 90 degrees and travel a bit, then turn left 90 degrees
again and look at the second landmark. (Note that you will have to
work out the landmark positions and distances in advance and declare
the landmark positions using code similar to what's in Lab3.py.)
Can Cozmo figure out where he is? Document your program's behavior
with screenshots.
- How much confidence should Cozmo have in his position estimate?
The estimate is the weighted average of all the particle positions.
When the particles are clustered tightly together, confidence should
be high. When they are spread out, confidence should be low. Type
"show particle 5" in simple_cli to examine the fifth particle. You
can access all the particles at
robot.world.particle_filter.particles.
Write a program Confidence that measures the variance of the
particle positions (i.e., the variance in distance from the mean)
and announces one of "I'm lost", "I have low confidence", or "I have
high confidence", depending on the variance. It should be set up as
a loop so that every time the user types a "tm" command in
simple_cli, the robot speaks its current confidence level. Then you
can drive the robot around with the particle viewer and type "tm" to
have the robot narrate its journey. Use the a similar landmark
setup as either Lab3.py or TwoTags.
Hand In
Collect the following into a zip file:
- The code you wrote to determine the center of rotation distance R, and the value you came up with.
- The code you wrote for the programming problems, and some screen
shots and a brief narrative to describe your results.
- The name of your partner if you worked in a team.
Hand in your work through Canvas by next Friday.
|