Ask Your Question
1

Combine two parent-child transformations for common link

asked 2019-05-01 18:27:39 -0500

ceres gravatar image

updated 2019-05-01 21:05:47 -0500

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.

image description

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:

image description

image description

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:

image description

https://answers.ros.org/question/322015/tf-static_transform_publisher-removes-link-instead-of-linking-two-tf-trees/

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2019-05-01 19:48:06 -0500

janindu gravatar image

updated 2019-05-01 19:48:46 -0500

You are trying to connect the tf trees at by adding the base_link -> camera_link. However, given that alvar_link_1 and robot_link_1 refer to the same frame, you can make the problem simpler by connecting the tf trees by adding the robot_link_1 -> camera_link transformation.

For that, get

known_transform = tf_bufffer.lookup_transform('alvar_link_1' , 'camera_link', rospy.Time.now(), rospy.Duration(1.0))

and publish the same transformation with

parent_frame = robot_link_1
child_frame = camera_link
edit flag offensive delete link more

Comments

Thanks, but alvar_link_1 and robot_link_1 are not part of the same frame. They refer to the same point in the real world, but in two separate tf_trees. This is why I need to join their parent elements (base_link and camera_link using the transformations. I attempted to directly join them in a prior question that I referenced, which didn't work.

ceres gravatar image ceres  ( 2019-05-01 19:57:08 -0500 )edit

I saw that question. However, if bothalvar_link_1 and robot_link_1consistently refer to the same point in space, you can add the transformation as I have mentioned.

janindu gravatar image janindu  ( 2019-05-01 19:59:43 -0500 )edit

In that question you have tried to join alvar_link_1 with robot_link_1 and you faced the problem of having multiple parents for a frame, which is not allowed in ROS. However, now you are actually computing robot_link_1 -> camera_linkassuming that alvar_link_1 -> robot_link_1 is a zero rotation zero translation transformation (they refer to the same frame in space).

janindu gravatar image janindu  ( 2019-05-01 20:02:30 -0500 )edit

I tried what you are describing again as a sanity check, and the tf tree appears to be connected using rqt_tf_tree, but Rviz shows No transform from [camera_link] to [robot_link_1].

ceres gravatar image ceres  ( 2019-05-01 20:25:19 -0500 )edit

That sounds like a problem with publishing rate. How fast do you publish the robot_link_1 -> camera_link transformation?

janindu gravatar image janindu  ( 2019-05-01 20:29:19 -0500 )edit

Wouldn't I need to also publish a zero rotation zero translation transformation for alvar_link_1 and robot_link_1 if I wanted to directly compute robot_link_1 and camera_link?

ceres gravatar image ceres  ( 2019-05-01 20:30:29 -0500 )edit

The Rate is set to 1.0. What should it be set at?

ceres gravatar image ceres  ( 2019-05-01 20:35:34 -0500 )edit

I can confirm that setting the rate to 100.0 fixes the frame linking, but the translation is still incorrect.

ceres gravatar image ceres  ( 2019-05-01 20:48:26 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2019-05-01 18:27:39 -0500

Seen: 342 times

Last updated: May 01 '19