Get velocities of robot links from /tf

Hi everyone

So, what I basically need is the absolute velocities (angular and linear) of all links of my robot arm. My approach was the following. Write a subscriber to /tf and then do something like this:

  geometry_msgs::TransformStamped  transformStamped = tfBuffer.lookupTransform(frameName, now, frameName, past, "world", ros::Duration(0.1));


where tfBuffer is a tf2_ros::Buffer, frameName is the name of the frame attached to the link I am looking at, now is the current time of the subscribed message, past is the time of the last message and world is the fixed frame. This transform I would use to get the translation as well as the rotation between these two times of the same frame. Dividing this by the difference between past and now should give me the velocity. The problem is: I could not find out in which frame the above transform is represented? I am not quite wure from the documentation. I guess it is in the "source frame", so in my case the "frameName" at time "past"? I would need it in the world frame since I want the absolute velocity. So would I need to transform the translation vector and the rotation quaternion I get first into world frame, right? Anyone has an idea? Also if there are ideas of how to get the velocities in a more straight forward way, any help is much appreciated.

edit retag close merge delete

Sort by » oldest newest most voted

To get approximate velocities out of a tf system you can do discrete differentiation.

You can compute the pose of the object in question at t0 and at t1, giving you p0 and p1. Use tf to get those poses in the coordinate frame of interest. Once they're in the same coordinate frame then the velocity is simply (p1 - p0)/(t1-t0) ala the change in pose divided by the change in time.

You can do the same with the transforms as well, they have slightly more information, but I find imagining the poses above to be easier.

more