def prepare_ocp(biorbd_model_path, phase_time, n_shooting, min_bound, max_bound): # --- Options --- # # 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 dof_mapping = BiMappingList() dof_mapping.add("tau", [None, None, None, 0], [3]) # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_PREDICTED_COM_HEIGHT) # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.MUSCLE_DRIVEN, with_residual_torque=True, with_contact=True) # Constraints constraints = ConstraintList() constraints.add( ConstraintFcn.TRACK_CONTACT_FORCES, min_bound=min_bound, max_bound=max_bound, node=Node.ALL_SHOOTING, contact_index=1, ) constraints.add( ConstraintFcn.TRACK_CONTACT_FORCES, min_bound=min_bound, max_bound=max_bound, node=Node.ALL_SHOOTING, contact_index=2, ) # 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] * len(dof_mapping["tau"].to_first) + [activation_min] * biorbd_model.nbMuscleTotal(), [tau_max] * len(dof_mapping["tau"].to_first) + [activation_max] * biorbd_model.nbMuscleTotal(), ) u_init = InitialGuessList() u_init.add([tau_init] * len(dof_mapping["tau"].to_first) + [activation_init] * biorbd_model.nbMuscleTotal()) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, n_shooting, phase_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints=constraints, variable_mappings=dof_mapping, )
def prepare_ocp( biorbd_model: tuple, final_time: list, nb_shooting: list, markers_ref: list, grf_ref: list, q_ref: list, qdot_ref: list, M_ref: list, CoP: list, nb_threads: int, ) -> OptimalControlProgram: """ Prepare the ocp Parameters ---------- biorbd_model: tuple Tuple of bioMod (1 bioMod for each phase) final_time: list List of the time at the final node. The length of the list corresponds to the phase number nb_shooting: list List of the number of shooting points markers_ref: list List of the array of markers trajectories to track grf_ref: list List of the array of ground reaction forces to track q_ref: list List of the array of joint trajectories. Those trajectories were computed using Kalman filter They are used as initial guess qdot_ref: list List of the array of joint velocities. Those velocities were computed using Kalman filter They are used as initial guess M_ref: list List of the array of ground reaction moments to track CoP: list List of the array of the measured center of pressure trajectory nb_threads:int The number of threads used Returns ------- The OptimalControlProgram ready to be solved """ # Problem parameters nb_phases = len(biorbd_model) nb_q = biorbd_model[0].nbQ() nb_qdot = biorbd_model[0].nbQdot() nb_tau = biorbd_model[0].nbGeneralizedTorque() nb_mus = biorbd_model[0].nbMuscleTotal() min_bound, max_bound = 0, np.inf torque_min, torque_max, torque_init = -1000, 1000, 0 activation_min, activation_max, activation_init = 1e-3, 1.0, 0.1 # Add objective functions markers_pelvis = [0, 1, 2, 3] markers_anat = [4, 9, 10, 11, 12, 17, 18] markers_tissus = [5, 6, 7, 8, 13, 14, 15, 16] markers_foot = [19, 20, 21, 22, 23, 24, 25] objective_functions = ObjectiveList() for p in range(nb_phases): objective_functions.add(ObjectiveFcn.Lagrange.TRACK_STATE, weight=1, index=range(nb_q), target=q_ref[p], phase=p, quadratic=True) objective_functions.add( ObjectiveFcn.Lagrange.TRACK_MARKERS, weight=1000, index=markers_anat, target=markers_ref[p][:, markers_anat, :], phase=p, quadratic=True, ) objective_functions.add( ObjectiveFcn.Lagrange.TRACK_MARKERS, weight=100000, index=markers_pelvis, target=markers_ref[p][:, markers_pelvis, :], phase=p, quadratic=True, ) objective_functions.add( ObjectiveFcn.Lagrange.TRACK_MARKERS, weight=100000, index=markers_foot, target=markers_ref[p][:, markers_foot, :], phase=p, quadratic=True, ) objective_functions.add( ObjectiveFcn.Lagrange.TRACK_MARKERS, weight=100, index=markers_tissus, target=markers_ref[p][:, markers_tissus, :], phase=p, quadratic=True, ) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=0.001, index=(10), phase=p, quadratic=True) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=1, index=(6, 7, 8, 9, 11), phase=p, quadratic=True) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_MUSCLES_CONTROL, weight=10, phase=p, quadratic=True) objective_functions.add( ObjectiveFcn.Lagrange.MINIMIZE_TORQUE_DERIVATIVE, weight=0.1, phase=p, quadratic=True) # --- track contact forces for the stance phase --- for p in range(nb_phases - 1): objective_functions.add( track_sum_contact_forces, # track contact forces grf=grf_ref[p], custom_type=ObjectiveFcn.Lagrange, node=Node.ALL, weight=0.1, quadratic=True, phase=p, ) for p in range(1, nb_phases - 1): objective_functions.add( track_sum_contact_moments, CoP=CoP[p], M_ref=M_ref[p], custom_type=ObjectiveFcn.Lagrange, node=Node.ALL, weight=0.01, quadratic=True, phase=p, ) # Dynamics dynamics = DynamicsList() for p in range(nb_phases - 1): dynamics.add( DynamicsFcn.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN_WITH_CONTACT, phase=p) dynamics.add(DynamicsFcn.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN, phase=3) # Constraints constraints = ConstraintList() constraints.add( # null speed for the first phase --> non sliding contact point ConstraintFcn.TRACK_MARKERS_VELOCITY, node=Node.START, index=26, phase=0, ) # --- phase flatfoot --- constraints.add( # positive vertical forces ConstraintFcn.CONTACT_FORCE, min_bound=min_bound, max_bound=max_bound, node=Node.ALL, contact_force_idx=(1, 2, 5), phase=1, ) constraints.add( # non slipping y ConstraintFcn.NON_SLIPPING, node=Node.ALL, normal_component_idx=(1, 2, 5), tangential_component_idx=4, static_friction_coefficient=0.2, phase=1, ) constraints.add( # non slipping x m5 ConstraintFcn.NON_SLIPPING, node=Node.ALL, normal_component_idx=(2, 5), tangential_component_idx=3, static_friction_coefficient=0.2, phase=1, ) constraints.add( # non slipping x heel ConstraintFcn.NON_SLIPPING, node=Node.ALL, normal_component_idx=1, tangential_component_idx=0, static_friction_coefficient=0.2, phase=1, ) constraints.add( # forces heel at zeros at the end of the phase get_last_contact_force_null, node=Node.ALL, contact_name="Heel_r", phase=1, ) # --- phase forefoot --- constraints.add( # positive vertical forces ConstraintFcn.CONTACT_FORCE, min_bound=min_bound, max_bound=max_bound, node=Node.ALL, contact_force_idx=(2, 4, 5), phase=2, ) constraints.add( ConstraintFcn.NON_SLIPPING, node=Node.ALL, normal_component_idx=(2, 4, 5), tangential_component_idx=1, static_friction_coefficient=0.2, phase=2, ) constraints.add( # non slipping x m1 ConstraintFcn.NON_SLIPPING, node=Node.ALL, normal_component_idx=2, tangential_component_idx=0, static_friction_coefficient=0.2, phase=2, ) constraints.add( get_last_contact_force_null, node=Node.ALL, contact_name="all", phase=2, ) # Phase Transitions phase_transitions = PhaseTransitionList() phase_transitions.add(PhaseTransitionFcn.IMPACT, phase_pre_idx=0) phase_transitions.add(PhaseTransitionFcn.IMPACT, phase_pre_idx=1) # Path constraint x_bounds = BoundsList() u_bounds = BoundsList() for p in range(nb_phases): x_bounds.add(bounds=QAndQDotBounds(biorbd_model[p])) u_bounds.add( [torque_min] * nb_tau + [activation_min] * nb_mus, [torque_max] * nb_tau + [activation_max] * nb_mus, ) # Initial guess x_init = InitialGuessList() u_init = InitialGuessList() n_shoot = 0 for p in range(nb_phases): init_x = np.zeros((nb_q + nb_qdot, nb_shooting[p] + 1)) init_x[:nb_q, :] = q_ref[p] init_x[nb_q:nb_q + nb_qdot, :] = qdot_ref[p] x_init.add(init_x, interpolation=InterpolationType.EACH_FRAME) init_u = [torque_init] * nb_tau + [activation_init] * nb_mus u_init.add(init_u) n_shoot += nb_shooting[p] # ------------- # return OptimalControlProgram( biorbd_model, dynamics, nb_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, phase_transitions=phase_transitions, n_threads=nb_threads, )
def prepare_ocp(model_path, phase_time, number_shooting_points, min_bound, max_bound): # --- Options --- # # 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 tau_mapping = BidirectionalMapping(Mapping([-1, -1, -1, 0]), Mapping([3])) # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_PREDICTED_COM_HEIGHT) # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN_WITH_CONTACT) # Constraints constraints = ConstraintList() constraints.add( ConstraintFcn.CONTACT_FORCE, min_bound=min_bound, max_bound=max_bound, node=Node.ALL, contact_force_idx=1, ) constraints.add( ConstraintFcn.CONTACT_FORCE, min_bound=min_bound, max_bound=max_bound, node=Node.ALL, contact_force_idx=2, ) # 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][:, 0] = pose_at_first_node + [0] * nb_qdot # Initial guess x_init = InitialGuessList() x_init.add(pose_at_first_node + [0] * nb_qdot) # Define control path constraint u_bounds = BoundsList() u_bounds.add([ [tau_min] * tau_mapping.reduce.len + [activation_min] * biorbd_model.nbMuscleTotal(), [tau_max] * tau_mapping.reduce.len + [activation_max] * biorbd_model.nbMuscleTotal(), ]) u_init = InitialGuessList() u_init.add([tau_init] * tau_mapping.reduce.len + [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, constraints, tau_mapping=tau_mapping, )
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(phase_time_constraint, use_parameter): # --- Inputs --- # final_time = (2, 5, 4) time_min = [1, 3, 0.1] time_max = [2, 4, 0.8] ns = (20, 30, 20) PROJECT_FOLDER = Path(__file__).parent / ".." biorbd_model_path = str( PROJECT_FOLDER) + "/examples/optimal_time_ocp/cube.bioMod" ode_solver = OdeSolver.RK # --- Options --- # nb_phases = len(ns) # Model path biorbd_model = (biorbd.Model(biorbd_model_path), biorbd.Model(biorbd_model_path), biorbd.Model(biorbd_model_path)) # Problem parameters tau_min, tau_max, tau_init = -100, 100, 0 # Add objective functions objective_functions = ObjectiveList() objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, weight=100, phase=0) objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, weight=100, phase=1) objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, weight=100, phase=2) # Dynamics dynamics = DynamicsTypeList() dynamics.add(DynamicsType.TORQUE_DRIVEN, phase=0) dynamics.add(DynamicsType.TORQUE_DRIVEN, phase=1) dynamics.add(DynamicsType.TORQUE_DRIVEN, phase=2) # Constraints constraints = ConstraintList() constraints.add(Constraint.ALIGN_MARKERS, node=Node.START, first_marker_idx=0, second_marker_idx=1, phase=0) constraints.add(Constraint.ALIGN_MARKERS, node=Node.END, first_marker_idx=0, second_marker_idx=2, phase=0) constraints.add(Constraint.ALIGN_MARKERS, node=Node.END, first_marker_idx=0, second_marker_idx=1, phase=1) constraints.add(Constraint.ALIGN_MARKERS, node=Node.END, first_marker_idx=0, second_marker_idx=2, phase=2) constraints.add( Constraint.TIME_CONSTRAINT, node=Node.END, minimum=time_min[0], maximum=time_max[0], phase=phase_time_constraint, ) # Path constraint x_bounds = BoundsList() x_bounds.add(QAndQDotBounds(biorbd_model[0])) # Phase 0 x_bounds.add(QAndQDotBounds(biorbd_model[0])) # Phase 1 x_bounds.add(QAndQDotBounds(biorbd_model[0])) # Phase 2 for bounds in x_bounds: for i in [1, 3, 4, 5]: bounds[i, [0, -1]] = 0 x_bounds[0][2, 0] = 0.0 x_bounds[2][2, [0, -1]] = [0.0, 1.57] # Initial guess x_init = InitialGuessList() x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot())) x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot())) x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot())) # Define control path constraint u_bounds = BoundsList() u_bounds.add([[tau_min] * biorbd_model[0].nbGeneralizedTorque(), [tau_max] * biorbd_model[0].nbGeneralizedTorque()]) u_bounds.add([[tau_min] * biorbd_model[0].nbGeneralizedTorque(), [tau_max] * biorbd_model[0].nbGeneralizedTorque()]) u_bounds.add([[tau_min] * biorbd_model[0].nbGeneralizedTorque(), [tau_max] * biorbd_model[0].nbGeneralizedTorque()]) u_init = InitialGuessList() u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque()) u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque()) u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque()) parameters = ParameterList() if use_parameter: def my_target_function(ocp, value, target_value): return value - target_value def my_parameter_function(biorbd_model, value, extra_value): biorbd_model.setGravity(biorbd.Vector3d(0, 0, 2)) min_g = -10 max_g = -6 target_g = -8 bound_gravity = Bounds(min_bound=min_g, max_bound=max_g, interpolation=InterpolationType.CONSTANT) initial_gravity = InitialGuess((min_g + max_g) / 2) parameter_objective_functions = ObjectiveOption( my_target_function, weight=10, quadratic=True, custom_type=Objective.Parameter, target_value=target_g) parameters.add( "gravity_z", my_parameter_function, initial_gravity, bound_gravity, size=1, penalty_list=parameter_objective_functions, extra_value=1, ) # ------------- # return OptimalControlProgram( biorbd_model[:nb_phases], dynamics, ns, final_time[:nb_phases], x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, ode_solver=ode_solver, parameters=parameters, )
def prepare_ocp(biorbd_model_path="HandSpinner.bioMod"): end_crank_idx = 0 hand_marker_idx = 18 # --- Options --- # # Model path biorbd_model = biorbd.Model(biorbd_model_path) tau_min, tau_max, tau_init = -100, 100, 0 muscle_min, muscle_max, muscle_init = 0, 1, 0.5 # Problem parameters number_shooting_points = 30 final_time = 1.0 # Add objective functions objective_functions = ObjectiveList() objective_functions.add(Objective.Lagrange.MINIMIZE_MARKERS_DISPLACEMENT, index=hand_marker_idx) objective_functions.add(Objective.Lagrange.MINIMIZE_MUSCLES_CONTROL) objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE) # Dynamics dynamics = DynamicsTypeList() dynamics.add(DynamicsType.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN) # Constraints constraints = ConstraintList() constraints.add( Constraint.ALIGN_MARKERS, first_marker_idx=hand_marker_idx, second_marker_idx=end_crank_idx, node=Node.ALL ) constraints.add( Constraint.TRACK_STATE, node=Node.ALL, index=0, target=np.linspace(0, 2 * np.pi, number_shooting_points + 1), ) state_transitions = StateTransitionList() state_transitions.add(state_transition_function, phase_pre_idx=0) # Path constraint x_bounds = BoundsList() x_bounds.add(QAndQDotBounds(biorbd_model)) # Initial guess x_init = InitialGuessList() x_init.add([0, -0.9, 1.7, 0.9, 2.0, -1.3] + [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 = InitialGuessList() 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, constraints, state_transitions=state_transitions, )
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_TORQUE) # Dynamics dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN) # 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: 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, ode_solver: OdeSolver = OdeSolver.RK4(), weight: float = 1, min_time=0, max_time=np.inf, ) -> OptimalControlProgram: """ Prepare the optimal control program Parameters ---------- biorbd_model_path: str The path to the bioMod final_time: float The initial guess for the final time n_shooting: int The number of shooting points ode_solver: OdeSolver The ode solver to use weight: float The weighting of the minimize time objective function min_time: float The minimum time allowed for the final node max_time: float The maximum time allowed for the final node Returns ------- The OptimalControlProgram ready to be solved """ # --- Options --- # biorbd_model = biorbd.Model(biorbd_model_path) tau_min, tau_max, tau_init = -100, 100, 0 n_q = biorbd_model.nbQ() n_qdot = biorbd_model.nbQdot() n_tau = biorbd_model.nbGeneralizedTorque() # Add objective functions objective_functions = ObjectiveList() # A weight of -1 will maximize time objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_TIME, weight=weight, min_bound=min_time, max_bound=max_time) # Dynamics dynamics = DynamicsList() expand = False if isinstance(ode_solver, OdeSolver.IRK) else True dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand) # Path constraint x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model)) x_bounds[0][:, [0, -1]] = 0 x_bounds[0][n_q - 1, -1] = 3.14 # Initial guess x_init = InitialGuessList() x_init.add([0] * (n_q + n_qdot)) # Define control path constraint u_bounds = BoundsList() u_bounds.add([tau_min] * n_tau, [tau_max] * n_tau) u_bounds[0][n_tau - 1, :] = 0 u_init = InitialGuessList() u_init.add([tau_init] * n_tau) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, ode_solver=ode_solver, )
def prepare_ocp( biorbd_model_path: str, n_shooting: int, final_time: float, ode_solver: OdeSolver = OdeSolver.RK4() ) -> OptimalControlProgram: """ Prepare the ocp Parameters ---------- biorbd_model_path: str The path to the bioMod file n_shooting: int The number of shooting points final_time: float The time at the final node ode_solver: OdeSolver The ode solver to use Returns ------- The OptimalControlProgram ready to be solved """ biorbd_model = biorbd.Model(biorbd_model_path) # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_MARKERS, index=1, weight=-1) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100) # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN) # Path constraint x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model)) # Define control path constraint n_tau = biorbd_model.nbGeneralizedTorque( ) # biorbd_model.nbGeneralizedTorque() tau_min, tau_max, tau_init = -100, 100, 0 u_bounds = BoundsList() u_bounds.add([tau_min] * n_tau, [tau_max] * n_tau) # Initial guesses # TODO put this in a function defined before and explain what it does, and what are the variables x = np.vstack((np.zeros( (biorbd_model.nbQ(), 2)), np.ones((biorbd_model.nbQdot(), 2)))) Arm_init_D = np.zeros((3, 2)) Arm_init_D[1, 0] = 0 Arm_init_D[1, 1] = -np.pi + 0.01 Arm_init_G = np.zeros((3, 2)) Arm_init_G[1, 0] = 0 Arm_init_G[1, 1] = np.pi - 0.01 for i in range(2): Arm_Quat_D = eul2quat(Arm_init_D[:, i]) Arm_Quat_G = eul2quat(Arm_init_G[:, i]) x[6:9, i] = Arm_Quat_D[1:] x[12, i] = Arm_Quat_D[0] x[9:12, i] = Arm_Quat_G[1:] x[13, i] = Arm_Quat_G[0] x_init = InitialGuessList() x_init.add(x, interpolation=InterpolationType.LINEAR) u_init = InitialGuessList() u_init.add([tau_init] * n_tau) return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, ode_solver=ode_solver, )
def prepare_ocp(biorbd_model_path, final_time, number_shooting_points, use_SX=True): # --- 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( ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100.0, ) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE, weight=1.0) objective_functions.add( ObjectiveFcn.Mayer.MINIMIZE_STATE, weight=50000.0, target=data_to_track[-1:, :].T, node=Node.END, ) # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN) # Path constraint x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model)) x_bounds[0][:, 0] = 0 # Initial guess x_init = InitialGuessList() x_init.add([0] * (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][n_tau - 1, :] = 0 u_init = InitialGuessList() 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, use_SX=use_SX, )
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()) u_init = InitialGuess([0] * m.nbGeneralizedTorque()) ocp = OptimalControlProgram( m, dynamics, number_shooting_points=30, phase_time=0.5, x_init=x_init, u_init=u_init, x_bounds=x_bounds, u_bounds=u_bounds, objective_functions=objective_functions, ) # --- Solve the program --- # sol = ocp.solve(show_online_optim=True) # --- Show results --- # result = ShowResult(ocp, sol) result.animate()
def prepare_ocp_phase_transitions( biorbd_model_path: str, with_constraints: bool, with_mayer: bool, with_lagrange: bool, ) -> OptimalControlProgram: """ Parameters ---------- biorbd_model_path: str The path to the bioMod Returns ------- The ocp ready to be solved """ # Model path biorbd_model = ( biorbd.Model(biorbd_model_path), biorbd.Model(biorbd_model_path), biorbd.Model(biorbd_model_path), biorbd.Model(biorbd_model_path), ) # Problem parameters n_shooting = (20, 20, 20, 20) final_time = (2, 5, 4, 2) tau_min, tau_max, tau_init = -100, 100, 0 # Add objective functions objective_functions = ObjectiveList() if with_lagrange: objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, phase=0) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, phase=1) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, phase=2) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, phase=3) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_COM_VELOCITY, weight=0, phase=3) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_MARKERS, weight=0, phase=3, marker_index=[0, 1]) if with_mayer: objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_TIME) objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_COM_POSITION, phase=0, node=1) objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_MARKERS, weight=0, phase=3, marker_index=[0, 1]) objective_functions.add( minimize_difference, custom_type=ObjectiveFcn.Mayer, node=Node.TRANSITION, weight=100, phase=1, quadratic=True, ) # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN) dynamics.add(DynamicsFcn.TORQUE_DRIVEN) dynamics.add(DynamicsFcn.TORQUE_DRIVEN) dynamics.add(DynamicsFcn.TORQUE_DRIVEN) # Constraints constraints = ConstraintList() if with_constraints: constraints.add( ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.START, first_marker="m0", second_marker="m1", phase=0, list_index=1, ) constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=2, first_marker="m0", second_marker="m1", phase=0, list_index=2) constraints.add( ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m2", phase=0, list_index=3, ) constraints.add( ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m1", phase=1, list_index=4, ) constraints.add( ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m2", phase=2, list_index=5, ) constraints.add(custom_func_track_markers, node=Node.ALL, first_marker="m0", second_marker="m1", phase=3, list_index=6) # Path constraint x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0])) x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0])) x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0])) x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0])) x_bounds[0][[1, 3, 4, 5], 0] = 0 x_bounds[-1][[1, 3, 4, 5], -1] = 0 x_bounds[0][2, 0] = 0.0 x_bounds[2][2, [0, -1]] = [0.0, 1.57] # Initial guess x_init = InitialGuessList() x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot())) x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot())) x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot())) x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot())) # Define control path constraint u_bounds = BoundsList() u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(), [tau_max] * biorbd_model[0].nbGeneralizedTorque()) u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(), [tau_max] * biorbd_model[0].nbGeneralizedTorque()) u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(), [tau_max] * biorbd_model[0].nbGeneralizedTorque()) u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(), [tau_max] * biorbd_model[0].nbGeneralizedTorque()) u_init = InitialGuessList() u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque()) u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque()) u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque()) u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque()) # Define phase transitions phase_transitions = PhaseTransitionList() phase_transitions.add(PhaseTransitionFcn.IMPACT, phase_pre_idx=1) phase_transitions.add(PhaseTransitionFcn.CONTINUOUS, phase_pre_idx=2) phase_transitions.add(PhaseTransitionFcn.CYCLIC) return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, phase_transitions=phase_transitions, )
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, )
def prepare_ocp( biorbd_model_path: str = "cubeSym.bioMod", ode_solver: OdeSolver = OdeSolver.RK4() ) -> OptimalControlProgram: """ Prepare the ocp Parameters ---------- biorbd_model_path: str Path to the bioMod ode_solver: OdeSolver The ode solver to use Returns ------- The OptimalControlProgram ready to be solved """ biorbd_model = biorbd.Model(biorbd_model_path) # Problem parameters n_shooting = 30 final_time = 2 tau_min, tau_max, tau_init = -100, 100, 0 all_generalized_mapping = BiMapping([0, 1, 2, -2], [0, 1, 2]) # 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.SUPERIMPOSE_MARKERS, node=Node.START, first_marker_idx=0, second_marker_idx=1) constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker_idx=0, second_marker_idx=2) # Path constraint x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model, all_generalized_mapping)) x_bounds[0][3:6, [0, -1]] = 0 # Initial guess x_init = InitialGuessList() x_init.add([0] * all_generalized_mapping.to_first.len * 2) # Define control path constraint u_bounds = BoundsList() u_bounds.add([tau_min] * all_generalized_mapping.to_first.len, [tau_max] * all_generalized_mapping.to_first.len) u_init = InitialGuessList() u_init.add([tau_init] * all_generalized_mapping.to_first.len) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, ode_solver=ode_solver, all_generalized_mapping=all_generalized_mapping, )
import time import sys from bioptim import OptimalControlProgram, ShowResult, Data from up_and_down_bow import xia_model_dynamic, xia_model_configuration, xia_model_fibers, xia_initial_fatigue_at_zero file_path = "/home/theophile/Documents/programmation/ViolinOptimalControl/optimal_control_python/results/2020_7_29_upDown.bo" if len(sys.argv) > 1: file_path = str(sys.argv[1]) if not isinstance(file_path, str): t = time.localtime(time.time()) file_path = f"results/{t.tm_year}_{t.tm_mon}_{t.tm_mday}_upDown.bo" ocp, sol = OptimalControlProgram.load(file_path) d = Data.get_data(ocp, sol) result = ShowResult(ocp, sol) result.graphs()
def prepare_ocp(biorbd_model, final_time, x0, nbGT, number_shooting_points, use_SX=False, nb_threads=8, use_torque=True): # --- Options --- # # Model path biorbd_model = biorbd_model nbQ = biorbd_model.nbQ() nbGT = nbGT nbMT = biorbd_model.nbMuscleTotal() tau_min, tau_max, tau_init = -100, 100, 0 muscle_min, muscle_max, muscle_init = 0, 1, 0.5 activation_min, activation_max, activation_init = 0, 1, 0.2 # Add objective functions objective_functions = ObjectiveList() # Dynamics dynamics = DynamicsTypeList() if use_activation and use_torque: dynamics.add(DynamicsType.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN) elif use_activation is not True and use_torque: dynamics.add(DynamicsType.MUSCLE_EXCITATIONS_AND_TORQUE_DRIVEN) elif use_activation and use_torque is not True: dynamics.add(DynamicsType.MUSCLE_ACTIVATIONS_DRIVEN) elif use_activation is not True and use_torque is not True: dynamics.add(DynamicsType.MUSCLE_EXCITATIONS_DRIVEN) # State path constraint x_bounds = BoundsList() x_bounds.add(QAndQDotBounds(biorbd_model)) if use_activation is not True: x_bounds[0].concatenate( Bounds([activation_min] * biorbd_model.nbMuscles(), [activation_max] * biorbd_model.nbMuscles())) # Control path constraint u_bounds = BoundsList() u_bounds.add([ [tau_min] * nbGT + [muscle_min] * biorbd_model.nbMuscleTotal(), [tau_max] * nbGT + [muscle_max] * biorbd_model.nbMuscleTotal(), ]) # Initial guesses x_init = InitialGuessOption(np.tile(x0, (number_shooting_points + 1, 1)).T, interpolation=InterpolationType.EACH_FRAME) u0 = np.array([tau_init] * nbGT + [muscle_init] * nbMT) + 0.1 u_init = InitialGuessOption(np.tile(u0, (number_shooting_points, 1)).T, interpolation=InterpolationType.EACH_FRAME) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, number_shooting_points, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, use_SX=use_SX, nb_threads=nb_threads, )
def prepare_ocp(model_path, phase_time, number_shooting_points, mu): # --- Options --- # # Model path biorbd_model = biorbd.Model(model_path) tau_min, tau_max, tau_init = -500, 500, 0 tau_mapping = BidirectionalMapping(Mapping([-1, -1, -1, 0]), Mapping([3])) # Add objective functions objective_functions = ObjectiveList() objective_functions.add(Objective.Mayer.MINIMIZE_PREDICTED_COM_HEIGHT, weight=-1) # Dynamics dynamics = DynamicsTypeList() dynamics.add(DynamicsType.TORQUE_DRIVEN_WITH_CONTACT) # Constraints constraints = ConstraintList() constraints.add( Constraint.CONTACT_FORCE, max_bound=np.inf, node=Node.ALL, contact_force_idx=1, ) constraints.add( Constraint.CONTACT_FORCE, max_bound=np.inf, node=Node.ALL, contact_force_idx=2, ) constraints.add( Constraint.NON_SLIPPING, node=Node.ALL, normal_component_idx=(1, 2), tangential_component_idx=0, static_friction_coefficient=mu, ) # 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 = BoundsList() x_bounds.add(QAndQDotBounds(biorbd_model)) x_bounds[0][:, 0] = pose_at_first_node + [0] * nb_qdot # Initial guess x_init = InitialGuessList() x_init.add(pose_at_first_node + [0] * nb_qdot) # Define control path constraint u_bounds = BoundsList() u_bounds.add([[tau_min] * tau_mapping.reduce.len, [tau_max] * tau_mapping.reduce.len]) u_init = InitialGuessList() u_init.add([tau_init] * tau_mapping.reduce.len) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, 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: str, final_time: float, n_shooting: int, initialize_near_solution: bool, ode_solver: OdeSolver = OdeSolver.RK4(), constr: bool = True, use_sx: bool = False, ) -> OptimalControlProgram: """ Prepare the ocp Parameters ---------- biorbd_model_path: str The path to the bioMod file final_time: float The time at the final node n_shooting: int The number of shooting points initialize_near_solution: bool If the initial guess should be almost the solution (this is merely to reduce the time of the tests) ode_solver: OdeSolver The ode solver to use constr: bool If the constraint should be applied (this is merely to reduce the time of the tests) use_sx: bool If SX CasADi variables should be used Returns ------- The OptimalControlProgram ready to be solved """ biorbd_model = biorbd.Model(biorbd_model_path) # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100) # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN) # Constraints if constr: constraints = ConstraintList() constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.START, first_marker_idx=0, second_marker_idx=4) constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker_idx=0, second_marker_idx=5) constraints.add(ConstraintFcn.TRACK_MARKER_WITH_SEGMENT_AXIS, node=Node.ALL, marker_idx=1, segment_idx=2, axis=Axis.X) else: constraints = ConstraintList() # Path constraint x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model)) for i in range(1, 8): if i != 3: x_bounds[0][i, [0, -1]] = 0 x_bounds[0][2, -1] = 1.57 # Initial guess x_init = InitialGuessList() x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot())) if initialize_near_solution: for i in range(2): x_init[0].init[i] = 1.5 for i in range(4, 6): x_init[0].init[i] = 0.7 for i in range(6, 8): x_init[0].init[i] = 0.6 # Define control path constraint tau_min, tau_max, tau_init = -100, 100, 0 u_bounds = BoundsList() u_bounds.add([tau_min] * biorbd_model.nbGeneralizedTorque(), [tau_max] * biorbd_model.nbGeneralizedTorque()) u_init = InitialGuessList() u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque()) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, ode_solver=ode_solver, use_sx=use_sx, )
def test_double_update_bounds_and_init(): bioptim_folder = TestUtils.bioptim_folder() biorbd_model = biorbd.Model(bioptim_folder + "/examples/track/models/cube_and_line.bioMod") nq = biorbd_model.nbQ() ns = 10 dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN) ocp = OptimalControlProgram(biorbd_model, dynamics, ns, 1.0) x_bounds = Bounds(-np.ones((nq * 2, 1)), np.ones((nq * 2, 1))) u_bounds = Bounds(-2.0 * np.ones((nq, 1)), 2.0 * np.ones((nq, 1))) ocp.update_bounds(x_bounds, u_bounds) expected = np.array([[-1] * (nq * 2) * (ns + 1) + [-2] * nq * ns]).T np.testing.assert_almost_equal(ocp.v.bounds.min, expected) expected = np.array([[1] * (nq * 2) * (ns + 1) + [2] * nq * ns]).T np.testing.assert_almost_equal(ocp.v.bounds.max, expected) x_init = InitialGuess(0.5 * np.ones((nq * 2, 1))) u_init = InitialGuess(-0.5 * np.ones((nq, 1))) ocp.update_initial_guess(x_init, u_init) expected = np.array([[0.5] * (nq * 2) * (ns + 1) + [-0.5] * nq * ns]).T np.testing.assert_almost_equal(ocp.v.init.init, expected) x_bounds = Bounds(-2.0 * np.ones((nq * 2, 1)), 2.0 * np.ones((nq * 2, 1))) u_bounds = Bounds(-4.0 * np.ones((nq, 1)), 4.0 * np.ones((nq, 1))) ocp.update_bounds(x_bounds=x_bounds) ocp.update_bounds(u_bounds=u_bounds) expected = np.array([[-2] * (nq * 2) * (ns + 1) + [-4] * nq * ns]).T np.testing.assert_almost_equal(ocp.v.bounds.min, expected) expected = np.array([[2] * (nq * 2) * (ns + 1) + [4] * nq * ns]).T np.testing.assert_almost_equal(ocp.v.bounds.max, expected) x_init = InitialGuess(0.25 * np.ones((nq * 2, 1))) u_init = InitialGuess(-0.25 * np.ones((nq, 1))) ocp.update_initial_guess(x_init, u_init) expected = np.array([[0.25] * (nq * 2) * (ns + 1) + [-0.25] * nq * ns]).T np.testing.assert_almost_equal(ocp.v.init.init, expected) with pytest.raises( RuntimeError, match= "x_init should be built from a InitialGuess or InitialGuessList"): ocp.update_initial_guess(x_bounds, u_bounds) with pytest.raises( RuntimeError, match="x_bounds should be built from a Bounds or BoundsList"): ocp.update_bounds(x_init, u_init)
def prepare_ocp( biorbd_model_path, final_time, number_shooting_points, marker_velocity_or_displacement, marker_in_first_coordinates_system, control_type, ode_solver=OdeSolver.RK, ): # --- Options --- # # Model path biorbd_model = biorbd.Model(biorbd_model_path) nq = biorbd_model.nbQ() biorbd_model.markerNames() # Problem parameters tau_min, tau_max, tau_init = -100, 100, 0 # Add objective functions if marker_in_first_coordinates_system: # Marker should follow this segment (0 velocity when compare to this one) coordinates_system_idx = 0 else: # Marker should be static in global reference frame coordinates_system_idx = -1 objective_functions = ObjectiveList() if marker_velocity_or_displacement == "disp": objective_functions.add( ObjectiveFcn.Lagrange.MINIMIZE_MARKERS_DISPLACEMENT, coordinates_system_idx=coordinates_system_idx, index=6, weight=1000, ) elif marker_velocity_or_displacement == "velo": objective_functions.add( ObjectiveFcn.Lagrange.MINIMIZE_MARKERS_VELOCITY, index=6, weight=1000) else: raise RuntimeError( "Wrong choice of marker_velocity_or_displacement, actual value is " "{marker_velocity_or_displacement}, should be 'velo' or 'disp'.") # Make sure the segments actually moves (in order to test the relative speed objective) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE, index=6, weight=-1) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE, index=7, weight=-1) # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN) # Path constraint x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model)) for i in range(nq, 2 * nq): x_bounds[0].min[i, :] = -10 x_bounds[0].max[i, :] = 10 # Initial guess x_init = InitialGuessList() x_init.add([1.5, 1.5, 0.0, 0.0, 0.7, 0.7, 0.6, 0.6]) # Define control path constraint 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, nb_integration_steps=5, control_type=control_type, ode_solver=ode_solver, )
def test_update_bounds_and_init_with_param(): def my_parameter_function(biorbd_model, value, extra_value): new_gravity = MX.zeros(3, 1) new_gravity[2] = value + extra_value biorbd_model.setGravity(new_gravity) def my_target_function(ocp, value, target_value): return value + target_value biorbd_model = biorbd.Model(TestUtils.bioptim_folder() + "/examples/track/models/cube_and_line.bioMod") nq = biorbd_model.nbQ() ns = 10 g_min, g_max, g_init = -10, -6, -8 dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN) parameters = ParameterList() bounds_gravity = Bounds(g_min, g_max, interpolation=InterpolationType.CONSTANT) initial_gravity = InitialGuess(g_init) parameter_objective_functions = Objective( my_target_function, weight=10, quadratic=True, custom_type=ObjectiveFcn.Parameter, target_value=-8) parameters.add( "gravity_z", my_parameter_function, initial_gravity, bounds_gravity, size=1, penalty_list=parameter_objective_functions, extra_value=1, ) ocp = OptimalControlProgram(biorbd_model, dynamics, ns, 1.0, parameters=parameters) x_bounds = Bounds(-np.ones((nq * 2, 1)), np.ones((nq * 2, 1))) u_bounds = Bounds(-2.0 * np.ones((nq, 1)), 2.0 * np.ones((nq, 1))) ocp.update_bounds(x_bounds, u_bounds) expected = np.array([[-1] * (nq * 2) * (ns + 1) + [-2] * nq * ns]).T np.testing.assert_almost_equal(ocp.v.bounds.min, np.append(expected, [g_min])[:, np.newaxis]) expected = np.array([[1] * (nq * 2) * (ns + 1) + [2] * nq * ns]).T np.testing.assert_almost_equal(ocp.v.bounds.max, np.append(expected, [g_max])[:, np.newaxis]) x_init = InitialGuess(0.5 * np.ones((nq * 2, 1))) u_init = InitialGuess(-0.5 * np.ones((nq, 1))) ocp.update_initial_guess(x_init, u_init) expected = np.array([[0.5] * (nq * 2) * (ns + 1) + [-0.5] * nq * ns]).T np.testing.assert_almost_equal( ocp.v.init.init, np.append(expected, [g_init])[:, np.newaxis])
def prepare_ocp( biorbd_model_path: str, final_time: float, n_shooting: int, weight: float, ode_solver: OdeSolver = OdeSolver.RK4(), ) -> 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_TORQUE) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_MUSCLES_CONTROL) objective_functions.add( ObjectiveFcn.Mayer.SUPERIMPOSE_MARKERS, first_marker_idx=0, second_marker_idx=5, weight=weight ) # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN) # 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, )
def prepare_ocp(biorbd_model_path, final_time, n_shooting, x_warm=None, use_sx=False, n_threads=1): # --- Options --- # # Model path biorbd_model = biorbd.Model(biorbd_model_path) tau_min, tau_max, tau_init = -50, 50, 0 muscle_min, muscle_max, muscle_init = 0, 1, 0.5 # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE, key="q", weight=10, multi_thread=False) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE, key="qdot", weight=10, multi_thread=False) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=10, multi_thread=False) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="muscles", weight=10, multi_thread=False) objective_functions.add(ObjectiveFcn.Mayer.SUPERIMPOSE_MARKERS, weight=100000, first_marker="target", second_marker="COM_hand") # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.MUSCLE_DRIVEN, with_torque=True) # Path constraint x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model)) x_bounds[0][:, 0] = (1.0, 1.0, 0, 0) # Initial guess if x_warm is None: x_init = InitialGuess([1.57] * biorbd_model.nbQ() + [0] * biorbd_model.nbQdot()) else: x_init = InitialGuess(x_warm, interpolation=InterpolationType.EACH_FRAME) # Define control path constraint u_bounds = BoundsList() u_bounds.add( [tau_min] * biorbd_model.nbGeneralizedTorque() + [muscle_min] * biorbd_model.nbMuscleTotal(), [tau_max] * biorbd_model.nbGeneralizedTorque() + [muscle_max] * biorbd_model.nbMuscleTotal(), ) u_init = InitialGuessList() u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque() + [muscle_init] * biorbd_model.nbMuscleTotal()) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, use_sx=use_sx, n_threads=n_threads, )
def 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: str, final_time: float, n_shooting: int, marker_velocity_or_displacement: str, marker_in_first_coordinates_system: bool, control_type: ControlType, ode_solver: OdeSolver = OdeSolver.RK4(), ) -> OptimalControlProgram: """ Prepare an ocp that targets some marker velocities, either by finite differences or by jacobian Parameters ---------- biorbd_model_path: str The path to the bioMod file final_time: float The time of the final node n_shooting: int The number of shooting points marker_velocity_or_displacement: str which type of tracking: finite difference ('disp') or by jacobian ('velo') marker_in_first_coordinates_system: bool If the marker to track should be expressed in the global or local reference frame control_type: ControlType The type of controls ode_solver: OdeSolver The ode solver to use Returns ------- The OptimalControlProgram ready to be solved """ biorbd_model = biorbd.Model(biorbd_model_path) # Add objective functions if marker_in_first_coordinates_system: # Marker should follow this segment (0 velocity when compare to this one) coordinates_system_idx = 0 else: # Marker should be static in global reference frame coordinates_system_idx = -1 objective_functions = ObjectiveList() if marker_velocity_or_displacement == "disp": objective_functions.add( ObjectiveFcn.Lagrange.MINIMIZE_MARKERS_DISPLACEMENT, coordinates_system_idx=coordinates_system_idx, index=6, weight=1000, ) elif marker_velocity_or_displacement == "velo": objective_functions.add( ObjectiveFcn.Lagrange.MINIMIZE_MARKERS_VELOCITY, index=6, weight=1000) else: raise RuntimeError( f"Wrong choice of marker_velocity_or_displacement, actual value is " f"{marker_velocity_or_displacement}, should be 'velo' or 'disp'.") # Make sure the segments actually moves (in order to test the relative speed objective) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE, index=6, weight=-1) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE, index=7, weight=-1) # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN) # Path constraint nq = biorbd_model.nbQ() x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model)) for i in range(nq, 2 * nq): x_bounds[0].min[i, :] = -10 x_bounds[0].max[i, :] = 10 # Initial guess x_init = InitialGuessList() x_init.add([1.5, 1.5, 0.0, 0.0, 0.7, 0.7, 0.6, 0.6]) # Define control path constraint tau_min, tau_max, tau_init = -100, 100, 0 u_bounds = BoundsList() u_bounds.add([tau_min] * biorbd_model.nbGeneralizedTorque(), [tau_max] * biorbd_model.nbGeneralizedTorque()) u_init = InitialGuessList() u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque()) return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, control_type=control_type, ode_solver=ode_solver, )
def prepare_ocp( biorbd_model: biorbd.Model, final_time: float, n_shooting: int, markers_ref: np.ndarray, activations_ref: np.ndarray, q_ref: np.ndarray, kin_data_to_track: str = "markers", use_residual_torque: bool = True, ode_solver: OdeSolver = OdeSolver.RK4(), ) -> 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 activations_ref: np.ndarray The muscle activation 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_MUSCLES_CONTROL, target=activations_ref) if use_residual_torque: objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE) if kin_data_to_track == "markers": objective_functions.add(ObjectiveFcn.Lagrange.TRACK_MARKERS, weight=100, target=markers_ref) elif kin_data_to_track == "q": objective_functions.add(ObjectiveFcn.Lagrange.TRACK_STATE, weight=100, target=q_ref, index=range(biorbd_model.nbQ())) else: raise RuntimeError("Wrong choice of kin_data_to_track") # Dynamics dynamics = DynamicsList() if use_residual_torque: dynamics.add(DynamicsFcn.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN) else: dynamics.add(DynamicsFcn.MUSCLE_ACTIVATIONS_DRIVEN) # 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 nq = biorbd_model.nbQ() x_bounds[0].min[:nq, :] = -2 * np.pi x_bounds[0].max[:nq, :] = 2 * np.pi # Initial guess x_init = InitialGuessList() x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot())) # Define control path constraint activation_min, activation_max, activation_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() + [activation_min] * biorbd_model.nbMuscleTotal(), [tau_max] * biorbd_model.nbGeneralizedTorque() + [activation_max] * biorbd_model.nbMuscleTotal(), ) u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque() + [activation_init] * biorbd_model.nbMuscleTotal()) else: u_bounds.add([activation_min] * biorbd_model.nbMuscleTotal(), [activation_max] * biorbd_model.nbMuscleTotal()) u_init.add([activation_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, )
def prepare_ocp( biorbd_model_path: str = "cubeSym.bioMod", ode_solver: OdeSolver = OdeSolver.RK4() ) -> OptimalControlProgram: """ Prepare the ocp Parameters ---------- biorbd_model_path: str Path to the bioMod ode_solver: OdeSolver The ode solver to use Returns ------- The OptimalControlProgram ready to be solved """ biorbd_model = biorbd.Model(biorbd_model_path) # Problem parameters n_shooting = 30 final_time = 2 tau_min, tau_max, tau_init = -100, 100, 0 # 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.SUPERIMPOSE_MARKERS, node=Node.START, first_marker_idx=0, second_marker_idx=1) constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker_idx=0, second_marker_idx=2) constraints.add(ConstraintFcn.PROPORTIONAL_STATE, node=Node.ALL, first_dof=2, second_dof=3, coef=-1) # Path constraint x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model)) x_bounds[0][4:8, [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.nbQ(), [tau_max] * biorbd_model.nbQ()) u_init = InitialGuessList() u_init.add([tau_init] * biorbd_model.nbQ()) # ------------- # 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: biorbd.Model, final_time: float, n_shooting: int, markers_ref: np.ndarray, tau_ref: np.ndarray, ode_solver: OdeSolver = OdeSolver.RK4(), ) -> OptimalControlProgram: """ Prepare the ocp 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 markers to track tau_ref: np.ndarray The generalized forces to track ode_solver: OdeSolver The ode solver to use Returns ------- The OptimalControlProgram ready to be solved """ # Add objective functions objective_functions = ObjectiveList() objective_functions.add( ObjectiveFcn.Lagrange.TRACK_MARKERS, axes=[Axis.Y, Axis.Z], node=Node.ALL, weight=100, target=markers_ref[1:, :, :], ) objective_functions.add(ObjectiveFcn.Lagrange.TRACK_CONTROL, key="tau", target=tau_ref) # Dynamics dynamics = DynamicsList() expand = False if isinstance(ode_solver, OdeSolver.IRK) else True dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand) # Path constraint x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model)) x_bounds[0][:, 0] = 0 # Initial guess n_q = biorbd_model.nbQ() n_qdot = biorbd_model.nbQdot() x_init = InitialGuessList() x_init.add([0] * (n_q + n_qdot)) # Define control path constraint n_tau = biorbd_model.nbGeneralizedTorque() tau_min, tau_max, tau_init = -100, 100, 0 u_bounds = BoundsList() u_bounds.add([tau_min] * n_tau, [tau_max] * n_tau) u_init = InitialGuessList() u_init.add([tau_init] * n_tau) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, ode_solver=ode_solver, )
def prepare_ocp(biorbd_model_path: str = "cube_with_forces.bioMod", ode_solver: OdeSolver = OdeSolver.RK4()) -> OptimalControlProgram: """ Prepare the ocp Parameters ---------- biorbd_model_path: str The path to the bioMod ode_solver: OdeSolver The ode solver to use Returns ------- The OptimalControlProgram ready to be solved """ biorbd_model = biorbd.Model(biorbd_model_path) # Problem parameters n_shooting = 30 final_time = 2 # 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.SUPERIMPOSE_MARKERS, node=Node.START, first_marker_idx=0, second_marker_idx=1) constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker_idx=0, second_marker_idx=2) # External forces. external_forces is of len 1 because there is only one phase. # The array inside it is 6x2x30 since there is [Mx, My, Mz, Fx, Fy, Fz] for the two externalforceindex for each node external_forces = [ np.repeat( np.array([[0, 0, 0, 0, 0, -2], [0, 0, 0, 0, 0, 5]]).T[:, :, np.newaxis], n_shooting, axis=2 ) ] # Path constraint x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model)) x_bounds[0][3:6, [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() tau_min, tau_max, tau_init = -100, 100, 0 u_bounds.add([tau_min] * biorbd_model.nbGeneralizedTorque(), [tau_max] * biorbd_model.nbGeneralizedTorque()) u_init = InitialGuessList() u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque()) return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions=objective_functions, constraints=constraints, external_forces=external_forces, ode_solver=ode_solver, )