Cleanup LeggedRobotPlanningProblem