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

### I. Software Update, SDK Update, and Initial Setup

Note: This lab can be done individually or in teams of 2.

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

Put cube 1 in front of the robot and do `PickUpCube(cube1).now()`. The robot should pick up the cube. Dor `DropObject().now()` to put it down again.

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

Show Cozmo a wall, then:
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

You can spend the rest of the lab period working on your assignments from earlier labs that are due this Friday.

### Hand In

Hand in your modified version of `pick_side`.

Dave Touretzky