def generate_table(out): model_path = "/".join(__file__.split("/")[:-1]) + "/models/arm26.bioMod" biorbd_model_ip = biorbd.Model(model_path) # IPOPT use_ipopt = True weights = np.array([100, 1, 1, 100000]) ocp = prepare_ocp(biorbd_model=biorbd_model_ip, final_time=2, n_shooting=50, use_sx=not use_ipopt, weights=weights) opts = {"linear_solver": "ma57", "hessian_approximation": "exact"} solver = Solver.IPOPT # --- Solve the program --- # tic = time() sol = ocp.solve( solver=solver, solver_options=opts, ) toc = time() - tic sol_merged = sol.merge_phases() out.nx = sol_merged.states["all"].shape[0] out.nu = sol_merged.controls["all"].shape[0] out.ns = sol_merged.ns[0] out.solver.append(out.Solver("Ipopt")) out.solver[0].n_iteration = sol.iterations out.solver[0].cost = sol.cost out.solver[0].convergence_time = toc out.solver[0].compute_error_single_shooting(sol, 1) # ACADOS use_ipopt = False biorbd_model_ac = biorbd.Model(model_path) ocp = prepare_ocp(biorbd_model=biorbd_model_ac, final_time=2, n_shooting=50, use_sx=not use_ipopt, weights=weights) opts = { "sim_method_num_steps": 5, "tol": 1e-8, "integrator_type": "ERK", "hessian_approx": "GAUSS_NEWTON" } solver = Solver.ACADOS # --- Solve the program --- # sol = ocp.solve( solver=solver, solver_options=opts, ) out.solver.append(out.Solver("Acados")) out.solver[1].n_iteration = sol.iterations out.solver[1].cost = sol.cost out.solver[1].convergence_time = sol.time_to_optimize out.solver[1].compute_error_single_shooting(sol, 1)
def test_plot_merged_graphs(): # Load graphs_one_phase bioptim_folder = TestUtils.bioptim_folder() merged_graphs = TestUtils.load_module( bioptim_folder + "/examples/muscle_driven_ocp/muscle_excitations_tracker.py") # Define the problem model_path = bioptim_folder + "/examples/muscle_driven_ocp/arm26.bioMod" biorbd_model = biorbd.Model(model_path) final_time = 0.5 n_shooting = 9 # Generate random data to fit np.random.seed(42) t, markers_ref, x_ref, muscle_excitations_ref = merged_graphs.generate_data( biorbd_model, final_time, n_shooting) biorbd_model = biorbd.Model( model_path ) # To prevent from non free variable, the model must be reloaded ocp = merged_graphs.prepare_ocp( biorbd_model, final_time, n_shooting, markers_ref, muscle_excitations_ref, x_ref[:biorbd_model.nbQ(), :].T, use_residual_torque=True, kin_data_to_track="markers", ) sol = ocp.solve() sol.graphs(automatically_organize=False)
def test_plot_merged_graphs(): # Load graphs_one_phase PROJECT_FOLDER = Path(__file__).parent / ".." spec = importlib.util.spec_from_file_location( "align_markers", str(PROJECT_FOLDER) + "/examples/muscle_driven_ocp/muscle_excitations_tracker.py" ) merged_graphs = importlib.util.module_from_spec(spec) spec.loader.exec_module(merged_graphs) # Define the problem model_path = str(PROJECT_FOLDER) + "/examples/muscle_driven_ocp/arm26.bioMod" biorbd_model = biorbd.Model(model_path) final_time = 0.5 nb_shooting = 9 # Generate random data to fit np.random.seed(42) t, markers_ref, x_ref, muscle_excitations_ref = merged_graphs.generate_data(biorbd_model, final_time, nb_shooting) biorbd_model = biorbd.Model(model_path) # To prevent from non free variable, the model must be reloaded ocp = merged_graphs.prepare_ocp( biorbd_model, final_time, nb_shooting, markers_ref, muscle_excitations_ref, x_ref[: biorbd_model.nbQ(), :].T, use_residual_torque=True, kin_data_to_track="markers", ) sol = ocp.solve() plt = ShowResult(ocp, sol) plt.graphs(automatically_organize=False)
def test_plot_merged_graphs(): # Define the problem model_path = str( PROJECT_FOLDER) + "/examples/muscle_driven_ocp/arm26.bioMod" biorbd_model = biorbd.Model(model_path) final_time = 0.5 nb_shooting = 9 # Generate random data to fit np.random.seed(42) t, markers_ref, x_ref, muscle_excitations_ref = merged_graphs.generate_data( biorbd_model, final_time, nb_shooting) biorbd_model = biorbd.Model( model_path ) # To allow for non free variable, the model must be reloaded ocp = merged_graphs.prepare_ocp( biorbd_model, final_time, nb_shooting, markers_ref, muscle_excitations_ref, x_ref[:biorbd_model.nbQ(), :].T, kin_data_to_track="markers", ) sol = ocp.solve() plt = ShowResult(ocp, sol) plt.graphs(automatically_organize=False)
def generate_table(out): root_path = "/".join(__file__.split("/")[:-1]) # Define the problem -- model path biorbd_model = ( biorbd.Model(root_path + "/models/Gait_1leg_12dof_heel.bioMod"), biorbd.Model(root_path + "/models/Gait_1leg_12dof_flatfoot.bioMod"), biorbd.Model(root_path + "/models/Gait_1leg_12dof_forefoot.bioMod"), biorbd.Model(root_path + "/models/Gait_1leg_12dof_0contact.bioMod"), ) # --- files path --- c3d_file = root_path + "/data/normal01_out.c3d" q_kalman_filter_file = root_path + "/data/normal01_q_KalmanFilter.txt" qdot_kalman_filter_file = root_path + "/data/normal01_qdot_KalmanFilter.txt" data = LoadData(biorbd_model[0], c3d_file, q_kalman_filter_file, qdot_kalman_filter_file) # --- phase time and number of shooting --- phase_time, number_shooting_points = get_phase_time_shooting_numbers(data, 0.01) # --- get experimental data --- q_ref, qdot_ref, markers_ref, grf_ref, moments_ref, cop_ref = get_experimental_data(data, number_shooting_points) ocp = prepare_ocp( biorbd_model=biorbd_model, final_time=phase_time, nb_shooting=number_shooting_points, markers_ref=markers_ref, grf_ref=grf_ref, q_ref=q_ref, qdot_ref=qdot_ref, M_ref=moments_ref, CoP=cop_ref, nb_threads=8, ) # --- Solve the program --- # tic = time() sol = ocp.solve( solver=Solver.IPOPT, solver_options={ "tol": 1e-3, "max_iter": 1000, "hessian_approximation": "exact", "limited_memory_max_history": 50, "linear_solver": "ma57", }, ) toc = time() - tic sol_merged = sol.merge_phases() out.nx = sol_merged.states["all"].shape[0] out.nu = sol_merged.controls["all"].shape[0] out.ns = sol_merged.ns[0] out.solver.append(out.Solver("Ipopt")) out.solver[0].n_iteration = sol.iterations out.solver[0].cost = sol.cost out.solver[0].convergence_time = toc out.solver[0].compute_error_single_shooting(sol, 1, use_final_time=True)
def test_muscle_activations_and_states_tracking(): # Define the problem model_path = str(PROJECT_FOLDER) + "/examples/muscle_driven_ocp/arm26.bioMod" biorbd_model = biorbd.Model(model_path) final_time = 2 nb_shooting = 9 # Generate random data to fit np.random.seed(42) t, markers_ref, x_ref, muscle_activations_ref = muscle_activations_tracker.generate_data( biorbd_model, final_time, nb_shooting ) biorbd_model = biorbd.Model(model_path) # To allow for non free variable, the model must be reloaded ocp = muscle_activations_tracker.prepare_ocp( biorbd_model, final_time, nb_shooting, markers_ref, muscle_activations_ref, x_ref[: biorbd_model.nbQ(), :].T, kin_data_to_track="q", ) sol = ocp.solve() # Check objective function value f = np.array(sol["f"]) np.testing.assert_equal(f.shape, (1, 1)) np.testing.assert_almost_equal(f[0, 0], 1.4506639252752042e-06) # Check constraints g = np.array(sol["g"]) np.testing.assert_equal(g.shape, (36, 1)) np.testing.assert_almost_equal(g, np.zeros((36, 1)), decimal=6) # Check some of the results states, controls = Data.get_data(ocp, sol["x"]) q, qdot, tau, mus = states["q"], states["q_dot"], controls["tau"], controls["muscles"] # initial and final position np.testing.assert_almost_equal(q[:, 0], np.array([-1.13043502e-05, -1.35629661e-05])) np.testing.assert_almost_equal(q[:, -1], np.array([-0.49387966, -1.44924784])) # initial and final velocities np.testing.assert_almost_equal(qdot[:, 0], np.array([-8.66527631e-05, -1.31486656e-04])) np.testing.assert_almost_equal(qdot[:, -1], np.array([0.8780829, -2.6474387])) # initial and final controls np.testing.assert_almost_equal(tau[:, 0], np.array([-1.55359644e-06, 1.26569700e-05])) np.testing.assert_almost_equal(tau[:, -1], np.array([-7.41845169e-06, -7.67568954e-07])) np.testing.assert_almost_equal( mus[:, 0], np.array([0.37439688, 0.95073361, 0.73203047, 0.59855246, 0.15592687, 0.15595739]), ) np.testing.assert_almost_equal( mus[:, -1], np.array([0.54685367, 0.18482085, 0.96945157, 0.77512036, 0.93947405, 0.89483397]), )
def test_mhe(solver): if platform == "win32" and solver == Solver.ACADOS: print("Test for ACADOS on Windows is skipped") return root_folder = TestUtils.bioptim_folder() + "/examples/moving_horizon_estimation/" pendulum = TestUtils.load_module(root_folder + "mhe.py") biorbd_model = biorbd.Model(root_folder + "cart_pendulum.bioMod") nq = biorbd_model.nbQ() torque_max = 5 # Give a bit of slack on the max torque n_cycles = 5 if solver == Solver.ACADOS else 1 n_frame_by_cycle = 20 window_len = 5 window_duration = 0.2 final_time = window_duration / window_len * n_cycles * n_frame_by_cycle x_init = np.zeros((nq * 2, window_len + 1)) u_init = np.zeros((nq, window_len)) target_q, _, _, _ = pendulum.generate_data( biorbd_model, final_time, [0, np.pi / 2, 0, 0], torque_max, n_cycles * n_frame_by_cycle, 0 ) target = pendulum.states_to_markers(biorbd_model, target_q) def update_functions(mhe, t, _): def target_func(i: int): return target[:, :, i : i + window_len + 1] mhe.update_objectives_target(target=target_func(t), list_index=0) return t < n_frame_by_cycle * n_cycles - window_len - 1 biorbd_model = biorbd.Model(root_folder + "cart_pendulum.bioMod") sol = pendulum.prepare_mhe( biorbd_model=biorbd_model, window_len=window_len, window_duration=window_duration, max_torque=torque_max, x_init=x_init, u_init=u_init, ).solve(update_functions, **pendulum.get_solver_options(solver)) # Clean test folder if solver == Solver.ACADOS: # Compare the position on the first few frames (only ACADOS, since IPOPT is not precise with current options) np.testing.assert_almost_equal( sol.states["q"][:, : -2 * window_len], target_q[:nq, : -3 * window_len - 1], decimal=3 ) os.remove(f"./acados_ocp.json") shutil.rmtree(f"./c_generated_code/")
def prepare_test_ocp(with_muscles=False, with_contact=False, with_actuator=False): PROJECT_FOLDER = Path(__file__).parent / ".." if with_muscles and with_contact or with_muscles and with_actuator or with_contact and with_actuator: raise RuntimeError( "With muscles and with contact and with_actuator together is not defined" ) elif with_muscles: biorbd_model = biorbd.Model( str(PROJECT_FOLDER) + "/examples/muscle_driven_ocp/arm26.bioMod") dynamics = DynamicsList() dynamics.add(DynamicsFcn.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN) nx = biorbd_model.nbQ() + biorbd_model.nbQdot() nu = biorbd_model.nbGeneralizedTorque() + biorbd_model.nbMuscles() elif with_contact: biorbd_model = biorbd.Model( str(PROJECT_FOLDER) + "/examples/muscle_driven_with_contact/2segments_4dof_2contacts_1muscle.bioMod" ) dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN_WITH_CONTACT) nx = biorbd_model.nbQ() + biorbd_model.nbQdot() nu = biorbd_model.nbGeneralizedTorque() elif with_actuator: biorbd_model = biorbd.Model( str(PROJECT_FOLDER) + "/examples/torque_driven_ocp/cube.bioMod") dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN) nx = biorbd_model.nbQ() + biorbd_model.nbQdot() nu = biorbd_model.nbGeneralizedTorque() else: biorbd_model = biorbd.Model( str(PROJECT_FOLDER) + "/examples/align/cube_and_line.bioMod") dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN) nx = biorbd_model.nbQ() + biorbd_model.nbQdot() nu = biorbd_model.nbGeneralizedTorque() x_init = InitialGuess(np.zeros((nx, 1))) u_init = InitialGuess(np.zeros((nu, 1))) x_bounds = Bounds(np.zeros((nx, 1)), np.zeros((nx, 1))) u_bounds = Bounds(np.zeros((nu, 1)), np.zeros((nu, 1))) ocp = OptimalControlProgram(biorbd_model, dynamics, 10, 1.0, x_init, u_init, x_bounds, u_bounds) ocp.nlp[0].J = [list()] ocp.nlp[0].g = [list()] ocp.nlp[0].g_bounds = [list()] return ocp
def test_acados_one_mayer(cost_type): PROJECT_FOLDER = Path(__file__).parent / ".." spec = importlib.util.spec_from_file_location( "cube", str(PROJECT_FOLDER) + "/examples/acados/cube.py", ) cube = importlib.util.module_from_spec(spec) spec.loader.exec_module(cube) ocp = cube.prepare_ocp( biorbd_model_path=str(PROJECT_FOLDER) + "/examples/acados/cube.bioMod", nbs=10, tf=2, ) objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_STATE, index=[0], target=np.array([[1.0]]).T) ocp.update_objectives(objective_functions) sol = ocp.solve(solver=Solver.ACADOS, solver_options={"cost_type": cost_type}) # Check end state value model = biorbd.Model(str(PROJECT_FOLDER) + "/examples/acados/cube.bioMod") q = np.array(sol["qqdot"])[:model.nbQ()] np.testing.assert_almost_equal(q[0, -1], 1.0) # Clean test folder os.remove(f"./acados_ocp.json") shutil.rmtree(f"./c_generated_code/")
def test_acados_one_lagrange(cost_type): PROJECT_FOLDER = Path(__file__).parent / ".." spec = importlib.util.spec_from_file_location( "cube", str(PROJECT_FOLDER) + "/examples/acados/cube.py", ) cube = importlib.util.module_from_spec(spec) spec.loader.exec_module(cube) nbs = 10 target = np.expand_dims(np.arange(0, nbs + 1), axis=0) target[0, -1] = nbs - 2 ocp = cube.prepare_ocp( biorbd_model_path=str(PROJECT_FOLDER) + "/examples/acados/cube.bioMod", nbs=nbs, tf=2, ) objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.TRACK_STATE, weight=10, index=[0], target=target) ocp.update_objectives(objective_functions) sol = ocp.solve(solver=Solver.ACADOS, solver_options={"cost_type": cost_type}) # Check end state value model = biorbd.Model(str(PROJECT_FOLDER) + "/examples/acados/cube.bioMod") q = np.array(sol["qqdot"])[:model.nbQ()] np.testing.assert_almost_equal(q[0, :], target[0, :].squeeze()) # Clean test folder os.remove(f"./acados_ocp.json") shutil.rmtree(f"./c_generated_code/")
def prepare_ocp(model_path, phase_time, number_shooting_points, muscle_activations_ref, contact_forces_ref): # Model path biorbd_model = biorbd.Model(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(Objective.Lagrange.TRACK_MUSCLES_CONTROL, target=muscle_activations_ref) objective_functions.add(Objective.Lagrange.TRACK_CONTACT_FORCES, target=contact_forces_ref) # Dynamics dynamics = DynamicsTypeList() dynamics.add( DynamicsType.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN_WITH_CONTACT) # Path constraint nb_q = biorbd_model.nbQ() nb_qdot = nb_q pose_at_first_node = [0, 0, -0.75, 0.75] # Initialize X_bounds x_bounds = BoundsList() x_bounds.add(QAndQDotBounds(biorbd_model)) x_bounds[0].min[:, 0] = pose_at_first_node + [0] * nb_qdot x_bounds[0].max[:, 0] = pose_at_first_node + [0] * nb_qdot # Initial guess x_init = InitialConditionsList() x_init.add(pose_at_first_node + [0] * nb_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 = InitialConditionsList() u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque() + [activation_init] * biorbd_model.nbMuscleTotal()) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, number_shooting_points, phase_time, x_init, u_init, x_bounds, u_bounds, objective_functions=objective_functions, )
def impact(ocp, transition): """ TODO """ if ocp.nlp[transition.phase_pre_idx]["nx"] != ocp.nlp[ (transition.phase_pre_idx + 1) % ocp.nb_phases]["nx"]: raise RuntimeError( "Impact transition without same nx is not possible, please provide a custom state transition" ) # Aliases nlp_pre, nlp_post = StateTransitionFunctions.Functions.__get_nlp_pre_and_post( ocp, transition.phase_pre_idx) nbQ = nlp_pre["nbQ"] nbQdot = nlp_pre["nbQdot"] q = nlp_pre["q_mapping"].expand.map(nlp_pre["X"][-1][:nbQ]) qdot_pre = nlp_pre["q_dot_mapping"].expand.map( nlp_pre["X"][-1][nbQ:nbQ + nbQdot]) if nlp_post["model"].nbContacts() == 0: warn("The chosen model does not have any contact") # A new model is loaded here so we can use pre Qdot with post model, this is a hack and should be dealt # a better way (e.g. create a supplementary variable in V that link the pre and post phase with a # constraint. The transition would therefore apply to node_0 and node_1 (with an augmented ns) model = biorbd.Model( nlp_post["model"].path().absolutePath().to_string()) func = biorbd.to_casadi_func("impulse_direct", model.ComputeConstraintImpulsesDirect, nlp_pre["q"], nlp_pre["qdot"]) qdot_post = func(q, qdot_pre) qdot_post = nlp_post["q_dot_mapping"].reduce.map(qdot_post) val = nlp_pre["X"][-1][:nbQ] - nlp_post["X"][0][:nbQ] val = vertcat(val, qdot_post - nlp_post["X"][0][nbQ:nbQ + nbQdot]) return val
def test_mhe_redim_xbounds_and_init(): root_folder = TestUtils.bioptim_folder() + "/examples/moving_horizon_estimation/" biorbd_model = biorbd.Model(root_folder + "cart_pendulum.bioMod") nq = biorbd_model.nbQ() ntau = biorbd_model.nbGeneralizedTorque() n_cycles = 3 window_len = 5 window_duration = 0.2 x_init = InitialGuess(np.zeros((nq * 2, 1)), interpolation=InterpolationType.CONSTANT) u_init = InitialGuess(np.zeros((ntau, 1)), interpolation=InterpolationType.CONSTANT) x_bounds = Bounds(np.zeros((nq * 2, 1)), np.zeros((nq * 2, 1)), interpolation=InterpolationType.CONSTANT) u_bounds = Bounds(np.zeros((ntau, 1)), np.zeros((ntau, 1))) mhe = MovingHorizonEstimator( biorbd_model, Dynamics(DynamicsFcn.TORQUE_DRIVEN), window_len, window_duration, x_init=x_init, u_init=u_init, x_bounds=x_bounds, u_bounds=u_bounds, n_threads=4, ) def update_functions(mhe, t, _): return t < n_cycles mhe.solve(update_functions, Solver.IPOPT)
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_CoM(): m = biorbd.Model("../../models/pyomecaman.bioMod") q = np.array( [0.1, 0.1, 0.1, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3]) q_dot = np.array([1, 1, 1, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3]) q_ddot = np.array([10, 10, 10, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30]) expected_CoM = np.array( [-0.0034679564024098523, 0.15680579877453169, 0.07808112642459612]) expected_CoM_dot = np.array( [-0.05018973433722229, 1.4166208451420528, 1.4301750486035787]) expected_CoM_ddot = np.array( [-0.7606169667295027, 11.508107073695976, 16.58853835505851]) if biorbd.currentLinearAlgebraBackend() == 1: # If CasADi backend is used from casadi import Function, MX q_sym = MX.sym("q", m.nbQ(), 1) q_dot_sym = MX.sym("q_dot", m.nbQdot(), 1) q_ddot_sym = MX.sym("q_ddot", m.nbQddot(), 1) CoM_func = Function( "Compute_CoM", [q_sym], [m.CoM(q_sym).to_mx()], ["q"], ["CoM"], ).expand() CoM_dot_func = Function( "Compute_CoM_dot", [q_sym, q_dot_sym], [m.CoMdot(q_sym, q_dot_sym).to_mx()], ["q", "q_dot"], ["CoM_dot"], ).expand() CoM_ddot_func = Function( "Compute_CoM_ddot", [q_sym, q_dot_sym, q_ddot_sym], [m.CoMddot(q_sym, q_dot_sym, q_ddot_sym).to_mx()], ["q", "q_dot", "q_ddot"], ["CoM_ddot"], ).expand() CoM = np.array(CoM_func(q)) CoM_dot = np.array(CoM_dot_func(q, q_dot)) CoM_ddot = np.array(CoM_ddot_func(q, q_dot, q_ddot)) elif not biorbd.currentLinearAlgebraBackend(): # If Eigen backend is used CoM = m.CoM(q).to_array() CoM_dot = m.CoMdot(q, q_dot).to_array() CoM_ddot = m.CoMddot(q, q_dot, q_ddot).to_array() np.testing.assert_almost_equal(CoM.squeeze(), expected_CoM) np.testing.assert_almost_equal(CoM_dot.squeeze(), expected_CoM_dot) np.testing.assert_almost_equal(CoM_ddot.squeeze(), expected_CoM_ddot)
def test_pendulum_max_time_lagrange_constrained(ode_solver): # Load pendulum_min_time_Lagrange biorbd_model_path = (TestUtils.bioptim_folder() + "/examples/optimal_time_ocp/pendulum.bioMod", ) # --- Options --- # biorbd_model = biorbd.Model(biorbd_model_path[0]) # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TIME, weigth=-1, max_bound=1) # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN) # ------------- # with pytest.raises( RuntimeError, match= "ObjectiveFcn.Lagrange.MINIMIZE_TIME cannot have max_bound. Please either use MAYER or constraint", ): OptimalControlProgram(biorbd_model, dynamics, 10, 2, objective_functions=objective_functions)
def muscles_forces(states, controls, nlp, force=None, len=None, tsl=None, pa=None, insx=None, insy=None, insz=None): nq = nlp["q_mapping"].reduce.len q = nlp["q_mapping"].expand.map(states[:nq]) qdot = nlp["q_dot_mapping"].expand.map(states[nq:]) biorbd_model = biorbd.Model("arm_Belaise.bioMod") activations = states[nlp["nbQ"] + nlp["nbQdot"]:] muscles_states = biorbd.VecBiorbdMuscleState(nlp["nbMuscle"]) for k in range(nlp["nbMuscle"]): muscles_states[k].setActivation(activations[k]) muscles_states[k].setExcitation(controls[k]) if force is not None: biorbd_model.muscle(k).characteristics().setForceIsoMax(force[k]) if len is not None: biorbd_model.muscle(k).characteristics().setOptimalLength(len[k]) if tsl is not None: biorbd_model.muscle(k).characteristics().setTendonSlackLength(tsl[k]) if pa is not None: biorbd_model.muscle(k).characteristics().setPennationAngle(pa[k]) if insx is not None: biorbd_model.muscle(k).position().setInsertionInLocal(biorbd.Vector3d(insx[k], insy[k], insz[k])) return biorbd_model.muscleForces(muscles_states, q, qdot).to_mx()
def prepare_ocp(biorbd_model_path, n_shooting, tf, ode_solver=OdeSolver.RK4(), use_sx=True): # Model path biorbd_model = biorbd.Model(biorbd_model_path) # Dynamics dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN) # Path constraint x_bounds = QAndQDotBounds(biorbd_model) 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, tf, x_init, u_init, x_bounds, u_bounds, ode_solver=ode_solver, use_sx=use_sx, )
def test_set_scalar(): def check_value(target): if biorbd.currentLinearAlgebraBackend() == 1: assert m.segment(0).characteristics().mass().to_mx() == target else: assert m.segment(0).characteristics().mass() == target m = biorbd.Model("../../models/pyomecaman.bioMod") m.segment(0).characteristics().setMass(10) check_value(10) m.segment(0).characteristics().setMass(11.0) check_value(11.0) with pytest.raises(ValueError, match="Scalar must be a 1x1 array or a float"): m.segment(0).characteristics().setMass(np.array([])) m.segment(0).characteristics().setMass(np.array((12, ))) check_value(12.0) m.segment(0).characteristics().setMass(np.array([[13]])) check_value(13.0) with pytest.raises(ValueError, match="Scalar must be a 1x1 array or a float"): m.segment(0).characteristics().setMass(np.array([[[14]]])) if biorbd.currentLinearAlgebraBackend() == 1: from casadi import MX m.segment(0).characteristics().setMass(MX(15)) check_value(15.0)
def prepare_ocp(model_path, phase_time, number_shooting_points): # --- Options --- # # Model path biorbd_model = biorbd.Model(model_path) torque_min, torque_max, torque_init = -500, 500, 0 tau_mapping = BidirectionalMapping(Mapping([-1, -1, -1, 0]), Mapping([3])) # Add objective functions objective_functions = ( { "type": Objective.Mayer.MINIMIZE_PREDICTED_COM_HEIGHT, "weight": -1 }, { "type": Objective.Lagrange.MINIMIZE_TORQUE, "weight": 1 / 100 }, ) # Dynamics problem_type = ProblemType.torque_driven_with_contact # Constraints constraints = () # Path constraint nb_q = biorbd_model.nbQ() nb_qdot = nb_q pose_at_first_node = [0, 0, -0.5, 0.5] # Initialize X_bounds X_bounds = QAndQDotBounds(biorbd_model) X_bounds.min[:, 0] = pose_at_first_node + [0] * nb_qdot X_bounds.max[:, 0] = pose_at_first_node + [0] * nb_qdot # Initial guess X_init = InitialConditions(pose_at_first_node + [0] * nb_qdot) # Define control path constraint U_bounds = Bounds(min_bound=[torque_min] * tau_mapping.reduce.len, max_bound=[torque_max] * tau_mapping.reduce.len) U_init = InitialConditions([torque_init] * tau_mapping.reduce.len) # ------------- # return OptimalControlProgram( biorbd_model, problem_type, number_shooting_points, phase_time, X_init, U_init, X_bounds, U_bounds, objective_functions, constraints, tau_mapping=tau_mapping, )
def prepare_ocp(biorbd_model_path, final_time, number_shooting_points, min_g, max_g, target_g): # --- Options --- # biorbd_model = biorbd.Model(biorbd_model_path) tau_min, tau_max, tau_init = -30, 30, 0 n_q = biorbd_model.nbQ() n_qdot = biorbd_model.nbQdot() n_tau = biorbd_model.nbGeneralizedTorque() # Add objective functions objective_functions = ObjectiveOption(Objective.Lagrange.MINIMIZE_TORQUE, weight=10) # Dynamics dynamics = DynamicsTypeOption(DynamicsType.TORQUE_DRIVEN) # Path constraint x_bounds = BoundsOption(QAndQDotBounds(biorbd_model)) x_bounds[:, [0, -1]] = 0 x_bounds[1, -1] = 3.14 # Initial guess x_init = InitialGuessOption([0] * (n_q + n_qdot)) # Define control path constraint u_bounds = BoundsOption([[tau_min] * n_tau, [tau_max] * n_tau]) u_bounds[1, :] = 0 u_init = InitialGuessOption([tau_init] * n_tau) # Define the parameter to optimize # Give the parameter some min and max bounds parameters = ParameterList() bound_gravity = Bounds(min_bound=min_g, max_bound=max_g, interpolation=InterpolationType.CONSTANT) # and an initial condition initial_gravity = InitialGuess((min_g + max_g) / 2) parameter_objective_functions = ObjectiveOption( my_target_function, weight=10, quadratic=True, custom_type=Objective.Parameter, target=target_g ) parameters.add( "gravity_z", # The name of the parameter my_parameter_function, # The function that modifies the biorbd model initial_gravity, # The initial guess bound_gravity, # The bounds size=1, # The number of elements this particular parameter vector has penalty_list=parameter_objective_functions, # Objective of constraint for this particular parameter extra_value=1, # You can define as many extra arguments as you want ) return OptimalControlProgram( biorbd_model, dynamics, number_shooting_points, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, parameters=parameters, )
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 test_muscle_activation_and_contacts_tracking(ode_solver): # Load muscle_activations_contact_tracker bioptim_folder = TestUtils.bioptim_folder() contact = TestUtils.load_module( bioptim_folder + "/examples/muscle_driven_with_contact/muscle_activations_contacts_tracker.py" ) ode_solver = ode_solver() # Define the problem model_path = bioptim_folder + "/examples/muscle_driven_with_contact/2segments_4dof_2contacts_1muscle.bioMod" biorbd_model = biorbd.Model(model_path) final_time = 0.1 n_shooting = 5 # Generate random data to fit np.random.seed(42) contact_forces_ref = np.random.rand(biorbd_model.nbContacts(), n_shooting) muscle_activations_ref = np.random.rand(biorbd_model.nbMuscles(), n_shooting + 1) ocp = contact.prepare_ocp( model_path, final_time, n_shooting, muscle_activations_ref[:, :-1], contact_forces_ref, ode_solver=ode_solver, ) sol = ocp.solve() # 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], 1.2080146471135251) # Check constraints g = np.array(sol.constraints) np.testing.assert_equal(g.shape, (40, 1)) np.testing.assert_almost_equal(g, np.zeros((40, 1)), decimal=6) # Check some of the results q, qdot, tau, mus_controls = sol.states["q"], sol.states["qdot"], sol.controls["tau"], sol.controls["muscles"] # initial and final position np.testing.assert_almost_equal(q[:, 0], np.array([0.0, 0.0, -0.75, 0.75])) np.testing.assert_almost_equal(q[:, -1], np.array([0.01785865, -0.01749107, -0.8, 0.8]), decimal=5) # initial and final velocities np.testing.assert_almost_equal(qdot[:, 0], np.array([0.0, 0.0, 0.0, 0.0])) np.testing.assert_almost_equal(qdot[:, -1], np.array([0.5199767, -0.535388, -1.49267023, 1.4926703]), decimal=5) # initial and final controls np.testing.assert_almost_equal(tau[:, 0], np.array([5.3773376, 127.6205162, -21.9933179, 1.3644034]), decimal=5) np.testing.assert_almost_equal(tau[:, -1], np.array([57.203734, 72.3153286, -7.4076227, 1.2641681]), decimal=5) np.testing.assert_almost_equal(mus_controls[:, 0], np.array([0.18722964]), decimal=5) np.testing.assert_almost_equal(mus_controls[:, -1], np.array([0.29591125]), decimal=5) # save and load TestUtils.save_and_load(sol, ocp, False) # simulate TestUtils.simulate(sol)
def prepare_ocp(biorbd_model_path, number_shooting_points, final_time, loop_from_constraint, ode_solver=OdeSolver.RK): # --- 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 = ObjectiveOption(Objective.Lagrange.MINIMIZE_TORQUE, weight=100) # Dynamics dynamics = DynamicsTypeOption(DynamicsType.TORQUE_DRIVEN) # Constraints constraints = ConstraintList() constraints.add(Constraint.ALIGN_MARKERS, instant=Instant.MID, first_marker_idx=0, second_marker_idx=2) constraints.add(Constraint.TRACK_STATE, instant=Instant.MID, states_idx=2) constraints.add(Constraint.ALIGN_MARKERS, instant=Instant.END, first_marker_idx=0, second_marker_idx=1) # Path constraint x_bounds = BoundsOption(QAndQDotBounds(biorbd_model)) x_bounds.min[2:6, -1] = [1.57, 0, 0, 0] x_bounds.max[2:6, -1] = [1.57, 0, 0, 0] # Initial guess x_init = InitialConditionsOption([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 = InitialConditionsOption([tau_init] * biorbd_model.nbGeneralizedTorque()) # ------------- # # A state 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 state_transitions = StateTransitionList() if loop_from_constraint: state_transitions.add(StateTransition.CYCLIC, weight=0) else: state_transitions.add(StateTransition.CYCLIC, weight=10000) 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, state_transitions=state_transitions, )
def test_forward_dynamics_with_external_forces(): m = biorbd.Model("../../models/pyomecaman_withActuators.bioMod") q = np.array([i * 1.1 for i in range(m.nbQ())]) qdot = np.array([i * 1.1 for i in range(m.nbQ())]) tau = np.array([i * 1.1 for i in range(m.nbQ())]) # With external forces if biorbd.currentLinearAlgebraBackend() == 1: from casadi import Function, MX sv1 = MX((11.1, 22.2, 33.3, 44.4, 55.5, 66.6)) sv2 = MX((11.1 * 2, 22.2 * 2, 33.3 * 2, 44.4 * 2, 55.5 * 2, 66.6 * 2)) else: sv1 = np.array((11.1, 22.2, 33.3, 44.4, 55.5, 66.6)) sv2 = np.array( (11.1 * 2, 22.2 * 2, 33.3 * 2, 44.4 * 2, 55.5 * 2, 66.6 * 2)) f_ext = biorbd.VecBiorbdSpatialVector([ biorbd.SpatialVector(sv1), biorbd.SpatialVector(sv2), ]) if biorbd.currentLinearAlgebraBackend() == 1: q_sym = MX.sym("q", m.nbQ(), 1) qdot_sym = MX.sym("qdot", m.nbQdot(), 1) tau_sym = MX.sym("tau", m.nbGeneralizedTorque(), 1) ForwardDynamics = Function( "ForwardDynamics", [q_sym, qdot_sym, tau_sym], [m.ForwardDynamics(q_sym, qdot_sym, tau_sym, f_ext).to_mx()], ["q_sym", "qdot_sym", "tau_sym"], ["qddot"], ).expand() qddot = ForwardDynamics(q, qdot, tau) qddot = np.array(qddot)[:, 0] elif biorbd.currentLinearAlgebraBackend() == 0: # if Eigen backend is used qddot = m.ForwardDynamics(q, qdot, tau, f_ext).to_array() qddot_expected = np.array([ 8.8871711208009998, -13.647827029817943, -33.606145294752132, 16.922669487341341, -21.882821189868423, 41.15364990805439, 68.892537246574463, -324.59756885799197, -447.99217990207387, 18884.241415786601, -331.24622725851572, 1364.7620674666462, 3948.4748602722384, ]) np.testing.assert_almost_equal(qddot, qddot_expected)
def prepare_ocp(biorbd_model_path, final_time, number_shooting_points, use_SX=False): # --- Options --- # # Model path biorbd_model = biorbd.Model(biorbd_model_path) tau_min, tau_max, tau_init = -1, 1, 0 muscle_min, muscle_max, muscle_init = 0, 1, 0.5 # Add objective functions objective_functions = ObjectiveList() objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE) objective_functions.add(Objective.Lagrange.MINIMIZE_MUSCLES_CONTROL) objective_functions.add(Objective.Mayer.ALIGN_MARKERS, first_marker_idx=0, second_marker_idx=5) # Dynamics dynamics = DynamicsTypeList() dynamics.add(DynamicsType.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN) # Path constraint x_bounds = BoundsList() x_bounds.add(QAndQDotBounds(biorbd_model)) x_bounds[0].min[:, 0] = (0.07, 1.4, 0, 0) x_bounds[0].max[:, 0] = (0.07, 1.4, 0, 0) # Initial guess x_init = InitialConditionsList() x_init.add([1.57] * biorbd_model.nbQ() + [0] * biorbd_model.nbQdot()) # 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 = InitialConditionsList() u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque() + [muscle_init] * biorbd_model.nbMuscleTotal()) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, number_shooting_points, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, use_SX=use_SX, )
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_MUSCLES_CONTROL, target=muscle_activations_ref) objective_functions.add(ObjectiveFcn.Lagrange.TRACK_CONTACT_FORCES, target=contact_forces_ref) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE, weight=0.001) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_ALL_CONTROLS, weight=0.001) # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN_WITH_CONTACT) # 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, final_time, number_shooting_points, nb_threads, use_SX=False): # --- Options --- # biorbd_model = biorbd.Model(biorbd_model_path) torque_min, torque_max, torque_init = -100, 100, 0 n_q = biorbd_model.nbQ() n_qdot = biorbd_model.nbQdot() n_tau = biorbd_model.nbGeneralizedTorque() data_to_track = np.zeros((number_shooting_points + 1, n_q + n_qdot)) data_to_track[:, 1] = 3.14 # Add objective functions objective_functions = ObjectiveList() objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, weight=100.0) objective_functions.add(Objective.Lagrange.MINIMIZE_STATE, weight=1.0) objective_functions.add( Objective.Mayer.MINIMIZE_STATE, weight=50000.0, target=data_to_track.T, instant=Instant.END, ) # Dynamics dynamics = DynamicsTypeList() dynamics.add(DynamicsType.TORQUE_DRIVEN) # Path constraint x_bounds = BoundsList() x_bounds.add(QAndQDotBounds(biorbd_model)) x_bounds[0].min[:, 0] = 0 x_bounds[0].max[:, 0] = 0 # Initial guess x_init = InitialConditionsList() x_init.add([0] * (n_q + n_qdot)) # Define control path constraint u_bounds = BoundsList() u_bounds.add( [[torque_min] * n_tau, [torque_max] * n_tau,] ) u_bounds[0].min[n_tau - 1, :] = 0 u_bounds[0].max[n_tau - 1, :] = 0 u_init = InitialConditionsList() u_init.add([torque_init] * n_tau) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, number_shooting_points, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, nb_threads=nb_threads, use_SX=use_SX, )
def impact(ocp, transition: PhaseTransition) -> MX: """ A discontinuous function that simulates an inelastic impact of a new contact point Parameters ---------- ocp: OptimalControlProgram A reference to the ocp transition: PhaseTransition A reference to the phase transition Returns ------- The difference between the last and first node after applying the impulse equations """ if ( ocp.nlp[transition.phase_pre_idx].states.shape != ocp.nlp[(transition.phase_pre_idx + 1) % ocp.n_phases].states.shape ): raise RuntimeError( "Impact transition without same nx is not possible, please provide a custom phase transition" ) # Aliases nlp_pre, nlp_post = PhaseTransitionFunctions.Functions.__get_nlp_pre_and_post(ocp, transition.phase_pre_idx) n_q = len(nlp_pre.states["q"]) n_qdot = len(nlp_pre.states["qdot"]) q = DynamicsFunctions.get(nlp_pre.states["q"], nlp_pre.X[-1]) qdot_pre = DynamicsFunctions.get(nlp_pre.states["qdot"], nlp_pre.X[-1]) if nlp_post.model.nbContacts() == 0: warn("The chosen model does not have any contact") # A new model is loaded here so we can use pre Qdot with post model, this is a hack and should be dealt # a better way (e.g. create a supplementary variable in v that link the pre and post phase with a # constraint. The transition would therefore apply to node_0 and node_1 (with an augmented ns) model = biorbd.Model(nlp_post.model.path().absolutePath().to_string()) func = biorbd.to_casadi_func( "impulse_direct", model.ComputeConstraintImpulsesDirect, nlp_pre.states["q"].mx, nlp_pre.states["qdot"].mx, ) qdot_post = func(q, qdot_pre) qdot_post = nlp_post.variable_mappings["qdot"].to_first.map(qdot_post) val = [] for key in nlp_pre.states: if key != "qdot": # Continuity constraint var_pre = DynamicsFunctions.get(nlp_pre.states[key], nlp_pre.X[-1]) var_post = DynamicsFunctions.get(nlp_post.states[key], nlp_post.X[0]) val = vertcat(val, var_pre - var_post) else: var_post = DynamicsFunctions.get(nlp_post.states[key], nlp_post.X[0]) val = vertcat(val, qdot_post - var_post) return val
def prepare_ocp(biorbd_model_path="cubeSym.bioMod", show_online_optim=False, ode_solver=OdeSolver.RK): # --- Options --- # # Model path biorbd_model = biorbd.Model(biorbd_model_path) # Problem parameters number_shooting_points = 30 final_time = 2 torque_min, torque_max, torque_init = -100, 100, 0 all_generalized_mapping = BidirectionalMapping(Mapping([0, 1, 2, 2], [3]), Mapping([0, 1, 2])) # Add objective functions objective_functions = {"type": Objective.Lagrange.MINIMIZE_TORQUE, "weight": 100} # Dynamics variable_type = ProblemType.torque_driven # Constraints constraints = ( {"type": Constraint.ALIGN_MARKERS, "instant": Instant.START, "first_marker": 0, "second_marker": 1,}, {"type": Constraint.ALIGN_MARKERS, "instant": Instant.END, "first_marker": 0, "second_marker": 2,}, ) # Path constraint X_bounds = QAndQDotBounds(biorbd_model, all_generalized_mapping) for i in range(3, 6): X_bounds.first_node_min[i] = 0 X_bounds.last_node_min[i] = 0 X_bounds.first_node_max[i] = 0 X_bounds.last_node_max[i] = 0 # Initial guess X_init = InitialConditions([0] * all_generalized_mapping.reduce.len * 2) # Define control path constraint U_bounds = Bounds( [torque_min] * all_generalized_mapping.reduce.len, [torque_max] * all_generalized_mapping.reduce.len, ) U_init = InitialConditions([torque_init] * all_generalized_mapping.reduce.len) # ------------- # return OptimalControlProgram( biorbd_model, variable_type, number_shooting_points, final_time, objective_functions, X_init, U_init, X_bounds, U_bounds, constraints, ode_solver=ode_solver, all_generalized_mapping=all_generalized_mapping, show_online_optim=show_online_optim, )