Ask Your Question

# 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

## 3 Answers

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

## Comments

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?

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

Are you asking about whether this is necessary mathematically, or whether r_l accounts for it? 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

## Comments

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

## Comments

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.

## Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

## Stats

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

Seen: 6,255 times

Last updated: May 29 '19