def test_acados_custom_dynamics(problem_type_custom): if platform == "win32": print("Test for ACADOS on Windows is skipped") return from bioptim.examples.getting_started import custom_dynamics as ocp_module bioptim_folder = os.path.dirname(ocp_module.__file__) ocp = ocp_module.prepare_ocp( biorbd_model_path=bioptim_folder + "/models/cube.bioMod", problem_type_custom=problem_type_custom, ode_solver=OdeSolver.RK4(), use_sx=True, ) constraints = ConstraintList() constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m2") ocp.update_constraints(constraints) sol = ocp.solve(solver=Solver.ACADOS()) # Check some of the results q, qdot, tau = sol.states["q"], sol.states["qdot"], sol.controls["tau"] # initial and final position np.testing.assert_almost_equal(q[:, 0], np.array((2, 0, 0)), decimal=6) np.testing.assert_almost_equal(q[:, -1], np.array((2, 0, 1.57))) # initial and final velocities np.testing.assert_almost_equal(qdot[:, 0], np.array((0, 0, 0))) np.testing.assert_almost_equal(qdot[:, -1], np.array((0, 0, 0))) # initial and final controls np.testing.assert_almost_equal(tau[:, 0], np.array((0, 9.81, 2.27903226))) np.testing.assert_almost_equal(tau[:, -2], np.array((0, 9.81, -2.27903226)))
def test_acados_custom_dynamics(problem_type_custom): bioptim_folder = TestUtils.bioptim_folder() cube = TestUtils.load_module( bioptim_folder + "/examples/getting_started/custom_dynamics.py") ocp = cube.prepare_ocp( biorbd_model_path=bioptim_folder + "/examples/getting_started/cube.bioMod", problem_type_custom=problem_type_custom, ode_solver=OdeSolver.RK4(), use_sx=True, ) constraints = ConstraintList() constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker_idx=0, second_marker_idx=2) ocp.update_constraints(constraints) sol = ocp.solve(solver=Solver.ACADOS) # Check some of the results q, qdot, tau = sol.states["q"], sol.states["qdot"], sol.controls["tau"] # initial and final position np.testing.assert_almost_equal(q[:, 0], np.array((2, 0, 0)), decimal=6) np.testing.assert_almost_equal(q[:, -1], np.array((2, 0, 1.57))) # initial and final velocities np.testing.assert_almost_equal(qdot[:, 0], np.array((0, 0, 0))) np.testing.assert_almost_equal(qdot[:, -1], np.array((0, 0, 0))) # initial and final controls np.testing.assert_almost_equal(tau[:, 0], np.array((0, 9.81, 2.27903226))) np.testing.assert_almost_equal(tau[:, -1], np.array( (0, 9.81, -2.27903226)))
def prepare_ocp(biorbd_model_path, final_time, number_shooting_points, ode_solver=OdeSolver.RK): # --- Options --- #nq # Model path biorbd_model = biorbd.Model(biorbd_model_path) nq = biorbd_model.nbQ() # Problem parameters tau_min, tau_max, tau_init = -100, 100, 0 # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100) # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN) # Constraints constraints = ConstraintList() constraints.add(ConstraintFcn.ALIGN_SEGMENT_WITH_CUSTOM_RT, node=Node.ALL, segment_idx=2, rt_idx=0) # Path constraint x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model)) x_bounds[0][2, [0, -1]] = [-1.57, 1.57] x_bounds[0][nq:, [0, -1]] = 0 # Initial guess x_init = InitialGuessList() x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot())) # Define control path constraint u_bounds = BoundsList() u_bounds.add([tau_min] * biorbd_model.nbGeneralizedTorque(), [tau_max] * biorbd_model.nbGeneralizedTorque()) u_init = InitialGuessList() u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque()) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, number_shooting_points, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, ode_solver=ode_solver, )
def test_acados_constraints_all(): PROJECT_FOLDER = Path(__file__).parent / ".." spec = importlib.util.spec_from_file_location( "constraint", str(PROJECT_FOLDER) + "/examples/align/align_marker_on_segment.py", ) constraint = importlib.util.module_from_spec(spec) spec.loader.exec_module(constraint) ocp = constraint.prepare_ocp( biorbd_model_path=str(PROJECT_FOLDER) + "/examples/align/cube_and_line.bioMod", number_shooting_points=30, final_time=2, initialize_near_solution=True, constr=False, use_SX=True, ) constraints = ConstraintList() constraints.add(ConstraintFcn.ALIGN_MARKER_WITH_SEGMENT_AXIS, node=Node.ALL, marker_idx=1, segment_idx=2, axis=(Axe.X)) ocp.update_constraints(constraints) sol = ocp.solve(solver=Solver.ACADOS, solver_options={"print_level": 0}) # Check some of the results states, controls = Data.get_data(ocp, sol["x"]) q, qdot, tau = states["q"], states["q_dot"], controls["tau"] # final position np.testing.assert_almost_equal(q[:, 0], np.array([0.8385190835, 0, 0, -0.212027938]), decimal=6) np.testing.assert_almost_equal(q[:, -1], np.array( (0.8385190835, 0, 1.57, -0.212027938)), decimal=6) np.testing.assert_almost_equal(qdot[:, 0], np.array([0, 0, 0, 0]), decimal=6) np.testing.assert_almost_equal(qdot[:, -1], np.array([0, 0, 0, 0]), decimal=6) # initial and final controls np.testing.assert_almost_equal(tau[:, 0], np.array((0, 9.81, 2.27903226, 0)), decimal=6) np.testing.assert_almost_equal(tau[:, -1], np.array((0, 9.81, -2.27903226, 0)), decimal=6)
def __init__( self, model_path: str, violin: Violin, bow: Bow, n_cycles: int, bow_starting: BowPosition.TIP, init_file: str = None, use_muscles: bool = True, time_per_cycle: float = 1, n_shooting_per_cycle: int = 30, solver: Solver = Solver.IPOPT, n_threads: int = 8, ): self.model_path = model_path self.model = biorbd.Model(self.model_path) self.n_q = self.model.nbQ() self.n_tau = self.model.nbGeneralizedTorque() self.use_muscles = use_muscles self.n_mus = self.model.nbMuscles() if self.use_muscles else 0 self.violin = violin self.bow = bow self.bow_starting = bow_starting self.n_cycles = n_cycles self.n_shooting_per_cycle = n_shooting_per_cycle self.n_shooting = self.n_shooting_per_cycle * self.n_cycles self.time_per_cycle = time_per_cycle self.time = self.time_per_cycle * self.n_cycles self.solver = solver self.n_threads = n_threads if self.use_muscles: self.dynamics = Dynamics( DynamicsFcn.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN) else: self.dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN) self.x_bounds = Bounds() self.u_bounds = Bounds() self._set_bounds() self.x_init = InitialGuess() self.u_init = InitialGuess() self._set_initial_guess(init_file) self.objective_functions = ObjectiveList() self._set_generic_objective_functions() self.constraints = ConstraintList() self._set_generic_constraints() self._set_generic_ocp() if use_muscles: online_muscle_torque(self.ocp)
def test_acados_constraints_all(): if platform == "win32": print("Test for ACADOS on Windows is skipped") return bioptim_folder = TestUtils.bioptim_folder() track = TestUtils.load_module(bioptim_folder + "/examples/track/track_marker_on_segment.py") ocp = track.prepare_ocp( biorbd_model_path=bioptim_folder + "/examples/track/models/cube_and_line.bioMod", n_shooting=30, final_time=2, initialize_near_solution=True, constr=False, use_sx=True, ) constraints = ConstraintList() constraints.add(ConstraintFcn.TRACK_MARKER_WITH_SEGMENT_AXIS, node=Node.ALL, marker="m1", segment="seg_rt", axis=Axis.X) ocp.update_constraints(constraints) sol = ocp.solve(solver=Solver.ACADOS) # Check some of the results q, qdot, tau = sol.states["q"], sol.states["qdot"], sol.controls["tau"] # final position np.testing.assert_almost_equal(q[:, 0], np.array([2.28988221, 0, 0, 2.95087911e-01]), decimal=6) np.testing.assert_almost_equal(q[:, -1], np.array( (2.28215749, 0, 1.57, 6.62470772e-01)), decimal=6) np.testing.assert_almost_equal(qdot[:, 0], np.array([0, 0, 0, 0]), decimal=6) np.testing.assert_almost_equal(qdot[:, -1], np.array([0, 0, 0, 0]), decimal=6) # initial and final controls np.testing.assert_almost_equal(tau[:, 0], np.array((0.04483914, 9.90739842, 2.24951691, 0.78496612)), decimal=6) np.testing.assert_almost_equal(tau[:, -2], np.array((0.15945561, 10.03978178, -2.36075327, 0.07267697)), decimal=6)
def test_acados_one_end_constraints(): if platform == "win32": print("Test for ACADOS on Windows is skipped") return bioptim_folder = TestUtils.bioptim_folder() cube = TestUtils.load_module(bioptim_folder + "/examples/acados/cube.py") ocp = cube.prepare_ocp( biorbd_model_path=bioptim_folder + "/examples/acados/models/cube.bioMod", n_shooting=10, tf=2, ) model = ocp.nlp[0].model objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Mayer.TRACK_STATE, index=0, key="q", weight=100, target=np.array([[1]]), multi_thread=False) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, multi_thread=False) ocp.update_objectives(objective_functions) # Path constraint x_bounds = QAndQDotBounds(model) x_bounds[1:6, [0, -1]] = 0 x_bounds[0, 0] = 0 ocp.update_bounds(x_bounds=x_bounds) constraints = ConstraintList() constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m2") ocp.update_constraints(constraints) sol = ocp.solve(solver=Solver.ACADOS) # Check some of the results q, qdot, tau = sol.states["q"], sol.states["qdot"], sol.controls["tau"] # final position np.testing.assert_almost_equal(q[:, -1], np.array((2, 0, 0)), decimal=6) # initial and final controls np.testing.assert_almost_equal(tau[:, 0], np.array((2.72727272, 9.81, 0)), decimal=6) np.testing.assert_almost_equal(tau[:, -2], np.array((-2.72727272, 9.81, 0)), decimal=6)
def test_acados_constraints_all(): if platform == "win32": print("Test for ACADOS on Windows is skipped") return bioptim_folder = TestUtils.bioptim_folder() track = TestUtils.load_module(bioptim_folder + "/examples/track/track_marker_on_segment.py") ocp = track.prepare_ocp( biorbd_model_path=bioptim_folder + "/examples/track/cube_and_line.bioMod", n_shooting=30, final_time=2, initialize_near_solution=True, constr=False, use_sx=True, ) constraints = ConstraintList() constraints.add(ConstraintFcn.TRACK_MARKER_WITH_SEGMENT_AXIS, node=Node.ALL, marker="m1", segment="seg_rt", axis=(Axis.X)) ocp.update_constraints(constraints) sol = ocp.solve(solver=Solver.ACADOS) # Check some of the results q, qdot, tau = sol.states["q"], sol.states["qdot"], sol.controls["tau"] # final position np.testing.assert_almost_equal(q[:, 0], np.array([0.8385190835, 0, 0, -0.212027938]), decimal=6) np.testing.assert_almost_equal(q[:, -1], np.array( (0.8385190835, 0, 1.57, -0.212027938)), decimal=6) np.testing.assert_almost_equal(qdot[:, 0], np.array([0, 0, 0, 0]), decimal=6) np.testing.assert_almost_equal(qdot[:, -1], np.array([0, 0, 0, 0]), decimal=6) # initial and final controls np.testing.assert_almost_equal(tau[:, 0], np.array((0, 9.81, 2.27903226, 0)), decimal=6) np.testing.assert_almost_equal(tau[:, -1], np.array((0, 9.81, -2.27903226, 0)), decimal=6)
def test_acados_one_end_constraints(): PROJECT_FOLDER = Path(__file__).parent / ".." spec = importlib.util.spec_from_file_location( "constraint", str(PROJECT_FOLDER) + "/examples/acados/cube.py", ) constraint = importlib.util.module_from_spec(spec) spec.loader.exec_module(constraint) ocp = constraint.prepare_ocp( biorbd_model_path=str(PROJECT_FOLDER) + "/examples/acados/cube.bioMod", nbs=10, tf=2, ) model = ocp.nlp[0].model objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Mayer.TRACK_STATE, index=0, weight=100, target=np.array([[1]])) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100) ocp.update_objectives(objective_functions) # Path constraint x_bounds = QAndQDotBounds(model) x_bounds[1:6, [0, -1]] = 0 x_bounds[0, 0] = 0 ocp.update_bounds(x_bounds=x_bounds) constraints = ConstraintList() constraints.add(ConstraintFcn.ALIGN_MARKERS, node=Node.END, first_marker_idx=0, second_marker_idx=2) ocp.update_constraints(constraints) sol = ocp.solve(solver=Solver.ACADOS, solver_options={"print_level": 0}) # Check some of the results states, controls = Data.get_data(ocp, sol["x"]) q, qdot, tau = states["q"], states["q_dot"], controls["tau"] # final position np.testing.assert_almost_equal(q[:, -1], np.array((2, 0, 0)), decimal=6) # initial and final controls np.testing.assert_almost_equal(tau[:, 0], np.array((2.72727272, 9.81, 0)), decimal=6) np.testing.assert_almost_equal(tau[:, -1], np.array((-2.72727272, 9.81, 0)), decimal=6)
def test_acados_one_end_constraints(): bioptim_folder = TestUtils.bioptim_folder() cube = TestUtils.load_module(bioptim_folder + "/examples/acados/cube.py") ocp = cube.prepare_ocp( biorbd_model_path=bioptim_folder + "/examples/acados/cube.bioMod", n_shooting=10, tf=2, ) model = ocp.nlp[0].model objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Mayer.TRACK_STATE, index=0, weight=100, target=np.array([[1]])) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100) ocp.update_objectives(objective_functions) # Path constraint x_bounds = QAndQDotBounds(model) x_bounds[1:6, [0, -1]] = 0 x_bounds[0, 0] = 0 ocp.update_bounds(x_bounds=x_bounds) constraints = ConstraintList() constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker_idx=0, second_marker_idx=2) ocp.update_constraints(constraints) sol = ocp.solve(solver=Solver.ACADOS, solver_options={"print_level": 0}) # Check some of the results q, qdot, tau = sol.states["q"], sol.states["qdot"], sol.controls["tau"] # final position np.testing.assert_almost_equal(q[:, -1], np.array((2, 0, 0)), decimal=6) # initial and final controls np.testing.assert_almost_equal(tau[:, 0], np.array((2.72727272, 9.81, 0)), decimal=6) np.testing.assert_almost_equal(tau[:, -1], np.array((-2.72727272, 9.81, 0)), decimal=6)
def test_custom_constraint_mx_fail(): def custom_mx_fail(pn): if pn.u is None: return None u = pn.nlp.controls return MX.zeros(u.shape), u.cx, MX.zeros(u.shape) bioptim_folder = TestUtils.bioptim_folder() model_path = bioptim_folder + "/examples/getting_started/models/cube.bioMod" constraints = ConstraintList() constraints.add(custom_mx_fail, node=Node.ALL) ocp = OptimalControlProgram( biorbd.Model(model_path), Dynamics(DynamicsFcn.TORQUE_DRIVEN), 30, 2, constraints=constraints ) with pytest.raises(RuntimeError, match="Ipopt doesn't support SX/MX types in constraints bounds"): ocp.solve()
def test_custom_constraint_mx_fail(): def custom_mx_fail(pn): return MX(0), vertcat(*pn.u), MX(0) bioptim_folder = TestUtils.bioptim_folder() model_path = bioptim_folder + "/examples/getting_started/cube.bioMod" constraints = ConstraintList() constraints.add(custom_mx_fail, node=Node.ALL) ocp = OptimalControlProgram( biorbd.Model(model_path), Dynamics(DynamicsFcn.TORQUE_DRIVEN), 30, 2, constraints=constraints, ) with pytest.raises(RuntimeError, match="Ipopt doesn't support SX/MX types in constraints bounds"): sol = ocp.solve(show_online_optim=True)
def test_acados_custom_dynamics(problem_type_custom): # Load pendulum PROJECT_FOLDER = Path(__file__).parent / ".." spec = importlib.util.spec_from_file_location( "custom_problem_type_and_dynamics", str(PROJECT_FOLDER) + "/examples/getting_started/custom_dynamics.py", ) pendulum = importlib.util.module_from_spec(spec) spec.loader.exec_module(pendulum) ocp = pendulum.prepare_ocp( biorbd_model_path=str(PROJECT_FOLDER) + "/examples/getting_started/cube.bioMod", problem_type_custom=problem_type_custom, ode_solver=OdeSolver.RK, use_SX=True, ) constraints = ConstraintList() constraints.add(ConstraintFcn.ALIGN_MARKERS, node=Node.END, first_marker_idx=0, second_marker_idx=2) ocp.update_constraints(constraints) sol = ocp.solve(solver=Solver.ACADOS) # Check some of the results states, controls = Data.get_data(ocp, sol["x"]) q, qdot, tau = states["q"], states["q_dot"], controls["tau"] # initial and final position np.testing.assert_almost_equal(q[:, 0], np.array((2, 0, 0)), decimal=6) np.testing.assert_almost_equal(q[:, -1], np.array((2, 0, 1.57))) # initial and final velocities np.testing.assert_almost_equal(qdot[:, 0], np.array((0, 0, 0))) np.testing.assert_almost_equal(qdot[:, -1], np.array((0, 0, 0))) # initial and final controls np.testing.assert_almost_equal(tau[:, 0], np.array((0, 9.81, 2.27903226))) np.testing.assert_almost_equal(tau[:, -1], np.array( (0, 9.81, -2.27903226)))
def test_custom_constraint_mx_fail(): def custom_mx_fail(ocp, nlp, t, x, u, p): return MX(0), vertcat(*u), MX(0) PROJECT_FOLDER = Path(__file__).parent / ".." model_path = str(PROJECT_FOLDER) + "/examples/getting_started/cube.bioMod" constraints = ConstraintList() constraints.add(custom_mx_fail, node=Node.ALL) ocp = OptimalControlProgram( biorbd.Model(model_path), Dynamics(DynamicsFcn.TORQUE_DRIVEN), 30, 2, constraints=constraints, ) with pytest.raises( RuntimeError, match="Ipopt doesn't support SX/MX types in constraints bounds"): sol = ocp.solve(show_online_optim=True)
def test_acados_constraints_end_all(): if platform == "win32": print("Test for ACADOS on Windows is skipped") return bioptim_folder = TestUtils.bioptim_folder() track = TestUtils.load_module(bioptim_folder + "/examples/track/track_marker_on_segment.py") ocp = track.prepare_ocp( biorbd_model_path=bioptim_folder + "/examples/track/models/cube_and_line.bioMod", n_shooting=30, final_time=2, initialize_near_solution=True, constr=False, use_sx=True, ) constraints = ConstraintList() constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m5") constraints.add(ConstraintFcn.TRACK_MARKER_WITH_SEGMENT_AXIS, node=Node.ALL_SHOOTING, marker="m1", segment="seg_rt", axis=Axis.X) ocp.update_constraints(constraints) sol = ocp.solve(solver=Solver.ACADOS) # Check some of the results q, qdot, tau = sol.states["q"], sol.states["qdot"], sol.controls["tau"] # final position np.testing.assert_almost_equal(q[:, 0], np.array([2.01701330, 0, 0, 3.20057865e-01]), decimal=6) np.testing.assert_almost_equal(q[:, -1], np.array((2, 0, 1.57, 7.85398168e-01)), decimal=6) np.testing.assert_almost_equal(qdot[:, 0], np.array([0, 0, 0, 0]), decimal=6) np.testing.assert_almost_equal(qdot[:, -1], np.array([0, 0, 0, 0]), decimal=6) # initial and final controls np.testing.assert_almost_equal(tau[:, 0], np.array((0.04648408, 9.88616194, 2.24285498, 0.864213)), decimal=6) np.testing.assert_almost_equal(tau[:, -2], np.array((0.19389194, 9.99905781, -2.37713652, -0.19858311)), decimal=6)
def prepare_ocp(biorbd_model_path, ode_solver=OdeSolver.RK): # --- Options --- # # Model path biorbd_model = biorbd.Model(biorbd_model_path) # Problem parameters number_shooting_points = 30 final_time = 2 tau_min, tau_max, tau_init = -100, 100, 0 # Add objective functions objective_functions = ObjectiveOption(Objective.Lagrange.MINIMIZE_TORQUE, weight=100) # Dynamics dynamics = DynamicsTypeOption(DynamicsType.TORQUE_DRIVEN) # Constraints constraints = ConstraintList() constraints.add(custom_func_align_markers, node=Node.START, first_marker_idx=0, second_marker_idx=1) constraints.add(custom_func_align_markers, node=Node.END, first_marker_idx=0, second_marker_idx=2) # Path constraint x_bounds = BoundsOption(QAndQDotBounds(biorbd_model)) x_bounds[1:6, [0, -1]] = 0 x_bounds[2, -1] = 1.57 # Initial guess x_init = InitialGuessOption([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot())) # Define control path constraint u_bounds = BoundsOption([[tau_min] * biorbd_model.nbGeneralizedTorque(), [tau_max] * biorbd_model.nbGeneralizedTorque()]) u_init = InitialGuessOption([tau_init] * biorbd_model.nbGeneralizedTorque()) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, number_shooting_points, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, ode_solver=ode_solver, )
def prepare_ocp(biorbd_model_path="cubeSym.bioMod", ode_solver=OdeSolver.RK): # --- Options --- # # Model path biorbd_model = biorbd.Model(biorbd_model_path) # Problem parameters number_shooting_points = 30 final_time = 2 tau_min, tau_max, tau_init = -100, 100, 0 # Add objective functions objective_functions = ObjectiveList() objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, weight=100) # Dynamics dynamics = DynamicsTypeList() dynamics.add(DynamicsType.TORQUE_DRIVEN) # Constraints constraints = ConstraintList() constraints.add(Constraint.ALIGN_MARKERS, node=Node.START, first_marker_idx=0, second_marker_idx=1) constraints.add(Constraint.ALIGN_MARKERS, node=Node.END, first_marker_idx=0, second_marker_idx=2) constraints.add(Constraint.PROPORTIONAL_STATE, node=Node.ALL, first_dof=2, second_dof=3, coef=-1) # Path constraint x_bounds = BoundsList() x_bounds.add(QAndQDotBounds(biorbd_model)) x_bounds[0][4:8, [0, -1]] = 0 # Initial guess x_init = InitialGuessList() x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot())) # Define control path constraint u_bounds = BoundsList() u_bounds.add([[tau_min] * biorbd_model.nbQ(), [tau_max] * biorbd_model.nbQ()]) u_init = InitialGuessList() u_init.add([tau_init] * biorbd_model.nbQ()) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, number_shooting_points, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, ode_solver=ode_solver, )
def test_acados_constraints_end_all(): bioptim_folder = TestUtils.bioptim_folder() track = TestUtils.load_module(bioptim_folder + "/examples/track/track_marker_on_segment.py") ocp = track.prepare_ocp( biorbd_model_path=bioptim_folder + "/examples/track/cube_and_line.bioMod", n_shooting=30, final_time=2, initialize_near_solution=True, constr=False, use_sx=True, ) constraints = ConstraintList() constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker_idx=0, second_marker_idx=5) constraints.add(ConstraintFcn.TRACK_MARKER_WITH_SEGMENT_AXIS, node=Node.ALL, marker_idx=1, segment_idx=2, axis=(Axis.X)) ocp.update_constraints(constraints) sol = ocp.solve(solver=Solver.ACADOS, solver_options={"print_level": 0}) # Check some of the results q, qdot, tau = sol.states["q"], sol.states["qdot"], sol.controls["tau"] # final position np.testing.assert_almost_equal(q[:, 0], np.array([2, 0, 0, -0.139146705]), decimal=6) np.testing.assert_almost_equal(q[:, -1], np.array((2, 0, 1.57, -0.139146705)), decimal=6) np.testing.assert_almost_equal(qdot[:, 0], np.array([0, 0, 0, 0]), decimal=6) np.testing.assert_almost_equal(qdot[:, -1], np.array([0, 0, 0, 0]), decimal=6) # initial and final controls np.testing.assert_almost_equal(tau[:, 0], np.array((0, 9.81, 2.27903226, 0)), decimal=6) np.testing.assert_almost_equal(tau[:, -1], np.array((0, 9.81, -2.27903226, 0)), decimal=6)
def test_lagrange1_neg_multiphase_time_constraint(): with pytest.raises( RuntimeError, match="Time constraint/objective cannot declare more than once"): ( biorbd_model, n_shooting, final_time, time_min, time_max, torque_min, torque_max, torque_init, dynamics, x_bounds, x_init, u_bounds, u_init, ) = partial_ocp_parameters(n_phases=3) objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, phase=0) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TIME, phase=0) constraints = ConstraintList() constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.START, first_marker="m0", second_marker="m1", phase=0) constraints.add(ConstraintFcn.TIME_CONSTRAINT, node=Node.END, minimum=time_min[0], maximum=time_max[0], phase=0) OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, )
def prepare_nmpc(model_path, cycle_len, cycle_duration, n_cycles_simultaneous, n_cycles_to_advance, max_torque): model = biorbd.Model(model_path) dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN) x_bound = QAndQDotBounds(model) x_bound.min[0, :] = -2 * np.pi * n_cycles_simultaneous # Allow the wheel to spin as much as needed x_bound.max[0, :] = 0 u_bound = Bounds([-max_torque] * model.nbQ(), [max_torque] * model.nbQ()) x_init = InitialGuess( np.zeros( model.nbQ() * 2, ) ) u_init = InitialGuess( np.zeros( model.nbQ(), ) ) new_objectives = Objective(ObjectiveFcn.Lagrange.MINIMIZE_STATE, key="q") # Rotate the wheel and force the marker of the hand to follow the marker on the wheel wheel_target = np.linspace(-2 * np.pi * n_cycles_simultaneous, 0, cycle_len * n_cycles_simultaneous + 1)[ np.newaxis, : ] constraints = ConstraintList() constraints.add(ConstraintFcn.TRACK_STATE, key="q", index=0, node=Node.ALL, target=wheel_target) constraints.add( ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.ALL, first_marker="wheel", second_marker="COM_hand", axes=[Axis.X, Axis.Y], ) return MyCyclicNMPC( model, dynamics, cycle_len=cycle_len, cycle_duration=cycle_duration, n_cycles_simultaneous=n_cycles_simultaneous, n_cycles_to_advance=n_cycles_to_advance, objective_functions=new_objectives, constraints=constraints, x_init=x_init, u_init=u_init, x_bounds=x_bound, u_bounds=u_bound, )
def test_lagrange2_neg_multiphase_time_constraint(): with pytest.raises( RuntimeError, match="Time constraint/objective cannot declare more than once"): ( biorbd_model, number_shooting_points, final_time, time_min, time_max, torque_min, torque_max, torque_init, dynamics, x_bounds, x_init, u_bounds, u_init, ) = partial_ocp_parameters(nb_phases=3) objective_functions = ObjectiveList() objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, weight=100, phase=2) objective_functions.add(Objective.Lagrange.MINIMIZE_TIME, phase=2) constraints = ConstraintList() constraints.add(Constraint.ALIGN_MARKERS, node=Node.START, first_marker_idx=0, second_marker_idx=1, phase=2) constraints.add(Constraint.TIME_CONSTRAINT, node=Node.END, minimum=time_min[0], maximum=time_max[0], phase=2) OptimalControlProgram( biorbd_model, dynamics, number_shooting_points, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, )
def test_mayer_multiphase_time_constraint(): ( biorbd_model, n_shooting, final_time, time_min, time_max, torque_min, torque_max, torque_init, dynamics, x_bounds, x_init, u_bounds, u_init, ) = partial_ocp_parameters(n_phases=3) objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100, phase=0) objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_TIME, phase=0) constraints = ConstraintList() constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.START, first_marker_idx=0, second_marker_idx=1, phase=2) constraints.add(ConstraintFcn.TIME_CONSTRAINT, node=Node.END, minimum=time_min[0], maximum=time_max[0], phase=2) OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, )
def test_mayer_neg_monophase_time_constraint(): ( biorbd_model, n_shooting, final_time, time_min, time_max, torque_min, torque_max, torque_init, dynamics, x_bounds, x_init, u_bounds, u_init, ) = partial_ocp_parameters(n_phases=1) objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_TIME) constraints = ConstraintList() constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.START, first_marker_idx=0, second_marker_idx=1) constraints.add(ConstraintFcn.TIME_CONSTRAINT, node=Node.END, minimum=time_min[0], maximum=time_max[0]) with pytest.raises( RuntimeError, match="Time constraint/objective cannot declare more than once"): OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, )
def prepare_ocp(biorbd_model_path, phase_time, n_shooting, min_bound, max_bound, mu, ode_solver=OdeSolver.RK4()): # --- Options --- # # Model path biorbd_model = biorbd.Model(biorbd_model_path) tau_min, tau_max, tau_init = -500, 500, 0 tau_mapping = BiMapping([None, None, None, 0], [3]) # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_PREDICTED_COM_HEIGHT, weight=-1) # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN_WITH_CONTACT) # Constraints constraints = ConstraintList() constraints.add( ConstraintFcn.CONTACT_FORCE, min_bound=min_bound, max_bound=max_bound, node=Node.ALL, contact_force_idx=1, ) constraints.add( ConstraintFcn.CONTACT_FORCE, min_bound=min_bound, max_bound=max_bound, node=Node.ALL, contact_force_idx=2, ) constraints.add( ConstraintFcn.NON_SLIPPING, node=Node.ALL, normal_component_idx=(1, 2), tangential_component_idx=0, static_friction_coefficient=mu, ) # Path constraint n_q = biorbd_model.nbQ() n_qdot = n_q pose_at_first_node = [0, 0, -0.75, 0.75] # Initialize x_bounds x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model)) x_bounds[0][:, 0] = pose_at_first_node + [0] * n_qdot # Initial guess x_init = InitialGuessList() x_init.add(pose_at_first_node + [0] * n_qdot) # Define control path constraint u_bounds = BoundsList() u_bounds.add([tau_min] * tau_mapping.to_first.len, [tau_max] * tau_mapping.to_first.len) u_init = InitialGuessList() u_init.add([tau_init] * tau_mapping.to_first.len) return OptimalControlProgram( biorbd_model, dynamics, n_shooting, phase_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, tau_mapping=tau_mapping, ode_solver=ode_solver, )
def prepare_ocp( biorbd_model_path: str, n_shooting: int, final_time: float, loop_from_constraint: bool, ode_solver: OdeSolver = OdeSolver.RK4(), ) -> OptimalControlProgram: """ Prepare the program Parameters ---------- biorbd_model_path: str The path of the biorbd model final_time: float The time at the final node n_shooting: int The number of shooting points loop_from_constraint: bool If the looping cost should be a constraint [True] or an objective [False] ode_solver: OdeSolver The type of ode solver used Returns ------- The ocp ready to be solved """ biorbd_model = biorbd.Model(biorbd_model_path) # Add objective functions objective_functions = Objective(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100) # Dynamics dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN) # Constraints constraints = ConstraintList() constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.MID, first_marker_idx=0, second_marker_idx=2) constraints.add(ConstraintFcn.TRACK_STATE, node=Node.MID, index=2) constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker_idx=0, second_marker_idx=1) # Path constraint x_bounds = QAndQDotBounds(biorbd_model) # First node is free but mid and last are constrained to be exactly at a certain point. # The cyclic penalty ensures that the first node and the last node are the same. x_bounds[2:6, -1] = [1.57, 0, 0, 0] # Initial guess x_init = InitialGuess([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot())) # Define control path constraint tau_min, tau_max, tau_init = -100, 100, 0 u_bounds = Bounds([tau_min] * biorbd_model.nbGeneralizedTorque(), [tau_max] * biorbd_model.nbGeneralizedTorque()) u_init = InitialGuess([tau_init] * biorbd_model.nbGeneralizedTorque()) # ------------- # # A phase transition loop constraint is treated as # hard penalty (constraint) if weight is <= 0 [or if no weight is provided], or # as a soft penalty (objective) otherwise phase_transitions = PhaseTransitionList() if loop_from_constraint: phase_transitions.add(PhaseTransitionFcn.CYCLIC, weight=0) else: phase_transitions.add(PhaseTransitionFcn.CYCLIC, weight=10000) return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, ode_solver=ode_solver, phase_transitions=phase_transitions, )
def prepare_ocp(phase_time_constraint, use_parameter): # --- Inputs --- # final_time = (2, 5, 4) time_min = [1, 3, 0.1] time_max = [2, 4, 0.8] ns = (20, 30, 20) biorbd_model_path = TestUtils.bioptim_folder( ) + "/examples/optimal_time_ocp/cube.bioMod" ode_solver = OdeSolver.RK4() # --- Options --- # n_phases = len(ns) # Model path biorbd_model = (biorbd.Model(biorbd_model_path), biorbd.Model(biorbd_model_path), biorbd.Model(biorbd_model_path)) # Problem parameters tau_min, tau_max, tau_init = -100, 100, 0 # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100, phase=0) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100, phase=1) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100, phase=2) # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN, phase=0) dynamics.add(DynamicsFcn.TORQUE_DRIVEN, phase=1) dynamics.add(DynamicsFcn.TORQUE_DRIVEN, phase=2) # Constraints constraints = ConstraintList() constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.START, first_marker="m0", second_marker="m1", phase=0) constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m2", phase=0) constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m1", phase=1) constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m2", phase=2) constraints.add( ConstraintFcn.TIME_CONSTRAINT, node=Node.END, minimum=time_min[0], maximum=time_max[0], phase=phase_time_constraint, ) # Path constraint x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0])) # Phase 0 x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0])) # Phase 1 x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0])) # Phase 2 for bounds in x_bounds: for i in [1, 3, 4, 5]: bounds[i, [0, -1]] = 0 x_bounds[0][2, 0] = 0.0 x_bounds[2][2, [0, -1]] = [0.0, 1.57] # Initial guess x_init = InitialGuessList() x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot())) x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot())) x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot())) # Define control path constraint u_bounds = BoundsList() u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(), [tau_max] * biorbd_model[0].nbGeneralizedTorque()) u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(), [tau_max] * biorbd_model[0].nbGeneralizedTorque()) u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(), [tau_max] * biorbd_model[0].nbGeneralizedTorque()) u_init = InitialGuessList() u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque()) u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque()) u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque()) parameters = ParameterList() if use_parameter: def my_target_function(ocp, value, target_value): return value - target_value def my_parameter_function(biorbd_model, value, extra_value): new_gravity = MX.zeros(3, 1) new_gravity[2] = value + extra_value biorbd_model.setGravity(new_gravity) min_g = -10 max_g = -6 target_g = -8 bound_gravity = Bounds(min_g, max_g, interpolation=InterpolationType.CONSTANT) initial_gravity = InitialGuess((min_g + max_g) / 2) parameter_objective_functions = Objective( my_target_function, weight=10, quadratic=True, custom_type=ObjectiveFcn.Parameter, target_value=target_g) parameters.add( "gravity_z", my_parameter_function, initial_gravity, bound_gravity, size=1, penalty_list=parameter_objective_functions, extra_value=1, ) # ------------- # return OptimalControlProgram( biorbd_model[:n_phases], dynamics, ns, final_time[:n_phases], x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, ode_solver=ode_solver, parameters=parameters, )
def prepare_ocp(biorbd_model_path, number_shooting_points, final_time, use_actuators=False, ode_solver=OdeSolver.RK): # --- Options --- # # Model path biorbd_model = biorbd.Model(biorbd_model_path) # Problem parameters if use_actuators: tau_min, tau_max, tau_init = -1, 1, 0 else: tau_min, tau_max, tau_init = -100, 100, 0 # Add objective functions objective_functions = ObjectiveList() objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, weight=100) # Dynamics dynamics = DynamicsTypeList() if use_actuators: dynamics.add(DynamicsType.TORQUE_ACTIVATIONS_DRIVEN) else: dynamics.add(DynamicsType.TORQUE_DRIVEN) # Constraints constraints = ConstraintList() constraints.add(Constraint.ALIGN_MARKERS, node=Node.START, first_marker_idx=0, second_marker_idx=1) constraints.add(Constraint.ALIGN_MARKERS, node=Node.END, first_marker_idx=0, second_marker_idx=2) # Path constraint x_bounds = BoundsList() x_bounds.add(QAndQDotBounds(biorbd_model)) x_bounds[0][3:6, [0, -1]] = 0 x_bounds[0][2, [0, -1]] = [0, 1.57] # Initial guess x_init = InitialGuessList() x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot())) # Define control path constraint u_bounds = BoundsList() u_bounds.add([[tau_min] * biorbd_model.nbGeneralizedTorque(), [tau_max] * biorbd_model.nbGeneralizedTorque()]) u_init = InitialGuessList() u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque()) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, number_shooting_points, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, ode_solver=ode_solver, )
def prepare_ocp( biorbd_model_path: str, phase_time: float, n_shooting: int, use_actuators: bool = False, ode_solver: OdeSolver = OdeSolver.RK4(), objective_name: str = "MINIMIZE_PREDICTED_COM_HEIGHT", com_constraints: bool = False, ) -> OptimalControlProgram: """ Prepare the ocp Parameters ---------- biorbd_model_path: str The path to the bioMod file phase_time: float The time at the final node n_shooting: int The number of shooting points use_actuators: bool If torque or torque activation should be used for the dynamics ode_solver: OdeSolver The ode solver to use objective_name: str The objective function to run ('MINIMIZE_PREDICTED_COM_HEIGHT', 'MINIMIZE_COM_POSITION' or 'MINIMIZE_COM_VELOCITY') com_constraints: bool If a constraint on the COM should be applied Returns ------- The OptimalControlProgram ready to be solved """ biorbd_model = biorbd.Model(biorbd_model_path) if use_actuators: tau_min, tau_max, tau_init = -1, 1, 0 else: tau_min, tau_max, tau_init = -500, 500, 0 dof_mapping = BiMappingList() dof_mapping.add("tau", [None, None, None, 0], [3]) # Add objective functions objective_functions = ObjectiveList() if objective_name == "MINIMIZE_PREDICTED_COM_HEIGHT": objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_PREDICTED_COM_HEIGHT, weight=-1) elif objective_name == "MINIMIZE_COM_POSITION": objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_COM_POSITION, node=Node.ALL, axes=Axis.Z, weight=-1) elif objective_name == "MINIMIZE_COM_VELOCITY": objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_COM_VELOCITY, node=Node.ALL, axes=Axis.Z, weight=-1) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=1 / 100) # Dynamics dynamics = DynamicsList() if use_actuators: dynamics.add(DynamicsFcn.TORQUE_ACTIVATIONS_DRIVEN, with_contact=True) else: dynamics.add(DynamicsFcn.TORQUE_DRIVEN, with_contact=True) # Constraints constraints = ConstraintList() if com_constraints: constraints.add( ConstraintFcn.TRACK_COM_VELOCITY, node=Node.ALL, min_bound=np.array([-100, -100, -100]), max_bound=np.array([100, 100, 100]), ) constraints.add( ConstraintFcn.TRACK_COM_POSITION, node=Node.ALL, min_bound=np.array([-1, -1, -1]), max_bound=np.array([1, 1, 1]), ) # Path constraint n_q = biorbd_model.nbQ() n_qdot = n_q pose_at_first_node = [0, 0, -0.5, 0.5] # Initialize x_bounds x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model)) x_bounds[0][:, 0] = pose_at_first_node + [0] * n_qdot # Initial guess x_init = InitialGuessList() x_init.add(pose_at_first_node + [0] * n_qdot) # Define control path constraint u_bounds = BoundsList() u_bounds.add([tau_min] * len(dof_mapping["tau"].to_first), [tau_max] * len(dof_mapping["tau"].to_first)) u_init = InitialGuessList() u_init.add([tau_init] * len(dof_mapping["tau"].to_first)) return OptimalControlProgram( biorbd_model, dynamics, n_shooting, phase_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints=constraints, variable_mappings=dof_mapping, ode_solver=ode_solver, )
def prepare_ocp( biorbd_model_path, final_time, number_shooting_points, initialize_near_solution, ode_solver=OdeSolver.RK, constr=True, use_SX=False, ): # --- Options --- # # Model path biorbd_model = biorbd.Model(biorbd_model_path) # Problem parameters tau_min, tau_max, tau_init = -100, 100, 0 # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100) # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN) # Constraints if constr is True: constraints = ConstraintList() constraints.add(ConstraintFcn.ALIGN_MARKERS, node=Node.START, first_marker_idx=0, second_marker_idx=4) constraints.add(ConstraintFcn.ALIGN_MARKERS, node=Node.END, first_marker_idx=0, second_marker_idx=5) constraints.add( ConstraintFcn.ALIGN_MARKER_WITH_SEGMENT_AXIS, node=Node.ALL, marker_idx=1, segment_idx=2, axis=(Axe.X) ) else: constraints = ConstraintList() # Path constraint x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model)) for i in range(1, 8): if i != 3: x_bounds[0][i, [0, -1]] = 0 x_bounds[0][2, -1] = 1.57 # Initial guess x_init = InitialGuessList() x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot())) if initialize_near_solution: for i in range(2): x_init[0].init[i] = 1.5 for i in range(4, 6): x_init[0].init[i] = 0.7 for i in range(6, 8): x_init[0].init[i] = 0.6 # Define control path constraint u_bounds = BoundsList() u_bounds.add([tau_min] * biorbd_model.nbGeneralizedTorque(), [tau_max] * biorbd_model.nbGeneralizedTorque()) u_init = InitialGuessList() u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque()) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, number_shooting_points, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, ode_solver=ode_solver, use_SX=use_SX, )
def prepare_ocp_phase_transitions( biorbd_model_path: str, with_constraints: bool, with_mayer: bool, with_lagrange: bool, ) -> OptimalControlProgram: """ Parameters ---------- biorbd_model_path: str The path to the bioMod Returns ------- The ocp ready to be solved """ # Model path biorbd_model = ( biorbd.Model(biorbd_model_path), biorbd.Model(biorbd_model_path), biorbd.Model(biorbd_model_path), biorbd.Model(biorbd_model_path), ) # Problem parameters n_shooting = (20, 20, 20, 20) final_time = (2, 5, 4, 2) tau_min, tau_max, tau_init = -100, 100, 0 # Add objective functions objective_functions = ObjectiveList() if with_lagrange: objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100, phase=0) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100, phase=1) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100, phase=2) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100, phase=3) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_COM_VELOCITY, weight=0, phase=3, axis=None) if with_mayer: objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_TIME) objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_COM_POSITION, phase=0, node=1) objective_functions.add( minimize_difference, custom_type=ObjectiveFcn.Mayer, node=Node.TRANSITION, weight=100, phase=1, get_all_nodes_at_once=True, quadratic=True, ) # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN) dynamics.add(DynamicsFcn.TORQUE_DRIVEN) dynamics.add(DynamicsFcn.TORQUE_DRIVEN) dynamics.add(DynamicsFcn.TORQUE_DRIVEN) # Constraints constraints = ConstraintList() if with_constraints: constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.START, first_marker="m0", second_marker="m1", phase=0) constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=2, first_marker="m0", second_marker="m1", phase=0) constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m2", phase=0) constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m1", phase=1) constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m2", phase=2) constraints.add(custom_func_track_markers, node=Node.ALL, first_marker="m0", second_marker="m1", phase=3) # Path constraint x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0])) x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0])) x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0])) x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0])) x_bounds[0][[1, 3, 4, 5], 0] = 0 x_bounds[-1][[1, 3, 4, 5], -1] = 0 x_bounds[0][2, 0] = 0.0 x_bounds[2][2, [0, -1]] = [0.0, 1.57] # Initial guess x_init = InitialGuessList() x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot())) x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot())) x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot())) x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot())) # Define control path constraint u_bounds = BoundsList() u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(), [tau_max] * biorbd_model[0].nbGeneralizedTorque()) u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(), [tau_max] * biorbd_model[0].nbGeneralizedTorque()) u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(), [tau_max] * biorbd_model[0].nbGeneralizedTorque()) u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(), [tau_max] * biorbd_model[0].nbGeneralizedTorque()) u_init = InitialGuessList() u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque()) u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque()) u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque()) u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque()) # Define phase transitions phase_transitions = PhaseTransitionList() phase_transitions.add(PhaseTransitionFcn.IMPACT, phase_pre_idx=1) phase_transitions.add(PhaseTransitionFcn.CONTINUOUS, phase_pre_idx=2, idx_1=1, idx_2=3) phase_transitions.add(PhaseTransitionFcn.CYCLIC) return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, phase_transitions=phase_transitions, )