Inverted x and y in the trajectory as the forward reference axis of the robot is y instead of x
Fixed a bug where initial and final trajectory values were not correct