Ask Your Question

How to implement velocity transformation?

asked 2014-09-09 08:22:10 -0500

Qt_Yeung gravatar image

updated 2014-09-09 08:24:44 -0500

Hi all,

i have the velocity topic /cmd_vel, and it's the type geometry_msgs::Twist, it is given in the object reference frame, and now i need to transform the velocity to another frame, such as my robot's base frame. How can i do that, if i know the translation and rotation between the two frames?

I think it is different from position transformation, because velocity is 6 dimension and position only 3.

Does someone have this knowledge? Any help is appreciated.

edit retag flag offensive close merge delete

3 Answers

Sort by ยป oldest newest most voted

answered 2014-09-09 10:29:29 -0500

Tom Moore gravatar image

You can think of linear velocity as a vector with three components. If velocity is given in some frame, and you want it in another frame, it's the same as just giving the vector in the target frame's coordinate system. The tf TransformListener has a handy transform for doing this. It's called transformVector.

Put the three components of linear velocity into a vector and transform it to the target frame.

Alternatively, you can put the values in a point and just rotate them using the rotation in the target frame's transform (and only the rotation). This should work for the angular velocities as well (e.g., a robot that has positive yaw velocity in its body frame and is rolled over -90 degrees in the odom frame will have positive pitch velocity in the odom frame).

edit flag offensive delete link more


Do you mean:

  • Velocity transformation only depends on frame rotation, but not translation?

  • I can divide the Twist velocity type into two separate vectors, linear velocity vector and angular velocity vector, each with three components, and multiply them by rotation matrix separately?

Qt_Yeung gravatar image Qt_Yeung  ( 2014-09-09 16:19:25 -0500 )edit

Yes to both your bullet points. Apologies if I was unclear.

Not knowing exactly what you're trying to do, though, it's probably a good idea to check out the code review posted by @tfoote. You can also review some of the tf transform utility methods to see what they're doing.

Tom Moore gravatar image Tom Moore  ( 2014-09-10 07:54:31 -0500 )edit

But does transformVector apply a translation to the velocity vector?

claudione gravatar image claudione  ( 2018-10-30 04:25:31 -0500 )edit

@Tom Moore, Hi, do you know if transforming the linear velocities get affected by the angular velocities? If I have a TF Tree A-->B--->C, where A is the root node and C is the grandhchild of A. I want to transform the velocity vector v(a twist message that has both linear and angular component) from C frame to A frame. So, in order to find the linear veclocity vector of v from C to A, do I go about by just multiplying the v.linear with transformation matrix of B and then with transformation matrix of A ? Or should I account for the effects of angular velocity of v.

Joy16 gravatar image Joy16  ( 2020-05-31 16:19:28 -0500 )edit

Are you asking about whether this is necessary mathematically, or whether r_l accounts for it?

Tom Moore gravatar image Tom Moore  ( 2020-06-15 03:45:32 -0500 )edit

answered 2014-09-09 19:20:36 -0500

tfoote gravatar image

There is code in tf for transforming twists (6dof) in tf but commented out.

It's commented out due to lack of clarity of the twist message representation:

edit flag offensive delete link more


It is commented out, so it is not added into Hydro? Do i have to transform twists with my own code?

Such as:

tf::Vector3 out_rot = transform.getBasis() * twist_rot;

tf::Vector3 out_vel = transform.getBasis()* twist_vel + transform.getOrigin().cross(out_rot);

Qt_Yeung gravatar image Qt_Yeung  ( 2014-09-10 07:02:44 -0500 )edit

@Qt_Yeung As far as I understood, if we have a transformation matrix that transforms Frame_2 (F2) to Frame_1(F1) given by: T= [R T ; 0 0 0 1] where R is the rotational matrix and T is the translational matrix then a vector V2 with coordinates in F2 is expressed in F1 by : V1 = [R ass(T)*R ; 0(3x3) R] V2 where ass is the skew symetric matrix. so from this we notice that the first three components of the velocity which is the linear one depend on the translational vector T !!Is what I was saying wrong!!! but why these lines are added:

geometry_msgs::TwistStamped interframe_twist;  lookupVelocity(target_frame, msg_in.header.frame_id, msg_in.header.stamp, ros::Duration(0.1), interframe_twist); //\todo get rid of hard coded number
rayane gravatar image rayane  ( 2019-08-26 03:35:45 -0500 )edit

answered 2018-10-22 13:09:30 -0500

JadTawil gravatar image

updated 2019-05-29 18:00:43 -0500

Adapted from another answer . I used this to transform my velocity from global coordinates (so velocity in the north direction, east directions, and rotation about the z-axis, which does not change from global to base coordinates) to the coordinates of the base-frame (base_footprint)

# rotate vector v1 by quaternion q1 
def qv_mult(q1, v1):
    #v1 = t.unit_vector(v1)
    q2 = list(v1)
    return t.quaternion_multiply(
        t.quaternion_multiply(q1, q2), 

#twist_rot is the velocity in euler angles (rate of roll, rate of pitch, rate of yaw; angular.x, angular.y, angular.z)
#twist_vel is the velocity in cartesian coordinates (linear.x, linear.y, linear.z)
def transform_twist(twist_rot, twist_vel):
    map_to_base_trans, map_to_base_rot = listener.lookupTransform("map", "base_footprint", rospy.Time());

    q1 = map_to_base_rot
    out_vel = qv_mult(q1, twist_vel)
    out_rot = twist_rot

    return out_vel, out_rot
#out_vel is the linear velocity w.r.t to the base_footpring, linear.x, linear.y, linear.z
#out_Rot is the angular velocity w.r.t. to the base_footprint, angular.x, angular.y, angular.z
out_vel, out_rot = transform_twist(twist_rot, twist_vel)
edit flag offensive delete link more



Looks like this would only work with a twist containing only translational velocity components. The complication mentioned 4 years ago by @tfoote likely stems from a lack of clarity about how a twist in, e.g., the target_frame is being measured. There are multiple, equally valid ways to describe a 6-DOF twist including body twists, spatial twists, and hybrid twists. The correct transformation for each type of twist is different.

jarvisschultz gravatar image jarvisschultz  ( 2019-05-29 19:33:04 -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



Asked: 2014-09-09 08:22:10 -0500

Seen: 6,255 times

Last updated: May 29 '19