Code added to ensure the orientation at the final knotpoint of a
trajectory is met. This solution works, but there is probably a better
way to do it using the Reflexxes flags.