Enhance minimum jerk problem formulation