Ask Your Question
0

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

asked 2018-10-10 04:52:05 -0500

aPonza gravatar image

updated 2018-10-11 05:45:57 -0500

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 ... (more)

edit retag flag offensive 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

1 Answer

Sort by ยป oldest newest most voted
4

answered 2018-10-10 12:27:38 -0500

tfoote gravatar image

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

See: https://answers.ros.org/question/1940... or https://answers.ros.org/question/2968... or https://answers.ros.org/question/2294...

edit flag offensive delete link more

Comments

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.

aPonza gravatar imageaPonza ( 2018-10-11 05:48:11 -0500 )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').

aPonza gravatar imageaPonza ( 2018-10-11 08:34:07 -0500 )edit

Question Tools

2 followers

Stats

Asked: 2018-10-10 04:52:05 -0500

Seen: 118 times

Last updated: Oct 11 '18