def test_track_marker_2D_pendulum(ode_solver): # Load muscle_activations_contact_tracker from bioptim.examples.torque_driven_ocp import track_markers_2D_pendulum as ocp_module bioptim_folder = os.path.dirname(ocp_module.__file__) ode_solver = ode_solver() # Define the problem model_path = bioptim_folder + "/models/pendulum.bioMod" biorbd_model = biorbd.Model(model_path) final_time = 2 n_shooting = 30 # Generate data to fit np.random.seed(42) markers_ref = np.random.rand(3, 2, n_shooting + 1) tau_ref = np.random.rand(2, n_shooting) if isinstance(ode_solver, OdeSolver.IRK): tau_ref = tau_ref * 5 ocp = ocp_module.prepare_ocp(biorbd_model, final_time, n_shooting, markers_ref, tau_ref, ode_solver=ode_solver) sol = ocp.solve() # Check constraints g = np.array(sol.constraints) np.testing.assert_equal(g.shape, (n_shooting * 4, 1)) np.testing.assert_almost_equal(g, np.zeros((n_shooting * 4, 1))) # Check some of the results q, qdot, tau = sol.states["q"], sol.states["qdot"], sol.controls["tau"] if isinstance(ode_solver, OdeSolver.IRK): # Check objective function value f = np.array(sol.cost) np.testing.assert_equal(f.shape, (1, 1)) np.testing.assert_almost_equal(f[0, 0], 290.6751231) # initial and final position np.testing.assert_almost_equal(q[:, 0], np.array((0, 0))) np.testing.assert_almost_equal(q[:, -1], np.array((0.64142484, 2.85371719))) # initial and final velocities np.testing.assert_almost_equal(qdot[:, 0], np.array((0, 0))) np.testing.assert_almost_equal(qdot[:, -1], np.array((3.46921861, 3.24168308))) # initial and final controls np.testing.assert_almost_equal(tau[:, 0], np.array((9.11770196, -13.83677175))) np.testing.assert_almost_equal(tau[:, -2], np.array((1.16836132, 4.77230548))) elif isinstance(ode_solver, OdeSolver.RK8): pass else: # Check objective function value f = np.array(sol.cost) np.testing.assert_equal(f.shape, (1, 1)) np.testing.assert_almost_equal(f[0, 0], 281.8560713312711) # initial and final position np.testing.assert_almost_equal(q[:, 0], np.array((0, 0))) np.testing.assert_almost_equal(q[:, -1], np.array((0.8367364, 3.37533055))) # initial and final velocities np.testing.assert_almost_equal(qdot[:, 0], np.array((0, 0))) np.testing.assert_almost_equal(qdot[:, -1], np.array((3.2688391, 3.88242643))) # initial and final controls np.testing.assert_almost_equal(tau[:, 0], np.array((6.93890241, -12.76433504))) np.testing.assert_almost_equal(tau[:, -2], np.array((0.13156876, 0.93749913))) # save and load TestUtils.save_and_load(sol, ocp, False) # simulate TestUtils.simulate(sol)
def prepare_ocp( biorbd_model_path: str, final_time: float, n_shooting: int, ode_solver: OdeSolver = OdeSolver.RK4(), weight: float = 1, ) -> OptimalControlProgram: """ Prepare the optimal control program Parameters ---------- biorbd_model_path: str The path to the bioMod final_time: float The initial guess for the final time n_shooting: int The number of shooting points ode_solver: OdeSolver The ode solver to use weight: float The weighting of the minimize time objective function Returns ------- The OptimalControlProgram ready to be solved """ biorbd_model = biorbd.Model(biorbd_model_path) # Add objective functions objective_functions = ObjectiveList() # A weight of -1 will maximize time objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TIME, weight=weight) # Dynamics dynamics = DynamicsList() expand = False if isinstance(ode_solver, OdeSolver.IRK) else True dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand) # Path constraint n_q = biorbd_model.nbQ() n_qdot = biorbd_model.nbQdot() x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model)) x_bounds[0][:, [0, -1]] = 0 x_bounds[0][n_q - 1, -1] = 3.14 # Initial guess x_init = InitialGuessList() x_init.add([0] * (n_q + n_qdot)) # Define control path constraint n_tau = biorbd_model.nbGeneralizedTorque() tau_min, tau_max, tau_init = -100, 100, 0 u_bounds = BoundsList() u_bounds.add([tau_min] * n_tau, [tau_max] * n_tau) u_bounds[0][n_tau - 1, :] = 0 u_init = InitialGuessList() u_init.add([tau_init] * n_tau) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, ode_solver=ode_solver, )
def prepare_ocp( final_time: list, time_min: list, time_max: list, n_shooting: list, biorbd_model_path: str = "cube.bioMod", ode_solver: OdeSolver = OdeSolver.RK4(), ) -> OptimalControlProgram: """ Prepare the optimal control program. This example can be called as a normal single phase (all list len equals to 1) or as a three phases (all list len equals to 3) Parameters ---------- final_time: list The initial guess for the final time of each phase time_min: list The minimal time for each phase time_max: list The maximal time for each phase n_shooting: list The number of shooting points for each phase biorbd_model_path: str The path to the bioMod ode_solver: OdeSolver The ode solver to use Returns ------- The multiphase OptimalControlProgram ready to be solved """ # --- Options --- # n_phases = len(n_shooting) if n_phases != 1 and n_phases != 3: raise RuntimeError("Number of phases must be 1 to 3") # 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_CONTROL, key="tau", weight=100, phase=0) if n_phases == 3: objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, phase=1) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, phase=2) # Dynamics dynamics = DynamicsList() expand = False if isinstance(ode_solver, OdeSolver.IRK) else True dynamics.add(DynamicsFcn.TORQUE_DRIVEN, phase=0, expand=expand) if n_phases == 3: dynamics.add(DynamicsFcn.TORQUE_DRIVEN, phase=1, expand=expand) dynamics.add(DynamicsFcn.TORQUE_DRIVEN, phase=2, expand=expand) # 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.TIME_CONSTRAINT, node=Node.END, min_bound=time_min[0], max_bound=time_max[0], phase=0) if n_phases == 3: constraints.add( ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m1", phase=1 ) constraints.add( ConstraintFcn.TIME_CONSTRAINT, node=Node.END, min_bound=time_min[1], max_bound=time_max[1], 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, min_bound=time_min[2], max_bound=time_max[2], phase=2 ) # Path constraint x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0])) # Phase 0 if n_phases == 3: 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 if n_phases == 3: 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())) if n_phases == 3: 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()) if n_phases == 3: 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()) if n_phases == 3: u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque()) u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque()) # ------------- # return OptimalControlProgram( biorbd_model[:n_phases], dynamics, n_shooting, final_time[:n_phases], x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, ode_solver=ode_solver, )
def test_torque_derivative_driven_implicit(with_contact, cx): # Prepare the program nlp = NonLinearProgram() nlp.model = biorbd.Model( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod" ) nlp.ns = 5 nlp.cx = cx nlp.phase_idx = 0 nlp.x_bounds = np.zeros((nlp.model.nbQ() * 4, 1)) nlp.u_bounds = np.zeros((nlp.model.nbQ(), 2)) ocp = OptimalControlProgram(nlp) nlp.control_type = ControlType.CONSTANT NonLinearProgram.add( ocp, "dynamics_type", Dynamics(DynamicsFcn.TORQUE_DERIVATIVE_DRIVEN, with_contact=with_contact, implicit_dynamics=True), False, ) # Prepare the dynamics ConfigureProblem.initialize(ocp, nlp) # Test the results np.random.seed(42) states = np.random.rand(nlp.states.shape, nlp.ns) controls = np.random.rand(nlp.controls.shape, nlp.ns) params = np.random.rand(nlp.parameters.shape, nlp.ns) x_out = np.array(nlp.dynamics_func(states, controls, params)) if with_contact: contact_out = np.array(nlp.contact_forces_func(states, controls, params)) np.testing.assert_almost_equal( x_out[:, 0], [ 0.6118529, 0.785176, 0.6075449, 0.8083973, 0.3886773, 0.5426961, 0.7722448, 0.7290072, 0.8631034, 0.3251833, 0.1195942, 0.4937956, 0.0314292, 0.2492922, 0.2897515, 0.8714606, ], ) np.testing.assert_almost_equal(contact_out[:, 0], [-2.444071, 128.8816865, 2.7245124]) else: np.testing.assert_almost_equal( x_out[:, 0], [ 0.6118529, 0.785176, 0.6075449, 0.8083973, 0.3886773, 0.5426961, 0.7722448, 0.7290072, 0.8631034, 0.3251833, 0.1195942, 0.4937956, 0.0314292, 0.2492922, 0.2897515, 0.8714606, ], )
def test_torque_activation_driven(with_contact, with_external_force, cx): # Prepare the program nlp = NonLinearProgram() nlp.model = biorbd.Model( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod" ) nlp.ns = 5 nlp.cx = cx nlp.x_bounds = np.zeros((nlp.model.nbQ() * 2, 1)) nlp.u_bounds = np.zeros((nlp.model.nbQ(), 1)) ocp = OptimalControlProgram(nlp) nlp.control_type = ControlType.CONSTANT NonLinearProgram.add( ocp, "dynamics_type", Dynamics(DynamicsFcn.TORQUE_ACTIVATIONS_DRIVEN, with_contact=with_contact), False ) np.random.seed(42) if with_external_force: external_forces = [np.random.rand(6, nlp.model.nbSegment(), nlp.ns)] nlp.external_forces = BiorbdInterface.convert_array_to_external_forces(external_forces)[0] # Prepare the dynamics ConfigureProblem.initialize(ocp, nlp) # Test the results states = np.random.rand(nlp.states.shape, nlp.ns) controls = np.random.rand(nlp.controls.shape, nlp.ns) params = np.random.rand(nlp.parameters.shape, nlp.ns) x_out = np.array(nlp.dynamics_func(states, controls, params)) if with_contact: contact_out = np.array(nlp.contact_forces_func(states, controls, params)) if with_external_force: np.testing.assert_almost_equal( x_out[:, 0], [0.8631, 0.32518, 0.11959, 0.4938, 19.01887, 18.51503, -53.08574, 58.48719], decimal=5, ) np.testing.assert_almost_equal(contact_out[:, 0], [109.8086936, 3790.3932439, -3571.7858574]) else: np.testing.assert_almost_equal( x_out[:, 0], [0.61185289, 0.78517596, 0.60754485, 0.80839735, 0.78455384, -0.16844256, -1.56184114, 1.97658587], decimal=5, ) np.testing.assert_almost_equal(contact_out[:, 0], [-7.88958997, 329.70828173, -263.55516549]) else: if with_external_force: np.testing.assert_almost_equal( x_out[:, 0], [ 8.63103426e-01, 3.25183322e-01, 1.19594246e-01, 4.93795596e-01, 1.73558072e01, -4.69891264e01, 1.81396922e02, 3.61170139e03, ], decimal=5, ) else: np.testing.assert_almost_equal( x_out[:, 0], [ 6.11852895e-01, 7.85175961e-01, 6.07544852e-01, 8.08397348e-01, -2.38262975e01, -5.82033454e01, 1.27439020e02, 3.66531163e03, ], decimal=5, )
def prepare_ocp( biorbd_model_path: str, n_shooting: int, final_time: float, ode_solver: OdeSolver = OdeSolver.RK4() ) -> OptimalControlProgram: """ Prepare the ocp Parameters ---------- biorbd_model_path: str The path to the bioMod file n_shooting: int The number of shooting points final_time: float The time at the final node ode_solver: OdeSolver The ode solver to use Returns ------- The OptimalControlProgram ready to be solved """ biorbd_model = biorbd.Model(biorbd_model_path) # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_MARKERS, marker_index=1, weight=-1) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", node=Node.ALL_SHOOTING, weight=100) # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN) # Path constraint x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model)) # Define control path constraint n_tau = biorbd_model.nbGeneralizedTorque( ) # biorbd_model.nbGeneralizedTorque() tau_min, tau_max, tau_init = -100, 100, 0 u_bounds = BoundsList() u_bounds.add([tau_min] * n_tau, [tau_max] * n_tau) # Initial guesses # TODO put this in a function defined before and explain what it does, and what are the variables x = np.vstack((np.zeros( (biorbd_model.nbQ(), 2)), np.ones((biorbd_model.nbQdot(), 2)))) Arm_init_D = np.zeros((3, 2)) Arm_init_D[1, 0] = 0 Arm_init_D[1, 1] = -np.pi + 0.01 Arm_init_G = np.zeros((3, 2)) Arm_init_G[1, 0] = 0 Arm_init_G[1, 1] = np.pi - 0.01 for i in range(2): Arm_Quat_D = eul2quat(Arm_init_D[:, i]) Arm_Quat_G = eul2quat(Arm_init_G[:, i]) x[6:9, i] = Arm_Quat_D[1:] x[12, i] = Arm_Quat_D[0] x[9:12, i] = Arm_Quat_G[1:] x[13, i] = Arm_Quat_G[0] x_init = InitialGuessList() x_init.add(x, interpolation=InterpolationType.LINEAR) u_init = InitialGuessList() u_init.add([tau_init] * n_tau) return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, ode_solver=ode_solver, )
def prepare_ocp( biorbd_model_path: str, n_shooting: int, final_time: float, initial_guess: InterpolationType = InterpolationType.CONSTANT, ode_solver=OdeSolver.RK4(), ) -> OptimalControlProgram: """ Prepare the program Parameters ---------- biorbd_model_path: str The path of the biorbd model n_shooting: int The number of shooting points final_time: float The time at the final node initial_guess: InterpolationType The type of interpolation to use for the initial guesses ode_solver: OdeSolver The type of ode solver used Returns ------- The ocp ready to be solved """ # --- Options --- # # Model path biorbd_model = biorbd.Model(biorbd_model_path) nq = biorbd_model.nbQ() nqdot = biorbd_model.nbQdot() ntau = biorbd_model.nbGeneralizedTorque() tau_min, tau_max, tau_init = -100, 100, 0 # Add objective functions objective_functions = Objective(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100) # Dynamics expand = False if isinstance(ode_solver, OdeSolver.IRK) else True dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN, expand=expand) # Constraints constraints = ConstraintList() constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.START, first_marker="m0", second_marker="m1") constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m2") # Path constraint and control path constraints x_bounds = QAndQDotBounds(biorbd_model) x_bounds[1:6, [0, -1]] = 0 x_bounds[2, -1] = 1.57 u_bounds = Bounds([tau_min] * ntau, [tau_max] * ntau) # Initial guesses t = None extra_params_x = {} extra_params_u = {} if initial_guess == InterpolationType.CONSTANT: x = [0] * (nq + nqdot) u = [tau_init] * ntau elif initial_guess == InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT: x = np.array([[1.0, 0.0, 0.0, 0, 0, 0], [1.5, 0.0, 0.785, 0, 0, 0], [2.0, 0.0, 1.57, 0, 0, 0]]).T u = np.array([[1.45, 9.81, 2.28], [0, 9.81, 0], [-1.45, 9.81, -2.28]]).T elif initial_guess == InterpolationType.LINEAR: x = np.array([[1.0, 0.0, 0.0, 0, 0, 0], [2.0, 0.0, 1.57, 0, 0, 0]]).T u = np.array([[1.45, 9.81, 2.28], [-1.45, 9.81, -2.28]]).T elif initial_guess == InterpolationType.EACH_FRAME: x = np.random.random((nq + nqdot, n_shooting + 1)) u = np.random.random((ntau, n_shooting)) elif initial_guess == InterpolationType.SPLINE: # Bound spline assume the first and last point are 0 and final respectively t = np.hstack((0, np.sort(np.random.random( (3, )) * final_time), final_time)) x = np.random.random((nq + nqdot, 5)) u = np.random.random((ntau, 5)) elif initial_guess == InterpolationType.CUSTOM: # The custom function refers to the one at the beginning of the file. It emulates a Linear interpolation x = custom_init_func u = custom_init_func extra_params_x = { "my_values": np.random.random((nq + nqdot, 2)), "n_shooting": n_shooting } extra_params_u = { "my_values": np.random.random((ntau, 2)), "n_shooting": n_shooting } else: raise RuntimeError("Initial guess not implemented yet") x_init = InitialGuess(x, t=t, interpolation=initial_guess, **extra_params_x) u_init = InitialGuess(u, t=t, interpolation=initial_guess, **extra_params_u) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, ode_solver=ode_solver, )
def prepare_ocp(biorbd_model_path: str = "cube.bioMod", ode_solver: OdeSolver = OdeSolver.RK4(), long_optim: bool = False) -> OptimalControlProgram: """ Prepare the ocp Parameters ---------- biorbd_model_path: str The path to the bioMod ode_solver: OdeSolver The ode solve to use long_optim: bool If the solver should solve the precise optimization (500 shooting points) or the approximate (50 points) Returns ------- The OptimalControlProgram ready to be solved """ biorbd_model = (biorbd.Model(biorbd_model_path), biorbd.Model(biorbd_model_path), biorbd.Model(biorbd_model_path)) # Problem parameters if long_optim: n_shooting = (100, 300, 100) else: n_shooting = (20, 30, 20) final_time = (2, 5, 4) tau_min, tau_max, tau_init = -100, 100, 0 # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, phase=0) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, phase=1) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, phase=2) objective_functions.add( minimize_difference, custom_type=ObjectiveFcn.Mayer, node=Node.TRANSITION, weight=100, phase=1, quadratic=True, ) # Dynamics dynamics = DynamicsList() expand = False if isinstance(ode_solver, OdeSolver.IRK) else True dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand) dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand) dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand) # 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) # 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])) 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()) return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, ode_solver=ode_solver, )
def prepare_ocp( biorbd_model_path: str, final_time: float, n_shooting: int, marker_velocity_or_displacement: str, marker_in_first_coordinates_system: bool, control_type: ControlType, ode_solver: OdeSolver = OdeSolver.RK4(), ) -> OptimalControlProgram: """ Prepare an ocp that targets some marker velocities, either by finite differences or by jacobian Parameters ---------- biorbd_model_path: str The path to the bioMod file final_time: float The time of the final node n_shooting: int The number of shooting points marker_velocity_or_displacement: str which type of tracking: finite difference ('disp') or by jacobian ('velo') marker_in_first_coordinates_system: bool If the marker to track should be expressed in the global or local reference frame control_type: ControlType The type of controls ode_solver: OdeSolver The ode solver to use Returns ------- The OptimalControlProgram ready to be solved """ biorbd_model = biorbd.Model(biorbd_model_path) # Add objective functions if marker_in_first_coordinates_system: # Marker should follow this segment (0 velocity when compare to this one) coordinates_system_idx = 0 else: # Marker should be static in global reference frame coordinates_system_idx = None objective_functions = ObjectiveList() if marker_velocity_or_displacement == "disp": objective_functions.add( ObjectiveFcn.Lagrange.MINIMIZE_MARKERS, derivative=True, reference_jcs=coordinates_system_idx, marker_index=6, weight=1000, ) elif marker_velocity_or_displacement == "velo": objective_functions.add( ObjectiveFcn.Lagrange.MINIMIZE_MARKERS_VELOCITY, node=Node.ALL, marker_index=6, weight=1000 ) else: raise RuntimeError( f"Wrong choice of marker_velocity_or_displacement, actual value is " f"{marker_velocity_or_displacement}, should be 'velo' or 'disp'." ) # Make sure the segments actually moves (in order to test the relative speed objective) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE, key="qdot", node=Node.ALL, index=[2, 3], weight=-1) # Dynamics dynamics = DynamicsList() expand = False if isinstance(ode_solver, OdeSolver.IRK) else True dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand) # Path constraint nq = biorbd_model.nbQ() x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model)) x_bounds[0].min[nq:, :] = -10 x_bounds[0].max[nq:, :] = 10 # Initial guess x_init = InitialGuessList() x_init.add([1.5, 1.5, 0.0, 0.0, 0.7, 0.7, 0.6, 0.6]) # Define control path constraint tau_min, tau_max, tau_init = -100, 100, 0 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, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, control_type=control_type, ode_solver=ode_solver, )
def prepare_ocp( biorbd_model_path: str, final_time: float, n_shooting: int, time_min: float, time_max: float, ode_solver: OdeSolver = OdeSolver.RK4(), ) -> OptimalControlProgram: """ Prepare the optimal control program Parameters ---------- biorbd_model_path: str The path to the bioMod final_time: float The initial guess for the final time n_shooting: int The number of shooting points time_min: float The minimal time the phase can have time_max: float The maximal time the phase can have ode_solver: OdeSolver The ode solver to use Returns ------- The OptimalControlProgram ready to be solved """ biorbd_model = biorbd.Model(biorbd_model_path) # Add objective functions objective_functions = Objective(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau") # Dynamics expand = False if isinstance(ode_solver, OdeSolver.IRK) else True dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN, expand=expand) # Constraints constraints = Constraint(ConstraintFcn.TIME_CONSTRAINT, node=Node.END, min_bound=time_min, max_bound=time_max) # Path constraint n_q = biorbd_model.nbQ() n_qdot = biorbd_model.nbQdot() x_bounds = QAndQDotBounds(biorbd_model) x_bounds[:, [0, -1]] = 0 x_bounds[n_q - 1, -1] = 3.14 # Initial guess x_init = InitialGuess([0] * (n_q + n_qdot)) # Define control path constraint n_tau = biorbd_model.nbGeneralizedTorque() tau_min, tau_max, tau_init = -100, 100, 0 u_bounds = Bounds([tau_min] * n_tau, [tau_max] * n_tau) u_bounds[n_tau - 1, :] = 0 u_init = InitialGuess([tau_init] * n_tau) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, ode_solver=ode_solver, )
def main(): biorbd_model_path = "models/cart_pendulum.bioMod" biorbd_model = biorbd.Model(biorbd_model_path) solver = Solver.ACADOS() # or Solver.IPOPT() final_time = 5 n_shoot_per_second = 100 window_len = 10 window_duration = 1 / n_shoot_per_second * window_len n_frames_total = final_time * n_shoot_per_second - window_len - 1 x0 = np.array([0, np.pi / 2, 0, 0]) noise_std = 0.05 # STD of noise added to measurements torque_max = 2 # Max torque applied to the model states, markers, markers_noised, controls = generate_data( biorbd_model, final_time, x0, torque_max, n_shoot_per_second * final_time, noise_std, show_plots=False) x_init = np.zeros((biorbd_model.nbQ() * 2, window_len + 1)) u_init = np.zeros((biorbd_model.nbQ(), window_len)) torque_max = 5 # Give a bit of slack on the max torque biorbd_model = biorbd.Model(biorbd_model_path) mhe = prepare_mhe( biorbd_model, window_len=window_len, window_duration=window_duration, max_torque=torque_max, x_init=x_init, u_init=u_init, ) def update_functions(mhe, t, _): def target(i: int): return markers_noised[:, :, i:i + window_len + 1] mhe.update_objectives_target(target=target(t), list_index=0) return t < n_frames_total # True if there are still some frames to reconstruct # Solve the program sol = mhe.solve(update_functions, **get_solver_options(solver)) print("ACADOS with Bioptim") print(f"Window size of MHE : {window_duration} s.") print(f"New measurement every : {1 / n_shoot_per_second} s.") print( f"Average time per iteration of MHE : {sol.solver_time_to_optimize / (n_frames_total - 1)} s." ) print( f"Average real time per iteration of MHE : {sol.real_time_to_optimize / (n_frames_total - 1)} s." ) print( f"Norm of the error on state = {np.linalg.norm(states[:, :n_frames_total] - sol.states['all'])}" ) markers_estimated = states_to_markers(biorbd_model, sol.states["all"]) plt.plot( markers_noised[1, :, :n_frames_total].T, markers_noised[2, :, :n_frames_total].T, "x", label="Noised markers trajectory", ) plt.gca().set_prop_cycle(None) plt.plot(markers[1, :, :n_frames_total].T, markers[2, :, :n_frames_total].T, label="True markers trajectory") plt.gca().set_prop_cycle(None) plt.plot(markers_estimated[1, :, :].T, markers_estimated[2, :, :].T, "o", label="Estimated marker trajectory") plt.legend() plt.figure() plt.plot(sol.states["all"].T, "--", label="States estimate") plt.plot(states[:, :n_frames_total].T, label="State truth") plt.legend() plt.show() sol.animate()
def prepare_ocp(biorbd_model_path, phase_time, n_shooting, muscle_activations_ref, contact_forces_ref, ode_solver=OdeSolver.RK4()): # Model path biorbd_model = biorbd.Model(biorbd_model_path) tau_min, tau_max, tau_init = -500, 500, 0 activation_min, activation_max, activation_init = 0, 1, 0.5 # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.TRACK_CONTROL, key="muscles", target=muscle_activations_ref) objective_functions.add(ObjectiveFcn.Lagrange.TRACK_CONTACT_FORCES, target=contact_forces_ref) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE, key="qdot", weight=0.001) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE, key="q", weight=0.001) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="muscles", weight=0.001) # objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="torque", weight=0.001) # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.MUSCLE_DRIVEN, with_residual_torque=True, with_contact=True) # 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] * biorbd_model.nbGeneralizedTorque() + [activation_min] * biorbd_model.nbMuscleTotal(), [tau_max] * biorbd_model.nbGeneralizedTorque() + [activation_max] * biorbd_model.nbMuscleTotal(), ) u_init = InitialGuessList() u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque() + [activation_init] * biorbd_model.nbMuscleTotal()) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, n_shooting, phase_time, x_init, u_init, x_bounds, u_bounds, objective_functions=objective_functions, ode_solver=ode_solver, )
def prepare_ocp( biorbd_model_path: str, final_time: float, n_shooting: int, ode_solver: OdeSolver = OdeSolver.RK4() ) -> OptimalControlProgram: """ Prepare the ocp Parameters ---------- biorbd_model_path: str The path to the model final_time: float The time of the final node n_shooting: int The number of shooting points ode_solver: The ode solver to use Returns ------- The OptimalControlProgram ready to be solved """ biorbd_model = biorbd.Model(biorbd_model_path) # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100) # Dynamics dynamics = DynamicsList() expand = False if isinstance(ode_solver, OdeSolver.IRK) else True dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand) # Constraints constraints = ConstraintList() constraints.add(ConstraintFcn.TRACK_SEGMENT_WITH_CUSTOM_RT, node=Node.ALL, segment="seg_rt", rt=0) # Path constraint nq = biorbd_model.nbQ() 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 tau_min, tau_max, tau_init = -100, 100, 0 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, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, ode_solver=ode_solver, )
def prepare_ocp( biorbd_model_path: str = "models/cube.bioMod", ode_solver: OdeSolver = OdeSolver.RK4() ) -> OptimalControlProgram: """ Parameters ---------- biorbd_model_path: str The path to the bioMod ode_solver: OdeSolver The type of ode solver used 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() objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, phase=0) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, phase=1) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, phase=2) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, phase=3) # Dynamics dynamics = DynamicsList() expand = False if isinstance(ode_solver, OdeSolver.IRK) else True dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand) dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand) dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand) dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand) # 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.SUPERIMPOSE_MARKERS, node=Node.END, 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()) """ By default, all phase transitions (here phase 0 to phase 1, phase 1 to phase 2 and phase 2 to phase 3) are continuous. In the event that one (or more) phase transition(s) is desired to be discontinuous, as for example IMPACT or CUSTOM can be used as below. "phase_pre_idx" corresponds to the index of the phase preceding the transition. IMPACT will cause an impact related discontinuity when defining one or more contact points in the model. CUSTOM will allow to call the custom function previously presented in order to have its own phase transition. Finally, if you want a phase transition (continuous or not) between the last and the first phase (cyclicity) you can use the dedicated PhaseTransitionFcn.Cyclic or use a continuous set at the last phase_pre_idx. If for some reason, you don't want the phase transition to be hard constraint, you can specify a weight higher than zero. It will thereafter be treated as a Mayer objective function with the specified weight. """ phase_transitions = PhaseTransitionList() phase_transitions.add(PhaseTransitionFcn.CONTINUOUS, phase_pre_idx=0, states_mapping=BiMapping(range(3), range(3))) phase_transitions.add(PhaseTransitionFcn.IMPACT, phase_pre_idx=1) phase_transitions.add(custom_phase_transition, phase_pre_idx=2, coef=0.5) 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, ode_solver=ode_solver, phase_transitions=phase_transitions, )
import biorbd_casadi as biorbd import numpy as np m = biorbd.Model("../models/jumper2contacts.bioMod") q0 = np.array([0, 0, -0.5336, 0, 1.4, 0, 1.4, 0.8, -0.9, 0.47, 0.8, -0.9, 0.47]) q1 = np.array([-0.12, -0.23, -1.10, 0, 1.85, 0, 1.85, 2.06, -1.67, 0.55, 2.06, -1.67, 0.55]) q_mat = [q0, q1] torque_act = np.array([0, 0, 0, 0, 1, 0, 1, -1, 1, -1, -1, 1, -1]) qdot_null = np.zeros((13,)) qdot0 = np.array( [ -0.69722964, -1.33958085, -1.8479633, 0.0, -0.64892519, -0.0, -0.64892519, 6.15902598, -5.95116095, 1.63984244, 6.15902598, -5.95116095, 1.63984244, ] )
def prepare_ocp(biorbd_model_path, phase_time, n_shooting, min_bound, ode_solver=OdeSolver.RK4()): biorbd_model = biorbd.Model(biorbd_model_path) torque_min, torque_max, torque_init = -500, 500, 0 activation_min, activation_max, activation_init = 0, 1, 0.5 dof_mapping = BiMappingList() dof_mapping.add("tau", [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.MUSCLE_DRIVEN, with_excitations=True, with_torque=True, with_contact=True) # Constraints constraints = ConstraintList() constraints.add( ConstraintFcn.TRACK_CONTACT_FORCES, min_bound=min_bound, max_bound=np.inf, node=Node.ALL_SHOOTING, contact_index=1, ) constraints.add( ConstraintFcn.TRACK_CONTACT_FORCES, min_bound=min_bound, max_bound=np.inf, node=Node.ALL_SHOOTING, contact_index=2, ) # Path constraint n_q = biorbd_model.nbQ() n_qdot = n_q n_mus = biorbd_model.nbMuscleTotal() 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].concatenate( Bounds([activation_min] * n_mus, [activation_max] * n_mus)) x_bounds[0][:, 0] = pose_at_first_node + [0] * n_qdot + [0.5] * n_mus # Initial guess x_init = InitialGuessList() x_init.add(pose_at_first_node + [0] * n_qdot + [0.5] * n_mus) # Define control path constraint u_bounds = BoundsList() u_bounds.add( [torque_min] * len(dof_mapping["tau"].to_first) + [activation_min] * biorbd_model.nbMuscleTotal(), [torque_max] * len(dof_mapping["tau"].to_first) + [activation_max] * biorbd_model.nbMuscleTotal(), ) u_init = InitialGuessList() u_init.add([torque_init] * len(dof_mapping["tau"].to_first) + [activation_init] * biorbd_model.nbMuscleTotal()) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, n_shooting, phase_time, x_init, u_init, x_bounds, u_bounds, objective_functions=objective_functions, constraints=constraints, variable_mappings=dof_mapping, ode_solver=ode_solver, )
def partial_ocp_parameters(n_phases): if n_phases != 1 and n_phases != 3: raise RuntimeError("n_phases should be 1 or 3") biorbd_model_path = TestUtils.bioptim_folder( ) + "/examples/optimal_time_ocp/models/cube.bioMod" biorbd_model = biorbd.Model(biorbd_model_path), biorbd.Model( biorbd_model_path), biorbd.Model(biorbd_model_path) n_shooting = (2, 2, 2) final_time = (2, 5, 4) time_min = [1, 3, 0.1] time_max = [2, 4, 0.8] tau_min, tau_max, tau_init = -100, 100, 0 dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN) if n_phases > 1: dynamics.add(DynamicsFcn.TORQUE_DRIVEN) dynamics.add(DynamicsFcn.TORQUE_DRIVEN) x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0])) if n_phases > 1: x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0])) x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0])) for bounds in x_bounds: for i in [1, 3, 4, 5]: bounds.min[i, [0, -1]] = 0 bounds.max[i, [0, -1]] = 0 x_bounds[0].min[2, 0] = 0.0 x_bounds[0].max[2, 0] = 0.0 if n_phases > 1: x_bounds[2].min[2, [0, -1]] = [0.0, 1.57] x_bounds[2].max[2, [0, -1]] = [0.0, 1.57] x_init = InitialGuessList() x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot())) if n_phases > 1: x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot())) x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot())) u_bounds = BoundsList() u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(), [tau_max] * biorbd_model[0].nbGeneralizedTorque()) if n_phases > 1: 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()) if n_phases > 1: u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque()) u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque()) return ( biorbd_model[:n_phases], n_shooting[:n_phases], final_time[:n_phases], time_min[:n_phases], time_max[:n_phases], tau_min, tau_max, tau_init, dynamics, x_bounds, x_init, u_bounds, u_init, )
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_CONTROL, key="tau", weight=100) # Dynamics expand = False if isinstance(ode_solver, OdeSolver.IRK) else True dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN, expand=expand) # Constraints constraints = ConstraintList() constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.MID, first_marker="m0", second_marker="m2") constraints.add(ConstraintFcn.TRACK_STATE, key="q", node=Node.MID, index=2) constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m1") # 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( biorbd_model_path: str = "models/double_pendulum.bioMod", biorbd_model_path_withTranslations: str = "models/double_pendulum_with_translations.bioMod", ) -> OptimalControlProgram: biorbd_model = (biorbd.Model(biorbd_model_path), biorbd.Model(biorbd_model_path_withTranslations)) # Problem parameters n_shooting = (40, 40) final_time = (1.5, 2.5) tau_min, tau_max, tau_init = -200, 200, 0 # Mapping tau_mappings = BiMappingList() tau_mappings.add("tau", [None, 0], [1], phase=0) tau_mappings.add("tau", [None, None, None, 0], [3], phase=1) # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=1, phase=0) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=1, phase=1) objective_functions.add( ObjectiveFcn.Mayer.MINIMIZE_COM_POSITION, node=Node.END, weight=-1000, axes=Axis.Z, phase=1, quadratic=False ) objective_functions.add( ObjectiveFcn.Mayer.MINIMIZE_STATE, key="q", index=2, node=Node.END, weight=-100, phase=1, quadratic=False ) # Constraints constraints = ConstraintList() constraints.add(ConstraintFcn.TIME_CONSTRAINT, node=Node.END, min_bound=0.3, max_bound=3, phase=0) constraints.add(ConstraintFcn.TIME_CONSTRAINT, node=Node.END, min_bound=0.3, max_bound=3, phase=1) # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN, with_contact=False) dynamics.add(DynamicsFcn.TORQUE_DRIVEN, with_contact=False) # Path constraint x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0])) x_bounds.add(bounds=QAndQDotBounds(biorbd_model[1])) # Phase 0 x_bounds[0][1, 0] = 0 x_bounds[0][0, 0] = 3.14 x_bounds[0].min[0, -1] = 2 * 3.14 # Phase 1 x_bounds[1][[0, 1, 4, 5], 0] = 0 x_bounds[1].min[2, -1] = 3 * 3.14 # Initial guess x_init = InitialGuessList() x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot())) x_init.add([1] * (biorbd_model[1].nbQ() + biorbd_model[1].nbQdot())) # Define control path constraint u_bounds = BoundsList() u_bounds.add([tau_min] * len(tau_mappings[0]["tau"].to_first), [tau_max] * len(tau_mappings[0]["tau"].to_first)) u_bounds.add([tau_min] * len(tau_mappings[1]["tau"].to_first), [tau_max] * len(tau_mappings[1]["tau"].to_first)) # Control initial guess u_init = InitialGuessList() u_init.add([tau_init] * len(tau_mappings[0]["tau"].to_first)) u_init.add([tau_init] * len(tau_mappings[1]["tau"].to_first)) phase_transitions = PhaseTransitionList() phase_transitions.add( PhaseTransitionFcn.CONTINUOUS, phase_pre_idx=0, states_mapping=BiMapping([0, 1, 2, 3], [2, 3, 6, 7]) ) return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init=x_init, u_init=u_init, x_bounds=x_bounds, u_bounds=u_bounds, objective_functions=objective_functions, constraints=constraints, variable_mappings=tau_mappings, phase_transitions=phase_transitions, )
def prepare_ocp( biorbd_model_path: str, final_time: float, n_shooting: int, n_threads: int, use_sx: bool = False, 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 n_threads: int The number of threads to use while using multithreading ode_solver: OdeSolver The type of ode solver used use_sx: bool If the program should be constructed using SX instead of MX (longer to create the CasADi graph, faster to solve) Returns ------- The ocp ready to be solved """ biorbd_model = biorbd.Model(biorbd_model_path) # Add objective functions objective_functions = Objective(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", derivative=True) # Dynamics expand = False if isinstance(ode_solver, OdeSolver.IRK) else True dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN, expand=expand) # Path constraint x_bounds = QAndQDotBounds(biorbd_model) x_bounds[:, [0, -1]] = 0 x_bounds[1, -1] = 3.14 # Initial guess n_q = biorbd_model.nbQ() n_qdot = biorbd_model.nbQdot() x_init = InitialGuess([0] * (n_q + n_qdot)) # Define control path constraint n_tau = biorbd_model.nbGeneralizedTorque() tau_min, tau_max, tau_init = -100, 100, 0 u_bounds = Bounds([tau_min] * n_tau, [tau_max] * n_tau) u_bounds[n_tau - 1, :] = 0 u_init = InitialGuess([tau_init] * n_tau) return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions=objective_functions, n_threads=n_threads, use_sx=use_sx, ode_solver=ode_solver, )
def test_torque_derivative_driven(with_contact, with_external_force, cx): # Prepare the program nlp = NonLinearProgram() nlp.model = biorbd.Model( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod" ) nlp.ns = 5 nlp.cx = cx nlp.x_bounds = np.zeros((nlp.model.nbQ() * 3, 1)) nlp.u_bounds = np.zeros((nlp.model.nbQ(), 1)) ocp = OptimalControlProgram(nlp) nlp.control_type = ControlType.CONSTANT nlp.phase_idx = 0 NonLinearProgram.add( ocp, "dynamics_type", Dynamics(DynamicsFcn.TORQUE_DERIVATIVE_DRIVEN, with_contact=with_contact), False ) np.random.seed(42) if with_external_force: external_forces = [np.random.rand(6, nlp.model.nbSegment(), nlp.ns)] nlp.external_forces = BiorbdInterface.convert_array_to_external_forces(external_forces)[0] # Prepare the dynamics ConfigureProblem.initialize(ocp, nlp) # Test the results states = np.random.rand(nlp.states.shape, nlp.ns) controls = np.random.rand(nlp.controls.shape, nlp.ns) params = np.random.rand(nlp.parameters.shape, nlp.ns) x_out = np.array(nlp.dynamics_func(states, controls, params)) if with_contact: contact_out = np.array(nlp.contact_forces_func(states, controls, params)) if with_external_force: np.testing.assert_almost_equal( x_out[:, 0], [ 0.8631034, 0.3251833, 0.1195942, 0.4937956, -7.7700092, -7.5782306, 21.7073786, -16.3059315, 0.8074402, 0.4271078, 0.417411, 0.3232029, ], ) np.testing.assert_almost_equal(contact_out[:, 0], [-47.8131136, 111.1726516, -24.4449121]) else: np.testing.assert_almost_equal( x_out[:, 0], [ 0.61185289, 0.78517596, 0.60754485, 0.80839735, -0.32149054, -0.19121314, 0.65071636, -0.23597164, 0.38867729, 0.54269608, 0.77224477, 0.72900717, ], ) np.testing.assert_almost_equal(contact_out[:, 0], [-2.444071, 128.8816865, 2.7245124]) else: if with_external_force: np.testing.assert_almost_equal( x_out[:, 0], [ 0.86310343, 0.32518332, 0.11959425, 0.4937956, 0.30731739, -9.97912778, 1.15263778, 36.02430956, 0.80744016, 0.42710779, 0.417411, 0.32320293, ], ) else: np.testing.assert_almost_equal( x_out[:, 0], [ 0.61185289, 0.78517596, 0.60754485, 0.80839735, -0.30241366, -10.38503791, 1.60445173, 35.80238642, 0.38867729, 0.54269608, 0.77224477, 0.72900717, ], )
def prepare_ocp( biorbd_model_path: str, final_time: float, n_shooting: int, initialize_near_solution: bool, ode_solver: OdeSolver = OdeSolver.RK4(), constr: bool = True, use_sx: bool = False, ) -> OptimalControlProgram: """ Prepare the ocp Parameters ---------- biorbd_model_path: str The path to the bioMod file final_time: float The time at the final node n_shooting: int The number of shooting points initialize_near_solution: bool If the initial guess should be almost the solution (this is merely to reduce the time of the tests) ode_solver: OdeSolver The ode solver to use constr: bool If the constraint should be applied (this is merely to reduce the time of the tests) use_sx: bool If SX CasADi variables should be used Returns ------- The OptimalControlProgram ready to be solved """ biorbd_model = biorbd.Model(biorbd_model_path) # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, multi_thread=False) # Dynamics dynamics = DynamicsList() expand = False if isinstance(ode_solver, OdeSolver.IRK) else True dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand) # Constraints if constr: constraints = ConstraintList() constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.START, first_marker="m0", second_marker="m4") 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, marker="m1", segment="seg_rt", axis=Axis.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 tau_min, tau_max, tau_init = -100, 100, 0 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, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, ode_solver=ode_solver, use_sx=use_sx, )
def test_torque_derivative_driven_implicit_soft_contacts(with_contact, cx, implicit_contact): # Prepare the program nlp = NonLinearProgram() nlp.model = biorbd.Model( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod" ) nlp.ns = 5 nlp.cx = cx nlp.x_bounds = np.zeros((nlp.model.nbQ() * (2 + 3), 1)) nlp.u_bounds = np.zeros((nlp.model.nbQ() * 4, 1)) ocp = OptimalControlProgram(nlp) nlp.control_type = ControlType.CONSTANT NonLinearProgram.add( ocp, "dynamics_type", Dynamics( DynamicsFcn.TORQUE_DERIVATIVE_DRIVEN, with_contact=with_contact, implicit_soft_contacts=implicit_contact ), False, ) # Prepare the dynamics ConfigureProblem.initialize(ocp, nlp) # Test the results np.random.seed(42) states = np.random.rand(nlp.states.shape, nlp.ns) controls = np.random.rand(nlp.controls.shape, nlp.ns) params = np.random.rand(nlp.parameters.shape, nlp.ns) x_out = np.array(nlp.dynamics_func(states, controls, params)) if with_contact: contact_out = np.array(nlp.contact_forces_func(states, controls, params)) np.testing.assert_almost_equal( x_out[:, 0], [ 0.6118529, 0.785176, 0.6075449, 0.8083973, -0.3214905, -0.1912131, 0.6507164, -0.2359716, 0.3886773, 0.5426961, 0.7722448, 0.7290072, ], ) np.testing.assert_almost_equal(contact_out[:, 0], [-2.444071, 128.8816865, 2.7245124]) else: np.testing.assert_almost_equal( x_out[:, 0], [ 0.6118529, 0.785176, 0.6075449, 0.8083973, -0.3024137, -10.3850379, 1.6044517, 35.8023864, 0.3886773, 0.5426961, 0.7722448, 0.7290072, ], )
def prepare_ocp(biorbd_model_path, final_time, n_shooting, x_warm=None, use_sx=False, n_threads=1): # --- Options --- # # Model path biorbd_model = biorbd.Model(biorbd_model_path) tau_min, tau_max, tau_init = -50, 50, 0 muscle_min, muscle_max, muscle_init = 0, 1, 0.5 # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE, key="q", weight=10, multi_thread=False) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE, key="qdot", weight=10, multi_thread=False) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=10, multi_thread=False) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="muscles", weight=10, multi_thread=False) objective_functions.add(ObjectiveFcn.Mayer.SUPERIMPOSE_MARKERS, weight=100000, first_marker="target", second_marker="COM_hand") # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.MUSCLE_DRIVEN, with_torque=True) # Path constraint x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model)) x_bounds[0][:, 0] = (1.0, 1.0, 0, 0) # Initial guess if x_warm is None: x_init = InitialGuess([1.57] * biorbd_model.nbQ() + [0] * biorbd_model.nbQdot()) else: x_init = InitialGuess(x_warm, interpolation=InterpolationType.EACH_FRAME) # Define control path constraint u_bounds = BoundsList() u_bounds.add( [tau_min] * biorbd_model.nbGeneralizedTorque() + [muscle_min] * biorbd_model.nbMuscleTotal(), [tau_max] * biorbd_model.nbGeneralizedTorque() + [muscle_max] * biorbd_model.nbMuscleTotal(), ) u_init = InitialGuessList() u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque() + [muscle_init] * biorbd_model.nbMuscleTotal()) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, use_sx=use_sx, n_threads=n_threads, )
def test_muscle_driven(with_excitations, with_contact, with_torque, with_external_force, cx): # Prepare the program nlp = NonLinearProgram() nlp.model = biorbd.Model( TestUtils.bioptim_folder() + "/examples/muscle_driven_ocp/models/arm26_with_contact.bioMod" ) nlp.ns = 5 nlp.cx = cx nlp.x_bounds = np.zeros((nlp.model.nbQ() * 2 + nlp.model.nbMuscles(), 1)) nlp.u_bounds = np.zeros((nlp.model.nbMuscles(), 1)) ocp = OptimalControlProgram(nlp) nlp.control_type = ControlType.CONSTANT NonLinearProgram.add( ocp, "dynamics_type", Dynamics( DynamicsFcn.MUSCLE_DRIVEN, with_torque=with_torque, with_excitations=with_excitations, with_contact=with_contact, ), False, ) np.random.seed(42) if with_external_force: external_forces = [np.random.rand(6, nlp.model.nbSegment(), nlp.ns)] nlp.external_forces = BiorbdInterface.convert_array_to_external_forces(external_forces)[0] # Prepare the dynamics ConfigureProblem.initialize(ocp, nlp) # Test the results states = np.random.rand(nlp.states.shape, nlp.ns) controls = np.random.rand(nlp.controls.shape, nlp.ns) params = np.random.rand(nlp.parameters.shape, nlp.ns) x_out = np.array(nlp.dynamics_func(states, controls, params)) if with_contact: # Warning this test is a bit bogus, there since the model does not have contacts if with_torque: if with_excitations: if with_external_force: np.testing.assert_almost_equal( x_out[:, 0], [ 0.6158501, 0.50313626, 0.64241928, 1.46421499, -45.27535002, 73.61890834, 46.87928022, -1.80189035, 53.3914525, 48.30056919, 63.69373374, -28.15700995, ], ) else: np.testing.assert_almost_equal( x_out[:, 0], [ 1.83404510e-01, 6.11852895e-01, 7.85175961e-01, -9.06144782e00, 2.93915658e02, -9.24229516e02, 8.60630831e00, 3.19433638e00, 2.97405608e01, -2.02754226e01, -2.32467778e01, -4.19135012e01, ], decimal=6, ) else: if with_external_force: np.testing.assert_almost_equal( x_out[:, 0], [6.15850098e-01, 5.03136259e-01, 6.42419278e-01, -7.67236491e00, 2.30765930e02, -7.34713354e02], decimal=6, ) else: np.testing.assert_almost_equal( x_out[:, 0], [1.83404510e-01, 6.11852895e-01, 7.85175961e-01, -3.57374110e00, 1.13519647e02, -4.07165959e02], decimal=6, ) else: if with_excitations: if with_external_force: np.testing.assert_almost_equal( x_out[:, 0], [ 0.6158501, 0.50313626, 0.64241928, 1.31194581, -50.56193318, 82.71912199, 55.65557816, 50.47052688, 0.36025589, 58.92377491, 29.70094194, -15.13534937, ], ) else: np.testing.assert_almost_equal( x_out[:, 0], [ 1.83404510e-01, 6.11852895e-01, 7.85175961e-01, -9.49194254e00, 3.03909766e02, -9.56600268e02, -7.72228930e00, -1.13759732e01, 9.51906209e01, 4.45077128e00, -5.20261014e00, -2.80864106e01, ], decimal=6, ) else: if with_external_force: np.testing.assert_almost_equal( x_out[:, 0], [0.6158501, 0.50313626, 0.64241928, 1.31194581, -50.56193318, 82.71912199], ) else: np.testing.assert_almost_equal( x_out[:, 0], [1.83404510e-01, 6.11852895e-01, 7.85175961e-01, -9.49194254e00, 3.03909766e02, -9.56600268e02], decimal=6, ) else: if with_torque: if with_excitations: if with_external_force: np.testing.assert_almost_equal( x_out[:, 0], [ 0.6158501, 0.50313626, 0.64241928, 1.46421499, -45.27535002, 73.61890834, 46.87928022, -1.80189035, 53.3914525, 48.30056919, 63.69373374, -28.15700995, ], ) else: np.testing.assert_almost_equal( x_out[:, 0], [ 1.83404510e-01, 6.11852895e-01, 7.85175961e-01, -9.06144782e00, 2.93915658e02, -9.24229516e02, 8.60630831e00, 3.19433638e00, 2.97405608e01, -2.02754226e01, -2.32467778e01, -4.19135012e01, ], decimal=6, ) else: if with_external_force: np.testing.assert_almost_equal( x_out[:, 0], [6.15850098e-01, 5.03136259e-01, 6.42419278e-01, -7.67236491e00, 2.30765930e02, -7.34713354e02], decimal=6, ) else: np.testing.assert_almost_equal( x_out[:, 0], [1.83404510e-01, 6.11852895e-01, 7.85175961e-01, -3.57374110e00, 1.13519647e02, -4.07165959e02], decimal=6, ) else: if with_excitations: if with_external_force: np.testing.assert_almost_equal( x_out[:, 0], [ 0.6158501, 0.50313626, 0.64241928, 1.31194581, -50.56193318, 82.71912199, 55.65557816, 50.47052688, 0.36025589, 58.92377491, 29.70094194, -15.13534937, ], ) else: np.testing.assert_almost_equal( x_out[:, 0], [ 1.83404510e-01, 6.11852895e-01, 7.85175961e-01, -9.49194254e00, 3.03909766e02, -9.56600268e02, -7.72228930e00, -1.13759732e01, 9.51906209e01, 4.45077128e00, -5.20261014e00, -2.80864106e01, ], decimal=6, ) else: if with_external_force: np.testing.assert_almost_equal( x_out[:, 0], [0.6158501, 0.50313626, 0.64241928, 1.31194581, -50.56193318, 82.71912199], ) else: np.testing.assert_almost_equal( x_out[:, 0], [1.83404510e-01, 6.11852895e-01, 7.85175961e-01, -9.49194254e00, 3.03909766e02, -9.56600268e02], decimal=6, )
def prepare_ocp( biorbd_model_path: str, final_time: float, n_shooting: int, fatigue_type: str, split_controls: bool, use_sx: bool = True, ) -> OptimalControlProgram: """ The initialization of an ocp Parameters ---------- biorbd_model_path: str The path to the biorbd model final_time: float The time in second required to perform the task n_shooting: int The number of shooting points to define int the direct multiple shooting program fatigue_type: str The type of dynamics to apply ("xia" or "michaud") split_controls: bool If the tau should be split into minus and plus or a if_else should be used use_sx: bool If the program should be built from SX (True) or MX (False) Returns ------- The OptimalControlProgram ready to be solved """ biorbd_model = biorbd.Model(biorbd_model_path) n_tau = biorbd_model.nbGeneralizedTorque() tau_min, tau_max, tau_init = -100, 100, 0 # Add objective functions objective_functions = Objective(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", expand=True) # Fatigue parameters fatigue_dynamics = FatigueList() for i in range(n_tau): if fatigue_type == "xia": fatigue_dynamics.add( XiaTauFatigue( XiaFatigue(LD=100, LR=100, F=5, R=10, scaling=tau_min), XiaFatigue(LD=100, LR=100, F=5, R=10, scaling=tau_max), state_only=False, split_controls=split_controls, ), ) elif fatigue_type == "xia_stabilized": fatigue_dynamics.add( XiaTauFatigue( XiaFatigueStabilized(LD=100, LR=100, F=5, R=10, stabilization_factor=10, scaling=tau_min), XiaFatigueStabilized(LD=100, LR=100, F=5, R=10, stabilization_factor=10, scaling=tau_max), state_only=False, split_controls=split_controls, ), ) elif fatigue_type == "michaud": fatigue_dynamics.add( MichaudTauFatigue( MichaudFatigue( LD=100, LR=100, F=0.005, R=0.005, effort_threshold=0.2, effort_factor=0.001, stabilization_factor=10, scaling=tau_min, ), MichaudFatigue( LD=100, LR=100, F=0.005, R=0.005, effort_threshold=0.2, effort_factor=0.001, stabilization_factor=10, scaling=tau_max, ), state_only=False, split_controls=split_controls, ), ) elif fatigue_type == "effort": fatigue_dynamics.add( TauEffortPerception( EffortPerception(effort_threshold=0.2, effort_factor=0.001, scaling=tau_min), EffortPerception(effort_threshold=0.2, effort_factor=0.001, scaling=tau_max), split_controls=split_controls, ) ) else: raise ValueError("fatigue_type not implemented") # Dynamics dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN, fatigue=fatigue_dynamics, expand=True) # Path constraint x_bounds = QAndQDotBounds(biorbd_model) x_bounds[:, [0, -1]] = 0 x_bounds[1, -1] = 3.14 x_bounds.concatenate(FatigueBounds(fatigue_dynamics, fix_first_frame=True)) if fatigue_type != "effort": x_bounds[[5, 11], 0] = 0 # The rotation dof is passive (fatigue_ma = 0) if fatigue_type == "xia": x_bounds[[7, 13], 0] = 1 # The rotation dof is passive (fatigue_mr = 1) # Initial guess n_q = biorbd_model.nbQ() n_qdot = biorbd_model.nbQdot() x_init = InitialGuess([0] * (n_q + n_qdot)) x_init.concatenate(FatigueInitialGuess(fatigue_dynamics)) # Define control path constraint u_bounds = FatigueBounds(fatigue_dynamics, variable_type=VariableType.CONTROLS) if split_controls: u_bounds[[1, 3], :] = 0 # The rotation dof is passive else: u_bounds[1, :] = 0 # The rotation dof is passive u_init = FatigueInitialGuess(fatigue_dynamics, variable_type=VariableType.CONTROLS) return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init=x_init, u_init=u_init, x_bounds=x_bounds, u_bounds=u_bounds, objective_functions=objective_functions, use_sx=use_sx, )
def prepare_ocp( biorbd_model_path: str, problem_type_custom: bool = True, ode_solver: OdeSolver = OdeSolver.RK4(), use_sx: bool = False, ) -> OptimalControlProgram: """ Prepare the program Parameters ---------- biorbd_model_path: str The path of the biorbd model problem_type_custom: bool If the preparation should be done using the user-defined dynamic function or the normal TORQUE_DRIVEN. They should return the same results ode_solver: OdeSolver The type of ode solver used use_sx: bool If the program should be constructed using SX instead of MX (longer to create the CasADi graph, faster to solve) Returns ------- The ocp ready to be solved """ # --- Options --- # # Model path biorbd_model = biorbd.Model(biorbd_model_path) # Problem parameters n_shooting = 30 final_time = 2 # Add objective functions objective_functions = Objective(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, multi_thread=False) # Dynamics dynamics = DynamicsList() expand = False if isinstance(ode_solver, OdeSolver.IRK) else True if problem_type_custom: dynamics.add(custom_configure, dynamic_function=custom_dynamic, my_additional_factor=1, expand=expand) else: dynamics.add(DynamicsFcn.TORQUE_DRIVEN, dynamic_function=custom_dynamic, expand=expand) # Constraints constraints = ConstraintList() constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.START, first_marker="m0", second_marker="m1") constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m2") # Path constraint x_bounds = QAndQDotBounds(biorbd_model) x_bounds[1:6, [0, -1]] = 0 x_bounds[2, -1] = 1.57 # 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()) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, ode_solver=ode_solver, use_sx=use_sx, )
def prepare_ocp( biorbd_model_path: str, final_time: float, n_shooting: int, use_sx: bool = True ) -> OptimalControlProgram: """ The initialization of an ocp Parameters ---------- biorbd_model_path: str The path to the biorbd model final_time: float The time in second required to perform the task n_shooting: int The number of shooting points to define int the direct multiple shooting program use_sx: bool If the ocp should be built with SX. Please note that ACADOS requires SX Returns ------- The OptimalControlProgram ready to be solved """ biorbd_model = biorbd.Model(biorbd_model_path) nq = biorbd_model.nbQ() nqdot = biorbd_model.nbQdot() target = np.zeros((nq + nqdot, 1)) target[1, 0] = 3.14 # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100.0, multi_thread=False) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE, key="q", weight=1.0, multi_thread=False) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE, key="qdot", weight=1.0, multi_thread=False) objective_functions.add( ObjectiveFcn.Mayer.MINIMIZE_STATE, weight=50000, key="q", target=target[:nq, :], multi_thread=False ) objective_functions.add( ObjectiveFcn.Mayer.MINIMIZE_STATE, weight=50000, key="qdot", target=target[nq:, :], multi_thread=False ) # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN) # Path constraint x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model)) x_bounds[0][:, 0] = 0 # Initial guess x_init = InitialGuessList() x_init.add([0] * (nq + nqdot)) # Define control path constraint n_tau = biorbd_model.nbGeneralizedTorque() torque_min, torque_max, torque_init = -100, 100, 0 u_bounds = BoundsList() u_bounds.add([torque_min] * n_tau, [torque_max] * n_tau) u_bounds[0][n_tau - 1, :] = 0 u_init = InitialGuessList() u_init.add([torque_init] * n_tau) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, use_sx=use_sx, )
def main(): """ Generate random data, then create a tracking problem, and finally solve it and plot some relevant information """ # Define the problem biorbd_model = biorbd.Model("models/arm26.bioMod") final_time = 0.5 n_shooting_points = 50 use_residual_torque = True # Generate random data to fit t, markers_ref, x_ref, muscle_activations_ref = generate_data( biorbd_model, final_time, n_shooting_points, use_residual_torque=use_residual_torque) # Track these data biorbd_model = biorbd.Model( "models/arm26.bioMod" ) # To allow for non free variable, the model must be reloaded ocp = prepare_ocp( biorbd_model, final_time, n_shooting_points, markers_ref, muscle_activations_ref, x_ref[:biorbd_model.nbQ(), :], kin_data_to_track="q", use_residual_torque=use_residual_torque, ) # --- Solve the program --- # sol = ocp.solve(show_online_optim=True) # --- Show the results --- # q = sol.states["q"] n_q = ocp.nlp[0].model.nbQ() n_mark = ocp.nlp[0].model.nbMarkers() n_frames = q.shape[1] markers = np.ndarray((3, n_mark, q.shape[1])) symbolic_states = MX.sym("x", n_q, 1) markers_func = biorbd.to_casadi_func("ForwardKin", biorbd_model.markers, symbolic_states) for i in range(n_frames): markers[:, :, i] = markers_func(q[:, i]) plt.figure("Markers") n_steps_ode = ocp.nlp[0].ode_solver.steps + 1 if ocp.nlp[ 0].ode_solver.is_direct_collocation else 1 for i in range(markers.shape[1]): plt.plot(np.linspace(0, final_time, n_shooting_points + 1), markers_ref[:, i, :].T, "k") plt.plot( np.linspace(0, final_time, n_shooting_points * n_steps_ode + 1), markers[:, i, :].T, "r--") # --- Plot --- # plt.show()
def prepare_ocp( biorbd_model_path: str = "models/cubeSym.bioMod", ode_solver: OdeSolver = OdeSolver.RK4() ) -> OptimalControlProgram: """ Prepare the ocp Parameters ---------- biorbd_model_path: str Path to the bioMod ode_solver: OdeSolver The ode solver to use Returns ------- The OptimalControlProgram ready to be solved """ biorbd_model = biorbd.Model(biorbd_model_path) # Problem parameters n_shooting = 30 final_time = 2 tau_min, tau_max, tau_init = -100, 100, 0 dof_mappings = BiMappingList() dof_mappings.add("q", [0, 1, None, 2, 2], [0, 1, 3], 4) # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100) # Dynamics dynamics = DynamicsList() expand = False if isinstance(ode_solver, OdeSolver.IRK) else True dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand) # Constraints constraints = ConstraintList() constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.START, first_marker="m0", second_marker="m1") constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m2") # Path constraint x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model, dof_mappings[0])) x_bounds[0][3:6, [0, -1]] = 0 # Initial guess x_init = InitialGuessList() x_init.add([0] * len(dof_mappings["q"].to_first) * 2) # Define control path constraint u_bounds = BoundsList() u_bounds.add([tau_min] * len(dof_mappings["q"].to_first), [tau_max] * len(dof_mappings["q"].to_first)) u_init = InitialGuessList() u_init.add([tau_init] * len(dof_mappings["q"].to_first)) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, ode_solver=ode_solver, variable_mappings=dof_mappings, )