Trajectory Control Framework for a 6 DoF