First working tracking of the robot with the Mocap.
DANGEROUS be careful.