diff --git a/bioptim/dynamics/configure_problem.py b/bioptim/dynamics/configure_problem.py index cd36fef92..9181ea0f0 100644 --- a/bioptim/dynamics/configure_problem.py +++ b/bioptim/dynamics/configure_problem.py @@ -1166,8 +1166,6 @@ def configure_dynamics_function(ocp, nlp, dyn_func, **extra_params): The function to get the derivative of the states """ - DynamicsFunctions.apply_parameters(nlp) - dynamics_eval = dyn_func( nlp.time_cx, nlp.states.scaled.cx, @@ -1355,7 +1353,7 @@ def configure_soft_contact_function(ocp, nlp): q = nlp.states.cx[nlp.states["q"].index] qdot = nlp.states.cx[nlp.states["qdot"].index] - global_soft_contact_force_func = nlp.model.soft_contact_forces()(q, qdot) + global_soft_contact_force_func = nlp.model.soft_contact_forces()(q, qdot, nlp.parameters.cx) nlp.soft_contact_forces_func = global_soft_contact_force_func for i_sc in range(nlp.model.nb_soft_contacts): diff --git a/bioptim/dynamics/dynamics_functions.py b/bioptim/dynamics/dynamics_functions.py index 411b5ef40..6851be4c8 100644 --- a/bioptim/dynamics/dynamics_functions.py +++ b/bioptim/dynamics/dynamics_functions.py @@ -147,8 +147,8 @@ def torque_driven( dq = DynamicsFunctions.compute_qdot(nlp, q, qdot) tau = DynamicsFunctions.__get_fatigable_tau(nlp, states, controls, fatigue) - tau = tau + nlp.model.passive_joint_torque()(q, qdot) if with_passive_torque else tau - tau = tau + nlp.model.ligament_joint_torque()(q, qdot) if with_ligament else tau + tau = tau + nlp.model.passive_joint_torque()(q, qdot, nlp.parameters.cx) if with_passive_torque else tau + tau = tau + nlp.model.ligament_joint_torque()(q, qdot, nlp.parameters.cx) if with_ligament else tau tau = tau - nlp.model.friction_coefficients @ qdot if with_friction else tau if ( @@ -260,7 +260,7 @@ def torque_driven_free_floating_base( n_qdot = nlp.model.nb_qdot tau_joints = ( - tau_joints + nlp.model.passive_joint_torque()(q_full, qdot_full) if with_passive_torque else tau_joints + tau_joints + nlp.model.passive_joint_torque()(q_full, qdot_full, nlp.parameters.cx) if with_passive_torque else tau_joints ) tau_joints = tau_joints + nlp.model.ligament_joint_torque()(q_full, qdot_full) if with_ligament else tau_joints tau_joints = tau_joints - nlp.model.friction_coefficients @ qdot_joints if with_friction else tau_joints @@ -528,13 +528,13 @@ def torque_activations_driven( qdot = DynamicsFunctions.get(nlp.states["qdot"], states) tau_activation = DynamicsFunctions.get(nlp.controls["tau"], controls) - tau = nlp.model.torque()(tau_activation, q, qdot) + tau = nlp.model.torque()(tau_activation, q, qdot, nlp.parameters.cx) if with_passive_torque: - tau += nlp.model.passive_joint_torque()(q, qdot) + tau += nlp.model.passive_joint_torque()(q, qdot, nlp.parameters.cx) if with_residual_torque: tau += DynamicsFunctions.get(nlp.controls["residual_tau"], controls) if with_ligament: - tau += nlp.model.ligament_joint_torque()(q, qdot) + tau += nlp.model.ligament_joint_torque()(q, qdot, nlp.parameters.cx) dq = DynamicsFunctions.compute_qdot(nlp, q, qdot) ddq = DynamicsFunctions.forward_dynamics(nlp, q, qdot, tau, with_contact, external_forces) @@ -598,8 +598,8 @@ def torque_derivative_driven( qdot = DynamicsFunctions.get(nlp.states["qdot"], states) tau = DynamicsFunctions.get(nlp.states["tau"], states) - tau = tau + nlp.model.passive_joint_torque()(q, qdot) if with_passive_torque else tau - tau = tau + nlp.model.ligament_joint_torque()(q, qdot) if with_ligament else tau + tau = tau + nlp.model.passive_joint_torque()(q, qdot, nlp.parameters.cx) if with_passive_torque else tau + tau = tau + nlp.model.ligament_joint_torque()(q, qdot, nlp.parameters.cx) if with_ligament else tau dq = DynamicsFunctions.compute_qdot(nlp, q, qdot) dtau = DynamicsFunctions.get(nlp.controls["taudot"], controls) @@ -673,11 +673,11 @@ def forces_from_torque_driven( q = nlp.get_var_from_states_or_controls("q", states, controls) qdot = nlp.get_var_from_states_or_controls("qdot", states, controls) tau = nlp.get_var_from_states_or_controls("tau", states, controls) - tau = tau + nlp.model.passive_joint_torque()(q, qdot) if with_passive_torque else tau - tau = tau + nlp.model.ligament_joint_torque()(q, qdot) if with_ligament else tau + tau = tau + nlp.model.passive_joint_torque()(q, qdot, nlp.parameters.cx) if with_passive_torque else tau + tau = tau + nlp.model.ligament_joint_torque()(q, qdot, nlp.parameters.cx) if with_ligament else tau external_forces = [] if external_forces is None else external_forces - return nlp.model.contact_forces()(q, qdot, tau, external_forces) + return nlp.model.contact_forces()(q, qdot, tau, external_forces, nlp.parameters.cx) @staticmethod def forces_from_torque_activation_driven( @@ -726,12 +726,12 @@ def forces_from_torque_activation_driven( q = nlp.get_var_from_states_or_controls("q", states, controls) qdot = nlp.get_var_from_states_or_controls("qdot", states, controls) tau_activations = nlp.get_var_from_states_or_controls("tau", states, controls) - tau = nlp.model.torque()(tau_activations, q, qdot) - tau = tau + nlp.model.passive_joint_torque()(q, qdot) if with_passive_torque else tau - tau = tau + nlp.model.ligament_joint_torque()(q, qdot) if with_ligament else tau + tau = nlp.model.torque()(tau_activations, q, qdot, nlp.parameters.cx) + tau = tau + nlp.model.passive_joint_torque()(q, qdot, nlp.parameters.cx) if with_passive_torque else tau + tau = tau + nlp.model.ligament_joint_torque()(q, qdot, nlp.parameters.cx) if with_ligament else tau external_forces = [] if external_forces is None else external_forces - return nlp.model.contact_forces()(q, qdot, tau, external_forces) + return nlp.model.contact_forces()(q, qdot, tau, external_forces, nlp.parameters.cx) @staticmethod def muscles_driven( @@ -831,8 +831,8 @@ def muscles_driven( muscles_tau = DynamicsFunctions.compute_tau_from_muscle(nlp, q, qdot, mus_activations, fatigue_states) tau = muscles_tau + residual_tau if residual_tau is not None else muscles_tau - tau = tau + nlp.model.passive_joint_torque()(q, qdot) if with_passive_torque else tau - tau = tau + nlp.model.ligament_joint_torque()(q, qdot) if with_ligament else tau + tau = tau + nlp.model.passive_joint_torque()(q, qdot, nlp.parameters.cx) if with_passive_torque else tau + tau = tau + nlp.model.ligament_joint_torque()(q, qdot, nlp.parameters.cx) if with_ligament else tau dq = DynamicsFunctions.compute_qdot(nlp, q, qdot) @@ -932,8 +932,8 @@ def forces_from_muscle_driven( muscles_tau = DynamicsFunctions.compute_tau_from_muscle(nlp, q, qdot, mus_activations) tau = muscles_tau + residual_tau if residual_tau is not None else muscles_tau - tau = tau + nlp.model.passive_joint_torque()(q, qdot) if with_passive_torque else tau - tau = tau + nlp.model.ligament_joint_torque()(q, qdot) if with_ligament else tau + tau = tau + nlp.model.passive_joint_torque()(q, qdot, nlp.parameters.cx) if with_passive_torque else tau + tau = tau + nlp.model.ligament_joint_torque()(q, qdot, nlp.parameters.cx) if with_ligament else tau external_forces = [] if external_forces is None else external_forces return nlp.model.contact_forces()(q, qdot, tau, external_forces) @@ -1040,27 +1040,6 @@ def get(var: OptimizationVariable, cx: MX | SX): return var.mapping.to_second.map(cx[var.index, :]) - @staticmethod - def apply_parameters(nlp): - """ - Apply the parameter variables to the model. This should be called before calling the dynamics - - Parameters - ---------- - parameters: MX.sym | SX.sym - The state of the system - nlp: NonLinearProgram - The definition of the system - """ - - for param_key in nlp.parameters: - # Call the pre dynamics function - if nlp.parameters[param_key].function: - param = nlp.parameters[param_key] - param_scaling = nlp.parameters[param_key].scaling.scaling - param_reduced = nlp.parameters.scaled.cx[param.index] - param.function(nlp.model, param_reduced * param_scaling, **param.kwargs) - @staticmethod def compute_qdot(nlp, q: MX | SX, qdot: MX | SX): """ @@ -1093,7 +1072,7 @@ def compute_qdot(nlp, q: MX | SX, qdot: MX | SX): mapping = nlp.controls["q"].mapping else: raise RuntimeError("Your q key combination was not found in states or controls") - return mapping.to_first.map(nlp.model.reshape_qdot()(q, qdot)) + return mapping.to_first.map(nlp.model.reshape_qdot()(q, qdot, nlp.parameters.cx)) @staticmethod def forward_dynamics( @@ -1134,10 +1113,10 @@ def forward_dynamics( qdot_var_mapping = BiMapping([i for i in range(qdot.shape[0])], [i for i in range(qdot.shape[0])]).to_first if external_forces is None: - qddot = nlp.model.forward_dynamics(with_contact=with_contact)(q, qdot, tau, []) + qddot = nlp.model.forward_dynamics(with_contact=with_contact)(q, qdot, tau, [], nlp.parameters.cx) return qdot_var_mapping.map(qddot) else: - qddot = nlp.model.forward_dynamics(with_contact=with_contact)(q, qdot, tau, external_forces) + qddot = nlp.model.forward_dynamics(with_contact=with_contact)(q, qdot, tau, external_forces, nlp.parameters.cx) return qdot_var_mapping.map(qddot) @staticmethod @@ -1173,7 +1152,7 @@ def inverse_dynamics( """ if external_forces is None: - tau = nlp.model.inverse_dynamics(with_contact=with_contact)(q, qdot, qddot, []) + tau = nlp.model.inverse_dynamics(with_contact=with_contact)(q, qdot, qddot, [], nlp.parameters.cx) else: if "tau" in nlp.states: tau_shape = nlp.states["tau"].cx.shape[0] @@ -1183,7 +1162,7 @@ def inverse_dynamics( tau_shape = nlp.model.nb_tau tau = nlp.cx(tau_shape, nlp.ns) for i in range(external_forces.shape[1]): - tau[:, i] = nlp.model.inverse_dynamics(with_contact=with_contact)(q, qdot, qddot, external_forces[:, i]) + tau[:, i] = nlp.model.inverse_dynamics(with_contact=with_contact)(q, qdot, qddot, external_forces[:, i], nlp.parameters.cx) return tau # We ignore on purpose the mapping to keep zeros in the defects of the dynamic. @staticmethod @@ -1240,7 +1219,7 @@ def compute_tau_from_muscle( activations = vertcat(activations, muscle_activations[k] * (1 - fatigue_states[k])) else: activations = vertcat(activations, muscle_activations[k]) - return nlp.model.muscle_joint_torque()(activations, q, qdot) + return nlp.model.muscle_joint_torque()(activations, q, qdot, nlp.parameters.cx) @staticmethod def holonomic_torque_driven( @@ -1283,6 +1262,6 @@ def holonomic_torque_driven( q_u = DynamicsFunctions.get(nlp.states["q_u"], states) qdot_u = DynamicsFunctions.get(nlp.states["qdot_u"], states) tau = DynamicsFunctions.get(nlp.controls["tau"], controls) - qddot_u = nlp.model.partitioned_forward_dynamics(q_u, qdot_u, tau, external_forces=external_forces) + qddot_u = nlp.model.partitioned_forward_dynamics(q_u, qdot_u, tau, external_forces, nlp.parameters.cx) return DynamicsEvaluation(dxdt=vertcat(qdot_u, qddot_u), defects=None) diff --git a/bioptim/examples/getting_started/custom_objectives.py b/bioptim/examples/getting_started/custom_objectives.py index 86b7a72e5..eff7b05cb 100644 --- a/bioptim/examples/getting_started/custom_objectives.py +++ b/bioptim/examples/getting_started/custom_objectives.py @@ -108,7 +108,6 @@ def prepare_ocp( first_marker="m0", second_marker="m1", weight=1000, - method=0, ) objective_functions.add( custom_func_track_markers, @@ -118,7 +117,6 @@ def prepare_ocp( first_marker="m0", second_marker="m2", weight=1000, - method=1, ) # Dynamics diff --git a/bioptim/examples/getting_started/custom_parameters.py b/bioptim/examples/getting_started/custom_parameters.py index 9f0d518b1..dcca8cb3e 100644 --- a/bioptim/examples/getting_started/custom_parameters.py +++ b/bioptim/examples/getting_started/custom_parameters.py @@ -49,7 +49,7 @@ def my_parameter_function(bio_model: BiorbdModel, value: MX, extra_value: Any): """ value[2] *= extra_value - bio_model.set_gravity(value) + bio_model.model.setGravity(value) def set_mass(bio_model: BiorbdModel, value: MX): @@ -65,7 +65,7 @@ def set_mass(bio_model: BiorbdModel, value: MX): The CasADi variables to modify the model """ - bio_model.segments[0].characteristics().setMass(value) + bio_model.model.segments()[0].characteristics().setMass(value) def my_target_function(controller: PenaltyController, key: str) -> MX: @@ -149,32 +149,6 @@ def prepare_ocp( The ocp ready to be solved """ - # --- Options --- # - bio_model = BiorbdModel(biorbd_model_path) - n_tau = bio_model.nb_tau - - # Add objective functions - objective_functions = ObjectiveList() - objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=1) - objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE, key="q", weight=1) - - # Dynamics - dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN, expand_dynamics=expand_dynamics, phase_dynamics=phase_dynamics) - - # Path constraint - x_bounds = BoundsList() - x_bounds["q"] = bio_model.bounds_from_ranges("q") - x_bounds["q"][:, [0, -1]] = 0 - x_bounds["q"][1, -1] = 3.14 - x_bounds["qdot"] = bio_model.bounds_from_ranges("qdot") - x_bounds["qdot"][:, [0, -1]] = 0 - - # Define control path constraint - tau_min, tau_max = -300, 300 - u_bounds = BoundsList() - u_bounds["tau"] = [tau_min] * n_tau, [tau_max] * n_tau - u_bounds["tau"][1, :] = 0 - # Define the parameter to optimize parameters = ParameterList(use_sx=use_sx) parameter_objectives = ParameterObjectiveList() @@ -230,6 +204,32 @@ def prepare_ocp( key="mass", ) + # --- Options --- # + bio_model = BiorbdModel(biorbd_model_path, parameters=parameters) + n_tau = bio_model.nb_tau + + # Add objective functions + objective_functions = ObjectiveList() + objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=1) + objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE, key="q", weight=1) + + # Dynamics + dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN, expand_dynamics=expand_dynamics, phase_dynamics=phase_dynamics) + + # Path constraint + x_bounds = BoundsList() + x_bounds["q"] = bio_model.bounds_from_ranges("q") + x_bounds["q"][:, [0, -1]] = 0 + x_bounds["q"][1, -1] = 3.14 + x_bounds["qdot"] = bio_model.bounds_from_ranges("qdot") + x_bounds["qdot"][:, [0, -1]] = 0 + + # Define control path constraint + tau_min, tau_max = -300, 300 + u_bounds = BoundsList() + u_bounds["tau"] = [tau_min] * n_tau, [tau_max] * n_tau + u_bounds["tau"][1, :] = 0 + return OptimalControlProgram( bio_model, dynamics, diff --git a/bioptim/examples/moving_horizon_estimation/mhe.py b/bioptim/examples/moving_horizon_estimation/mhe.py index 36955f42d..2df1c330a 100644 --- a/bioptim/examples/moving_horizon_estimation/mhe.py +++ b/bioptim/examples/moving_horizon_estimation/mhe.py @@ -42,20 +42,15 @@ def states_to_markers(bio_model, states): nq = bio_model.nb_q n_mark = bio_model.nb_markers - q = cas.MX.sym("q", nq) - markers_func = biorbd.to_casadi_func("makers", bio_model.markers, q) - return np.array(markers_func(states[:nq, :])).reshape((3, n_mark, -1), order="F") + return np.array(bio_model.markers()(states[:nq, :])).reshape((3, n_mark, -1), order="F") def generate_data(bio_model, tf, x0, t_max, n_shoot, noise_std, show_plots=False): def pendulum_ode(t, x, u): - return np.concatenate((x[nq:, np.newaxis], qddot_func(x[:nq], x[nq:], u)))[:, 0] + return np.concatenate((x[nq:, np.newaxis], qddot_func(x[:nq], x[nq:], u, [])))[:, 0] nq = bio_model.nb_q - q = cas.MX.sym("q", nq) - qdot = cas.MX.sym("qdot", nq) - tau = cas.MX.sym("tau", nq) - qddot_func = biorbd.to_casadi_func("forw_dyn", bio_model.forward_dynamics, q, qdot, tau) + qddot_func = bio_model.forward_dynamics() # Simulated data dt = tf / n_shoot diff --git a/bioptim/examples/track/track_segment_on_rt.py b/bioptim/examples/track/track_segment_on_rt.py index bebe64c45..319606b4a 100644 --- a/bioptim/examples/track/track_segment_on_rt.py +++ b/bioptim/examples/track/track_segment_on_rt.py @@ -73,7 +73,7 @@ def prepare_ocp( # Constraints constraints = ConstraintList() - constraints.add(ConstraintFcn.TRACK_SEGMENT_WITH_CUSTOM_RT, node=Node.ALL, segment="seg_rt", rt=0) + constraints.add(ConstraintFcn.TRACK_SEGMENT_WITH_CUSTOM_RT, node=Node.ALL, segment="seg_rt", rt_idx=0) # Path constraint x_bounds = BoundsList() diff --git a/bioptim/limits/constraints.py b/bioptim/limits/constraints.py index 82e7460dc..7aa3b7d4b 100644 --- a/bioptim/limits/constraints.py +++ b/bioptim/limits/constraints.py @@ -408,14 +408,14 @@ def qddot_equals_forward_dynamics( q = controller.q.cx qdot = controller.qdot.cx - passive_torque = controller.model.passive_joint_torque()(q, qdot) + passive_torque = controller.model.passive_joint_torque()(q, qdot, controller.parameters.cx) tau = controller.states["tau"].cx if "tau" in controller.states else controller.tau.cx tau = tau + passive_torque if with_passive_torque else tau - tau = tau + controller.model.ligament_joint_torque(q, qdot) if with_ligament else tau + tau = tau + controller.model.ligament_joint_torque(q, qdot, controller.parameters.cx) if with_ligament else tau qddot = controller.controls["qddot"].cx if "qddot" in controller.controls else controller.states["qddot"].cx # @Ipuch: no f_ext allowed ? - qddot_fd = controller.model.forward_dynamics(with_contact=with_contact)(q, qdot, tau, []) + qddot_fd = controller.model.forward_dynamics(with_contact=with_contact)(q, qdot, tau, [], controller.parameters.cx) return qddot - qddot_fd @staticmethod @@ -451,9 +451,9 @@ def tau_equals_inverse_dynamics( qdot = controller.qdot.cx tau = controller.states["tau"].cx if "tau" in controller.states else controller.tau.cx qddot = controller.states["qddot"].cx if "qddot" in controller.states else controller.controls["qddot"].cx - passive_torque = controller.model.passive_joint_torque()(q, qdot) + passive_torque = controller.model.passive_joint_torque()(q, qdot, controller.parameters.cx) tau = tau + passive_torque if with_passive_torque else tau - tau = tau + controller.model.ligament_joint_torque()(q, qdot) if with_ligament else tau + tau = tau + controller.model.ligament_joint_torque()(q, qdot, controller.parameters.cx) if with_ligament else tau if "fext" in controller.controls: f_ext = controller.controls["fext"].cx @@ -464,7 +464,7 @@ def tau_equals_inverse_dynamics( else: raise ValueError("External forces must be provided") - tau_id = controller.model.inverse_dynamics(with_contact=with_contact)(q, qdot, qddot, f_ext) + tau_id = controller.model.inverse_dynamics(with_contact=with_contact)(q, qdot, qddot, f_ext, controller.parameters.cx) var = [] var.extend([controller.states[key] for key in controller.states]) @@ -529,12 +529,12 @@ def tau_from_muscle_equal_inverse_dynamics( qdot = controller.qdot.cx muscle_activations = controller.controls["muscles"].cx muscles_states = controller.model.state_set() - passive_torque = controller.model.passive_joint_torque()(q, qdot) + passive_torque = controller.model.passive_joint_torque()(q, qdot, controller.parameters.cx) for k in range(len(controller.controls["muscles"])): muscles_states[k].setActivation(muscle_activations[k]) - muscle_tau = controller.model.muscle_joint_torque(muscles_states, q, qdot) + muscle_tau = controller.model.muscle_joint_torque(muscles_states, q, qdot, controller.parameters.cx) muscle_tau = muscle_tau + passive_torque if with_passive_torque else muscle_tau - muscle_tau = muscle_tau + controller.model.ligament_joint_torque(q, qdot) if with_ligament else muscle_tau + muscle_tau = muscle_tau + controller.model.ligament_joint_torque(q, qdot, controller.parameters.cx) if with_ligament else muscle_tau qddot = controller.states["qddot"].cx if "qddot" in controller.states else controller.controls["qddot"].cx if controller.get_nlp.numerical_timeseries: @@ -544,7 +544,7 @@ def tau_from_muscle_equal_inverse_dynamics( # Todo: add fext tau_id = nlp.model.inverse_dynamics(q, qdot, qddot, fext).to_mx() # fext need to be a mx - tau_id = controller.model.inverse_dynamics()(q, qdot, qddot) + tau_id = controller.model.inverse_dynamics()(q, qdot, qddot, controller.parameters.cx) return tau_id - muscle_tau diff --git a/bioptim/limits/penalty.py b/bioptim/limits/penalty.py index 79eded294..fec15bcac 100644 --- a/bioptim/limits/penalty.py +++ b/bioptim/limits/penalty.py @@ -124,7 +124,7 @@ def minimize_power( if key_control == "tau": return controls * controller.qdot.cx_start elif key_control == "muscles": - muscles_dot = controller.model.muscle_velocity()(controller.q.cx, controller.qdot.cx) + muscles_dot = controller.model.muscle_velocity()(controller.q.cx, controller.qdot.cx, controller.parameters.cx) return controls * muscles_dot @@ -291,16 +291,15 @@ def minimize_markers( # Compute the position of the marker in the requested reference frame (None for global) q = controller.q - model: BiorbdModel = controller.model CX_eye = SX_eye if controller.ocp.cx == SX else MX_eye jcs_t = ( CX_eye(4) if reference_jcs is None - else model.homogeneous_matrices_in_global(reference_jcs, inverse=True)(q.cx) + else controller.model.homogeneous_matrices_in_global(reference_jcs, inverse=True)(q.cx, controller.parameters.cx) ) markers = [] - for m in model.markers()(q.cx): + for m in controller.model.markers()(q.cx, controller.parameters.cx): markers_in_jcs = jcs_t @ vertcat(m, 1) markers = horzcat(markers, markers_in_jcs[:3]) @@ -383,7 +382,7 @@ def minimize_markers_acceleration( markers = horzcat( *controller.model.marker_accelerations(reference_index=reference_jcs)( - controller.q.cx, controller.qdot.cx, qddot + controller.q.cx, controller.qdot.cx, qddot, controller.parameters.cx ) ) @@ -427,9 +426,9 @@ def superimpose_markers( PenaltyFunctionAbstract.set_axes_rows(penalty, axes) penalty.quadratic = True if penalty.quadratic is None else penalty.quadratic - diff_markers = controller.model.marker(second_marker_idx)(controller.q.cx) - controller.model.marker( + diff_markers = controller.model.marker(second_marker_idx)(controller.q.cx, controller.parameters.cx) - controller.model.marker( first_marker_idx - )(controller.q.cx) + )(controller.q.cx, controller.parameters.cx) return diff_markers @@ -471,7 +470,7 @@ def superimpose_markers_velocity( PenaltyFunctionAbstract.set_axes_rows(penalty, axes) penalty.quadratic = True if penalty.quadratic is None else penalty.quadratic - marker_velocity = controller.model.marker_velocities()(controller.q.cx, controller.qdot.cx) + marker_velocity = controller.model.marker_velocities()(controller.q.cx, controller.qdot.cx, controller.parameters.cx) marker_1 = marker_velocity[first_marker_idx][:] marker_2 = marker_velocity[second_marker_idx][:] @@ -600,8 +599,8 @@ def minimize_predicted_com_height(_: PenaltyOption, controller: PenaltyControlle """ g = controller.model.gravity()["o0"][2] - com = controller.model.center_of_mass()(controller.q.cx) - com_dot = controller.model.center_of_mass_velocity()(controller.q.cx, controller.qdot.cx) + com = controller.model.center_of_mass()(controller.q.cx, controller.parameters.cx) + com_dot = controller.model.center_of_mass_velocity()(controller.q.cx, controller.qdot.cx, controller.parameters.cx) com_height = (com_dot[2] * com_dot[2]) / (2 * -g) + com[2] return com_height @@ -626,7 +625,7 @@ def minimize_com_position(penalty: PenaltyOption, controller: PenaltyController, PenaltyFunctionAbstract.set_axes_rows(penalty, axes) penalty.quadratic = True if penalty.quadratic is None else penalty.quadratic - return controller.model.center_of_mass()(controller.q.cx) + return controller.model.center_of_mass()(controller.q.cx, controller.parameters.cx) @staticmethod def minimize_com_velocity(penalty: PenaltyOption, controller: PenaltyController, axes: tuple | list = None): @@ -649,7 +648,7 @@ def minimize_com_velocity(penalty: PenaltyOption, controller: PenaltyController, PenaltyFunctionAbstract.set_axes_rows(penalty, axes) penalty.quadratic = True if penalty.quadratic is None else penalty.quadratic - return controller.model.center_of_mass_velocity()(controller.q.cx, controller.qdot.cx) + return controller.model.center_of_mass_velocity()(controller.q.cx, controller.qdot.cx, controller.parameters.cx) @staticmethod def minimize_com_acceleration(penalty: PenaltyOption, controller: PenaltyController, axes: tuple | list = None): @@ -674,7 +673,7 @@ def minimize_com_acceleration(penalty: PenaltyOption, controller: PenaltyControl qddot = PenaltyFunctionAbstract._get_qddot(controller, "cx") - marker = controller.model.center_of_mass_acceleration()(controller.q.cx, controller.qdot.cx, qddot) + marker = controller.model.center_of_mass_acceleration()(controller.q.cx, controller.qdot.cx, qddot, controller.parameters.cx) return marker @@ -697,7 +696,7 @@ def minimize_angular_momentum(penalty: PenaltyOption, controller: PenaltyControl PenaltyFunctionAbstract.set_axes_rows(penalty, axes) penalty.quadratic = True if penalty.quadratic is None else penalty.quadratic - return controller.model.angular_momentum()(controller.q.cx, controller.qdot.cx) + return controller.model.angular_momentum()(controller.q.cx, controller.qdot.cx, controller.parameters.cx) @staticmethod def minimize_linear_momentum(penalty: PenaltyOption, controller: PenaltyController, axes: tuple | list = None): @@ -719,7 +718,7 @@ def minimize_linear_momentum(penalty: PenaltyOption, controller: PenaltyControll PenaltyFunctionAbstract.set_axes_rows(penalty, axes) penalty.quadratic = True if penalty.quadratic is None else penalty.quadratic - com_velocity = controller.model.center_of_mass_velocity()(controller.q.cx, controller.qdot.cx) + com_velocity = controller.model.center_of_mass_velocity()(controller.q.cx, controller.qdot.cx, controller.parameters.cx) mass = controller.model.mass()["o0"] linear_momentum_cx = com_velocity * mass return linear_momentum_cx @@ -893,8 +892,8 @@ def track_segment_with_custom_rt( raise NotImplementedError( "The track_segment_with_custom_rt penalty can only be called with a BiorbdModel" ) - r_seg_transposed = controller.model.homogeneous_matrices_in_global(segment_index)(controller.q.cx)[:3, :3].T - r_rt = controller.model.rt(rt_idx)(controller.q.cx)[:3, :3] + r_seg_transposed = controller.model.homogeneous_matrices_in_global(segment_index)(controller.q.cx, controller.parameters.cx)[:3, :3].T + r_rt = controller.model.rt(rt_idx)(controller.q.cx, controller.parameters.cx)[:3, :3] # @Pariterre: why was this sequence is fixed? # @Pariterre: this is suspicious and it breaks the tests! angles_diff = controller.model.rotation_matrix_to_euler_angles(sequence)(r_seg_transposed * r_rt) @@ -936,7 +935,7 @@ def track_marker_with_segment_axis( segment_idx = controller.model.segment_index(segment) if isinstance(segment, str) else segment # Get the marker in rt reference frame - marker = controller.model.marker(marker_idx, segment_idx)(controller.q.cx) + marker = controller.model.marker(marker_idx, segment_idx)(controller.q.cx, controller.parameters.cx) # To align an axis, the other must be equal to 0 if not penalty.rows_is_set: @@ -986,7 +985,7 @@ def minimize_segment_rotation( if not isinstance(controller.model, BiorbdModel): raise NotImplementedError("The minimize_segment_rotation penalty can only be called with a BiorbdModel") - jcs_segment = controller.model.homogeneous_matrices_in_global(segment_idx)(controller.q.cx)[:3, :3] + jcs_segment = controller.model.homogeneous_matrices_in_global(segment_idx)(controller.q.cx, controller.parameters.cx)[:3, :3] angles_segment = controller.model.rotation_matrix_to_euler_angles(sequence)(jcs_segment) if axes is None: @@ -1031,7 +1030,7 @@ def minimize_segment_velocity( "The minimize_segments_velocity penalty can only be called with a BiorbdModel" ) model: BiorbdModel = controller.model - segment_angular_velocity = model.segment_angular_velocity(segment_idx)(controller.q.cx, controller.qdot.cx) + segment_angular_velocity = model.segment_angular_velocity(segment_idx)(controller.q.cx, controller.qdot.cx, controller.parameters.cx) if axes is None: axes = [Axis.X, Axis.Y, Axis.Z] @@ -1098,10 +1097,10 @@ def track_vector_orientations_from_markers( else vector_1_marker_1 ) - vector_0_marker_0_position = controller.model.marker(vector_0_marker_0_idx)(controller.q.cx) - vector_0_marker_1_position = controller.model.marker(vector_0_marker_1_idx)(controller.q.cx) - vector_1_marker_0_position = controller.model.marker(vector_1_marker_0_idx)(controller.q.cx) - vector_1_marker_1_position = controller.model.marker(vector_1_marker_1_idx)(controller.q.cx) + vector_0_marker_0_position = controller.model.marker(vector_0_marker_0_idx)(controller.q.cx, controller.parameters.cx) + vector_0_marker_1_position = controller.model.marker(vector_0_marker_1_idx)(controller.q.cx, controller.parameters.cx) + vector_1_marker_0_position = controller.model.marker(vector_1_marker_0_idx)(controller.q.cx, controller.parameters.cx) + vector_1_marker_1_position = controller.model.marker(vector_1_marker_1_idx)(controller.q.cx, controller.parameters.cx) vector_0 = vector_0_marker_1_position - vector_0_marker_0_position vector_1 = vector_1_marker_1_position - vector_1_marker_0_position diff --git a/bioptim/models/biorbd/biorbd_model.py b/bioptim/models/biorbd/biorbd_model.py index 800dfee01..6f95347a5 100644 --- a/bioptim/models/biorbd/biorbd_model.py +++ b/bioptim/models/biorbd/biorbd_model.py @@ -13,6 +13,7 @@ from ...limits.path_conditions import Bounds from ...misc.mapping import BiMapping, BiMappingList from ...misc.utils import check_version +from ...optimization.parameters import ParameterList check_version(biorbd, "1.11.1", "1.12.0") @@ -27,11 +28,14 @@ def __init__( bio_model: str | biorbd.Model, friction_coefficients: np.ndarray = None, segments_to_apply_external_forces: list[str] = [], + parameters: ParameterList = None, ): if not isinstance(bio_model, str) and not isinstance(bio_model, biorbd.Model): raise ValueError("The model should be of type 'str' or 'biorbd.Model'") self.model = biorbd.Model(bio_model) if isinstance(bio_model, str) else bio_model + for param_key in parameters: + parameters[param_key].apply_parameter(self) self._friction_coefficients = friction_coefficients self._segments_to_apply_external_forces = segments_to_apply_external_forces @@ -43,6 +47,7 @@ def __init__( self.tau = MX.sym("tau_mx", self.nb_tau, 1) self.muscle = MX.sym("muscle_mx", self.nb_muscles, 1) self.external_forces = MX.sym("external_forces_mx", 9, len(segments_to_apply_external_forces)) + self.parameters = parameters.mx if parameters else MX() @property def name(self) -> str: @@ -78,7 +83,7 @@ def gravity(self) -> Function: biorbd_return = self.model.getGravity().to_mx() casadi_fun = Function( "gravity", - [MX()], + [self.parameters], [biorbd_return], ) return casadi_fun @@ -149,7 +154,7 @@ def homogeneous_matrices_in_global(self, segment_idx, inverse=False) -> Function biorbd_return = jcs.T if inverse else jcs casadi_fun = Function( "homogeneous_matrices_in_global", - [self.q], + [self.q, self.parameters], [biorbd_return], ) return casadi_fun @@ -163,7 +168,7 @@ def homogeneous_matrices_in_child(self, segment_id) -> Function: biorbd_return = self.model.localJCS(segment_id).to_mx() casadi_fun = Function( "homogeneous_matrices_in_child", - [MX()], + [self.parameters], [biorbd_return], ) return casadi_fun @@ -178,7 +183,7 @@ def mass(self) -> Function: biorbd_return = self.model.mass().to_mx() casadi_fun = Function( "mass", - [MX()], + [self.parameters], [biorbd_return], ) return casadi_fun @@ -188,7 +193,7 @@ def rt(self, rt_index) -> Function: biorbd_return = self.model.RT(q_biorbd, rt_index).to_mx() casadi_fun = Function( "rt", - [self.q], + [self.q, self.parameters], [biorbd_return], ) return casadi_fun @@ -198,7 +203,7 @@ def center_of_mass(self) -> Function: biorbd_return = self.model.CoM(q_biorbd, True).to_mx() casadi_fun = Function( "center_of_mass", - [self.q], + [self.q, self.parameters], [biorbd_return], ) return casadi_fun @@ -209,7 +214,7 @@ def center_of_mass_velocity(self) -> Function: biorbd_return = self.model.CoMdot(q_biorbd, qdot_biorbd, True).to_mx() casadi_fun = Function( "center_of_mass_velocity", - [self.q, self.qdot], + [self.q, self.qdot, self.parameters], [biorbd_return], ) return casadi_fun @@ -221,7 +226,7 @@ def center_of_mass_acceleration(self) -> Function: biorbd_return = self.model.CoMddot(q_biorbd, qdot_biorbd, qddot_biorbd, True).to_mx() casadi_fun = Function( "center_of_mass_acceleration", - [self.q, self.qdot, self.qddot], + [self.q, self.qdot, self.qddot, self.parameters], [biorbd_return], ) return casadi_fun @@ -232,7 +237,7 @@ def body_rotation_rate(self) -> Function: biorbd_return = self.model.bodyAngularVelocity(q_biorbd, qdot_biorbd, True).to_mx() casadi_fun = Function( "body_rotation_rate", - [self.q, self.qdot], + [self.q, self.qdot, self.parameters], [biorbd_return], ) return casadi_fun @@ -242,7 +247,7 @@ def mass_matrix(self) -> Function: biorbd_return = self.model.massMatrix(q_biorbd).to_mx() casadi_fun = Function( "mass_matrix", - [self.q], + [self.q, self.parameters], [biorbd_return], ) return casadi_fun @@ -253,7 +258,7 @@ def non_linear_effects(self) -> Function: biorbd_return = self.model.NonLinearEffect(q_biorbd, qdot_biorbd).to_mx() casadi_fun = Function( "non_linear_effects", - [self.q, self.qdot], + [self.q, self.qdot, self.parameters], [biorbd_return], ) return casadi_fun @@ -264,7 +269,7 @@ def angular_momentum(self) -> Function: biorbd_return = self.model.angularMomentum(q_biorbd, qdot_biorbd, True).to_mx() casadi_fun = Function( "angular_momentum", - [self.q, self.qdot], + [self.q, self.qdot, self.parameters], [biorbd_return], ) return casadi_fun @@ -277,7 +282,7 @@ def reshape_qdot(self, k_stab=1) -> Function: ).to_mx() casadi_fun = Function( "reshape_qdot", - [self.q, self.qdot], + [self.q, self.qdot, self.parameters], [biorbd_return], ) return casadi_fun @@ -291,7 +296,7 @@ def segment_angular_velocity(self, idx) -> Function: biorbd_return = self.model.segmentAngularVelocity(q_biorbd, qdot_biorbd, idx, True).to_mx() casadi_fun = Function( "segment_angular_velocity", - [self.q, self.qdot], + [self.q, self.qdot, self.parameters], [biorbd_return], ) return casadi_fun @@ -318,7 +323,7 @@ def segment_orientation(self, idx) -> Function: ).to_mx() casadi_fun = Function( "segment_orientation", - [self.q], + [self.q, self.parameters], [biorbd_return], ) return casadi_fun @@ -357,11 +362,11 @@ def torque(self) -> Function: """ q_biorbd = GeneralizedCoordinates(self.q) qdot_biorbd = GeneralizedVelocity(self.qdot) - tau_activations_biorbd = self.tau # TODO: Charbie check this + tau_activations_biorbd = self.tau biorbd_return = self.model.torque(tau_activations_biorbd, q_biorbd, qdot_biorbd).to_mx() casadi_fun = Function( "torque_activation", - [self.tau, self.q, self.qdot], + [self.tau, self.q, self.qdot, self.parameters], [biorbd_return], ) return casadi_fun @@ -373,7 +378,7 @@ def forward_dynamics_free_floating_base(self) -> Function: biorbd_return = self.model.ForwardDynamicsFreeFloatingBase(q_biorbd, qdot_biorbd, qddot_joints_biorbd).to_mx() casadi_fun = Function( "forward_dynamics_free_floating_base", - [self.q, self.qdot, self.qddot_joints], + [self.q, self.qdot, self.qddot_joints, self.parameters], [biorbd_return], ) return casadi_fun @@ -420,14 +425,14 @@ def forward_dynamics(self, with_contact: bool = False) -> Function: ).to_mx() casadi_fun = Function( "constrained_forward_dynamics", - [self.q, self.qdot, self.tau, self.external_forces], + [self.q, self.qdot, self.tau, self.external_forces, self.parameters], [biorbd_return], ) else: biorbd_return = self.model.ForwardDynamics(q_biorbd, qdot_biorbd, tau_biorbd, external_forces_set).to_mx() casadi_fun = Function( "forward_dynamics", - [self.q, self.qdot, self.tau, self.external_forces], + [self.q, self.qdot, self.tau, self.external_forces, self.parameters], [biorbd_return], ) return casadi_fun @@ -447,7 +452,7 @@ def inverse_dynamics(self, with_contact: bool = False) -> Function: biorbd_return = self.model.InverseDynamics(q_biorbd, qdot_biorbd, qddot_biorbd, external_forces_set).to_mx() casadi_fun = Function( "inverse_dynamics", - [self.q, self.qdot, self.qddot, self.external_forces], + [self.q, self.qdot, self.qddot, self.external_forces, self.parameters], [biorbd_return], ) return casadi_fun @@ -463,7 +468,7 @@ def contact_forces_from_constrained_forward_dynamics(self) -> Function: ).to_mx() casadi_fun = Function( "contact_forces_from_constrained_forward_dynamics", - [self.q, self.qdot, self.tau, self.external_forces], + [self.q, self.qdot, self.tau, self.external_forces, self.parameters], [biorbd_return], ) return casadi_fun @@ -474,7 +479,7 @@ def qdot_from_impact(self) -> Function: biorbd_return = self.model.ComputeConstraintImpulsesDirect(q_biorbd, qdot_pre_impact_biorbd).to_mx() casadi_fun = Function( "qdot_from_impact", - [self.q, self.qdot], + [self.q, self.qdot, self.parameters], [biorbd_return], ) return casadi_fun @@ -488,7 +493,7 @@ def muscle_activation_dot(self) -> Function: biorbd_return = self.model.activationDot(muscle_states).to_mx() casadi_fun = Function( "muscle_activation_dot", - [self.muscle], + [self.muscle, self.parameters], [biorbd_return], ) return casadi_fun @@ -498,7 +503,7 @@ def muscle_length_jacobian(self) -> Function: biorbd_return = self.model.musclesLengthJacobian(q_biorbd).to_mx() casadi_fun = Function( "muscle_length_jacobian", - [self.q], + [self.q, self.parameters], [biorbd_return], ) return casadi_fun @@ -508,7 +513,7 @@ def muscle_velocity(self) -> Function: biorbd_return = J @ self.qdot casadi_fun = Function( "muscle_velocity", - [self.q, self.qdot], + [self.q, self.qdot, self.parameters], [biorbd_return], ) return casadi_fun @@ -523,7 +528,7 @@ def muscle_joint_torque(self) -> Function: biorbd_return = self.model.muscularJointTorque(muscles_states, q_biorbd, qdot_biorbd).to_mx() casadi_fun = Function( "muscle_joint_torque", - [self.muscle, self.q, self.qdot], + [self.muscle, self.q, self.qdot, self.parameters], [biorbd_return], ) return casadi_fun @@ -532,7 +537,7 @@ def markers(self) -> list[MX]: biorbd_return = [m.to_mx() for m in self.model.markers(GeneralizedCoordinates(self.q))] casadi_fun = Function( "markers", - [self.q], + [self.q, self.parameters], biorbd_return, ) return casadi_fun @@ -552,7 +557,7 @@ def marker(self, index, reference_segment_index=None) -> Function: biorbd_return = marker.to_mx() casadi_fun = Function( "marker", - [self.q], + [self.q, self.parameters], [biorbd_return], ) return casadi_fun @@ -613,7 +618,7 @@ def marker_velocities(self, reference_index=None) -> list[MX]: casadi_fun = Function( "marker_velocities", - [self.q, self.qdot], + [self.q, self.qdot, self.parameters], biorbd_return, ) return casadi_fun @@ -647,7 +652,7 @@ def marker_accelerations(self, reference_index=None) -> list[MX]: casadi_fun = Function( "marker_accelerations", - [self.q, self.qdot, self.qddot], + [self.q, self.qdot, self.qddot, self.parameters], biorbd_return, ) return casadi_fun @@ -659,7 +664,7 @@ def tau_max(self) -> tuple[MX, MX]: torque_max, torque_min = self.model.torqueMax(q_biorbd, qdot_biorbd) casadi_fun = Function( "tau_max", - [self.q, self.qdot], + [self.q, self.qdot, self.parameters], [torque_max.to_mx(), torque_min.to_mx()], ) return casadi_fun @@ -673,7 +678,7 @@ def rigid_contact_acceleration(self, contact_index, contact_axis) -> Function: ).to_mx()[contact_axis] casadi_fun = Function( "rigid_contact_acceleration", - [self.q, self.qdot, self.qddot], + [self.q, self.qdot, self.qddot, self.parameters], [biorbd_return], ) return casadi_fun @@ -682,7 +687,7 @@ def markers_jacobian(self) -> list[MX]: biorbd_return = [m.to_mx() for m in self.model.markersJacobian(GeneralizedCoordinates(self.q))] casadi_fun = Function( "markers_jacobian", - [self.q], + [self.q, self.parameters], biorbd_return, ) return casadi_fun @@ -705,7 +710,7 @@ def soft_contact_forces(self) -> Function: casadi_fun = Function( "soft_contact_forces", - [self.q, self.qdot], + [self.q, self.qdot, self.parameters], [biorbd_return], ) return casadi_fun @@ -735,7 +740,7 @@ def reshape_fext_to_fcontact(self, fext: MX | SX) -> list: casadi_fun_evaluated = Function( "reshape_fext_to_fcontact", - [fext_sym], + [fext_sym, self.parameters], [f_contact_vec], )(fext) return casadi_fun_evaluated @@ -760,7 +765,7 @@ def normalize_state_quaternions(self) -> Function: casadi_fun = Function( "soft_contact_forces", - [self.q], + [self.q, self.parameters], [biorbd_return], ) return casadi_fun @@ -782,7 +787,7 @@ def contact_forces(self) -> Function: ) casadi_fun = Function( "contact_forces", - [self.q, self.qdot, self.tau, self.external_forces], + [self.q, self.qdot, self.tau, self.external_forces, self.parameters], [force], ) return casadi_fun @@ -793,7 +798,7 @@ def passive_joint_torque(self) -> Function: biorbd_return = self.model.passiveJointTorque(q_biorbd, qdot_biorbd).to_mx() casadi_fun = Function( "passive_joint_torque", - [self.q, self.qdot], + [self.q, self.qdot, self.parameters], [biorbd_return], ) return casadi_fun @@ -804,7 +809,7 @@ def ligament_joint_torque(self) -> Function: biorbd_return = self.model.ligamentsJointTorque(q_biorbd, qdot_biorbd).to_mx() casadi_fun = Function( "ligament_joint_torque", - [self.q, self.qdot], + [self.q, self.qdot, self.parameters], [biorbd_return], ) return casadi_fun diff --git a/bioptim/models/biorbd/multi_biorbd_model.py b/bioptim/models/biorbd/multi_biorbd_model.py index 91c526c66..fed481f97 100644 --- a/bioptim/models/biorbd/multi_biorbd_model.py +++ b/bioptim/models/biorbd/multi_biorbd_model.py @@ -37,6 +37,9 @@ def __init__( bio_model: tuple[str | biorbd.Model | BiorbdModel, ...], extra_bio_models: tuple[str | biorbd.Model | BiorbdModel, ...] = (), ): + """ + MultiBiorbdModel does not handle external_forces and parameters yet. + """ self.models = [] if not isinstance(bio_model, tuple): raise ValueError("The models must be a 'str', 'biorbd.Model', 'bioptim.BiorbdModel'" " or a tuple of those") diff --git a/bioptim/optimization/parameters.py b/bioptim/optimization/parameters.py index c2b325745..18b9e960d 100644 --- a/bioptim/optimization/parameters.py +++ b/bioptim/optimization/parameters.py @@ -91,6 +91,15 @@ def cx_end(self): def cx_intermediates_list(self): raise RuntimeError("cx_intermediates_list is not available for parameters, only cx_start is accepted.") + def apply_parameter(self, model): + """ + Apply the parameter variables to the model. This should be called during the creation of the biomodel + """ + if self.function: + param_scaling = self.scaling.scaling + param_reduced = self.mx # because this function will be used directly in the biorbd model + self.function(model, param_reduced * param_scaling, **self.kwargs) + class ParameterList(OptimizationVariableList): """