15-494/694 Cognitive Robotics: Lab 2

I. Software Setup

1. Clone cozmo-tools into your personal AFS volume. Do this:

   $ cd 
   $ git clone https://github.com/touretzkyds/cozmo-tools
   

2. Get a robot, a charger, and a Kindle from the cabinet. Leave the light cubes behind; you do not want Cozmo to see any light cubes while you're testing his odometry.
3. Raise and lower Cozmo's lift to reveal the robot's network name and WiFi password.
4. 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.
5. Connect the Kindle to the workstation via its USB cable.
6. Run the Cozmo app and put it into SDK mode.
7. Open a shell on the workstation and type "simple_cli".
8. Take a Cozmo Odometry Test Sheet and tape it down to the table you're working on.

II. Investigate Cozmo's Odometry: Translation

1. You can do this part of the lab in teams of 2. Write both your names on the sheet. You only need to turn in one sheet at the end of the lab.
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.
3. Type robot.pose and enter Cozmo's initial x coordinate in the table below.
4. Tell Cozmo to drive forward 100 mm by typing: Forward(100).now()
5. Mark his ending position with your pen.
6. Look at robot.pose and enter the x coordinate in the table.
7. Measure the location of your pen mark (eyeball it using the 5 mm reference lines) and enter that in the last column of the table.
8. 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

9. 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

1. Forward() uses the robot.drive_straight() action. That means all the logic runs inside the app, and the code can directly control the motors. Now we're going to repeat the test using the DriveForward() node, which uses robot.drive_wheels() to set the wheel velocities; it does its repeated checks of distance traveled in Python, on the workstation.
2. 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

3. 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: __________
   

4. 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

1. 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. Let's use 90 degrees for our tests.
2. Have Cozmo make 90 degree turns using Turn(90).now() 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

3. 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

1. Like DriveForward, DriveTurn sets Cozmo's wheel speeds directly and then monitors the distance turned in Python. It is useful because it does not lock an action track on the robot the way turn_in_place does. 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

2. 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: __________
   

3. 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.
4. Write down the number of your robot (there's a sticker on the bottom).

   Robot number: _______________
   

5. Hand in this sheet to your lab instructor. Make sure all your names are on it.

VI. Write Your First State Machine

1. Create a lab2 folder somewhere in your filespace, but do not put it inside the cozmo-tools folder.

2. cd to your lab2 folder.

3. Using your favorite text editor, create a file called Example1.fsm in your lab2 folder with the following content:

       from cozmo_fsm import *
   
       class Example1(StateMachineProgram):
         $setup{
           Forward(50) =C=> Say("Hello there")    
         }
   

4. In the shell, type the command genfsm Example1.fsm. The should produce an Example1.py file.

5. In the shell, run simple_cli.

6. In simple_cli, type the command runfsm("Example1") to run your example.

VII. State Machine Programming (Homework)

• 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 (in the world reference frame) 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. Also, you must subtract the robot's current heading from the bearing to get the turn angle.

• LightCycle.fsm: When this behavior starts, cube 1 should glow red, cube 2 should glow green, and cube 3 should go blue. Then, any time a cube is tapped, all three cubes should shift colors: the red cube should glow green, the green clube should glow blue, and the blue cube should glow red. You can use the SetLights node to change a cube's color. Read the SDK documentation for "Light" to understand how lights work. Use the state machine formalism's fork/join mechanism to perform all three color changes at once; that is the main point of this exercise.

Hand In

Hand in your work through AutoLab by next Friday.