# Static tf2 transform returns correct position but opposite quaternion [closed]

Trying to read the state of my robotic arm, I found that the manufacturer provided transforms for everything up to the flange but not to the center of the closed fingers in the end effector. In the manual, they give the transform as an Rz(-pi/4)+Tz(0.1034). I found out the most convenient way to get to that pose seems to be to set up a static tf2 transform with the given data, using the static_transform_publisher.

<?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE launch>

<launch>
<node pkg="tf2_ros" type="static_transform_publisher" name="panda_closed_fingers_center_broadcaster" args="0 0 0.1034 0 0 0 1 panda_link8 panda_cfc" />

</launch>


As you can see from the args, I am disregarding the rotation, as it seems I give args="0 0 0.1034 0 0 -0.383 0.924 panda_link8 panda_cfc" I get very incorrect results, but it might for other reasons, hence me posting this as well. I then have this python script which gets panda_link8's pose from a MoveGroupCommander, tries to lookup the transform between panda_link0 and panda_cfc, and then applies said transform to a null pose (0, 0, 0, 1, 0, 0, 0).

#!/usr/bin/env python

import rospy
import tf2_ros
import tf2_geometry_msgs
import moveit_commander

from actionlib_msgs.msg import GoalStatusArray
from geometry_msgs.msg import PoseStamped

if __name__ == '__main__':

tf_buffer = tf2_ros.Buffer(rospy.Duration(1200))
tf_listener = tf2_ros.TransformListener(tf_buffer)

rospy.wait_for_message('move_group/status', GoalStatusArray)

mgc_pa = moveit_commander.MoveGroupCommander('panda_arm')

rate = rospy.Rate(1)
while not rospy.is_shutdown():
print('--')

print("flange pos: [{: f}, {: f}, {: f} ]".format(pose_flange.position.x,
pose_flange.position.y,
pose_flange.position.z))
print("flange rot: [{: f}, {: f}, {: f}, {: f} ]".format(pose_flange.orientation.w,
pose_flange.orientation.x,
pose_flange.orientation.y,
pose_flange.orientation.z))

try:
'panda_cfc',     # source frame
rospy.Time(0))   # get the tf at first available time
except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
rate.sleep()
continue

pose_empty_stamped = PoseStamped()
pose_empty_stamped.pose.orientation.w = 1.0

pose_cfc = tf2_geometry_msgs.do_transform_pose(pose_empty_stamped, transform).pose
print("cfc    pos: [{: f}, {: f}, {: f} ]".format(pose_cfc.position.x,
pose_cfc.position.y,
pose_cfc.position.z))
print("cfc    rot: [{: f}, {: f}, {: f}, {: f} ]".format(pose_cfc.orientation.w,
pose_cfc.orientation.x,
pose_cfc.orientation.y,
pose_cfc.orientation.z))
rate.sleep()


This all results in me getting the correct flange pose, the (seemingly) correct cfc position and a quaternion exactly opposite to the one I need, for example:

flange pos: [ 0.558647, -0.035861,  0.267268 ]
flange rot: [ 0.143811, -0.833978,  0.397952, -0.354166 ]
cfc    pos: [ 0.631565, -0.040205,  0.194085 ]
cfc    rot: [-0.143811,  0.833978, -0.397952,  0.354166 ]


Some point to it being due to the transformation matrices being transposed, but I have no access to them.

EDIT: @tfoote's answer got me tinkering with the code, as at first his suggestion had me happy the quaternion was now ...

edit retag reopen merge delete

### Closed for the following reason the question is answered, right answer was accepted by aPonza close date 2018-12-18 05:45:04.236422

Sort by » oldest newest most voted

If you're getting the inverse of what you're expecting it's likely that you're confusing the direction of the API.

more

It was a confusing notation also in the robotics course at uni, and the API works exactly like transformation matrices are supposed to. Thank you! However the problem is not quite solved yet, I added an edit, as it still feels related to this question rather than its own.

( 2018-10-11 05:48:11 -0600 )edit

I replied before having the whole thing extra clear, thanks. The issue was I needed the transform from cfc to link0 (i.e. tf_buffer.lookup_transform('panda_link0', 'panda_cfc', rospy.Time(0))) and to apply it to a point in the frame panda_cfc (i.e. pose_empty_stamped.header.frame_id = 'panda_cfc').

( 2018-10-11 08:34:07 -0600 )edit