Fix shoulder motion in minimum jerk problem