ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | Q&A answers.ros.org

# How can the velocity of a robot be found if we have VICON coordinates available?

I have coordinates of the robot available using VICON motion capture system. How can they be used to find the velocity of robot at each instant?

edit retag close merge delete

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.

( 2016-05-13 02:08:33 -0600 )edit

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.

( 2016-05-13 10:41:26 -0600 )edit

Sort by ยป oldest newest most voted

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.

more

1

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.

( 2016-05-17 09:54:27 -0600 )edit

What is o[k-1]}^(-1) for? I took every nth X-Y coordinates, calculated the 2D distance between these two coordinates and divided the distance by the difference in timestamp between the coordinates. It is similar to your approach except o[k-1]}^(-1).

( 2016-06-01 15:33:17 -0600 )edit

This is the de-rotation. When using it, you will have the robot's velocity in the robot's body frame. Otherwise, your velocity will depend on the orientation of your motion tracking reference frame.

( 2016-06-06 07:36:07 -0600 )edit