Several improvements in the trajectory tracking.
Added contraints and minimizing the jerk in the orientations optimization.
Added visual constraints in main optimization.