Lab 6

1. Get the latest labs repository from GitHub. For Lab 6 you will work in:

• pose_transform.py – This script involves having the robot wait in place until it detects a Cozmo cube. Then it simply displays the robot's current pose and the detected cube pose.

Try running this script and show the robot a cube. Move the cube around and observe how the pose changes to get a better sense of the coordinate frames and cube orientations.

2. You will notice that the cube pose and the robot pose are both in the global/world coordinate frames. For this exercise, you will convert the cube pose back into the robot coordinate frame. In other words we would like to know where the cube is relative to the robot. For that, implement the get_relative_pose() function that takes in two poses in the same frame of reference and finds the relative transform.

Note that many tools and libraries already exist for doing such transforms; however, the purpose of the exercise is to think through how transformations work, so we ask that you implement the function only using low level trigonometry and matrix algebra using numpy.