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
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.
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
III. Progress on Wall and Doorway Representations
Show Cozmo a wall, then:
- Display the wall using the worldmap viewer.
- Type "show objects".
- 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
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 your modified version of