15-494/694 Cognitive Robotics: Lab 4

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. For this lab you will need a robot, a charger, a Kindle, and some light cubes.
3. Log in to the workstation.
4. Make a lab4 directory.
5. Put the robot on the desktop facing sideways (away from the edge).
6. Put the cubes the robot so he can't see them.
7. Connect the Kindle to the robot and start simple_cli.
8. Make sure the robot is in radio contact with the cubes even though he can't see them. Type "cube1" in simple_cli and it should show up.
9. Type "show viewer" in simple_cli so you can see what the robot sees. He should not be seeing any cubes.

II. Examining Cozmo's Built-In Localization

1. Hide the charger so the robot can't see it even if it turns. (There should be no cubes in view either.)
2. What is cube1's initial pose? What about cube2 and cube3?

   ________________________________________

3. Type cube1.pose.is_valid. What do you get?

   ________________________________________

4. Write down the x/y/z poses and origin_id's:

   Robot: ____________________          Cube 1: ____________________

5. Place cube1 (the "paperclip") in front of the robot so he can see it.
6. What are cube1's x,y,z coordinates and origin_id now?

   ________________________________________

7. Use your hand to block the robot's eyes and shift the cube slightly. Keep your hand there so the robot can't see it.
8. What are cube1's x,y,z coordinates and origin_id now? Is the cube visible?

   ________________________________________

9. Take your hand away.
10. What are cube1's x,y,z coordinates and origin_id now? Is the cube visible?

    ________________________________________

11. Quickly pick up the robot by grabbing and lifting him so that his camera points toward the ceiling. Wave him around a little.
12. Put the robot down facing away from you, so that cube1 is about 6-7 inches off his right side and he can't see it. What are the robot's and cube's poses and origin_id now?

    Robot: ____________________          Cube 1: ____________________

13. In simple_cli do Turn(-90).now() so that the robot is now facing the cube and can see it. What are the robot's and cube's poses and origin_id now?

    Robot: ____________________          Cube 1: ____________________

14. Pick up the robot and wave him around again, then put him back facing away from you, like before. What are the robot's and cube's poses and origin_id now?

    Robot: ____________________          Cube 1: ____________________

15. Now pick up the cube, wave it around a bit, and put it six inches behind the robot, not off its right side. What are the robot's and cube's poses and origin_id now?

    Robot: ____________________          Cube 1: ____________________

16. Do Turn(-90).now(). The cube should be off the robot's right side and still out of sight. What are the robot's and cube's poses and origin_id now?

    Robot: ____________________          Cube 1: ____________________

17. Do Turn(-90).now() again. The robot should now see the cube. What are the robot's and cube's poses and origin_id now?

    Robot: ____________________          Cube 1: ____________________

18. Pick up the robot, wave him around, and put him back down in a position where he can see the cube. What are the robot's and cube's poses and origin_id now?

    Robot: ____________________          Cube 1: ____________________

19. What you should have observed in this experiment is that sometimes the robot gets a new origin_id when picked up, and sometimes he doesn't. Furthermore, when the robot turns to bring a cube into view, sometimes that changes the cube's origin_id, but other times it changes the robot's origin_id. Explain the logic behind this:
    
    
    
    
    

III. Kinematics Engine

1. Put the robot on his charger.
2. Examine the source code in /opt/cozmo_fsm/cozmo_kin.py. This file creates a kinematic description of the robot using the Denavit-Hartenberg conventions discussed in lecture. Study it.
3. In simple_cli, type robot.kine to see the Kinematics object. It's defined in cozmo_fsm/kine.py.
4. Type robot.kine.joints to see the joints.
5. Type robot.kine.joints['shoulder'] to see the shoulder joint, which controls the lift.
6. Move the lift, and examine how robot.kine.joints['shoulder'] changes.
7. Let's make a point pt_base that is 100 mm in front of the base frame by typing pt_base = transform.point(0,0,100). Note that pt_base is in homogeneous coordinates.
8. Observe the shoulder joint's base-to-joint transformation matrix by typing tprint(robot.kine.base_to_joint('shoulder'))
9. Find the coordinates of pt_base relative to the shoulder frame by writing: pt_shoulder = robot.kine.base_to_joint('shoulder').dot(pt_base).

IV. Displaying the Kinematic Tree

Examine the kinematic tree betweent the base frame and the camera frame. The base frame coordinate system has z pointing up and x pointing forward. What is the camera frame coordinate system?

What trick did we have to use to get the camera frame set up correctly?

________________________________

V. Extending the Kinematic Description

• left_front_wheel and right_front_wheel: the z axis for each wheel should point outward, away from the robot's centerline. The x axis should point forward.
• left_hook and right_hook: these are the hooks on the lift. The z axis should point up and the x axis should point forward. You may need to use a trick like we did for the camera frame.

VI. Kinematics Calculations

Then write a program to move the head through a range of head angles (-10, 0, +10, +20, and +30 degrees) and print the answer for each head angle. Write down your results and include them with your hand-in. Note: this problem works better if you put the object 100 mm in front of the focal plane and go as high as +40 degrees.

VII. 15-694 Problems

Visualizing the lift. The code in cozmo-tools/world_viewer.py draws Cozmo's body an head, but not his lift. Modify the code to draw the lift mechanism in its current position. You can represent the lift as three rectangles: one for the front part and two for the left and right links that lift the front.

Pointing the camera. This is a simple IK problem. Given an object of height h at position (x,y) in base frame coordinates, write code to point the camera directly at the top of the object. First you should turn to the correct heading. Assume this can be done without error. Then you must set the camera angle so that the center of the camera (the z axis in the camera reference frame) points directly at the top of the object.

Hand In

Collect all your code and your results file a zip file.

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