How to use tf to point a Kinect toward a target location?

edit

Hello ROS People,

I am trying to mimic the head pointing action found in the pr2_head_action package using a robot that has a Kinect as its head. The Kinect is mounted on top of the tilt servo with a 7cm long bracket in addition to the Kinect's own base. I have a URDF model of the whole robot that works well.

The trouble I am having is figuring out how to modify the pr2_head_action code to account for the fact that the Kinect's RGB frame is well above (and slightly offset from) the pan and tilt servos. For example, suppose I want the Kinect's RGB camera to look straight at a point 1 meter in front of the robot's base_link frame. If I use the pr2_head_action code without modification, the head will not tilt down far enough because the target's location is projected onto the pan and tilt frames rather than the Kinect's RGB frame. However, if I project the target's coordinates onto the Kinect's RGB frame, I initially get the correct tilt--the camera points at the target. But if I repeat the same goal, the head then tilts back toward vertical rather than simply not moving at all as it should.

The key part of the code I am using is as follows:

def transform_target_point(self, target):
    pan_ref_frame = '/kinect_rgb_frame'
    tilt_ref_frame = '/kinect_rgb_frame'

    # Wait for tf info (timeout in 5 seconds)
    self.tf.waitForTransform(pan_ref_frame, target.header.frame_id, rospy.Time(), rospy.Duration(5.0))
    self.tf.waitForTransform(tilt_ref_frame, target.header.frame_id, rospy.Time(), rospy.Duration(5.0))

    # Transform target point to pan reference frame & retrieve the pan angle
    pan_target = self.tf.transformPoint(pan_ref_frame, target)
    pan_angle = math.atan2(pan_target.point.y, pan_target.point.x)

    # Transform target point to tilt reference frame & retrieve the tilt angle
    tilt_target = self.tf.transformPoint(tilt_ref_frame, target)
    tilt_angle = math.atan2(-tilt_target.point.z,
            math.sqrt(math.pow(tilt_target.point.x, 2) + math.pow(tilt_target.point.y, 2)))

    return [pan_angle, tilt_angle]

I'm guessing a tf guru can see what I am missing. Some how I need to project the target onto the /kinect_rgb_frame but then publish the pan/tilt values as delta's on the current pan/tilt positions. While I could code it this way, I'm guessing there is a simpler way using tf and projecting on the right combination of frames?

--patrick