Lab 23b: Implementing look at

Kuri's default joint controllers speak in terms of trajectories or angles, but a lot of the time we'd like to control the robot in terms of a real-world point. TF can help us bridge between the robot's reference frame (in which joint angles are defined) and other frames (where interesting things, like face-detections may live).

Implement head-only look-at

Using your knowledge of TF's transformation helpers and Kuri's frames, implement a function on the robot_api.head interface called look_at.

• It should take a geometry_msgs.PointStamped
• It should the required pan and tilt joint angles to point the eyes at the point
• If Kuri's head can move to point the eyes at the point, it should move them and return true
• Otherwise, it should return false

Hint: look carefully at the frames in Kuri's head. You'll need to transform the target point into frames where points have some natural interpretation in terms of the pan and tilt joint angles. Some elementary trigonometry will likely be required. atan2 is your friend.

Test it out by extending your head_demo.py to accept points to look at.

Implement full-body look at

You'll notice that there's only so much that we can make Kuri's pan-tilt look at before we hit the joint limits. Fortunately, we can rotate the base as well.

Create a new FullBodyLookAt class in head_demo.py. It should the same interface as the previous look_at, but it should be able to look at points behind the robot by turning to look.

• Note that you have some redundant degrees of freedom. You could just not use the pan joint, but the result would be a very unnatural movement. Instead, try to mimic how a human might lead with their head when turning.