How to implement velocity transformation?

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

Qt_Yeung
90 ●10 ●15 ●17

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

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

add a comment

3

answered 2014-09-09 10:29:29 -0600

Tom Moore

flag of United Kingdom of Great Britain and Northern Ireland

13689 ●74 ●240 ●232 http://github.com/ayrton04

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

edit flag offensive delete link

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?

Qt_Yeung ( 2014-09-09 16:19:25 -0600 )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 ( 2014-09-10 07:54:31 -0600 )edit

But does transformVector apply a translation to the velocity vector?

claudione ( 2018-10-30 04:25:31 -0600 )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 ( 2020-05-31 16:19:28 -0600 )edit

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

Tom Moore ( 2020-06-15 03:45:32 -0600 )edit

add a comment

2

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

tfoote

58457 ●128 ●550 ●526 http://www.ros.org/

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

edit flag offensive delete link

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 ( 2014-09-10 07:02:44 -0600 )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 ( 2019-08-26 03:35:45 -0600 )edit

add a comment

0

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

JadTawil
71 ●21 ●25 ●32

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

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)

edit flag offensive delete link

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.

jarvisschultz ( 2019-05-29 19:33:04 -0600 )edit

add a comment

How to implement velocity transformation?

3 Answers

Comments

Comments

Comments

Question Tools

Stats

Related questions

How to implement velocity transformation? edit

3 Answers

Comments

Comments

Comments

Question Tools

Stats

Related questions

How to implement velocity transformation?