| ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
One possibility is to use tf's Transformer::lookupTwist to calculate the velocity. If you want to do it "by hand" you can compute the current (online) linear velocity expressed in the robot's body frame as:
v[k] = {o[k-1]}^(-1) * (p[k] - p[k-1]) / (t[k] - t[k-1]),
using the variables
v[k]: current linear velocity (delayed by 1/200 seconds),
{o[k-1]}^(-1): previous orientation inverted (unit quaternion or rotation matrix),
p[k], p[k-1]: positions of this and previous frame,
t[k], t[k-1]: timestamp of this and previous frame.
This is, you de-rotate the translation vector from the last to the current transform and divide it by the time difference.
The offline version of this is
v[k] = {o[k]}^(-1) * (p[k+1] - p[k-1]) / (t[k+1] - t[k-1]),
which is the symmetric derivative.
Depending on how well your robot is tracked, the velocity signal might be noisy, so you might want to use a filter.
Note that the lookupTwist function does not currently seem correct (bug report). I was playing with this function last week and I believe that (at a minimum) @tfoote suggestion needs to be incorporated.
Asked: 2016-05-10 14:56:40 -0500
Seen: 1,004 times
Last updated: May 17 '16
ROS Answers is licensed under Creative Commons Attribution 3.0 Content on this site is licensed under a Creative Commons Attribution Share Alike 3.0 license.
Can you specify this a bit? What exactly are you given from the mocap sytstem (position, orientation, timestamp)? Do you want to have the velocity in the robot body frame? I did the same lately for an Optitrack system, so maybe I could give you some hints.
The system gives x,y,z position of robot's center,orientation of robot and timestamps(secs).These messages are published at the rate of 100 Hz.As my robot is ground based one,I am using only x and y position(not orientation) to find the velocity. I want to find velocity of the robot's center.