Lab Partner Names: ________________________________________________
15494/694 Cognitive Robotics: Lab 2
I. Software Update and Initial Setup
 If you are running on your personal laptop you will need to
update your copy of the cozmotools package. (The REL workstation
copies are kept updated by course staff.) To apply the update,
assuming you put the cozmotools package in /opt, do this:
$ cd /opt/cozmotools
$ sudo git pull
 Get a robot, a charger, and a Kindle from the cabinet. 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 testing his odometry.
 Log in to the workstation.
 Raise and lower Cozmo's lift to reveal the robot's network name
and WiFi password.
 Connect the Kindle to the robot's WiFi network. The Kindle's
WiFi Settings page will tell you "Internet service not working".
That's because you're connected to a robot, not the Internet. It's
normal.
 Connect the Kindle to the workstation via its USB cable.
 Run the Cozmo app and put it into SDK mode.
 Open a shell on the workstation and type "simple_cli".
 Take a Cozmo Odometry Test Sheet
and tape it down to the table you'll be working on.
II. Investigate Cozmo's Odometry: Translation
 You will do this part of the lab in teams of 2.
 Place Cozmo on the test sheet as indicated. If you use a pen to mark his lift position,
the mark should end up right on top of the thick black line.
 Type
robot.pose and enter Cozmo's initial x coordinate in the table below.
 Tell Cozmo to drive forward 100 mm by typing:
Forward(100).now()
 Mark his ending position with your pen.
 Look at
robot.pose and enter the x coordinate in the table.
 Measure the location of your pen mark (eyeball it using the reference lines) and enter
that in the last column of the table.
 Repeat the above steps to complete the table.
Test of Forward(100) 
Trial 
Initial pose x 
Final pose x 
Pose Difference 
Measured Difference 
1     
2     
3     
4     
5     
 How good is Cozmo's odometry? Calculate the mean and standard
deviation of the Pose Difference and Measured Difference values. You can use
numpy.mean() and numpy.std() for this. (You'll need to import numpy first.)
Pose Difference Mean: __________ Std. Dev: __________
Measured Difference Mean: __________ Std. Dev: __________
III. Test of DriveForward
 Forward() uses the
robot.drive_straight() action.
Now we're going to repeat the test using the DriveForward() node,
which uses robot.drive_wheels() to move the wheels, which
is not an action and thus avoids locking up an action track.
 Follow the same steps as before, but using
DriveForward(100).now() , and fill in the table below.
Test of DriveForward(100) 
Trial 
Initial pose x 
Final pose x 
Pose Difference 
Measured Difference 
1     
2     
3     
4     
5     
 How accurate is DriveForward? Compare Calculate the mean and
standard deviation of the Pose Difference and Measured Difference
values.
Pose Difference Mean: __________ Std. Dev: __________
Measured Difference Mean: __________ Std. Dev: __________
 The reason DriveForward is less accurate is that when the robot
reaches the desired distance, it doesn't stop immediately. The code
for DriveForward sets the wheel speeds to 0, but the implementation of
this operation uses a gradual deceleration rather than a sudden stop,
so the robot moves a little further than intended.
IV. Investigate Cozmo's Odometry: Rotation
 We want to perform a similar test of Cozmo's rotation accuracy.
The Turn() node uses
robot.turn_in_place() , which is a
Cozmo action. There is currently a bug in the action which prevents
Cozmo from turning more than 180 degrees with a single call. Let's
use 90 degrees for our tests.
 Have Cozmo make 90 degree turns and record the results. Use
degrees, not radians, in your measurements. Estimate the turn angle
visually.
Test of Turn(90) 
Trial 
Initial angle_z 
Final angle_z 
Angle Difference 
Measured Difference 
1     
2     
3     
4     
5     
 How accurate is Turn? Calculate the mean and standard deviation
of the Angle Difference and Measured Difference values.
Angle Difference Mean: __________ Std. Dev: __________
Measured Difference Mean: __________ Std. Dev: __________
V. Test of DriveTurn
 Like DriveForward, DriveTurn sets Cozmo's wheel speeds directly
and is not an action, so it does not lock an action track. Perform
an experiment to measure the accuracy of DriveTurn.
Test of DriveTurn(90) 
Trial 
Initial angle_z 
Final angle_z 
Angle Difference 
Measured Difference 
1     
2     
3     
4     
5     
 How accurate is DriveTurn? Calculate the mean and standard deviation
of the Angle Difference and Measured Difference values.
Angle Difference Mean: __________ Std. Dev: __________
Measured Difference Mean: __________ Std. Dev: __________
 DriveTurn is less accurate than Turn for the same reason that
DriveForward is less accurate than Forward. But DriveTurn has some
advantages over Turn: (1) it accepts an optional speed argument, and
(2) it can turn by any amount rather than being limited to angles less
than 180 degrees.
 Write down the number of your robot (there's a sticker on the bottom).
Robot number: _______________
 Hand in this sheet to your lab instructor. Make sure your names are on it.
VI. Building A Better DriveTurn
Do this portion on your own, not in pairs. You can start it in lab and finish it for homework.
 You can find the definition of the DriveTurn class in
/opt/cozmotools/cozmo_fsm/nodes.py. Extract this definition to a
file called MyTurn.py, and rename the class to MyTurn.
 At the beginning of the file you should put
from cozmo_fsm import * .
 You can test MyTurn using simple_cli by doing: '
from MyTurn import * '
and then MyTurn(90).now() .
 MyTurn takes an optional speed argument, but it's linear speed,
in mm/sec. It would make more sense to use angular speed, in
degrees/sec. You can calculate this by considering that when the
robot is turning in place, the left and right treads are moving around
a circle whose diameter is equal to the wheelbase: the distance
between the wheels. Cozmo's wheelbase is approximately 45 mm. The
two wheels are turning at equal but opposite speeds.
 Rewrite MyTurn to treat the speed argument as an angular speed
and convert this to linear speed for use with the
drive_wheels method.
 You can make MyTurn more accurate by calculating a turn angle
that takes deceleration time into account. For example, if the user
asks to turn 90 degrees, perhaps you should stop the turn at 80
degrees so that the robot ends up at close to 90.
 How early should you stop? Let's not worry about very small
turns, which will behave differently. Focus on turns of 20 degrees or
more. Determine empirically a stopping "fudge factor" that leaves the
robot at approximately the desired heading. Make sure your code works
for both left and right turns (positive and negative turn angles).
 Your fudge factor is probably only good for one speed.
Try MyTurn at three different speeds (pick reasonable speeds, not too slow and not
dangerously fast) and see what the best fudge factor is for each speed.
 Develop a function to calculate the fudge factor as a function of
speed. You can use linear interpolation or some nonlinear formula you
come up with. Implement this formula in MyTurn.
VII. State Machine Programming
Do this portion on your own, not with a partner, for homework. You
may want to review the Finite State Machine lecture notes and the
examples in cozmo_fsm/examples first. Write the following programs
using the state machine shorthand notation we covered in class:
 Square.fsm: make the robot move along a square 100 mm on a side. After
it finishes moving it should say "Cozmo squared".
 LoveHate.fsm: make the robot react to the cubes. If
cube1.is_visible is True, the robot should turn to face
that cube. If the robot sees cube2 it should turn its back to it,
i.e., turn so that cube2 is directly behind it. In either case,
simultaneously with turning, the robot should say "love it" or "hate
it". If the robot doesn't see any cubes, it shouldn't turn or say
anything.
The behavior should loop so that the robot constantly looks for cubes,
and you can move the cubes and watch the robot react. Test your
behavior by placing cubes at various locations within the robot's
field of view.
Hint: the bearing of a cube in radians is equal to atan2(ydiff,xdiff),
where the cube's x and y can be extracted from
cube.pose.position.x, and similarly for the robot's x and y.
atan2 is part of the math package, which you'll need to import.
Remember to convert to degrees before turning.
 15694 only: DriveArc node: write a DriveArc node that
causes Cozmo to drive in an arc. The full syntax should be
DriveArc(radius, angle=None, distance=None, speed=None, angspeed=None).
Radius (in mm) defines a point to the left or right of Cozmo's center
(positive for left) that is the center point of the arc along which
Cozmo will drive. A larger radius means a shallower turn; a radius of
zero means "turn in place". Angle is the angle he should travel in
degrees; distance is the distance he should travel, in mm. Exactly
one of angle or distance may be specified; if neither is given,
Cozmo drives forever. (Some transition could still fire and stop the
node.) Speed is expressed in mm/sec and angspeed in degrees/sec; at
most one can be specified. If neither speed nor angspeed is
specified, use an angspeed of 3 degrees per second.
The formula for driving along an arc can be found with a Google
search. Since this problem is being assigned late, you can hand it in
past the Friday deadline without penalty. Please submit your solution
through AutoLab by 11:59 pm on Sunday.
Hand In
Collect your MyTurn.py, Square.fsm, and LoveHate.fsm files into a zip
file called handin.zip.
Hand in your work through AutoLab by Friday Feb. 3.
