15-494/694 Cognitive Robotics: Lab 2

I. Software Update and Initial Setup

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

2. 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.
3. Log in to the workstation.
4. Raise and lower Cozmo's lift to reveal the robot's network name and WiFi password.
5. 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.
6. Connect the Kindle to the workstation via its USB cable.
7. Run the Cozmo app and put it into SDK mode.
8. Open a shell on the workstation and type "simple_cli".
9. Take a Cozmo Odometry Test Sheet and tape it down to the table you'll be working on.

II. Investigate Cozmo's Odometry: Translation

1. You will do this part of the lab in teams of 2.
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 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. 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.
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. 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.
2. 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

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

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 your names are on it.

VI. Building A Better DriveTurn

1. You can find the definition of the DriveTurn class in /opt/cozmo-tools/cozmo_fsm/nodes.py. Extract this definition to a file called MyTurn.py, and rename the class to MyTurn.
2. At the beginning of the file you should put from cozmo_fsm import *.
3. You can test MyTurn using simple_cli by doing: 'from MyTurn import *' and then MyTurn(90).now().
4. 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.
5. Rewrite MyTurn to treat the speed argument as an angular speed and convert this to linear speed for use with the drive_wheels method.
6. 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.
7. 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).
8. 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.
9. 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

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

• 15-694 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

Hand in your work through AutoLab by Friday Feb. 3.