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 prepare_ocp( biorbd_model_path, final_time, number_shooting_points, min_g, max_g, target_g, ode_solver=OdeSolver.RK, use_SX=False ): # --- 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 = Objective(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=10) # Dynamics dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN) # Path constraint x_bounds = QAndQDotBounds(biorbd_model) x_bounds[:, [0, -1]] = 0 x_bounds[1, -1] = 3.14 # Initial guess x_init = InitialGuess([0] * (n_q + n_qdot)) # Define control path constraint u_bounds = Bounds([tau_min] * n_tau, [tau_max] * n_tau) u_bounds[1, :] = 0 u_init = InitialGuess([tau_init] * n_tau) # Define the parameter to optimize # Give the parameter some min and max bounds parameters = ParameterList() bound_gravity = Bounds(min_g, max_g, interpolation=InterpolationType.CONSTANT) # and an initial condition initial_gravity = InitialGuess((min_g + max_g) / 2) parameter_objective_functions = Objective( my_target_function, weight=10, quadratic=True, custom_type=ObjectiveFcn.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, # ObjectiveFcn 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, ode_solver=ode_solver, use_SX=use_SX, )
def prepare_ocp( biorbd_model_path: str, n_shooting: int, final_time: float, interpolation_type: InterpolationType = InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT, ) -> OptimalControlProgram: """ Prepare the ocp for the specified interpolation type Parameters ---------- biorbd_model_path: str The path to the biorbd model n_shooting: int The number of shooting point final_time: float The movement time interpolation_type: InterpolationType The requested InterpolationType Returns ------- The OCP fully prepared and ready to be solved """ # 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_TORQUE) # Dynamics dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN) # 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 constraints if interpolation_type == InterpolationType.CONSTANT: x_min = [-100] * (nq + nqdot) x_max = [100] * (nq + nqdot) x_bounds = Bounds(x_min, x_max, interpolation=InterpolationType.CONSTANT) u_min = [tau_min] * ntau u_max = [tau_max] * ntau u_bounds = Bounds(u_min, u_max, interpolation=InterpolationType.CONSTANT) elif interpolation_type == InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT: x_min = np.random.random((6, 3)) * (-10) - 5 x_max = np.random.random((6, 3)) * 10 + 5 x_bounds = Bounds(x_min, x_max, interpolation=InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT) u_min = np.random.random((3, 3)) * tau_min + tau_min / 2 u_max = np.random.random((3, 3)) * tau_max + tau_max / 2 u_bounds = Bounds(u_min, u_max, interpolation=InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT) elif interpolation_type == InterpolationType.LINEAR: x_min = np.random.random((6, 2)) * (-10) - 5 x_max = np.random.random((6, 2)) * 10 + 5 x_bounds = Bounds(x_min, x_max, interpolation=InterpolationType.LINEAR) u_min = np.random.random((3, 2)) * tau_min + tau_min / 2 u_max = np.random.random((3, 2)) * tau_max + tau_max / 2 u_bounds = Bounds(u_min, u_max, interpolation=InterpolationType.LINEAR) elif interpolation_type == InterpolationType.EACH_FRAME: x_min = np.random.random((nq + nqdot, n_shooting + 1)) * (-10) - 5 x_max = np.random.random((nq + nqdot, n_shooting + 1)) * 10 + 5 x_bounds = Bounds(x_min, x_max, interpolation=InterpolationType.EACH_FRAME) u_min = np.random.random((ntau, n_shooting)) * tau_min + tau_min / 2 u_max = np.random.random((ntau, n_shooting)) * tau_max + tau_max / 2 u_bounds = Bounds(u_min, u_max, interpolation=InterpolationType.EACH_FRAME) elif interpolation_type == InterpolationType.SPLINE: spline_time = np.hstack((0, np.sort(np.random.random((3,)) * final_time), final_time)) x_min = np.random.random((nq + nqdot, 5)) * (-10) - 5 x_max = np.random.random((nq + nqdot, 5)) * 10 + 5 u_min = np.random.random((ntau, 5)) * tau_min + tau_min / 2 u_max = np.random.random((ntau, 5)) * tau_max + tau_max / 2 x_bounds = Bounds(x_min, x_max, interpolation=InterpolationType.SPLINE, t=spline_time) u_bounds = Bounds(u_min, u_max, interpolation=InterpolationType.SPLINE, t=spline_time) elif interpolation_type == InterpolationType.CUSTOM: # The custom functions refer to the ones at the beginning of the file. # For this particular instance, they emulate a Linear interpolation extra_params_x = {"n_elements": nq + nqdot, "n_shooting": n_shooting} extra_params_u = {"n_elements": ntau, "n_shooting": n_shooting} x_bounds = Bounds( custom_x_bounds_min, custom_x_bounds_max, interpolation=InterpolationType.CUSTOM, **extra_params_x ) u_bounds = Bounds( custom_u_bounds_min, custom_u_bounds_max, interpolation=InterpolationType.CUSTOM, **extra_params_u ) else: raise NotImplementedError("Not implemented yet") # Initial guess x_init = InitialGuess([0] * (nq + nqdot)) u_init = InitialGuess([tau_init] * ntau) return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, )
def prepare_ocp(biorbd_model_path, ode_solver=OdeSolver.RK4()) -> OptimalControlProgram: """ Prepare the program Parameters ---------- biorbd_model_path: str The path of the biorbd model 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) # Problem parameters n_shooting = 30 final_time = 2 tau_min, tau_max, tau_init = -100, 100, 0 # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE) objective_functions.add( custom_func_track_markers, custom_type=ObjectiveFcn.Mayer, node=Node.START, quadratic=True, first_marker="m0", second_marker="m1", weight=1000, ) objective_functions.add( custom_func_track_markers, custom_type=ObjectiveFcn.Mayer, node=Node.END, quadratic=True, first_marker="m0", second_marker="m2", weight=1000, ) # Dynamics dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN) # 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 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, ode_solver=ode_solver, )
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 = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE) objective_functions.add( custom_func_align_markers, custom_type=ObjectiveFcn.Mayer, node=Node.START, quadratic=True, first_marker_idx=0, second_marker_idx=1, weight=1000, ) objective_functions.add( custom_func_align_markers, custom_type=ObjectiveFcn.Mayer, node=Node.END, quadratic=True, first_marker_idx=0, second_marker_idx=2, weight=1000, ) # Dynamics dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN) # 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 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, number_shooting_points, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, ode_solver=ode_solver, )
def prepare_ocp( biorbd_model_path: str, ode_solver: OdeSolver = OdeSolver.IRK() ) -> OptimalControlProgram: """ Prepare the program Parameters ---------- biorbd_model_path: str The path of the biorbd model 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) # Problem parameters n_shooting = 30 final_time = 2 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(custom_func_track_markers, node=Node.START, first_marker="m0", second_marker="m1", method=0) constraints.add(custom_func_track_markers, node=Node.END, first_marker="m0", second_marker="m2", method=1) # 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 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, )
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 == "xia_stabilized": fatigue_dynamics.add( XiaFatigueStabilized(LD=10, LR=10, F=0.01, R=0.002, stabilization_factor=10), 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 == "xia_stabilized": fatigue_dynamics.add( XiaTauFatigue( XiaFatigueStabilized(LD=10, LR=10, F=5, R=10, stabilization_factor=10, scaling=tau_min), XiaFatigueStabilized(LD=10, LR=10, F=5, R=10, stabilization_factor=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_parameters( biorbd_model_path, final_time, n_shooting, optim_gravity, optim_mass, min_g, max_g, target_g, min_m, max_m, target_m, ode_solver=OdeSolver.RK4(), use_sx=False, ) -> 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 optim_gravity: bool If the gravity should be optimized optim_mass: bool If the mass should be optimized min_g: np.ndarray The minimal value for the gravity max_g: np.ndarray The maximal value for the gravity target_g: np.ndarray The target value for the gravity min_m: float The minimal value for the mass max_m: float The maximal value for the mass target_m: float The target value for the mass 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 --- # biorbd_model = biorbd.Model(biorbd_model_path) n_tau = biorbd_model.nbGeneralizedTorque() # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=10) objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_TIME, weight=10) # Dynamics dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN) # 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 tau_min, tau_max, tau_init = -300, 300, 0 u_bounds = Bounds([tau_min] * n_tau, [tau_max] * n_tau) u_bounds[1, :] = 0 u_init = InitialGuess([tau_init] * n_tau) # Define the parameter to optimize parameters = ParameterList() if optim_gravity: # Give the parameter some min and max bounds bound_gravity = Bounds(min_g, max_g, interpolation=InterpolationType.CONSTANT) # and an initial condition initial_gravity = InitialGuess((min_g + max_g) / 2) # and an objective function parameter_objective_functions = Objective( my_target_function, weight=1000, quadratic=False, custom_type=ObjectiveFcn.Parameter, target=target_g) parameters.add( "gravity_xyz", # 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= 3, # The number of elements this particular parameter vector has penalty_list= parameter_objective_functions, # ObjectiveFcn of constraint for this particular parameter scaling=np.array([1, 1, 10.0]), extra_value=1, # You can define as many extra arguments as you want ) if optim_mass: bound_mass = Bounds(min_m, max_m, interpolation=InterpolationType.CONSTANT) initial_mass = InitialGuess((min_m + max_m) / 2) mass_objective_functions = Objective( my_target_function, weight=100, quadratic=False, custom_type=ObjectiveFcn.Parameter, target=target_m) parameters.add( "mass", # The name of the parameter set_mass, # The function that modifies the biorbd model initial_mass, # The initial guess bound_mass, # The bounds size= 1, # The number of elements this particular parameter vector has penalty_list= mass_objective_functions, # ObjectiveFcn of constraint for this particular parameter scaling=np.array([10.0]), ) return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, parameters=parameters, ode_solver=ode_solver, use_sx=use_sx, )
Problem.configure_q_qdot(nlp, as_states=True, as_controls=False) Problem.configure_tau(nlp, as_states=False, as_controls=True) Problem.configure_forward_dyn_func(ocp, nlp, custom_dynamic) # Model path m = biorbd.Model("mass_point.bioMod") m.setGravity(biorbd.Vector3d(0, 0, 0)) # Add objective functions (high upward velocity at end point) objective_functions = Objective(ObjectiveFcn.Mayer.MINIMIZE_STATE, index=1, weight=-1) # Dynamics dynamics = Dynamics(custom_configure, dynamic_function=custom_dynamic) # Path constraint x_bounds = QAndQDotBounds(m) x_bounds[:, 0] = [0] * m.nbQ() + [0] * m.nbQdot() x_bounds.min[:, 1] = [-1] * m.nbQ() + [-100] * m.nbQdot() x_bounds.max[:, 1] = [1] * m.nbQ() + [100] * m.nbQdot() x_bounds.min[:, 2] = [-1] * m.nbQ() + [-100] * m.nbQdot() x_bounds.max[:, 2] = [1] * m.nbQ() + [100] * m.nbQdot() # Initial guess x_init = InitialGuess([0] * (m.nbQ() + m.nbQdot())) # Define control path constraint u_bounds = Bounds([-100] * m.nbGeneralizedTorque(), [0] * m.nbGeneralizedTorque())
def prepare_ocp( biorbd_model_path: str, final_time: float, n_shooting: int, ode_solver: OdeSolver = OdeSolver.RK4(), use_sx: bool = True, n_threads: int = 1, ) -> 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 ode_solver: OdeSolver = OdeSolver.RK4() Which type of OdeSolver to use use_sx: bool If the SX variable should be used instead of MX (can be extensive on RAM) n_threads: int The number of threads to use in the paralleling (1 = no parallel computing) 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 dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN) # 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=x_init, u_init=u_init, x_bounds=x_bounds, u_bounds=u_bounds, objective_functions=objective_functions, ode_solver=ode_solver, use_sx=use_sx, n_threads=n_threads, )
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, final_time: float, n_shooting: int, ode_solver: OdeSolver = OdeSolver.RK1(n_integration_steps=1), use_sx: bool = False, n_threads: int = 1, implicit_dynamics: bool = False, ) -> 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 ode_solver: OdeSolver = OdeSolver.RK4() Which type of OdeSolver to use use_sx: bool If the SX variable should be used instead of MX (can be extensive on RAM) n_threads: int The number of threads to use in the paralleling (1 = no parallel computing) implicit_dynamics: bool implicit Returns ------- The OptimalControlProgram ready to be solved """ biorbd_model = biorbd.Model(biorbd_model_path) objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau") # Dynamics dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN, implicit_dynamics=implicit_dynamics) # Path constraint tau_min, tau_max, tau_init = -100, 100, 0 # Be careful to let the accelerations not to much bounded to find the same solution in implicit dynamics if implicit_dynamics: qddot_min, qddot_max, qddot_init = -1000, 1000, 0 x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model)) x_bounds[0][:, [0, -1]] = 0 x_bounds[0][1, -1] = 3.14 # Initial guess n_q = biorbd_model.nbQ() n_qdot = biorbd_model.nbQdot() n_qddot = biorbd_model.nbQddot() n_tau = biorbd_model.nbGeneralizedTorque() x_init = InitialGuess([0] * (n_q + n_qdot)) # Define control path constraint # There are extra controls in implicit dynamics which are joint acceleration qddot. if implicit_dynamics: u_bounds = Bounds([tau_min] * n_tau + [qddot_min] * n_qddot, [tau_max] * n_tau + [qddot_max] * n_qddot) else: u_bounds = Bounds([tau_min] * n_tau, [tau_max] * n_tau) u_bounds[1, :] = 0 # Prevent the model from actively rotate if implicit_dynamics: u_init = InitialGuess([0] * (n_tau + n_qddot)) else: u_init = InitialGuess([0] * n_tau) 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, ode_solver=ode_solver, use_sx=use_sx, n_threads=n_threads, )
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_TORQUE_DERIVATIVE) # Dynamics dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN) # 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 prepare_ocp( biorbd_model_path, number_shooting_points, final_time, initial_guess=InterpolationType.CONSTANT, ode_solver=OdeSolver.RK, ): # --- 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_TORQUE, weight=100) # Dynamics dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN) # Constraints constraints = ConstraintList() constraints.add(ConstraintFcn.ALIGN_MARKERS, node=Node.START, first_marker_idx=0, second_marker_idx=1) constraints.add(ConstraintFcn.ALIGN_MARKERS, node=Node.END, first_marker_idx=0, second_marker_idx=2) # 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, number_shooting_points + 1)) u = np.random.random((ntau, number_shooting_points)) 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)), "nb_shooting": number_shooting_points } extra_params_u = { "my_values": np.random.random((ntau, 2)), "nb_shooting": number_shooting_points } 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, number_shooting_points, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, ode_solver=ode_solver, )