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.
