15-494/694 Cognitive Robotics Lab 12:
Fun With Quaternions

I. Software Update, SDK 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

II. Cube Pickup

Now turn the cube so that instead of the LEDs facing upward, they face toward the robot. The cube cannot be grasped in this position. Tell the robot to pick up the cube. He will try and fail.

The function pick_side in cozmo_fsm/pickup.py returns a set of points in the world from where the robot could drive forward a few millimeters and pick up the cube. When the cube is upright or upside-down, there are four such points, one for each vertical face. The logic for picking up a cube chooses the closest of these four based on the robot's current position.

When the cube is on its side, the only valid pickup location is the face opposite the LEDs. So pick_side should return just one point, corresponding to that face.

Your task is to modify pick_side so that it does this. Unfortunately, the angle_z value of the cube's pose is not sufficient to solve this problem when the LEDs are not facing upward. You must look at the full quaternion. See the world map lecture for a review of quaternions. You may also want to play with quaternions.online. The functions RotationMatrixToEulerAngles in perched.py and quat2rot in transforms.py may prove helpful in fixing pick_side.

III. Progress on Wall and Doorway Representations

1. Display the wall using the worldmap viewer.

2. Type "show objects".

3. Type "show particle_viewer". Presently the particle viewer display uses raw landmarks from the best particle, rather than the "cleaned up" world map objects that the worldmap viewer uses. Compare the results of the particle viewer with those of the worldmap viewer.

IV. Rest of the Lab

Hand In

Hand in your modified version of pick_side.