Bug fixed when calculating the angular speed.
The speed sent in a trajectory should not be multiplied by pi.