Bugs fixed to move robot along a trajectory