Fixed followTrajectory

linear and angular velocity was switched in line 66 and wrong sign was
used in line 66