Ask Your Question
0

Is there a standard way to transform a pose between coordinate frames?

asked 2017-07-26 04:33:17 -0600

linusnie gravatar image

updated 2017-07-26 04:35:20 -0600

I have a pose message that I want to transform between two (non-static) coordinate frames. Currently I am doing the following in python:

   def rotate_orientation(ori, q):
    rot_mat = tf.transformations.quaternion_matrix(q)
    pose_rot = rot_mat.dot([ori.x, ori.y, ori.z, ori.w])
    ori.x = pose_rot[0]
    ori.y = pose_rot[1]
    ori.z = pose_rot[2]
    ori.w = pose_rot[3]

  def translate_position(pos, t):
    pos.x += t[0]
    pos.y += t[1]
    pos.z += t[2]

  def odom_to_utm(odom):
    global utm_pub

    trans,rot = listener.lookupTransform('utm', 'odom', rospy.Time(0))

    translate_position(odom.pose.pose.position, trans)
    rotate_orientation(odom.pose.pose.orientation, rot)

    utm_pub.publish(utm)

But it feels a bit clunky. Is there a more standard way to switch between coordinates frames? Something like a tf.transformPose(pose, frame1, frame2) that returns pose in frame 2.

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
2

answered 2017-07-26 13:13:19 -0600

Airuno2L gravatar image

updated 2017-07-26 13:14:30 -0600

There is something like that, and you almost guessed it!

listener.transformPose(target_frame, pose_msg) -> pose_msg

You don't need to enter the frame1 because it is listed inside the pose_msg.

You can find more information here. And here is the function in the API.

edit flag offensive delete link more

Comments

Are you using -> to say that function returns pose_msg?

jayess gravatar imagejayess ( 2017-07-26 13:23:28 -0600 )edit

thanks! this is exactly what I was looking for

linusnie gravatar imagelinusnie ( 2017-07-26 13:26:27 -0600 )edit

Yes, it returns a new pose_msg

Airuno2L gravatar imageAiruno2L ( 2017-07-26 13:43:28 -0600 )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

Stats

Asked: 2017-07-26 04:33:17 -0600

Seen: 2,304 times

Last updated: Jul 26 '17