change the plan a bit, add a constraint on pelvis height at apex