Combine two parent-child transformations for common link
I am trying to connect two tf trees via a known link by following the advice from @tfoote here. One link belongs to the robot tf tree and the other belongs to a camera tf tree that is not part of the robot urdf. ar_tracker_alvar is providing the known link from the camera to a link defined in the robot urdf. I have looked up the transformations using tf2_ros, where robot_link_1 and alvar_link_1 share the same point in space:
tf_buffer =tf.Buffer()
known_transform_1 = tf_bufffer.lookup_transform('robot_link_1' , 'base_link', rospy.Time.now(), rospy.Duration(1.0))
known_transform_2 = tf_bufffer.lookup_transform('alvar_link_1' , 'camera_link', rospy.Time.now(), rospy.Duration(1.0))
I have added the position information to get a new combined xyz transformation:
t = TransformStamped()
t1 = known_transform_1.transform.translation
t2 = known_transform_2.transform.translation
t.x = t1.x + t2.x
t.y = t1.x + t2.x
t.z = t1.x + t2.x
The resulting vector appears to be the correct length from the camera to the robot base. Next, my understanding of how to combine the rotation information is as follows:
q_robot = known_transform_1.transform.rotation
q_camera = known_transform_2.transform.rotation
rotation = tf.transformations.quaternion_multiply(q_robot, q_camera)
I am using this information to send to a broadcaster to provide a link between the two trees:
tf_broadcaster = tf.TransformBroadcaster()
t_broadcaster.sendTransform(t)
Combining the quaternions, however, does not work as expected.
As you can see, the rotation is off.
I have tried changing the placement of ar tags as well as other analytical solutions, but I appear to be missing a critical piece of information for how to combine the rotation components.
How do I perform the translation to link the two trees?
You can see in the visualization that the robot and camera appear to be aligned as expected.
For more context, below is the outcome of a tag placed in a different location:
EDIT:
Trying the suggestion by @janindu to directly link robot_link_1 and camera_link results in a similar situation where the tf_tree is completed but the translation is still incorrect at a different pose. See the following:
