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 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, )