Static tf2 transform returns correct position but opposite quaternion
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" />
<node pkg="panda_manager" type="read_state.py" name="state_reader" output="screen" />
</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__':
rospy.init_node("read_state")
tf_buffer = tf2_ros.Buffer(rospy.Duration(1200))
tf_listener = tf2_ros.TransformListener(tf_buffer)
rospy.loginfo("Waiting for move_group/status")
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('--')
pose_flange = mgc_pa.get_current_pose('panda_link8').pose
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:
transform = tf_buffer.lookup_transform('panda_link0',
'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.header.frame_id = 'panda_link0'
pose_empty_stamped.header.stamp = rospy.Time().now()
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 ...