# How to implement velocity transformation?

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 close merge delete

Sort by » oldest newest most voted 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.

http://docs.ros.org/hydro/api/tf/html...

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

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?

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.

But does transformVector apply a translation to the velocity vector? There is code in tf for transforming twists (6dof) in tf but commented out. http://docs.ros.org/hydro/api/tf/html...

It's commented out due to lack of clarity of the twist message representation: http://wiki.ros.org/tf/Reviews/2010-0...

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 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


Adapted from another answer https://answers.ros.org/question/1961... . 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)
q2.append(0.0)
return t.quaternion_multiply(
t.quaternion_multiply(q1, q2),
t.quaternion_conjugate(q1)
)[:3]

#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)

more

1

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.