def prepare_ocp( biorbd_model: biorbd.Model, final_time: float, n_shooting: int, markers_ref: np.ndarray, excitations_ref: np.ndarray, q_ref: np.ndarray, use_residual_torque: bool, kin_data_to_track: str = "markers", ode_solver: OdeSolver = OdeSolver.COLLOCATION(), ) -> OptimalControlProgram: """ Prepare the ocp to solve Parameters ---------- biorbd_model: biorbd.Model The loaded biorbd model final_time: float The time at final node n_shooting: int The number of shooting points markers_ref: np.ndarray The marker to track if 'markers' is chosen in kin_data_to_track excitations_ref: np.ndarray The muscle excitation (EMG) to track q_ref: np.ndarray The state to track if 'q' is chosen in kin_data_to_track kin_data_to_track: str The type of kin data to track ('markers' or 'q') use_residual_torque: bool If residual torque are present or not in the dynamics ode_solver: OdeSolver The ode solver to use Returns ------- The OptimalControlProgram ready to solve """ # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.TRACK_CONTROL, key="muscles", target=excitations_ref) if use_residual_torque: objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau") if kin_data_to_track == "markers": objective_functions.add(ObjectiveFcn.Lagrange.TRACK_MARKERS, node=Node.ALL, weight=100, target=markers_ref) elif kin_data_to_track == "q": objective_functions.add( ObjectiveFcn.Lagrange.TRACK_STATE, key="q", weight=100, node=Node.ALL, target=q_ref, index=range(biorbd_model.nbQ()), ) else: raise RuntimeError("Wrong choice of kin_data_to_track") # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.MUSCLE_DRIVEN, with_excitations=True, with_residual_torque=use_residual_torque) # Path constraint x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model)) # Due to unpredictable movement of the forward dynamics that generated the movement, the bound must be larger x_bounds[0].min[[0, 1], :] = -2 * np.pi x_bounds[0].max[[0, 1], :] = 2 * np.pi # Add muscle to the bounds activation_min, activation_max, activation_init = 0, 1, 0.5 x_bounds[0].concatenate( Bounds([activation_min] * biorbd_model.nbMuscles(), [activation_max] * biorbd_model.nbMuscles())) x_bounds[0][(biorbd_model.nbQ() + biorbd_model.nbQdot()):, 0] = excitations_ref[:, 0] # Initial guess x_init = InitialGuessList() x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()) + [0] * biorbd_model.nbMuscles()) # Define control path constraint excitation_min, excitation_max, excitation_init = 0, 1, 0.5 u_bounds = BoundsList() u_init = InitialGuessList() if use_residual_torque: tau_min, tau_max, tau_init = -100, 100, 0 u_bounds.add( [tau_min] * biorbd_model.nbGeneralizedTorque() + [excitation_min] * biorbd_model.nbMuscles(), [tau_max] * biorbd_model.nbGeneralizedTorque() + [excitation_max] * biorbd_model.nbMuscles(), ) u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque() + [excitation_init] * biorbd_model.nbMuscles()) else: u_bounds.add([excitation_min] * biorbd_model.nbMuscles(), [excitation_max] * biorbd_model.nbMuscles()) u_init.add([excitation_init] * biorbd_model.nbMuscles()) # ------------- # 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, final_time: float, n_shooting: int, fatigue_type: str, ode_solver: OdeSolver = OdeSolver.COLLOCATION(), torque_level: int = 0, ) -> OptimalControlProgram: """ Prepare the ocp Parameters ---------- biorbd_model_path: str The path to the bioMod final_time: float The time at the final node n_shooting: int The number of shooting points fatigue_type: str The type of dynamics to apply ("xia" or "michaud") ode_solver: OdeSolver The ode solver to use torque_level: int 0 no residual torque, 1 with residual torque, 2 with fatigable residual torque Returns ------- The OptimalControlProgram ready to be solved """ biorbd_model = biorbd.Model(biorbd_model_path) n_tau = biorbd_model.nbGeneralizedTorque() n_muscles = biorbd_model.nbMuscleTotal() tau_min, tau_max = -10, 10 # Define fatigue parameters for each muscle and residual torque fatigue_dynamics = FatigueList() for i in range(n_muscles): if fatigue_type == "xia": fatigue_dynamics.add(XiaFatigue(LD=10, LR=10, F=0.01, R=0.002), state_only=False) elif fatigue_type == "michaud": fatigue_dynamics.add( MichaudFatigue( LD=100, LR=100, F=0.005, R=0.005, effort_threshold=0.2, effort_factor=0.001, stabilization_factor=10 ), state_only=True, ) elif fatigue_type == "effort": fatigue_dynamics.add(EffortPerception(effort_threshold=0.2, effort_factor=0.001)) else: raise ValueError("fatigue_type not implemented") if torque_level >= 2: for i in range(n_tau): if fatigue_type == "xia": fatigue_dynamics.add( XiaTauFatigue( XiaFatigue(LD=10, LR=10, F=5, R=10, scaling=tau_min), XiaFatigue(LD=10, LR=10, F=5, R=10, scaling=tau_max), ), state_only=False, ) elif fatigue_type == "michaud": fatigue_dynamics.add( MichaudTauFatigue( MichaudFatigue( LD=10, LR=10, F=5, R=10, effort_threshold=0.15, effort_factor=0.07, scaling=tau_min ), MichaudFatigue( LD=10, LR=10, F=5, R=10, effort_threshold=0.15, effort_factor=0.07, scaling=tau_max ), ), state_only=False, ) elif fatigue_type == "effort": fatigue_dynamics.add( TauEffortPerception( EffortPerception(effort_threshold=0.15, effort_factor=0.001, scaling=tau_min), EffortPerception(effort_threshold=0.15, effort_factor=0.001, scaling=tau_max), ), state_only=False, ) else: raise ValueError("fatigue_type not implemented") # Dynamics dynamics = Dynamics(DynamicsFcn.MUSCLE_DRIVEN, expand=False, fatigue=fatigue_dynamics, with_torque=torque_level > 0) # Add objective functions objective_functions = ObjectiveList() if torque_level > 0: objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau") objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="muscles", weight=100) objective_functions.add( ObjectiveFcn.Mayer.SUPERIMPOSE_MARKERS, first_marker="target", second_marker="COM_hand", weight=0.01 ) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_FATIGUE, key="muscles", weight=1000) # Constraint constraint = Constraint( ConstraintFcn.SUPERIMPOSE_MARKERS, first_marker="target", second_marker="COM_hand", node=Node.END, axes=[Axis.X, Axis.Y], ) x_bounds = QAndQDotBounds(biorbd_model) x_bounds[:, 0] = (0.07, 1.4, 0, 0) x_bounds.concatenate(FatigueBounds(fatigue_dynamics, fix_first_frame=True)) x_init = InitialGuess([1.57] * biorbd_model.nbQ() + [0] * biorbd_model.nbQdot()) x_init.concatenate(FatigueInitialGuess(fatigue_dynamics)) # Define control path constraint u_bounds = Bounds([tau_min] * n_tau, [tau_max] * n_tau) if torque_level == 1 else Bounds() u_bounds.concatenate(FatigueBounds(fatigue_dynamics, variable_type=VariableType.CONTROLS)) u_init = InitialGuess([0] * n_tau) if torque_level == 1 else InitialGuess() u_init.concatenate(FatigueInitialGuess(fatigue_dynamics, variable_type=VariableType.CONTROLS)) return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraint, ode_solver=ode_solver, use_sx=False, n_threads=8, )
def test_michaud_fatigable_muscles(): bioptim_folder = TestUtils.bioptim_folder() fatigue = TestUtils.load_module( f"{bioptim_folder}/examples/fatigue/static_arm_with_fatigue.py") model_path = f"{bioptim_folder}/examples/fatigue/models/arm26_constant.bioMod" ocp = fatigue.prepare_ocp( biorbd_model_path=model_path, final_time=0.9, n_shooting=5, fatigue_type="michaud", ode_solver=OdeSolver.COLLOCATION(), torque_level=1, ) sol = ocp.solve() # Check objective function value f = np.array(sol.cost) np.testing.assert_equal(f.shape, (1, 1)) if platform.system() == "Linux": np.testing.assert_almost_equal(f[0, 0], 16.32400654587575) # Check constraints g = np.array(sol.constraints) np.testing.assert_equal(g.shape, (702, 1)) np.testing.assert_almost_equal(g, np.zeros((702, 1))) # Check some of the results states, controls = sol.states, sol.controls q, qdot, ma, mr, mf = states["q"], states["qdot"], states[ "muscles_ma"], states["muscles_mr"], states["muscles_mf"] tau, muscles = controls["tau"], controls["muscles"] # initial and final position np.testing.assert_almost_equal(q[:, 0], np.array((0.07, 1.4))) np.testing.assert_almost_equal(q[:, -1], np.array( (1.64470726, 2.25033212))) # initial and final velocities np.testing.assert_almost_equal(qdot[:, 0], np.array((0, 0))) np.testing.assert_almost_equal(ma[:, 0], np.array((0, 0, 0, 0, 0, 0))) np.testing.assert_almost_equal(mr[:, 0], np.array((1, 1, 1, 1, 1, 1))) np.testing.assert_almost_equal(mf[:, 0], np.array((0, 0, 0, 0, 0, 0))) np.testing.assert_almost_equal( mf[:, -1], np.array((0, 3.59773278e-04, 3.59740895e-04, 0, 0, 0)), ) if platform.system() == "Linux": np.testing.assert_almost_equal(qdot[:, -1], np.array((-3.8913551, 3.68787122))) np.testing.assert_almost_equal( ma[:, -1], np.array((0.03924828, 0.01089071, 0.00208428, 0.05019898, 0.05019898, 0.00058203))) np.testing.assert_almost_equal( mr[:, -1], np.array((0.96071394, 0.98795266, 0.99699829, 0.9496845, 0.9496845, 0.99917771))) np.testing.assert_almost_equal(tau[:, 0], np.array((0.96697626, 0.7686893))) np.testing.assert_almost_equal(tau[:, -2], np.array((0.59833412, -0.73455049))) np.testing.assert_almost_equal( muscles[:, 0], np.array((1.25202085e-07, 3.21982969e-01, 2.28408549e-01, 3.74330449e-07, 3.74330448e-07, 1.69987512e-01)), ) np.testing.assert_almost_equal( muscles[:, -2], np.array((0.0441982, 0.00474236, 0.0009076, 0.04843388, 0.04843388, 0.00025345)), ) # save and load TestUtils.save_and_load(sol, ocp, True) # simulate TestUtils.simulate(sol)
def test_xia_fatigable_muscles(): bioptim_folder = TestUtils.bioptim_folder() fatigue = TestUtils.load_module( f"{bioptim_folder}/examples/fatigue/static_arm_with_fatigue.py") model_path = f"{bioptim_folder}/examples/fatigue/models/arm26_constant.bioMod" ocp = fatigue.prepare_ocp( biorbd_model_path=model_path, final_time=0.9, n_shooting=5, fatigue_type="xia", ode_solver=OdeSolver.COLLOCATION(), torque_level=1, ) 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], 19.770521758810368) # Check constraints g = np.array(sol.constraints) np.testing.assert_equal(g.shape, (552, 1)) np.testing.assert_almost_equal(g, np.zeros((552, 1))) # Check some of the results states, controls = sol.states, sol.controls q, qdot, ma, mr, mf = states["q"], states["qdot"], states[ "muscles_ma"], states["muscles_mr"], states["muscles_mf"] tau, muscles = controls["tau"], controls["muscles"] # initial and final position np.testing.assert_almost_equal(q[:, 0], np.array((0.07, 1.4))) np.testing.assert_almost_equal(q[:, -1], np.array( (1.64470726, 2.25033212))) # initial and final velocities np.testing.assert_almost_equal(qdot[:, 0], np.array((0, 0))) np.testing.assert_almost_equal(qdot[:, -1], np.array((-2.93853331, 3.00564551))) # fatigue parameters np.testing.assert_almost_equal(ma[:, 0], np.array((0, 0, 0, 0, 0, 0))) np.testing.assert_almost_equal( ma[:, -1], np.array((0.00739128, 0.00563555, 0.00159309, 0.02418655, 0.02418655, 0.00041913))) np.testing.assert_almost_equal(mr[:, 0], np.array((1, 1, 1, 1, 1, 1))) np.testing.assert_almost_equal( mr[:, -1], np.array((0.99260018, 0.99281414, 0.99707397, 0.97566527, 0.97566527, 0.99904065))) np.testing.assert_almost_equal(mf[:, 0], np.array((0, 0, 0, 0, 0, 0))) np.testing.assert_almost_equal( mf[:, -1], np.array((8.54868154e-06, 1.55030599e-03, 1.33293886e-03, 1.48176210e-04, 1.48176210e-04, 5.40217808e-04)), ) # initial and final controls np.testing.assert_almost_equal(tau[:, 0], np.array( (0.80920008, 1.66855572))) np.testing.assert_almost_equal(tau[:, -2], np.array((0.81847388, -0.85234628))) np.testing.assert_almost_equal( muscles[:, 0], np.array((6.22395441e-08, 4.38966513e-01, 3.80781292e-01, 2.80532297e-07, 2.80532297e-07, 2.26601989e-01)), ) np.testing.assert_almost_equal( muscles[:, -2], np.array((8.86069119e-03, 1.17337666e-08, 1.28715148e-08, 2.02340603e-02, 2.02340603e-02, 2.16517945e-088)), ) # save and load TestUtils.save_and_load(sol, ocp, True) # simulate TestUtils.simulate(sol)
def test_effort_fatigable_muscles(): bioptim_folder = TestUtils.bioptim_folder() fatigue = TestUtils.load_module( f"{bioptim_folder}/examples/fatigue/static_arm_with_fatigue.py") model_path = f"{bioptim_folder}/examples/fatigue/models/arm26_constant.bioMod" ocp = fatigue.prepare_ocp( biorbd_model_path=model_path, final_time=0.9, n_shooting=5, fatigue_type="effort", ode_solver=OdeSolver.COLLOCATION(), torque_level=1, ) 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], 15.670921245579846) # Check constraints g = np.array(sol.constraints) np.testing.assert_equal(g.shape, (252, 1)) np.testing.assert_almost_equal(g, np.zeros((252, 1))) # Check some of the results states, controls = sol.states, sol.controls q, qdot, mf = states["q"], states["qdot"], states["muscles_mf"] tau, muscles = controls["tau"], controls["muscles"] # initial and final position np.testing.assert_almost_equal(q[:, 0], np.array((0.07, 1.4))) np.testing.assert_almost_equal(q[:, -1], np.array( (1.64470726, 2.25033212))) # 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.88775204, 3.6333437))) # fatigue parameters np.testing.assert_almost_equal(mf[:, 0], np.array((0, 0, 0, 0, 0, 0))) np.testing.assert_almost_equal( mf[:, -1], np.array((0, 3.59773278e-04, 3.59740895e-04, 0, 0, 0)), ) # initial and final controls np.testing.assert_almost_equal(tau[:, 0], np.array( (1.00151725, 0.75680955))) np.testing.assert_almost_equal(tau[:, -2], np.array((0.5258675, -0.65113292))) np.testing.assert_almost_equal( muscles[:, 0], np.array((-3.28714919e-09, 3.22449026e-01, 2.29706846e-01, 2.48558352e-08, 2.48558352e-08, 1.68035357e-01)), ) np.testing.assert_almost_equal( muscles[:, -2], np.array((3.86483779e-02, 1.10050544e-09, 2.74222976e-09, 4.25097691e-02, 4.25097691e-02, 6.56233979e-09)), ) # save and load TestUtils.save_and_load(sol, ocp, True) # simulate TestUtils.simulate(sol)
def prepare_ocp( biorbd_model_path: str, final_time: float, n_shooting: int, weight: float, ode_solver: OdeSolver = OdeSolver.COLLOCATION(), ) -> OptimalControlProgram: """ Prepare the ocp Parameters ---------- biorbd_model_path: str The path to the bioMod final_time: float The time at the final node n_shooting: int The number of shooting points weight: float The weight applied to the SUPERIMPOSE_MARKERS final objective function. The bigger this number is, the greater the model will try to reach the marker. This is in relation with the other objective functions 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.Lagrange.MINIMIZE_CONTROL, key="tau") objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="muscles") objective_functions.add(ObjectiveFcn.Mayer.SUPERIMPOSE_MARKERS, first_marker="target", second_marker="COM_hand", weight=weight) # 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] = (0.07, 1.4, 0, 0) # Initial guess x_init = InitialGuessList() x_init.add([1.57] * biorbd_model.nbQ() + [0] * biorbd_model.nbQdot()) # Define control path constraint muscle_min, muscle_max, muscle_init = 0, 1, 0.5 tau_min, tau_max, tau_init = -1, 1, 0 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, ode_solver=ode_solver, )