Ask Your Question

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

asked 2016-05-10 14:56:40 -0500

Ros User gravatar image

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

donald gravatar image donald  ( 2016-05-13 02:08:33 -0500 )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.

Ros User gravatar image Ros User  ( 2016-05-13 10:41:26 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted

answered 2016-05-17 09:07:39 -0500

donald gravatar image

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.

edit flag offensive delete link more



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.

jarvisschultz gravatar image jarvisschultz  ( 2016-05-17 09:54:27 -0500 )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).

Ros User gravatar image Ros User  ( 2016-06-01 15:33:17 -0500 )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.

donald gravatar image donald  ( 2016-06-06 07:36:07 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools



Asked: 2016-05-10 14:56:40 -0500

Seen: 640 times

Last updated: May 17 '16