From 26b9e0b4c337cd1e835489f64b162c67b30a9fa2 Mon Sep 17 00:00:00 2001 From: Charbie Date: Fri, 21 Jun 2024 06:35:13 -0400 Subject: [PATCH 001/108] initial thoughts --- bioptim/dynamics/configure_problem.py | 76 +++++++++---------- bioptim/dynamics/dynamics_functions.py | 20 +++-- bioptim/models/biorbd/biorbd_model.py | 44 +++++++---- bioptim/optimization/optimization_variable.py | 12 +-- bioptim/optimization/parameters.py | 4 +- 5 files changed, 86 insertions(+), 70 deletions(-) diff --git a/bioptim/dynamics/configure_problem.py b/bioptim/dynamics/configure_problem.py index 9c0f4d622..de2cddb3b 100644 --- a/bioptim/dynamics/configure_problem.py +++ b/bioptim/dynamics/configure_problem.py @@ -293,21 +293,21 @@ def torque_driven( external_forces=external_forces, ) - # Configure the contact forces - if with_contact: - ConfigureProblem.configure_contact_function( - ocp, nlp, DynamicsFunctions.forces_from_torque_driven, external_forces=external_forces - ) - # Configure the soft contact forces - ConfigureProblem.configure_soft_contact_function(ocp, nlp) - # Algebraic constraints of soft contact forces if needed - if soft_contacts_dynamics == SoftContactDynamics.CONSTRAINT: - ocp.implicit_constraints.add( - ImplicitConstraintFcn.SOFT_CONTACTS_EQUALS_SOFT_CONTACTS_DYNAMICS, - node=Node.ALL_SHOOTING, - penalty_type=ConstraintType.IMPLICIT, - phase=nlp.phase_idx, - ) + # # Configure the contact forces + # if with_contact: + # ConfigureProblem.configure_contact_function( + # ocp, nlp, DynamicsFunctions.forces_from_torque_driven, external_forces=external_forces + # ) + # # Configure the soft contact forces + # ConfigureProblem.configure_soft_contact_function(ocp, nlp) + # # Algebraic constraints of soft contact forces if needed + # if soft_contacts_dynamics == SoftContactDynamics.CONSTRAINT: + # ocp.implicit_constraints.add( + # ImplicitConstraintFcn.SOFT_CONTACTS_EQUALS_SOFT_CONTACTS_DYNAMICS, + # node=Node.ALL_SHOOTING, + # penalty_type=ConstraintType.IMPLICIT, + # phase=nlp.phase_idx, + # ) @staticmethod def torque_driven_free_floating_base( @@ -1179,12 +1179,12 @@ def configure_dynamics_function(ocp, nlp, dyn_func, **extra_params): DynamicsFunctions.apply_parameters(nlp) dynamics_eval = dyn_func( - nlp.time_mx, - nlp.states.scaled.mx_reduced, - nlp.controls.scaled.mx_reduced, - nlp.parameters.scaled.mx_reduced, - nlp.algebraic_states.scaled.mx_reduced, - nlp.numerical_timeseries.mx, + nlp.time_cx, + nlp.states.scaled.cx, + nlp.controls.scaled.cx, + nlp.parameters.scaled.cx, + nlp.algebraic_states.scaled.cx, + nlp.numerical_timeseries.cx, nlp, **extra_params, ) @@ -1192,17 +1192,17 @@ def configure_dynamics_function(ocp, nlp, dyn_func, **extra_params): if isinstance(dynamics_dxdt, (list, tuple)): dynamics_dxdt = vertcat(*dynamics_dxdt) - time_span_sym = vertcat(nlp.time_mx, nlp.dt_mx) + time_span_sym = vertcat(nlp.time_cx, nlp.dt) if nlp.dynamics_func is None: nlp.dynamics_func = Function( "ForwardDyn", [ time_span_sym, - nlp.states.scaled.mx_reduced, - nlp.controls.scaled.mx_reduced, - nlp.parameters.scaled.mx_reduced, - nlp.algebraic_states.scaled.mx_reduced, - nlp.numerical_timeseries.mx, + nlp.states.scaled.cx, + nlp.controls.scaled.cx, + nlp.parameters.scaled.cx, + nlp.algebraic_states.scaled.cx, + nlp.numerical_timeseries.cx, ], [dynamics_dxdt], ["t_span", "x", "u", "p", "a", "d"], @@ -1227,12 +1227,12 @@ def configure_dynamics_function(ocp, nlp, dyn_func, **extra_params): "DynamicsDefects", [ time_span_sym, - nlp.states.scaled.mx_reduced, - nlp.controls.scaled.mx_reduced, - nlp.parameters.scaled.mx_reduced, - nlp.algebraic_states.scaled.mx_reduced, - nlp.numerical_timeseries.mx, - nlp.states_dot.scaled.mx_reduced, + nlp.states.scaled.cx, + nlp.controls.scaled.cx, + nlp.parameters.scaled.cx, + nlp.algebraic_states.scaled.cx, + nlp.numerical_timeseries.cx, + nlp.states_dot.scaled.cx, ], [dynamics_eval.defects], ["t_span", "x", "u", "p", "a", "d", "xdot"], @@ -1256,11 +1256,11 @@ def configure_dynamics_function(ocp, nlp, dyn_func, **extra_params): "ForwardDyn", [ time_span_sym, - nlp.states.scaled.mx_reduced, - nlp.controls.scaled.mx_reduced, - nlp.parameters.scaled.mx_reduced, - nlp.algebraic_states.scaled.mx_reduced, - nlp.numerical_timeseries.mx, + nlp.states.scaled.cx, + nlp.controls.scaled.cx, + nlp.parameters.scaled.cx, + nlp.algebraic_states.scaled.cx, + nlp.numerical_timeseries.cx, ], [dynamics_dxdt], ["t_span", "x", "u", "p", "a", "d"], diff --git a/bioptim/dynamics/dynamics_functions.py b/bioptim/dynamics/dynamics_functions.py index 6b17fadad..b58f3b91f 100644 --- a/bioptim/dynamics/dynamics_functions.py +++ b/bioptim/dynamics/dynamics_functions.py @@ -146,10 +146,13 @@ 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.friction_coefficients @ qdot if with_friction else tau + # Charbie: to remove + tau = DynamicsFunctions.get(nlp.controls["tau"], controls) + # + # 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.friction_coefficients @ qdot if with_friction else tau if ( rigidbody_dynamics == RigidBodyDynamics.DAE_INVERSE_DYNAMICS @@ -169,7 +172,7 @@ def torque_driven( dxdt[nlp.states["qddot"].index, :] = DynamicsFunctions.get(nlp.controls["qdddot"], controls) else: ddq = DynamicsFunctions.forward_dynamics(nlp, q, qdot, tau, with_contact, external_forces) - dxdt = MX(nlp.states.shape, ddq.shape[1]) + dxdt = nlp.cx(nlp.states.shape, ddq.shape[1]) dxdt[nlp.states["q"].index, :] = horzcat(*[dq for _ in range(ddq.shape[1])]) dxdt[nlp.states["qdot"].index, :] = ddq @@ -1090,7 +1093,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)) @staticmethod def forward_dynamics( @@ -1122,6 +1125,7 @@ def forward_dynamics( ------- The derivative of qdot """ + # Get the mapping of the output if "qdot" in nlp.states: qdot_var_mapping = nlp.states["qdot"].mapping.to_first elif "qdot" in nlp.controls: @@ -1133,14 +1137,14 @@ def forward_dynamics( if with_contact: qddot = nlp.model.constrained_forward_dynamics(q, qdot, tau) else: - qddot = nlp.model.forward_dynamics(q, qdot, tau) + qddot = nlp.model.forward_dynamics()(q, qdot, tau) return qdot_var_mapping.map(qddot) else: if with_contact: qddot = nlp.model.constrained_forward_dynamics(q, qdot, tau, external_forces) else: - qddot = nlp.model.forward_dynamics(q, qdot, tau, external_forces) + qddot = nlp.model.forward_dynamics()(q, qdot, tau, external_forces) return qdot_var_mapping.map(qddot) @staticmethod diff --git a/bioptim/models/biorbd/biorbd_model.py b/bioptim/models/biorbd/biorbd_model.py index 3b6dac313..64e3299c2 100644 --- a/bioptim/models/biorbd/biorbd_model.py +++ b/bioptim/models/biorbd/biorbd_model.py @@ -6,8 +6,8 @@ GeneralizedTorque, GeneralizedAcceleration, ) -from casadi import SX, MX, vertcat, horzcat, norm_fro -from typing import Callable +from casadi import SX, MX, vertcat, horzcat, norm_fro, Function +from typing import Callable, Any from ..utils import _var_mapping, bounds_from_ranges from ...limits.path_conditions import Bounds @@ -208,14 +208,20 @@ def angular_momentum(self, q, qdot) -> MX: qdot_biorbd = GeneralizedVelocity(qdot) return self.model.angularMomentum(q_biorbd, qdot_biorbd, True).to_mx() - def reshape_qdot(self, q, qdot, k_stab=1) -> MX: - self.check_q_size(q) - self.check_qdot_size(qdot) - return self.model.computeQdot( - GeneralizedCoordinates(q), - GeneralizedCoordinates(qdot), # mistake in biorbd + def reshape_qdot(self, k_stab=1) -> MX: + q_mx_reduced = MX.sym("q_mx_reduced", self.nb_q, 1) + qdot_mx_reduced = MX.sym("qdot_mx_reduced", self.nb_qdot, 1) + biorbd_return = self.model.computeQdot( + GeneralizedCoordinates(q_mx_reduced), + GeneralizedCoordinates(qdot_mx_reduced), # mistake in biorbd k_stab, ).to_mx() + casadi_fun = Function( + "reshape_qdot", + [q_mx_reduced, qdot_mx_reduced], + [biorbd_return], + ) + return casadi_fun def segment_angular_velocity(self, q, qdot, idx) -> MX: """ @@ -345,16 +351,22 @@ def _dispatch_forces(self, external_forces: MX, translational_forces: MX): return external_forces_set - def forward_dynamics(self, q, qdot, tau, external_forces=None, translational_forces=None) -> MX: - self.check_q_size(q) - self.check_qdot_size(qdot) - self.check_tau_size(tau) + def forward_dynamics(self, external_forces=None, translational_forces=None) -> MX: + q_mx_reduced = MX.sym("q_mx_reduced", self.nb_q, 1) + qdot_mx_reduced = MX.sym("qdot_mx_reduced", self.nb_qdot, 1) + tau_mx_reduced = MX.sym("tau_mx_reduced", self.nb_tau, 1) external_forces_set = self._dispatch_forces(external_forces, translational_forces) - q_biorbd = GeneralizedCoordinates(q) - qdot_biorbd = GeneralizedVelocity(qdot) - tau_biorbd = GeneralizedTorque(tau) - return self.model.ForwardDynamics(q_biorbd, qdot_biorbd, tau_biorbd, external_forces_set).to_mx() + q_biorbd = GeneralizedCoordinates(q_mx_reduced) + qdot_biorbd = GeneralizedVelocity(qdot_mx_reduced) + tau_biorbd = GeneralizedTorque(tau_mx_reduced) + biorbd_return = self.model.ForwardDynamics(q_biorbd, qdot_biorbd, tau_biorbd, external_forces_set).to_mx() + casadi_fun = Function( + "forward_dynamics", + [q_mx_reduced, qdot_mx_reduced, tau_mx_reduced], + [biorbd_return], + ) + return casadi_fun def constrained_forward_dynamics(self, q, qdot, tau, external_forces=None, translational_forces=None) -> MX: external_forces_set = self._dispatch_forces(external_forces, translational_forces) diff --git a/bioptim/optimization/optimization_variable.py b/bioptim/optimization/optimization_variable.py index a9a7a8578..9b5f812c2 100644 --- a/bioptim/optimization/optimization_variable.py +++ b/bioptim/optimization/optimization_variable.py @@ -189,7 +189,7 @@ def __init__(self, cx_constructor, phase_dynamics): self._cx_mid: MX | SX | np.ndarray = np.array([]) self._cx_end: MX | SX | np.ndarray = np.array([]) self._cx_intermediates: list = [] - self.mx_reduced: MX = MX.sym("var", 0, 0) + # self.mx_reduced: MX = MX.sym("var", 0, 0) self.cx_constructor = cx_constructor self._current_cx_to_get = 0 self.phase_dynamics = phase_dynamics @@ -310,7 +310,7 @@ def append(self, name: str, cx: list, mx: MX, bimapping: BiMapping): else: self._cx_intermediates[i] = vertcat(self._cx_intermediates[i], c) - self.mx_reduced = vertcat(self.mx_reduced, MX.sym("var", cx[0].shape[0])) + # self.mx_reduced = vertcat(self.mx_reduced, MX.sym("var", cx[0].shape[0])) self.elements.append(OptimizationVariable(name, mx, cx, index, bimapping, parent_list=self)) def append_from_scaled( @@ -346,7 +346,7 @@ def append_from_scaled( else: self._cx_intermediates[i] = vertcat(self._cx_intermediates[i], c) - self.mx_reduced = scaled_optimization_variable.mx_reduced + # self.mx_reduced = scaled_optimization_variable.mx_reduced var = scaled_optimization_variable[name] self.elements.append(OptimizationVariable(name, var.mx, cx, var.index, var.mapping, self)) @@ -564,9 +564,9 @@ def shape(self): def mx(self): return self.unscaled.mx - @property - def mx_reduced(self): - return self.unscaled.mx_reduced + # @property + # def mx_reduced(self): + # return self.unscaled.mx_reduced @property def cx(self): diff --git a/bioptim/optimization/parameters.py b/bioptim/optimization/parameters.py index f6df2e3a7..c8bf19254 100644 --- a/bioptim/optimization/parameters.py +++ b/bioptim/optimization/parameters.py @@ -170,7 +170,7 @@ def add( self.scaling.add(key=name, scaling=scaling) index = range(self._cx_start.shape[0], self._cx_start.shape[0] + cx[0].shape[0]) self._cx_start = vertcat(self._cx_start, cx[0]) - self.mx_reduced = vertcat(self.mx_reduced, MX.sym("var", cx[0].shape[0])) + # self.mx_reduced = vertcat(self.mx_reduced, MX.sym("var", cx[0].shape[0])) mx = MX.sym(name, size) self.elements.append( Parameter( @@ -224,7 +224,7 @@ def to_unscaled( unscaled_parameter._cx_start = vertcat( unscaled_parameter._cx_start, element.cx_start * element.scaling.scaling ) - unscaled_parameter.mx_reduced = vertcat(unscaled_parameter.mx_reduced, element.mx * element.scaling.scaling) + # unscaled_parameter.mx_reduced = vertcat(unscaled_parameter.cx, element.cx * element.scaling.scaling) return unscaled_parameter From a05e823cc2ac3d46d05c2f0bc69c087aa2f16d86 Mon Sep 17 00:00:00 2001 From: Charbie Date: Fri, 21 Jun 2024 18:35:08 -0400 Subject: [PATCH 002/108] made the changes to the biorbd_model --- bioptim/models/biorbd/biorbd_model.py | 696 ++++++++++++++++---------- 1 file changed, 422 insertions(+), 274 deletions(-) diff --git a/bioptim/models/biorbd/biorbd_model.py b/bioptim/models/biorbd/biorbd_model.py index 64e3299c2..1440acceb 100644 --- a/bioptim/models/biorbd/biorbd_model.py +++ b/bioptim/models/biorbd/biorbd_model.py @@ -35,6 +35,14 @@ def __init__( self._friction_coefficients = friction_coefficients self._segments_to_apply_external_forces = segments_to_apply_external_forces + # Declaration of MX variables of the right shape for the creation of CasADi Functions + self.q = MX.sym("q_mx", self.nb_q, 1) + self.qdot = MX.sym("qdot_mx", self.nb_qdot, 1) + self.qddot = MX.sym("qddot_mx", self.nb_qddot, 1) + self.qddot_joints = MX.sym("qddot_joints_mx", self.nb_qddot - self.nb_root, 1) + self.tau = MX.sym("tau_mx", self.nb_tau, 1) + self.muscle = MX.sym("muscle_mx", self.nb_muscles, 1) + @property def name(self) -> str: # parse the path and split to get the .bioMod name @@ -51,7 +59,7 @@ def serialize(self) -> tuple[Callable, dict]: return BiorbdModel, dict(bio_model=self.path) @property - def friction_coefficients(self) -> MX | np.ndarray: + def friction_coefficients(self) -> MX | SX | np.ndarray: return self._friction_coefficients def set_friction_coefficients(self, new_friction_coefficients) -> None: @@ -60,8 +68,19 @@ def set_friction_coefficients(self, new_friction_coefficients) -> None: return self._friction_coefficients @property - def gravity(self) -> MX: - return self.model.getGravity().to_mx() + def gravity(self) -> Function: + """ + Returns the gravity of the model. + Since the gravity is self-defined in the model, you need to provide the type of the output when calling the function like this: + model.gravity()(MX() / SX()) + """ + biorbd_return = self.model.getGravity().to_mx() + casadi_fun = Function( + "gravity", + [MX()], + [biorbd_return], + ) + return casadi_fun def set_gravity(self, new_gravity) -> None: self.model.setGravity(new_gravity) @@ -110,136 +129,162 @@ def biorbd_homogeneous_matrices_in_global(self, q, segment_idx, inverse=False) - """ Returns a biorbd object containing the roto-translation matrix of the segment in the global reference frame. This is useful if you want to interact with biorbd directly later on. + TODO: Charbie fis this with ApplyRT wrapper """ rt_matrix = self.model.globalJCS(GeneralizedCoordinates(q), segment_idx) return rt_matrix.transpose() if inverse else rt_matrix - def homogeneous_matrices_in_global(self, q, segment_idx, inverse=False) -> MX: + def homogeneous_matrices_in_global(self, segment_idx, inverse=False) -> Function: """ Returns the roto-translation matrix of the segment in the global reference frame. """ - return self.biorbd_homogeneous_matrices_in_global(q, segment_idx, inverse).to_mx() + biorbd_return = self.biorbd_homogeneous_matrices_in_global(self.q, segment_idx, inverse).to_mx() + casadi_fun = Function( + "homogeneous_matrices_in_global", + [self.q], + [biorbd_return], + ) + return casadi_fun - def homogeneous_matrices_in_child(self, segment_id) -> MX: - return self.model.localJCS(segment_id).to_mx() + def homogeneous_matrices_in_child(self, segment_id) -> Function: + """ + Returns the roto-translation matrix of the segment in the child reference frame. + Since the homogeneous matrix is self-defined in the model, you need to provide the type of the output when calling the function like this: + model.homogeneous_matrices_in_child(segment_id)(MX() / SX()) + """ + biorbd_return = self.model.localJCS(segment_id).to_mx() + casadi_fun = Function( + "homogeneous_matrices_in_child", + [MX()], + [biorbd_return], + ) + return casadi_fun @property - def mass(self) -> MX: - return self.model.mass().to_mx() - - def check_q_size(self, q): - if q.shape[0] != self.nb_q: - raise ValueError(f"Length of q size should be: {self.nb_q}, but got: {q.shape[0]}") - - def check_qdot_size(self, qdot): - if qdot.shape[0] != self.nb_qdot: - raise ValueError(f"Length of qdot size should be: {self.nb_qdot}, but got: {qdot.shape[0]}") - - def check_qddot_size(self, qddot): - if qddot.shape[0] != self.nb_qddot: - raise ValueError(f"Length of qddot size should be: {self.nb_qddot}, but got: {qddot.shape[0]}") - - def check_qddot_joints_size(self, qddot_joints): - nb_qddot_joints = self.nb_q - self.nb_root - if qddot_joints.shape[0] != nb_qddot_joints: - raise ValueError( - f"Length of qddot_joints size should be: {nb_qddot_joints}, but got: {qddot_joints.shape[0]}" - ) + def mass(self) -> Function: + """ + Returns the mass of the model. + Since the mass is self-defined in the model, you need to provide the type of the output when calling the function like this: + model.mass()(MX() / SX()) + """ + biorbd_return = self.model.mass().to_mx() + casadi_fun = Function( + "mass", + [MX()], + [biorbd_return], + ) + return casadi_fun - def check_tau_size(self, tau): - if tau.shape[0] != self.nb_tau: - raise ValueError(f"Length of tau size should be: {self.nb_tau}, but got: {tau.shape[0]}") + def center_of_mass(self) -> Function: + q_biorbd = GeneralizedCoordinates(self.q) + biorbd_return = self.model.CoM(q_biorbd, True).to_mx() + casadi_fun = Function( + "center_of_mass", + [self.q], + [biorbd_return], + ) + return casadi_fun - def check_muscle_size(self, muscle): - if isinstance(muscle, list): - muscle_size = len(muscle) - elif hasattr(muscle, "shape"): - muscle_size = muscle.shape[0] - else: - raise TypeError("Unsupported type for muscle.") - - if muscle_size != self.nb_muscles: - raise ValueError(f"Length of muscle size should be: {self.nb_muscles}, but got: {muscle_size}") - - def center_of_mass(self, q) -> MX: - self.check_q_size(q) - q_biorbd = GeneralizedCoordinates(q) - return self.model.CoM(q_biorbd, True).to_mx() - - def center_of_mass_velocity(self, q, qdot) -> MX: - self.check_q_size(q) - self.check_qdot_size(qdot) - q_biorbd = GeneralizedCoordinates(q) - qdot_biorbd = GeneralizedVelocity(qdot) - return self.model.CoMdot(q_biorbd, qdot_biorbd, True).to_mx() - - def center_of_mass_acceleration(self, q, qdot, qddot) -> MX: - self.check_q_size(q) - self.check_qdot_size(qdot) - self.check_qddot_size(qddot) - q_biorbd = GeneralizedCoordinates(q) - qdot_biorbd = GeneralizedVelocity(qdot) - qddot_biorbd = GeneralizedAcceleration(qddot) - return self.model.CoMddot(q_biorbd, qdot_biorbd, qddot_biorbd, True).to_mx() - - def body_rotation_rate(self, q, qdot) -> MX: - self.check_q_size(q) - self.check_qdot_size(qdot) - q_biorbd = GeneralizedCoordinates(q) - qdot_biorbd = GeneralizedVelocity(qdot) - return self.model.bodyAngularVelocity(q_biorbd, qdot_biorbd, True).to_mx() - - def mass_matrix(self, q) -> MX: - self.check_q_size(q) - q_biorbd = GeneralizedCoordinates(q) - return self.model.massMatrix(q_biorbd).to_mx() - - def non_linear_effects(self, q, qdot) -> MX: - self.check_q_size(q) - self.check_qdot_size(qdot) - q_biorbd = GeneralizedCoordinates(q) - qdot_biorbd = GeneralizedVelocity(qdot) - return self.model.NonLinearEffect(q_biorbd, qdot_biorbd).to_mx() - - def angular_momentum(self, q, qdot) -> MX: - self.check_q_size(q) - self.check_qdot_size(qdot) - q_biorbd = GeneralizedCoordinates(q) - qdot_biorbd = GeneralizedVelocity(qdot) - return self.model.angularMomentum(q_biorbd, qdot_biorbd, True).to_mx() - - def reshape_qdot(self, k_stab=1) -> MX: - q_mx_reduced = MX.sym("q_mx_reduced", self.nb_q, 1) - qdot_mx_reduced = MX.sym("qdot_mx_reduced", self.nb_qdot, 1) + def center_of_mass_velocity(self) -> Function: + q_biorbd = GeneralizedCoordinates(self.q) + qdot_biorbd = GeneralizedVelocity(self.qdot) + biorbd_return = self.model.CoMdot(q_biorbd, qdot_biorbd, True).to_mx() + casadi_fun = Function( + "center_of_mass_velocity", + [self.q, self.qdot], + [biorbd_return], + ) + return casadi_fun + + def center_of_mass_acceleration(self) -> Function: + q_biorbd = GeneralizedCoordinates(self.q) + qdot_biorbd = GeneralizedVelocity(self.qdot) + qddot_biorbd = GeneralizedAcceleration(self.qddot) + 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.tau], + [biorbd_return], + ) + return casadi_fun + + def body_rotation_rate(self) -> Function: + q_biorbd = GeneralizedCoordinates(self.q) + qdot_biorbd = GeneralizedVelocity(self.qdot) + biorbd_return = self.model.bodyAngularVelocity(q_biorbd, qdot_biorbd, True).to_mx() + casadi_fun = Function( + "body_rotation_rate", + [self.q, self.qdot], + [biorbd_return], + ) + return casadi_fun + + def mass_matrix(self) -> Function: + q_biorbd = GeneralizedCoordinates(self.q) + biorbd_return = self.model.massMatrix(q_biorbd).to_mx() + casadi_fun = Function( + "mass_matrix", + [self.q], + [biorbd_return], + ) + return casadi_fun + + def non_linear_effects(self) -> Function: + q_biorbd = GeneralizedCoordinates(self.q) + qdot_biorbd = GeneralizedVelocity(self.qdot) + biorbd_return = self.model.NonLinearEffect(q_biorbd, qdot_biorbd).to_mx() + casadi_fun = Function( + "non_linear_effects", + [self.q, self.qdot], + [biorbd_return], + ) + return casadi_fun + + def angular_momentum(self) -> Function: + q_biorbd = GeneralizedCoordinates(self.q) + qdot_biorbd = GeneralizedVelocity(self.qdot) + biorbd_return = self.model.angularMomentum(q_biorbd, qdot_biorbd, True).to_mx() + casadi_fun = Function( + "angular_momentum", + [self.q, self.qdot], + [biorbd_return], + ) + return casadi_fun + + def reshape_qdot(self, k_stab=1) -> Function: biorbd_return = self.model.computeQdot( - GeneralizedCoordinates(q_mx_reduced), - GeneralizedCoordinates(qdot_mx_reduced), # mistake in biorbd + GeneralizedCoordinates(self.q), + GeneralizedCoordinates(self.qdot), # mistake in biorbd k_stab, ).to_mx() casadi_fun = Function( "reshape_qdot", - [q_mx_reduced, qdot_mx_reduced], + [self.q, self.qdot], [biorbd_return], ) return casadi_fun - def segment_angular_velocity(self, q, qdot, idx) -> MX: + def segment_angular_velocity(self, idx) -> Function: """ Returns the angular velocity of the segment in the global reference frame. """ - self.check_q_size(q) - self.check_qdot_size(qdot) - q_biorbd = GeneralizedCoordinates(q) - qdot_biorbd = GeneralizedVelocity(qdot) - return self.model.segmentAngularVelocity(q_biorbd, qdot_biorbd, idx, True).to_mx() + q_biorbd = GeneralizedCoordinates(self.q) + qdot_biorbd = GeneralizedVelocity(self.qdot) + biorbd_return = self.model.segmentAngularVelocity(q_biorbd, qdot_biorbd, idx, True).to_mx() + casadi_fun = Function( + "segment_angular_velocity", + [self.q, self.qdot], + [biorbd_return], + ) + return casadi_fun - def segment_orientation(self, q, idx) -> MX: + def segment_orientation(self, idx) -> Function: """ Returns the angular position of the segment in the global reference frame. """ - q_biorbd = GeneralizedCoordinates(q) + q_biorbd = GeneralizedCoordinates(self.q) rotation_matrix = self.homogeneous_matrices_in_global(q_biorbd, idx)[:3, :3] - segment_orientation = biorbd.Rotation.toEulerAngles( + biorbd_return = biorbd.Rotation.toEulerAngles( biorbd.Rotation( rotation_matrix[0, 0], rotation_matrix[0, 1], @@ -253,7 +298,12 @@ def segment_orientation(self, q, idx) -> MX: ), "xyz", ).to_mx() - return segment_orientation + casadi_fun = Function( + "segment_orientation", + [self.q], + [biorbd_return], + ) + return casadi_fun @property def name_dof(self) -> tuple[str, ...]: @@ -282,26 +332,36 @@ def muscle_names(self) -> tuple[str, ...]: def nb_muscles(self) -> int: return self.model.nbMuscles() - def torque(self, tau_activations, q, qdot) -> MX: - self.check_tau_size(tau_activations) - self.check_q_size(q) - self.check_qdot_size(qdot) - q_biorbd = GeneralizedCoordinates(q) - qdot_biorbd = GeneralizedVelocity(qdot) - tau_activation = self.model.torque(tau_activations, q_biorbd, qdot_biorbd) - return tau_activation.to_mx() - - def forward_dynamics_free_floating_base(self, q, qdot, qddot_joints) -> MX: - self.check_q_size(q) - self.check_qdot_size(qdot) - self.check_qddot_joints_size(qddot_joints) - q_biorbd = GeneralizedCoordinates(q) - qdot_biorbd = GeneralizedVelocity(qdot) - qddot_joints_biorbd = GeneralizedAcceleration(qddot_joints) - return self.model.ForwardDynamicsFreeFloatingBase(q_biorbd, qdot_biorbd, qddot_joints_biorbd).to_mx() + def torque(self) -> Function: + """ + Returns the torque from the torque_activations. + Note that tau_activation should be between 0 and 1. + """ + q_biorbd = GeneralizedCoordinates(self.q) + qdot_biorbd = GeneralizedVelocity(self.qdot) + tau_activations_biorbd = self.tau #TODO: Charbie check this + 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], + [biorbd_return], + ) + return casadi_fun + + def forward_dynamics_free_floating_base(self) -> Function: + q_biorbd = GeneralizedCoordinates(self.q) + qdot_biorbd = GeneralizedVelocity(self.qdot) + qddot_joints_biorbd = GeneralizedAcceleration(self.qddot_joints) + 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], + [biorbd_return], + ) + return casadi_fun @staticmethod - def reorder_qddot_root_joints(qddot_root, qddot_joints) -> MX: + def reorder_qddot_root_joints(qddot_root, qddot_joints) -> MX | SX: return vertcat(qddot_root, qddot_joints) def _dispatch_forces(self, external_forces: MX, translational_forces: MX): @@ -351,102 +411,147 @@ def _dispatch_forces(self, external_forces: MX, translational_forces: MX): return external_forces_set - def forward_dynamics(self, external_forces=None, translational_forces=None) -> MX: - q_mx_reduced = MX.sym("q_mx_reduced", self.nb_q, 1) - qdot_mx_reduced = MX.sym("qdot_mx_reduced", self.nb_qdot, 1) - tau_mx_reduced = MX.sym("tau_mx_reduced", self.nb_tau, 1) + def forward_dynamics(self, external_forces=None, translational_forces=None) -> Function: + """ + TODO: Charbie create a different function for with external_forces and translational_forces + """ external_forces_set = self._dispatch_forces(external_forces, translational_forces) - q_biorbd = GeneralizedCoordinates(q_mx_reduced) - qdot_biorbd = GeneralizedVelocity(qdot_mx_reduced) - tau_biorbd = GeneralizedTorque(tau_mx_reduced) + q_biorbd = GeneralizedCoordinates(self.q) + qdot_biorbd = GeneralizedVelocity(self.qdot) + tau_biorbd = GeneralizedTorque(self.tau) biorbd_return = self.model.ForwardDynamics(q_biorbd, qdot_biorbd, tau_biorbd, external_forces_set).to_mx() casadi_fun = Function( "forward_dynamics", - [q_mx_reduced, qdot_mx_reduced, tau_mx_reduced], + [self.q, self.qdot, self.tau], [biorbd_return], ) return casadi_fun - def constrained_forward_dynamics(self, q, qdot, tau, external_forces=None, translational_forces=None) -> MX: + def constrained_forward_dynamics(self, external_forces=None, translational_forces=None) -> Function: + """ + TODO: Charbie external_forces=None, translational_forces=None + """ external_forces_set = self._dispatch_forces(external_forces, translational_forces) - self.check_q_size(q) - self.check_qdot_size(qdot) - self.check_tau_size(tau) - - q_biorbd = GeneralizedCoordinates(q) - qdot_biorbd = GeneralizedVelocity(qdot) - tau_biorbd = GeneralizedTorque(tau) - return self.model.ForwardDynamicsConstraintsDirect( + + q_biorbd = GeneralizedCoordinates(self.q) + qdot_biorbd = GeneralizedVelocity(self.qdot) + tau_biorbd = GeneralizedTorque(self.tau) + biorbd_return = self.model.ForwardDynamicsConstraintsDirect( q_biorbd, qdot_biorbd, tau_biorbd, external_forces_set ).to_mx() + casadi_fun = Function( + "constrained_forward_dynamics", + [self.q, self.qdot, self.tau], + [biorbd_return], + ) + return casadi_fun - def inverse_dynamics(self, q, qdot, qddot, external_forces=None, translational_forces=None) -> MX: + def inverse_dynamics(self, external_forces=None, translational_forces=None) -> Function: + """ + TODO: Charbie external_forces=None, translational_forces=None + """ external_forces_set = self._dispatch_forces(external_forces, translational_forces) - self.check_q_size(q) - self.check_qdot_size(qdot) - self.check_qddot_size(qddot) - q_biorbd = GeneralizedCoordinates(q) - qdot_biorbd = GeneralizedVelocity(qdot) - qddot_biorbd = GeneralizedAcceleration(qddot) - return self.model.InverseDynamics(q_biorbd, qdot_biorbd, qddot_biorbd, external_forces_set).to_mx() + q_biorbd = GeneralizedCoordinates(self.q) + qdot_biorbd = GeneralizedVelocity(self.qdot) + qddot_biorbd = GeneralizedAcceleration(self.qddot) + 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], + [biorbd_return], + ) + return casadi_fun def contact_forces_from_constrained_forward_dynamics( - self, q, qdot, tau, external_forces=None, translational_forces=None - ) -> MX: + self, external_forces=None, translational_forces=None + ) -> Function: + """ + TODO: Charbie external_forces=None, translational_forces=None + """ external_forces_set = self._dispatch_forces(external_forces, translational_forces) - self.check_q_size(q) - self.check_qdot_size(qdot) - self.check_tau_size(tau) - - q_biorbd = GeneralizedCoordinates(q) - qdot_biorbd = GeneralizedVelocity(qdot) - tau_biorbd = GeneralizedTorque(tau) - return self.model.ContactForcesFromForwardDynamicsConstraintsDirect( + + q_biorbd = GeneralizedCoordinates(self.q) + qdot_biorbd = GeneralizedVelocity(self.qdot) + tau_biorbd = GeneralizedTorque(self.tau) + biorbd_return = self.model.ContactForcesFromForwardDynamicsConstraintsDirect( q_biorbd, qdot_biorbd, tau_biorbd, external_forces_set ).to_mx() + casadi_fun = Function( + "contact_forces_from_constrained_forward_dynamics", + [self.q, self.qdot, self.tau], + [biorbd_return], + ) + return casadi_fun - def qdot_from_impact(self, q, qdot_pre_impact) -> MX: - self.check_q_size(q) - self.check_qdot_size(qdot_pre_impact) - q_biorbd = GeneralizedCoordinates(q) - qdot_pre_impact_biorbd = GeneralizedVelocity(qdot_pre_impact) - return self.model.ComputeConstraintImpulsesDirect(q_biorbd, qdot_pre_impact_biorbd).to_mx() + def qdot_from_impact(self) -> Function: + q_biorbd = GeneralizedCoordinates(self.q) + qdot_pre_impact_biorbd = GeneralizedVelocity(self.qdot) + biorbd_return = self.model.ComputeConstraintImpulsesDirect(q_biorbd, qdot_pre_impact_biorbd).to_mx() + casadi_fun = Function( + "qdot_from_impact", + [self.q, self.qdot], + [biorbd_return], + ) + return casadi_fun - def muscle_activation_dot(self, muscle_excitations) -> MX: - self.check_muscle_size(muscle_excitations) + def muscle_activation_dot(self) -> Function: + muscle_excitation_biorbd = self.muscle muscle_states = self.model.stateSet() for k in range(self.model.nbMuscles()): - muscle_states[k].setExcitation(muscle_excitations[k]) - return self.model.activationDot(muscle_states).to_mx() - - def muscle_length_jacobian(self, q) -> MX: - self.check_q_size(q) - q_biorbd = GeneralizedCoordinates(q) - return self.model.musclesLengthJacobian(q_biorbd).to_mx() + muscle_states[k].setExcitation(muscle_excitation_biorbd[k]) + biorbd_return = self.model.activationDot(muscle_states).to_mx() + casadi_fun = Function( + "muscle_activation_dot", + [self.muscle], + [biorbd_return], + ) + return casadi_fun - def muscle_velocity(self, q, qdot) -> MX: - self.check_q_size(q) - self.check_qdot_size(qdot) - J = self.muscle_length_jacobian(q) - return J @ qdot + def muscle_length_jacobian(self) -> Function: + q_biorbd = GeneralizedCoordinates(self.q) + biorbd_return = self.model.musclesLengthJacobian(q_biorbd).to_mx() + casadi_fun = Function( + "muscle_length_jacobian", + [self.q], + [biorbd_return], + ) + return casadi_fun - def muscle_joint_torque(self, activations, q, qdot) -> MX: - self.check_q_size(q) - self.check_qdot_size(qdot) - self.check_muscle_size(activations) + def muscle_velocity(self) -> Function: + J = self.muscle_length_jacobian(self.q) + biorbd_return = J @ self.qdot + casadi_fun = Function( + "muscle_velocity", + [self.q, self.qdot], + [biorbd_return], + ) + return casadi_fun + def muscle_joint_torque(self) -> Function: muscles_states = self.model.stateSet() + muscles_activations = self.muscles for k in range(self.model.nbMuscles()): - muscles_states[k].setActivation(activations[k]) - q_biorbd = GeneralizedCoordinates(q) - qdot_biorbd = GeneralizedVelocity(qdot) - return self.model.muscularJointTorque(muscles_states, q_biorbd, qdot_biorbd).to_mx() + muscles_states[k].setActivation(muscles_activations[k]) + q_biorbd = GeneralizedCoordinates(self.q) + qdot_biorbd = GeneralizedVelocity(self.qdot) + biorbd_return = self.model.muscularJointTorque(muscles_states, q_biorbd, qdot_biorbd).to_mx() + casadi_fun = Function( + "muscle_joint_torque", + [self.q, self.qdot, self.muscle], + [biorbd_return], + ) + return casadi_fun - def markers(self, q) -> list[MX]: - self.check_q_size(q) - return [m.to_mx() for m in self.model.markers(GeneralizedCoordinates(q))] + 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], + biorbd_return, + ) + return casadi_fun @property def nb_markers(self) -> int: @@ -455,13 +560,18 @@ def nb_markers(self) -> int: def marker_index(self, name): return biorbd.marker_index(self.model, name) - def marker(self, q, index, reference_segment_index=None) -> MX: - self.check_q_size(q) - marker = self.model.marker(GeneralizedCoordinates(q), index) + def marker(self, index, reference_segment_index=None) -> Function: + marker = self.model.marker(GeneralizedCoordinates(self.q), index) if reference_segment_index is not None: - global_homogeneous_matrix = self.model.globalJCS(GeneralizedCoordinates(q), reference_segment_index) + global_homogeneous_matrix = self.model.globalJCS(GeneralizedCoordinates(self.q), reference_segment_index) marker.applyRT(global_homogeneous_matrix.transpose()) - return marker.to_mx() + biorbd_return = marker.to_mx() + casadi_fun = Function( + "marker", + [self.q], + [biorbd_return], + ) + return casadi_fun @property def nb_rigid_contacts(self) -> int: @@ -495,107 +605,126 @@ def rigid_contact_index(self, contact_index) -> tuple: """ return self.model.rigidContacts()[contact_index].availableAxesIndices() - def marker_velocities(self, q, qdot, reference_index=None) -> list[MX]: - self.check_q_size(q) - self.check_qdot_size(qdot) + def marker_velocities(self, reference_index=None) -> list[MX]: if reference_index is None: - return [ + biorbd_return = [ m.to_mx() for m in self.model.markersVelocity( - GeneralizedCoordinates(q), - GeneralizedVelocity(qdot), + GeneralizedCoordinates(self.q), + GeneralizedVelocity(self.qdot), True, ) ] else: - out = [] + biorbd_return = [] homogeneous_matrix_transposed = self.biorbd_homogeneous_matrices_in_global( - GeneralizedCoordinates(q), + GeneralizedCoordinates(self.q), reference_index, inverse=True, ) - for m in self.model.markersVelocity(GeneralizedCoordinates(q), GeneralizedVelocity(qdot)): + for m in self.model.markersVelocity(GeneralizedCoordinates(self.q), GeneralizedVelocity(self.qdot)): if m.applyRT(homogeneous_matrix_transposed) is None: - out.append(m.to_mx()) + biorbd_return.append(m.to_mx()) - return out + casadi_fun = Function( + "marker_velocities", + [self.q, self.qdot], + biorbd_return, + ) + return casadi_fun - def marker_accelerations(self, q, qdot, qddot, reference_index=None) -> list[MX]: - self.check_q_size(q) - self.check_qdot_size(qdot) - self.check_qddot_size(qddot) + def marker_accelerations(self, reference_index=None) -> list[MX]: if reference_index is None: - return [ + biorbd_return = [ m.to_mx() for m in self.model.markerAcceleration( - GeneralizedCoordinates(q), - GeneralizedVelocity(qdot), - GeneralizedAcceleration(qddot), + GeneralizedCoordinates(self.q), + GeneralizedVelocity(self.qdot), + GeneralizedAcceleration(self.qddot), True, ) ] else: - out = [] + biorbd_return = [] homogeneous_matrix_transposed = self.biorbd_homogeneous_matrices_in_global( - GeneralizedCoordinates(q), + GeneralizedCoordinates(self.q), reference_index, inverse=True, ) for m in self.model.markersAcceleration( - GeneralizedCoordinates(q), - GeneralizedVelocity(qdot), - GeneralizedAcceleration(qddot), + GeneralizedCoordinates(self.q), + GeneralizedVelocity(self.qdot), + GeneralizedAcceleration(self.qddot), ): if m.applyRT(homogeneous_matrix_transposed) is None: - out.append(m.to_mx()) + biorbd_return.append(m.to_mx()) - return out + casadi_fun = Function( + "marker_accelerations", + [self.q, self.qdot, self.qddot], + biorbd_return, + ) + return casadi_fun - def tau_max(self, q, qdot) -> tuple[MX, MX]: + def tau_max(self) -> tuple[MX, MX]: self.model.closeActuator() - self.check_q_size(q) - self.check_qdot_size(qdot) - q_biorbd = GeneralizedCoordinates(q) - qdot_biorbd = GeneralizedVelocity(qdot) + q_biorbd = GeneralizedCoordinates(self.q) + qdot_biorbd = GeneralizedVelocity(self.qdot) torque_max, torque_min = self.model.torqueMax(q_biorbd, qdot_biorbd) - return torque_max.to_mx(), torque_min.to_mx() - - def rigid_contact_acceleration(self, q, qdot, qddot, contact_index, contact_axis) -> MX: - self.check_q_size(q) - self.check_qdot_size(qdot) - self.check_qddot_size(qddot) - q_biorbd = GeneralizedCoordinates(q) - qdot_biorbd = GeneralizedVelocity(qdot) - qddot_biorbd = GeneralizedAcceleration(qddot) - return self.model.rigidContactAcceleration(q_biorbd, qdot_biorbd, qddot_biorbd, contact_index, True).to_mx()[ + casadi_fun = Function( + "tau_max", + [self.q, self.qdot], + [torque_max.to_mx(), torque_min.to_mx()], + ) + return casadi_fun + + def rigid_contact_acceleration(self, contact_index, contact_axis) -> Function: + q_biorbd = GeneralizedCoordinates(self.q) + qdot_biorbd = GeneralizedVelocity(self.qdot) + qddot_biorbd = GeneralizedAcceleration(self.qddot) + biorbd_return = self.model.rigidContactAcceleration(q_biorbd, qdot_biorbd, qddot_biorbd, contact_index, True).to_mx()[ contact_axis ] + casadi_fun = Function( + "rigid_contact_acceleration", + [self.q, self.qdot, self.qddot], + [biorbd_return], + ) + return casadi_fun - def markers_jacobian(self, q) -> list[MX]: - self.check_q_size(q) - return [m.to_mx() for m in self.model.markersJacobian(GeneralizedCoordinates(q))] + 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], + biorbd_return, + ) + return casadi_fun @property def marker_names(self) -> tuple[str, ...]: return tuple([s.to_string() for s in self.model.markerNames()]) - def soft_contact_forces(self, q, qdot) -> MX: - self.check_q_size(q) - self.check_qdot_size(qdot) - q_biorbd = GeneralizedCoordinates(q) - qdot_biorbd = GeneralizedVelocity(qdot) + def soft_contact_forces(self) -> Function: + q_biorbd = GeneralizedCoordinates(self.q) + qdot_biorbd = GeneralizedVelocity(self.qdot) - soft_contact_forces = MX.zeros(self.nb_soft_contacts * 6, 1) + biorbd_return = MX.zeros(self.nb_soft_contacts * 6, 1) for i_sc in range(self.nb_soft_contacts): soft_contact = self.soft_contact(i_sc) - soft_contact_forces[i_sc * 6 : (i_sc + 1) * 6, :] = ( + biorbd_return[i_sc * 6 : (i_sc + 1) * 6, :] = ( biorbd.SoftContactSphere(soft_contact).computeForceAtOrigin(self.model, q_biorbd, qdot_biorbd).to_mx() ) - return soft_contact_forces + casadi_fun = Function( + "soft_contact_forces", + [self.q, self.qdot], + [biorbd_return], + ) + return casadi_fun def reshape_fext_to_fcontact(self, fext: MX) -> list: if len(self._segments_to_apply_external_forces) == 0: @@ -648,34 +777,47 @@ def get_quaternion_idx(self) -> list[list[int]]: n_dof += self.segments[j].nbDof() return quat_idx - def contact_forces(self, q, qdot, tau, external_forces: MX = None) -> MX: - self.check_q_size(q) - self.check_qdot_size(qdot) - self.check_tau_size(tau) + def contact_forces(self, external_forces: MX = None) -> Function: + """ + TODO: Charbie external_forces=None + """ if external_forces is not None: - all_forces = MX() + biorbd_return = MX() for i in range(external_forces.shape[1]): force = self.contact_forces_from_constrained_forward_dynamics( - q, qdot, tau, external_forces=external_forces[:, i] + self.q, self.qdot, self.tau, external_forces=external_forces[:, i] ) - all_forces = horzcat(all_forces, force) - return all_forces + biorbd_return = horzcat(biorbd_return, force) else: - return self.contact_forces_from_constrained_forward_dynamics(q, qdot, tau, external_forces=None) - - def passive_joint_torque(self, q, qdot) -> MX: - self.check_q_size(q) - self.check_qdot_size(qdot) - q_biorbd = GeneralizedCoordinates(q) - qdot_biorbd = GeneralizedVelocity(qdot) - return self.model.passiveJointTorque(q_biorbd, qdot_biorbd).to_mx() - - def ligament_joint_torque(self, q, qdot) -> MX: - self.check_q_size(q) - self.check_qdot_size(qdot) - q_biorbd = GeneralizedCoordinates(q) - qdot_biorbd = GeneralizedVelocity(qdot) - return self.model.ligamentsJointTorque(q_biorbd, qdot_biorbd).to_mx() + biorbd_return = self.contact_forces_from_constrained_forward_dynamics(self.q, self.qdot, self.tau, external_forces=None) + casadi_fun = Function( + "contact_forces", + [self.q, self.qdot, self.tau], + [biorbd_return], + ) + return casadi_fun + + def passive_joint_torque(self) -> Function: + q_biorbd = GeneralizedCoordinates(self.q) + qdot_biorbd = GeneralizedVelocity(self.qdot) + biorbd_return = self.model.passiveJointTorque(q_biorbd, qdot_biorbd).to_mx() + casadi_fun = Function( + "passive_joint_torque", + [self.q, self.qdot], + [biorbd_return], + ) + return casadi_fun + + def ligament_joint_torque(self) -> Function: + q_biorbd = GeneralizedCoordinates(self.q) + qdot_biorbd = GeneralizedVelocity(self.qdot) + biorbd_return = self.model.ligamentsJointTorque(q_biorbd, qdot_biorbd).to_mx() + casadi_fun = Function( + "ligament_joint_torque", + [self.q, self.qdot], + [biorbd_return], + ) + return casadi_fun def ranges_from_model(self, variable: str): ranges = [] @@ -734,12 +876,18 @@ def _var_mapping( def bounds_from_ranges(self, variables: str | list[str], mapping: BiMapping | BiMappingList = None) -> Bounds: return bounds_from_ranges(self, variables, mapping) - def lagrangian(self, q: MX | SX, qdot: MX | SX) -> MX | SX: - q_biorbd = GeneralizedCoordinates(q) - qdot_biorbd = GeneralizedVelocity(qdot) - return self.model.Lagrangian(q_biorbd, qdot_biorbd).to_mx() + def lagrangian(self) -> Function: + q_biorbd = GeneralizedCoordinates(self.q) + qdot_biorbd = GeneralizedVelocity(self.qdot) + biorbd_return = self.model.Lagrangian(q_biorbd, qdot_biorbd).to_mx() + casadi_fun = Function( + "lagrangian", + [self.q, self.qdot], + [biorbd_return], + ) + return casadi_fun - def partitioned_forward_dynamics(self, q_u, qdot_u, tau, external_forces=None, f_contacts=None, q_v_init=None): + def partitioned_forward_dynamics(self, external_forces=None, f_contacts=None, q_v_init=None): raise NotImplementedError("partitioned_forward_dynamics is not implemented for BiorbdModel") @staticmethod From f24034ede93e5deb855fec850cf4202ff09c3541 Mon Sep 17 00:00:00 2001 From: Pariterre Date: Mon, 23 Sep 2024 14:14:08 -0400 Subject: [PATCH 003/108] just adding todos --- bioptim/dynamics/configure_new_variable.py | 3 ++ bioptim/dynamics/dynamics_functions.py | 1 + .../getting_started/custom_constraint.py | 1 + .../getting_started/custom_objectives.py | 2 + bioptim/limits/penalty_controller.py | 2 +- bioptim/models/biorbd/biorbd_model.py | 38 ++++++++++++------- bioptim/optimization/optimization_variable.py | 3 +- 7 files changed, 34 insertions(+), 16 deletions(-) diff --git a/bioptim/dynamics/configure_new_variable.py b/bioptim/dynamics/configure_new_variable.py index a0cfc201a..424dbeaaa 100644 --- a/bioptim/dynamics/configure_new_variable.py +++ b/bioptim/dynamics/configure_new_variable.py @@ -120,6 +120,7 @@ def __init__( self.copy_states_dot = False self.copy_controls = False + # todo: Charbie self.mx_states = None self.mx_states_dot = None self.mx_controls = None @@ -331,6 +332,7 @@ def _declare_variable_scaling(self): def _use_copy(self): """Use of states[0] and controls[0] is permitted since nlp.phase_dynamics is PhaseDynamics.SHARED_DURING_THE_PHASE""" + # todo: Charbie self.mx_states = ( [] if not self.copy_states else [self.ocp.nlp[self.nlp.use_states_from_phase_idx].states[0][self.name].mx] ) @@ -369,6 +371,7 @@ def _use_copy(self): self.mx_algebraic_states.append(MX.sym(var_name, 1, 1)) + # todo: Charbie self.mx_states = vertcat(*self.mx_states) self.mx_states_dot = vertcat(*self.mx_states_dot) self.mx_controls = vertcat(*self.mx_controls) diff --git a/bioptim/dynamics/dynamics_functions.py b/bioptim/dynamics/dynamics_functions.py index b58f3b91f..fbcaf64fc 100644 --- a/bioptim/dynamics/dynamics_functions.py +++ b/bioptim/dynamics/dynamics_functions.py @@ -185,6 +185,7 @@ def torque_driven( rigidbody_dynamics is RigidBodyDynamics.ODE and nlp.ode_solver.defects_type == DefectType.IMPLICIT ): if not with_contact and fatigue is None: + # todo: Charbie qddot = DynamicsFunctions.get(nlp.states_dot["qddot"], nlp.states_dot.scaled.mx_reduced) tau_id = DynamicsFunctions.inverse_dynamics(nlp, q, qdot, qddot, with_contact, external_forces) defects = MX(dq.shape[0] + tau_id.shape[0], tau_id.shape[1]) diff --git a/bioptim/examples/getting_started/custom_constraint.py b/bioptim/examples/getting_started/custom_constraint.py index f114a4b9c..3748a11f4 100644 --- a/bioptim/examples/getting_started/custom_constraint.py +++ b/bioptim/examples/getting_started/custom_constraint.py @@ -59,6 +59,7 @@ def custom_func_track_markers(controller: PenaltyController, first_marker: str, from bioptim import BiorbdModel # noinspection PyTypeChecker + # todo: Charbie model: BiorbdModel = controller.model markers = controller.mx_to_cx("markers", model.model.markers, controller.states["q"]) markers_diff = markers[:, marker_1_idx] - markers[:, marker_0_idx] diff --git a/bioptim/examples/getting_started/custom_objectives.py b/bioptim/examples/getting_started/custom_objectives.py index a4f72fe69..8c938c67c 100644 --- a/bioptim/examples/getting_started/custom_objectives.py +++ b/bioptim/examples/getting_started/custom_objectives.py @@ -54,11 +54,13 @@ def custom_func_track_markers(controller: PenaltyController, first_marker: str, marker_1_idx = controller.model.marker_index(second_marker) if method == 0: + # todo: Charbie # Convert the function to the required format and then subtract markers = controller.mx_to_cx("markers", controller.model.markers, controller.states["q"]) markers_diff = markers[:, marker_1_idx] - markers[:, marker_0_idx] else: + # todo: Charbie # Do the calculation in biorbd API and then convert to the required format markers = controller.model.markers(controller.states["q"].mx) markers_diff = markers[marker_1_idx] - markers[marker_0_idx] diff --git a/bioptim/limits/penalty_controller.py b/bioptim/limits/penalty_controller.py index 68432d0a9..4bf07ca42 100644 --- a/bioptim/limits/penalty_controller.py +++ b/bioptim/limits/penalty_controller.py @@ -148,7 +148,7 @@ def phases_dt(self) -> OptimizationVariable: """ tp = OptimizationVariableList(self._nlp.cx, self._nlp.phase_dynamics == PhaseDynamics.SHARED_DURING_THE_PHASE) - n_val = self.ocp.dt_parameter.mx.shape[0] + n_val = self.ocp.dt_parameter.cx.shape[0] tp.append( "phases_dt", mx=self.ocp.dt_parameter.mx, diff --git a/bioptim/models/biorbd/biorbd_model.py b/bioptim/models/biorbd/biorbd_model.py index 1440acceb..9b4af424d 100644 --- a/bioptim/models/biorbd/biorbd_model.py +++ b/bioptim/models/biorbd/biorbd_model.py @@ -7,7 +7,7 @@ GeneralizedAcceleration, ) from casadi import SX, MX, vertcat, horzcat, norm_fro, Function -from typing import Callable, Any +from typing import Callable from ..utils import _var_mapping, bounds_from_ranges from ...limits.path_conditions import Bounds @@ -339,7 +339,7 @@ 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 # TODO: Charbie check this biorbd_return = self.model.torque(tau_activations_biorbd, q_biorbd, qdot_biorbd).to_mx() casadi_fun = Function( "torque_activation", @@ -684,9 +684,9 @@ def rigid_contact_acceleration(self, contact_index, contact_axis) -> Function: q_biorbd = GeneralizedCoordinates(self.q) qdot_biorbd = GeneralizedVelocity(self.qdot) qddot_biorbd = GeneralizedAcceleration(self.qddot) - biorbd_return = self.model.rigidContactAcceleration(q_biorbd, qdot_biorbd, qddot_biorbd, contact_index, True).to_mx()[ - contact_axis - ] + biorbd_return = self.model.rigidContactAcceleration( + q_biorbd, qdot_biorbd, qddot_biorbd, contact_index, True + ).to_mx()[contact_axis] casadi_fun = Function( "rigid_contact_acceleration", [self.q, self.qdot, self.qddot], @@ -749,22 +749,30 @@ def reshape_fext_to_fcontact(self, fext: MX) -> list: count += n_contacts return f_contact_vec - def normalize_state_quaternions(self, x: MX | SX) -> MX | SX: + def normalize_state_quaternions(self) -> Function: + quat_idx = self.get_quaternion_idx() + biorbd_return = MX.zeros(self.nb_q) + biorbd_return[:] = self.q # Normalize quaternion, if needed for j in range(self.nb_quaternions): quaternion = vertcat( - x[quat_idx[j][3]], - x[quat_idx[j][0]], - x[quat_idx[j][1]], - x[quat_idx[j][2]], + self.q[quat_idx[j][3]], + self.q[quat_idx[j][0]], + self.q[quat_idx[j][1]], + self.q[quat_idx[j][2]], ) quaternion /= norm_fro(quaternion) - x[quat_idx[j][0] : quat_idx[j][2] + 1] = quaternion[1:4] - x[quat_idx[j][3]] = quaternion[0] + biorbd_return[quat_idx[j][0] : quat_idx[j][2] + 1] = quaternion[1:4] + biorbd_return[quat_idx[j][3]] = quaternion[0] - return x + casadi_fun = Function( + "soft_contact_forces", + [self.q], + [biorbd_return], + ) + return casadi_fun def get_quaternion_idx(self) -> list[list[int]]: n_dof = 0 @@ -789,7 +797,9 @@ def contact_forces(self, external_forces: MX = None) -> Function: ) biorbd_return = horzcat(biorbd_return, force) else: - biorbd_return = self.contact_forces_from_constrained_forward_dynamics(self.q, self.qdot, self.tau, external_forces=None) + biorbd_return = self.contact_forces_from_constrained_forward_dynamics( + self.q, self.qdot, self.tau, external_forces=None + ) casadi_fun = Function( "contact_forces", [self.q, self.qdot, self.tau], diff --git a/bioptim/optimization/optimization_variable.py b/bioptim/optimization/optimization_variable.py index 9b5f812c2..8a228f124 100644 --- a/bioptim/optimization/optimization_variable.py +++ b/bioptim/optimization/optimization_variable.py @@ -416,7 +416,7 @@ def mx(self): ------- The MX of all variable concatenated together """ - + # todo Charbie remove return vertcat(*[elt.mx for elt in self.elements]) def __contains__(self, item: str): @@ -562,6 +562,7 @@ def shape(self): @property def mx(self): + # todo Charbie remove return self.unscaled.mx # @property From c8b1752c08752c28240d10d2683482f5e6df7216 Mon Sep 17 00:00:00 2001 From: eve-mac Date: Wed, 18 Sep 2024 18:53:08 -0400 Subject: [PATCH 004/108] removed some MX --- bioptim/dynamics/dynamics_functions.py | 378 +++++++++--------- .../getting_started/custom_constraint.py | 26 +- .../getting_started/custom_objectives.py | 19 +- bioptim/limits/constraints.py | 112 +++--- bioptim/limits/penalty.py | 90 ++--- bioptim/models/biorbd/biorbd_model.py | 8 +- 6 files changed, 288 insertions(+), 345 deletions(-) diff --git a/bioptim/dynamics/dynamics_functions.py b/bioptim/dynamics/dynamics_functions.py index fbcaf64fc..8fae3d4c9 100644 --- a/bioptim/dynamics/dynamics_functions.py +++ b/bioptim/dynamics/dynamics_functions.py @@ -44,12 +44,12 @@ class DynamicsFunctions: @staticmethod def custom( - time: MX.sym, - states: MX.sym, - controls: MX.sym, - parameters: MX.sym, - algebraic_states: MX.sym, - numerical_timeseries: MX.sym, + time, + states, + controls, + parameters, + algebraic_states, + numerical_timeseries, nlp, ) -> DynamicsEvaluation: """ @@ -57,26 +57,26 @@ def custom( Parameters ---------- - time: MX.sym + time: MX.sym | SX.sym The time of the system - states: MX.sym + states: MX.sym | SX.sym The state of the system - controls: MX.sym + controls: MX.sym | SX.sym The controls of the system - parameters: MX.sym + parameters: MX.sym | SX.sym The parameters of the system - algebraic_states: MX.sym + algebraic_states: MX.sym | SX.sym The algebraic_states of the system - numerical_timeseries: MX.sym + numerical_timeseries: MX.sym | SX.sym The numerical timeseries of the system nlp: NonLinearProgram The definition of the system Returns ---------- - MX.sym + MX.sym | SX.sym The derivative of the states - MX.sym + MX.sym | SX.sym The defects of the implicit dynamics """ @@ -86,12 +86,12 @@ def custom( @staticmethod def torque_driven( - time: MX.sym, - states: MX.sym, - controls: MX.sym, - parameters: MX.sym, - algebraic_states: MX.sym, - numerical_timeseries: MX.sym, + time, + states, + controls, + parameters, + algebraic_states, + numerical_timeseries, nlp, with_contact: bool, with_passive_torque: bool, @@ -106,17 +106,17 @@ def torque_driven( Parameters ---------- - time: MX.sym + time: MX.sym | SX.sym The time of the system - states: MX.sym + states: MX.sym | SX.sym The state of the system - controls: MX.sym + controls: MX.sym | SX.sym The controls of the system - parameters: MX.sym + parameters: MX.sym | SX.sym The parameters of the system - algebraic_states: MX.sym + algebraic_states: MX.sym | SX.sym The algebraic states of the system - numerical_timeseries: MX.sym + numerical_timeseries: MX.sym | SX.sym The numerical timeseries of the system nlp: NonLinearProgram The definition of the system @@ -158,14 +158,14 @@ def torque_driven( rigidbody_dynamics == RigidBodyDynamics.DAE_INVERSE_DYNAMICS or rigidbody_dynamics == RigidBodyDynamics.DAE_FORWARD_DYNAMICS ): - dxdt = MX(nlp.states.shape, 1) + dxdt = nlp.cx(nlp.states.shape, 1) dxdt[nlp.states["q"].index, :] = dq dxdt[nlp.states["qdot"].index, :] = DynamicsFunctions.get(nlp.controls["qddot"], controls) elif ( rigidbody_dynamics == RigidBodyDynamics.DAE_INVERSE_DYNAMICS_JERK or rigidbody_dynamics == RigidBodyDynamics.DAE_FORWARD_DYNAMICS_JERK ): - dxdt = MX(nlp.states.shape, 1) + dxdt = nlp.cx(nlp.states.shape, 1) dxdt[nlp.states["q"].index, :] = dq qddot = DynamicsFunctions.get(nlp.states["qddot"], states) dxdt[nlp.states["qdot"].index, :] = qddot @@ -188,7 +188,7 @@ def torque_driven( # todo: Charbie qddot = DynamicsFunctions.get(nlp.states_dot["qddot"], nlp.states_dot.scaled.mx_reduced) tau_id = DynamicsFunctions.inverse_dynamics(nlp, q, qdot, qddot, with_contact, external_forces) - defects = MX(dq.shape[0] + tau_id.shape[0], tau_id.shape[1]) + defects = nlp.cx(dq.shape[0] + tau_id.shape[0], tau_id.shape[1]) dq_defects = [] for _ in range(tau_id.shape[1]): @@ -208,12 +208,12 @@ def torque_driven( @staticmethod def torque_driven_free_floating_base( - time: MX.sym, - states: MX.sym, - controls: MX.sym, - parameters: MX.sym, - algebraic_states: MX.sym, - numerical_timeseries: MX.sym, + time, + # states, + controls, + parameters, + algebraic_states, + numerical_timeseries, nlp, with_passive_torque: bool, with_ligament: bool, @@ -224,17 +224,17 @@ def torque_driven_free_floating_base( Parameters ---------- - time: MX.sym + time: MX.sym | SX.sym The time of the system - states: MX.sym + states: MX.sym | SX.sym The state of the system - controls: MX.sym + controls: MX.sym | SX.sym The controls of the system - parameters: MX.sym + parameters: MX.sym | SX.sym The parameters of the system - algebraic_states: MX.sym + algebraic_states: MX.sym | SX.sym The algebraic states of the system - numerical_timeseries: MX.sym + numerical_timeseries: MX.sym | SX.sym The numerical timeseries of the system nlp: NonLinearProgram The definition of the system @@ -269,10 +269,10 @@ def torque_driven_free_floating_base( 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 - tau_full = vertcat(MX.zeros(nlp.model.nb_root), tau_joints) + tau_full = vertcat(nlp.cx.zeros(nlp.model.nb_root), tau_joints) ddq = DynamicsFunctions.forward_dynamics(nlp, q_full, qdot_full, tau_full, with_contact=False) - dxdt = MX(n_q + n_qdot, ddq.shape[1]) + dxdt = nlp.cx(n_q + n_qdot, ddq.shape[1]) dxdt[:n_q, :] = horzcat(*[dq for _ in range(ddq.shape[1])]) dxdt[n_q:, :] = ddq @@ -282,12 +282,12 @@ def torque_driven_free_floating_base( @staticmethod def stochastic_torque_driven( - time: MX.sym, - states: MX.sym, - controls: MX.sym, - parameters: MX.sym, - algebraic_states: MX.sym, - numerical_timeseries: MX.sym, + time, + states, + controls, + parameters, + algebraic_states, + numerical_timeseries, nlp, with_contact: bool, with_friction: bool, @@ -297,17 +297,17 @@ def stochastic_torque_driven( Parameters ---------- - time: MX.sym + time: MX.sym | SX.sym The time - states: MX.sym + states: MX.sym | SX.sym The state of the system - controls: MX.sym + controls: MX.sym | SX.sym The controls of the system - parameters: MX.sym + parameters: MX.sym | SX.sym The parameters of the system - algebraic_states: MX.sym + algebraic_states: MX.sym | SX.sym The algebraic states variables of the system - numerical_timeseries: MX.sym + numerical_timeseries: MX.sym | SX.sym The numerical timeseries of the system nlp: NonLinearProgram The definition of the system @@ -343,7 +343,7 @@ def stochastic_torque_driven( dq = DynamicsFunctions.compute_qdot(nlp, q, qdot) ddq = DynamicsFunctions.forward_dynamics(nlp, q, qdot, tau, with_contact) - dxdt = MX(nlp.states.shape, ddq.shape[1]) + dxdt = nlp.cx(nlp.states.shape, ddq.shape[1]) dxdt[nlp.states["q"].index, :] = horzcat(*[dq for _ in range(ddq.shape[1])]) dxdt[nlp.states["qdot"].index, :] = ddq @@ -351,12 +351,12 @@ def stochastic_torque_driven( @staticmethod def stochastic_torque_driven_free_floating_base( - time: MX.sym, - states: MX.sym, - controls: MX.sym, - parameters: MX.sym, - algebraic_states: MX.sym, - numerical_timeseries: MX.sym, + time, + states, + controls, + parameters, + algebraic_states, + numerical_timeseries, nlp, with_friction: bool, ) -> DynamicsEvaluation: @@ -365,17 +365,17 @@ def stochastic_torque_driven_free_floating_base( Parameters ---------- - time: MX.sym + time: MX.sym | SX.sym The time - states: MX.sym + states: MX.sym | SX.sym The state of the system - controls: MX.sym + controls: MX.sym | SX.sym The controls of the system - parameters: MX.sym + parameters: MX.sym | SX.sym The parameters of the system - algebraic_states: MX.sym + algebraic_states: MX.sym | SX.sym The algebraic states of the system - numerical_timeseries: MX.sym + numerical_timeseries: MX.sym | SX.sym The numerical timeseries of the system nlp: NonLinearProgram The definition of the system @@ -412,18 +412,18 @@ def stochastic_torque_driven_free_floating_base( ) tau_joints = (tau_joints - nlp.model.friction_coefficients @ qdot_joints) if with_friction else tau_joints - tau_full = vertcat(MX.zeros(nlp.model.nb_root), tau_joints) + tau_full = vertcat(nlp.cx.zeros(nlp.model.nb_root), tau_joints) dq = DynamicsFunctions.compute_qdot(nlp, q_full, qdot_full) ddq = DynamicsFunctions.forward_dynamics(nlp, q_full, qdot_full, tau_full, with_contact=False) - dxdt = MX(nlp.states.shape, ddq.shape[1]) + dxdt = nlp.cx(nlp.states.shape, ddq.shape[1]) dxdt[:n_q, :] = horzcat(*[dq for _ in range(ddq.shape[1])]) dxdt[n_q:, :] = ddq return DynamicsEvaluation(dxdt=dxdt, defects=None) @staticmethod - def __get_fatigable_tau(nlp, states: MX, controls: MX, fatigue: FatigueList) -> MX: + def __get_fatigable_tau(nlp, states: MX | SX, controls: MX | SX, fatigue: FatigueList) -> MX | SX: """ Apply the forward dynamics including (or not) the torque fatigue @@ -431,9 +431,9 @@ def __get_fatigable_tau(nlp, states: MX, controls: MX, fatigue: FatigueList) -> ---------- nlp: NonLinearProgram The current phase - states: MX + states: MX | SX The states variable that may contains the tau and the tau fatigue variables - controls: MX + controls: MX | SX The controls variable that may contains the tau fatigue: FatigueList The dynamics for the torque fatigue @@ -442,7 +442,7 @@ def __get_fatigable_tau(nlp, states: MX, controls: MX, fatigue: FatigueList) -> ------- The generalized accelerations """ - tau_var, tau_mx = (nlp.controls, controls) if "tau" in nlp.controls else (nlp.states, states) + tau_var, tau_cx = (nlp.controls, controls) if "tau" in nlp.controls else (nlp.states, states) tau = nlp.get_var_from_states_or_controls("tau", states, controls) if fatigue is not None and "tau" in fatigue: tau_fatigue = fatigue["tau"] @@ -463,11 +463,11 @@ def __get_fatigable_tau(nlp, states: MX, controls: MX, fatigue: FatigueList) -> if not tau_fatigue[0].models.split_controls and "tau" in nlp.controls: pass elif tau_fatigue[0].models.state_only: - tau = sum([DynamicsFunctions.get(tau_var[f"tau_{suffix}"], tau_mx) for suffix in tau_suffix]) + tau = sum([DynamicsFunctions.get(tau_var[f"tau_{suffix}"], tau_cx) for suffix in tau_suffix]) else: - tau = MX() + tau = nlp.cx() for i, t in enumerate(tau_fatigue): - tau_tp = MX(1, 1) + tau_tp = nlp.cx(1, 1) for suffix in tau_suffix: model = t.models.models[suffix] tau_tp += ( @@ -479,12 +479,12 @@ def __get_fatigable_tau(nlp, states: MX, controls: MX, fatigue: FatigueList) -> @staticmethod def torque_activations_driven( - time: MX.sym, - states: MX.sym, - controls: MX.sym, - parameters: MX.sym, - algebraic_states: MX.sym, - numerical_timeseries: MX.sym, + time, + states, + controls, + parameters, + algebraic_states, + numerical_timeseries, nlp, with_contact: bool, with_passive_torque: bool, @@ -497,17 +497,17 @@ def torque_activations_driven( Parameters ---------- - time: MX.sym + time: MX.sym | SX.sym The time of the system - states: MX.sym + states: MX.sym | SX.sym The state of the system - controls: MX.sym + controls: MX.sym | SX.sym The controls of the system - parameters: MX.sym + parameters: MX.sym | SX.sym The parameters of the system - algebraic_states: MX.sym + algebraic_states: MX.sym | SX.sym The algebraic states of the system - numerical_timeseries: MX.sym + numerical_timeseries: MX.sym | SX.sym The numerical timeseries of the system nlp: NonLinearProgram The definition of the system @@ -549,12 +549,12 @@ def torque_activations_driven( @staticmethod def torque_derivative_driven( - time: MX.sym, - states: MX.sym, - controls: MX.sym, - parameters: MX.sym, - algebraic_states: MX.sym, - numerical_timeseries: MX.sym, + time, + states, + controls, + parameters, + algebraic_states, + numerical_timeseries, nlp, rigidbody_dynamics: RigidBodyDynamics, with_contact: bool, @@ -567,17 +567,17 @@ def torque_derivative_driven( Parameters ---------- - time: MX.sym + time: MX.sym | SX.sym The time of the system - states: MX.sym + states: MX.sym | SX.sym The state of the system - controls: MX.sym + controls: MX.sym | SX.sym The controls of the system - parameters: MX.sym + parameters: MX.sym | SX.sym The parameters of the system - algebraic_states: MX.sym + algebraic_states: MX.sym | SX.sym The algebraic states of the system - numerical_timeseries: MX.sym + numerical_timeseries: MX.sym | SX.sym The numerical timeseries of the system nlp: NonLinearProgram The definition of the system @@ -615,14 +615,14 @@ def torque_derivative_driven( ddq = DynamicsFunctions.get(nlp.states["qddot"], states) dddq = DynamicsFunctions.get(nlp.controls["qdddot"], controls) - dxdt = MX(nlp.states.shape, 1) + dxdt = nlp.cx(nlp.states.shape, 1) dxdt[nlp.states["q"].index, :] = dq dxdt[nlp.states["qdot"].index, :] = ddq dxdt[nlp.states["qddot"].index, :] = dddq dxdt[nlp.states["tau"].index, :] = dtau else: ddq = DynamicsFunctions.forward_dynamics(nlp, q, qdot, tau, with_contact, external_forces) - dxdt = MX(nlp.states.shape, ddq.shape[1]) + dxdt = nlp.cx(nlp.states.shape, ddq.shape[1]) dxdt[nlp.states["q"].index, :] = horzcat(*[dq for _ in range(ddq.shape[1])]) dxdt[nlp.states["qdot"].index, :] = ddq dxdt[nlp.states["tau"].index, :] = horzcat(*[dtau for _ in range(ddq.shape[1])]) @@ -631,33 +631,33 @@ def torque_derivative_driven( @staticmethod def forces_from_torque_driven( - time: MX.sym, - states: MX.sym, - controls: MX.sym, - parameters: MX.sym, - algebraic_states: MX.sym, + time, + states, + controls, + parameters, + algebraic_states, numerical_timeseries: MX.sym, nlp, with_passive_torque: bool = False, with_ligament: bool = False, external_forces: np.ndarray = None, - ) -> MX: + ) -> MX | SX: """ Contact forces of a forward dynamics driven by joint torques with contact constraints. Parameters ---------- - time: MX.sym + time: MX.sym | SX.sym The time of the system - states: MX.sym + states: MX.sym | SX.sym The state of the system - controls: MX.sym + controls: MX.sym | SX.sym The controls of the system - parameters: MX.sym + parameters: MX.sym | SX.sym The parameters of the system - algebraic_states: MX.sym + algebraic_states: MX.sym | SX.sym The algebraic states of the system - numerical_timeseries: MX.sym + numerical_timeseries: MX.sym | SX.sym The numerical timeseries of the system nlp: NonLinearProgram The definition of the system @@ -670,7 +670,7 @@ def forces_from_torque_driven( Returns ---------- - MX.sym + MX.sym | SX.sym The contact forces that ensure no acceleration at these contact points """ @@ -684,33 +684,33 @@ def forces_from_torque_driven( @staticmethod def forces_from_torque_activation_driven( - time: MX.sym, - states: MX.sym, - controls: MX.sym, - parameters: MX.sym, - algebraic_states: MX.sym, - numerical_timeseries: MX.sym, + time, + states, + controls, + parameters, + algebraic_states, + numerical_timeseries, nlp, with_passive_torque: bool = False, with_ligament: bool = False, external_forces: np.ndarray = None, - ) -> MX: + ) -> MX | SX: """ Contact forces of a forward dynamics driven by joint torques with contact constraints. Parameters ---------- - time: MX.sym + time: MX.sym | SX.sym The time of the system - states: MX.sym + states: MX.sym | SX.sym The state of the system - controls: MX.sym + controls: MX.sym | SX.sym The controls of the system - parameters: MX.sym + parameters: MX.sym | SX.sym The parameters of the system - algebraic_states: MX.sym + algebraic_states: MX.sym | SX.sym The algebraic states of the system - numerical_timeseries: MX.sym + numerical_timeseries: MX.sym | SX.sym The numerical timeseries of the system nlp: NonLinearProgram The definition of the system @@ -723,7 +723,7 @@ def forces_from_torque_activation_driven( Returns ---------- - MX.sym + MX.sym | SX.sym The contact forces that ensure no acceleration at these contact points """ q = nlp.get_var_from_states_or_controls("q", states, controls) @@ -737,12 +737,12 @@ def forces_from_torque_activation_driven( @staticmethod def muscles_driven( - time: MX.sym, - states: MX.sym, - controls: MX.sym, - parameters: MX.sym, - algebraic_states: MX.sym, - numerical_timeseries: MX.sym, + time, + states, + controls, + parameters, + algebraic_states, + numerical_timeseries, nlp, with_contact: bool, with_passive_torque: bool = False, @@ -757,17 +757,17 @@ def muscles_driven( Parameters ---------- - time: MX.sym + time: MX.sym | SX.sym The time of the system - states: MX.sym + states: MX.sym | SX.sym The state of the system - controls: MX.sym + controls: MX.sym | SX.sym The controls of the system - parameters: MX.sym + parameters: MX.sym | SX.sym The parameters of the system - algebraic_states: MX.sym + algebraic_states: MX.sym | SX.sym The algebraic states of the system - numerical_timeseries: MX.sym + numerical_timeseries: MX.sym | SX.sym The numerical timeseries of the system nlp: NonLinearProgram The definition of the system @@ -840,12 +840,12 @@ def muscles_driven( if rigidbody_dynamics == RigidBodyDynamics.DAE_INVERSE_DYNAMICS: ddq = DynamicsFunctions.get(nlp.controls["qddot"], controls) - dxdt = MX(nlp.states.shape, 1) + dxdt = nlp.cx(nlp.states.shape, 1) dxdt[nlp.states["q"].index, :] = dq dxdt[nlp.states["qdot"].index, :] = DynamicsFunctions.get(nlp.controls["qddot"], controls) else: ddq = DynamicsFunctions.forward_dynamics(nlp, q, qdot, tau, with_contact, external_forces) - dxdt = MX(nlp.states.shape, ddq.shape[1]) + dxdt = nlp.cx(nlp.states.shape, ddq.shape[1]) dxdt[nlp.states["q"].index, :] = horzcat(*[dq for _ in range(ddq.shape[1])]) dxdt[nlp.states["qdot"].index, :] = ddq @@ -866,7 +866,7 @@ def muscles_driven( if not with_contact and fatigue is None: qddot = DynamicsFunctions.get(nlp.states_dot["qddot"], nlp.states_dot.mx_reduced) tau_id = DynamicsFunctions.inverse_dynamics(nlp, q, qdot, qddot, with_contact, external_forces) - defects = MX(dq.shape[0] + tau_id.shape[0], tau_id.shape[1]) + defects = nlp.cx(dq.shape[0] + tau_id.shape[0], tau_id.shape[1]) dq_defects = [] for _ in range(tau_id.shape[1]): @@ -885,33 +885,33 @@ def muscles_driven( @staticmethod def forces_from_muscle_driven( - time: MX.sym, - states: MX.sym, - controls: MX.sym, - parameters: MX.sym, - algebraic_states: MX.sym, - numerical_timeseries: MX.sym, + time, + states, + controls, + parameters, + algebraic_states, + numerical_timeseries, nlp, with_passive_torque: bool = False, with_ligament: bool = False, external_forces: list = None, - ) -> MX: + ) -> MX | SX: """ Contact forces of a forward dynamics driven by muscles activations and joint torques with contact constraints. Parameters ---------- - time: MX.sym + time: MX.sym | SX.sym The time of the system - states: MX.sym + states: MX.sym | SX.sym The state of the system - controls: MX.sym + controls: MX.sym | SX.sym The controls of the system - parameters: MX.sym + parameters: MX.sym | SX.sym The parameters of the system - algebraic_states: MX.sym + algebraic_states: MX.sym | SX.sym The algebraic states of the system - numerical_timeseries: MX.sym + numerical_timeseries: MX.sym | SX.sym The numerical timeseries of the system nlp: NonLinearProgram The definition of the system @@ -923,7 +923,7 @@ def forces_from_muscle_driven( The external forces Returns ---------- - MX.sym + MX.sym | SX.sym The contact forces that ensure no acceleration at these contact points """ @@ -941,12 +941,12 @@ def forces_from_muscle_driven( @staticmethod def joints_acceleration_driven( - time: MX.sym, - states: MX.sym, - controls: MX.sym, - parameters: MX.sym, - algebraic_states: MX.sym, - numerical_timeseries: MX.sym, + time, + states, + controls, + parameters, + algebraic_states, + numerical_timeseries, nlp, rigidbody_dynamics: RigidBodyDynamics = RigidBodyDynamics.ODE, ) -> DynamicsEvaluation: @@ -955,17 +955,17 @@ def joints_acceleration_driven( Parameters ---------- - time: MX.sym + time: MX.sym | SX.sym The time of the system - states: MX.sym + states: MX.sym | SX.sym The state of the system - controls: MX.sym + controls: MX.sym | SX.sym The controls of the system - parameters: MX.sym + parameters: MX.sym | SX.sym The parameters of the system - algebraic_states: MX.sym + algebraic_states: MX.sym | SX.sym The algebraic states of the system - numerical_timeseries: MX.sym + numerical_timeseries: MX.sym | SX.sym The numerical timeseries of the system nlp: NonLinearProgram The definition of the system @@ -974,7 +974,7 @@ def joints_acceleration_driven( Returns ---------- - MX.sym + MX.sym | SX.sym The derivative of states """ if rigidbody_dynamics != RigidBodyDynamics.ODE: @@ -1002,7 +1002,7 @@ def joints_acceleration_driven( floating_base_constraint = nlp.model.inverse_dynamics(q, qdot, qddot_defects_reordered)[: nlp.model.nb_root] - defects = MX(qdot_mapped.shape[0] + qddot_root_mapped.shape[0] + qddot_joints_mapped.shape[0], 1) + defects = nlp.cx(qdot_mapped.shape[0] + qddot_root_mapped.shape[0] + qddot_joints_mapped.shape[0], 1) defects[: qdot_mapped.shape[0], :] = qdot_mapped - nlp.variable_mappings["qdot"].to_first.map( DynamicsFunctions.compute_qdot( @@ -1048,7 +1048,7 @@ def apply_parameters(nlp): Parameters ---------- - parameters: MX.sym + parameters: MX.sym | SX.sym The state of the system nlp: NonLinearProgram The definition of the system @@ -1184,12 +1184,12 @@ def inverse_dynamics( tau = nlp.model.inverse_dynamics(q, qdot, qddot) else: if "tau" in nlp.states: - tau_shape = nlp.states["tau"].mx.shape[0] + tau_shape = nlp.states["tau"].cx.shape[0] elif "tau" in nlp.controls: - tau_shape = nlp.controls["tau"].mx.shape[0] + tau_shape = nlp.controls["tau"].cx.shape[0] else: tau_shape = nlp.model.nb_tau - tau = MX(tau_shape, nlp.ns) + tau = nlp.cx(tau_shape, nlp.ns) for i in range(external_forces.shape[1]): tau[:, i] = nlp.model.inverse_dynamics(q, qdot, qddot, external_forces[:, i]) return tau # We ignore on purpose the mapping to keep zeros in the defects of the dynamic. @@ -1252,12 +1252,12 @@ def compute_tau_from_muscle( @staticmethod def holonomic_torque_driven( - time: MX.sym, - states: MX.sym, - controls: MX.sym, - parameters: MX.sym, - algebraic_states: MX.sym, - numerical_timeseries: MX.sym, + time, + states, + controls, + parameters, + algebraic_states, + numerical_timeseries, nlp, external_forces: list = None, ) -> DynamicsEvaluation: @@ -1266,17 +1266,17 @@ def holonomic_torque_driven( Parameters ---------- - time: MX.sym + time: MX.sym | SX.sym The time of the system - states: MX.sym + states: MX.sym | SX.sym The state of the system - controls: MX.sym + controls: MX.sym | SX.sym The controls of the system - parameters: MX.sym + parameters: MX.sym | SX.sym The parameters acting on the system - algebraic_states: MX.sym + algebraic_states: MX.sym | SX.sym The algebraic states of the system - numerical_timeseries: MX.sym + numerical_timeseries: MX.sym | SX.sym The numerical timeseries of the system nlp: NonLinearProgram A reference to the phase diff --git a/bioptim/examples/getting_started/custom_constraint.py b/bioptim/examples/getting_started/custom_constraint.py index 3748a11f4..cd8dafe39 100644 --- a/bioptim/examples/getting_started/custom_constraint.py +++ b/bioptim/examples/getting_started/custom_constraint.py @@ -28,7 +28,7 @@ ) -def custom_func_track_markers(controller: PenaltyController, first_marker: str, second_marker: str, method) -> MX: +def custom_func_track_markers(controller: PenaltyController, first_marker: str, second_marker: str) -> MX: """ The used-defined objective function (This particular one mimics the ObjectiveFcn.SUPERIMPOSE_MARKERS) Except for the last two @@ -41,8 +41,6 @@ def custom_func_track_markers(controller: PenaltyController, first_marker: str, The index of the first marker in the bioMod second_marker: str The index of the second marker in the bioMod - method: int - Two identical ways are shown to help the new user to navigate the biorbd API Returns ------- @@ -54,21 +52,9 @@ def custom_func_track_markers(controller: PenaltyController, first_marker: str, marker_0_idx = controller.model.marker_index(first_marker) marker_1_idx = controller.model.marker_index(second_marker) - if method == 0: - # Convert the function to the required format and then subtract - from bioptim import BiorbdModel - - # noinspection PyTypeChecker - # todo: Charbie - model: BiorbdModel = controller.model - markers = controller.mx_to_cx("markers", model.model.markers, controller.states["q"]) - markers_diff = markers[:, marker_1_idx] - markers[:, marker_0_idx] - - else: - # Do the calculation in biorbd API and then convert to the required format - markers = controller.model.markers(controller.states["q"].mx) - markers_diff = markers[marker_1_idx] - markers[marker_0_idx] - markers_diff = controller.mx_to_cx("markers", markers_diff, controller.states["q"]) + # compute the position of the markers using the markers function from the BioModel (here a BiorbdModel) + markers = controller.model.markers()(controller.states["q"].cx) + markers_diff = markers[marker_1_idx] - markers[marker_0_idx] return markers_diff @@ -119,8 +105,8 @@ def prepare_ocp( # 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) + constraints.add(custom_func_track_markers, node=Node.START, first_marker="m0", second_marker="m1") + constraints.add(custom_func_track_markers, node=Node.END, first_marker="m0", second_marker="m2") # Path constraint x_bounds = BoundsList() diff --git a/bioptim/examples/getting_started/custom_objectives.py b/bioptim/examples/getting_started/custom_objectives.py index 8c938c67c..86b7a72e5 100644 --- a/bioptim/examples/getting_started/custom_objectives.py +++ b/bioptim/examples/getting_started/custom_objectives.py @@ -27,7 +27,7 @@ ) -def custom_func_track_markers(controller: PenaltyController, first_marker: str, second_marker: str, method: int) -> MX: +def custom_func_track_markers(controller: PenaltyController, first_marker: str, second_marker: str) -> MX: """ The used-defined objective function (This particular one mimics the ObjectiveFcn.SUPERIMPOSE_MARKERS) Except for the last two @@ -40,8 +40,6 @@ def custom_func_track_markers(controller: PenaltyController, first_marker: str, The index of the first marker in the bioMod second_marker: str The index of the second marker in the bioMod - method: int - Two identical ways are shown to help the new user to navigate the biorbd API Returns ------- @@ -53,18 +51,9 @@ def custom_func_track_markers(controller: PenaltyController, first_marker: str, marker_0_idx = controller.model.marker_index(first_marker) marker_1_idx = controller.model.marker_index(second_marker) - if method == 0: - # todo: Charbie - # Convert the function to the required format and then subtract - markers = controller.mx_to_cx("markers", controller.model.markers, controller.states["q"]) - markers_diff = markers[:, marker_1_idx] - markers[:, marker_0_idx] - - else: - # todo: Charbie - # Do the calculation in biorbd API and then convert to the required format - markers = controller.model.markers(controller.states["q"].mx) - markers_diff = markers[marker_1_idx] - markers[marker_0_idx] - markers_diff = controller.mx_to_cx("markers", markers_diff, controller.states["q"]) + # Convert the function to the required format and then subtract + markers = controller.model.markers()(controller.states["q"].cx) + markers_diff = markers[marker_1_idx] - markers[marker_0_idx] return markers_diff diff --git a/bioptim/limits/constraints.py b/bioptim/limits/constraints.py index 590133564..2689fb8af 100644 --- a/bioptim/limits/constraints.py +++ b/bioptim/limits/constraints.py @@ -301,19 +301,9 @@ def torque_max_from_q_and_qdot( if min_torque and min_torque < 0: raise ValueError("min_torque cannot be negative in tau_max_from_actuators") - bound = controller.model.tau_max(controller.q.mx, controller.qdot.mx) - min_bound = controller.mx_to_cx( - "min_bound", - controller.tau.mapping.to_first.map(bound[1]), - controller.q, - controller.qdot, - ) - max_bound = controller.mx_to_cx( - "max_bound", - controller.tau.mapping.to_first.map(bound[0]), - controller.q, - controller.qdot, - ) + bound = controller.model.tau_max()(controller.q.cx, controller.qdot.cx) + min_bound = controller.tau.mapping.to_first.map(bound[1]) + max_bound = controller.tau.mapping.to_first.map(bound[0]) if min_torque: min_bound = if_else(lt(min_bound, min_torque), min_torque, min_bound) max_bound = if_else(lt(max_bound, min_torque), min_torque, max_bound) @@ -416,25 +406,21 @@ def qddot_equals_forward_dynamics( Since the function does nothing, we can safely ignore any argument """ - q = controller.q.mx - qdot = controller.qdot.mx - passive_torque = controller.model.passive_joint_torque(q, qdot) - tau = controller.states["tau"].mx if "tau" in controller.states else controller.tau.mx + q = controller.q.cx + qdot = controller.qdot.cx + passive_torque = controller.model.passive_joint_torque()(q, qdot) + 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 - qddot = controller.controls["qddot"].mx if "qddot" in controller.controls else controller.states["qddot"].mx + qddot = controller.controls["qddot"].cx if "qddot" in controller.controls else controller.states["qddot"].cx + # Charbie: todo controller.model.forward_dynamics(with_contact=True)(q, qdot, tau) if with_contact: - qddot_fd = controller.model.constrained_forward_dynamics(q, qdot, tau) + qddot_fd = controller.model.constrained_forward_dynamics()(q, qdot, tau) else: - qddot_fd = controller.model.forward_dynamics(q, qdot, tau) - - var = [] - var.extend([controller.states[key] for key in controller.states]) - var.extend([controller.controls[key] for key in controller.controls]) - var.extend([param for param in controller.parameters]) + qddot_fd = controller.model.forward_dynamics()(q, qdot, tau) - return controller.mx_to_cx("forward_dynamics", qddot - qddot_fd, *var) + return qddot - qddot_fd @staticmethod def tau_equals_inverse_dynamics( @@ -465,13 +451,13 @@ def tau_equals_inverse_dynamics( Since the function does nothing, we can safely ignore any argument """ - q = controller.q.mx - qdot = controller.qdot.mx - tau = controller.states["tau"].mx if "tau" in controller.states else controller.tau.mx - qddot = controller.states["qddot"].mx if "qddot" in controller.states else controller.controls["qddot"].mx - passive_torque = controller.model.passive_joint_torque(q, qdot) + q = controller.q.cx + 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) 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) if with_ligament else tau if controller.get_nlp.numerical_timeseries: # TODO: deal with external forces @@ -482,21 +468,21 @@ def tau_equals_inverse_dynamics( if with_contact: # todo: this should be done internally in BiorbdModel f_contact = ( - controller.controls["fext"].mx if "fext" in controller.controls else controller.states["fext"].mx + controller.controls["fext"].cx if "fext" in controller.controls else controller.states["fext"].cx ) - f_contact_vec = controller.model.reshape_fext_to_fcontact(f_contact) + f_contact_vec = controller.model.reshape_fext_to_fcontact()(f_contact) tau_id = controller.model.inverse_dynamics(q, qdot, qddot, None, f_contact_vec) else: - tau_id = controller.model.inverse_dynamics(q, qdot, qddot) + tau_id = controller.model.inverse_dynamics()(q, qdot, qddot) var = [] var.extend([controller.states[key] for key in controller.states]) var.extend([controller.controls[key] for key in controller.controls]) var.extend([param for param in controller.parameters]) - return controller.mx_to_cx("inverse_dynamics", tau_id - tau, *var) + return tau_id - tau @staticmethod def implicit_marker_acceleration( @@ -517,21 +503,16 @@ def implicit_marker_acceleration( Since the function does nothing, we can safely ignore any argument """ - q = controller.q.mx - qdot = controller.qdot.mx - qddot = controller.states["qddot"].mx if "qddot" in controller.states else controller.controls["qddot"].mx + q = controller.q.cx + qdot = controller.qdot.cx + qddot = controller.states["qddot"].cx if "qddot" in controller.states else controller.controls["qddot"].cx # TODO get the index of the marker - contact_acceleration = controller.model.rigid_contact_acceleration( - q, qdot, qddot, contact_index, contact_axis + contact_acceleration = controller.model.rigid_contact_acceleration(contact_index, contact_axis)( + q, qdot, qddot ) - var = [] - var.extend([controller.states[key] for key in controller.states.keys()]) - var.extend([controller.controls[key] for key in controller.controls.keys()]) - var.extend([controller.parameters[key] for key in controller.parameters.keys()]) - - return controller.mx_to_cx("contact_acceleration", contact_acceleration, *var) + return contact_acceleration @staticmethod def tau_from_muscle_equal_inverse_dynamics( @@ -555,17 +536,17 @@ def tau_from_muscle_equal_inverse_dynamics( Since the function does nothing, we can safely ignore any argument """ - q = controller.q.mx - qdot = controller.qdot.mx - muscle_activations = controller.controls["muscles"].mx + q = controller.q.cx + 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) 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 = 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 - qddot = controller.states["qddot"].mx if "qddot" in controller.states else controller.controls["qddot"].mx + qddot = controller.states["qddot"].cx if "qddot" in controller.states else controller.controls["qddot"].cx if controller.get_nlp.numerical_timeseries: raise NotImplementedError( @@ -574,14 +555,9 @@ 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) - - var = [] - var.extend([controller.states[key] for key in controller.states]) - var.extend([controller.controls[key] for key in controller.controls]) - var.extend([param for param in controller.parameters]) + tau_id = controller.model.inverse_dynamics()(q, qdot, qddot) - return controller.mx_to_cx("inverse_dynamics", tau_id - muscle_tau, *var) + return tau_id - muscle_tau @staticmethod def implicit_soft_contact_forces(_: Constraint, controller: PenaltyController, **unused_param): @@ -605,7 +581,7 @@ def implicit_soft_contact_forces(_: Constraint, controller: PenaltyController, * force_idx.append(5 + (6 * i_sc)) soft_contact_all = controller.get_nlp.soft_contact_forces_func( - controller.states.mx, controller.controls.mx, controller.parameters.mx + controller.states.cx, controller.controls.cx, controller.parameters.cx ) soft_contact_force = soft_contact_all[force_idx] @@ -614,7 +590,7 @@ def implicit_soft_contact_forces(_: Constraint, controller: PenaltyController, * var.extend([controller.controls[key] for key in controller.controls]) var.extend([param for param in controller.parameters]) - return controller.mx_to_cx("forward_dynamics", controller.controls["fext"].mx - soft_contact_force, *var) + return controller.controls["fext"].cx - soft_contact_force @staticmethod def stochastic_covariance_matrix_continuity_implicit( @@ -691,6 +667,7 @@ def stochastic_df_dx_implicit( controller.algebraic_states["a"].cx, controller.model.matrix_shape_a ) + # Charbie todo remove this function q_root = MX.sym("q_root", nb_root, 1) q_joints = MX.sym("q_joints", nu, 1) qdot_root = MX.sym("qdot_root", nb_root, 1) @@ -843,15 +820,16 @@ def stochastic_mean_sensory_input_equals_reference( ref = controller.algebraic_states["ref"].cx_start sensory_input = controller.model.sensory_reference( - time=controller.time.mx, - states=controller.states.mx, - controls=controller.controls.mx, - parameters=controller.parameters.mx, - algebraic_states=controller.algebraic_states.mx, - numerical_timeseries=controller.numerical_timeseries.mx, + time=controller.time.cx, + states=controller.states.cx, + controls=controller.controls.cx, + parameters=controller.parameters.cx, + algebraic_states=controller.algebraic_states.cx, + numerical_timeseries=controller.numerical_timeseries.cx, nlp=controller.get_nlp, ) + # Charbie todo remove this function sensory_input = Function( "tp", [ diff --git a/bioptim/limits/penalty.py b/bioptim/limits/penalty.py index 2cfd6489b..22de59b48 100644 --- a/bioptim/limits/penalty.py +++ b/bioptim/limits/penalty.py @@ -124,12 +124,11 @@ def minimize_power( if key_control == "tau": return controls * controller.qdot.cx_start elif key_control == "muscles": - q_mx = controller.q.mx + q_mx = controller.q.cx qdot_mx = controller.qdot.mx - muscles_dot = controller.model.muscle_velocity(q_mx, qdot_mx) - objective = controller.mx_to_cx("muscle_velocity", muscles_dot, controller.q, controller.qdot) + muscles_dot = controller.model.muscle_velocity()(controller.q.cx, controller.qdot.mx) - return controls * objective + return controls * muscles_dot @staticmethod def minimize_algebraic_states(penalty: PenaltyOption, controller: PenaltyController, key: str): @@ -199,27 +198,28 @@ def stochastic_minimize_expected_feedback_efforts(penalty: PenaltyOption, contro e_fb_mx = controller.model.compute_torques_from_noise_and_feedback( nlp=controller.get_nlp, - time=controller.time.mx, - states=controller.states_scaled.mx, - controls=controller.controls_scaled.mx, - parameters=controller.parameters_scaled.mx, - algebraic_states=controller.algebraic_states_scaled.mx, - numerical_timeseries=controller.numerical_timeseries.mx, + time=controller.time.cx, + states=controller.states_scaled.cx, + controls=controller.controls_scaled.cx, + parameters=controller.parameters_scaled.cx, + algebraic_states=controller.algebraic_states_scaled.cx, + numerical_timeseries=controller.numerical_timeseries.cx, sensory_noise=controller.model.sensory_noise_magnitude, motor_noise=controller.model.motor_noise_magnitude, ) - jac_e_fb_x = jacobian(e_fb_mx, controller.states_scaled.mx) + jac_e_fb_x = jacobian(e_fb_mx, controller.states_scaled.cx) + # todo: Charbie remode this function jac_e_fb_x_cx = Function( "jac_e_fb_x", [ - controller.t_span.mx, - controller.states_scaled.mx, - controller.controls_scaled.mx, - controller.parameters_scaled.mx, - controller.algebraic_states_scaled.mx, - controller.numerical_timeseries.mx, + controller.t_span.cx, + controller.states_scaled.cx, + controller.controls_scaled.cx, + controller.parameters_scaled.cx, + controller.algebraic_states_scaled.cx, + controller.numerical_timeseries.cx, ], [jac_e_fb_x], )( @@ -294,19 +294,19 @@ 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 = ( - biorbd.RotoTrans() + CX_eye(4) if reference_jcs is None - else model.biorbd_homogeneous_matrices_in_global(q.mx, reference_jcs, inverse=True) + else model.homogeneous_matrices_in_global(reference_jcs, inverse=True)(q.cx) ) markers = [] - for m in model.markers(q.mx): - markers_in_jcs = jcs_t.to_mx() @ vertcat(m, 1) + for m in model.markers()(q.cx): + markers_in_jcs = jcs_t @ vertcat(m, 1) markers = horzcat(markers, markers_in_jcs[:3]) - markers_objective = controller.mx_to_cx("markers", markers, controller.q) - return markers_objective + return markers @staticmethod def minimize_markers_velocity( @@ -343,13 +343,9 @@ def minimize_markers_velocity( penalty.quadratic = True if penalty.quadratic is None else penalty.quadratic # Add the penalty in the requested reference frame. None for global - q_mx = controller.q.mx - qdot_mx = controller.qdot.mx - - markers = horzcat(*controller.model.marker_velocities(q_mx, qdot_mx, reference_index=reference_jcs)) + markers = horzcat(*controller.model.marker_velocities(reference_index=reference_jcs)(controller.q.cx, controller.qdot.cx)) - markers_objective = controller.mx_to_cx("markers_velocity", markers, controller.q, controller.qdot) - return markers_objective + return markers @staticmethod def minimize_markers_acceleration( @@ -383,16 +379,15 @@ def minimize_markers_acceleration( PenaltyFunctionAbstract.set_axes_rows(penalty, axes) penalty.quadratic = True if penalty.quadratic is None else penalty.quadratic - q_mx = controller.q.mx - qdot_mx = controller.qdot.mx - qddot_mx = PenaltyFunctionAbstract._get_qddot(controller, "mx") + qddot = PenaltyFunctionAbstract._get_qddot(controller, "cx") markers = horzcat( - *controller.model.marker_accelerations(q_mx, qdot_mx, qddot_mx, reference_index=reference_jcs) + *controller.model.marker_accelerations(reference_index=reference_jcs)(controller.q.cx, + controller.qdot.cx, + qddot) ) - markers_objective = PenaltyFunctionAbstract._get_markers_acceleration(controller, markers, CoM=False) - return markers_objective + return markers @staticmethod def superimpose_markers( @@ -432,15 +427,11 @@ 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(controller.q.mx, second_marker_idx) - controller.model.marker( - controller.q.mx, first_marker_idx + diff_markers = controller.model.marker(second_marker_idx)(controller.q.cx) - controller.model.marker(first_marker_idx)( + controller.q.cx ) - return controller.mx_to_cx( - f"diff_markers", - diff_markers, - controller.q, - ) + return diff_markers @staticmethod def superimpose_markers_velocity( @@ -691,14 +682,13 @@ def minimize_com_acceleration(penalty: PenaltyOption, controller: PenaltyControl PenaltyFunctionAbstract.set_axes_rows(penalty, axes) penalty.quadratic = True if penalty.quadratic is None else penalty.quadratic - q_mx = controller.q.mx - qdot_mx = controller.qdot.mx - qddot_mx = PenaltyFunctionAbstract._get_qddot(controller, "mx") + qddot = PenaltyFunctionAbstract._get_qddot(controller, "cx") - marker = controller.model.center_of_mass_acceleration(q_mx, qdot_mx, qddot_mx) - com_objective = PenaltyFunctionAbstract._get_markers_acceleration(controller, marker, CoM=True) + marker = controller.model.center_of_mass_acceleration()(controller.q.cx, + controller.qdot.cx, + qddot) - return com_objective + return marker @staticmethod def minimize_angular_momentum(penalty: PenaltyOption, controller: PenaltyController, axes: tuple | list = None): @@ -1451,8 +1441,8 @@ def _get_qddot(controller: PenaltyController, attribute: str): attribute : str Specifies which attribute ('cx_start' or 'mx') to use for the extraction. """ - if attribute not in ["mx", "cx_start"]: - raise ValueError("atrribute should be either mx or cx_start") + # if attribute not in ["mx", "cx_start"]: + # raise ValueError("atrribute should be either mx or cx_start") if "qddot" not in controller.states and "qddot" not in controller.controls: return controller.dynamics( diff --git a/bioptim/models/biorbd/biorbd_model.py b/bioptim/models/biorbd/biorbd_model.py index 9b4af424d..f83fa7eaa 100644 --- a/bioptim/models/biorbd/biorbd_model.py +++ b/bioptim/models/biorbd/biorbd_model.py @@ -129,7 +129,7 @@ def biorbd_homogeneous_matrices_in_global(self, q, segment_idx, inverse=False) - """ Returns a biorbd object containing the roto-translation matrix of the segment in the global reference frame. This is useful if you want to interact with biorbd directly later on. - TODO: Charbie fis this with ApplyRT wrapper + TODO: Charbie fix this with ApplyRT wrapper """ rt_matrix = self.model.globalJCS(GeneralizedCoordinates(q), segment_idx) return rt_matrix.transpose() if inverse else rt_matrix @@ -726,7 +726,7 @@ def soft_contact_forces(self) -> Function: ) return casadi_fun - def reshape_fext_to_fcontact(self, fext: MX) -> list: + def reshape_fext_to_fcontact(self, fext: MX | SX) -> list: if len(self._segments_to_apply_external_forces) == 0: parent_name = [] for i in range(self.nb_rigid_contacts): @@ -737,10 +737,10 @@ def reshape_fext_to_fcontact(self, fext: MX) -> list: self._segments_to_apply_external_forces = parent_name count = 0 - f_contact_vec = MX() + f_contact_vec = fext.type() for i in range(self.nb_rigid_contacts): contact = self.model.rigidContact(i) - tp = MX.zeros(6) + tp = fext.type.zeros(6) used_axes = [i for i, val in enumerate(contact.axes()) if val] n_contacts = len(used_axes) tp[used_axes] = fext[count : count + n_contacts] From ad48e26cb0d065ce599a7192f5ca316008cc5b82 Mon Sep 17 00:00:00 2001 From: eve-mac Date: Wed, 18 Sep 2024 21:40:29 -0400 Subject: [PATCH 005/108] some more --- bioptim/dynamics/dynamics_functions.py | 13 +- bioptim/limits/constraints.py | 7 +- bioptim/limits/penalty.py | 131 +++++------------- bioptim/models/biorbd/biorbd_model.py | 55 ++++---- .../models/biorbd/holonomic_biorbd_model.py | 1 + bioptim/models/biorbd/multi_biorbd_model.py | 121 ++++++++-------- bioptim/models/protocols/biomodel.py | 5 +- tests/shard1/test_biorbd_model_size.py | 8 +- tests/shard1/test_biorbd_multi_model.py | 4 +- 9 files changed, 133 insertions(+), 212 deletions(-) diff --git a/bioptim/dynamics/dynamics_functions.py b/bioptim/dynamics/dynamics_functions.py index 8fae3d4c9..0fd7df896 100644 --- a/bioptim/dynamics/dynamics_functions.py +++ b/bioptim/dynamics/dynamics_functions.py @@ -1135,17 +1135,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: - if with_contact: - qddot = nlp.model.constrained_forward_dynamics(q, qdot, tau) - else: - qddot = nlp.model.forward_dynamics()(q, qdot, tau) - + qddot = nlp.model.forward_dynamics(with_contact=with_contact)(q, qdot, tau) return qdot_var_mapping.map(qddot) else: - if with_contact: - qddot = nlp.model.constrained_forward_dynamics(q, qdot, tau, external_forces) - else: - qddot = nlp.model.forward_dynamics()(q, qdot, tau, external_forces) + qddot = nlp.model.forward_dynamics(with_contact=with_contact)(q, qdot, tau, external_forces) return qdot_var_mapping.map(qddot) @staticmethod @@ -1248,7 +1241,7 @@ def compute_tau_from_muscle( activations.append(muscle_activations[k] * (1 - fatigue_states[k])) else: activations.append(muscle_activations[k]) - return nlp.model.muscle_joint_torque(activations, q, qdot) + return nlp.model.muscle_joint_torque()(q, qdot, activations) @staticmethod def holonomic_torque_driven( diff --git a/bioptim/limits/constraints.py b/bioptim/limits/constraints.py index 2689fb8af..25ad0fafb 100644 --- a/bioptim/limits/constraints.py +++ b/bioptim/limits/constraints.py @@ -414,12 +414,7 @@ def qddot_equals_forward_dynamics( tau = tau + controller.model.ligament_joint_torque(q, qdot) if with_ligament else tau qddot = controller.controls["qddot"].cx if "qddot" in controller.controls else controller.states["qddot"].cx - # Charbie: todo controller.model.forward_dynamics(with_contact=True)(q, qdot, tau) - if with_contact: - qddot_fd = controller.model.constrained_forward_dynamics()(q, qdot, tau) - else: - qddot_fd = controller.model.forward_dynamics()(q, qdot, tau) - + qddot_fd = controller.model.forward_dynamics(with_contact=with_contact)(q, qdot, tau) return qddot - qddot_fd @staticmethod diff --git a/bioptim/limits/penalty.py b/bioptim/limits/penalty.py index 22de59b48..71d16f18d 100644 --- a/bioptim/limits/penalty.py +++ b/bioptim/limits/penalty.py @@ -124,9 +124,7 @@ def minimize_power( if key_control == "tau": return controls * controller.qdot.cx_start elif key_control == "muscles": - q_mx = controller.q.cx - qdot_mx = controller.qdot.mx - muscles_dot = controller.model.muscle_velocity()(controller.q.cx, controller.qdot.mx) + muscles_dot = controller.model.muscle_velocity()(controller.q.cx, controller.qdot.cx) return controls * muscles_dot @@ -471,18 +469,13 @@ 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.mx, controller.qdot.mx) + marker_velocity = controller.model.marker_velocities()(controller.q.cx, controller.qdot.cx) marker_1 = marker_velocity[first_marker_idx][:] marker_2 = marker_velocity[second_marker_idx][:] diff_markers = marker_2 - marker_1 - return controller.mx_to_cx( - f"diff_markers", - diff_markers, - controller.q, - controller.qdot, - ) + return diff_markers @staticmethod def proportional_states( @@ -604,12 +597,11 @@ def minimize_predicted_com_height(_: PenaltyOption, controller: PenaltyControlle The penalty node elements """ - g = controller.model.gravity[2] - com = controller.model.center_of_mass(controller.q.mx) - com_dot = controller.model.center_of_mass_velocity(controller.q.mx, controller.qdot.mx) + g = controller.model.gravity()[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_height = (com_dot[2] * com_dot[2]) / (2 * -g) + com[2] - com_height_cx = controller.mx_to_cx("com_height", com_height, controller.q, controller.qdot) - return com_height_cx + return com_height @staticmethod def minimize_com_position(penalty: PenaltyOption, controller: PenaltyController, axes: tuple | list = None): @@ -632,8 +624,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 - com_cx = controller.mx_to_cx("com", controller.model.center_of_mass, controller.q) - return com_cx + return controller.model.center_of_mass()(controller.q.cx) @staticmethod def minimize_com_velocity(penalty: PenaltyOption, controller: PenaltyController, axes: tuple | list = None): @@ -656,10 +647,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 - com_dot_cx = controller.mx_to_cx( - "com_dot", controller.model.center_of_mass_velocity, controller.q, controller.qdot - ) - return com_dot_cx + return controller.model.center_of_mass_velocity()(controller.q.cx, controller.qdot.cx) @staticmethod def minimize_com_acceleration(penalty: PenaltyOption, controller: PenaltyController, axes: tuple | list = None): @@ -708,13 +696,8 @@ 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 - angular_momentum_cx = controller.mx_to_cx( - "angular_momentum", - controller.model.angular_momentum, - controller.q, - controller.qdot, - ) - return angular_momentum_cx + + return controller.model.angular_momentum()(controller.q.cx, controller.qdot.cx) @staticmethod def minimize_linear_momentum(penalty: PenaltyOption, controller: PenaltyController, axes: tuple | list = None): @@ -736,17 +719,12 @@ 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.mx_to_cx( - "com_velocity", - controller.model.center_of_mass_velocity, - controller.q, - controller.qdot, - ) + com_velocity = controller.model.center_of_mass_velocity()(controller.q.cx, controller.qdot.cx) if isinstance(com_velocity, SX): - mass = Function("mass", [], [controller.model.mass]).expand() + mass = Function("mass", [], [controller.model.mass()]).expand() mass = mass()["o0"] else: - mass = controller.model.mass + mass = controller.model.mass() linear_momentum_cx = com_velocity * mass return linear_momentum_cx @@ -913,13 +891,14 @@ def track_segment_with_custom_rt( raise NotImplementedError( "The track_segment_with_custom_rt penalty can only be called with a BiorbdModel" ) + # Charbie todo find a way to do this in the biorbdmodel model: BiorbdModel = controller.model - r_seg_transposed = model.model.globalJCS(controller.q.mx, segment_index).rot().transpose() - r_rt = model.model.RT(controller.q.mx, rt).rot() - angles_diff = biorbd.Rotation.toEulerAngles(r_seg_transposed * r_rt, "zyx").to_mx() + r_seg_transposed = model.model.globalJCS(segment_index)(controller.q.mx).rot().transpose() + r_rt = model.model.RT(controller.q.mx, rt).rot() # @pariterre is RT the same as GlobalJCS? + # @Pariterre: why was this sequence is fixed? + angles_diff = controller.model.rotation_matrix_to_euler_angles("zyx")(r_seg_transposed * r_rt) - angle_objective = controller.mx_to_cx(f"track_segment", angles_diff, controller.q) - return angle_objective + return angles_diff @staticmethod def track_marker_with_segment_axis( @@ -956,8 +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(controller.q.mx, marker_idx, segment_idx) - marker_objective = controller.mx_to_cx("marker", marker, controller.q) + marker = controller.model.marker(marker_idx, segment_idx)(controller.q.cx) # To align an axis, the other must be equal to 0 if not penalty.rows_is_set: @@ -966,7 +944,7 @@ def track_marker_with_segment_axis( penalty.rows = [ax for ax in [Axis.X, Axis.Y, Axis.Z] if ax != axis] penalty.rows_is_set = True - return marker_objective + return marker @staticmethod def minimize_segment_rotation( @@ -1003,9 +981,9 @@ def minimize_segment_rotation( if not isinstance(controller.model, BiorbdModel): raise NotImplementedError("The minimize_segment_rotation penalty can only be called with a BiorbdModel") - model: BiorbdModel = controller.model - jcs_segment = model.model.globalJCS(controller.q.mx, segment_idx).rot() - angles_segment = biorbd.Rotation.toEulerAngles(jcs_segment, "xyz").to_mx() + + jcs_segment = controller.model.homogeneous_matrices_in_global(segment_idx)(controller.q.cx)[:3, :3] + angles_segment = controller.model.rotation_matrix_to_euler_angles("xyz")(jcs_segment) if axes is None: axes = [Axis.X, Axis.Y, Axis.Z] @@ -1014,9 +992,7 @@ def minimize_segment_rotation( if not isinstance(ax, Axis): raise RuntimeError("axes must be a list of bioptim.Axis") - segment_rotation_objective = controller.mx_to_cx("segment_rotation", angles_segment[axes], controller.q) - - return segment_rotation_objective + return angles_segment[axes] @staticmethod def minimize_segment_velocity( @@ -1051,7 +1027,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(controller.q.mx, controller.qdot.mx, segment_idx) + segment_angular_velocity = model.segment_angular_velocity(segment_idx)(controller.q.cx, controller.qdot.cx) if axes is None: axes = [Axis.X, Axis.Y, Axis.Z] @@ -1060,14 +1036,7 @@ def minimize_segment_velocity( if not isinstance(ax, Axis): raise RuntimeError("axes must be a list of bioptim.Axis") - segment_velocity_objective = controller.mx_to_cx( - "segment_velocity", - segment_angular_velocity[axes], - controller.q, - controller.qdot, - ) - - return segment_velocity_objective + return segment_angular_velocity[axes] @staticmethod def track_vector_orientations_from_markers( @@ -1125,10 +1094,10 @@ def track_vector_orientations_from_markers( else vector_1_marker_1 ) - vector_0_marker_0_position = controller.model.marker(controller.q.mx, vector_0_marker_0_idx) - vector_0_marker_1_position = controller.model.marker(controller.q.mx, vector_0_marker_1_idx) - vector_1_marker_0_position = controller.model.marker(controller.q.mx, vector_1_marker_0_idx) - vector_1_marker_1_position = controller.model.marker(controller.q.mx, vector_1_marker_1_idx) + 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 = vector_0_marker_1_position - vector_0_marker_0_position vector_1 = vector_1_marker_1_position - vector_1_marker_0_position @@ -1136,7 +1105,7 @@ def track_vector_orientations_from_markers( cross_prod_norm = sqrt(cross_prod[0] ** 2 + cross_prod[1] ** 2 + cross_prod[2] ** 2) out = atan2(cross_prod_norm, dot(vector_0, vector_1)) - return controller.mx_to_cx("vector_orientations_difference", out, controller.q) + return out @staticmethod def state_continuity(penalty: PenaltyOption, controller: PenaltyController | list): @@ -1456,37 +1425,3 @@ def _get_qddot(controller: PenaltyController, attribute: str): source = controller.states if "qddot" in controller.states else controller.controls return getattr(source["qddot"], attribute) - - @staticmethod - def _get_markers_acceleration(controller: PenaltyController, markers, CoM=False): - """ - Retrieve the acceleration of either the markers or the center of mass (CoM) from the controller. - - Parameters - ---------- - controller - An object containing 'states' and 'controls' data. - - markers : MX - A generic expression of the marker or CoM acceleration. - - CoM : bool, optional - A boolean indicating the type of acceleration to be returned. If True, returns the CoM - acceleration. If False, returns the markers acceleration. Defaults to False. - - """ - - if "qddot" not in controller.states and "qddot" not in controller.controls: - last_param = controller.controls["tau"] - else: - last_param = controller.states["qddot"] if "qddot" in controller.states else controller.controls["qddot"] - - return controller.mx_to_cx( - "com_ddot" if CoM else "markers_acceleration", - markers, - controller.time, - controller.parameters.unscaled, - controller.q, - controller.qdot, - last_param, - ) diff --git a/bioptim/models/biorbd/biorbd_model.py b/bioptim/models/biorbd/biorbd_model.py index f83fa7eaa..8ce4a2dfe 100644 --- a/bioptim/models/biorbd/biorbd_model.py +++ b/bioptim/models/biorbd/biorbd_model.py @@ -125,6 +125,19 @@ def nb_root(self) -> int: def segments(self) -> tuple[biorbd.Segment]: return self.model.segments() + def rotation_matrix_to_euler_angles(self, sequence) -> Function: + """ + Returns the rotation matrix to euler angles function. + """ + r = MX.sym("r_mx", 3, 3) + biorbd_return = biorbd.Rotation.toEulerAngles(r, sequence).to_mx() + casadi_fun = Function( + "rotation_matrix_to_euler_angles", + [r], + [biorbd_return], + ) + return casadi_fun + def biorbd_homogeneous_matrices_in_global(self, q, segment_idx, inverse=False) -> tuple: """ Returns a biorbd object containing the roto-translation matrix of the segment in the global reference frame. @@ -411,7 +424,7 @@ def _dispatch_forces(self, external_forces: MX, translational_forces: MX): return external_forces_set - def forward_dynamics(self, external_forces=None, translational_forces=None) -> Function: + def forward_dynamics(self, with_contact: bool=False, external_forces=None, translational_forces=None) -> Function: """ TODO: Charbie create a different function for with external_forces and translational_forces """ @@ -420,31 +433,23 @@ def forward_dynamics(self, external_forces=None, translational_forces=None) -> F q_biorbd = GeneralizedCoordinates(self.q) qdot_biorbd = GeneralizedVelocity(self.qdot) tau_biorbd = GeneralizedTorque(self.tau) - 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], - [biorbd_return], - ) - return casadi_fun - def constrained_forward_dynamics(self, external_forces=None, translational_forces=None) -> Function: - """ - TODO: Charbie external_forces=None, translational_forces=None - """ - external_forces_set = self._dispatch_forces(external_forces, translational_forces) - - q_biorbd = GeneralizedCoordinates(self.q) - qdot_biorbd = GeneralizedVelocity(self.qdot) - tau_biorbd = GeneralizedTorque(self.tau) - biorbd_return = self.model.ForwardDynamicsConstraintsDirect( - q_biorbd, qdot_biorbd, tau_biorbd, external_forces_set - ).to_mx() - casadi_fun = Function( - "constrained_forward_dynamics", - [self.q, self.qdot, self.tau], - [biorbd_return], - ) + if with_contact: + biorbd_return = self.model.ForwardDynamicsConstraintsDirect( + q_biorbd, qdot_biorbd, tau_biorbd, external_forces_set + ).to_mx() + casadi_fun = Function( + "constrained_forward_dynamics", + [self.q, self.qdot, self.tau], + [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], + [biorbd_return], + ) return casadi_fun def inverse_dynamics(self, external_forces=None, translational_forces=None) -> Function: diff --git a/bioptim/models/biorbd/holonomic_biorbd_model.py b/bioptim/models/biorbd/holonomic_biorbd_model.py index e931b7a75..f5ef5cd7e 100644 --- a/bioptim/models/biorbd/holonomic_biorbd_model.py +++ b/bioptim/models/biorbd/holonomic_biorbd_model.py @@ -145,6 +145,7 @@ def holonomic_constraints_double_derivative(self, q: MX, qdot: MX, qddot: MX): return vertcat(*[c(q, qdot, qddot) for c in self._holonomic_constraints_double_derivatives]) def constrained_forward_dynamics(self, q, qdot, tau, external_forces=None, f_contacts=None) -> MX: + # @ipuch does this stays contrained_ if external_forces is not None: raise NotImplementedError("External forces are not implemented yet.") if f_contacts is not None: diff --git a/bioptim/models/biorbd/multi_biorbd_model.py b/bioptim/models/biorbd/multi_biorbd_model.py index 716dd2266..75945bbc8 100644 --- a/bioptim/models/biorbd/multi_biorbd_model.py +++ b/bioptim/models/biorbd/multi_biorbd_model.py @@ -1,5 +1,5 @@ import biorbd_casadi as biorbd -from casadi import SX, MX, vertcat +from casadi import SX, MX, vertcat, Function from typing import Callable, Any from .biorbd_model import BiorbdModel @@ -264,13 +264,16 @@ def segments(self) -> tuple[biorbd.Segment, ...]: out += (seg,) return out - def biorbd_homogeneous_matrices_in_global(self, q, segment_idx, inverse=False) -> biorbd.RotoTrans: + def biorbd_homogeneous_matrices_in_global(self, segment_idx, inverse=False) -> biorbd.RotoTrans: + # Charbie todo remove local_segment_id, model_id = self.local_variable_id("segment", segment_idx) - q_model = q[self.variable_index("q", model_id)] + q_model = self.models[model_id].q return self.models[model_id].homogeneous_matrices_in_global(q_model, local_segment_id, inverse) - def homogeneous_matrices_in_global(self, q, segment_idx, inverse=False) -> MX: - return self.biorbd_homogeneous_matrices_in_global(q, segment_idx, inverse).to_mx() + def homogeneous_matrices_in_global(self, segment_idx, inverse=False) -> MX: + local_segment_id, model_id = self.local_variable_id("segment", segment_idx) + q_model = self.models[model_id].q + return self.models[model_id].homogeneous_matrices_in_global(local_segment_id, inverse)(q_model) def homogeneous_matrices_in_child(self, segment_id) -> MX: local_id, model_id = self.local_variable_id("segment", segment_id) @@ -280,62 +283,62 @@ def homogeneous_matrices_in_child(self, segment_id) -> MX: def mass(self) -> MX: return vertcat(*(model.mass for model in self.models)) - def center_of_mass(self, q) -> MX: + def center_of_mass(self) -> MX: out = MX() for i, model in enumerate(self.models): - q_model = q[self.variable_index("q", i)] - out = vertcat(out, model.center_of_mass(q_model)) + q_model = model.q[self.variable_index("q", i)] + out = model.center_of_mass if i == 0 else vertcat(out, model.center_of_mass()(q_model)) return out - def center_of_mass_velocity(self, q, qdot) -> MX: + def center_of_mass_velocity(self) -> MX: out = MX() for i, model in enumerate(self.models): - q_model = q[self.variable_index("q", i)] - qdot_model = qdot[self.variable_index("qdot", i)] - out = vertcat( + q_model = model.q[self.variable_index("q", i)] + qdot_model = model.qdot[self.variable_index("qdot", i)] + out = model.center_of_mass_velocity()(q_model, qdot_model) if i == 0 else vertcat( out, - model.center_of_mass_velocity(q_model, qdot_model), + model.center_of_mass_velocity()(q_model, qdot_model), ) return out - def center_of_mass_acceleration(self, q, qdot, qddot) -> MX: + def center_of_mass_acceleration(self) -> MX: out = MX() for i, model in enumerate(self.models): - q_model = q[self.variable_index("q", i)] - qdot_model = qdot[self.variable_index("qdot", i)] - qddot_model = qddot[self.variable_index("qddot", i)] - out = vertcat( + q_model = model.q[self.variable_index("q", i)] + qdot_model = model.qdot[self.variable_index("qdot", i)] + qddot_model = model.qddot[self.variable_index("qddot", i)] + out = model.center_of_mass_acceleration()(q_model, qdot_model, qddot_model) if i == 0 else vertcat( out, - model.center_of_mass_acceleration(q_model, qdot_model, qddot_model), + model.center_of_mass_acceleration()(q_model, qdot_model, qddot_model), ) return out - def mass_matrix(self, q) -> list[MX]: + def mass_matrix(self) -> list[MX]: out = [] for i, model in enumerate(self.models): - q_model = q[self.variable_index("q", i)] - out += [model.mass_matrix(q_model)] + q_model = model.q[self.variable_index("q", i)] + out += [model.mass_matrix()(q_model)] return out - def non_linear_effects(self, q, qdot) -> list[MX]: + def non_linear_effects(self) -> list[MX]: out = [] for i, model in enumerate(self.models): - q_model = q[self.variable_index("q", i)] - qdot_model = qdot[self.variable_index("qdot", i)] - out += [model.non_linear_effects(q_model, qdot_model)] + q_model = model.q[self.variable_index("q", i)] + qdot_model = model.qdot[self.variable_index("qdot", i)] + out += [model.non_linear_effects()(q_model, qdot_model)] return out - def angular_momentum(self, q, qdot) -> MX: - out = MX() + def angular_momentum(self) -> MX: for i, model in enumerate(self.models): - q_model = q[self.variable_index("q", i)] - qdot_model = qdot[self.variable_index("qdot", i)] - out = vertcat( + q_model = self.q[self.variable_index("q", i)] + qdot_model = self.qdot[self.variable_index("qdot", i)] + out = model.angular_momentum()(q_model, qdot_model) if i == 0 else vertcat( out, - model.angular_momentum(q_model, qdot_model), + model.angular_momentum()(q_model, qdot_model), ) return out + # Charbie ici ... todo def reshape_qdot(self, q, qdot, k_stab=1) -> MX: out = MX() for i, model in enumerate(self.models): @@ -436,7 +439,7 @@ def reorder_qddot_root_joints(self, qddot_root, qddot_joints): return out - def forward_dynamics(self, q, qdot, tau, external_forces=None, f_contacts=None) -> MX: + def forward_dynamics(self, with_contact, q, qdot, tau, external_forces=None, f_contacts=None) -> MX: if f_contacts is not None or external_forces is not None: raise NotImplementedError( "External forces and contact forces are not implemented yet for MultiBiorbdModel." @@ -447,36 +450,28 @@ def forward_dynamics(self, q, qdot, tau, external_forces=None, f_contacts=None) q_model = q[self.variable_index("q", i)] qdot_model = qdot[self.variable_index("qdot", i)] tau_model = tau[self.variable_index("tau", i)] - out = vertcat( - out, - model.forward_dynamics( - q_model, - qdot_model, - tau_model, - external_forces, - f_contacts, - ), - ) - return out - - def constrained_forward_dynamics(self, q, qdot, tau, external_forces=None) -> MX: - if external_forces is not None: - raise NotImplementedError("External forces are not implemented yet for MultiBiorbdModel.") - - out = MX() - for i, model in enumerate(self.models): - q_model = q[self.variable_index("q", i)] - qdot_model = qdot[self.variable_index("qdot", i)] - tau_model = tau[self.variable_index("qddot", i)] # Due to a bug in biorbd - out = vertcat( - out, - model.constrained_forward_dynamics( - q_model, - qdot_model, - tau_model, - external_forces, - ), - ) + if with_contact: + out = vertcat( + out, + model.constrained_forward_dynamics( + q_model, + qdot_model, + tau_model, + external_forces, + f_contacts, + ), + ) + else: + out = vertcat( + out, + model.forward_dynamics( + q_model, + qdot_model, + tau_model, + external_forces, + f_contacts, + ), + ) return out def inverse_dynamics(self, q, qdot, qddot, external_forces=None, f_contacts=None) -> MX: diff --git a/bioptim/models/protocols/biomodel.py b/bioptim/models/protocols/biomodel.py index 8106984f1..e9d8b4db7 100644 --- a/bioptim/models/protocols/biomodel.py +++ b/bioptim/models/protocols/biomodel.py @@ -151,12 +151,9 @@ def forward_dynamics_free_floating_base(self, q, qdot, qddot_joints) -> MX: def reorder_qddot_root_joints(self, qddot_root, qddot_joints) -> MX: """reorder the qddot, from the root dof and the joints dof""" - def forward_dynamics(self, q, qdot, tau, external_forces=None, translational_forces=None) -> MX: + def forward_dynamics(self, q, qdot, tau, with_contact=False, external_forces=None, translational_forces=None) -> MX: """compute the forward dynamics""" - def constrained_forward_dynamics(self, q, qdot, tau, external_forces=None, translational_forces=None) -> MX: - """compute the forward dynamics with constraints""" - def inverse_dynamics(self, q, qdot, qddot, f_ext=None, external_forces=None, translational_forces=None) -> MX: """compute the inverse dynamics""" diff --git a/tests/shard1/test_biorbd_model_size.py b/tests/shard1/test_biorbd_model_size.py index 39e3e167a..6ee312a99 100644 --- a/tests/shard1/test_biorbd_model_size.py +++ b/tests/shard1/test_biorbd_model_size.py @@ -276,16 +276,16 @@ def test_constrained_forward_dynamics_valid_and_too_large_q_or_qdot_or_tau_input tau_valid, tau_too_large = generate_tau_activations_vectors(model) # q, qdot and tau valid - model.constrained_forward_dynamics(q_valid, qdot_valid, tau_valid) + model.forward_dynamics(with_contact=True)(q_valid, qdot_valid, tau_valid) # qdot and tau valid but q not valid with pytest.raises(ValueError, match="Length of q size should be: 4, but got: 5"): - model.constrained_forward_dynamics(q_too_large, qdot_valid, tau_valid) + model.forward_dynamics(with_contact=True)(q_too_large, qdot_valid, tau_valid) # q and tau valid but qdot not valid with pytest.raises(ValueError, match="Length of qdot size should be: 4, but got: 5"): - model.constrained_forward_dynamics(q_valid, qdot_too_large, tau_valid) + model.forward_dynamics(with_contact=True)(q_valid, qdot_too_large, tau_valid) # q and qdot valid but tau not valid with pytest.raises(ValueError, match="Length of tau size should be: 4, but got: 5"): - model.constrained_forward_dynamics(q_valid, qdot_valid, tau_too_large) + model.forward_dynamics(with_contact=True)(q_valid, qdot_valid, tau_too_large) def test_inverse_dynamics_valid_and_too_large_q_or_qdot_or_qddot_input(model): diff --git a/tests/shard1/test_biorbd_multi_model.py b/tests/shard1/test_biorbd_multi_model.py index 64e319cc2..815d69fee 100644 --- a/tests/shard1/test_biorbd_multi_model.py +++ b/tests/shard1/test_biorbd_multi_model.py @@ -212,7 +212,7 @@ def test_biorbd_model(): ) TestUtils.assert_equal( - models.constrained_forward_dynamics( + models.forward_dynamics(with_contact=True)( np.array([1, 2.1, 3, 4.1, 5, 6.1]), np.array([1, 2.1, 3, 4.1, 5, 6]), np.array([3.1, 1, 2, 9.1, 1, 2]), @@ -221,7 +221,7 @@ def test_biorbd_model(): ) with pytest.raises(NotImplementedError, match="External forces are not implemented yet for MultiBiorbdModel."): - models.constrained_forward_dynamics( + models.forward_dynamics(with_contact=True)( np.array([1, 2.1, 3, 4.1, 5, 6.1]), np.array([1, 2.1, 3, 4.1, 5, 6]), np.array([3.1, 1, 2, 9.1, 1, 2]), From c63ca11957267891b4207e9eb98ff4a3f771d09c Mon Sep 17 00:00:00 2001 From: eve-mac Date: Thu, 19 Sep 2024 13:30:31 -0400 Subject: [PATCH 006/108] typos --- bioptim/dynamics/dynamics_functions.py | 8 ++++---- bioptim/models/biorbd/biorbd_model.py | 4 ++-- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/bioptim/dynamics/dynamics_functions.py b/bioptim/dynamics/dynamics_functions.py index 0fd7df896..4ce1d80b7 100644 --- a/bioptim/dynamics/dynamics_functions.py +++ b/bioptim/dynamics/dynamics_functions.py @@ -1235,13 +1235,13 @@ def compute_tau_from_muscle( The generalized forces computed from the muscles """ - activations = [] + activations = type(q)() for k in range(len(nlp.controls["muscles"])): if fatigue_states is not None: - activations.append(muscle_activations[k] * (1 - fatigue_states[k])) + activations = vertcat(activations, muscle_activations[k] * (1 - fatigue_states[k])) else: - activations.append(muscle_activations[k]) - return nlp.model.muscle_joint_torque()(q, qdot, activations) + activations = vertcat(activations, muscle_activations[k]) + return nlp.model.muscle_joint_torque()(activations, q, qdot) @staticmethod def holonomic_torque_driven( diff --git a/bioptim/models/biorbd/biorbd_model.py b/bioptim/models/biorbd/biorbd_model.py index 8ce4a2dfe..10484cbbd 100644 --- a/bioptim/models/biorbd/biorbd_model.py +++ b/bioptim/models/biorbd/biorbd_model.py @@ -536,7 +536,7 @@ def muscle_velocity(self) -> Function: def muscle_joint_torque(self) -> Function: muscles_states = self.model.stateSet() - muscles_activations = self.muscles + muscles_activations = self.muscle for k in range(self.model.nbMuscles()): muscles_states[k].setActivation(muscles_activations[k]) q_biorbd = GeneralizedCoordinates(self.q) @@ -544,7 +544,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.q, self.qdot, self.muscle], + [self.muscle, self.q, self.qdot], [biorbd_return], ) return casadi_fun From 21bc253164c1080f33dc69f37e802675b13e55dc Mon Sep 17 00:00:00 2001 From: eve-mac Date: Thu, 19 Sep 2024 14:51:37 -0400 Subject: [PATCH 007/108] self.external_forces, -translational_forces, protocol --- bioptim/models/biorbd/biorbd_model.py | 74 +++------ bioptim/models/protocols/biomodel.py | 212 +++++++++++++++++--------- 2 files changed, 156 insertions(+), 130 deletions(-) diff --git a/bioptim/models/biorbd/biorbd_model.py b/bioptim/models/biorbd/biorbd_model.py index 10484cbbd..3c7a53e4e 100644 --- a/bioptim/models/biorbd/biorbd_model.py +++ b/bioptim/models/biorbd/biorbd_model.py @@ -42,6 +42,7 @@ def __init__( self.qddot_joints = MX.sym("qddot_joints_mx", self.nb_qddot - self.nb_root, 1) 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)) @property def name(self) -> str: @@ -377,13 +378,9 @@ def forward_dynamics_free_floating_base(self) -> Function: def reorder_qddot_root_joints(qddot_root, qddot_joints) -> MX | SX: return vertcat(qddot_root, qddot_joints) - def _dispatch_forces(self, external_forces: MX, translational_forces: MX): + def _dispatch_forces(self, external_forces: MX): - if external_forces is not None and translational_forces is not None: - raise NotImplementedError( - "You cannot provide both external_forces and translational_forces at the same time." - ) - elif external_forces is not None: + if external_forces is not None: if not isinstance(external_forces, MX): raise ValueError("external_forces should be a numpy array of shape 9 x nb_forces.") if external_forces.shape[0] != 9: @@ -394,17 +391,6 @@ def _dispatch_forces(self, external_forces: MX, translational_forces: MX): raise ValueError( f"external_forces has {external_forces.shape[1]} columns and {len(self._segments_to_apply_external_forces)} segments to apply forces on, they should have the same length." ) - elif translational_forces is not None: - if not isinstance(translational_forces, MX): - raise ValueError("translational_forces should be a numpy array of shape 6 x nb_forces.") - if translational_forces.shape[0] != 6: - raise ValueError( - f"translational_forces has {translational_forces.shape[0]} rows, it should have 6 rows (Fx, Fy, Fz, Px, Py, Pz). You should provide the forces and points of application." - ) - if len(self._segments_to_apply_external_forces) != translational_forces.shape[1]: - raise ValueError( - f"translational_forces has {translational_forces.shape[1]} columns and {len(self._segments_to_apply_external_forces)} segments to apply forces on, they should have the same length." - ) external_forces_set = self.model.externalForceSet() @@ -415,20 +401,10 @@ def _dispatch_forces(self, external_forces: MX, translational_forces: MX): point_of_application = external_forces[6:9, i_element] external_forces_set.add(name, values, point_of_application) - if translational_forces is not None: - for i_elements in range(translational_forces.shape[1]): - name = self._segments_to_apply_external_forces[i_elements] - values = translational_forces[:3, i_elements] - point_of_application = translational_forces[3:6, i_elements] - external_forces_set.addTranslationalForce(values, name, point_of_application) - return external_forces_set - def forward_dynamics(self, with_contact: bool=False, external_forces=None, translational_forces=None) -> Function: - """ - TODO: Charbie create a different function for with external_forces and translational_forces - """ - external_forces_set = self._dispatch_forces(external_forces, translational_forces) + def forward_dynamics(self, with_contact: bool=False) -> Function: + external_forces_set = self._dispatch_forces(self.external_forces) q_biorbd = GeneralizedCoordinates(self.q) qdot_biorbd = GeneralizedVelocity(self.qdot) @@ -440,23 +416,20 @@ def forward_dynamics(self, with_contact: bool=False, external_forces=None, trans ).to_mx() casadi_fun = Function( "constrained_forward_dynamics", - [self.q, self.qdot, self.tau], + [self.q, self.qdot, self.tau, self.external_forces], [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.q, self.qdot, self.tau, self.external_forces], [biorbd_return], ) return casadi_fun - def inverse_dynamics(self, external_forces=None, translational_forces=None) -> Function: - """ - TODO: Charbie external_forces=None, translational_forces=None - """ - external_forces_set = self._dispatch_forces(external_forces, translational_forces) + def inverse_dynamics(self) -> Function: + external_forces_set = self._dispatch_forces(self.external_forces) q_biorbd = GeneralizedCoordinates(self.q) qdot_biorbd = GeneralizedVelocity(self.qdot) @@ -464,18 +437,13 @@ def inverse_dynamics(self, external_forces=None, translational_forces=None) -> F 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.q, self.qdot, self.qddot, self.external_forces], [biorbd_return], ) return casadi_fun - def contact_forces_from_constrained_forward_dynamics( - self, external_forces=None, translational_forces=None - ) -> Function: - """ - TODO: Charbie external_forces=None, translational_forces=None - """ - external_forces_set = self._dispatch_forces(external_forces, translational_forces) + def contact_forces_from_constrained_forward_dynamics(self) -> Function: + external_forces_set = self._dispatch_forces(self.external_forces) q_biorbd = GeneralizedCoordinates(self.q) qdot_biorbd = GeneralizedVelocity(self.qdot) @@ -485,7 +453,7 @@ def contact_forces_from_constrained_forward_dynamics( ).to_mx() casadi_fun = Function( "contact_forces_from_constrained_forward_dynamics", - [self.q, self.qdot, self.tau], + [self.q, self.qdot, self.tau, self.external_forces], [biorbd_return], ) return casadi_fun @@ -525,7 +493,7 @@ def muscle_length_jacobian(self) -> Function: return casadi_fun def muscle_velocity(self) -> Function: - J = self.muscle_length_jacobian(self.q) + J = self.muscle_length_jacobian()(self.q) biorbd_return = J @ self.qdot casadi_fun = Function( "muscle_velocity", @@ -791,19 +759,15 @@ def get_quaternion_idx(self) -> list[list[int]]: return quat_idx def contact_forces(self, external_forces: MX = None) -> Function: - """ - TODO: Charbie external_forces=None - """ if external_forces is not None: - biorbd_return = MX() for i in range(external_forces.shape[1]): - force = self.contact_forces_from_constrained_forward_dynamics( - self.q, self.qdot, self.tau, external_forces=external_forces[:, i] + force = self.contact_forces_from_constrained_forward_dynamics(external_forces=external_forces[:, i])( + self.q, self.qdot, self.tau, ) - biorbd_return = horzcat(biorbd_return, force) + biorbd_return = force if i == 0 else horzcat(biorbd_return, force) else: - biorbd_return = self.contact_forces_from_constrained_forward_dynamics( - self.q, self.qdot, self.tau, external_forces=None + biorbd_return = self.contact_forces_from_constrained_forward_dynamics(external_forces=None)( + self.q, self.qdot, self.tau, ) casadi_fun = Function( "contact_forces", diff --git a/bioptim/models/protocols/biomodel.py b/bioptim/models/protocols/biomodel.py index e9d8b4db7..d697b96dc 100644 --- a/bioptim/models/protocols/biomodel.py +++ b/bioptim/models/protocols/biomodel.py @@ -1,7 +1,7 @@ import numpy as np from typing import Protocol, Callable, Any -from casadi import MX, SX +from casadi import MX, SX, Function from ...misc.mapping import BiMapping, BiMappingList from ...limits.path_conditions import Bounds @@ -25,14 +25,14 @@ def serialize(self) -> tuple[Callable, dict]: """transform the class into a save and load format""" @property - def friction_coefficients(self) -> MX: + def friction_coefficients(self) -> Function: """Get the coefficient of friction to apply to specified elements in the dynamics""" - return MX() + return Function([], []) @property - def gravity(self) -> MX: + def gravity(self) -> Function: """Get the current gravity applied to the model""" - return MX() + return Function("F", [], []) def set_gravity(self, new_gravity): """Set the gravity vector""" @@ -85,7 +85,7 @@ def segments(self) -> tuple: """Get all segments""" return () - def homogeneous_matrices_in_child(self, segment_id) -> MX: + def homogeneous_matrices_in_child(self, segment_id) -> Function: """ Get the homogeneous matrices of one segment in its parent frame, such as: P_R1 = T_R1_R2 * P_R2 @@ -95,26 +95,39 @@ def homogeneous_matrices_in_child(self, segment_id) -> MX: """ @property - def mass(self) -> MX: + def mass(self) -> Function: """Get the mass of the model""" - return MX() + return Function("F", [], []) - def center_of_mass(self, q) -> MX: - """Get the center of mass of the model""" + def center_of_mass(self) -> Function: + """ + Get the center of mass of the model + args: q + """ - def center_of_mass_velocity(self, q, qdot) -> MX: - """Get the center of mass velocity of the model""" + def center_of_mass_velocity(self) -> Function: + """ + Get the center of mass velocity of the model + args: q, qdot + """ - def center_of_mass_acceleration(self, q, qdot, qddot) -> MX: - """Get the center of mass acceleration of the model""" + def center_of_mass_acceleration(self) -> Function: + """ + Get the center of mass acceleration of the model + args: q, qdot, qddot + """ - def angular_momentum(self, q, qdot) -> MX: - """Get the angular momentum of the model""" + def angular_momentum(self) -> Function: + """ + Get the angular momentum of the model + args: q, qdot + """ - def reshape_qdot(self, q, qdot) -> MX: + def reshape_qdot(self) -> Function: """ In case, qdot need to be reshaped, such as if one want to get velocities from quaternions. Since we don't know if this is the case, this function is always called + args: q, qdot """ @property @@ -142,46 +155,83 @@ def muscle_names(self) -> tuple[str, ...]: """Get the muscle names""" return () - def torque(self, activation, q, qdot) -> MX: - """Get the muscle torque""" + def torque(self) -> Function: + """ + Get the muscle torque + args: activation, q, qdot + """ - def forward_dynamics_free_floating_base(self, q, qdot, qddot_joints) -> MX: - """compute the free floating base forward dynamics""" + def forward_dynamics_free_floating_base(self) -> Function: + """ + compute the free floating base forward dynamics + args: q, qdot, qddot_joints + """ - def reorder_qddot_root_joints(self, qddot_root, qddot_joints) -> MX: - """reorder the qddot, from the root dof and the joints dof""" + def reorder_qddot_root_joints(self) -> Function: + """ + reorder the qddot, from the root dof and the joints dof + args: qddot_root, qddot_joints + """ - def forward_dynamics(self, q, qdot, tau, with_contact=False, external_forces=None, translational_forces=None) -> MX: - """compute the forward dynamics""" + def forward_dynamics(self, with_contact=False) -> Function: + """ + compute the forward dynamics + args: q, qdot, tau, external_forces + """ - def inverse_dynamics(self, q, qdot, qddot, f_ext=None, external_forces=None, translational_forces=None) -> MX: - """compute the inverse dynamics""" + def inverse_dynamics(self) -> Function: + """ + compute the inverse dynamics + args: q, qdot, qddot, external_forces + """ - def contact_forces_from_constrained_forward_dynamics( - self, q, qdot, tau, external_forces=None, translational_forces=None - ) -> MX: - """compute the contact forces""" + def contact_forces_from_constrained_forward_dynamics(self) -> Function: + """ + compute the contact forces + args: q, qdot, tau, external_forces + """ - def qdot_from_impact(self, q, qdot_pre_impact) -> MX: - """compute the constraint impulses""" + def qdot_from_impact(self) -> Function: + """ + compute the constraint impulses + args: q, qdot_pre_impact + """ - def muscle_activation_dot(self, muscle_excitations) -> MX: - """Get the activation derivative of the muscles states""" + def muscle_activation_dot(self) -> Function: + """ + Get the activation derivative of the muscles states + args: muscle_excitations + """ - def muscle_joint_torque(self, muscle_states, q, qdot) -> MX: - """Get the muscular joint torque""" + def muscle_joint_torque(self) -> Function: + """ + Get the muscular joint torque + args: muscle_states, q, qdot + """ - def muscle_length_jacobian(self, q) -> MX: - """Get the muscle velocity""" + def muscle_length_jacobian(self) -> Function: + """ + Get the muscle velocity + args: q + """ - def muscle_velocity(self, q, qdot) -> MX: - """Get the muscle velocity""" + def muscle_velocity(self) -> Function: + """ + Get the muscle velocity + args: q, qdot + """ - def marker(self, q, marker_index: int, reference_frame_idx: int = None) -> MX: - """Get the position of a marker""" + def marker(self, marker_index: int, reference_frame_idx: int = None) -> Function: + """ + Get the position of a marker + args: q + """ - def markers(self, q) -> list[MX]: - """Get the markers of the model""" + def markers(self) -> list[MX]: + """ + Get the markers of the model + args: q + """ @property def nb_markers(self) -> int: @@ -196,41 +246,48 @@ def nb_rigid_contacts(self) -> int: """Get the number of rigid contacts""" return -1 - def marker_velocities(self, q, qdot, reference_index=None) -> list[MX]: - """Get the marker velocities of the model""" + def marker_velocities(self, reference_index=None) -> list[MX]: + """ + Get the marker velocities of the model + args: q, qdot + """ - def marker_accelerations(self, q, qdot, qddot, reference_index=None) -> list[MX]: - """Get the marker accelerations of the model""" + def marker_accelerations(self, reference_index=None) -> list[MX]: + """ + Get the marker accelerations of the model + args: q, qdot, qddot + """ - def tau_max(self, q, qdot) -> tuple[MX, MX]: - """Get the maximum torque""" + def tau_max(self) -> tuple[MX, MX]: + """ + Get the maximum torque + args: q, qdot + """ - def rigid_contact_acceleration(self, q, qdot, qddot, contact_index, contact_axis) -> MX: - """Get the rigid contact acceleration""" + def rigid_contact_acceleration(self, contact_index, contact_axis) -> Function: + """ + Get the rigid contact acceleration + args: q, qdot, qddot + """ @property def marker_names(self) -> tuple[str, ...]: """Get the marker names""" return () - def soft_contact_forces(self, q, qdot) -> MX: - """Get the soft contact forces in the global frame""" + def soft_contact_forces(self) -> Function: + """ + Get the soft contact forces in the global frame + args: q, qdot + """ - def normalize_state_quaternions(self, x: MX | SX) -> MX | SX: + def normalize_state_quaternions(self) -> Function: """ Normalize the quaternions of the state - - Parameters - ---------- - x: MX | SX - The state to normalize - - Returns - ------- - The normalized states + args: x (The state to normalize) """ - def contact_forces(self, q, qdot, tau, external_forces: list = None) -> MX: + def contact_forces(self) -> Function: """ Easy accessor for the contact forces in contact dynamics @@ -251,11 +308,17 @@ def contact_forces(self, q, qdot, tau, external_forces: list = None) -> MX: or [nb_rigid_contacts, n_frames] if external_forces is not None """ - def passive_joint_torque(self, q, qdot) -> MX: - """Get the passive joint torque""" + def passive_joint_torque(self) -> Function: + """ + Get the passive joint torque + args: q, qdot + """ - def ligament_joint_torque(self, q, qdot) -> MX: - """Get the ligament joint torque""" + def ligament_joint_torque(self) -> Function: + """ + Get the ligament joint torque + args: q, qdot + """ def bounds_from_ranges(self, variables: str | list[str], mapping: BiMapping | BiMappingList = None) -> Bounds: """ @@ -273,7 +336,7 @@ def bounds_from_ranges(self, variables: str | list[str], mapping: BiMapping | Bi Create the desired bounds """ - def lagrangian(self, q: MX | SX, qdot: MX | SX) -> MX | SX: + def lagrangian(self) -> Function: """ Compute the Lagrangian of a biorbd model. @@ -290,9 +353,10 @@ def lagrangian(self, q: MX | SX, qdot: MX | SX) -> MX | SX: """ def partitioned_forward_dynamics( - self, q_u, qdot_u, tau, external_forces=None, translational_forces=None, q_v_init=None - ) -> MX: + self, q_u, qdot_u, tau, external_forces=None, q_v_init=None + ) -> Function: """ + @ipuch: I need help on how to implement this! This is the forward dynamics of the model, but only for the independent joints Parameters @@ -305,8 +369,6 @@ def partitioned_forward_dynamics( The generalized torques external_forces: MX The external forces - translational_forces: MX - The translational forces Returns ------- From 8b9cc78041a18942ef0cb1c25ca16ac602c6d1a9 Mon Sep 17 00:00:00 2001 From: eve-mac Date: Thu, 19 Sep 2024 16:56:53 -0400 Subject: [PATCH 008/108] fixed some tests --- bioptim/dynamics/configure_problem.py | 79 ++++++++----------- bioptim/dynamics/dynamics_functions.py | 24 +++--- .../custom_package/custom_dynamics.py | 2 +- .../getting_started/custom_dynamics.py | 2 +- .../example_pendulum_time_dependent.py | 2 +- bioptim/limits/constraints.py | 25 +++--- bioptim/limits/penalty.py | 27 +++---- bioptim/models/biorbd/biorbd_model.py | 64 +++++++++++---- bioptim/models/protocols/biomodel.py | 6 ++ .../stochastic_optimal_control_program.py | 2 +- tests/shard4/test_penalty.py | 7 +- tests/shard6/test_time_dependent_problems.py | 2 +- 12 files changed, 126 insertions(+), 116 deletions(-) diff --git a/bioptim/dynamics/configure_problem.py b/bioptim/dynamics/configure_problem.py index de2cddb3b..c117d368f 100644 --- a/bioptim/dynamics/configure_problem.py +++ b/bioptim/dynamics/configure_problem.py @@ -201,9 +201,9 @@ def torque_driven( for key in numerical_data_timeseries.keys(): if key == "external_forces": _check_numerical_timeseries_format(numerical_data_timeseries[key], nlp.ns, nlp.phase_idx) - external_forces = nlp.numerical_timeseries[0].mx + external_forces = nlp.numerical_timeseries[0].cx for i in range(1, numerical_data_timeseries[key].shape[1]): - external_forces = horzcat(external_forces, nlp.numerical_timeseries[i].mx) + external_forces = horzcat(external_forces, nlp.numerical_timeseries[i].cx) # Declared rigidbody states and controls ConfigureProblem.configure_q(ocp, nlp, as_states=True, as_controls=False) @@ -293,21 +293,21 @@ def torque_driven( external_forces=external_forces, ) - # # Configure the contact forces - # if with_contact: - # ConfigureProblem.configure_contact_function( - # ocp, nlp, DynamicsFunctions.forces_from_torque_driven, external_forces=external_forces - # ) - # # Configure the soft contact forces - # ConfigureProblem.configure_soft_contact_function(ocp, nlp) - # # Algebraic constraints of soft contact forces if needed - # if soft_contacts_dynamics == SoftContactDynamics.CONSTRAINT: - # ocp.implicit_constraints.add( - # ImplicitConstraintFcn.SOFT_CONTACTS_EQUALS_SOFT_CONTACTS_DYNAMICS, - # node=Node.ALL_SHOOTING, - # penalty_type=ConstraintType.IMPLICIT, - # phase=nlp.phase_idx, - # ) + # Configure the contact forces + if with_contact: + ConfigureProblem.configure_contact_function( + ocp, nlp, DynamicsFunctions.forces_from_torque_driven, external_forces=external_forces + ) + # Configure the soft contact forces + ConfigureProblem.configure_soft_contact_function(ocp, nlp) + # Algebraic constraints of soft contact forces if needed + if soft_contacts_dynamics == SoftContactDynamics.CONSTRAINT: + ocp.implicit_constraints.add( + ImplicitConstraintFcn.SOFT_CONTACTS_EQUALS_SOFT_CONTACTS_DYNAMICS, + node=Node.ALL_SHOOTING, + penalty_type=ConstraintType.IMPLICIT, + phase=nlp.phase_idx, + ) @staticmethod def torque_driven_free_floating_base( @@ -1296,25 +1296,25 @@ def configure_contact_function(ocp, nlp, dyn_func: Callable, **extra_params): The function to get the values of contact forces from the dynamics """ - time_span_sym = vertcat(nlp.time_mx, nlp.dt_mx) + time_span_sym = vertcat(nlp.time_cx, nlp.dt) nlp.contact_forces_func = Function( "contact_forces_func", [ time_span_sym, - nlp.states.scaled.mx_reduced, - nlp.controls.scaled.mx_reduced, - nlp.parameters.scaled.mx_reduced, - nlp.algebraic_states.scaled.mx_reduced, - nlp.numerical_timeseries.mx, + nlp.states.scaled.cx, + nlp.controls.scaled.cx, + nlp.parameters.scaled.cx, + nlp.algebraic_states.scaled.cx, + nlp.numerical_timeseries.cx, ], [ dyn_func( time_span_sym, - nlp.states.scaled.mx_reduced, - nlp.controls.scaled.mx_reduced, - nlp.parameters.scaled.mx_reduced, - nlp.algebraic_states.scaled.mx_reduced, - nlp.numerical_timeseries.mx, + nlp.states.scaled.cx, + nlp.controls.scaled.cx, + nlp.parameters.scaled.cx, + nlp.algebraic_states.scaled.cx, + nlp.numerical_timeseries.cx, nlp, **extra_params, ) @@ -1363,25 +1363,10 @@ def configure_soft_contact_function(ocp, nlp): """ component_list = ["Mx", "My", "Mz", "Fx", "Fy", "Fz"] - q = nlp.states.mx_reduced[nlp.states["q"].index] - qdot = nlp.states.mx_reduced[nlp.states["qdot"].index] - global_soft_contact_force_func = nlp.model.soft_contact_forces( - nlp.states["q"].mapping.to_second.map(q), nlp.states["qdot"].mapping.to_second.map(qdot) - ) - - # TODO: do not declare unuseful functions! - nlp.soft_contact_forces_func = Function( - "soft_contact_forces_func", - [ - nlp.time_mx, - nlp.states.scaled.mx_reduced, - nlp.controls.scaled.mx_reduced, - nlp.parameters.scaled.mx_reduced, - ], - [global_soft_contact_force_func], - ["t", "x", "u", "p"], - ["soft_contact_forces"], - ).expand() + 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) + nlp.soft_contact_forces_func = global_soft_contact_force_func for i_sc in range(nlp.model.nb_soft_contacts): all_soft_contact_names = [] diff --git a/bioptim/dynamics/dynamics_functions.py b/bioptim/dynamics/dynamics_functions.py index 4ce1d80b7..d6a4452e0 100644 --- a/bioptim/dynamics/dynamics_functions.py +++ b/bioptim/dynamics/dynamics_functions.py @@ -146,13 +146,10 @@ def torque_driven( dq = DynamicsFunctions.compute_qdot(nlp, q, qdot) - # Charbie: to remove - tau = DynamicsFunctions.get(nlp.controls["tau"], controls) - # - # 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.friction_coefficients @ qdot if with_friction else tau + 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.friction_coefficients @ qdot if with_friction else tau if ( rigidbody_dynamics == RigidBodyDynamics.DAE_INVERSE_DYNAMICS @@ -185,8 +182,7 @@ def torque_driven( rigidbody_dynamics is RigidBodyDynamics.ODE and nlp.ode_solver.defects_type == DefectType.IMPLICIT ): if not with_contact and fatigue is None: - # todo: Charbie - qddot = DynamicsFunctions.get(nlp.states_dot["qddot"], nlp.states_dot.scaled.mx_reduced) + qddot = DynamicsFunctions.get(nlp.states_dot["qddot"], nlp.states_dot.scaled.cx) tau_id = DynamicsFunctions.inverse_dynamics(nlp, q, qdot, qddot, with_contact, external_forces) defects = nlp.cx(dq.shape[0] + tau_id.shape[0], tau_id.shape[1]) @@ -197,7 +193,7 @@ def torque_driven( - DynamicsFunctions.compute_qdot( nlp, q, - DynamicsFunctions.get(nlp.states_dot.scaled["qdot"], nlp.states_dot.scaled.mx_reduced), + DynamicsFunctions.get(nlp.states_dot.scaled["qdot"], nlp.states_dot.scaled.cx), ) ) defects[: dq.shape[0], :] = horzcat(*dq_defects) @@ -264,9 +260,9 @@ 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) 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.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 tau_full = vertcat(nlp.cx.zeros(nlp.model.nb_root), tau_joints) @@ -680,7 +676,7 @@ def forces_from_torque_driven( 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 - return nlp.model.contact_forces(q, qdot, tau, external_forces) + return nlp.model.contact_forces(external_forces=external_forces)(q, qdot, tau) @staticmethod def forces_from_torque_activation_driven( @@ -1135,7 +1131,7 @@ 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, []) return qdot_var_mapping.map(qddot) else: qddot = nlp.model.forward_dynamics(with_contact=with_contact)(q, qdot, tau, external_forces) diff --git a/bioptim/examples/custom_model/custom_package/custom_dynamics.py b/bioptim/examples/custom_model/custom_package/custom_dynamics.py index d59fc317f..207b85f34 100644 --- a/bioptim/examples/custom_model/custom_package/custom_dynamics.py +++ b/bioptim/examples/custom_model/custom_package/custom_dynamics.py @@ -40,7 +40,7 @@ def custom_dynamics( """ return DynamicsEvaluation( - dxdt=vertcat(states[1], nlp.model.forward_dynamics(states[0], states[1], controls[0])), defects=None + dxdt=vertcat(states[1], nlp.model.forward_dynamics(states[0], states[1], controls[0], None)), defects=None ) diff --git a/bioptim/examples/getting_started/custom_dynamics.py b/bioptim/examples/getting_started/custom_dynamics.py index c570cf8ca..69faeb168 100644 --- a/bioptim/examples/getting_started/custom_dynamics.py +++ b/bioptim/examples/getting_started/custom_dynamics.py @@ -74,7 +74,7 @@ def custom_dynamics( # You can directly call biorbd function (as for ddq) or call bioptim accessor (as for dq) dq = DynamicsFunctions.compute_qdot(nlp, q, qdot) * my_additional_factor - ddq = nlp.model.forward_dynamics(q, qdot, tau) + ddq = nlp.model.forward_dynamics(q, qdot, tau, []) # the user has to choose if want to return the explicit dynamics dx/dt = f(x,u,p) # as the first argument of DynamicsEvaluation or diff --git a/bioptim/examples/torque_driven_ocp/example_pendulum_time_dependent.py b/bioptim/examples/torque_driven_ocp/example_pendulum_time_dependent.py index 1a9d9b93f..20d0fcca4 100644 --- a/bioptim/examples/torque_driven_ocp/example_pendulum_time_dependent.py +++ b/bioptim/examples/torque_driven_ocp/example_pendulum_time_dependent.py @@ -74,7 +74,7 @@ def time_dependent_dynamic( # You can directly call biorbd function (as for ddq) or call bioptim accessor (as for dq) dq = DynamicsFunctions.compute_qdot(nlp, q, qdot) - ddq = nlp.model.forward_dynamics(q, qdot, tau) + ddq = nlp.model.forward_dynamics(q, qdot, tau, []) return DynamicsEvaluation(dxdt=vertcat(dq, ddq), defects=None) diff --git a/bioptim/limits/constraints.py b/bioptim/limits/constraints.py index 25ad0fafb..90d53233f 100644 --- a/bioptim/limits/constraints.py +++ b/bioptim/limits/constraints.py @@ -454,23 +454,16 @@ def tau_equals_inverse_dynamics( tau = tau + passive_torque if with_passive_torque else tau tau = tau + controller.model.ligament_joint_torque()(q, qdot) if with_ligament else tau - if controller.get_nlp.numerical_timeseries: - # TODO: deal with external forces - raise NotImplementedError( - "This implicit constraint tau_equals_inverse_dynamics is not implemented yet with external forces" - ) - # Todo: add fext tau_id = nlp.model.inverse_dynamics(q, qdot, qddot, fext).to_mx() - if with_contact: - # todo: this should be done internally in BiorbdModel - f_contact = ( - controller.controls["fext"].cx if "fext" in controller.controls else controller.states["fext"].cx - ) - f_contact_vec = controller.model.reshape_fext_to_fcontact()(f_contact) - - tau_id = controller.model.inverse_dynamics(q, qdot, qddot, None, f_contact_vec) - + if "fext" in controller.controls: + f_ext = controller.controls["fext"].cx + elif "fext" in controller.states: + f_ext = controller.states["fext"].cx + elif "external_forces" in controller.get_nlp.numerical_timeseries: + f_ext = controller.numerical_timeseries["external_forces"].cx else: - tau_id = controller.model.inverse_dynamics()(q, qdot, qddot) + raise ValueError("External forces must be provided") + + tau_id = controller.model.inverse_dynamics(with_contact=with_contact)(q, qdot, qddot, f_ext) var = [] var.extend([controller.states[key] for key in controller.states]) diff --git a/bioptim/limits/penalty.py b/bioptim/limits/penalty.py index 71d16f18d..50989be13 100644 --- a/bioptim/limits/penalty.py +++ b/bioptim/limits/penalty.py @@ -597,9 +597,9 @@ def minimize_predicted_com_height(_: PenaltyOption, controller: PenaltyControlle The penalty node elements """ - g = controller.model.gravity()[2] - com = controller.model.center_of_mass(controller.q.cx) - com_dot = controller.model.center_of_mass_velocity(controller.q.cx, controller.qdot.cx) + 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_height = (com_dot[2] * com_dot[2]) / (2 * -g) + com[2] return com_height @@ -720,11 +720,7 @@ def minimize_linear_momentum(penalty: PenaltyOption, controller: PenaltyControll 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) - if isinstance(com_velocity, SX): - mass = Function("mass", [], [controller.model.mass()]).expand() - mass = mass()["o0"] - else: - mass = controller.model.mass() + mass = controller.model.mass()["o0"] linear_momentum_cx = com_velocity * mass return linear_momentum_cx @@ -864,7 +860,7 @@ def minimize_soft_contact_forces( @staticmethod def track_segment_with_custom_rt( - penalty: PenaltyOption, controller: PenaltyController, segment: int | str, rt: int + penalty: PenaltyOption, controller: PenaltyController, segment: int | str, rt_idx: int, sequence: str = "zyx" ): """ Minimize the difference of the euler angles extracted from the coordinate system of a segment @@ -878,8 +874,10 @@ def track_segment_with_custom_rt( The penalty node elements segment: int | str The name or index of the segment - rt: int + rt_idx: int The index of the RT in the bioMod + sequence: str + The sequence of the euler angles (default="zyx") """ from ..models.biorbd.biorbd_model import BiorbdModel @@ -891,12 +889,11 @@ def track_segment_with_custom_rt( raise NotImplementedError( "The track_segment_with_custom_rt penalty can only be called with a BiorbdModel" ) - # Charbie todo find a way to do this in the biorbdmodel - model: BiorbdModel = controller.model - r_seg_transposed = model.model.globalJCS(segment_index)(controller.q.mx).rot().transpose() - r_rt = model.model.RT(controller.q.mx, rt).rot() # @pariterre is RT the same as GlobalJCS? + 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] # @Pariterre: why was this sequence is fixed? - angles_diff = controller.model.rotation_matrix_to_euler_angles("zyx")(r_seg_transposed * r_rt) + # @Pariterre: this is suspicious and it breaks the tests! + angles_diff = controller.model.rotation_matrix_to_euler_angles(sequence)(r_seg_transposed * r_rt) return angles_diff diff --git a/bioptim/models/biorbd/biorbd_model.py b/bioptim/models/biorbd/biorbd_model.py index 3c7a53e4e..7d0e0b05e 100644 --- a/bioptim/models/biorbd/biorbd_model.py +++ b/bioptim/models/biorbd/biorbd_model.py @@ -131,7 +131,11 @@ def rotation_matrix_to_euler_angles(self, sequence) -> Function: Returns the rotation matrix to euler angles function. """ r = MX.sym("r_mx", 3, 3) - biorbd_return = biorbd.Rotation.toEulerAngles(r, sequence).to_mx() + # @Pariterre: is this the right order? + r_matrix = biorbd.Rotation(r[0, 0], r[0, 1], r[0, 2], + r[0, 0], r[0, 1], r[0, 2], + r[0, 0], r[0, 1], r[0, 2]) + biorbd_return = biorbd.Rotation.toEulerAngles(r_matrix, sequence).to_mx() casadi_fun = Function( "rotation_matrix_to_euler_angles", [r], @@ -189,6 +193,16 @@ def mass(self) -> Function: ) return casadi_fun + def rt(self, rt_index) -> Function: + q_biorbd = GeneralizedCoordinates(self.q) + biorbd_return = self.model.RT(q_biorbd, rt_index).to_mx() + casadi_fun = Function( + "rt", + [self.q], + [biorbd_return], + ) + return casadi_fun + def center_of_mass(self) -> Function: q_biorbd = GeneralizedCoordinates(self.q) biorbd_return = self.model.CoM(q_biorbd, True).to_mx() @@ -428,8 +442,14 @@ def forward_dynamics(self, with_contact: bool=False) -> Function: ) return casadi_fun - def inverse_dynamics(self) -> Function: - external_forces_set = self._dispatch_forces(self.external_forces) + def inverse_dynamics(self, with_contact: bool=False) -> Function: + # @ipuch: I do not understand what is happening here? Do we have f_ext or it is just the contact forces? + if with_contact: + f_ext = self.reshape_fext_to_fcontact(self.external_forces) + else: + f_ext = self.external_forces + + external_forces_set = self._dispatch_forces(f_ext) q_biorbd = GeneralizedCoordinates(self.q) qdot_biorbd = GeneralizedVelocity(self.qdot) @@ -709,18 +729,25 @@ def reshape_fext_to_fcontact(self, fext: MX | SX) -> list: ] self._segments_to_apply_external_forces = parent_name + fext_sym = MX.sym("Fext", fext.shape[0], fext.shape[1]) count = 0 - f_contact_vec = fext.type() + f_contact_vec = MX() for i in range(self.nb_rigid_contacts): contact = self.model.rigidContact(i) - tp = fext.type.zeros(6) + tp = MX.zeros(6) used_axes = [i for i, val in enumerate(contact.axes()) if val] n_contacts = len(used_axes) - tp[used_axes] = fext[count : count + n_contacts] + tp[used_axes] = fext_sym[count : count + n_contacts] tp[3:] = contact.to_mx() f_contact_vec = horzcat(f_contact_vec, tp) count += n_contacts - return f_contact_vec + + casadi_fun_evaluated = Function( + "reshape_fext_to_fcontact", + [fext_sym], + [f_contact_vec], + )(fext) + return casadi_fun_evaluated def normalize_state_quaternions(self) -> Function: @@ -761,19 +788,24 @@ def get_quaternion_idx(self) -> list[list[int]]: def contact_forces(self, external_forces: MX = None) -> Function: if external_forces is not None: for i in range(external_forces.shape[1]): - force = self.contact_forces_from_constrained_forward_dynamics(external_forces=external_forces[:, i])( - self.q, self.qdot, self.tau, + force = self.contact_forces_from_constrained_forward_dynamics()( + self.q, self.qdot, self.tau, external_forces[:, i] ) biorbd_return = force if i == 0 else horzcat(biorbd_return, force) + casadi_fun = Function( + "contact_forces", + [self.q, self.qdot, self.tau, self.external_forces], + [biorbd_return], + ) else: - biorbd_return = self.contact_forces_from_constrained_forward_dynamics(external_forces=None)( - self.q, self.qdot, self.tau, + biorbd_return = self.contact_forces_from_constrained_forward_dynamics()( + self.q, self.qdot, self.tau, MX() + ) + casadi_fun = Function( + "contact_forces", + [self.q, self.qdot, self.tau], + [biorbd_return], ) - casadi_fun = Function( - "contact_forces", - [self.q, self.qdot, self.tau], - [biorbd_return], - ) return casadi_fun def passive_joint_torque(self) -> Function: diff --git a/bioptim/models/protocols/biomodel.py b/bioptim/models/protocols/biomodel.py index d697b96dc..d67bbc268 100644 --- a/bioptim/models/protocols/biomodel.py +++ b/bioptim/models/protocols/biomodel.py @@ -99,6 +99,12 @@ def mass(self) -> Function: """Get the mass of the model""" return Function("F", [], []) + def rt(self, rt_idx) -> Function: + """ + Get the rototrans matrix of an object (e.g., an IMU) that is placed on the model + args: q + """ + def center_of_mass(self) -> Function: """ Get the center of mass of the model diff --git a/bioptim/optimization/stochastic_optimal_control_program.py b/bioptim/optimization/stochastic_optimal_control_program.py index f2f8b2f76..7940e34ff 100644 --- a/bioptim/optimization/stochastic_optimal_control_program.py +++ b/bioptim/optimization/stochastic_optimal_control_program.py @@ -343,7 +343,7 @@ def replace_initial_guess(key, n_var, var_init, a_init, i_phase): a_init.add(key, initial_guess=var_init, interpolation=InterpolationType.EACH_FRAME, phase=i_phase) def get_ref_init(time_vector, x_guess, u_guess, p_guess, nlp): - if nlp.numerical_timeseries.mx.shape[0] != 0: + if nlp.numerical_timeseries.cx.shape[0] != 0: raise RuntimeError( "The automatic initialization of stochastic variables is not implemented yet for nlp with numerical_timeseries." ) diff --git a/tests/shard4/test_penalty.py b/tests/shard4/test_penalty.py index d6bc0d473..e35f84051 100644 --- a/tests/shard4/test_penalty.py +++ b/tests/shard4/test_penalty.py @@ -51,7 +51,8 @@ def prepare_test_ocp( ) elif with_contact: bio_model = BiorbdModel( - bioptim_folder + "/examples/muscle_driven_with_contact/models/2segments_4dof_2contacts_1muscle.bioMod" + bioptim_folder + "/examples/muscle_driven_with_contact/models/2segments_4dof_2contacts_1muscle.bioMod", + segments_to_apply_external_forces=["", ""], # TODO: Charbie add the proper segments ) dynamics = DynamicsList() rigidbody_dynamics = RigidBodyDynamics.DAE_INVERSE_DYNAMICS if implicit else RigidBodyDynamics.ODE @@ -974,9 +975,9 @@ def test_penalty_track_segment_with_custom_rt(penalty_origin, value, phase_dynam penalty_type = penalty_origin.TRACK_SEGMENT_WITH_CUSTOM_RT if isinstance(penalty_type, (ObjectiveFcn.Lagrange, ObjectiveFcn.Mayer)): - penalty = Objective(penalty_type, segment="ground", rt=0) + penalty = Objective(penalty_type, segment="ground", rt_idx=0) else: - penalty = Constraint(penalty_type, segment="ground", rt=0) + penalty = Constraint(penalty_type, segment="ground", rt_idx=0) res = get_penalty_value(ocp, penalty, t, phases_dt, x, u, p, a, d) expected = np.array([[0], [0.1], [0]]) diff --git a/tests/shard6/test_time_dependent_problems.py b/tests/shard6/test_time_dependent_problems.py index b7582ffd1..3928f1e30 100644 --- a/tests/shard6/test_time_dependent_problems.py +++ b/tests/shard6/test_time_dependent_problems.py @@ -72,7 +72,7 @@ def time_dynamic( # You can directly call biorbd function (as for ddq) or call bioptim accessor (as for dq) dq = DynamicsFunctions.compute_qdot(nlp, q, qdot) - ddq = nlp.model.forward_dynamics(q, qdot, tau) + ddq = nlp.model.forward_dynamics(q, qdot, tau, []) return DynamicsEvaluation(dxdt=vertcat(dq, ddq), defects=None) From 14020d4ee0650258cc588751d4cecc425d2238a2 Mon Sep 17 00:00:00 2001 From: eve-mac Date: Thu, 19 Sep 2024 17:10:44 -0400 Subject: [PATCH 009/108] JCS --- bioptim/limits/penalty.py | 5 ++++- bioptim/models/biorbd/biorbd_model.py | 12 ++---------- tests/shard4/test_penalty.py | 2 +- 3 files changed, 7 insertions(+), 12 deletions(-) diff --git a/bioptim/limits/penalty.py b/bioptim/limits/penalty.py index 50989be13..800fc1f16 100644 --- a/bioptim/limits/penalty.py +++ b/bioptim/limits/penalty.py @@ -949,6 +949,7 @@ def minimize_segment_rotation( controller: PenaltyController, segment: int | str, axes: list | tuple = None, + sequence: str = "xyz", ): """ Track the orientation of a segment in the global with the sequence XYZ. @@ -964,6 +965,8 @@ def minimize_segment_rotation( Name or index of the segment to align with the marker axes: list | tuple The axis that the JCS rotation should be tracked + sequence: str + The sequence of the euler angles (default="xyz") """ from ..models.biorbd.biorbd_model import BiorbdModel @@ -980,7 +983,7 @@ def minimize_segment_rotation( 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] - angles_segment = controller.model.rotation_matrix_to_euler_angles("xyz")(jcs_segment) + angles_segment = controller.model.rotation_matrix_to_euler_angles(sequence)(jcs_segment) if axes is None: axes = [Axis.X, Axis.Y, Axis.Z] diff --git a/bioptim/models/biorbd/biorbd_model.py b/bioptim/models/biorbd/biorbd_model.py index 7d0e0b05e..77d22e3c7 100644 --- a/bioptim/models/biorbd/biorbd_model.py +++ b/bioptim/models/biorbd/biorbd_model.py @@ -143,20 +143,12 @@ def rotation_matrix_to_euler_angles(self, sequence) -> Function: ) return casadi_fun - def biorbd_homogeneous_matrices_in_global(self, q, segment_idx, inverse=False) -> tuple: - """ - Returns a biorbd object containing the roto-translation matrix of the segment in the global reference frame. - This is useful if you want to interact with biorbd directly later on. - TODO: Charbie fix this with ApplyRT wrapper - """ - rt_matrix = self.model.globalJCS(GeneralizedCoordinates(q), segment_idx) - return rt_matrix.transpose() if inverse else rt_matrix - def homogeneous_matrices_in_global(self, segment_idx, inverse=False) -> Function: """ Returns the roto-translation matrix of the segment in the global reference frame. """ - biorbd_return = self.biorbd_homogeneous_matrices_in_global(self.q, segment_idx, inverse).to_mx() + jcs = self.model.globalJCS(GeneralizedCoordinates(self.q), segment_idx).to_mx() + biorbd_return = jcs.T if inverse else jcs casadi_fun = Function( "homogeneous_matrices_in_global", [self.q], diff --git a/tests/shard4/test_penalty.py b/tests/shard4/test_penalty.py index e35f84051..0367e0e00 100644 --- a/tests/shard4/test_penalty.py +++ b/tests/shard4/test_penalty.py @@ -52,7 +52,7 @@ def prepare_test_ocp( elif with_contact: bio_model = BiorbdModel( bioptim_folder + "/examples/muscle_driven_with_contact/models/2segments_4dof_2contacts_1muscle.bioMod", - segments_to_apply_external_forces=["", ""], # TODO: Charbie add the proper segments + # segments_to_apply_external_forces=["", ""], # TODO: Charbie add the proper segments ) dynamics = DynamicsList() rigidbody_dynamics = RigidBodyDynamics.DAE_INVERSE_DYNAMICS if implicit else RigidBodyDynamics.ODE From f638af0f7e6161f5072b91b5cfb8fa71cc8d5531 Mon Sep 17 00:00:00 2001 From: eve-mac Date: Thu, 19 Sep 2024 17:11:05 -0400 Subject: [PATCH 010/108] blacked --- bioptim/limits/penalty.py | 28 ++++++++++--------- bioptim/models/biorbd/biorbd_model.py | 12 +++------ bioptim/models/biorbd/multi_biorbd_model.py | 30 ++++++++++++++------- bioptim/models/protocols/biomodel.py | 4 +-- 4 files changed, 42 insertions(+), 32 deletions(-) diff --git a/bioptim/limits/penalty.py b/bioptim/limits/penalty.py index 800fc1f16..79eded294 100644 --- a/bioptim/limits/penalty.py +++ b/bioptim/limits/penalty.py @@ -341,7 +341,9 @@ def minimize_markers_velocity( penalty.quadratic = True if penalty.quadratic is None else penalty.quadratic # Add the penalty in the requested reference frame. None for global - markers = horzcat(*controller.model.marker_velocities(reference_index=reference_jcs)(controller.q.cx, controller.qdot.cx)) + markers = horzcat( + *controller.model.marker_velocities(reference_index=reference_jcs)(controller.q.cx, controller.qdot.cx) + ) return markers @@ -380,9 +382,9 @@ def minimize_markers_acceleration( qddot = PenaltyFunctionAbstract._get_qddot(controller, "cx") markers = horzcat( - *controller.model.marker_accelerations(reference_index=reference_jcs)(controller.q.cx, - controller.qdot.cx, - qddot) + *controller.model.marker_accelerations(reference_index=reference_jcs)( + controller.q.cx, controller.qdot.cx, qddot + ) ) return markers @@ -425,9 +427,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(first_marker_idx)( - controller.q.cx - ) + diff_markers = controller.model.marker(second_marker_idx)(controller.q.cx) - controller.model.marker( + first_marker_idx + )(controller.q.cx) return diff_markers @@ -597,7 +599,7 @@ def minimize_predicted_com_height(_: PenaltyOption, controller: PenaltyControlle The penalty node elements """ - g = controller.model.gravity()['o0'][2] + 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_height = (com_dot[2] * com_dot[2]) / (2 * -g) + com[2] @@ -672,9 +674,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) return marker @@ -860,7 +860,11 @@ def minimize_soft_contact_forces( @staticmethod def track_segment_with_custom_rt( - penalty: PenaltyOption, controller: PenaltyController, segment: int | str, rt_idx: int, sequence: str = "zyx" + penalty: PenaltyOption, + controller: PenaltyController, + segment: int | str, + rt_idx: int, + sequence: str = "zyx", ): """ Minimize the difference of the euler angles extracted from the coordinate system of a segment diff --git a/bioptim/models/biorbd/biorbd_model.py b/bioptim/models/biorbd/biorbd_model.py index 77d22e3c7..8d8931b4d 100644 --- a/bioptim/models/biorbd/biorbd_model.py +++ b/bioptim/models/biorbd/biorbd_model.py @@ -132,9 +132,7 @@ def rotation_matrix_to_euler_angles(self, sequence) -> Function: """ r = MX.sym("r_mx", 3, 3) # @Pariterre: is this the right order? - r_matrix = biorbd.Rotation(r[0, 0], r[0, 1], r[0, 2], - r[0, 0], r[0, 1], r[0, 2], - r[0, 0], r[0, 1], r[0, 2]) + r_matrix = biorbd.Rotation(r[0, 0], r[0, 1], r[0, 2], r[0, 0], r[0, 1], r[0, 2], r[0, 0], r[0, 1], r[0, 2]) biorbd_return = biorbd.Rotation.toEulerAngles(r_matrix, sequence).to_mx() casadi_fun = Function( "rotation_matrix_to_euler_angles", @@ -409,7 +407,7 @@ def _dispatch_forces(self, external_forces: MX): return external_forces_set - def forward_dynamics(self, with_contact: bool=False) -> Function: + def forward_dynamics(self, with_contact: bool = False) -> Function: external_forces_set = self._dispatch_forces(self.external_forces) q_biorbd = GeneralizedCoordinates(self.q) @@ -434,7 +432,7 @@ def forward_dynamics(self, with_contact: bool=False) -> Function: ) return casadi_fun - def inverse_dynamics(self, with_contact: bool=False) -> Function: + def inverse_dynamics(self, with_contact: bool = False) -> Function: # @ipuch: I do not understand what is happening here? Do we have f_ext or it is just the contact forces? if with_contact: f_ext = self.reshape_fext_to_fcontact(self.external_forces) @@ -790,9 +788,7 @@ def contact_forces(self, external_forces: MX = None) -> Function: [biorbd_return], ) else: - biorbd_return = self.contact_forces_from_constrained_forward_dynamics()( - self.q, self.qdot, self.tau, MX() - ) + biorbd_return = self.contact_forces_from_constrained_forward_dynamics()(self.q, self.qdot, self.tau, MX()) casadi_fun = Function( "contact_forces", [self.q, self.qdot, self.tau], diff --git a/bioptim/models/biorbd/multi_biorbd_model.py b/bioptim/models/biorbd/multi_biorbd_model.py index 75945bbc8..a32e0f2e6 100644 --- a/bioptim/models/biorbd/multi_biorbd_model.py +++ b/bioptim/models/biorbd/multi_biorbd_model.py @@ -295,9 +295,13 @@ def center_of_mass_velocity(self) -> MX: for i, model in enumerate(self.models): q_model = model.q[self.variable_index("q", i)] qdot_model = model.qdot[self.variable_index("qdot", i)] - out = model.center_of_mass_velocity()(q_model, qdot_model) if i == 0 else vertcat( - out, - model.center_of_mass_velocity()(q_model, qdot_model), + out = ( + model.center_of_mass_velocity()(q_model, qdot_model) + if i == 0 + else vertcat( + out, + model.center_of_mass_velocity()(q_model, qdot_model), + ) ) return out @@ -307,9 +311,13 @@ def center_of_mass_acceleration(self) -> MX: q_model = model.q[self.variable_index("q", i)] qdot_model = model.qdot[self.variable_index("qdot", i)] qddot_model = model.qddot[self.variable_index("qddot", i)] - out = model.center_of_mass_acceleration()(q_model, qdot_model, qddot_model) if i == 0 else vertcat( - out, - model.center_of_mass_acceleration()(q_model, qdot_model, qddot_model), + out = ( + model.center_of_mass_acceleration()(q_model, qdot_model, qddot_model) + if i == 0 + else vertcat( + out, + model.center_of_mass_acceleration()(q_model, qdot_model, qddot_model), + ) ) return out @@ -332,9 +340,13 @@ def angular_momentum(self) -> MX: for i, model in enumerate(self.models): q_model = self.q[self.variable_index("q", i)] qdot_model = self.qdot[self.variable_index("qdot", i)] - out = model.angular_momentum()(q_model, qdot_model) if i == 0 else vertcat( - out, - model.angular_momentum()(q_model, qdot_model), + out = ( + model.angular_momentum()(q_model, qdot_model) + if i == 0 + else vertcat( + out, + model.angular_momentum()(q_model, qdot_model), + ) ) return out diff --git a/bioptim/models/protocols/biomodel.py b/bioptim/models/protocols/biomodel.py index d67bbc268..9fc63d89e 100644 --- a/bioptim/models/protocols/biomodel.py +++ b/bioptim/models/protocols/biomodel.py @@ -358,9 +358,7 @@ def lagrangian(self) -> Function: The Lagrangian. """ - def partitioned_forward_dynamics( - self, q_u, qdot_u, tau, external_forces=None, q_v_init=None - ) -> Function: + def partitioned_forward_dynamics(self, q_u, qdot_u, tau, external_forces=None, q_v_init=None) -> Function: """ @ipuch: I need help on how to implement this! This is the forward dynamics of the model, but only for the independent joints From 420d5d1ad7b1e7d5fa666bc0f196691b2483e628 Mon Sep 17 00:00:00 2001 From: eve-mac Date: Fri, 20 Sep 2024 05:43:47 -0400 Subject: [PATCH 011/108] fixed some dynamics --- bioptim/dynamics/configure_problem.py | 60 +++++++------- bioptim/dynamics/dynamics_functions.py | 23 +++--- .../example_continuity_as_objective.py | 4 +- .../arm_reaching_muscle_driven.py | 20 ++--- .../arm_reaching_torque_driven_explicit.py | 21 ++--- bioptim/limits/constraints.py | 64 ++++++--------- bioptim/limits/multinode_penalty.py | 10 +-- bioptim/limits/penalty_controller.py | 4 - bioptim/limits/phase_transition.py | 15 ++-- bioptim/models/biorbd/biorbd_model.py | 28 +++---- bioptim/optimization/non_linear_program.py | 39 +-------- .../optimization/optimal_control_program.py | 2 +- bioptim/optimization/optimization_variable.py | 7 -- bioptim/optimization/parameters.py | 8 +- .../stochastic_optimal_control_program.py | 10 +-- .../variational_optimal_control_program.py | 46 +++++------ tests/shard1/test_dynamics.py | 80 +++++++++---------- tests/shard3/test_graph.py | 8 +- 18 files changed, 185 insertions(+), 264 deletions(-) diff --git a/bioptim/dynamics/configure_problem.py b/bioptim/dynamics/configure_problem.py index c117d368f..5c2b03cc5 100644 --- a/bioptim/dynamics/configure_problem.py +++ b/bioptim/dynamics/configure_problem.py @@ -640,9 +640,9 @@ def torque_derivative_driven( for key in numerical_data_timeseries.keys(): if key == "external_forces": _check_numerical_timeseries_format(numerical_data_timeseries[key], nlp.ns, nlp.phase_idx) - external_forces = nlp.numerical_timeseries[0].mx + external_forces = nlp.numerical_timeseries[0].cx for i in range(1, numerical_data_timeseries[key].shape[1]): - external_forces = horzcat(external_forces, nlp.numerical_timeseries[i].mx) + external_forces = horzcat(external_forces, nlp.numerical_timeseries[i].cx) ConfigureProblem.configure_q(ocp, nlp, True, False) ConfigureProblem.configure_qdot(ocp, nlp, True, False) @@ -731,9 +731,9 @@ def torque_activations_driven( for key in numerical_data_timeseries.keys(): if key == "external_forces": _check_numerical_timeseries_format(numerical_data_timeseries[key], nlp.ns, nlp.phase_idx) - external_forces = nlp.numerical_timeseries[0].mx + external_forces = nlp.numerical_timeseries[0].cx for i in range(1, numerical_data_timeseries[key].shape[1]): - external_forces = horzcat(external_forces, nlp.numerical_timeseries[i].mx) + external_forces = horzcat(external_forces, nlp.numerical_timeseries[i].cx) ConfigureProblem.configure_q(ocp, nlp, True, False) ConfigureProblem.configure_qdot(ocp, nlp, True, False) @@ -867,9 +867,9 @@ def muscle_driven( for key in numerical_data_timeseries.keys(): if key == "external_forces": _check_numerical_timeseries_format(numerical_data_timeseries[key], nlp.ns, nlp.phase_idx) - external_forces = nlp.numerical_timeseries[0].mx + external_forces = nlp.numerical_timeseries[0].cx for i in range(1, numerical_data_timeseries[key].shape[1]): - external_forces = horzcat(external_forces, nlp.numerical_timeseries[i].mx) + external_forces = horzcat(external_forces, nlp.numerical_timeseries[i].cx) if fatigue is not None and "tau" in fatigue and not with_residual_torque: raise RuntimeError("Residual torques need to be used to apply fatigue on torques") @@ -985,26 +985,26 @@ def configure_lagrange_multipliers_function(ocp, nlp, dyn_func: Callable, **extr The function to get the values of contact forces from the dynamics """ - time_span_sym = vertcat(nlp.time_mx, nlp.dt_mx) + time_span_sym = vertcat(nlp.time_cx, nlp.dt) nlp.lagrange_multipliers_function = Function( "lagrange_multipliers_function", [ time_span_sym, - nlp.states.scaled.mx_reduced, - nlp.controls.scaled.mx_reduced, - nlp.parameters.scaled.mx_reduced, - nlp.algebraic_states.scaled.mx_reduced, - nlp.numerical_timeseries.mx, + nlp.states.scaled.cx, + nlp.controls.scaled.cx, + nlp.parameters.scaled.cx, + nlp.algebraic_states.scaled.cx, + nlp.numerical_timeseries.cx, ], [ dyn_func( nlp.get_var_from_states_or_controls( - "q_u", nlp.states.scaled.mx_reduced, nlp.controls.scaled.mx_reduced + "q_u", nlp.states.scaled.cx, nlp.controls.scaled.cx ), nlp.get_var_from_states_or_controls( - "qdot_u", nlp.states.scaled.mx_reduced, nlp.controls.scaled.mx_reduced + "qdot_u", nlp.states.scaled.cx, nlp.controls.scaled.cx ), - DynamicsFunctions.get(nlp.controls["tau"], nlp.controls.scaled.mx_reduced), + DynamicsFunctions.get(nlp.controls["tau"], nlp.controls.scaled.cx), ) ], ["t_span", "x", "u", "p", "a", "d"], @@ -1053,21 +1053,21 @@ def configure_qv(ocp, nlp, dyn_func: Callable, **extra_params): The function to get the values of contact forces from the dynamics """ - time_span_sym = vertcat(nlp.time_mx, nlp.dt_mx) + time_span_sym = vertcat(nlp.time_cx, nlp.dt) nlp.q_v_function = Function( "qv_function", [ time_span_sym, - nlp.states.scaled.mx_reduced, - nlp.controls.scaled.mx_reduced, - nlp.parameters.scaled.mx_reduced, - nlp.algebraic_states.scaled.mx_reduced, - nlp.numerical_timeseries.mx, + nlp.states.scaled.cx, + nlp.controls.scaled.cx, + nlp.parameters.scaled.cx, + nlp.algebraic_states.scaled.cx, + nlp.numerical_timeseries.cx, ], [ dyn_func( nlp.get_var_from_states_or_controls( - "q_u", nlp.states.scaled.mx_reduced, nlp.controls.scaled.mx_reduced + "q_u", nlp.states.scaled.cx, nlp.controls.scaled.cx ), ) ], @@ -1113,24 +1113,24 @@ def configure_qdotv(ocp, nlp, dyn_func: Callable, **extra_params): The function to get the values of contact forces from the dynamics """ - time_span_sym = vertcat(nlp.time_mx, nlp.dt_mx) + time_span_sym = vertcat(nlp.time_cx, nlp.dt) nlp.q_v_function = Function( "qdot_v_function", [ time_span_sym, - nlp.states.scaled.mx_reduced, - nlp.controls.scaled.mx_reduced, - nlp.parameters.scaled.mx_reduced, - nlp.algebraic_states.scaled.mx_reduced, - nlp.numerical_timeseries.mx, + nlp.states.scaled.cx, + nlp.controls.scaled.cx, + nlp.parameters.scaled.cx, + nlp.algebraic_states.scaled.cx, + nlp.numerical_timeseries.cx, ], [ dyn_func( nlp.get_var_from_states_or_controls( - "q_u", nlp.states.scaled.mx_reduced, nlp.controls.scaled.mx_reduced + "q_u", nlp.states.scaled.cx, nlp.controls.scaled.cx ), nlp.get_var_from_states_or_controls( - "qdot_u", nlp.states.scaled.mx_reduced, nlp.controls.scaled.mx_reduced + "qdot_u", nlp.states.scaled.cx, nlp.controls.scaled.cx ), ) ], diff --git a/bioptim/dynamics/dynamics_functions.py b/bioptim/dynamics/dynamics_functions.py index d6a4452e0..7d6a2023b 100644 --- a/bioptim/dynamics/dynamics_functions.py +++ b/bioptim/dynamics/dynamics_functions.py @@ -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) if with_passive_torque else tau + tau = tau + nlp.model.ligament_joint_torque()(q, qdot) if with_ligament else tau dq = DynamicsFunctions.compute_qdot(nlp, q, qdot) dtau = DynamicsFunctions.get(nlp.controls["taudot"], controls) @@ -675,8 +675,9 @@ def forces_from_torque_driven( 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 + f_ext = [] if external_forces is None else external_forces - return nlp.model.contact_forces(external_forces=external_forces)(q, qdot, tau) + return nlp.model.contact_forces()(q, qdot, tau, f_ext) @staticmethod def forces_from_torque_activation_driven( @@ -860,7 +861,7 @@ def muscles_driven( rigidbody_dynamics is RigidBodyDynamics.ODE and nlp.ode_solver.defects_type == DefectType.IMPLICIT ): if not with_contact and fatigue is None: - qddot = DynamicsFunctions.get(nlp.states_dot["qddot"], nlp.states_dot.mx_reduced) + qddot = DynamicsFunctions.get(nlp.states_dot["qddot"], nlp.states_dot.cx) tau_id = DynamicsFunctions.inverse_dynamics(nlp, q, qdot, qddot, with_contact, external_forces) defects = nlp.cx(dq.shape[0] + tau_id.shape[0], tau_id.shape[1]) @@ -871,7 +872,7 @@ def muscles_driven( - DynamicsFunctions.compute_qdot( nlp, q, - DynamicsFunctions.get(nlp.states_dot["qdot"], nlp.states_dot.mx_reduced), + DynamicsFunctions.get(nlp.states_dot["qdot"], nlp.states_dot.cx), ) ) defects[: dq.shape[0], :] = horzcat(*dq_defects) @@ -993,7 +994,7 @@ def joints_acceleration_driven( if rigidbody_dynamics is not RigidBodyDynamics.ODE or ( rigidbody_dynamics is RigidBodyDynamics.ODE and nlp.ode_solver.defects_type == DefectType.IMPLICIT ): - qddot_root_defects = DynamicsFunctions.get(nlp.states_dot["qddot_roots"], nlp.states_dot.mx_reduced) + qddot_root_defects = DynamicsFunctions.get(nlp.states_dot["qddot_roots"], nlp.states_dot.cx) qddot_defects_reordered = nlp.model.reorder_qddot_root_joints(qddot_root_defects, qddot_joints) floating_base_constraint = nlp.model.inverse_dynamics(q, qdot, qddot_defects_reordered)[: nlp.model.nb_root] @@ -1002,7 +1003,7 @@ def joints_acceleration_driven( defects[: qdot_mapped.shape[0], :] = qdot_mapped - nlp.variable_mappings["qdot"].to_first.map( DynamicsFunctions.compute_qdot( - nlp, q, DynamicsFunctions.get((nlp.states_dot["qdot"]), nlp.states_dot.mx_reduced) + nlp, q, DynamicsFunctions.get((nlp.states_dot["qdot"]), nlp.states_dot.cx) ) ) @@ -1012,7 +1013,7 @@ def joints_acceleration_driven( defects[(qdot_mapped.shape[0] + qddot_root_mapped.shape[0]) :, :] = ( qddot_joints_mapped - nlp.variable_mappings["qddot_joints"].to_first.map( - DynamicsFunctions.get(nlp.states_dot["qddot_joints"], nlp.states_dot.mx_reduced) + DynamicsFunctions.get(nlp.states_dot["qddot_joints"], nlp.states_dot.cx) ) ) @@ -1055,7 +1056,7 @@ def apply_parameters(nlp): if nlp.parameters[param_key].function: param = nlp.parameters[param_key] param_scaling = nlp.parameters[param_key].scaling.scaling - param_reduced = nlp.parameters.scaled.mx_reduced[param.index] + param_reduced = nlp.parameters.scaled.cx[param.index] param.function(nlp.model, param_reduced * param_scaling, **param.kwargs) @staticmethod @@ -1170,7 +1171,7 @@ def inverse_dynamics( """ if external_forces is None: - tau = nlp.model.inverse_dynamics(q, qdot, qddot) + tau = nlp.model.inverse_dynamics(with_contact=with_contact)(q, qdot, qddot, []) else: if "tau" in nlp.states: tau_shape = nlp.states["tau"].cx.shape[0] @@ -1180,7 +1181,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(q, qdot, qddot, external_forces[:, i]) + tau[:, i] = nlp.model.inverse_dynamics(with_contact=with_contact)(q, qdot, qddot, external_forces[:, i]) return tau # We ignore on purpose the mapping to keep zeros in the defects of the dynamic. @staticmethod diff --git a/bioptim/examples/getting_started/example_continuity_as_objective.py b/bioptim/examples/getting_started/example_continuity_as_objective.py index d3e0289b5..46cb7f3cd 100644 --- a/bioptim/examples/getting_started/example_continuity_as_objective.py +++ b/bioptim/examples/getting_started/example_continuity_as_objective.py @@ -43,12 +43,12 @@ def out_of_sphere(controller: PenaltyController, y, z): - q = controller.states["q"].mx + q = controller.states["q"].cx marker_q = controller.model.markers(q)[1] distance = sqrt((y - marker_q[1]) ** 2 + (z - marker_q[2]) ** 2) - return controller.mx_to_cx("out_of_sphere", distance, controller.states["q"]) + return distance def prepare_ocp_first_pass( diff --git a/bioptim/examples/stochastic_optimal_control/arm_reaching_muscle_driven.py b/bioptim/examples/stochastic_optimal_control/arm_reaching_muscle_driven.py index 9aa9ebf16..4e5279db0 100644 --- a/bioptim/examples/stochastic_optimal_control/arm_reaching_muscle_driven.py +++ b/bioptim/examples/stochastic_optimal_control/arm_reaching_muscle_driven.py @@ -199,21 +199,21 @@ def get_cov_mat(nlp, node_index): dx = stochastic_forward_dynamics( nlp.time_cx, - nlp.states.mx, - nlp.controls.mx, - nlp.parameters.mx, - nlp.algebraic_states.mx, - nlp.numerical_timeseries.mx, + nlp.states.cx, + nlp.controls.cx, + nlp.parameters.cx, + nlp.algebraic_states.cx, + nlp.numerical_timeseries.cx, nlp, force_field_magnitude=nlp.model.force_field_magnitude, with_noise=True, ) - dx.dxdt = cas.Function( - "tp", - [nlp.states.mx, nlp.controls.mx, nlp.parameters.mx, nlp.algebraic_states.mx, nlp.numerical_timeseries.mx], - [dx.dxdt], - )(nlp.states.cx, nlp.controls.cx, nlp.parameters.cx, nlp.algebraic_states.cx, nlp.numerical_timeseries.cx) + # dx.dxdt = cas.Function( + # "tp", + # [nlp.states.mx, nlp.controls.mx, nlp.parameters.mx, nlp.algebraic_states.mx, nlp.numerical_timeseries.mx], + # [dx.dxdt], + # )(nlp.states.cx, nlp.controls.cx, nlp.parameters.cx, nlp.algebraic_states.cx, nlp.numerical_timeseries.cx) ddx_dwm = cas.jacobian(dx.dxdt, cas.vertcat(sensory_noise, motor_noise)) dg_dw = -ddx_dwm * dt diff --git a/bioptim/examples/stochastic_optimal_control/arm_reaching_torque_driven_explicit.py b/bioptim/examples/stochastic_optimal_control/arm_reaching_torque_driven_explicit.py index 53add3757..19fdbcd65 100644 --- a/bioptim/examples/stochastic_optimal_control/arm_reaching_torque_driven_explicit.py +++ b/bioptim/examples/stochastic_optimal_control/arm_reaching_torque_driven_explicit.py @@ -192,19 +192,19 @@ def get_cov_mat(nlp, node_index, use_sx): dx = stochastic_forward_dynamics( nlp.time_cx, - nlp.states.mx, - nlp.controls.mx, - nlp.parameters.mx, - nlp.algebraic_states.mx, - nlp.numerical_timeseries.mx, + nlp.states.cx, + nlp.controls.cx, + nlp.parameters.cx, + nlp.algebraic_states.cx, + nlp.numerical_timeseries.cx, nlp, with_noise=True, ) - dx.dxdt = cas.Function( - "tp", - [nlp.states.mx, nlp.controls.mx, nlp.parameters.mx, nlp.algebraic_states.mx, nlp.numerical_timeseries.mx], - [dx.dxdt], - )(nlp.states.cx, nlp.controls.cx, nlp.parameters.cx, nlp.algebraic_states.cx, nlp.numerical_timeseries.cx) + # dx.dxdt = cas.Function( + # "tp", + # [nlp.states.cx, nlp.controls.cx, nlp.parameters.cx, nlp.algebraic_states.cx, nlp.numerical_timeseries.cx], + # [dx.dxdt], + # )(nlp.states.cx, nlp.controls.cx, nlp.parameters.cx, nlp.algebraic_states.cx, nlp.numerical_timeseries.cx) ddx_dwm = cas.jacobian(dx.dxdt, cas.vertcat(sensory_noise, motor_noise)) dg_dw = -ddx_dwm * dt @@ -255,6 +255,7 @@ def reach_target_consistently(controllers: list[PenaltyController]) -> cas.MX: applies at the END node. """ + # Charbie todo remove these symbolics q_sym = cas.MX.sym("q_sym", controllers[-1].states["q"].cx.shape[0]) qdot_sym = cas.MX.sym("qdot_sym", controllers[-1].states["qdot"].cx.shape[0]) cov_sym = cas.MX.sym("cov", controllers[-1].integrated_values.cx.shape[0]) diff --git a/bioptim/limits/constraints.py b/bioptim/limits/constraints.py index 90d53233f..8d8db49f4 100644 --- a/bioptim/limits/constraints.py +++ b/bioptim/limits/constraints.py @@ -655,22 +655,27 @@ def stochastic_df_dx_implicit( controller.algebraic_states["a"].cx, controller.model.matrix_shape_a ) - # Charbie todo remove this function - q_root = MX.sym("q_root", nb_root, 1) - q_joints = MX.sym("q_joints", nu, 1) - qdot_root = MX.sym("qdot_root", nb_root, 1) - qdot_joints = MX.sym("qdot_joints", nu, 1) - tau_joints = MX.sym("tau_joints", nu, 1) - algebraic_states_sym = MX.sym("algebraic_states_sym", controller.algebraic_states.shape, 1) - numerical_timeseries_sym = MX.sym("numerical_timeseries_sym", controller.numerical_timeseries.shape, 1) + q_roots = controller.states["q_roots"].cx + q_joints = controller.states["q_joints"].cx + qdot_roots = controller.states["qdot_roots"].cx + qdot_joints = controller.states["qdot_joints"].cx + tau_joints = controller.controls["tau_joints"].cx + + # q_root = MX.sym("q_root", nb_root, 1) + # q_joints = MX.sym("q_joints", nu, 1) + # qdot_root = MX.sym("qdot_root", nb_root, 1) + # qdot_joints = MX.sym("qdot_joints", nu, 1) + # tau_joints = MX.sym("tau_joints", nu, 1) + # algebraic_states_sym = MX.sym("algebraic_states_sym", controller.algebraic_states.shape, 1) + # numerical_timeseries_sym = MX.sym("numerical_timeseries_sym", controller.numerical_timeseries.shape, 1) dx = controller.extra_dynamics(0)( - controller.t_span.mx, - vertcat(q_root, q_joints, qdot_root, qdot_joints), # States + controller.t_span.cx, + vertcat(q_roots, q_joints, qdot_roots, qdot_joints), # States tau_joints, - controller.parameters.mx, - algebraic_states_sym, - numerical_timeseries_sym, + controller.parameters.cx, + controller.algebraic_states.cx, + controller.numerical_timeseries.cx, ) non_root_index = list(range(nb_root, nb_root + nu)) + list( @@ -679,15 +684,15 @@ def stochastic_df_dx_implicit( DF_DX_fun = Function( "DF_DX_fun", [ - controller.t_span.mx, - q_root, + controller.t_span.cx, + q_roots, q_joints, - qdot_root, + qdot_roots, qdot_joints, tau_joints, - controller.parameters.mx, - algebraic_states_sym, - numerical_timeseries_sym, + controller.parameters.cx, + controller.algebraic_states.cx, + controller.numerical_timeseries.cx, ], [jacobian(dx[non_root_index], vertcat(q_joints, qdot_joints))], ) @@ -817,27 +822,6 @@ def stochastic_mean_sensory_input_equals_reference( nlp=controller.get_nlp, ) - # Charbie todo remove this function - sensory_input = Function( - "tp", - [ - controller.t_span.mx, - controller.states.mx, - controller.controls.mx, - controller.parameters.mx, - controller.algebraic_states.mx, - controller.numerical_timeseries.mx, - ], - [sensory_input], - )( - controller.t_span.cx, - controller.states.cx_start, - controller.controls.cx_start, - controller.parameters.cx, - controller.algebraic_states.cx_start, - controller.numerical_timeseries.cx, - ) - return sensory_input[: controller.model.n_feedbacks] - ref[: controller.model.n_feedbacks] @staticmethod diff --git a/bioptim/limits/multinode_penalty.py b/bioptim/limits/multinode_penalty.py index ae1dba68c..6abe9ca2d 100644 --- a/bioptim/limits/multinode_penalty.py +++ b/bioptim/limits/multinode_penalty.py @@ -558,10 +558,10 @@ def stochastic_df_dw_implicit( numerical_timeseries_sym = MX.sym("numerical_timeseries_sym", controllers[0].numerical_timeseries.shape, 1) dx = controllers[0].extra_dynamics(0)( - controllers[0].time.mx, + controllers[0].time.cx, vertcat(q_root, q_joints, qdot_root, qdot_joints), # States tau_joints, - controllers[0].parameters.mx, + controllers[0].parameters.cx, algebraic_states_sym, numerical_timeseries_sym, ) @@ -573,13 +573,13 @@ def stochastic_df_dw_implicit( DF_DW_fun = Function( "DF_DW_fun", [ - controllers[0].time.mx, + controllers[0].time.cx, q_root, q_joints, qdot_root, qdot_joints, tau_joints, - controllers[0].parameters.mx, + controllers[0].parameters.cx, algebraic_states_sym, numerical_timeseries_sym, ], @@ -587,7 +587,7 @@ def stochastic_df_dw_implicit( jacobian( dx[non_root_index], vertcat( - controllers[0].parameters["motor_noise"].mx, controllers[0].parameters["sensory_noise"].mx + controllers[0].parameters["motor_noise"].cx, controllers[0].parameters["sensory_noise"].cx ), ) ], diff --git a/bioptim/limits/penalty_controller.py b/bioptim/limits/penalty_controller.py index 4bf07ca42..dd6558a1d 100644 --- a/bioptim/limits/penalty_controller.py +++ b/bioptim/limits/penalty_controller.py @@ -116,10 +116,6 @@ def phase_idx(self) -> int: def ns(self) -> int: return self._nlp.ns - @property - def mx_to_cx(self): - return self._nlp.mx_to_cx - @property def model(self): return self._nlp.model diff --git a/bioptim/limits/phase_transition.py b/bioptim/limits/phase_transition.py index 5d3d9572a..601252f23 100644 --- a/bioptim/limits/phase_transition.py +++ b/bioptim/limits/phase_transition.py @@ -272,9 +272,9 @@ def impact(transition, controllers: list[PenaltyController, PenaltyController]): warn("The chosen model does not have any rigid contact") # Todo scaled? - q_pre = pre.states["q"].mx - qdot_pre = pre.states["qdot"].mx - qdot_impact = post.model.qdot_from_impact(q_pre, qdot_pre) + q_pre = pre.states["q"].cx + qdot_pre = pre.states["qdot"].cx + qdot_impact = post.model.qdot_from_impact()(q_pre, qdot_pre) val = [] cx_start = [] @@ -282,15 +282,14 @@ def impact(transition, controllers: list[PenaltyController, PenaltyController]): for key in pre.states: cx_end = vertcat(cx_end, pre.states[key].mapping.to_second.map(pre.states[key].cx)) cx_start = vertcat(cx_start, post.states[key].mapping.to_second.map(post.states[key].cx)) - post_mx = post.states[key].mx + post_cx = post.states[key].cx continuity = post.states["qdot"].mapping.to_first.map( - qdot_impact - post_mx if key == "qdot" else pre.states[key].mx - post_mx + qdot_impact - post_cx if key == "qdot" else pre.states[key].cx - post_cx ) val = vertcat(val, continuity) - name = f"PHASE_TRANSITION_{pre.phase_idx % ocp.n_phases}_{post.phase_idx % ocp.n_phases}" - func = pre.to_casadi_func(name, val, pre.states.mx, post.states.mx)(cx_end, cx_start) - return func + # name = f"PHASE_TRANSITION_{pre.phase_idx % ocp.n_phases}_{post.phase_idx % ocp.n_phases}" + return val @staticmethod def covariance_cyclic( diff --git a/bioptim/models/biorbd/biorbd_model.py b/bioptim/models/biorbd/biorbd_model.py index 8d8931b4d..ce0704290 100644 --- a/bioptim/models/biorbd/biorbd_model.py +++ b/bioptim/models/biorbd/biorbd_model.py @@ -775,25 +775,15 @@ def get_quaternion_idx(self) -> list[list[int]]: n_dof += self.segments[j].nbDof() return quat_idx - def contact_forces(self, external_forces: MX = None) -> Function: - if external_forces is not None: - for i in range(external_forces.shape[1]): - force = self.contact_forces_from_constrained_forward_dynamics()( - self.q, self.qdot, self.tau, external_forces[:, i] - ) - biorbd_return = force if i == 0 else horzcat(biorbd_return, force) - casadi_fun = Function( - "contact_forces", - [self.q, self.qdot, self.tau, self.external_forces], - [biorbd_return], - ) - else: - biorbd_return = self.contact_forces_from_constrained_forward_dynamics()(self.q, self.qdot, self.tau, MX()) - casadi_fun = Function( - "contact_forces", - [self.q, self.qdot, self.tau], - [biorbd_return], - ) + def contact_forces(self) -> Function: + force = self.contact_forces_from_constrained_forward_dynamics()( + self.q, self.qdot, self.tau, self.external_forces + ) + casadi_fun = Function( + "contact_forces", + [self.q, self.qdot, self.tau, self.external_forces], + [force], + ) return casadi_fun def passive_joint_torque(self) -> Function: diff --git a/bioptim/optimization/non_linear_program.py b/bioptim/optimization/non_linear_program.py index 5f918b4c0..37c06ec7d 100644 --- a/bioptim/optimization/non_linear_program.py +++ b/bioptim/optimization/non_linear_program.py @@ -126,13 +126,11 @@ class NonLinearProgram: Add to the pool of declared casadi function. If the function already exists, it is skipped to_casadi_func Converts a symbolic expression into a casadi function - mx_to_cx - Add to the pool of declared casadi function. If the function already exists, it is skipped node_time(self, node_idx: int) Gives the time for a specific index """ - def __init__(self, phase_dynamics: PhaseDynamics): + def __init__(self, phase_dynamics: PhaseDynamics, use_sx: bool): self.casadi_func = {} self.contact_forces_func = None self.soft_contact_forces_func = None @@ -195,7 +193,7 @@ def __init__(self, phase_dynamics: PhaseDynamics): # parameters is currently a clone of ocp.parameters, but should hold phase parameters from ..optimization.parameters import ParameterContainer - self.parameters = ParameterContainer() + self.parameters = ParameterContainer(use_sx=use_sx) self.algebraic_states = OptimizationVariableContainer(self.phase_dynamics) self.integrated_values = OptimizationVariableContainer(self.phase_dynamics) @@ -419,39 +417,6 @@ def add_casadi_func(self, name: str, function: Callable | SX | MX, *all_param: A self.casadi_func[name] = self.to_casadi_func(name, function, *mx) return self.casadi_func[name] - @staticmethod - def mx_to_cx(name: str, symbolic_expression: SX | MX | Callable, *all_param: Any) -> Function: - """ - Add to the pool of declared casadi function. If the function already exists, it is skipped - - Parameters - ---------- - name: str - The unique name of the function to add to the casadi functions pool - symbolic_expression: SX | MX | Callable - The symbolic expression to be converted, also support Callables - all_param: Any - Any parameters to pass to the biorbd function - """ - - from ..optimization.optimization_variable import OptimizationVariable, OptimizationVariableList - from ..optimization.parameters import Parameter, ParameterList - - cx_types = OptimizationVariable, OptimizationVariableList, Parameter, ParameterList - mx = [var.mx if isinstance(var, cx_types) else var for var in all_param] - cx = [] - for var in all_param: - if hasattr(var, "mapping"): - cx += [var.mapping.to_second.map(var.cx_start)] - elif hasattr(var, "cx_start"): - cx += [var.cx_start] - else: - raise RuntimeError( - f"Variable {var} is not of the good type ({type(var)}), it should be an OptimizationVariable or a Parameter." - ) - - return NonLinearProgram.to_casadi_func(name, symbolic_expression, *mx)(*cx) - @staticmethod def to_casadi_func(name, symbolic_expression: MX | SX | Callable, *all_param, expand=True) -> Function: """ diff --git a/bioptim/optimization/optimal_control_program.py b/bioptim/optimization/optimal_control_program.py index b1a006d9f..92bdbdfbc 100644 --- a/bioptim/optimization/optimal_control_program.py +++ b/bioptim/optimization/optimal_control_program.py @@ -534,7 +534,7 @@ def _check_arguments_and_build_nlp( self.g_implicit = [] # nlp is the core of a phase - self.nlp = [NLP(dynamics[i].phase_dynamics) for i in range(self.n_phases)] + self.nlp = [NLP(dynamics[i].phase_dynamics, use_sx) for i in range(self.n_phases)] NLP.add(self, "model", bio_model, False) NLP.add(self, "phase_idx", [i for i in range(self.n_phases)], False) diff --git a/bioptim/optimization/optimization_variable.py b/bioptim/optimization/optimization_variable.py index 8a228f124..cb7133d7a 100644 --- a/bioptim/optimization/optimization_variable.py +++ b/bioptim/optimization/optimization_variable.py @@ -189,7 +189,6 @@ def __init__(self, cx_constructor, phase_dynamics): self._cx_mid: MX | SX | np.ndarray = np.array([]) self._cx_end: MX | SX | np.ndarray = np.array([]) self._cx_intermediates: list = [] - # self.mx_reduced: MX = MX.sym("var", 0, 0) self.cx_constructor = cx_constructor self._current_cx_to_get = 0 self.phase_dynamics = phase_dynamics @@ -310,7 +309,6 @@ def append(self, name: str, cx: list, mx: MX, bimapping: BiMapping): else: self._cx_intermediates[i] = vertcat(self._cx_intermediates[i], c) - # self.mx_reduced = vertcat(self.mx_reduced, MX.sym("var", cx[0].shape[0])) self.elements.append(OptimizationVariable(name, mx, cx, index, bimapping, parent_list=self)) def append_from_scaled( @@ -346,7 +344,6 @@ def append_from_scaled( else: self._cx_intermediates[i] = vertcat(self._cx_intermediates[i], c) - # self.mx_reduced = scaled_optimization_variable.mx_reduced var = scaled_optimization_variable[name] self.elements.append(OptimizationVariable(name, var.mx, cx, var.index, var.mapping, self)) @@ -565,10 +562,6 @@ def mx(self): # todo Charbie remove return self.unscaled.mx - # @property - # def mx_reduced(self): - # return self.unscaled.mx_reduced - @property def cx(self): return self.unscaled.cx diff --git a/bioptim/optimization/parameters.py b/bioptim/optimization/parameters.py index c8bf19254..c2b325745 100644 --- a/bioptim/optimization/parameters.py +++ b/bioptim/optimization/parameters.py @@ -170,7 +170,6 @@ def add( self.scaling.add(key=name, scaling=scaling) index = range(self._cx_start.shape[0], self._cx_start.shape[0] + cx[0].shape[0]) self._cx_start = vertcat(self._cx_start, cx[0]) - # self.mx_reduced = vertcat(self.mx_reduced, MX.sym("var", cx[0].shape[0])) mx = MX.sym(name, size) self.elements.append( Parameter( @@ -224,7 +223,6 @@ def to_unscaled( unscaled_parameter._cx_start = vertcat( unscaled_parameter._cx_start, element.cx_start * element.scaling.scaling ) - # unscaled_parameter.mx_reduced = vertcat(unscaled_parameter.cx, element.cx * element.scaling.scaling) return unscaled_parameter @@ -261,10 +259,10 @@ class ParameterContainer(OptimizationVariableContainer): A parameter container (i.e., the list of scaled parameters and a list of unscaled parameters). """ - def __init__(self): + def __init__(self, use_sx: bool): super(ParameterContainer, self).__init__(phase_dynamics=PhaseDynamics.SHARED_DURING_THE_PHASE) - self._scaled: ParameterList = ParameterList(use_sx=True) - self._unscaled: ParameterList = ParameterList(use_sx=True) + self._scaled: ParameterList = ParameterList(use_sx=use_sx) + self._unscaled: ParameterList = ParameterList(use_sx=use_sx) @property def node_index(self): diff --git a/bioptim/optimization/stochastic_optimal_control_program.py b/bioptim/optimization/stochastic_optimal_control_program.py index 7940e34ff..e4e3d0519 100644 --- a/bioptim/optimization/stochastic_optimal_control_program.py +++ b/bioptim/optimization/stochastic_optimal_control_program.py @@ -349,13 +349,13 @@ def get_ref_init(time_vector, x_guess, u_guess, p_guess, nlp): ) casadi_func = Function( "sensory_reference", - [nlp.dt_mx, nlp.time_mx, nlp.states.mx, nlp.controls.mx, nlp.parameters.mx], + [nlp.dt, nlp.time_cx, nlp.states.cx, nlp.controls.cx, nlp.parameters.cx], [ nlp.model.sensory_reference( - time=nlp.time_mx, - states=nlp.states.mx, - controls=nlp.controls.mx, - parameters=nlp.parameters.mx, + time=nlp.time_cx, + states=nlp.states.cx, + controls=nlp.controls.cx, + parameters=nlp.parameters.cx, algebraic_states=None, # Sensory reference should not depend on stochastic variables numerical_timeseries=None, nlp=nlp, diff --git a/bioptim/optimization/variational_optimal_control_program.py b/bioptim/optimization/variational_optimal_control_program.py index f54483dbc..8451885bd 100644 --- a/bioptim/optimization/variational_optimal_control_program.py +++ b/bioptim/optimization/variational_optimal_control_program.py @@ -245,35 +245,35 @@ def configure_dynamics_function( nlp.dynamics_func = Function( "ForwardDyn", [ - vertcat(nlp.time_mx, nlp.dt_mx), - nlp.states.scaled.mx_reduced, - nlp.controls.scaled.mx_reduced, - nlp.parameters.mx_reduced, - nlp.algebraic_states.scaled.mx_reduced, - nlp.numerical_timeseries.mx, + vertcat(nlp.time_cx, nlp.dt), + nlp.states.scaled.cx, + nlp.controls.scaled.cx, + nlp.parameters.cx, + nlp.algebraic_states.scaled.cx, + nlp.numerical_timeseries.cx, ], [dynamics_dxdt], ["t_span", "x", "u", "p", "a", "d"], ["xdot"], ) - dt = MX.sym("time_step") - q_prev = MX.sym("q_prev", nlp.model.nb_q, 1) - q_cur = MX.sym("q_cur", nlp.model.nb_q, 1) - q_next = MX.sym("q_next", nlp.model.nb_q, 1) - control_prev = MX.sym("control_prev", nlp.model.nb_q, 1) - control_cur = MX.sym("control_cur", nlp.model.nb_q, 1) - control_next = MX.sym("control_next", nlp.model.nb_q, 1) - q0 = MX.sym("q0", nlp.model.nb_q, 1) - qdot0 = MX.sym("qdot_start", nlp.model.nb_q, 1) - q1 = MX.sym("q1", nlp.model.nb_q, 1) - control0 = MX.sym("control0", nlp.model.nb_q, 1) - control1 = MX.sym("control1", nlp.model.nb_q, 1) - q_ultimate = MX.sym("q_ultimate", nlp.model.nb_q, 1) - qdot_ultimate = MX.sym("qdot_ultimate", nlp.model.nb_q, 1) - q_penultimate = MX.sym("q_penultimate", nlp.model.nb_q, 1) - controlN_minus_1 = MX.sym("controlN_minus_1", nlp.model.nb_q, 1) - controlN = MX.sym("controlN", nlp.model.nb_q, 1) + dt = nlp.cx.sym("time_step") + q_prev = nlp.cx.sym("q_prev", nlp.model.nb_q, 1) + q_cur = nlp.cx.sym("q_cur", nlp.model.nb_q, 1) + q_next = nlp.cx.sym("q_next", nlp.model.nb_q, 1) + control_prev = nlp.cx.sym("control_prev", nlp.model.nb_q, 1) + control_cur = nlp.cx.sym("control_cur", nlp.model.nb_q, 1) + control_next = nlp.cx.sym("control_next", nlp.model.nb_q, 1) + q0 = nlp.cx.sym("q0", nlp.model.nb_q, 1) + qdot0 = nlp.cx.sym("qdot_start", nlp.model.nb_q, 1) + q1 = nlp.cx.sym("q1", nlp.model.nb_q, 1) + control0 = nlp.cx.sym("control0", nlp.model.nb_q, 1) + control1 = nlp.cx.sym("control1", nlp.model.nb_q, 1) + q_ultimate = nlp.cx.sym("q_ultimate", nlp.model.nb_q, 1) + qdot_ultimate = nlp.cx.sym("qdot_ultimate", nlp.model.nb_q, 1) + q_penultimate = nlp.cx.sym("q_penultimate", nlp.model.nb_q, 1) + controlN_minus_1 = nlp.cx.sym("controlN_minus_1", nlp.model.nb_q, 1) + controlN = nlp.cx.sym("controlN", nlp.model.nb_q, 1) three_nodes_input = [dt, q_prev, q_cur, q_next, control_prev, control_cur, control_next] two_first_nodes_input = [dt, q0, qdot0, q1, control0, control1] diff --git a/tests/shard1/test_dynamics.py b/tests/shard1/test_dynamics.py index db8c4a3e0..252f1ac38 100644 --- a/tests/shard1/test_dynamics.py +++ b/tests/shard1/test_dynamics.py @@ -32,7 +32,7 @@ def __init__(self, nlp, use_sx): self.n_phases = 1 self.nlp = [nlp] parameters_list = ParameterList(use_sx=use_sx) - self.parameters = ParameterContainer() + self.parameters = ParameterContainer(use_sx=use_sx) self.parameters.initialize(parameters_list) self.implicit_constraints = ConstraintList() self.n_threads = 1 @@ -48,15 +48,15 @@ def __init__(self, nlp, use_sx): ) def test_torque_driven(with_contact, with_external_force, cx, rigidbody_dynamics, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(True if cx == SX else False)) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod", segments_to_apply_external_forces=["Seg0"], ) nlp.ns = 5 nlp.cx = cx - nlp.time_mx = MX.sym("time", 1, 1) - nlp.dt_mx = MX.sym("dt", 1, 1) + nlp.time_cx = cx.sym("time", 1, 1) + nlp.dt = cx.sym("dt", 1, 1) nlp.initialize(cx) nlp.x_bounds = np.zeros((nlp.model.nb_q * 3, 1)) @@ -270,14 +270,14 @@ def test_torque_driven(with_contact, with_external_force, cx, rigidbody_dynamics @pytest.mark.parametrize("with_contact", [False, True]) def test_torque_driven_implicit(with_contact, cx, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(True if cx == SX else False)) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod" ) nlp.ns = 5 nlp.cx = cx - nlp.time_mx = MX.sym("time", 1, 1) - nlp.dt_mx = MX.sym("dt", 1, 1) + nlp.time_cx = cx.sym("time", 1, 1) + nlp.dt = cx.sym("dt", 1, 1) nlp.initialize(cx) nlp.x_bounds = np.zeros((nlp.model.nb_q * 3, 1)) @@ -348,14 +348,14 @@ def test_torque_driven_implicit(with_contact, cx, phase_dynamics): @pytest.mark.parametrize("implicit_contact", [False, True]) def test_torque_driven_soft_contacts_dynamics(with_contact, cx, implicit_contact, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(True if cx == SX else False)) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod" ) nlp.ns = 5 nlp.cx = cx - nlp.time_mx = MX.sym("time", 1, 1) - nlp.dt_mx = MX.sym("dt", 1, 1) + nlp.time_cx = cx.sym("time", 1, 1) + nlp.dt = cx.sym("dt", 1, 1) nlp.initialize(cx) nlp.x_bounds = np.zeros((nlp.model.nb_q * (2 + 3), 1)) @@ -423,19 +423,19 @@ def test_torque_driven_soft_contacts_dynamics(with_contact, cx, implicit_contact @pytest.mark.parametrize("phase_dynamics", [PhaseDynamics.SHARED_DURING_THE_PHASE, PhaseDynamics.ONE_PER_NODE]) @pytest.mark.parametrize("cx", [MX, SX]) -@pytest.mark.parametrize("with_external_force", [False, True]) +@pytest.mark.parametrize("with_external_force", [True]) @pytest.mark.parametrize("with_contact", [False, True]) def test_torque_derivative_driven(with_contact, with_external_force, cx, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(True if cx == SX else False)) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod", segments_to_apply_external_forces=["Seg0"], ) nlp.ns = 5 nlp.cx = cx - nlp.time_mx = MX.sym("time", 1, 1) - nlp.dt_mx = MX.sym("dt", 1, 1) + nlp.time_cx = cx.sym("time", 1, 1) + nlp.dt = cx.sym("dt", 1, 1) nlp.initialize(cx) nlp.x_bounds = np.zeros((nlp.model.nb_q * 3, 1)) nlp.u_bounds = np.zeros((nlp.model.nb_q, 1)) @@ -635,14 +635,14 @@ def test_torque_derivative_driven(with_contact, with_external_force, cx, phase_d @pytest.mark.parametrize("with_contact", [False, True]) def test_torque_derivative_driven_implicit(with_contact, cx, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(True if cx == SX else False)) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod" ) nlp.ns = 5 nlp.cx = cx - nlp.time_mx = MX.sym("time", 1, 1) - nlp.dt_mx = MX.sym("dt", 1, 1) + nlp.time_cx = cx.sym("time", 1, 1) + nlp.dt = cx.sym("dt", 1, 1) nlp.initialize(cx) nlp.phase_idx = 0 nlp.x_bounds = np.zeros((nlp.model.nb_q * 4, 1)) @@ -745,14 +745,14 @@ def test_torque_derivative_driven_implicit(with_contact, cx, phase_dynamics): @pytest.mark.parametrize("implicit_contact", [False, True]) def test_torque_derivative_driven_soft_contacts_dynamics(with_contact, cx, implicit_contact, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(True if cx == SX else False)) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod" ) nlp.ns = 5 nlp.cx = cx - nlp.time_mx = MX.sym("time", 1, 1) - nlp.dt_mx = MX.sym("dt", 1, 1) + nlp.time_cx = cx.sym("time", 1, 1) + nlp.dt = cx.sym("dt", 1, 1) nlp.initialize(cx) nlp.x_bounds = np.zeros((nlp.model.nb_q * (2 + 3), 1)) @@ -850,7 +850,7 @@ def test_torque_derivative_driven_soft_contacts_dynamics(with_contact, cx, impli ) def test_soft_contacts_dynamics_errors(dynamics, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(True if cx == SX else False)) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod" ) @@ -889,7 +889,7 @@ def test_soft_contacts_dynamics_errors(dynamics, phase_dynamics): @pytest.mark.parametrize("dynamics", [DynamicsFcn.TORQUE_ACTIVATIONS_DRIVEN]) def test_implicit_dynamics_errors(dynamics, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(True if cx == SX else False)) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod" ) @@ -935,15 +935,15 @@ def test_implicit_dynamics_errors(dynamics, phase_dynamics): @pytest.mark.parametrize("with_contact", [False, True]) def test_torque_activation_driven(with_contact, with_external_force, cx, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(True if cx == SX else False)) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod", segments_to_apply_external_forces=["Seg0"], ) nlp.ns = 5 nlp.cx = cx - nlp.time_mx = MX.sym("time", 1, 1) - nlp.dt_mx = MX.sym("dt", 1, 1) + nlp.time_cx = cx.sym("time", 1, 1) + nlp.dt = cx.sym("dt", 1, 1) nlp.initialize(cx) nlp.x_bounds = np.zeros((nlp.model.nb_q * 2, 1)) @@ -1116,15 +1116,15 @@ def test_torque_activation_driven_with_residual_torque( with_residual_torque, with_external_force, with_passive_torque, cx, phase_dynamics ): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(True if cx == SX else False)) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/torque_driven_ocp/models/2segments_2dof_2contacts.bioMod", segments_to_apply_external_forces=["Seg0"], ) nlp.ns = 5 nlp.cx = cx - nlp.time_mx = MX.sym("time", 1, 1) - nlp.dt_mx = MX.sym("dt", 1, 1) + nlp.time_cx = cx.sym("time", 1, 1) + nlp.dt = cx.sym("dt", 1, 1) nlp.initialize(cx) nlp.x_bounds = np.zeros((nlp.model.nb_q * 2, 1)) nlp.u_bounds = np.zeros((nlp.model.nb_q, 1)) @@ -1296,14 +1296,14 @@ def test_torque_activation_driven_with_residual_torque( @pytest.mark.parametrize("cx", [MX, SX]) def test_torque_driven_free_floating_base(cx, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(True if cx == SX else False)) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod" ) nlp.ns = 5 nlp.cx = cx - nlp.time_mx = MX.sym("time", 1, 1) - nlp.dt_mx = MX.sym("dt", 1, 1) + nlp.time_cx = cx.sym("time", 1, 1) + nlp.dt = cx.sym("dt", 1, 1) nlp.initialize(cx) nlp.x_bounds = np.zeros((nlp.model.nb_q * 3, 1)) @@ -1362,15 +1362,15 @@ def test_muscle_driven( with_excitations, with_contact, with_residual_torque, with_external_force, rigidbody_dynamics, cx, phase_dynamics ): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(True if cx == SX else False)) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/muscle_driven_ocp/models/arm26_with_contact.bioMod", segments_to_apply_external_forces=["r_ulna_radius_hand_rotation1"], ) nlp.ns = 5 nlp.cx = cx - nlp.time_mx = MX.sym("time", 1, 1) - nlp.dt_mx = MX.sym("dt", 1, 1) + nlp.time_cx = cx.sym("time", 1, 1) + nlp.dt = cx.sym("dt", 1, 1) nlp.initialize(cx) nlp.x_bounds = np.zeros((nlp.model.nb_q * 2 + nlp.model.nb_muscles, 1)) @@ -1932,13 +1932,13 @@ def test_muscle_driven( @pytest.mark.parametrize("rigid_body_dynamics", RigidBodyDynamics) def test_joints_acceleration_driven(cx, rigid_body_dynamics, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(True if cx == SX else False)) nlp.model = BiorbdModel(TestUtils.bioptim_folder() + "/examples/getting_started/models/double_pendulum.bioMod") nlp.ns = 5 nlp.cx = cx - nlp.time_mx = MX.sym("time", 1, 1) - nlp.dt_mx = MX.sym("dt", 1, 1) + nlp.time_cx = cx.sym("time", 1, 1) + nlp.dt = cx.sym("dt", 1, 1) nlp.initialize(nlp.cx) nlp.x_bounds = np.zeros((nlp.model.nb_q * 3, 1)) @@ -2018,14 +2018,14 @@ def configure(ocp, nlp, with_contact=None, numerical_data_timeseries=None): ConfigureProblem.configure_contact_function(ocp, nlp, DynamicsFunctions.forces_from_torque_driven) # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(True if cx == SX else False)) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod" ) nlp.ns = 5 nlp.cx = MX - nlp.time_mx = MX.sym("time", 1, 1) - nlp.dt_mx = MX.sym("dt", 1, 1) + nlp.time_cx = cx.sym("time", 1, 1) + nlp.dt = cx.sym("dt", 1, 1) nlp.initialize(nlp.cx) nlp.x_bounds = np.zeros((nlp.model.nb_q * 3, 1)) nlp.u_bounds = np.zeros((nlp.model.nb_q, 1)) diff --git a/tests/shard3/test_graph.py b/tests/shard3/test_graph.py index 90761f6c8..cb1e91881 100644 --- a/tests/shard3/test_graph.py +++ b/tests/shard3/test_graph.py @@ -43,13 +43,7 @@ def custom_func_track_markers(controller: PenaltyController, first_marker: str, # Get the index of the markers from their name marker_0_idx = controller.model.marker_index(first_marker) marker_1_idx = controller.model.marker_index(second_marker) - - # Convert the function to the required format and then subtract - from bioptim import BiorbdModel - - # noinspection PyTypeChecker - model: BiorbdModel = controller.model - markers = controller.mx_to_cx("markers", model.model.markers, controller.q) + markers = controller.model.markers()(controller.q.cx) return markers[:, marker_1_idx] - markers[:, marker_0_idx] From 55ee414726197bc99b8a9b820081b51a034c8b34 Mon Sep 17 00:00:00 2001 From: eve-mac Date: Fri, 20 Sep 2024 06:10:52 -0400 Subject: [PATCH 012/108] test_dynamics --- bioptim/dynamics/dynamics_functions.py | 40 ++++++++++--------- bioptim/models/biorbd/biorbd_model.py | 1 + .../optimization/optimal_control_program.py | 2 +- tests/shard1/test_dynamics.py | 10 ++--- 4 files changed, 28 insertions(+), 25 deletions(-) diff --git a/bioptim/dynamics/dynamics_functions.py b/bioptim/dynamics/dynamics_functions.py index 7d6a2023b..411b5ef40 100644 --- a/bioptim/dynamics/dynamics_functions.py +++ b/bioptim/dynamics/dynamics_functions.py @@ -205,7 +205,7 @@ def torque_driven( @staticmethod def torque_driven_free_floating_base( time, - # states, + states, controls, parameters, algebraic_states, @@ -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) if with_passive_torque: - tau += nlp.model.passive_joint_torque(q, qdot) + tau += nlp.model.passive_joint_torque()(q, qdot) 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) dq = DynamicsFunctions.compute_qdot(nlp, q, qdot) ddq = DynamicsFunctions.forward_dynamics(nlp, q, qdot, tau, with_contact, external_forces) @@ -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 - f_ext = [] if external_forces is None else external_forces + 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 - return nlp.model.contact_forces()(q, qdot, tau, f_ext) + external_forces = [] if external_forces is None else external_forces + return nlp.model.contact_forces()(q, qdot, tau, external_forces) @staticmethod def forces_from_torque_activation_driven( @@ -726,11 +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) + 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 - return nlp.model.contact_forces(q, qdot, tau, external_forces) + external_forces = [] if external_forces is None else external_forces + return nlp.model.contact_forces()(q, qdot, tau, external_forces) @staticmethod def muscles_driven( @@ -830,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) if with_passive_torque else tau + tau = tau + nlp.model.ligament_joint_torque()(q, qdot) if with_ligament else tau dq = DynamicsFunctions.compute_qdot(nlp, q, qdot) @@ -931,10 +932,11 @@ 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) if with_passive_torque else tau + tau = tau + nlp.model.ligament_joint_torque()(q, qdot) if with_ligament else tau - return nlp.model.contact_forces(q, qdot, tau, external_forces) + external_forces = [] if external_forces is None else external_forces + return nlp.model.contact_forces()(q, qdot, tau, external_forces) @staticmethod def joints_acceleration_driven( @@ -981,7 +983,7 @@ def joints_acceleration_driven( qdot = nlp.get_var_from_states_or_controls("qdot", states, controls) qddot_joints = nlp.get_var_from_states_or_controls("qddot", states, controls) - qddot_root = nlp.model.forward_dynamics_free_floating_base(q, qdot, qddot_joints) + qddot_root = nlp.model.forward_dynamics_free_floating_base()(q, qdot, qddot_joints) qddot_reordered = nlp.model.reorder_qddot_root_joints(qddot_root, qddot_joints) qdot_mapped = nlp.variable_mappings["qdot"].to_first.map(qdot) @@ -1201,7 +1203,7 @@ def compute_muscle_dot(nlp, muscle_excitations: MX | SX): The derivative of muscle activations """ - return nlp.model.muscle_activation_dot(muscle_excitations) + return nlp.model.muscle_activation_dot()(muscle_excitations) @staticmethod def compute_tau_from_muscle( diff --git a/bioptim/models/biorbd/biorbd_model.py b/bioptim/models/biorbd/biorbd_model.py index ce0704290..0538998ec 100644 --- a/bioptim/models/biorbd/biorbd_model.py +++ b/bioptim/models/biorbd/biorbd_model.py @@ -480,6 +480,7 @@ def qdot_from_impact(self) -> Function: return casadi_fun def muscle_activation_dot(self) -> Function: + # @Pariterre: there is a problem with the statesSet :/ muscle_excitation_biorbd = self.muscle muscle_states = self.model.stateSet() for k in range(self.model.nbMuscles()): diff --git a/bioptim/optimization/optimal_control_program.py b/bioptim/optimization/optimal_control_program.py index 92bdbdfbc..24994f893 100644 --- a/bioptim/optimization/optimal_control_program.py +++ b/bioptim/optimization/optimal_control_program.py @@ -1062,7 +1062,7 @@ def _declare_parameters(self, parameters: ParameterList): if not isinstance(parameters, ParameterList): raise RuntimeError("new_parameter must be a Parameter or a ParameterList") - self.parameters = ParameterContainer() + self.parameters = ParameterContainer(use_sx=(True if self.cx == SX else False)) self.parameters.initialize(parameters) def update_bounds( diff --git a/tests/shard1/test_dynamics.py b/tests/shard1/test_dynamics.py index 252f1ac38..f0460bd1c 100644 --- a/tests/shard1/test_dynamics.py +++ b/tests/shard1/test_dynamics.py @@ -850,7 +850,7 @@ def test_torque_derivative_driven_soft_contacts_dynamics(with_contact, cx, impli ) def test_soft_contacts_dynamics_errors(dynamics, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(True if cx == SX else False)) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=False) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod" ) @@ -889,7 +889,7 @@ def test_soft_contacts_dynamics_errors(dynamics, phase_dynamics): @pytest.mark.parametrize("dynamics", [DynamicsFcn.TORQUE_ACTIVATIONS_DRIVEN]) def test_implicit_dynamics_errors(dynamics, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(True if cx == SX else False)) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=False) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod" ) @@ -2018,14 +2018,14 @@ def configure(ocp, nlp, with_contact=None, numerical_data_timeseries=None): ConfigureProblem.configure_contact_function(ocp, nlp, DynamicsFunctions.forces_from_torque_driven) # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(True if cx == SX else False)) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=False) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod" ) nlp.ns = 5 nlp.cx = MX - nlp.time_cx = cx.sym("time", 1, 1) - nlp.dt = cx.sym("dt", 1, 1) + nlp.time_cx = nlp.cx.sym("time", 1, 1) + nlp.dt = nlp.cx.sym("dt", 1, 1) nlp.initialize(nlp.cx) nlp.x_bounds = np.zeros((nlp.model.nb_q * 3, 1)) nlp.u_bounds = np.zeros((nlp.model.nb_q, 1)) From 7952c056bd0cebe75cb10da86c521aea36cdbd45 Mon Sep 17 00:00:00 2001 From: eve-mac Date: Fri, 20 Sep 2024 06:11:05 -0400 Subject: [PATCH 013/108] blacked --- bioptim/dynamics/configure_problem.py | 20 +++++--------------- 1 file changed, 5 insertions(+), 15 deletions(-) diff --git a/bioptim/dynamics/configure_problem.py b/bioptim/dynamics/configure_problem.py index 5c2b03cc5..cd36fef92 100644 --- a/bioptim/dynamics/configure_problem.py +++ b/bioptim/dynamics/configure_problem.py @@ -998,12 +998,8 @@ def configure_lagrange_multipliers_function(ocp, nlp, dyn_func: Callable, **extr ], [ dyn_func( - nlp.get_var_from_states_or_controls( - "q_u", nlp.states.scaled.cx, nlp.controls.scaled.cx - ), - nlp.get_var_from_states_or_controls( - "qdot_u", nlp.states.scaled.cx, nlp.controls.scaled.cx - ), + nlp.get_var_from_states_or_controls("q_u", nlp.states.scaled.cx, nlp.controls.scaled.cx), + nlp.get_var_from_states_or_controls("qdot_u", nlp.states.scaled.cx, nlp.controls.scaled.cx), DynamicsFunctions.get(nlp.controls["tau"], nlp.controls.scaled.cx), ) ], @@ -1066,9 +1062,7 @@ def configure_qv(ocp, nlp, dyn_func: Callable, **extra_params): ], [ dyn_func( - nlp.get_var_from_states_or_controls( - "q_u", nlp.states.scaled.cx, nlp.controls.scaled.cx - ), + nlp.get_var_from_states_or_controls("q_u", nlp.states.scaled.cx, nlp.controls.scaled.cx), ) ], ["t_span", "x", "u", "p", "a", "d"], @@ -1126,12 +1120,8 @@ def configure_qdotv(ocp, nlp, dyn_func: Callable, **extra_params): ], [ dyn_func( - nlp.get_var_from_states_or_controls( - "q_u", nlp.states.scaled.cx, nlp.controls.scaled.cx - ), - nlp.get_var_from_states_or_controls( - "qdot_u", nlp.states.scaled.cx, nlp.controls.scaled.cx - ), + nlp.get_var_from_states_or_controls("q_u", nlp.states.scaled.cx, nlp.controls.scaled.cx), + nlp.get_var_from_states_or_controls("qdot_u", nlp.states.scaled.cx, nlp.controls.scaled.cx), ) ], ["t_span", "x", "u", "p", "a", "d"], From e62002c429c75264d73f5aa72e56921c22f8101c Mon Sep 17 00:00:00 2001 From: eve-mac Date: Fri, 20 Sep 2024 09:58:35 -0400 Subject: [PATCH 014/108] multi_model --- .../getting_started/custom_dynamics.py | 2 +- .../muscle_excitations_tracker.py | 14 +- bioptim/limits/constraints.py | 3 +- bioptim/models/biorbd/biorbd_model.py | 2 +- bioptim/models/biorbd/multi_biorbd_model.py | 662 +++++++++++------- bioptim/models/holonomic_constraints.py | 6 +- tests/shard1/test__global_plots.py | 2 +- tests/shard1/test_biorbd_model_size.py | 67 +- tests/shard1/test_biorbd_multi_model.py | 144 ++-- 9 files changed, 512 insertions(+), 390 deletions(-) diff --git a/bioptim/examples/getting_started/custom_dynamics.py b/bioptim/examples/getting_started/custom_dynamics.py index 69faeb168..d5a6f8e0a 100644 --- a/bioptim/examples/getting_started/custom_dynamics.py +++ b/bioptim/examples/getting_started/custom_dynamics.py @@ -74,7 +74,7 @@ def custom_dynamics( # You can directly call biorbd function (as for ddq) or call bioptim accessor (as for dq) dq = DynamicsFunctions.compute_qdot(nlp, q, qdot) * my_additional_factor - ddq = nlp.model.forward_dynamics(q, qdot, tau, []) + ddq = nlp.model.forward_dynamics()(q, qdot, tau, []) # the user has to choose if want to return the explicit dynamics dx/dt = f(x,u,p) # as the first argument of DynamicsEvaluation or diff --git a/bioptim/examples/muscle_driven_ocp/muscle_excitations_tracker.py b/bioptim/examples/muscle_driven_ocp/muscle_excitations_tracker.py index 659915f34..4f2235fa9 100644 --- a/bioptim/examples/muscle_driven_ocp/muscle_excitations_tracker.py +++ b/bioptim/examples/muscle_driven_ocp/muscle_excitations_tracker.py @@ -12,7 +12,7 @@ import biorbd_casadi as biorbd import numpy as np -from casadi import MX, vertcat +from casadi import MX, SX, vertcat, horzcat from matplotlib import pyplot as plt from scipy.integrate import solve_ivp @@ -44,6 +44,7 @@ def generate_data( n_shooting: int, use_residual_torque: bool = True, phase_dynamics: PhaseDynamics = PhaseDynamics.SHARED_DURING_THE_PHASE, + use_sx: bool = False, ) -> tuple: """ Generate random data. If np.random.seed is defined before, it will always return the same results @@ -91,7 +92,8 @@ def generate_data( symbolic_controls = vertcat(*(symbolic_tau, symbolic_mus_controls)) symbolic_parameters = MX.sym("u", 0, 0) - nlp = NonLinearProgram(phase_dynamics=phase_dynamics) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=use_sx) + nlp.cx = SX if use_sx else MX nlp.model = bio_model nlp.variable_mappings = { "q": BiMapping(range(n_q), range(n_q)), @@ -100,8 +102,6 @@ def generate_data( "tau": BiMapping(range(n_tau), range(n_tau)), "muscles": BiMapping(range(n_mus), range(n_mus)), } - markers_func = biorbd.to_casadi_func("ForwardKin", bio_model.markers, symbolic_q) - nlp.states = OptimizationVariableContainer(phase_dynamics=phase_dynamics) nlp.states_dot = OptimizationVariableContainer(phase_dynamics=phase_dynamics) nlp.controls = OptimizationVariableContainer(phase_dynamics=phase_dynamics) @@ -202,7 +202,7 @@ def dyn_interface(t, x, u): def add_to_data(i, q): X[:, i] = q - markers[:, :, i] = markers_func(q[:n_q]) + markers[:, :, i] = horzcat(*bio_model.markers()(q[:n_q])) x_init = np.array([0.0] * n_q + [0.0] * n_qdot + [0.5] * n_mus) add_to_data(0, x_init) @@ -374,10 +374,8 @@ def main(): n_frames = q.shape[1] markers = np.ndarray((3, n_mark, q.shape[1])) - symbolic_states = MX.sym("x", n_q, 1) - markers_func = biorbd.to_casadi_func("ForwardKin", bio_model.markers, symbolic_states) for i in range(n_frames): - markers[:, :, i] = markers_func(q[:, i]) + markers[:, :, i] = horzcat(*bio_model.markers()(q[:, i])) plt.figure("Markers") n_steps_ode = ocp.nlp[0].ode_solver.steps + 1 if ocp.nlp[0].ode_solver.is_direct_collocation else 1 diff --git a/bioptim/limits/constraints.py b/bioptim/limits/constraints.py index 8d8db49f4..82e7460dc 100644 --- a/bioptim/limits/constraints.py +++ b/bioptim/limits/constraints.py @@ -414,7 +414,8 @@ def qddot_equals_forward_dynamics( tau = tau + controller.model.ligament_joint_torque(q, qdot) if with_ligament else tau qddot = controller.controls["qddot"].cx if "qddot" in controller.controls else controller.states["qddot"].cx - qddot_fd = controller.model.forward_dynamics(with_contact=with_contact)(q, qdot, tau) + # @Ipuch: no f_ext allowed ? + qddot_fd = controller.model.forward_dynamics(with_contact=with_contact)(q, qdot, tau, []) return qddot - qddot_fd @staticmethod diff --git a/bioptim/models/biorbd/biorbd_model.py b/bioptim/models/biorbd/biorbd_model.py index 0538998ec..800dfee01 100644 --- a/bioptim/models/biorbd/biorbd_model.py +++ b/bioptim/models/biorbd/biorbd_model.py @@ -221,7 +221,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.tau], + [self.q, self.qdot, self.qddot], [biorbd_return], ) return casadi_fun diff --git a/bioptim/models/biorbd/multi_biorbd_model.py b/bioptim/models/biorbd/multi_biorbd_model.py index a32e0f2e6..91c526c66 100644 --- a/bioptim/models/biorbd/multi_biorbd_model.py +++ b/bioptim/models/biorbd/multi_biorbd_model.py @@ -1,6 +1,6 @@ import biorbd_casadi as biorbd -from casadi import SX, MX, vertcat, Function -from typing import Callable, Any +from casadi import MX, vertcat, Function, horzcat +from typing import Callable from .biorbd_model import BiorbdModel from ..utils import _var_mapping @@ -65,6 +65,15 @@ def __init__( else: raise ValueError("The models should be of type 'str', 'biorbd.Model' or 'bioptim.BiorbdModel'") + # Declaration of MX variables of the right shape for the creation of CasADi Functions + self.q = MX.sym("q_mx", self.nb_q, 1) + self.qdot = MX.sym("qdot_mx", self.nb_qdot, 1) + self.qddot = MX.sym("qddot_mx", self.nb_qddot, 1) + self.qddot_roots = MX.sym("qddot_roots_mx", self.nb_root, 1) + self.qddot_joints = MX.sym("qddot_joints_mx", self.nb_qddot - self.nb_root, 1) + self.tau = MX.sym("tau_mx", self.nb_tau, 1) + self.muscle = MX.sym("muscle_mx", self.nb_muscles, 1) + def __getitem__(self, index): return self.models[index] @@ -217,10 +226,17 @@ def nb_extra_models(self) -> int: return len(self.extra_models) @property - def gravity(self) -> MX: - return vertcat(*(model.gravity for model in self.models)) + def gravity(self) -> Function: + biorbd_return = vertcat(*(model.gravity()['o0'] for model in self.models)) + casadi_fun = Function( + "gravity", + [MX()], + [biorbd_return], + ) + return casadi_fun def set_gravity(self, new_gravity) -> None: + # All models have the same gravity, but it could be changed if needed for model in self.models: model.set_gravity(new_gravity) return @@ -264,114 +280,149 @@ def segments(self) -> tuple[biorbd.Segment, ...]: out += (seg,) return out - def biorbd_homogeneous_matrices_in_global(self, segment_idx, inverse=False) -> biorbd.RotoTrans: - # Charbie todo remove + def homogeneous_matrices_in_global(self, segment_idx, inverse=False) -> Function: local_segment_id, model_id = self.local_variable_id("segment", segment_idx) q_model = self.models[model_id].q - return self.models[model_id].homogeneous_matrices_in_global(q_model, local_segment_id, inverse) - - def homogeneous_matrices_in_global(self, segment_idx, inverse=False) -> MX: - local_segment_id, model_id = self.local_variable_id("segment", segment_idx) - q_model = self.models[model_id].q - return self.models[model_id].homogeneous_matrices_in_global(local_segment_id, inverse)(q_model) - - def homogeneous_matrices_in_child(self, segment_id) -> MX: + biorbd_return = self.models[model_id].homogeneous_matrices_in_global(local_segment_id, inverse)(q_model) + casadi_fun = Function( + "homogeneous_matrices_in_global", + [self.models[model_id].q], + [biorbd_return], + ) + return casadi_fun + + def homogeneous_matrices_in_child(self, segment_id) -> Function: local_id, model_id = self.local_variable_id("segment", segment_id) - return self.models[model_id].homogeneous_matrices_in_child(local_id) + casadi_fun = self.models[model_id].homogeneous_matrices_in_child(local_id) + return casadi_fun @property - def mass(self) -> MX: - return vertcat(*(model.mass for model in self.models)) - - def center_of_mass(self) -> MX: - out = MX() + def mass(self) -> Function: + biorbd_return = vertcat(*(model.mass()['o0'] for model in self.models)) + casadi_fun = Function( + "mass", + [MX()], + [biorbd_return], + ) + return casadi_fun + + def center_of_mass(self) -> Function: + biorbd_return = MX() for i, model in enumerate(self.models): - q_model = model.q[self.variable_index("q", i)] - out = model.center_of_mass if i == 0 else vertcat(out, model.center_of_mass()(q_model)) - return out - - def center_of_mass_velocity(self) -> MX: - out = MX() + q_model = self.q[self.variable_index("q", i)] + biorbd_return = vertcat(biorbd_return, model.center_of_mass()(q_model)) + casadi_fun = Function( + "center_of_mass", + [self.q], + [biorbd_return], + ) + return casadi_fun + + def center_of_mass_velocity(self) -> Function: + biorbd_return = MX() for i, model in enumerate(self.models): - q_model = model.q[self.variable_index("q", i)] - qdot_model = model.qdot[self.variable_index("qdot", i)] - out = ( - model.center_of_mass_velocity()(q_model, qdot_model) - if i == 0 - else vertcat( - out, + q_model = self.q[self.variable_index("q", i)] + qdot_model = self.qdot[self.variable_index("qdot", i)] + biorbd_return = vertcat( + biorbd_return, model.center_of_mass_velocity()(q_model, qdot_model), ) - ) - return out - - def center_of_mass_acceleration(self) -> MX: - out = MX() + casadi_fun = Function( + "center_of_mass_velocity", + [self.q, self.qdot], + [biorbd_return], + ) + return casadi_fun + + def center_of_mass_acceleration(self) -> Function: + biorbd_return = MX() for i, model in enumerate(self.models): - q_model = model.q[self.variable_index("q", i)] - qdot_model = model.qdot[self.variable_index("qdot", i)] - qddot_model = model.qddot[self.variable_index("qddot", i)] - out = ( - model.center_of_mass_acceleration()(q_model, qdot_model, qddot_model) - if i == 0 - else vertcat( - out, + q_model = self.q[self.variable_index("q", i)] + qdot_model = self.qdot[self.variable_index("qdot", i)] + qddot_model = self.qddot[self.variable_index("qddot", i)] + biorbd_return = vertcat( + biorbd_return, model.center_of_mass_acceleration()(q_model, qdot_model, qddot_model), ) - ) - return out - - def mass_matrix(self) -> list[MX]: - out = [] + casadi_fun = Function( + "center_of_mass_acceleration", + [self.q, self.qdot, self.qddot], + [biorbd_return], + ) + return casadi_fun + + def mass_matrix(self) -> Function: + biorbd_return = [] for i, model in enumerate(self.models): - q_model = model.q[self.variable_index("q", i)] - out += [model.mass_matrix()(q_model)] - return out - - def non_linear_effects(self) -> list[MX]: - out = [] + q_model = self.q[self.variable_index("q", i)] + biorbd_return += [model.mass_matrix()(q_model)] + casadi_fun = Function( + "mass_matrix", + [self.q], + biorbd_return, + ) + return casadi_fun + + def non_linear_effects(self) -> Function: + biorbd_return = [] for i, model in enumerate(self.models): - q_model = model.q[self.variable_index("q", i)] - qdot_model = model.qdot[self.variable_index("qdot", i)] - out += [model.non_linear_effects()(q_model, qdot_model)] - return out - - def angular_momentum(self) -> MX: + q_model = self.q[self.variable_index("q", i)] + qdot_model = self.qdot[self.variable_index("qdot", i)] + biorbd_return += [model.non_linear_effects()(q_model, qdot_model)] + casadi_fun = Function( + "non_linear_effects", + [self.q, self.qdot], + biorbd_return, + ) + return casadi_fun + + def angular_momentum(self) -> Function: + biorbd_return = MX() for i, model in enumerate(self.models): q_model = self.q[self.variable_index("q", i)] qdot_model = self.qdot[self.variable_index("qdot", i)] - out = ( - model.angular_momentum()(q_model, qdot_model) - if i == 0 - else vertcat( - out, + biorbd_return = vertcat( + biorbd_return, model.angular_momentum()(q_model, qdot_model), ) - ) - return out - - # Charbie ici ... todo - def reshape_qdot(self, q, qdot, k_stab=1) -> MX: - out = MX() + casadi_fun = Function( + "angular_momentum", + [self.q, self.qdot], + [biorbd_return], + ) + return casadi_fun + + def reshape_qdot(self, k_stab=1) -> Function: + biorbd_return = MX() for i, model in enumerate(self.models): - q_model = q[self.variable_index("q", i)] - qdot_model = qdot[self.variable_index("qdot", i)] - out = vertcat( - out, - model.reshape_qdot(q_model, qdot_model, k_stab), + q_model = self.q[self.variable_index("q", i)] + qdot_model = self.qdot[self.variable_index("qdot", i)] + biorbd_return = vertcat( + biorbd_return, + model.reshape_qdot(k_stab)(q_model, qdot_model), ) - return out - - def segment_angular_velocity(self, q, qdot, idx) -> MX: - out = MX() + casadi_fun = Function( + "reshape_qdot", + [self.q, self.qdot], + [biorbd_return], + ) + return casadi_fun + + def segment_angular_velocity(self, idx) -> Function: + biorbd_return = MX() for i, model in enumerate(self.models): - q_model = q[self.variable_index("q", i)] - qdot_model = qdot[self.variable_index("qdot", i)] - out = vertcat( - out, - model.segment_angular_velocity(q_model, qdot_model, idx), + q_model = self.q[self.variable_index("q", i)] + qdot_model = self.qdot[self.variable_index("qdot", i)] + biorbd_return = vertcat( + biorbd_return, + model.segment_angular_velocity(idx)(q_model, qdot_model), ) - return out + casadi_fun = Function( + "segment_angular_velocity", + [self.q, self.qdot], + [biorbd_return], + ) + return casadi_fun @property def name_dof(self) -> tuple[str, ...]: @@ -390,6 +441,7 @@ def soft_contact_names(self) -> tuple[str, ...]: return tuple([contact for model in self.models for contact in model.soft_contact_names]) def soft_contact(self, soft_contact_index, *args): + # What does that function return? current_number_of_soft_contacts = 0 out = [] for model in self.models: @@ -407,167 +459,199 @@ def muscle_names(self) -> tuple[str, ...]: def nb_muscles(self) -> int: return sum(model.nb_muscles for model in self.models) - def torque(self, tau_activations, q, qdot) -> MX: - out = MX() + def torque(self) -> Function: + biorbd_return = MX() for i, model in enumerate(self.models): - tau_activations_model = tau_activations[self.variable_index("tau", i)] - q_model = q[self.variable_index("q", i)] - qdot_model = qdot[self.variable_index("qdot", i)] - out = vertcat( - out, - model.torque( + tau_activations_model = self.muscle[self.variable_index("tau", i)] + q_model = self.q[self.variable_index("q", i)] + qdot_model = self.qdot[self.variable_index("qdot", i)] + biorbd_return = vertcat( + biorbd_return, + model.torque()( tau_activations_model, q_model, qdot_model, ), ) - return out + casadi_fun = Function( + "torque", + [self.muscle, self.q, self.qdot], + [biorbd_return], + ) + return casadi_fun - def forward_dynamics_free_floating_base(self, q, qdot, qddot_joints) -> MX: - out = MX() + + def forward_dynamics_free_floating_base(self) -> Function: + biorbd_return = MX() for i, model in enumerate(self.models): - q_model = q[self.variable_index("q", i)] - qdot_model = qdot[self.variable_index("qdot", i)] - qddot_joints_model = qddot_joints[self.variable_index("qddot_joints", i)] - out = vertcat( - out, - model.forward_dynamics_free_floating_base( + q_model = self.q[self.variable_index("q", i)] + qdot_model = self.qdot[self.variable_index("qdot", i)] + qddot_joints_model = self.qddot_joints[self.variable_index("qddot_joints", i)] + biorbd_return = vertcat( + biorbd_return, + model.forward_dynamics_free_floating_base()( q_model, qdot_model, qddot_joints_model, ), ) - return out - - def reorder_qddot_root_joints(self, qddot_root, qddot_joints): - out = MX() + casadi_fun = Function( + "forward_dynamics_free_floating_base", + [self.q, self.qdot, self.qddot_joints], + [biorbd_return], + ) + return casadi_fun + + def reorder_qddot_root_joints(self): + biorbd_return = MX() for i, model in enumerate(self.models): - qddot_root_model = qddot_root[self.variable_index("qddot_root", i)] - qddot_joints_model = qddot_joints[self.variable_index("qddot_joints", i)] - out = vertcat( - out, + qddot_root_model = self.qddot_roots[self.variable_index("qddot_root", i)] + qddot_joints_model = self.qddot_joints[self.variable_index("qddot_joints", i)] + biorbd_return = vertcat( + biorbd_return, model.reorder_qddot_root_joints(qddot_root_model, qddot_joints_model), ) - return out + casadi_fun = Function( + "reorder_qddot_root_joints", + [self.qddot_roots, self.qddot_joints], + [biorbd_return], + ) + return casadi_fun - def forward_dynamics(self, with_contact, q, qdot, tau, external_forces=None, f_contacts=None) -> MX: - if f_contacts is not None or external_forces is not None: - raise NotImplementedError( - "External forces and contact forces are not implemented yet for MultiBiorbdModel." - ) - out = MX() - for i, model in enumerate(self.models): - q_model = q[self.variable_index("q", i)] - qdot_model = qdot[self.variable_index("qdot", i)] - tau_model = tau[self.variable_index("tau", i)] - if with_contact: - out = vertcat( - out, - model.constrained_forward_dynamics( - q_model, - qdot_model, - tau_model, - external_forces, - f_contacts, - ), - ) - else: - out = vertcat( - out, - model.forward_dynamics( - q_model, - qdot_model, - tau_model, - external_forces, - f_contacts, - ), - ) - return out + def forward_dynamics(self, with_contact) -> Function: + """External forces and contact forces are not implemented yet for MultiBiorbdModel.""" - def inverse_dynamics(self, q, qdot, qddot, external_forces=None, f_contacts=None) -> MX: - if f_contacts is not None or external_forces is not None: - raise NotImplementedError( - "External forces and contact forces are not implemented yet for MultiBiorbdModel." + biorbd_return = MX() + for i, model in enumerate(self.models): + q_model = self.q[self.variable_index("q", i)] + qdot_model = self.qdot[self.variable_index("qdot", i)] + tau_model = self.tau[self.variable_index("tau", i)] + biorbd_return = vertcat( + biorbd_return, + model.forward_dynamics(with_contact=with_contact)( + q_model, + qdot_model, + tau_model, + [], + ), ) + casadi_fun = Function( + "forward_dynamics", + [self.q, self.qdot, self.tau], + [biorbd_return], + ) + return casadi_fun - out = MX() + def inverse_dynamics(self) -> Function: + """External forces and contact forces are not implemented yet for MultiBiorbdModel.""" + + biorbd_return = MX() for i, model in enumerate(self.models): - q_model = q[self.variable_index("q", i)] - qdot_model = qdot[self.variable_index("qdot", i)] - qddot_model = qddot[self.variable_index("qddot", i)] - out = vertcat( - out, - model.inverse_dynamics( + q_model = self.q[self.variable_index("q", i)] + qdot_model = self.qdot[self.variable_index("qdot", i)] + qddot_model = self.qddot[self.variable_index("qddot", i)] + biorbd_return = vertcat( + biorbd_return, + model.inverse_dynamics()( q_model, qdot_model, qddot_model, - external_forces, - f_contacts, + [] ), ) - return out - - def contact_forces_from_constrained_forward_dynamics(self, q, qdot, tau, external_forces=None) -> MX: - if external_forces is not None: - raise NotImplementedError("External forces are not implemented yet for MultiBiorbdModel.") - out = MX() + casadi_fun = Function( + "inverse_dynamics", + [self.q, self.qdot, self.qddot], + [biorbd_return], + ) + return casadi_fun + + def contact_forces_from_constrained_forward_dynamics(self) -> Function: + """External forces are not implemented yet for MultiBiorbdModel.""" + biorbd_return = MX() for i, model in enumerate(self.models): - q_model = q[self.variable_index("q", i)] - qdot_model = qdot[self.variable_index("qdot", i)] - tau_model = tau[self.variable_index("qddot", i)] # Due to a bug in biorbd - out = vertcat( - out, - model.contact_forces_from_constrained_forward_dynamics( + q_model = self.q[self.variable_index("q", i)] + qdot_model = self.qdot[self.variable_index("qdot", i)] + tau_model = self.tau[self.variable_index("qddot", i)] # Due to a bug in biorbd + biorbd_return = vertcat( + biorbd_return, + model.contact_forces_from_constrained_forward_dynamics()( q_model, qdot_model, tau_model, - external_forces, + [], ), ) - return out - - def qdot_from_impact(self, q, qdot_pre_impact) -> MX: - out = MX() + casadi_fun = Function( + "contact_forces_from_constrained_forward_dynamics", + [self.q, self.qdot, self.tau], + [biorbd_return], + ) + return casadi_fun + + def qdot_from_impact(self) -> Function: + biorbd_return = MX() for i, model in enumerate(self.models): - q_model = q[self.variable_index("q", i)] - qdot_pre_impact_model = qdot_pre_impact[self.variable_index("qdot", i)] - out = vertcat( - out, - model.qdot_from_impact( + q_model = self.q[self.variable_index("q", i)] + qdot_pre_impact_model = self.qdot[self.variable_index("qdot", i)] + biorbd_return = vertcat( + biorbd_return, + model.qdot_from_impact()( q_model, qdot_pre_impact_model, ), ) - return out - - def muscle_activation_dot(self, muscle_excitations) -> MX: - out = MX() + casadi_fun = Function( + "qdot_from_impact", + [self.q, self.qdot], + [biorbd_return], + ) + return casadi_fun + + def muscle_activation_dot(self) -> Function: + biorbd_return = MX() for model in self.models: muscle_states = model.model.stateSet() # still call from Biorbd for k in range(model.nb_muscles): - muscle_states[k].setExcitation(muscle_excitations[k]) - out = vertcat(out, model.model.activationDot(muscle_states).to_mx()) - return out - - def muscle_joint_torque(self, activations, q, qdot) -> MX: - out = MX() + muscle_states[k].setExcitation(self.muscle[k]) + biorbd_return = vertcat(biorbd_return, model.model.activationDot(muscle_states).to_mx()) + casadi_fun = Function( + "muscle_activation_dot", + [self.muscle], + [biorbd_return], + ) + return casadi_fun + + def muscle_joint_torque(self) -> Function: + biorbd_return = MX() for i, model in enumerate(self.models): muscles_states = model.model.stateSet() # still call from Biorbd for k in range(model.nb_muscles): - muscles_states[k].setActivation(activations[k]) - q_model = q[self.variable_index("q", i)] - qdot_model = qdot[self.variable_index("qdot", i)] - out = vertcat(out, model.model.muscularJointTorque(muscles_states, q_model, qdot_model).to_mx()) - return out - - def markers(self, q) -> Any | list[MX]: - out = [] + muscles_states[k].setActivation(self.activations[k]) + q_model = self.q[self.variable_index("q", i)] + qdot_model = self.qdot[self.variable_index("qdot", i)] + biorbd_return = vertcat(biorbd_return, model.model.muscularJointTorque(muscles_states, q_model, qdot_model).to_mx()) + casadi_fun = Function( + "muscle_joint_torque", + [self.muscle, self.q, self.qdot], + [biorbd_return], + ) + return casadi_fun + + def markers(self) -> Function: + biorbd_return = [] for i, model in enumerate(self.models): - q_model = q[self.variable_index("q", i)] - out.append(model.markers(q_model)) - return [item for sublist in out for item in sublist] + q_model = self.q[self.variable_index("q", i)] + biorbd_return += [model.markers()(q_model)] + biorbd_return = [item for sublist in biorbd_return for item in sublist] + casadi_fun = Function( + "markers", + [self.q], + [horzcat(*biorbd_return)], + ) + return casadi_fun @property def nb_markers(self) -> int: @@ -581,11 +665,16 @@ def marker_index(self, name): raise ValueError(f"{name} is not in the MultiBiorbdModel") - def marker(self, q, index, reference_segment_index=None) -> MX: + def marker(self, index, reference_segment_index=None) -> Function: local_marker_id, model_id = self.local_variable_id("markers", index) - q_model = q[self.variable_index("q", model_id)] - - return self.models[model_id].marker(q_model, local_marker_id, reference_segment_index) + q_model = self.q[self.variable_index("q", model_id)] + biorbd_return = self.models[model_id].marker(local_marker_id, reference_segment_index)(q_model) + casadi_fun = Function( + "marker", + [self.q], + [biorbd_return], + ) + return casadi_fun @property def nb_rigid_contacts(self) -> int: @@ -625,41 +714,56 @@ def rigid_contact_index(self, contact_index) -> tuple: # Note: may not work if the contact_index is not in the first model return model_selected.rigid_contact_index(contact_index) - def marker_velocities(self, q, qdot, reference_index=None) -> list[MX]: + def marker_velocities(self, reference_index=None) -> Function: if reference_index is not None: raise RuntimeError("marker_velocities is not implemented yet with reference_index for MultiBiorbdModel") - out = [] + biorbd_return = [] for i, model in enumerate(self.models): - q_model = q[self.variable_index("q", i)] - qdot_model = qdot[self.variable_index("qdot", i)] - out.extend( - model.marker_velocities(q_model, qdot_model, reference_index), - ) - return out - - def tau_max(self, q, qdot) -> tuple[MX, MX]: + q_model = self.q[self.variable_index("q", i)] + qdot_model = self.qdot[self.variable_index("qdot", i)] + biorbd_return += [model.marker_velocities(reference_index)(q_model, qdot_model)] + biorbd_return = [item for sublist in biorbd_return for item in sublist] + casadi_fun = Function( + "marker_velocities", + [self.q, self.qdot], + [horzcat(*biorbd_return)], + ) + return casadi_fun + + def tau_max(self) -> Function: out_max = MX() out_min = MX() for i, model in enumerate(self.models): - q_model = q[self.variable_index("q", i)] - qdot_model = qdot[self.variable_index("qdot", i)] - torque_max, torque_min = model.tau_max(q_model, qdot_model) + q_model = self.q[self.variable_index("q", i)] + qdot_model = self.qdot[self.variable_index("qdot", i)] + torque_max, torque_min = model.tau_max()(q_model, qdot_model) out_max = vertcat(out_max, torque_max) out_min = vertcat(out_min, torque_min) - return out_max, out_min - - def rigid_contact_acceleration(self, q, qdot, qddot, contact_index, contact_axis) -> MX: + casadi_fun = Function( + "tau_max", + [self.q, self.qdot], + [out_max, out_min], + ) + return casadi_fun + + def rigid_contact_acceleration(self, contact_index, contact_axis) -> Function: model_selected = None model_idx = -1 for i, model in enumerate(self.models): if contact_index in self.variable_index("contact", i): model_selected = model model_idx = i - q_model = q[self.variable_index("q", model_idx)] - qdot_model = qdot[self.variable_index("qdot", model_idx)] - qddot_model = qddot[self.variable_index("qddot", model_idx)] - return model_selected.rigid_contact_acceleration(q_model, qdot_model, qddot_model, contact_index, contact_axis) + q_model = self.q[self.variable_index("q", model_idx)] + qdot_model = self.qdot[self.variable_index("qdot", model_idx)] + qddot_model = self.qddot[self.variable_index("qddot", model_idx)] + biorbd_return = model_selected.rigid_contact_acceleration(contact_index, contact_axis)(q_model, qdot_model, qddot_model) + casadi_fun = Function( + "rigid_contact_acceleration", + [self.q, self.qdot, self.qddot], + [biorbd_return], + ) + return casadi_fun @property def nb_dof(self) -> int: @@ -669,59 +773,79 @@ def nb_dof(self) -> int: def marker_names(self) -> tuple[str, ...]: return tuple([name for model in self.models for name in model.marker_names]) - def soft_contact_forces(self, q, qdot) -> MX: - out = MX() + def soft_contact_forces(self, q, qdot) -> Function: + biorbd_return = MX() for i, model in enumerate(self.models): q_model = q[self.variable_index("q", i)] qdot_model = qdot[self.variable_index("qdot", i)] - soft_contact_forces = model.soft_contact_forces(q_model, qdot_model) - out = vertcat(out, soft_contact_forces) - return out - - def reshape_fext_to_fcontact(self, fext: MX) -> biorbd.VecBiorbdVector: + soft_contact_forces = model.soft_contact_forces()(q_model, qdot_model) + biorbd_return = vertcat(biorbd_return, soft_contact_forces) + casadi_fun = Function( + "soft_contact_forces", + [self.q, self.qdot], + [biorbd_return], + ) + return casadi_fun + + def reshape_fext_to_fcontact(self): raise NotImplementedError("reshape_fext_to_fcontact is not implemented yet for MultiBiorbdModel") - def normalize_state_quaternions(self, x: MX | SX) -> MX | SX: - all_q_normalized = MX() + def normalize_state_quaternions(self) -> Function: + biorbd_return = MX() for i, model in enumerate(self.models): - q_model = x[self.variable_index("q", i)] # quaternions are only in q - q_normalized = model.normalize_state_quaternions(q_model) - all_q_normalized = vertcat(all_q_normalized, q_normalized) - idx_first_qdot = self.nb_q # assuming x = [q, qdot] - x_normalized = vertcat(all_q_normalized, x[idx_first_qdot:]) - - return x_normalized - - def contact_forces(self, q, qdot, tau, external_forces: list = None) -> MX: - if external_forces is not None: - raise NotImplementedError("contact_forces is not implemented yet with external_forces for MultiBiorbdModel") - - out = MX() + q_model = self.q[self.variable_index("q", i)] + q_normalized = model.normalize_state_quaternions()(q_model) + biorbd_return = vertcat(biorbd_return, q_normalized) + casadi_fun = Function( + "normalize_state_quaternions", + [self.q], + [biorbd_return], + ) + return casadi_fun + + def contact_forces(self) -> Function: + """external_forces is not implemented yet for MultiBiorbdModel""" + biorbd_return = MX() for i, model in enumerate(self.models): - q_model = q[self.variable_index("q", i)] - qdot_model = qdot[self.variable_index("qdot", i)] - tau_model = tau[self.variable_index("tau", i)] + q_model = self.q[self.variable_index("q", i)] + qdot_model = self.qdot[self.variable_index("qdot", i)] + tau_model = self.tau[self.variable_index("tau", i)] - contact_forces = model.contact_forces(q_model, qdot_model, tau_model, external_forces) - out = vertcat(out, contact_forces) + contact_forces = model.contact_forces()(q_model, qdot_model, tau_model, []) + biorbd_return = vertcat(biorbd_return, contact_forces) + casadi_fun = Function( + "contact_forces", + [self.q, self.qdot, self.tau], + [biorbd_return], + ) + return casadi_fun - return out - def passive_joint_torque(self, q, qdot) -> MX: - out = MX() + def passive_joint_torque(self) -> Function: + biorbd_return = MX() for i, model in enumerate(self.models): - q_model = q[self.variable_index("q", i)] - qdot_model = qdot[self.variable_index("qdot", i)] - out = vertcat(out, model.passive_joint_torque(q_model, qdot_model)) - return out - - def ligament_joint_torque(self, q, qdot) -> MX: - out = MX() + q_model = self.q[self.variable_index("q", i)] + qdot_model = self.qdot[self.variable_index("qdot", i)] + biorbd_return = vertcat(biorbd_return, model.passive_joint_torque()(q_model, qdot_model)) + casadi_fun = Function( + "passive_joint_torque", + [self.q, self.qdot], + [biorbd_return], + ) + return casadi_fun + + def ligament_joint_torque(self) -> Function: + biorbd_return = MX() for i, model in enumerate(self.models): - q_model = q[self.variable_index("q", i)] - qdot_model = qdot[self.variable_index("qdot", i)] - out = vertcat(out, model.ligament_joint_torque(q_model, qdot_model)) - return out + q_model = self.q[self.variable_index("q", i)] + qdot_model = self.qdot[self.variable_index("qdot", i)] + biorbd_return = vertcat(biorbd_return, model.ligament_joint_torque()(q_model, qdot_model)) + casadi_fun = Function( + "ligament_joint_torque", + [self.q, self.qdot], + [biorbd_return], + ) + return casadi_fun def ranges_from_model(self, variable: str): return [the_range for model in self.models for the_range in model.ranges_from_model(variable)] diff --git a/bioptim/models/holonomic_constraints.py b/bioptim/models/holonomic_constraints.py index 248a97e3e..486baa928 100644 --- a/bioptim/models/holonomic_constraints.py +++ b/bioptim/models/holonomic_constraints.py @@ -52,16 +52,16 @@ def superimpose_markers( q_ddot_sym = MX.sym("q_ddot", biorbd_model.nb_qdot, 1) # symbolic markers in global frame - marker_1_sym = biorbd_model.marker(q_sym, index=biorbd_model.marker_index(marker_1)) + marker_1_sym = biorbd_model.marker(index=biorbd_model.marker_index(marker_1))(q_sym) if marker_2 is not None: - marker_2_sym = biorbd_model.marker(q_sym, index=biorbd_model.marker_index(marker_2)) + marker_2_sym = biorbd_model.marker(index=biorbd_model.marker_index(marker_2))(q_sym) else: marker_2_sym = MX([0, 0, 0]) # if local frame is provided, the markers are expressed in the same local frame if local_frame_index is not None: - jcs_t = biorbd_model.homogeneous_matrices_in_global(q_sym, local_frame_index, inverse=True) + jcs_t = biorbd_model.homogeneous_matrices_in_global(segment_idx=local_frame_index, inverse=True)(q_sym) marker_1_sym = (jcs_t @ vertcat(marker_1_sym, 1))[:3] marker_2_sym = (jcs_t @ vertcat(marker_2_sym, 1))[:3] diff --git a/tests/shard1/test__global_plots.py b/tests/shard1/test__global_plots.py index 12f9601fe..6845a8c54 100644 --- a/tests/shard1/test__global_plots.py +++ b/tests/shard1/test__global_plots.py @@ -117,7 +117,7 @@ def test_plot_merged_graphs(phase_dynamics): # Generate random data to fit np.random.seed(42) - t, markers_ref, x_ref, muscle_excitations_ref = ocp_module.generate_data(bio_model, final_time, n_shooting) + t, markers_ref, x_ref, muscle_excitations_ref = ocp_module.generate_data(bio_model, final_time, n_shooting, use_sx=False) bio_model = BiorbdModel(model_path) # To prevent from free variable, the model must be reloaded ocp = ocp_module.prepare_ocp( diff --git a/tests/shard1/test_biorbd_model_size.py b/tests/shard1/test_biorbd_model_size.py index 6ee312a99..1523ee2c2 100644 --- a/tests/shard1/test_biorbd_model_size.py +++ b/tests/shard1/test_biorbd_model_size.py @@ -10,6 +10,7 @@ def model(): return +# Pariterre: Can I remove this file all together? def generate_q_vectors(model): q_valid = MX([0.1] * model.nb_q) @@ -66,7 +67,7 @@ def test_center_of_mass_valid_and_too_large_q_input(model): q_valid, q_too_large = generate_q_vectors(model) # q valid - model.center_of_mass(q_valid) + model.center_of_mass()(q_valid) # q not valid with pytest.raises(ValueError, match="Length of q size should be: 4, but got: 5"): model.center_of_mass(q_too_large) @@ -80,13 +81,13 @@ def test_center_of_mass_velocity_valid_and_too_large_q_or_qdot_input(model): q_valid, qdot_valid, q_too_large, qdot_too_large = generate_q_and_qdot_vectors(model) # q and qdot valid - model.center_of_mass_velocity(q_valid, qdot_valid) + model.center_of_mass_velocity()(q_valid, qdot_valid) # qdot valid but q not valid with pytest.raises(ValueError, match="Length of q size should be: 4, but got: 5"): - model.center_of_mass_velocity(q_too_large, qdot_valid) + model.center_of_mass_velocity()(q_too_large, qdot_valid) # q valid but qdot not valid with pytest.raises(ValueError, match="Length of qdot size should be: 4, but got: 5"): - model.center_of_mass_velocity(q_valid, qdot_too_large) + model.center_of_mass_velocity()(q_valid, qdot_too_large) def test_center_of_mass_acceleration_valid_and_too_large_q_or_qdot_or_qddot_input( @@ -106,16 +107,16 @@ def test_center_of_mass_acceleration_valid_and_too_large_q_or_qdot_or_qddot_inpu ) = generate_q_qdot_qddot_vectors(model) # q, qdot and qddot valid - model.center_of_mass_acceleration(q_valid, qdot_valid, qddot_valid) + model.center_of_mass_acceleration()(q_valid, qdot_valid, qddot_valid) # qdot and qddot valid but q not valid with pytest.raises(ValueError, match="Length of q size should be: 4, but got: 5"): - model.center_of_mass_acceleration(q_too_large, qdot_valid, qddot_valid) + model.center_of_mass_acceleration()(q_too_large, qdot_valid, qddot_valid) # q and qddot valid but qdot not valid with pytest.raises(ValueError, match="Length of qdot size should be: 4, but got: 5"): - model.center_of_mass_acceleration(q_valid, qdot_too_large, qddot_valid) + model.center_of_mass_acceleration()(q_valid, qdot_too_large, qddot_valid) # q and qdot valid but qddot not valid with pytest.raises(ValueError, match="Length of qddot size should be: 4, but got: 5"): - model.center_of_mass_acceleration(q_valid, qdot_valid, qddot_too_large) + model.center_of_mass_acceleration()(q_valid, qdot_valid, qddot_too_large) def test_body_rotation_rate_valid_and_too_large_q_or_qdot_input(model): @@ -126,13 +127,13 @@ def test_body_rotation_rate_valid_and_too_large_q_or_qdot_input(model): q_valid, qdot_valid, q_too_large, qdot_too_large = generate_q_and_qdot_vectors(model) # q and qdot valid - model.body_rotation_rate(q_valid, qdot_valid) + model.body_rotation_rate()(q_valid, qdot_valid) # qdot valid but q not valid with pytest.raises(ValueError, match="Length of q size should be: 4, but got: 5"): - model.body_rotation_rate(q_too_large, qdot_valid) + model.body_rotation_rate()(q_too_large, qdot_valid) # q valid but qdot not valid with pytest.raises(ValueError, match="Length of qdot size should be: 4, but got: 5"): - model.body_rotation_rate(q_valid, qdot_too_large) + model.body_rotation_rate()(q_valid, qdot_too_large) def test_mass_matrix_valid_and_too_large_q_input(model): @@ -143,10 +144,10 @@ def test_mass_matrix_valid_and_too_large_q_input(model): q_valid, q_too_large = generate_q_vectors(model) # q valid - model.mass_matrix(q_valid) + model.mass_matrix()(q_valid) # q not valid with pytest.raises(ValueError, match="Length of q size should be: 4, but got: 5"): - model.mass_matrix(q_too_large) + model.mass_matrix()(q_too_large) def test_non_linear_effects_valid_and_too_large_q_or_qdot_input(model): @@ -157,13 +158,13 @@ def test_non_linear_effects_valid_and_too_large_q_or_qdot_input(model): q_valid, qdot_valid, q_too_large, qdot_too_large = generate_q_and_qdot_vectors(model) # q and qdot valid - model.non_linear_effects(q_valid, qdot_valid) + model.non_linear_effects()(q_valid, qdot_valid) # qdot valid but q not valid with pytest.raises(ValueError, match="Length of q size should be: 4, but got: 5"): - model.non_linear_effects(q_too_large, qdot_valid) + model.non_linear_effects()(q_too_large, qdot_valid) # q valid but qdot not valid with pytest.raises(ValueError, match="Length of qdot size should be: 4, but got: 5"): - model.non_linear_effects(q_valid, qdot_too_large) + model.non_linear_effects()(q_valid, qdot_too_large) def test_angular_momentum_valid_and_too_large_q_or_qdot_input(model): @@ -174,13 +175,13 @@ def test_angular_momentum_valid_and_too_large_q_or_qdot_input(model): q_valid, qdot_valid, q_too_large, qdot_too_large = generate_q_and_qdot_vectors(model) # q and qdot valid - model.angular_momentum(q_valid, qdot_valid) + model.angular_momentum()(q_valid, qdot_valid) # qdot valid but q not valid with pytest.raises(ValueError, match="Length of q size should be: 4, but got: 5"): - model.angular_momentum(q_too_large, qdot_valid) + model.angular_momentum()(q_too_large, qdot_valid) # q valid but qdot not valid with pytest.raises(ValueError, match="Length of qdot size should be: 4, but got: 5"): - model.angular_momentum(q_valid, qdot_too_large) + model.angular_momentum()(q_valid, qdot_too_large) def test_reshape_qdot_valid_and_too_large_q_or_qdot_input(model): @@ -191,13 +192,13 @@ def test_reshape_qdot_valid_and_too_large_q_or_qdot_input(model): q_valid, qdot_valid, q_too_large, qdot_too_large = generate_q_and_qdot_vectors(model) # q and qdot valid - model.reshape_qdot(q_valid, qdot_valid) + model.reshape_qdot()(q_valid, qdot_valid) # qdot valid but q not valid with pytest.raises(ValueError, match="Length of q size should be: 4, but got: 5"): - model.reshape_qdot(q_too_large, qdot_valid) + model.reshape_qdot()(q_too_large, qdot_valid) # q valid but qdot not valid with pytest.raises(ValueError, match="Length of qdot size should be: 4, but got: 5"): - model.reshape_qdot(q_valid, qdot_too_large) + model.reshape_qdot()(q_valid, qdot_too_large) def test_segment_angular_velocity_valid_and_too_large_q_or_qdot_input(model): @@ -208,13 +209,13 @@ def test_segment_angular_velocity_valid_and_too_large_q_or_qdot_input(model): q_valid, qdot_valid, q_too_large, qdot_too_large = generate_q_and_qdot_vectors(model) idx = 1 # q and qdot valid - model.segment_angular_velocity(q_valid, qdot_valid, idx) + model.segment_angular_velocity(idx)(q_valid, qdot_valid) # qdot valid but q not valid with pytest.raises(ValueError, match="Length of q size should be: 4, but got: 5"): - model.segment_angular_velocity(q_too_large, qdot_valid, idx) + model.segment_angular_velocity(idx)(q_too_large, qdot_valid) # q valid but qdot not valid with pytest.raises(ValueError, match="Length of qdot size should be: 4, but got: 5"): - model.segment_angular_velocity(q_valid, qdot_too_large, idx) + model.segment_angular_velocity(idx)(q_valid, qdot_too_large) def test_forward_dynamics_free_floating_base_valid_and_too_large_q_or_qdot_or_qddot_joints_input( @@ -234,16 +235,16 @@ def test_forward_dynamics_free_floating_base_valid_and_too_large_q_or_qdot_or_qd ) = generate_q_qdot_qddot_vectors(model, root_dynamics=True) # q, qdot and qddot_joints valid - model.forward_dynamics_free_floating_base(q_valid, qdot_valid, qddot_joints_valid) + model.forward_dynamics_free_floating_base()(q_valid, qdot_valid, qddot_joints_valid) # qdot and qddot_joints valid but q not valid with pytest.raises(ValueError, match="Length of q size should be: 4, but got: 5"): - model.forward_dynamics_free_floating_base(q_too_large, qdot_valid, qddot_joints_valid) + model.forward_dynamics_free_floating_base()(q_too_large, qdot_valid, qddot_joints_valid) # q and qddot_joints valid but qdot not valid with pytest.raises(ValueError, match="Length of qdot size should be: 4, but got: 5"): - model.forward_dynamics_free_floating_base(q_valid, qdot_too_large, qddot_joints_valid) + model.forward_dynamics_free_floating_base()(q_valid, qdot_too_large, qddot_joints_valid) # q and qdot valid but qddot_joints not valid with pytest.raises(ValueError, match="Length of qddot_joints size should be: 1, but got: 5"): - model.forward_dynamics_free_floating_base(q_valid, qdot_valid, qddot_joints_too_large) + model.forward_dynamics_free_floating_base()(q_valid, qdot_valid, qddot_joints_too_large) def test_forward_dynamics_valid_and_too_large_q_or_qdot_or_tau_activations_input(model): @@ -255,16 +256,16 @@ def test_forward_dynamics_valid_and_too_large_q_or_qdot_or_tau_activations_input tau_valid, tau_too_large = generate_tau_activations_vectors(model) # q, qdot and tau valid - model.forward_dynamics(q_valid, qdot_valid, tau_valid) + model.forward_dynamics()(q_valid, qdot_valid, tau_valid, []) # qdot and tau valid but q not valid with pytest.raises(ValueError, match="Length of q size should be: 4, but got: 5"): - model.forward_dynamics(q_too_large, qdot_valid, tau_valid) + model.forward_dynamics()(q_too_large, qdot_valid, tau_valid, []) # q and tau valid but qdot not valid with pytest.raises(ValueError, match="Length of qdot size should be: 4, but got: 5"): - model.forward_dynamics(q_valid, qdot_too_large, tau_valid) + model.forward_dynamics()(q_valid, qdot_too_large, tau_valid, []) # q and qdot valid but tau not valid with pytest.raises(ValueError, match="Length of tau size should be: 4, but got: 5"): - model.forward_dynamics(q_valid, qdot_valid, tau_too_large) + model.forward_dynamics()(q_valid, qdot_valid, tau_too_large, []) def test_constrained_forward_dynamics_valid_and_too_large_q_or_qdot_or_tau_input(model): diff --git a/tests/shard1/test_biorbd_multi_model.py b/tests/shard1/test_biorbd_multi_model.py index 815d69fee..58bad292f 100644 --- a/tests/shard1/test_biorbd_multi_model.py +++ b/tests/shard1/test_biorbd_multi_model.py @@ -95,7 +95,7 @@ def test_biorbd_model(): models.serialize() models.set_gravity(np.array([0, 0, -3])) - TestUtils.assert_equal(models.gravity, np.array([0, 0, -3, 0, 0, -3])) + TestUtils.assert_equal(models.gravity()['o0'], np.array([0, 0, -3, 0, 0, -3]).reshape(6, 1)) models.set_gravity(np.array([0, 0, -9.81])) with pytest.raises(NotImplementedError, match="segment_index is not implemented for MultiBiorbdModel"): @@ -106,8 +106,7 @@ def test_biorbd_model(): assert isinstance(models.segments[0], biorbd.biorbd.Segment) TestUtils.assert_equal( - # one of the last ouput of BiorbdModel which is not a MX but a biorbd object - models.biorbd_homogeneous_matrices_in_global(np.array([1, 2, 3]), 0, 0), + models.homogeneous_matrices_in_global(0, 0)(np.array([1, 2, 3])), np.array( [ [1.0, 0.0, 0.0, 0.0], @@ -118,29 +117,29 @@ def test_biorbd_model(): ), ) TestUtils.assert_equal( - models.homogeneous_matrices_in_child(4), + models.homogeneous_matrices_in_child(4)()["o0"], np.array([[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 1.0, -1.0], [0.0, 0.0, 0.0, 1.0]]), ) - TestUtils.assert_equal(models.mass, np.array([3, 3])) + TestUtils.assert_equal(models.mass()["o0"], np.array([3, 3]).reshape(2, 1)) TestUtils.assert_equal( - models.center_of_mass(np.array([1, 2, 3, 4, 5, 6])), - np.array([-5.000000e-04, 8.433844e-01, -1.764446e-01, -5.000000e-04, -3.232674e-01, 1.485815e00]), + models.center_of_mass()(np.array([1, 2, 3, 4, 5, 6])), + np.array([-5.000000e-04, 8.433844e-01, -1.764446e-01, -5.000000e-04, -3.232674e-01, 1.485815e00]).reshape(6, 1), ) TestUtils.assert_equal( - models.center_of_mass_velocity(np.array([1, 2.1, 3, 4.1, 5, 6.1]), np.array([1, 2.1, 3, 4.1, 5, 6])), - np.array([0.0, 0.425434, 0.638069, 0.0, -12.293126, 0.369492]), + models.center_of_mass_velocity()(np.array([1, 2.1, 3, 4.1, 5, 6.1]), np.array([1, 2.1, 3, 4.1, 5, 6])), + np.array([0.0, 0.425434, 0.638069, 0.0, -12.293126, 0.369492]).reshape(6, 1), ) TestUtils.assert_equal( - models.center_of_mass_acceleration( + models.center_of_mass_acceleration()( np.array([1, 2.1, 3, 4.1, 5, 6.1]), np.array([1, 2.1, 3, 4.1, 5, 6]), np.array([0.1, 0.2, 0.3, 0.4, 0.5, 0.6]), ), - np.array([0.0, 0.481652, 6.105858, 0.0, -33.566971, -126.795179]), + np.array([0.0, 0.481652, 6.105858, 0.0, -33.566971, -126.795179]).reshape(6, 1), ) - mass_matrices = models.mass_matrix(np.array([1, 2.1, 3, 4.1, 5, 6.1])) + mass_matrices = models.mass_matrix()(np.array([1, 2.1, 3, 4.1, 5, 6.1])) assert len(mass_matrices) == 2 TestUtils.assert_equal( mass_matrices[0], @@ -157,58 +156,58 @@ def test_biorbd_model(): np.array([[9.313616, 5.580191, 2.063886], [5.580191, 4.791997, 1.895999], [2.063886, 1.895999, 0.945231]]), ) - nonlinear_effects = models.non_linear_effects(np.array([1, 2.1, 3, 4.1, 5, 6.1]), np.array([1, 2.1, 3, 4.1, 5, 6])) + nonlinear_effects = models.non_linear_effects()(np.array([1, 2.1, 3, 4.1, 5, 6.1]), np.array([1, 2.1, 3, 4.1, 5, 6])) assert len(nonlinear_effects) == 2 TestUtils.assert_equal( nonlinear_effects[0], - np.array([38.817401, -1.960653, -1.259441]), + np.array([38.817401, -1.960653, -1.259441]).reshape(3, 1) ) TestUtils.assert_equal( nonlinear_effects[1], - np.array([322.060726, -22.147881, -20.660836]), + np.array([322.060726, -22.147881, -20.660836]).reshape(3, 1) ) TestUtils.assert_equal( - models.angular_momentum(np.array([1, 2.1, 3, 4.1, 5, 6.1]), np.array([1, 2.1, 3, 4.1, 5, 6])), - np.array([3.001448e00, 0.000000e00, -2.168404e-19, 2.514126e01, 3.252607e-19, 0.000000e00]), + models.angular_momentum()(np.array([1, 2.1, 3, 4.1, 5, 6.1]), np.array([1, 2.1, 3, 4.1, 5, 6])), + np.array([3.001448e00, 0.000000e00, -2.168404e-19, 2.514126e01, 3.252607e-19, 0.000000e00]).reshape(6, 1), decimal=5, ) TestUtils.assert_equal( - models.reshape_qdot(np.array([1, 2.1, 3, 4.1, 5, 6.1]), np.array([1, 2.1, 3, 4.1, 5, 6]), 1), - np.array([1.0, 2.1, 3.0, 4.1, 5.0, 6.0]), + models.reshape_qdot(1)(np.array([1, 2.1, 3, 4.1, 5, 6.1]), np.array([1, 2.1, 3, 4.1, 5, 6])), + np.array([1.0, 2.1, 3.0, 4.1, 5.0, 6.0]).reshape(6, 1), ) TestUtils.assert_equal( - models.segment_angular_velocity(np.array([1, 2.1, 3, 4.1, 5, 6.1]), np.array([1, 2.1, 3, 4.1, 5, 6]), 1), - np.array([3.1, 0.0, 0.0, 9.1, 0.0, 0.0]), + models.segment_angular_velocity(1)(np.array([1, 2.1, 3, 4.1, 5, 6.1]), np.array([1, 2.1, 3, 4.1, 5, 6])), + np.array([3.1, 0.0, 0.0, 9.1, 0.0, 0.0]).reshape(6, 1), ) assert models.soft_contact(0, 0) == [] # TODO: Fix soft contact (biorbd call error) - with pytest.raises(RuntimeError, match="Close the actuator model before calling torqueMax"): - models.torque( - np.array([3.1, 0.0, 0.0, 9.1, 0.0, 0.0]), - np.array([3.1, 0.0, 0.0, 9.1, 0.0, 0.0]), - np.array([3.1, 0.0, 0.0, 9.1, 0.0, 0.0]), - ) # TODO: Fix torque (Close the actuator model before calling torqueMax) + # with pytest.raises(RuntimeError, match="Close the actuator model before calling torqueMax"): + # models.torque()( + # np.array([3.1, 0.0, 0.0, 9.1, 0.0, 0.0]), + # np.array([3.1, 0.0, 0.0, 9.1, 0.0, 0.0]), + # np.array([3.1, 0.0, 0.0, 9.1, 0.0, 0.0]), + # ) # TODO: Fix torque (Close the actuator model before calling torqueMax) TestUtils.assert_equal( - models.forward_dynamics_free_floating_base( + models.forward_dynamics_free_floating_base()( np.array([1, 2.1, 3, 4.1, 5, 6.1]), np.array([1, 2.1, 3, 4.1, 5, 6]), - np.array([3.1, 0.0, 0.0, 9.1, 0.0, 0.0]), + np.array([3.1, 0.0, 9.1, 0.0]), ), - np.array([-14.750679, -36.596107]), + np.array([-14.750679, -40.031762]).reshape(2, 1) ) TestUtils.assert_equal( - models.forward_dynamics( + models.forward_dynamics(with_contact=False)( np.array([1, 2.1, 3, 4.1, 5, 6.1]), np.array([1, 2.1, 3, 4.1, 5, 6]), np.array([3.1, 1, 2, 9.1, 1, 2]), ), - np.array([-16.092093, 9.049853, 10.570878, -121.783105, 154.820759, -20.664452]), + np.array([-16.092093, 9.049853, 10.570878, -121.783105, 154.820759, -20.664452]).reshape(6, 1) ) TestUtils.assert_equal( @@ -217,10 +216,11 @@ def test_biorbd_model(): np.array([1, 2.1, 3, 4.1, 5, 6]), np.array([3.1, 1, 2, 9.1, 1, 2]), ), - np.array([-16.092093, 9.049853, 10.570878, -121.783105, 154.820759, -20.664452]), + np.array([-16.092093, 9.049853, 10.570878, -121.783105, 154.820759, -20.664452]).reshape(6, 1) ) - with pytest.raises(NotImplementedError, match="External forces are not implemented yet for MultiBiorbdModel."): + with pytest.raises(RuntimeError, match="Incorrect number of inputs: Expected 3, got 4"): + # Because external_forces are not implemented models.forward_dynamics(with_contact=True)( np.array([1, 2.1, 3, 4.1, 5, 6.1]), np.array([1, 2.1, 3, 4.1, 5, 6]), @@ -229,27 +229,27 @@ def test_biorbd_model(): ) TestUtils.assert_equal( - models.inverse_dynamics( + models.inverse_dynamics()( np.array([1, 2.1, 3, 4.1, 5, 6.1]), np.array([1, 2.1, 3, 4.1, 5, 6]), np.array([3.1, 1, 2, 9.1, 1, 2]), ), - np.array([4.844876e01, 2.121037e-01, 1.964626e00, 4.165226e02, 3.721585e01, 1.906986e00]), + np.array([4.844876e01, 2.121037e-01, 1.964626e00, 4.165226e02, 3.721585e01, 1.906986e00]).reshape(6, 1), decimal=5, ) TestUtils.assert_equal( - models.contact_forces_from_constrained_forward_dynamics( + models.contact_forces_from_constrained_forward_dynamics()( np.array([1, 2.1, 3, 4.1, 5, 6.1]), np.array([1, 2.1, 3, 4.1, 5, 6]), np.array([3.1, 1, 2, 9.1, 1, 2]), - None, ), - np.array([0.0, 0.0]), + np.array([0.0, 0.0]).reshape(2, 1) ) - with pytest.raises(NotImplementedError, match="External forces are not implemented yet for MultiBiorbdModel."): - models.contact_forces_from_constrained_forward_dynamics( + with pytest.raises(RuntimeError, match="Incorrect number of inputs: Expected 3, got 4"): + # Because external_forces are not implemented + models.contact_forces_from_constrained_forward_dynamics()( np.array([1, 2.1, 3, 4.1, 5, 6.1]), np.array([1, 2.1, 3, 4.1, 5, 6]), np.array([3.1, 1, 2, 9.1, 1, 2]), @@ -257,63 +257,61 @@ def test_biorbd_model(): ) TestUtils.assert_equal( - models.qdot_from_impact( + models.qdot_from_impact()( np.array([1, 2.1, 3, 4.1, 5, 6.1]), np.array([1, 2.1, 3, 4.1, 5, 6]), ), - np.array([1.0, 2.1, 3.0, 4.1, 5.0, 6.0]), + np.array([1.0, 2.1, 3.0, 4.1, 5.0, 6.0]).reshape(6, 1) ) TestUtils.assert_equal( - models.muscle_activation_dot( - np.array([1, 2.1, 3, 4.1, 5, 6.1]), + models.muscle_activation_dot()( + [], # There is no muscle in the models ), - np.array([], dtype=np.float64), + np.array([], dtype=np.float64).reshape(0, 1), ) TestUtils.assert_equal( - models.muscle_joint_torque( - np.array([1, 2.1, 3, 4.1, 5, 6.1]), + models.muscle_joint_torque()( + np.array([]), # There is no muscle in the models np.array([1, 2.1, 3, 4.1, 5, 6.1]), np.array([1, 2.1, 3, 4.1, 5, 6]), ), - np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]), + np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]).reshape(6, 1), ) TestUtils.assert_equal( - models.markers( + models.markers()( np.array([1, 2.1, 3, 4.1, 5, 6.1]), - )[0], - np.array([0.0, 0.0, 0.0]), + )[:, 0], + np.array([0.0, 0.0, 0.0]).reshape(3, 1), ) TestUtils.assert_equal( - models.marker( + models.marker(index=1)( np.array([1, 2.1, 3, 4.1, 5, 6.1]), - index=1, ), - np.array([0.0, 0.841471, -0.540302]), + np.array([0.0, 0.841471, -0.540302]).reshape(3, 1) ) assert models.marker_index("marker_3") == 2 - markers_velocities = models.marker_velocities( + markers_velocities = models.marker_velocities()( np.array([1, 2.1, 3, 4.1, 5, 6.1]), np.array([1, 2.1, 3, 4.1, 5, 6]), ) - assert isinstance(markers_velocities, list) TestUtils.assert_equal( - markers_velocities[0], - np.array([0.0, 0.0, 0.0]), + markers_velocities[:, 0], + np.array([0.0, 0.0, 0.0]).reshape(3, 1), ) TestUtils.assert_equal( - markers_velocities[1], - np.array([0.0, 0.540302, 0.841471]), + markers_velocities[:, 1], + np.array([0.0, 0.540302, 0.841471]).reshape(3, 1), ) with pytest.raises(RuntimeError, match="All dof must have their actuators set"): - models.tau_max( + models.tau_max()( np.array([1, 2.1, 3, 4.1, 5, 6.1]), np.array([1, 2.1, 3, 4.1, 5, 6]), ) # TODO: add an actuator model (AnaisFarr will do it when her PR will be merged) @@ -332,32 +330,32 @@ def test_biorbd_model(): with pytest.raises( NotImplementedError, match="reshape_fext_to_fcontact is not implemented yet for MultiBiorbdModel" ): - models.reshape_fext_to_fcontact(np.array([1, 2.1, 3, 4.1, 5, 6.1])) + models.reshape_fext_to_fcontact()(np.array([1, 2.1, 3, 4.1, 5, 6.1])) # this test doesn't properly test the function, but it's the best we can do for now # we should add a quaternion to the model to test it # anyway it's tested elsewhere. TestUtils.assert_equal( - models.normalize_state_quaternions( - np.array([1, 2.1, 3, 4.1, 5, 6.1, 1, 2.1, 3, 4.1, 5, 6.6]), + models.normalize_state_quaternions()( + np.array([1, 2.1, 3, 4.1, 5, 6.1]), ), - np.array([1.0, 2.1, 3.0, 4.1, 5.0, 6.1, 1.0, 2.1, 3.0, 4.1, 5.0, 6.6]), + np.array([1.0, 2.1, 3.0, 4.1, 5.0, 6.1]).reshape(6, 1), ) TestUtils.assert_equal( - models.contact_forces( + models.contact_forces()( np.array([1, 2.1, 3, 4.1, 5, 6.1]), np.array([1, 2.1, 3, 4.1, 5, 6]), np.array([3.1, 1, 2, 9.1, 1, 2]), - None, ), - np.array([0.0, 0.0]), + np.array([0.0, 0.0]).reshape(2, 1) ) + # Because external_forces are not implemented yet with pytest.raises( - NotImplementedError, match="contact_forces is not implemented yet with external_forces for MultiBiorbdModel" + RuntimeError, match="Incorrect number of inputs: Expected 3, got 4" ): - models.contact_forces( + models.contact_forces()( np.array([1, 2.1, 3, 4.1, 5, 6.1]), np.array([1, 2.1, 3, 4.1, 5, 6]), np.array([3.1, 1, 2, 9.1, 1, 2]), @@ -365,11 +363,11 @@ def test_biorbd_model(): ) TestUtils.assert_equal( - models.passive_joint_torque( + models.passive_joint_torque()( np.array([1, 2.1, 3, 4.1, 5, 6.1]), np.array([1, 2.1, 3, 4.1, 5, 6]), ), - np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]), + np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]).reshape(6, 1), ) q_mapping = models._var_mapping("q", None, variable_mappings) From f36058c61df13efdb7f4461912242cdaf9f02b1a Mon Sep 17 00:00:00 2001 From: eve-mac Date: Fri, 20 Sep 2024 11:55:47 -0400 Subject: [PATCH 015/108] fixed parameters --- bioptim/dynamics/configure_problem.py | 4 +- bioptim/dynamics/dynamics_functions.py | 73 ++++++---------- .../getting_started/custom_objectives.py | 2 - .../getting_started/custom_parameters.py | 56 ++++++------ .../examples/moving_horizon_estimation/mhe.py | 11 +-- bioptim/examples/track/track_segment_on_rt.py | 2 +- bioptim/limits/constraints.py | 20 ++--- bioptim/limits/penalty.py | 47 +++++----- bioptim/models/biorbd/biorbd_model.py | 85 ++++++++++--------- bioptim/models/biorbd/multi_biorbd_model.py | 3 + bioptim/optimization/parameters.py | 9 ++ 11 files changed, 149 insertions(+), 163 deletions(-) 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): """ From b1edf727630557170d1efa1bc09d0caea3e324be Mon Sep 17 00:00:00 2001 From: eve-mac Date: Fri, 20 Sep 2024 12:02:54 -0400 Subject: [PATCH 016/108] youpsi --- bioptim/models/biorbd/biorbd_model.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/bioptim/models/biorbd/biorbd_model.py b/bioptim/models/biorbd/biorbd_model.py index 6f95347a5..35ce9b845 100644 --- a/bioptim/models/biorbd/biorbd_model.py +++ b/bioptim/models/biorbd/biorbd_model.py @@ -34,8 +34,9 @@ def __init__( 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) + if parameters is not None: + 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 From 04ac46cecc9d70ac707eb9cb84db56183e62ba78 Mon Sep 17 00:00:00 2001 From: eve-mac Date: Fri, 20 Sep 2024 12:03:23 -0400 Subject: [PATCH 017/108] blacked --- bioptim/dynamics/dynamics_functions.py | 12 +++- bioptim/limits/constraints.py | 26 +++++++-- bioptim/limits/penalty.py | 62 +++++++++++++++------ bioptim/models/biorbd/multi_biorbd_model.py | 40 ++++++------- tests/shard1/test__global_plots.py | 4 +- 5 files changed, 96 insertions(+), 48 deletions(-) diff --git a/bioptim/dynamics/dynamics_functions.py b/bioptim/dynamics/dynamics_functions.py index 6851be4c8..19e03cde6 100644 --- a/bioptim/dynamics/dynamics_functions.py +++ b/bioptim/dynamics/dynamics_functions.py @@ -260,7 +260,9 @@ 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, nlp.parameters.cx) 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 @@ -1116,7 +1118,9 @@ def forward_dynamics( 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, nlp.parameters.cx) + qddot = nlp.model.forward_dynamics(with_contact=with_contact)( + q, qdot, tau, external_forces, nlp.parameters.cx + ) return qdot_var_mapping.map(qddot) @staticmethod @@ -1162,7 +1166,9 @@ 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], nlp.parameters.cx) + 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 diff --git a/bioptim/limits/constraints.py b/bioptim/limits/constraints.py index 7aa3b7d4b..68e41d90e 100644 --- a/bioptim/limits/constraints.py +++ b/bioptim/limits/constraints.py @@ -411,11 +411,17 @@ def qddot_equals_forward_dynamics( 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, controller.parameters.cx) 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, [], controller.parameters.cx) + qddot_fd = controller.model.forward_dynamics(with_contact=with_contact)( + q, qdot, tau, [], controller.parameters.cx + ) return qddot - qddot_fd @staticmethod @@ -453,7 +459,11 @@ def tau_equals_inverse_dynamics( qddot = controller.states["qddot"].cx if "qddot" in controller.states else controller.controls["qddot"].cx 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, controller.parameters.cx) 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 +474,9 @@ 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, controller.parameters.cx) + 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]) @@ -534,7 +546,11 @@ def tau_from_muscle_equal_inverse_dynamics( muscles_states[k].setActivation(muscle_activations[k]) 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, controller.parameters.cx) 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: diff --git a/bioptim/limits/penalty.py b/bioptim/limits/penalty.py index fec15bcac..56fa5c6ac 100644 --- a/bioptim/limits/penalty.py +++ b/bioptim/limits/penalty.py @@ -124,7 +124,9 @@ 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, controller.parameters.cx) + muscles_dot = controller.model.muscle_velocity()( + controller.q.cx, controller.qdot.cx, controller.parameters.cx + ) return controls * muscles_dot @@ -295,7 +297,9 @@ def minimize_markers( jcs_t = ( CX_eye(4) if reference_jcs is None - else controller.model.homogeneous_matrices_in_global(reference_jcs, inverse=True)(q.cx, controller.parameters.cx) + else controller.model.homogeneous_matrices_in_global(reference_jcs, inverse=True)( + q.cx, controller.parameters.cx + ) ) markers = [] @@ -426,9 +430,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.parameters.cx) - controller.model.marker( - first_marker_idx - )(controller.q.cx, controller.parameters.cx) + diff_markers = controller.model.marker(second_marker_idx)( + controller.q.cx, controller.parameters.cx + ) - controller.model.marker(first_marker_idx)(controller.q.cx, controller.parameters.cx) return diff_markers @@ -470,7 +474,9 @@ 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, controller.parameters.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,7 +606,9 @@ def minimize_predicted_com_height(_: PenaltyOption, controller: PenaltyControlle g = controller.model.gravity()["o0"][2] 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_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 @@ -648,7 +656,9 @@ 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, controller.parameters.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): @@ -673,7 +683,9 @@ 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, controller.parameters.cx) + marker = controller.model.center_of_mass_acceleration()( + controller.q.cx, controller.qdot.cx, qddot, controller.parameters.cx + ) return marker @@ -718,7 +730,9 @@ 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, controller.parameters.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 @@ -892,7 +906,9 @@ 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, controller.parameters.cx)[:3, :3].T + 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! @@ -985,7 +1001,9 @@ 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, controller.parameters.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: @@ -1030,7 +1048,9 @@ 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, controller.parameters.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] @@ -1097,10 +1117,18 @@ 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, 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_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/multi_biorbd_model.py b/bioptim/models/biorbd/multi_biorbd_model.py index fed481f97..fa124413e 100644 --- a/bioptim/models/biorbd/multi_biorbd_model.py +++ b/bioptim/models/biorbd/multi_biorbd_model.py @@ -230,7 +230,7 @@ def nb_extra_models(self) -> int: @property def gravity(self) -> Function: - biorbd_return = vertcat(*(model.gravity()['o0'] for model in self.models)) + biorbd_return = vertcat(*(model.gravity()["o0"] for model in self.models)) casadi_fun = Function( "gravity", [MX()], @@ -301,7 +301,7 @@ def homogeneous_matrices_in_child(self, segment_id) -> Function: @property def mass(self) -> Function: - biorbd_return = vertcat(*(model.mass()['o0'] for model in self.models)) + biorbd_return = vertcat(*(model.mass()["o0"] for model in self.models)) casadi_fun = Function( "mass", [MX()], @@ -327,9 +327,9 @@ def center_of_mass_velocity(self) -> Function: q_model = self.q[self.variable_index("q", i)] qdot_model = self.qdot[self.variable_index("qdot", i)] biorbd_return = vertcat( - biorbd_return, - model.center_of_mass_velocity()(q_model, qdot_model), - ) + biorbd_return, + model.center_of_mass_velocity()(q_model, qdot_model), + ) casadi_fun = Function( "center_of_mass_velocity", [self.q, self.qdot], @@ -344,9 +344,9 @@ def center_of_mass_acceleration(self) -> Function: qdot_model = self.qdot[self.variable_index("qdot", i)] qddot_model = self.qddot[self.variable_index("qddot", i)] biorbd_return = vertcat( - biorbd_return, - model.center_of_mass_acceleration()(q_model, qdot_model, qddot_model), - ) + biorbd_return, + model.center_of_mass_acceleration()(q_model, qdot_model, qddot_model), + ) casadi_fun = Function( "center_of_mass_acceleration", [self.q, self.qdot, self.qddot], @@ -385,9 +385,9 @@ def angular_momentum(self) -> Function: q_model = self.q[self.variable_index("q", i)] qdot_model = self.qdot[self.variable_index("qdot", i)] biorbd_return = vertcat( - biorbd_return, - model.angular_momentum()(q_model, qdot_model), - ) + biorbd_return, + model.angular_momentum()(q_model, qdot_model), + ) casadi_fun = Function( "angular_momentum", [self.q, self.qdot], @@ -483,7 +483,6 @@ def torque(self) -> Function: ) return casadi_fun - def forward_dynamics_free_floating_base(self) -> Function: biorbd_return = MX() for i, model in enumerate(self.models): @@ -522,7 +521,6 @@ def reorder_qddot_root_joints(self): ) return casadi_fun - def forward_dynamics(self, with_contact) -> Function: """External forces and contact forces are not implemented yet for MultiBiorbdModel.""" @@ -557,12 +555,7 @@ def inverse_dynamics(self) -> Function: qddot_model = self.qddot[self.variable_index("qddot", i)] biorbd_return = vertcat( biorbd_return, - model.inverse_dynamics()( - q_model, - qdot_model, - qddot_model, - [] - ), + model.inverse_dynamics()(q_model, qdot_model, qddot_model, []), ) casadi_fun = Function( "inverse_dynamics", @@ -635,7 +628,9 @@ def muscle_joint_torque(self) -> Function: muscles_states[k].setActivation(self.activations[k]) q_model = self.q[self.variable_index("q", i)] qdot_model = self.qdot[self.variable_index("qdot", i)] - biorbd_return = vertcat(biorbd_return, model.model.muscularJointTorque(muscles_states, q_model, qdot_model).to_mx()) + biorbd_return = vertcat( + biorbd_return, model.model.muscularJointTorque(muscles_states, q_model, qdot_model).to_mx() + ) casadi_fun = Function( "muscle_joint_torque", [self.muscle, self.q, self.qdot], @@ -760,7 +755,9 @@ def rigid_contact_acceleration(self, contact_index, contact_axis) -> Function: q_model = self.q[self.variable_index("q", model_idx)] qdot_model = self.qdot[self.variable_index("qdot", model_idx)] qddot_model = self.qddot[self.variable_index("qddot", model_idx)] - biorbd_return = model_selected.rigid_contact_acceleration(contact_index, contact_axis)(q_model, qdot_model, qddot_model) + biorbd_return = model_selected.rigid_contact_acceleration(contact_index, contact_axis)( + q_model, qdot_model, qddot_model + ) casadi_fun = Function( "rigid_contact_acceleration", [self.q, self.qdot, self.qddot], @@ -823,7 +820,6 @@ def contact_forces(self) -> Function: ) return casadi_fun - def passive_joint_torque(self) -> Function: biorbd_return = MX() for i, model in enumerate(self.models): diff --git a/tests/shard1/test__global_plots.py b/tests/shard1/test__global_plots.py index 6845a8c54..970fd09e4 100644 --- a/tests/shard1/test__global_plots.py +++ b/tests/shard1/test__global_plots.py @@ -117,7 +117,9 @@ def test_plot_merged_graphs(phase_dynamics): # Generate random data to fit np.random.seed(42) - t, markers_ref, x_ref, muscle_excitations_ref = ocp_module.generate_data(bio_model, final_time, n_shooting, use_sx=False) + t, markers_ref, x_ref, muscle_excitations_ref = ocp_module.generate_data( + bio_model, final_time, n_shooting, use_sx=False + ) bio_model = BiorbdModel(model_path) # To prevent from free variable, the model must be reloaded ocp = ocp_module.prepare_ocp( From c022af556aa3ef3a80ab517461856b6b03cb849c Mon Sep 17 00:00:00 2001 From: eve-mac Date: Sat, 21 Sep 2024 09:12:10 -0400 Subject: [PATCH 018/108] rocking those parameters! --- bioptim/dynamics/configure_problem.py | 4 +- bioptim/dynamics/dynamics_functions.py | 8 +-- .../getting_started/custom_constraint.py | 2 +- .../getting_started/custom_dynamics.py | 2 +- .../getting_started/custom_objectives.py | 2 +- .../example_continuity_as_objective.py | 2 +- .../examples/moving_horizon_estimation/mhe.py | 4 +- .../muscle_activations_tracker.py | 8 +-- .../muscle_excitations_tracker.py | 2 +- ...arm_reaching_torque_driven_collocations.py | 5 +- bioptim/limits/constraints.py | 46 ++++++-------- bioptim/limits/multinode_penalty.py | 12 ++-- bioptim/limits/penalty.py | 60 +++++++++---------- bioptim/limits/penalty_controller.py | 10 ++-- bioptim/limits/penalty_option.py | 2 +- bioptim/limits/phase_transition.py | 4 +- bioptim/models/biorbd/biorbd_model.py | 8 +-- .../models/biorbd/holonomic_biorbd_model.py | 8 ++- .../models/biorbd/stochastic_biorbd_model.py | 18 +++++- .../models/biorbd/variational_biorbd_model.py | 28 +++++---- bioptim/models/holonomic_constraints.py | 6 +- .../variational_optimal_control_program.py | 15 ++--- tests/shard1/test_biorbd_model_size.py | 2 + tests/shard1/test_biorbd_multi_model.py | 34 +++++------ tests/shard3/test_graph.py | 54 ++++++++--------- tests/shard3/test_ligaments.py | 26 ++++---- tests/shard3/test_passive_torque.py | 26 ++++---- tests/shard4/test_penalty.py | 10 ++-- tests/shard4/test_variational_biorbd_model.py | 2 +- 29 files changed, 213 insertions(+), 197 deletions(-) diff --git a/bioptim/dynamics/configure_problem.py b/bioptim/dynamics/configure_problem.py index 9181ea0f0..e380b009c 100644 --- a/bioptim/dynamics/configure_problem.py +++ b/bioptim/dynamics/configure_problem.py @@ -1351,8 +1351,8 @@ def configure_soft_contact_function(ocp, nlp): """ component_list = ["Mx", "My", "Mz", "Fx", "Fy", "Fz"] - q = nlp.states.cx[nlp.states["q"].index] - qdot = nlp.states.cx[nlp.states["qdot"].index] + q = nlp.states["q"].mapping.to_second.map(nlp.states["q"].cx) + qdot = nlp.states["qdot"].mapping.to_second.map(nlp.states["qdot"].cx) 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 diff --git a/bioptim/dynamics/dynamics_functions.py b/bioptim/dynamics/dynamics_functions.py index 19e03cde6..eb560f97c 100644 --- a/bioptim/dynamics/dynamics_functions.py +++ b/bioptim/dynamics/dynamics_functions.py @@ -30,8 +30,6 @@ class DynamicsFunctions: Contact forces of a forward dynamics driven by muscles activations and joint torques with contact constraints. get(var: OptimizationVariable, cx: MX | SX): Main accessor to a variable in states or controls (cx) - apply_parameters(parameters: MX.sym, nlp: NonLinearProgram) - Apply the parameter variables to the model. This should be called before calling the dynamics reshape_qdot(nlp: NonLinearProgram, q: MX | SX, qdot: MX | SX): Easy accessor to derivative of q forward_dynamics(nlp: NonLinearProgram, q: MX | SX, qdot: MX | SX, tau: MX | SX, with_contact: bool): @@ -938,7 +936,7 @@ def forces_from_muscle_driven( 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 joints_acceleration_driven( @@ -985,7 +983,7 @@ def joints_acceleration_driven( qdot = nlp.get_var_from_states_or_controls("qdot", states, controls) qddot_joints = nlp.get_var_from_states_or_controls("qddot", states, controls) - qddot_root = nlp.model.forward_dynamics_free_floating_base()(q, qdot, qddot_joints) + qddot_root = nlp.model.forward_dynamics_free_floating_base()(q, qdot, qddot_joints, nlp.parameters.cx) qddot_reordered = nlp.model.reorder_qddot_root_joints(qddot_root, qddot_joints) qdot_mapped = nlp.variable_mappings["qdot"].to_first.map(qdot) @@ -1188,7 +1186,7 @@ def compute_muscle_dot(nlp, muscle_excitations: MX | SX): The derivative of muscle activations """ - return nlp.model.muscle_activation_dot()(muscle_excitations) + return nlp.model.muscle_activation_dot()(muscle_excitations, nlp.parameters.cx) @staticmethod def compute_tau_from_muscle( diff --git a/bioptim/examples/getting_started/custom_constraint.py b/bioptim/examples/getting_started/custom_constraint.py index cd8dafe39..4f20b822f 100644 --- a/bioptim/examples/getting_started/custom_constraint.py +++ b/bioptim/examples/getting_started/custom_constraint.py @@ -53,7 +53,7 @@ def custom_func_track_markers(controller: PenaltyController, first_marker: str, marker_1_idx = controller.model.marker_index(second_marker) # compute the position of the markers using the markers function from the BioModel (here a BiorbdModel) - markers = controller.model.markers()(controller.states["q"].cx) + markers = controller.model.markers()(controller.states["q"].cx, controller.parameters.cx) markers_diff = markers[marker_1_idx] - markers[marker_0_idx] return markers_diff diff --git a/bioptim/examples/getting_started/custom_dynamics.py b/bioptim/examples/getting_started/custom_dynamics.py index d5a6f8e0a..2e53a5a30 100644 --- a/bioptim/examples/getting_started/custom_dynamics.py +++ b/bioptim/examples/getting_started/custom_dynamics.py @@ -74,7 +74,7 @@ def custom_dynamics( # You can directly call biorbd function (as for ddq) or call bioptim accessor (as for dq) dq = DynamicsFunctions.compute_qdot(nlp, q, qdot) * my_additional_factor - ddq = nlp.model.forward_dynamics()(q, qdot, tau, []) + ddq = nlp.model.forward_dynamics()(q, qdot, tau, [], nlp.parameters.cx) # the user has to choose if want to return the explicit dynamics dx/dt = f(x,u,p) # as the first argument of DynamicsEvaluation or diff --git a/bioptim/examples/getting_started/custom_objectives.py b/bioptim/examples/getting_started/custom_objectives.py index eff7b05cb..d26403b22 100644 --- a/bioptim/examples/getting_started/custom_objectives.py +++ b/bioptim/examples/getting_started/custom_objectives.py @@ -52,7 +52,7 @@ def custom_func_track_markers(controller: PenaltyController, first_marker: str, marker_1_idx = controller.model.marker_index(second_marker) # Convert the function to the required format and then subtract - markers = controller.model.markers()(controller.states["q"].cx) + markers = controller.model.markers()(controller.states["q"].cx, controller.parameters.cx) markers_diff = markers[marker_1_idx] - markers[marker_0_idx] return markers_diff diff --git a/bioptim/examples/getting_started/example_continuity_as_objective.py b/bioptim/examples/getting_started/example_continuity_as_objective.py index 46cb7f3cd..63dc972ed 100644 --- a/bioptim/examples/getting_started/example_continuity_as_objective.py +++ b/bioptim/examples/getting_started/example_continuity_as_objective.py @@ -44,7 +44,7 @@ def out_of_sphere(controller: PenaltyController, y, z): q = controller.states["q"].cx - marker_q = controller.model.markers(q)[1] + marker_q = controller.model.markers()(q, controller.parameters.cx)[1] distance = sqrt((y - marker_q[1]) ** 2 + (z - marker_q[2]) ** 2) diff --git a/bioptim/examples/moving_horizon_estimation/mhe.py b/bioptim/examples/moving_horizon_estimation/mhe.py index 2df1c330a..d992aad8e 100644 --- a/bioptim/examples/moving_horizon_estimation/mhe.py +++ b/bioptim/examples/moving_horizon_estimation/mhe.py @@ -42,12 +42,12 @@ def states_to_markers(bio_model, states): nq = bio_model.nb_q n_mark = bio_model.nb_markers - return np.array(bio_model.markers()(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 qddot_func = bio_model.forward_dynamics() diff --git a/bioptim/examples/muscle_driven_ocp/muscle_activations_tracker.py b/bioptim/examples/muscle_driven_ocp/muscle_activations_tracker.py index c91aa7c1c..056847daf 100644 --- a/bioptim/examples/muscle_driven_ocp/muscle_activations_tracker.py +++ b/bioptim/examples/muscle_driven_ocp/muscle_activations_tracker.py @@ -12,7 +12,7 @@ import biorbd_casadi as biorbd import numpy as np -from casadi import MX, vertcat +from casadi import MX, vertcat, horzcat from matplotlib import pyplot as plt from scipy.integrate import solve_ivp @@ -78,7 +78,8 @@ def generate_data( n_mus = bio_model.nb_muscles dt = final_time / n_shooting - nlp = NonLinearProgram(phase_dynamics=phase_dynamics) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=False) + nlp.cx = MX nlp.model = bio_model nlp.variable_mappings = { "q": BiMapping(range(n_q), range(n_q)), @@ -96,7 +97,6 @@ def generate_data( symbolic_tau = MX.sym("tau", n_tau, 1) symbolic_mus = MX.sym("muscles", n_mus, 1) symbolic_parameters = MX.sym("params", 0, 0) - markers_func = biorbd.to_casadi_func("ForwardKin", bio_model.markers, symbolic_q) nlp.states = OptimizationVariableContainer(phase_dynamics=phase_dynamics) nlp.states_dot = OptimizationVariableContainer(phase_dynamics=phase_dynamics) @@ -199,7 +199,7 @@ def dyn_interface(t, x, u): def add_to_data(i, q): X[:, i] = q - markers[:, :, i] = markers_func(q[0:n_q]) + markers[:, :, i] = horzcat(*bio_model.markers()(q[0:n_q], [])) x_init = np.array([0] * n_q + [0] * n_qdot) add_to_data(0, x_init) diff --git a/bioptim/examples/muscle_driven_ocp/muscle_excitations_tracker.py b/bioptim/examples/muscle_driven_ocp/muscle_excitations_tracker.py index 4f2235fa9..fd372bf1c 100644 --- a/bioptim/examples/muscle_driven_ocp/muscle_excitations_tracker.py +++ b/bioptim/examples/muscle_driven_ocp/muscle_excitations_tracker.py @@ -202,7 +202,7 @@ def dyn_interface(t, x, u): def add_to_data(i, q): X[:, i] = q - markers[:, :, i] = horzcat(*bio_model.markers()(q[:n_q])) + markers[:, :, i] = horzcat(*bio_model.markers()(q[:n_q], [])) x_init = np.array([0.0] * n_q + [0.0] * n_qdot + [0.5] * n_mus) add_to_data(0, x_init) diff --git a/bioptim/examples/stochastic_optimal_control/arm_reaching_torque_driven_collocations.py b/bioptim/examples/stochastic_optimal_control/arm_reaching_torque_driven_collocations.py index 306e7251a..a7420c7c3 100644 --- a/bioptim/examples/stochastic_optimal_control/arm_reaching_torque_driven_collocations.py +++ b/bioptim/examples/stochastic_optimal_control/arm_reaching_torque_driven_collocations.py @@ -47,8 +47,8 @@ def sensory_reference( """ q = states[nlp.states["q"].index] qdot = states[nlp.states["qdot"].index] - hand_pos = nlp.model.markers(q)[2][:2] - hand_vel = nlp.model.marker_velocities(q, qdot)[2][:2] + hand_pos = nlp.model.markers()(q, [])[2][:2] + hand_vel = nlp.model.marker_velocities()(q, qdot, [])[2][:2] hand_pos_velo = cas.vertcat(hand_pos, hand_vel) return hand_pos_velo @@ -115,6 +115,7 @@ def prepare_socp( n_noised_controls=2, n_collocation_points=polynomial_degree + 1, friction_coefficients=np.array([[0.05, 0.025], [0.025, 0.05]]), + use_sx=use_sx, ) n_tau = bio_model.nb_tau diff --git a/bioptim/limits/constraints.py b/bioptim/limits/constraints.py index 68e41d90e..3534d3bbc 100644 --- a/bioptim/limits/constraints.py +++ b/bioptim/limits/constraints.py @@ -301,14 +301,14 @@ def torque_max_from_q_and_qdot( if min_torque and min_torque < 0: raise ValueError("min_torque cannot be negative in tau_max_from_actuators") - bound = controller.model.tau_max()(controller.q.cx, controller.qdot.cx) + bound = controller.model.tau_max()(controller.q, controller.qdot, controller.parameters.cx) min_bound = controller.tau.mapping.to_first.map(bound[1]) max_bound = controller.tau.mapping.to_first.map(bound[0]) if min_torque: min_bound = if_else(lt(min_bound, min_torque), min_torque, min_bound) max_bound = if_else(lt(max_bound, min_torque), min_torque, max_bound) - value = vertcat(controller.tau.cx_start + min_bound, controller.tau.cx_start - max_bound) + value = vertcat(controller.tau + min_bound, controller.tau - max_bound) if constraint.rows is None or isinstance(constraint.rows, (tuple, list, np.ndarray)): n_rows = value.shape[0] // 2 @@ -406,13 +406,11 @@ def qddot_equals_forward_dynamics( Since the function does nothing, we can safely ignore any argument """ - q = controller.q.cx - qdot = controller.qdot.cx - 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 + passive_torque = controller.model.passive_joint_torque()(controller.q, controller.qdot, controller.parameters.cx) + tau = controller.states["tau"].cx if "tau" in controller.states else controller.tau tau = tau + passive_torque if with_passive_torque else tau tau = ( - tau + controller.model.ligament_joint_torque(q, qdot, controller.parameters.cx) + tau + controller.model.ligament_joint_torque()(controller.q, controller.qdot, controller.parameters.cx) if with_ligament else tau ) @@ -420,7 +418,7 @@ def qddot_equals_forward_dynamics( 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, [], controller.parameters.cx + controller.q, controller.qdot, tau, [], controller.parameters.cx ) return qddot - qddot_fd @@ -453,14 +451,12 @@ def tau_equals_inverse_dynamics( Since the function does nothing, we can safely ignore any argument """ - q = controller.q.cx - qdot = controller.qdot.cx - tau = controller.states["tau"].cx if "tau" in controller.states else controller.tau.cx + tau = controller.states["tau"].cx if "tau" in controller.states else controller.tau qddot = controller.states["qddot"].cx if "qddot" in controller.states else controller.controls["qddot"].cx - passive_torque = controller.model.passive_joint_torque()(q, qdot, controller.parameters.cx) + passive_torque = controller.model.passive_joint_torque()(controller.q, controller.qdot, controller.parameters.cx) tau = tau + passive_torque if with_passive_torque else tau tau = ( - tau + controller.model.ligament_joint_torque()(q, qdot, controller.parameters.cx) + tau + controller.model.ligament_joint_torque()(controller.q, controller.qdot, controller.parameters.cx) if with_ligament else tau ) @@ -475,7 +471,7 @@ def tau_equals_inverse_dynamics( raise ValueError("External forces must be provided") tau_id = controller.model.inverse_dynamics(with_contact=with_contact)( - q, qdot, qddot, f_ext, controller.parameters.cx + controller.q, controller.qdot, qddot, f_ext, controller.parameters.cx ) var = [] @@ -504,13 +500,11 @@ def implicit_marker_acceleration( Since the function does nothing, we can safely ignore any argument """ - q = controller.q.cx - qdot = controller.qdot.cx qddot = controller.states["qddot"].cx if "qddot" in controller.states else controller.controls["qddot"].cx # TODO get the index of the marker contact_acceleration = controller.model.rigid_contact_acceleration(contact_index, contact_axis)( - q, qdot, qddot + controller.q, controller.qdot, qddot ) return contact_acceleration @@ -537,17 +531,15 @@ def tau_from_muscle_equal_inverse_dynamics( Since the function does nothing, we can safely ignore any argument """ - q = controller.q.cx - 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, controller.parameters.cx) + passive_torque = controller.model.passive_joint_torque()(controller.q, controller.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, controller.parameters.cx) + muscle_tau = controller.model.muscle_joint_torque(muscles_states, controller.q, controller.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, controller.parameters.cx) + muscle_tau + controller.model.ligament_joint_torque(controller.q, controller.qdot, controller.parameters.cx) if with_ligament else muscle_tau ) @@ -560,7 +552,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, controller.parameters.cx) + tau_id = controller.model.inverse_dynamics()(controller.q, controller.qdot, qddot, controller.parameters.cx) return tau_id - muscle_tau @@ -720,10 +712,10 @@ def stochastic_df_dx_implicit( DF_DX = DF_DX_fun( controller.t_span.cx, - controller.q.cx[:nb_root], - controller.q.cx[nb_root:], - controller.qdot.cx[:nb_root], - controller.qdot.cx[nb_root:], + controller.q[:nb_root], + controller.q[nb_root:], + controller.qdot[:nb_root], + controller.qdot[nb_root:], controller.controls.cx, parameters, controller.algebraic_states.cx, diff --git a/bioptim/limits/multinode_penalty.py b/bioptim/limits/multinode_penalty.py index 6abe9ca2d..7355acd63 100644 --- a/bioptim/limits/multinode_penalty.py +++ b/bioptim/limits/multinode_penalty.py @@ -267,11 +267,11 @@ def com_equality(penalty, controllers: list[PenaltyController]): MultinodePenaltyFunctions.Functions._prepare_controller_cx(penalty, controllers) - com_0 = controllers[0].model.center_of_mass(controllers[0].states["q"].cx) + com_0 = controllers[0].model.center_of_mass()(controllers[0].states["q"].cx, controllers[0].parameters.cx) out = controllers[0].cx.zeros((3, 1)) for i in range(1, len(controllers)): - com_i = controllers[i].model.center_of_mass(controllers[i].states["q"].cx) + com_i = controllers[i].model.center_of_mass()(controllers[i].states["q"].cx, controllers[i].parameters.cx) out += com_0 - com_i return out @@ -295,14 +295,14 @@ def com_velocity_equality(penalty, controllers: list[PenaltyController]): MultinodePenaltyFunctions.Functions._prepare_controller_cx(penalty, controllers) - com_dot_0 = controllers[0].model.center_of_mass_velocity( - controllers[0].states["q"].cx, controllers[0].states["qdot"].cx + com_dot_0 = controllers[0].model.center_of_mass_velocity()( + controllers[0].states["q"].cx, controllers[0].states["qdot"].cx, controllers[0].parameters.cx ) out = controllers[0].cx.zeros((3, 1)) for i in range(1, len(controllers)): - com_dot_i = controllers[i].model.center_of_mass_velocity( - controllers[i].states["q"].cx, controllers[i].states["qdot"].cx + com_dot_i = controllers[i].model.center_of_mass_velocity()( + controllers[i].states["q"].cx, controllers[i].states["qdot"].cx, controllers[i].parameters.cx ) out += com_dot_0 - com_dot_i diff --git a/bioptim/limits/penalty.py b/bioptim/limits/penalty.py index 56fa5c6ac..395e9feed 100644 --- a/bioptim/limits/penalty.py +++ b/bioptim/limits/penalty.py @@ -122,10 +122,10 @@ def minimize_power( controls = controller.controls[key_control].cx_start # select only actuacted states if key_control == "tau": - return controls * controller.qdot.cx_start + return controls * controller.qdot elif key_control == "muscles": muscles_dot = controller.model.muscle_velocity()( - controller.q.cx, controller.qdot.cx, controller.parameters.cx + controller.q, controller.qdot, controller.parameters.cx ) return controls * muscles_dot @@ -292,22 +292,22 @@ def minimize_markers( penalty.plot_target = False # Compute the position of the marker in the requested reference frame (None for global) - q = controller.q CX_eye = SX_eye if controller.ocp.cx == SX else MX_eye jcs_t = ( CX_eye(4) if reference_jcs is None else controller.model.homogeneous_matrices_in_global(reference_jcs, inverse=True)( - q.cx, controller.parameters.cx + controller.q, controller.parameters.cx ) ) - markers = [] - 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]) + markers = controller.model.markers()(controller.q, controller.parameters.cx) + markers_in_jcs = [] + for i in range(markers.shape[1]): + marker_in_jcs = jcs_t @ vertcat(markers[:, i], 1) + markers_in_jcs = horzcat(markers_in_jcs, marker_in_jcs[:3]) - return markers + return markers_in_jcs @staticmethod def minimize_markers_velocity( @@ -345,7 +345,7 @@ def minimize_markers_velocity( # Add the penalty in the requested reference frame. None for global markers = horzcat( - *controller.model.marker_velocities(reference_index=reference_jcs)(controller.q.cx, controller.qdot.cx) + *controller.model.marker_velocities(reference_index=reference_jcs)(controller.q, controller.qdot, controller.parameters.cx) ) return markers @@ -386,7 +386,7 @@ def minimize_markers_acceleration( markers = horzcat( *controller.model.marker_accelerations(reference_index=reference_jcs)( - controller.q.cx, controller.qdot.cx, qddot, controller.parameters.cx + controller.q, controller.qdot, qddot, controller.parameters.cx ) ) @@ -431,8 +431,8 @@ def superimpose_markers( penalty.quadratic = True if penalty.quadratic is None else penalty.quadratic diff_markers = controller.model.marker(second_marker_idx)( - controller.q.cx, controller.parameters.cx - ) - controller.model.marker(first_marker_idx)(controller.q.cx, controller.parameters.cx) + controller.q, controller.parameters.cx + ) - controller.model.marker(first_marker_idx)(controller.q, controller.parameters.cx) return diff_markers @@ -475,7 +475,7 @@ def superimpose_markers_velocity( penalty.quadratic = True if penalty.quadratic is None else penalty.quadratic marker_velocity = controller.model.marker_velocities()( - controller.q.cx, controller.qdot.cx, controller.parameters.cx + controller.q, controller.qdot, controller.parameters.cx ) marker_1 = marker_velocity[first_marker_idx][:] marker_2 = marker_velocity[second_marker_idx][:] @@ -605,9 +605,9 @@ def minimize_predicted_com_height(_: PenaltyOption, controller: PenaltyControlle """ g = controller.model.gravity()["o0"][2] - com = controller.model.center_of_mass()(controller.q.cx, controller.parameters.cx) + com = controller.model.center_of_mass()(controller.q, controller.parameters.cx) com_dot = controller.model.center_of_mass_velocity()( - controller.q.cx, controller.qdot.cx, controller.parameters.cx + controller.q, controller.qdot, controller.parameters.cx ) com_height = (com_dot[2] * com_dot[2]) / (2 * -g) + com[2] return com_height @@ -633,7 +633,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, controller.parameters.cx) + return controller.model.center_of_mass()(controller.q, controller.parameters.cx) @staticmethod def minimize_com_velocity(penalty: PenaltyOption, controller: PenaltyController, axes: tuple | list = None): @@ -657,7 +657,7 @@ def minimize_com_velocity(penalty: PenaltyOption, controller: PenaltyController, penalty.quadratic = True if penalty.quadratic is None else penalty.quadratic return controller.model.center_of_mass_velocity()( - controller.q.cx, controller.qdot.cx, controller.parameters.cx + controller.q, controller.qdot, controller.parameters.cx ) @staticmethod @@ -684,7 +684,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, controller.parameters.cx + controller.q, controller.qdot, qddot, controller.parameters.cx ) return marker @@ -708,7 +708,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, controller.parameters.cx) + return controller.model.angular_momentum()(controller.q, controller.qdot, controller.parameters.cx) @staticmethod def minimize_linear_momentum(penalty: PenaltyOption, controller: PenaltyController, axes: tuple | list = None): @@ -731,7 +731,7 @@ def minimize_linear_momentum(penalty: PenaltyOption, controller: PenaltyControll 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, controller.parameters.cx + controller.q, controller.qdot, controller.parameters.cx ) mass = controller.model.mass()["o0"] linear_momentum_cx = com_velocity * mass @@ -907,9 +907,9 @@ def track_segment_with_custom_rt( "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, controller.parameters.cx + controller.q, controller.parameters.cx )[:3, :3].T - r_rt = controller.model.rt(rt_idx)(controller.q.cx, controller.parameters.cx)[:3, :3] + r_rt = controller.model.rt(rt_idx)(controller.q, 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) @@ -951,7 +951,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, controller.parameters.cx) + marker = controller.model.marker(marker_idx, segment_idx)(controller.q, controller.parameters.cx) # To align an axis, the other must be equal to 0 if not penalty.rows_is_set: @@ -1002,7 +1002,7 @@ def minimize_segment_rotation( 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, controller.parameters.cx + controller.q, controller.parameters.cx )[:3, :3] angles_segment = controller.model.rotation_matrix_to_euler_angles(sequence)(jcs_segment) @@ -1049,7 +1049,7 @@ def minimize_segment_velocity( ) model: BiorbdModel = controller.model segment_angular_velocity = model.segment_angular_velocity(segment_idx)( - controller.q.cx, controller.qdot.cx, controller.parameters.cx + controller.q, controller.qdot, controller.parameters.cx ) if axes is None: @@ -1118,16 +1118,16 @@ def track_vector_orientations_from_markers( ) vector_0_marker_0_position = controller.model.marker(vector_0_marker_0_idx)( - controller.q.cx, controller.parameters.cx + controller.q, controller.parameters.cx ) vector_0_marker_1_position = controller.model.marker(vector_0_marker_1_idx)( - controller.q.cx, controller.parameters.cx + controller.q, controller.parameters.cx ) vector_1_marker_0_position = controller.model.marker(vector_1_marker_0_idx)( - controller.q.cx, controller.parameters.cx + controller.q, controller.parameters.cx ) vector_1_marker_1_position = controller.model.marker(vector_1_marker_1_idx)( - controller.q.cx, controller.parameters.cx + controller.q, controller.parameters.cx ) vector_0 = vector_0_marker_1_position - vector_0_marker_0_position diff --git a/bioptim/limits/penalty_controller.py b/bioptim/limits/penalty_controller.py index dd6558a1d..999806950 100644 --- a/bioptim/limits/penalty_controller.py +++ b/bioptim/limits/penalty_controller.py @@ -392,8 +392,9 @@ def parameters_scaled(self) -> OptimizationVariableList: @property def q(self) -> OptimizationVariable: if "q" in self.states: - return self.states["q"] + return self.states["q"].mapping.to_second.map(self.states["q"].cx) elif "q_roots" in self.states and "q_joints" in self.states: + # TODO: add mapping for q_roots and q_joints cx_start = vertcat(self.states["q_roots"].cx_start, self.states["q_joints"].cx_start) q_parent_list = OptimizationVariableList( self._nlp.cx, self._nlp.phase_dynamics == PhaseDynamics.SHARED_DURING_THE_PHASE @@ -417,8 +418,9 @@ def q(self) -> OptimizationVariable: @property def qdot(self) -> OptimizationVariable: if "qdot" in self.states: - return self.states["qdot"] + return self.states["qdot"].mapping.to_second.map(self.states["qdot"].cx) elif "qdot_roots" in self.states and "qdot_joints" in self.states: + # TODO: add mapping for qdot_roots and qdot_joints cx_start = vertcat(self.states["qdot_roots"].cx_start, self.states["qdot_joints"].cx_start) qdot_parent_list = OptimizationVariableList( self._nlp.cx, self._nlp.phase_dynamics == PhaseDynamics.SHARED_DURING_THE_PHASE @@ -440,9 +442,9 @@ def qdot(self) -> OptimizationVariable: @property def tau(self) -> OptimizationVariable: if "tau" in self.controls: - return self.controls["tau"] + return self.controls["tau"].mapping.to_second.map(self.controls["tau"].cx) elif "tau_joints" in self.controls: - return self.controls["tau_joints"] + return self.controls["tau_joints"].mapping.to_second.map(self.controls["tau_joints"].cx) def copy(self): return PenaltyController( diff --git a/bioptim/limits/penalty_option.py b/bioptim/limits/penalty_option.py index f89f3033d..3054b662e 100644 --- a/bioptim/limits/penalty_option.py +++ b/bioptim/limits/penalty_option.py @@ -313,7 +313,7 @@ def transform_penalty_to_stochastic(self, controller: PenaltyController, fcn, st # TODO: Charbie -> This is just a first implementation (x=[q, qdot]), it should then be generalized - nx = controller.q.cx_start.shape[0] + nx = controller.q.shape[0] n_root = controller.model.nb_root n_joints = nx - n_root diff --git a/bioptim/limits/phase_transition.py b/bioptim/limits/phase_transition.py index 601252f23..185c685e8 100644 --- a/bioptim/limits/phase_transition.py +++ b/bioptim/limits/phase_transition.py @@ -220,7 +220,7 @@ def discontinuous(transition, controllers: list[PenaltyController, PenaltyContro The difference between the state after and before """ - return MX.zeros(0, 0) + return controllers.cx.zeros(0, 0) @staticmethod def cyclic(transition, controllers: list[PenaltyController, PenaltyController]) -> MX: @@ -274,7 +274,7 @@ def impact(transition, controllers: list[PenaltyController, PenaltyController]): # Todo scaled? q_pre = pre.states["q"].cx qdot_pre = pre.states["qdot"].cx - qdot_impact = post.model.qdot_from_impact()(q_pre, qdot_pre) + qdot_impact = post.model.qdot_from_impact()(q_pre, qdot_pre, pre.parameters.cx) val = [] cx_start = [] diff --git a/bioptim/models/biorbd/biorbd_model.py b/bioptim/models/biorbd/biorbd_model.py index 35ce9b845..c99a25fd6 100644 --- a/bioptim/models/biorbd/biorbd_model.py +++ b/bioptim/models/biorbd/biorbd_model.py @@ -510,7 +510,7 @@ def muscle_length_jacobian(self) -> Function: return casadi_fun def muscle_velocity(self) -> Function: - J = self.muscle_length_jacobian()(self.q) + J = self.muscle_length_jacobian()(self.q, self.parameters) biorbd_return = J @ self.qdot casadi_fun = Function( "muscle_velocity", @@ -535,11 +535,11 @@ def muscle_joint_torque(self) -> Function: return casadi_fun def markers(self) -> list[MX]: - biorbd_return = [m.to_mx() for m in self.model.markers(GeneralizedCoordinates(self.q))] + biorbd_return = horzcat(*[m.to_mx() for m in self.model.markers(GeneralizedCoordinates(self.q))]) casadi_fun = Function( "markers", [self.q, self.parameters], - biorbd_return, + [biorbd_return], ) return casadi_fun @@ -784,7 +784,7 @@ def get_quaternion_idx(self) -> list[list[int]]: def contact_forces(self) -> Function: force = self.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 ) casadi_fun = Function( "contact_forces", diff --git a/bioptim/models/biorbd/holonomic_biorbd_model.py b/bioptim/models/biorbd/holonomic_biorbd_model.py index f5ef5cd7e..fbcddad60 100644 --- a/bioptim/models/biorbd/holonomic_biorbd_model.py +++ b/bioptim/models/biorbd/holonomic_biorbd_model.py @@ -9,6 +9,7 @@ from .biorbd_model import BiorbdModel from ..holonomic_constraints import HolonomicConstraintsList +from ...optimization.parameters import ParameterList class HolonomicBiorbdModel(BiorbdModel): @@ -16,8 +17,11 @@ class HolonomicBiorbdModel(BiorbdModel): This class allows to define a biorbd model with custom holonomic constraints. """ - def __init__(self, bio_model: str | biorbd.Model): - super().__init__(bio_model) + def __init__(self, + bio_model: str | biorbd.Model, + parameters: ParameterList = None, + ): + super().__init__(bio_model, parameters=parameters) self._newton_tol = 1e-10 self._holonomic_constraints = [] self._holonomic_constraints_jacobians = [] diff --git a/bioptim/models/biorbd/stochastic_biorbd_model.py b/bioptim/models/biorbd/stochastic_biorbd_model.py index 9bb9be484..a5676f0ed 100644 --- a/bioptim/models/biorbd/stochastic_biorbd_model.py +++ b/bioptim/models/biorbd/stochastic_biorbd_model.py @@ -5,6 +5,8 @@ from bioptim import BiorbdModel, DynamicsFunctions from ...misc.mapping import BiMappingList +from ...optimization.parameters import ParameterList +from ...optimization.variable_scaling import VariableScaling def _compute_torques_from_noise_and_feedback_default( @@ -48,9 +50,23 @@ def __init__( compute_torques_from_noise_and_feedback: Callable = _compute_torques_from_noise_and_feedback_default, motor_noise_mapping: BiMappingList = BiMappingList(), n_collocation_points: int = 1, + use_sx: bool = False, + parameters: ParameterList = None, **kwargs, ): - super().__init__(bio_model if isinstance(bio_model, str) else bio_model.model, **kwargs) + if parameters is None: + parameters = ParameterList(use_sx=use_sx) + parameters.add("motor_noise", + lambda model, param: None, + size=motor_noise_magnitude.shape[0], + scaling=VariableScaling("motor_noise", [1.0] * motor_noise_magnitude.shape[0])) + parameters.add("sensory_noise", + lambda model, param: None, + size=sensory_noise_magnitude.shape[0], + scaling=VariableScaling("sensory_noise", [1.0] * sensory_noise_magnitude.shape[0])) + super().__init__(bio_model=(bio_model if isinstance(bio_model, str) else bio_model.model), + parameters=parameters, + **kwargs) self.motor_noise_magnitude = motor_noise_magnitude self.sensory_noise_magnitude = sensory_noise_magnitude diff --git a/bioptim/models/biorbd/variational_biorbd_model.py b/bioptim/models/biorbd/variational_biorbd_model.py index 72e78d0ae..f30e6f7b6 100644 --- a/bioptim/models/biorbd/variational_biorbd_model.py +++ b/bioptim/models/biorbd/variational_biorbd_model.py @@ -7,6 +7,7 @@ from .holonomic_biorbd_model import HolonomicBiorbdModel from ...misc.enums import ControlType, QuadratureRule +from ...optimization.parameters import ParameterList class VariationalBiorbdModel(HolonomicBiorbdModel): @@ -21,8 +22,9 @@ def __init__( discrete_approximation: QuadratureRule = QuadratureRule.TRAPEZOIDAL, control_type: ControlType = ControlType.CONSTANT, control_discrete_approximation: QuadratureRule = QuadratureRule.MIDPOINT, + parameters: ParameterList = None, ): - super().__init__(bio_model) + super().__init__(bio_model, parameters=parameters) self.discrete_approximation = discrete_approximation self.control_type = control_type self.control_discrete_approximation = control_discrete_approximation @@ -52,20 +54,20 @@ def discrete_lagrangian( if self.discrete_approximation == QuadratureRule.MIDPOINT: q_discrete = (q1 + q2) / 2 qdot_discrete = (q2 - q1) / time_step - return time_step * self.lagrangian(q_discrete, qdot_discrete) + return time_step * self.lagrangian()(q_discrete, qdot_discrete) elif self.discrete_approximation == QuadratureRule.RECTANGLE_LEFT: q_discrete = q1 qdot_discrete = (q2 - q1) / time_step - return time_step * self.lagrangian(q_discrete, qdot_discrete) + return time_step * self.lagrangian()(q_discrete, qdot_discrete) elif self.discrete_approximation == QuadratureRule.RECTANGLE_RIGHT: q_discrete = q2 qdot_discrete = (q2 - q1) / time_step - return time_step * self.lagrangian(q_discrete, qdot_discrete) + return time_step * self.lagrangian()(q_discrete, qdot_discrete) elif self.discrete_approximation == QuadratureRule.TRAPEZOIDAL: # from : M. West, “Variational integrators,” Ph.D. dissertation, California Inst. # Technol., Pasadena, CA, 2004. p 13 qdot_discrete = (q2 - q1) / time_step - return time_step / 2 * (self.lagrangian(q1, qdot_discrete) + self.lagrangian(q2, qdot_discrete)) + return time_step / 2 * (self.lagrangian()(q1, qdot_discrete) + self.lagrangian()(q2, qdot_discrete)) else: raise NotImplementedError(f"Discrete Lagrangian {self.discrete_approximation} is not implemented") @@ -179,6 +181,8 @@ def discrete_euler_lagrange_equations( The following equation as been calculated thanks to the paper "Discrete mechanics and optimal control for constrained systems" (https://onlinelibrary.wiley.com/doi/epdf/10.1002/oca.912), equations (10). """ + CX = MX if isinstance(q_prev, MX) else SX + # Refers to D_2 L_d(q_{k-1}, q_k) (D_2 is the partial derivative with respect to the second argument, L_d is the # discrete Lagrangian) p_current = transpose(jacobian(self.discrete_lagrangian(q_prev, q_cur, time_step), q_cur)) @@ -189,7 +193,7 @@ def discrete_euler_lagrange_equations( constraint_term = ( transpose(self.discrete_holonomic_constraints_jacobian(time_step, q_cur)) @ lambdas if self.has_holonomic_constraints - else MX.zeros(p_current.shape) + else CX.zeros(p_current.shape) ) residual = ( @@ -244,8 +248,10 @@ def compute_initial_states( constrained systems" (https://onlinelibrary.wiley.com/doi/epdf/10.1002/oca.912), equations (14) and the indications given just before the equation (18) for p0 and pN. """ + CX = MX if isinstance(q0, MX) else SX + # Refers to D_2 L(q_0, \dot{q_0}) (D_2 is the partial derivative with respect to the second argument) - d2_l_q0_qdot0 = transpose(jacobian(self.lagrangian(q0, qdot0), qdot0)) + d2_l_q0_qdot0 = transpose(jacobian(self.lagrangian()(q0, qdot0), qdot0)) # Refers to D_1 L_d(q_0, q_1) (D1 is the partial derivative with respect to the first argument, Ld is the # discrete Lagrangian) d1_ld_q0_q1 = transpose(jacobian(self.discrete_lagrangian(q0, q1, time_step), q0)) @@ -255,7 +261,7 @@ def compute_initial_states( constraint_term = ( 1 / 2 * transpose(self.discrete_holonomic_constraints_jacobian(time_step, q0)) @ lambdas0 if self.has_holonomic_constraints - else MX.zeros(self.nb_q, 1) + else CX.zeros(self.nb_q, 1) ) residual = d2_l_q0_qdot0 + d1_ld_q0_q1 + f0_minus - constraint_term @@ -310,8 +316,10 @@ def compute_final_states( constrained systems" (https://onlinelibrary.wiley.com/doi/epdf/10.1002/oca.912), equations (14) and the indications given just before the equation (18) for p0 and pN. """ + CX = MX if isinstance(q_penultimate, MX) else SX + # Refers to D_2 L(q_N, \dot{q_N}) (D_2 is the partial derivative with respect to the second argument) - d2_l_q_ultimate_qdot_ultimate = transpose(jacobian(self.lagrangian(q_ultimate, q_dot_ultimate), q_dot_ultimate)) + d2_l_q_ultimate_qdot_ultimate = transpose(jacobian(self.lagrangian()(q_ultimate, q_dot_ultimate), q_dot_ultimate)) # Refers to D_2 L_d(q_{n-1}, q_1) (Ld is the discrete Lagrangian) d2_ld_q_penultimate_q_ultimate = transpose( jacobian(self.discrete_lagrangian(q_penultimate, q_ultimate, time_step), q_ultimate) @@ -322,7 +330,7 @@ def compute_final_states( constraint_term = ( 1 / 2 * transpose(self.discrete_holonomic_constraints_jacobian(time_step, q_ultimate)) @ lambdas_ultimate if self.has_holonomic_constraints - else MX.zeros(self.nb_q, 1) + else CX.zeros(self.nb_q, 1) ) residual = -d2_l_q_ultimate_qdot_ultimate + d2_ld_q_penultimate_q_ultimate + fd_plus - constraint_term diff --git a/bioptim/models/holonomic_constraints.py b/bioptim/models/holonomic_constraints.py index 486baa928..e9a4f48ab 100644 --- a/bioptim/models/holonomic_constraints.py +++ b/bioptim/models/holonomic_constraints.py @@ -52,9 +52,11 @@ def superimpose_markers( q_ddot_sym = MX.sym("q_ddot", biorbd_model.nb_qdot, 1) # symbolic markers in global frame - marker_1_sym = biorbd_model.marker(index=biorbd_model.marker_index(marker_1))(q_sym) + # TODO: add the parameters + param = [] + marker_1_sym = biorbd_model.marker(index=biorbd_model.marker_index(marker_1))(q_sym, param) if marker_2 is not None: - marker_2_sym = biorbd_model.marker(index=biorbd_model.marker_index(marker_2))(q_sym) + marker_2_sym = biorbd_model.marker(index=biorbd_model.marker_index(marker_2))(q_sym, param) else: marker_2_sym = MX([0, 0, 0]) diff --git a/bioptim/optimization/variational_optimal_control_program.py b/bioptim/optimization/variational_optimal_control_program.py index 8451885bd..c3933cf5f 100644 --- a/bioptim/optimization/variational_optimal_control_program.py +++ b/bioptim/optimization/variational_optimal_control_program.py @@ -3,12 +3,11 @@ """ import numpy as np -from casadi import MX, Function, vertcat +from casadi import Function, vertcat from .optimal_control_program import OptimalControlProgram from ..dynamics.configure_problem import ConfigureProblem, DynamicsList from ..dynamics.dynamics_evaluation import DynamicsEvaluation -from ..dynamics.dynamics_functions import DynamicsFunctions from ..limits.constraints import ParameterConstraintList from ..limits.multinode_constraint import MultinodeConstraintList from ..limits.objective_functions import ParameterObjectiveList @@ -234,9 +233,7 @@ def configure_dynamics_function( If the dynamics should be expanded with casadi """ - DynamicsFunctions.apply_parameters(nlp) - - dynamics_eval = DynamicsEvaluation(MX(0), MX(0)) + dynamics_eval = DynamicsEvaluation(nlp.cx(0), nlp.cx(0)) dynamics_dxdt = dynamics_eval.dxdt if isinstance(dynamics_dxdt, (list, tuple)): dynamics_dxdt = vertcat(*dynamics_dxdt) @@ -280,7 +277,7 @@ def configure_dynamics_function( two_last_nodes_input = [dt, q_penultimate, q_ultimate, qdot_ultimate, controlN_minus_1, controlN] if self.bio_model.has_holonomic_constraints: - lambdas = MX.sym("lambda", self.bio_model.nb_holonomic_constraints, 1) + lambdas = nlp.cx.sym("lambda", self.bio_model.nb_holonomic_constraints, 1) three_nodes_input.append(lambdas) two_first_nodes_input.append(lambdas) two_last_nodes_input.append(lambdas) @@ -390,7 +387,7 @@ def variational_integrator_three_nodes( Returns ------- - The symbolic MX expression of the discrete Euler Lagrange equations + The symbolic expression of the discrete Euler Lagrange equations for the integration from node i-1, i,to i+1. """ @@ -433,7 +430,7 @@ def variational_integrator_initial( Returns ------- - The symbolic MX expression of the initial continuity constraint for the integration. + The symbolic expression of the initial continuity constraint for the integration. """ if self.bio_model.has_holonomic_constraints: @@ -474,7 +471,7 @@ def variational_integrator_final( Returns ------- - The symbolic MX expression of the final continuity constraint for the integration. + The symbolic expression of the final continuity constraint for the integration. """ if self.bio_model.has_holonomic_constraints: diff --git a/tests/shard1/test_biorbd_model_size.py b/tests/shard1/test_biorbd_model_size.py index 1523ee2c2..f8e7b5913 100644 --- a/tests/shard1/test_biorbd_model_size.py +++ b/tests/shard1/test_biorbd_model_size.py @@ -10,8 +10,10 @@ def model(): return + # Pariterre: Can I remove this file all together? + def generate_q_vectors(model): q_valid = MX([0.1] * model.nb_q) q_too_large = MX([0.1] * (model.nb_q + 1)) diff --git a/tests/shard1/test_biorbd_multi_model.py b/tests/shard1/test_biorbd_multi_model.py index 58bad292f..a9c76fad6 100644 --- a/tests/shard1/test_biorbd_multi_model.py +++ b/tests/shard1/test_biorbd_multi_model.py @@ -95,7 +95,7 @@ def test_biorbd_model(): models.serialize() models.set_gravity(np.array([0, 0, -3])) - TestUtils.assert_equal(models.gravity()['o0'], np.array([0, 0, -3, 0, 0, -3]).reshape(6, 1)) + TestUtils.assert_equal(models.gravity()["o0"], np.array([0, 0, -3, 0, 0, -3]).reshape(6, 1)) models.set_gravity(np.array([0, 0, -9.81])) with pytest.raises(NotImplementedError, match="segment_index is not implemented for MultiBiorbdModel"): @@ -156,16 +156,12 @@ def test_biorbd_model(): np.array([[9.313616, 5.580191, 2.063886], [5.580191, 4.791997, 1.895999], [2.063886, 1.895999, 0.945231]]), ) - nonlinear_effects = models.non_linear_effects()(np.array([1, 2.1, 3, 4.1, 5, 6.1]), np.array([1, 2.1, 3, 4.1, 5, 6])) - assert len(nonlinear_effects) == 2 - TestUtils.assert_equal( - nonlinear_effects[0], - np.array([38.817401, -1.960653, -1.259441]).reshape(3, 1) - ) - TestUtils.assert_equal( - nonlinear_effects[1], - np.array([322.060726, -22.147881, -20.660836]).reshape(3, 1) + nonlinear_effects = models.non_linear_effects()( + np.array([1, 2.1, 3, 4.1, 5, 6.1]), np.array([1, 2.1, 3, 4.1, 5, 6]) ) + assert len(nonlinear_effects) == 2 + TestUtils.assert_equal(nonlinear_effects[0], np.array([38.817401, -1.960653, -1.259441]).reshape(3, 1)) + TestUtils.assert_equal(nonlinear_effects[1], np.array([322.060726, -22.147881, -20.660836]).reshape(3, 1)) TestUtils.assert_equal( models.angular_momentum()(np.array([1, 2.1, 3, 4.1, 5, 6.1]), np.array([1, 2.1, 3, 4.1, 5, 6])), @@ -198,7 +194,7 @@ def test_biorbd_model(): np.array([1, 2.1, 3, 4.1, 5, 6]), np.array([3.1, 0.0, 9.1, 0.0]), ), - np.array([-14.750679, -40.031762]).reshape(2, 1) + np.array([-14.750679, -40.031762]).reshape(2, 1), ) TestUtils.assert_equal( @@ -207,7 +203,7 @@ def test_biorbd_model(): np.array([1, 2.1, 3, 4.1, 5, 6]), np.array([3.1, 1, 2, 9.1, 1, 2]), ), - np.array([-16.092093, 9.049853, 10.570878, -121.783105, 154.820759, -20.664452]).reshape(6, 1) + np.array([-16.092093, 9.049853, 10.570878, -121.783105, 154.820759, -20.664452]).reshape(6, 1), ) TestUtils.assert_equal( @@ -216,7 +212,7 @@ def test_biorbd_model(): np.array([1, 2.1, 3, 4.1, 5, 6]), np.array([3.1, 1, 2, 9.1, 1, 2]), ), - np.array([-16.092093, 9.049853, 10.570878, -121.783105, 154.820759, -20.664452]).reshape(6, 1) + np.array([-16.092093, 9.049853, 10.570878, -121.783105, 154.820759, -20.664452]).reshape(6, 1), ) with pytest.raises(RuntimeError, match="Incorrect number of inputs: Expected 3, got 4"): @@ -244,7 +240,7 @@ def test_biorbd_model(): np.array([1, 2.1, 3, 4.1, 5, 6]), np.array([3.1, 1, 2, 9.1, 1, 2]), ), - np.array([0.0, 0.0]).reshape(2, 1) + np.array([0.0, 0.0]).reshape(2, 1), ) with pytest.raises(RuntimeError, match="Incorrect number of inputs: Expected 3, got 4"): @@ -261,7 +257,7 @@ def test_biorbd_model(): np.array([1, 2.1, 3, 4.1, 5, 6.1]), np.array([1, 2.1, 3, 4.1, 5, 6]), ), - np.array([1.0, 2.1, 3.0, 4.1, 5.0, 6.0]).reshape(6, 1) + np.array([1.0, 2.1, 3.0, 4.1, 5.0, 6.0]).reshape(6, 1), ) TestUtils.assert_equal( @@ -291,7 +287,7 @@ def test_biorbd_model(): models.marker(index=1)( np.array([1, 2.1, 3, 4.1, 5, 6.1]), ), - np.array([0.0, 0.841471, -0.540302]).reshape(3, 1) + np.array([0.0, 0.841471, -0.540302]).reshape(3, 1), ) assert models.marker_index("marker_3") == 2 @@ -348,13 +344,11 @@ def test_biorbd_model(): np.array([1, 2.1, 3, 4.1, 5, 6]), np.array([3.1, 1, 2, 9.1, 1, 2]), ), - np.array([0.0, 0.0]).reshape(2, 1) + np.array([0.0, 0.0]).reshape(2, 1), ) # Because external_forces are not implemented yet - with pytest.raises( - RuntimeError, match="Incorrect number of inputs: Expected 3, got 4" - ): + with pytest.raises(RuntimeError, match="Incorrect number of inputs: Expected 3, got 4"): models.contact_forces()( np.array([1, 2.1, 3, 4.1, 5, 6.1]), np.array([1, 2.1, 3, 4.1, 5, 6]), diff --git a/tests/shard3/test_graph.py b/tests/shard3/test_graph.py index cb1e91881..cde4014f4 100644 --- a/tests/shard3/test_graph.py +++ b/tests/shard3/test_graph.py @@ -43,7 +43,7 @@ def custom_func_track_markers(controller: PenaltyController, first_marker: str, # Get the index of the markers from their name marker_0_idx = controller.model.marker_index(first_marker) marker_1_idx = controller.model.marker_index(second_marker) - markers = controller.model.markers()(controller.q.cx) + markers = controller.model.markers()(controller.q, controller.parameters.cx) return markers[:, marker_1_idx] - markers[:, marker_0_idx] @@ -258,32 +258,6 @@ def prepare_ocp_parameters( 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=10) - objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_TIME, weight=10) - - # Dynamics - dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN, expand_dynamics=True, 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, tau_init = -300, 300, 0 - 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() @@ -338,6 +312,32 @@ def prepare_ocp_parameters( 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=10) + objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_TIME, weight=10) + + # Dynamics + dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN, expand_dynamics=True, 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, tau_init = -300, 300, 0 + 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/tests/shard3/test_ligaments.py b/tests/shard3/test_ligaments.py index 882a6559e..b0c0f323b 100644 --- a/tests/shard3/test_ligaments.py +++ b/tests/shard3/test_ligaments.py @@ -30,7 +30,7 @@ def __init__(self, nlp, use_sx): self.n_phases = 1 self.nlp = [nlp] parameters_list = ParameterList(use_sx=use_sx) - self.parameters = ParameterContainer() + self.parameters = ParameterContainer(use_sx=use_sx) self.parameters.initialize(parameters_list) self.implicit_constraints = ConstraintList() self.n_threads = 1 @@ -41,14 +41,14 @@ def __init__(self, nlp, use_sx): @pytest.mark.parametrize("with_ligament", [False, True]) def test_torque_driven_with_ligament(with_ligament, cx, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(True if cx == SX else False)) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/torque_driven_ocp/models/mass_point_with_ligament.bioMod" ) nlp.ns = 5 nlp.cx = cx - nlp.time_mx = MX.sym("time", 1, 1) - nlp.dt_mx = MX.sym("dt", 1, 1) + nlp.time_cx = cx.sym("time", 1, 1) + nlp.dt = cx.sym("dt", 1, 1) nlp.initialize(cx) nlp.x_scaling = VariableScalingList() nlp.xdot_scaling = VariableScalingList() @@ -105,14 +105,14 @@ def test_torque_driven_with_ligament(with_ligament, cx, phase_dynamics): @pytest.mark.parametrize("with_ligament", [False, True]) def test_torque_derivative_driven_with_ligament(with_ligament, cx, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(True if cx == SX else False)) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/torque_driven_ocp/models/mass_point_with_ligament.bioMod" ) nlp.ns = 5 nlp.cx = cx - nlp.time_mx = MX.sym("time", 1, 1) - nlp.dt_mx = MX.sym("dt", 1, 1) + nlp.time_cx = cx.sym("time", 1, 1) + nlp.dt = cx.sym("dt", 1, 1) nlp.initialize(cx) nlp.x_scaling = VariableScalingList() nlp.xdot_scaling = VariableScalingList() @@ -171,14 +171,14 @@ def test_torque_derivative_driven_with_ligament(with_ligament, cx, phase_dynamic @pytest.mark.parametrize("with_ligament", [False, True]) def test_torque_activation_driven_with_ligament(with_ligament, cx, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(True if cx == SX else False)) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/torque_driven_ocp/models/mass_point_with_ligament.bioMod" ) nlp.ns = 5 nlp.cx = cx - nlp.time_mx = MX.sym("time", 1, 1) - nlp.dt_mx = MX.sym("dt", 1, 1) + nlp.time_cx = cx.sym("time", 1, 1) + nlp.dt = cx.sym("dt", 1, 1) nlp.initialize(cx) nlp.x_scaling = VariableScalingList() nlp.xdot_scaling = VariableScalingList() @@ -235,14 +235,14 @@ def test_torque_activation_driven_with_ligament(with_ligament, cx, phase_dynamic @pytest.mark.parametrize("with_ligament", [False, True]) def test_muscle_driven_with_ligament(with_ligament, cx, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(True if cx == SX else False)) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/muscle_driven_ocp/models/arm26_with_ligament.bioMod" ) nlp.ns = 5 nlp.cx = cx - nlp.time_mx = MX.sym("time", 1, 1) - nlp.dt_mx = MX.sym("dt", 1, 1) + nlp.time_cx = cx.sym("time", 1, 1) + nlp.dt = cx.sym("dt", 1, 1) nlp.initialize(cx) nlp.x_scaling = VariableScalingList() nlp.xdot_scaling = VariableScalingList() diff --git a/tests/shard3/test_passive_torque.py b/tests/shard3/test_passive_torque.py index 8c0a94003..1aa782ee4 100644 --- a/tests/shard3/test_passive_torque.py +++ b/tests/shard3/test_passive_torque.py @@ -30,7 +30,7 @@ def __init__(self, nlp, use_sx): self.n_phases = 1 self.nlp = [nlp] parameters_list = ParameterList(use_sx=use_sx) - self.parameters = ParameterContainer() + self.parameters = ParameterContainer(use_sx=use_sx) self.parameters.initialize(parameters_list) self.implicit_constraints = ConstraintList() self.n_threads = 1 @@ -42,14 +42,14 @@ def __init__(self, nlp, use_sx): @pytest.mark.parametrize("rigidbody_dynamics", [RigidBodyDynamics.ODE]) def test_torque_driven_with_passive_torque(with_passive_torque, cx, rigidbody_dynamics, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(True if cx == SX else False)) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod" ) nlp.ns = 5 nlp.cx = cx - nlp.time_mx = MX.sym("time", 1, 1) - nlp.dt_mx = MX.sym("dt", 1, 1) + nlp.time_cx = cx.sym("time", 1, 1) + nlp.dt = cx.sym("dt", 1, 1) nlp.initialize(cx) nlp.x_scaling = VariableScalingList() nlp.xdot_scaling = VariableScalingList() @@ -129,14 +129,14 @@ def test_torque_driven_with_passive_torque(with_passive_torque, cx, rigidbody_dy @pytest.mark.parametrize("with_passive_torque", [False, True]) def test_torque_derivative_driven_with_passive_torque(with_passive_torque, cx, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(True if cx == SX else False)) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod" ) nlp.ns = 5 nlp.cx = cx - nlp.time_mx = MX.sym("time", 1, 1) - nlp.dt_mx = MX.sym("dt", 1, 1) + nlp.time_cx = cx.sym("time", 1, 1) + nlp.dt = cx.sym("dt", 1, 1) nlp.initialize(cx) nlp.x_scaling = VariableScalingList() nlp.xdot_scaling = VariableScalingList() @@ -226,14 +226,14 @@ def test_torque_derivative_driven_with_passive_torque(with_passive_torque, cx, p @pytest.mark.parametrize("with_residual_torque", [False, True]) def test_torque_activation_driven_with_passive_torque(with_passive_torque, with_residual_torque, cx, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(True if cx == SX else False)) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod" ) nlp.ns = 5 nlp.cx = cx - nlp.time_mx = MX.sym("time", 1, 1) - nlp.dt_mx = MX.sym("dt", 1, 1) + nlp.time_cx = cx.sym("time", 1, 1) + nlp.dt = cx.sym("dt", 1, 1) nlp.initialize(cx) nlp.x_scaling = VariableScalingList() nlp.xdot_scaling = VariableScalingList() @@ -347,12 +347,12 @@ def test_torque_activation_driven_with_passive_torque(with_passive_torque, with_ @pytest.mark.parametrize("rigidbody_dynamics", [RigidBodyDynamics.ODE]) def test_muscle_driven_with_passive_torque(with_passive_torque, rigidbody_dynamics, cx, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(True if cx == SX else False)) nlp.model = BiorbdModel(TestUtils.bioptim_folder() + "/examples/muscle_driven_ocp/models/arm26_with_contact.bioMod") nlp.ns = 5 nlp.cx = cx - nlp.time_mx = MX.sym("time", 1, 1) - nlp.dt_mx = MX.sym("dt", 1, 1) + nlp.time_cx = cx.sym("time", 1, 1) + nlp.dt = cx.sym("dt", 1, 1) nlp.initialize(cx) nlp.x_scaling = VariableScalingList() nlp.xdot_scaling = VariableScalingList() diff --git a/tests/shard4/test_penalty.py b/tests/shard4/test_penalty.py index 0367e0e00..a397ff154 100644 --- a/tests/shard4/test_penalty.py +++ b/tests/shard4/test_penalty.py @@ -1263,7 +1263,7 @@ def test_penalty_constraint_total_time(value, phase_dynamics): @pytest.mark.parametrize("value", [0.1, -10]) def test_penalty_custom(penalty_origin, value, phase_dynamics): def custom(controller: PenaltyController, mult): - my_values = controller.q.cx_start * mult + my_values = controller.q * mult return my_values ocp = prepare_test_ocp(phase_dynamics=phase_dynamics) @@ -1346,7 +1346,7 @@ def custom_with_mult(ocp, nlp, t, x, u, p, mult): @pytest.mark.parametrize("value", [0.1, -10]) def test_penalty_custom_with_bounds(value, phase_dynamics): def custom_with_bounds(controller: PenaltyController): - return -10, controller.q.cx_start, 10 + return -10, controller.q, 10 ocp = prepare_test_ocp(phase_dynamics=phase_dynamics) t = [0] @@ -1369,7 +1369,7 @@ def custom_with_bounds(controller: PenaltyController): @pytest.mark.parametrize("value", [0.1, -10]) def test_penalty_custom_with_bounds_failing_min_bound(value, phase_dynamics): def custom_with_bounds(controller: PenaltyController): - return -10, controller.q.cx_start, 10 + return -10, controller.q, 10 ocp = prepare_test_ocp(phase_dynamics=phase_dynamics) t = [0] @@ -1392,7 +1392,7 @@ def custom_with_bounds(controller: PenaltyController): @pytest.mark.parametrize("value", [0.1, -10]) def test_penalty_custom_with_bounds_failing_max_bound(value, phase_dynamics): def custom_with_bounds(controller: PenaltyController): - return -10, controller.q.cx_start, 10 + return -10, controller.q, 10 ocp = prepare_test_ocp(phase_dynamics=phase_dynamics) t = [0] @@ -1418,7 +1418,7 @@ def custom_with_bounds(controller: PenaltyController): @pytest.mark.parametrize("node", [*Node, 2]) @pytest.mark.parametrize("ns", [3, 10, 11]) def test_PenaltyFunctionAbstract_get_node(node, ns, phase_dynamics): - nlp = NLP(phase_dynamics=phase_dynamics) + nlp = NLP(phase_dynamics=phase_dynamics, use_sx=False) nlp.control_type = ControlType.CONSTANT nlp.ns = ns nlp.X = np.linspace(0, -10, ns + 1) diff --git a/tests/shard4/test_variational_biorbd_model.py b/tests/shard4/test_variational_biorbd_model.py index 21381d1c6..2a832ab70 100644 --- a/tests/shard4/test_variational_biorbd_model.py +++ b/tests/shard4/test_variational_biorbd_model.py @@ -23,7 +23,7 @@ def test_variational_model(): q = MX([3.0, 4.0]) qdot = MX([1.0, 2.0]) - TestUtils.assert_equal(model.lagrangian(q, qdot), -4.34239126) + TestUtils.assert_equal(model.lagrangian()(q, qdot), -4.34239126) time_step = MX(0.5) From 55e89832666a511966a940360730b828bf829957 Mon Sep 17 00:00:00 2001 From: eve-mac Date: Sat, 21 Sep 2024 09:12:34 -0400 Subject: [PATCH 019/108] blacked --- bioptim/limits/constraints.py | 19 ++++++++++---- bioptim/limits/multinode_penalty.py | 4 ++- bioptim/limits/penalty.py | 8 +++--- .../models/biorbd/holonomic_biorbd_model.py | 9 ++++--- .../models/biorbd/stochastic_biorbd_model.py | 26 +++++++++++-------- .../models/biorbd/variational_biorbd_model.py | 4 ++- 6 files changed, 44 insertions(+), 26 deletions(-) diff --git a/bioptim/limits/constraints.py b/bioptim/limits/constraints.py index 3534d3bbc..38bd8363e 100644 --- a/bioptim/limits/constraints.py +++ b/bioptim/limits/constraints.py @@ -406,7 +406,9 @@ def qddot_equals_forward_dynamics( Since the function does nothing, we can safely ignore any argument """ - passive_torque = controller.model.passive_joint_torque()(controller.q, controller.qdot, controller.parameters.cx) + passive_torque = controller.model.passive_joint_torque()( + controller.q, controller.qdot, controller.parameters.cx + ) tau = controller.states["tau"].cx if "tau" in controller.states else controller.tau tau = tau + passive_torque if with_passive_torque else tau tau = ( @@ -453,7 +455,9 @@ def tau_equals_inverse_dynamics( tau = controller.states["tau"].cx if "tau" in controller.states else controller.tau qddot = controller.states["qddot"].cx if "qddot" in controller.states else controller.controls["qddot"].cx - passive_torque = controller.model.passive_joint_torque()(controller.q, controller.qdot, controller.parameters.cx) + passive_torque = controller.model.passive_joint_torque()( + controller.q, controller.qdot, controller.parameters.cx + ) tau = tau + passive_torque if with_passive_torque else tau tau = ( tau + controller.model.ligament_joint_torque()(controller.q, controller.qdot, controller.parameters.cx) @@ -533,13 +537,18 @@ def tau_from_muscle_equal_inverse_dynamics( muscle_activations = controller.controls["muscles"].cx muscles_states = controller.model.state_set() - passive_torque = controller.model.passive_joint_torque()(controller.q, controller.qdot, controller.parameters.cx) + passive_torque = controller.model.passive_joint_torque()( + controller.q, controller.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, controller.q, controller.qdot, controller.parameters.cx) + muscle_tau = controller.model.muscle_joint_torque( + muscles_states, controller.q, controller.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(controller.q, controller.qdot, controller.parameters.cx) + muscle_tau + + controller.model.ligament_joint_torque(controller.q, controller.qdot, controller.parameters.cx) if with_ligament else muscle_tau ) diff --git a/bioptim/limits/multinode_penalty.py b/bioptim/limits/multinode_penalty.py index 7355acd63..ffc202b22 100644 --- a/bioptim/limits/multinode_penalty.py +++ b/bioptim/limits/multinode_penalty.py @@ -271,7 +271,9 @@ def com_equality(penalty, controllers: list[PenaltyController]): out = controllers[0].cx.zeros((3, 1)) for i in range(1, len(controllers)): - com_i = controllers[i].model.center_of_mass()(controllers[i].states["q"].cx, controllers[i].parameters.cx) + com_i = controllers[i].model.center_of_mass()( + controllers[i].states["q"].cx, controllers[i].parameters.cx + ) out += com_0 - com_i return out diff --git a/bioptim/limits/penalty.py b/bioptim/limits/penalty.py index 395e9feed..e8f200217 100644 --- a/bioptim/limits/penalty.py +++ b/bioptim/limits/penalty.py @@ -345,7 +345,9 @@ def minimize_markers_velocity( # Add the penalty in the requested reference frame. None for global markers = horzcat( - *controller.model.marker_velocities(reference_index=reference_jcs)(controller.q, controller.qdot, controller.parameters.cx) + *controller.model.marker_velocities(reference_index=reference_jcs)( + controller.q, controller.qdot, controller.parameters.cx + ) ) return markers @@ -656,9 +658,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, controller.qdot, controller.parameters.cx - ) + return controller.model.center_of_mass_velocity()(controller.q, controller.qdot, controller.parameters.cx) @staticmethod def minimize_com_acceleration(penalty: PenaltyOption, controller: PenaltyController, axes: tuple | list = None): diff --git a/bioptim/models/biorbd/holonomic_biorbd_model.py b/bioptim/models/biorbd/holonomic_biorbd_model.py index fbcddad60..e199b32ef 100644 --- a/bioptim/models/biorbd/holonomic_biorbd_model.py +++ b/bioptim/models/biorbd/holonomic_biorbd_model.py @@ -17,10 +17,11 @@ class HolonomicBiorbdModel(BiorbdModel): This class allows to define a biorbd model with custom holonomic constraints. """ - def __init__(self, - bio_model: str | biorbd.Model, - parameters: ParameterList = None, - ): + def __init__( + self, + bio_model: str | biorbd.Model, + parameters: ParameterList = None, + ): super().__init__(bio_model, parameters=parameters) self._newton_tol = 1e-10 self._holonomic_constraints = [] diff --git a/bioptim/models/biorbd/stochastic_biorbd_model.py b/bioptim/models/biorbd/stochastic_biorbd_model.py index a5676f0ed..6aa51759b 100644 --- a/bioptim/models/biorbd/stochastic_biorbd_model.py +++ b/bioptim/models/biorbd/stochastic_biorbd_model.py @@ -56,17 +56,21 @@ def __init__( ): if parameters is None: parameters = ParameterList(use_sx=use_sx) - parameters.add("motor_noise", - lambda model, param: None, - size=motor_noise_magnitude.shape[0], - scaling=VariableScaling("motor_noise", [1.0] * motor_noise_magnitude.shape[0])) - parameters.add("sensory_noise", - lambda model, param: None, - size=sensory_noise_magnitude.shape[0], - scaling=VariableScaling("sensory_noise", [1.0] * sensory_noise_magnitude.shape[0])) - super().__init__(bio_model=(bio_model if isinstance(bio_model, str) else bio_model.model), - parameters=parameters, - **kwargs) + parameters.add( + "motor_noise", + lambda model, param: None, + size=motor_noise_magnitude.shape[0], + scaling=VariableScaling("motor_noise", [1.0] * motor_noise_magnitude.shape[0]), + ) + parameters.add( + "sensory_noise", + lambda model, param: None, + size=sensory_noise_magnitude.shape[0], + scaling=VariableScaling("sensory_noise", [1.0] * sensory_noise_magnitude.shape[0]), + ) + super().__init__( + bio_model=(bio_model if isinstance(bio_model, str) else bio_model.model), parameters=parameters, **kwargs + ) self.motor_noise_magnitude = motor_noise_magnitude self.sensory_noise_magnitude = sensory_noise_magnitude diff --git a/bioptim/models/biorbd/variational_biorbd_model.py b/bioptim/models/biorbd/variational_biorbd_model.py index f30e6f7b6..c4917d8e0 100644 --- a/bioptim/models/biorbd/variational_biorbd_model.py +++ b/bioptim/models/biorbd/variational_biorbd_model.py @@ -319,7 +319,9 @@ def compute_final_states( CX = MX if isinstance(q_penultimate, MX) else SX # Refers to D_2 L(q_N, \dot{q_N}) (D_2 is the partial derivative with respect to the second argument) - d2_l_q_ultimate_qdot_ultimate = transpose(jacobian(self.lagrangian()(q_ultimate, q_dot_ultimate), q_dot_ultimate)) + d2_l_q_ultimate_qdot_ultimate = transpose( + jacobian(self.lagrangian()(q_ultimate, q_dot_ultimate), q_dot_ultimate) + ) # Refers to D_2 L_d(q_{n-1}, q_1) (Ld is the discrete Lagrangian) d2_ld_q_penultimate_q_ultimate = transpose( jacobian(self.discrete_lagrangian(q_penultimate, q_ultimate, time_step), q_ultimate) From 7eefd9bb5c9b17c878e0c2c9da3cb078dbc54efa Mon Sep 17 00:00:00 2001 From: eve-mac Date: Sat, 21 Sep 2024 09:49:30 -0400 Subject: [PATCH 020/108] removed .mx from optimization_variables (seems to work) --- bioptim/dynamics/configure_new_variable.py | 120 +++++++++--------- bioptim/limits/penalty_controller.py | 4 +- bioptim/optimization/optimization_variable.py | 52 ++------ bioptim/optimization/parameters.py | 11 +- 4 files changed, 85 insertions(+), 102 deletions(-) diff --git a/bioptim/dynamics/configure_new_variable.py b/bioptim/dynamics/configure_new_variable.py index 424dbeaaa..733e14a7f 100644 --- a/bioptim/dynamics/configure_new_variable.py +++ b/bioptim/dynamics/configure_new_variable.py @@ -120,11 +120,11 @@ def __init__( self.copy_states_dot = False self.copy_controls = False - # todo: Charbie - self.mx_states = None - self.mx_states_dot = None - self.mx_controls = None - self.mx_algebraic_states = None + # # todo: Charbie + # self.mx_states = None + # self.mx_states_dot = None + # self.mx_controls = None + # self.mx_algebraic_states = None self._check_combine_state_control_plot() @@ -140,7 +140,7 @@ def __init__( self._declare_initial_guess() self._declare_variable_scaling() - self._use_copy() + # self._use_copy() # plot self.legend = None @@ -329,53 +329,53 @@ def _declare_variable_scaling(self): self.name, scaling=np.ones(len(self.nlp.variable_mappings[self.name].to_first.map_idx)) ) - def _use_copy(self): - """Use of states[0] and controls[0] is permitted since nlp.phase_dynamics - is PhaseDynamics.SHARED_DURING_THE_PHASE""" - # todo: Charbie - self.mx_states = ( - [] if not self.copy_states else [self.ocp.nlp[self.nlp.use_states_from_phase_idx].states[0][self.name].mx] - ) - self.mx_states_dot = ( - [] - if not self.copy_states_dot - else [self.ocp.nlp[self.nlp.use_states_dot_from_phase_idx].states_dot[0][self.name].mx] - ) - self.mx_controls = ( - [] - if not self.copy_controls - else [self.ocp.nlp[self.nlp.use_controls_from_phase_idx].controls[0][self.name].mx] - ) - self.mx_algebraic_states = ( - [] - if not self.copy_algebraic_states - else [self.ocp.nlp[self.nlp.use_states_from_phase_idx].algebraic_states[0][self.name].mx] - ) - - # todo: if mapping on variables, what do we do with mapping on the nodes - for i in self.nlp.variable_mappings[self.name].to_second.map_idx: - var_name = ( - f"{'-' if np.sign(i) < 0 else ''}{self.name}_{self.name_elements[abs(i)]}_MX" - if i is not None - else "zero" - ) - - if not self.copy_states: - self.mx_states.append(MX.sym(var_name, 1, 1)) - - if not self.copy_states_dot: - self.mx_states_dot.append(MX.sym(var_name, 1, 1)) - - if not self.copy_controls: - self.mx_controls.append(MX.sym(var_name, 1, 1)) - - self.mx_algebraic_states.append(MX.sym(var_name, 1, 1)) - - # todo: Charbie - self.mx_states = vertcat(*self.mx_states) - self.mx_states_dot = vertcat(*self.mx_states_dot) - self.mx_controls = vertcat(*self.mx_controls) - self.mx_algebraic_states = vertcat(*self.mx_algebraic_states) + # def _use_copy(self): + # """Use of states[0] and controls[0] is permitted since nlp.phase_dynamics + # is PhaseDynamics.SHARED_DURING_THE_PHASE""" + # # todo: Charbie + # self.mx_states = ( + # [] if not self.copy_states else [self.ocp.nlp[self.nlp.use_states_from_phase_idx].states[0][self.name].mx] + # ) + # self.mx_states_dot = ( + # [] + # if not self.copy_states_dot + # else [self.ocp.nlp[self.nlp.use_states_dot_from_phase_idx].states_dot[0][self.name].mx] + # ) + # self.mx_controls = ( + # [] + # if not self.copy_controls + # else [self.ocp.nlp[self.nlp.use_controls_from_phase_idx].controls[0][self.name].mx] + # ) + # self.mx_algebraic_states = ( + # [] + # if not self.copy_algebraic_states + # else [self.ocp.nlp[self.nlp.use_states_from_phase_idx].algebraic_states[0][self.name].mx] + # ) + # + # # todo: if mapping on variables, what do we do with mapping on the nodes + # for i in self.nlp.variable_mappings[self.name].to_second.map_idx: + # var_name = ( + # f"{'-' if np.sign(i) < 0 else ''}{self.name}_{self.name_elements[abs(i)]}_MX" + # if i is not None + # else "zero" + # ) + # + # if not self.copy_states: + # self.mx_states.append(MX.sym(var_name, 1, 1)) + # + # if not self.copy_states_dot: + # self.mx_states_dot.append(MX.sym(var_name, 1, 1)) + # + # if not self.copy_controls: + # self.mx_controls.append(MX.sym(var_name, 1, 1)) + # + # self.mx_algebraic_states.append(MX.sym(var_name, 1, 1)) + # + # # todo: Charbie + # self.mx_states = vertcat(*self.mx_states) + # self.mx_states_dot = vertcat(*self.mx_states_dot) + # self.mx_controls = vertcat(*self.mx_controls) + # self.mx_algebraic_states = vertcat(*self.mx_algebraic_states) def _declare_auto_axes_idx(self): """Declare the axes index if not already declared""" @@ -415,7 +415,7 @@ def _declare_cx_and_plot(self): self.name, cx[0], cx_scaled[0], - self.mx_states, + None, # self.mx_states, self.nlp.variable_mappings[self.name], node_index, ) @@ -450,7 +450,7 @@ def _declare_cx_and_plot(self): self.name, cx[0], cx_scaled[0], - self.mx_controls, + None, # self.mx_controls, self.nlp.variable_mappings[self.name], node_index, ) @@ -492,7 +492,7 @@ def _declare_cx_and_plot(self): self.name, cx[0], cx_scaled[0], - self.mx_states_dot, + None, # self.mx_states_dot, self.nlp.variable_mappings[self.name], node_index, ) @@ -517,7 +517,7 @@ def _declare_cx_and_plot(self): self.name, cx[0], cx_scaled[0], - self.mx_states, + None, # self.mx_states, self.nlp.variable_mappings[self.name], node_index, ) @@ -689,13 +689,13 @@ def append_faked_optim_var(name: str, optim_var, keys: list): """ index = [] - mx = MX() + # mx = MX() to_second = [] to_first = [] for key in keys: index.extend(list(optim_var[key].index)) - mx = vertcat(mx, optim_var[key].mx) + # mx = vertcat(mx, optim_var[key].mx) to_second.extend(list(np.array(optim_var[key].mapping.to_second.map_idx) + len(to_second))) to_first.extend(list(np.array(optim_var[key].mapping.to_first.map_idx) + len(to_first))) - optim_var.append_fake(name, index, mx, BiMapping(to_second, to_first)) + optim_var.append_fake(name, index, None, BiMapping(to_second, to_first)) diff --git a/bioptim/limits/penalty_controller.py b/bioptim/limits/penalty_controller.py index 999806950..4cb2deadb 100644 --- a/bioptim/limits/penalty_controller.py +++ b/bioptim/limits/penalty_controller.py @@ -129,12 +129,12 @@ def t_span(self) -> OptimizationVariable: ------- """ - mx = vertcat(self.time.mx, self.dt.mx) + # mx = vertcat(self.time.mx, self.dt.mx) cx = vertcat(self.time.cx, self.dt.cx) tp = OptimizationVariableList(self._nlp.cx, self._nlp.phase_dynamics == PhaseDynamics.SHARED_DURING_THE_PHASE) n_val = cx.shape[0] - tp.append("t_span", mx=mx, cx=[cx, cx, cx], bimapping=BiMapping(to_second=range(n_val), to_first=range(n_val))) + tp.append("t_span", mx=None, cx=[cx, cx, cx], bimapping=BiMapping(to_second=range(n_val), to_first=range(n_val))) return tp["t_span"] @property diff --git a/bioptim/optimization/optimization_variable.py b/bioptim/optimization/optimization_variable.py index cb7133d7a..64c5765f8 100644 --- a/bioptim/optimization/optimization_variable.py +++ b/bioptim/optimization/optimization_variable.py @@ -15,8 +15,8 @@ class OptimizationVariable: ---------- name: str The name of the variable - mx: MX - The MX variable associated with this variable + cx_start: MX | SX + The symbolic variable associated with this variable index: range The indices to find this variable mapping: BiMapping @@ -37,7 +37,7 @@ class OptimizationVariable: def __init__( self, name: str, - mx: MX, + mx: MX, # remove Charbie cx_start: list | None, index: range | list, mapping: BiMapping = None, @@ -48,15 +48,15 @@ def __init__( ---------- name: str The name of the variable - mx: MX - The MX variable associated with this variable + cx_start: MX | SX + The symbolic variable associated with this variable index: range | list The indices to find this variable parent_list: OptimizationVariableList The list the OptimizationVariable is in """ self.name: str = name - self.mx: MX = mx + # self.mx: MX = mx self.original_cx: list = cx_start self.index: range | list = index self.mapping: BiMapping = mapping @@ -159,8 +159,6 @@ class OptimizationVariableList: The symbolic MX or SX of the list (mid point) _cx_end: MX | SX The symbolic MX or SX of the list (ending point) - mx_reduced: MX - The reduced MX to the size of _cx cx_constructor: Callable The casadi symbolic type used for the optimization (MX or SX) @@ -174,8 +172,6 @@ class OptimizationVariableList: The cx of all elements together (starting point) cx_end(self) The cx of all elements together (ending point) - mx(self) - The MX of all variable concatenated together shape(self) The size of the CX __len__(self) @@ -214,7 +210,7 @@ def __getitem__(self, item: int | str | list | range): index = [] for elt in self.elements: index.extend(list(elt.index)) - return OptimizationVariable("all", self.mx, self.cx_start, index, None, self) + return OptimizationVariable("all", None, self.cx_start, index, None, self) for elt in self.elements: if item == elt.name: @@ -224,12 +220,11 @@ def __getitem__(self, item: int | str | list | range): return elt raise KeyError(f"{item} is not in the list") elif isinstance(item, (list, tuple)) or isinstance(item, range): - mx = vertcat([elt.mx for elt in self.elements if elt.name in item]) index = [] for elt in self.elements: if elt.name in item: index.extend(list(elt.index)) - return OptimizationVariable("some", mx, None, index) + return OptimizationVariable("some", None, None, index) else: raise ValueError("OptimizationVariableList can be sliced with int, list, range or str only") @@ -272,13 +267,11 @@ def append_fake(self, name: str, index: MX | SX | list, mx: MX, bimapping: BiMap The name of the variable index: MX | SX The SX or MX variable associated with this variable. Is interpreted as index if is_fake is true - mx: MX - The MX variable associated with this variable bimapping: BiMapping The Mapping of the MX against CX """ - self.fake_elements.append(OptimizationVariable(name, mx, None, index, bimapping, self)) + self.fake_elements.append(OptimizationVariable(name, None, None, index, bimapping, self)) def append(self, name: str, cx: list, mx: MX, bimapping: BiMapping): """ @@ -290,8 +283,6 @@ def append(self, name: str, cx: list, mx: MX, bimapping: BiMapping): The name of the variable cx: list The list of SX or MX variable associated with this variable - mx: MX - The MX variable associated with this variable """ if len(cx) < 2: @@ -309,7 +300,7 @@ def append(self, name: str, cx: list, mx: MX, bimapping: BiMapping): else: self._cx_intermediates[i] = vertcat(self._cx_intermediates[i], c) - self.elements.append(OptimizationVariable(name, mx, cx, index, bimapping, parent_list=self)) + self.elements.append(OptimizationVariable(name, None, cx, index, bimapping, parent_list=self)) def append_from_scaled( self, @@ -345,7 +336,7 @@ def append_from_scaled( self._cx_intermediates[i] = vertcat(self._cx_intermediates[i], c) var = scaled_optimization_variable[name] - self.elements.append(OptimizationVariable(name, var.mx, cx, var.index, var.mapping, self)) + self.elements.append(OptimizationVariable(name, None, cx, var.index, var.mapping, self)) @property def cx(self): @@ -406,16 +397,6 @@ def cx_intermediates_list(self): return self._cx_intermediates - @property - def mx(self): - """ - Returns - ------- - The MX of all variable concatenated together - """ - # todo Charbie remove - return vertcat(*[elt.mx for elt in self.elements]) - def __contains__(self, item: str): """ If the item of name item is in the list @@ -557,11 +538,6 @@ def key_index(self, key): def shape(self): return self._unscaled[0].shape - @property - def mx(self): - # todo Charbie remove - return self.unscaled.mx - @property def cx(self): return self.unscaled.cx @@ -583,7 +559,7 @@ def append( name: str, cx: list, cx_scaled: list, - mx: MX, + mx: MX, # remove Charbie mapping: BiMapping, node_index: int, ): @@ -598,14 +574,12 @@ def append( The list of unscaled SX or MX variable associated with this variable cx_scaled: list The list of scaled SX or MX variable associated with this variable - mx: MX - The symbolic variable associated to this variable mapping The mapping to apply to the unscaled variable node_index The index of the node for the scaled variable """ - self._scaled[node_index].append(name, cx_scaled, mx, mapping) + self._scaled[node_index].append(name, cx_scaled, None, mapping) self._unscaled[node_index].append_from_scaled(name, cx, self._scaled[node_index]) def __contains__(self, item: str): diff --git a/bioptim/optimization/parameters.py b/bioptim/optimization/parameters.py index 18b9e960d..f6b7e3d38 100644 --- a/bioptim/optimization/parameters.py +++ b/bioptim/optimization/parameters.py @@ -62,6 +62,11 @@ def __init__( self.scaling = scaling self.index = index self.kwargs = kwargs + self._mx = mx + + @property + def mx(self): + return self._mx @property def cx(self): @@ -100,7 +105,6 @@ def apply_parameter(self, model): 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): """ A list of Parameters. @@ -331,3 +335,8 @@ def cx_mid(self): @property def cx_end(self): raise RuntimeError("cx_end is not available for parameters, only cx_start is accepted.") + + @property + def mx(self): + return self.unscaled.mx + From d58bf8f5fb4c370288bcd4ddba474447c3342359 Mon Sep 17 00:00:00 2001 From: eve-mac Date: Sat, 21 Sep 2024 10:22:34 -0400 Subject: [PATCH 021/108] time consuming time parameter --' --- bioptim/dynamics/configure_problem.py | 2 +- bioptim/dynamics/dynamics_functions.py | 58 +++++++++---------- .../getting_started/custom_constraint.py | 2 +- .../getting_started/custom_dynamics.py | 2 +- .../getting_started/custom_objectives.py | 2 +- .../example_continuity_as_objective.py | 2 +- .../arm_reaching_muscle_driven.py | 2 +- ...imize_maximum_torque_by_extra_parameter.py | 12 ++-- bioptim/limits/constraints.py | 24 ++++---- bioptim/limits/multinode_penalty.py | 8 +-- bioptim/limits/penalty.py | 50 ++++++++-------- bioptim/limits/penalty_controller.py | 19 ++++-- bioptim/limits/phase_transition.py | 2 +- bioptim/optimization/non_linear_program.py | 2 + .../optimization/optimal_control_program.py | 14 +++-- tests/shard3/test_graph.py | 2 +- 16 files changed, 111 insertions(+), 92 deletions(-) diff --git a/bioptim/dynamics/configure_problem.py b/bioptim/dynamics/configure_problem.py index e380b009c..f8e943d5e 100644 --- a/bioptim/dynamics/configure_problem.py +++ b/bioptim/dynamics/configure_problem.py @@ -1353,7 +1353,7 @@ def configure_soft_contact_function(ocp, nlp): q = nlp.states["q"].mapping.to_second.map(nlp.states["q"].cx) qdot = nlp.states["qdot"].mapping.to_second.map(nlp.states["qdot"].cx) - global_soft_contact_force_func = nlp.model.soft_contact_forces()(q, qdot, nlp.parameters.cx) + global_soft_contact_force_func = nlp.model.soft_contact_forces()(q, qdot, nlp.parameters_except_time.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 eb560f97c..d2d9833fa 100644 --- a/bioptim/dynamics/dynamics_functions.py +++ b/bioptim/dynamics/dynamics_functions.py @@ -145,8 +145,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, 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.passive_joint_torque()(q, qdot, nlp.parameters_except_time.cx) if with_passive_torque else tau + tau = tau + nlp.model.ligament_joint_torque()(q, qdot, nlp.parameters_except_time.cx) if with_ligament else tau tau = tau - nlp.model.friction_coefficients @ qdot if with_friction else tau if ( @@ -258,7 +258,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, nlp.parameters.cx) + tau_joints + nlp.model.passive_joint_torque()(q_full, qdot_full, nlp.parameters_except_time.cx) if with_passive_torque 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, nlp.parameters.cx) + tau = nlp.model.torque()(tau_activation, q, qdot, nlp.parameters_except_time.cx) if with_passive_torque: - tau += nlp.model.passive_joint_torque()(q, qdot, nlp.parameters.cx) + tau += nlp.model.passive_joint_torque()(q, qdot, nlp.parameters_except_time.cx) if with_residual_torque: tau += DynamicsFunctions.get(nlp.controls["residual_tau"], controls) if with_ligament: - tau += nlp.model.ligament_joint_torque()(q, qdot, nlp.parameters.cx) + tau += nlp.model.ligament_joint_torque()(q, qdot, nlp.parameters_except_time.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, 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.passive_joint_torque()(q, qdot, nlp.parameters_except_time.cx) if with_passive_torque else tau + tau = tau + nlp.model.ligament_joint_torque()(q, qdot, nlp.parameters_except_time.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, 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.passive_joint_torque()(q, qdot, nlp.parameters_except_time.cx) if with_passive_torque else tau + tau = tau + nlp.model.ligament_joint_torque()(q, qdot, nlp.parameters_except_time.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, nlp.parameters.cx) + return nlp.model.contact_forces()(q, qdot, tau, external_forces, nlp.parameters_except_time.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, 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 + tau = nlp.model.torque()(tau_activations, q, qdot, nlp.parameters_except_time.cx) + tau = tau + nlp.model.passive_joint_torque()(q, qdot, nlp.parameters_except_time.cx) if with_passive_torque else tau + tau = tau + nlp.model.ligament_joint_torque()(q, qdot, nlp.parameters_except_time.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, nlp.parameters.cx) + return nlp.model.contact_forces()(q, qdot, tau, external_forces, nlp.parameters_except_time.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, 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.passive_joint_torque()(q, qdot, nlp.parameters_except_time.cx) if with_passive_torque else tau + tau = tau + nlp.model.ligament_joint_torque()(q, qdot, nlp.parameters_except_time.cx) if with_ligament else tau dq = DynamicsFunctions.compute_qdot(nlp, q, qdot) @@ -932,11 +932,11 @@ 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, 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.passive_joint_torque()(q, qdot, nlp.parameters_except_time.cx) if with_passive_torque else tau + tau = tau + nlp.model.ligament_joint_torque()(q, qdot, nlp.parameters_except_time.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, nlp.parameters.cx) + return nlp.model.contact_forces()(q, qdot, tau, external_forces, nlp.parameters_except_time.cx) @staticmethod def joints_acceleration_driven( @@ -983,7 +983,7 @@ def joints_acceleration_driven( qdot = nlp.get_var_from_states_or_controls("qdot", states, controls) qddot_joints = nlp.get_var_from_states_or_controls("qddot", states, controls) - qddot_root = nlp.model.forward_dynamics_free_floating_base()(q, qdot, qddot_joints, nlp.parameters.cx) + qddot_root = nlp.model.forward_dynamics_free_floating_base()(q, qdot, qddot_joints, nlp.parameters_except_time.cx) qddot_reordered = nlp.model.reorder_qddot_root_joints(qddot_root, qddot_joints) qdot_mapped = nlp.variable_mappings["qdot"].to_first.map(qdot) @@ -1072,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, nlp.parameters.cx)) + return mapping.to_first.map(nlp.model.reshape_qdot()(q, qdot, nlp.parameters_except_time.cx)) @staticmethod def forward_dynamics( @@ -1113,11 +1113,11 @@ 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, [], nlp.parameters.cx) + qddot = nlp.model.forward_dynamics(with_contact=with_contact)(q, qdot, tau, [], nlp.parameters_except_time.cx) return qdot_var_mapping.map(qddot) else: qddot = nlp.model.forward_dynamics(with_contact=with_contact)( - q, qdot, tau, external_forces, nlp.parameters.cx + q, qdot, tau, external_forces, nlp.parameters_except_time.cx ) return qdot_var_mapping.map(qddot) @@ -1154,7 +1154,7 @@ def inverse_dynamics( """ if external_forces is None: - tau = nlp.model.inverse_dynamics(with_contact=with_contact)(q, qdot, qddot, [], nlp.parameters.cx) + tau = nlp.model.inverse_dynamics(with_contact=with_contact)(q, qdot, qddot, [], nlp.parameters_except_time.cx) else: if "tau" in nlp.states: tau_shape = nlp.states["tau"].cx.shape[0] @@ -1165,7 +1165,7 @@ def inverse_dynamics( 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], nlp.parameters.cx + q, qdot, qddot, external_forces[:, i], nlp.parameters_except_time.cx ) return tau # We ignore on purpose the mapping to keep zeros in the defects of the dynamic. @@ -1186,7 +1186,7 @@ def compute_muscle_dot(nlp, muscle_excitations: MX | SX): The derivative of muscle activations """ - return nlp.model.muscle_activation_dot()(muscle_excitations, nlp.parameters.cx) + return nlp.model.muscle_activation_dot()(muscle_excitations, nlp.parameters_except_time.cx) @staticmethod def compute_tau_from_muscle( @@ -1223,7 +1223,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, nlp.parameters.cx) + return nlp.model.muscle_joint_torque()(activations, q, qdot, nlp.parameters_except_time.cx) @staticmethod def holonomic_torque_driven( @@ -1266,6 +1266,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, nlp.parameters.cx) + qddot_u = nlp.model.partitioned_forward_dynamics(q_u, qdot_u, tau, external_forces, nlp.parameters_except_time.cx) return DynamicsEvaluation(dxdt=vertcat(qdot_u, qddot_u), defects=None) diff --git a/bioptim/examples/getting_started/custom_constraint.py b/bioptim/examples/getting_started/custom_constraint.py index 4f20b822f..3d07f2171 100644 --- a/bioptim/examples/getting_started/custom_constraint.py +++ b/bioptim/examples/getting_started/custom_constraint.py @@ -53,7 +53,7 @@ def custom_func_track_markers(controller: PenaltyController, first_marker: str, marker_1_idx = controller.model.marker_index(second_marker) # compute the position of the markers using the markers function from the BioModel (here a BiorbdModel) - markers = controller.model.markers()(controller.states["q"].cx, controller.parameters.cx) + markers = controller.model.markers()(controller.states["q"].cx, controller.parameters_except_time.cx) markers_diff = markers[marker_1_idx] - markers[marker_0_idx] return markers_diff diff --git a/bioptim/examples/getting_started/custom_dynamics.py b/bioptim/examples/getting_started/custom_dynamics.py index 2e53a5a30..8e68ac9a0 100644 --- a/bioptim/examples/getting_started/custom_dynamics.py +++ b/bioptim/examples/getting_started/custom_dynamics.py @@ -74,7 +74,7 @@ def custom_dynamics( # You can directly call biorbd function (as for ddq) or call bioptim accessor (as for dq) dq = DynamicsFunctions.compute_qdot(nlp, q, qdot) * my_additional_factor - ddq = nlp.model.forward_dynamics()(q, qdot, tau, [], nlp.parameters.cx) + ddq = nlp.model.forward_dynamics()(q, qdot, tau, [], nlp.parameters_except_time.cx) # the user has to choose if want to return the explicit dynamics dx/dt = f(x,u,p) # as the first argument of DynamicsEvaluation or diff --git a/bioptim/examples/getting_started/custom_objectives.py b/bioptim/examples/getting_started/custom_objectives.py index d26403b22..acf9d08fd 100644 --- a/bioptim/examples/getting_started/custom_objectives.py +++ b/bioptim/examples/getting_started/custom_objectives.py @@ -52,7 +52,7 @@ def custom_func_track_markers(controller: PenaltyController, first_marker: str, marker_1_idx = controller.model.marker_index(second_marker) # Convert the function to the required format and then subtract - markers = controller.model.markers()(controller.states["q"].cx, controller.parameters.cx) + markers = controller.model.markers()(controller.states["q"].cx, controller.parameters_except_time.cx) markers_diff = markers[marker_1_idx] - markers[marker_0_idx] return markers_diff diff --git a/bioptim/examples/getting_started/example_continuity_as_objective.py b/bioptim/examples/getting_started/example_continuity_as_objective.py index 63dc972ed..55a28e1ec 100644 --- a/bioptim/examples/getting_started/example_continuity_as_objective.py +++ b/bioptim/examples/getting_started/example_continuity_as_objective.py @@ -44,7 +44,7 @@ def out_of_sphere(controller: PenaltyController, y, z): q = controller.states["q"].cx - marker_q = controller.model.markers()(q, controller.parameters.cx)[1] + marker_q = controller.model.markers()(q, controller.parameters_except_time.cx)[1] distance = sqrt((y - marker_q[1]) ** 2 + (z - marker_q[2]) ** 2) diff --git a/bioptim/examples/stochastic_optimal_control/arm_reaching_muscle_driven.py b/bioptim/examples/stochastic_optimal_control/arm_reaching_muscle_driven.py index 4e5279db0..92833482b 100644 --- a/bioptim/examples/stochastic_optimal_control/arm_reaching_muscle_driven.py +++ b/bioptim/examples/stochastic_optimal_control/arm_reaching_muscle_driven.py @@ -222,7 +222,7 @@ def get_cov_mat(nlp, node_index): p_next = m_matrix @ (dg_dx @ cov_matrix @ dg_dx.T + dg_dw @ sigma_w @ dg_dw.T) @ m_matrix.T - parameters = nlp.parameters.cx + parameters = nlp.parameters_except_time.cx parameters[nlp.parameters["sensory_noise"].index] = nlp.model.sensory_noise_magnitude parameters[nlp.parameters["motor_noise"].index] = nlp.model.motor_noise_magnitude diff --git a/bioptim/examples/torque_driven_ocp/minimize_maximum_torque_by_extra_parameter.py b/bioptim/examples/torque_driven_ocp/minimize_maximum_torque_by_extra_parameter.py index cd8096609..efc2c2d60 100644 --- a/bioptim/examples/torque_driven_ocp/minimize_maximum_torque_by_extra_parameter.py +++ b/bioptim/examples/torque_driven_ocp/minimize_maximum_torque_by_extra_parameter.py @@ -47,17 +47,12 @@ def prepare_ocp( method_bound_states: int = 0, bio_model_path: str = "models/double_pendulum.bioMod", ) -> OptimalControlProgram: - bio_model = BiorbdModel(bio_model_path) # Problem parameters n_shooting = 50 final_time = 1 tau_min, tau_max, tau_init = -10, 10, 0 - # Mapping - tau_mappings = BiMappingList() - tau_mappings.add("tau", to_second=[None, 0], to_first=[1]) - # Define the parameter to optimize parameters = ParameterList(use_sx=False) parameter_init = InitialGuessList() @@ -77,6 +72,13 @@ def prepare_ocp( parameter_objectives.add(ObjectiveFcn.Parameter.MINIMIZE_PARAMETER, key="max_tau", weight=10, quadratic=True) parameter_objectives.add(ObjectiveFcn.Parameter.MINIMIZE_PARAMETER, key="min_tau", weight=10, quadratic=True) + # Define the model + bio_model = BiorbdModel(bio_model_path, parameters=parameters) + + # Mapping + tau_mappings = BiMappingList() + tau_mappings.add("tau", to_second=[None, 0], to_first=[1]) + # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_TIME, weight=1, phase=0, min_bound=1, max_bound=5) diff --git a/bioptim/limits/constraints.py b/bioptim/limits/constraints.py index 38bd8363e..c4c2c2e7c 100644 --- a/bioptim/limits/constraints.py +++ b/bioptim/limits/constraints.py @@ -301,7 +301,7 @@ def torque_max_from_q_and_qdot( if min_torque and min_torque < 0: raise ValueError("min_torque cannot be negative in tau_max_from_actuators") - bound = controller.model.tau_max()(controller.q, controller.qdot, controller.parameters.cx) + bound = controller.model.tau_max()(controller.q, controller.qdot, controller.parameters_except_time.cx) min_bound = controller.tau.mapping.to_first.map(bound[1]) max_bound = controller.tau.mapping.to_first.map(bound[0]) if min_torque: @@ -407,12 +407,12 @@ def qddot_equals_forward_dynamics( """ passive_torque = controller.model.passive_joint_torque()( - controller.q, controller.qdot, controller.parameters.cx + controller.q, controller.qdot, controller.parameters_except_time.cx ) tau = controller.states["tau"].cx if "tau" in controller.states else controller.tau tau = tau + passive_torque if with_passive_torque else tau tau = ( - tau + controller.model.ligament_joint_torque()(controller.q, controller.qdot, controller.parameters.cx) + tau + controller.model.ligament_joint_torque()(controller.q, controller.qdot, controller.parameters_except_time.cx) if with_ligament else tau ) @@ -420,7 +420,7 @@ def qddot_equals_forward_dynamics( 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)( - controller.q, controller.qdot, tau, [], controller.parameters.cx + controller.q, controller.qdot, tau, [], controller.parameters_except_time.cx ) return qddot - qddot_fd @@ -456,11 +456,11 @@ def tau_equals_inverse_dynamics( tau = controller.states["tau"].cx if "tau" in controller.states else controller.tau qddot = controller.states["qddot"].cx if "qddot" in controller.states else controller.controls["qddot"].cx passive_torque = controller.model.passive_joint_torque()( - controller.q, controller.qdot, controller.parameters.cx + controller.q, controller.qdot, controller.parameters_except_time.cx ) tau = tau + passive_torque if with_passive_torque else tau tau = ( - tau + controller.model.ligament_joint_torque()(controller.q, controller.qdot, controller.parameters.cx) + tau + controller.model.ligament_joint_torque()(controller.q, controller.qdot, controller.parameters_except_time.cx) if with_ligament else tau ) @@ -475,7 +475,7 @@ def tau_equals_inverse_dynamics( raise ValueError("External forces must be provided") tau_id = controller.model.inverse_dynamics(with_contact=with_contact)( - controller.q, controller.qdot, qddot, f_ext, controller.parameters.cx + controller.q, controller.qdot, qddot, f_ext, controller.parameters_except_time.cx ) var = [] @@ -538,17 +538,17 @@ def tau_from_muscle_equal_inverse_dynamics( muscle_activations = controller.controls["muscles"].cx muscles_states = controller.model.state_set() passive_torque = controller.model.passive_joint_torque()( - controller.q, controller.qdot, controller.parameters.cx + controller.q, controller.qdot, controller.parameters_except_time.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, controller.q, controller.qdot, controller.parameters.cx + muscles_states, controller.q, controller.qdot, controller.parameters_except_time.cx ) muscle_tau = muscle_tau + passive_torque if with_passive_torque else muscle_tau muscle_tau = ( muscle_tau - + controller.model.ligament_joint_torque(controller.q, controller.qdot, controller.parameters.cx) + + controller.model.ligament_joint_torque(controller.q, controller.qdot, controller.parameters_except_time.cx) if with_ligament else muscle_tau ) @@ -561,7 +561,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()(controller.q, controller.qdot, qddot, controller.parameters.cx) + tau_id = controller.model.inverse_dynamics()(controller.q, controller.qdot, qddot, controller.parameters_except_time.cx) return tau_id - muscle_tau @@ -587,7 +587,7 @@ def implicit_soft_contact_forces(_: Constraint, controller: PenaltyController, * force_idx.append(5 + (6 * i_sc)) soft_contact_all = controller.get_nlp.soft_contact_forces_func( - controller.states.cx, controller.controls.cx, controller.parameters.cx + controller.states.cx, controller.controls.cx, controller.parameters_except_time.cx ) soft_contact_force = soft_contact_all[force_idx] diff --git a/bioptim/limits/multinode_penalty.py b/bioptim/limits/multinode_penalty.py index ffc202b22..608119dc3 100644 --- a/bioptim/limits/multinode_penalty.py +++ b/bioptim/limits/multinode_penalty.py @@ -267,12 +267,12 @@ def com_equality(penalty, controllers: list[PenaltyController]): MultinodePenaltyFunctions.Functions._prepare_controller_cx(penalty, controllers) - com_0 = controllers[0].model.center_of_mass()(controllers[0].states["q"].cx, controllers[0].parameters.cx) + com_0 = controllers[0].model.center_of_mass()(controllers[0].states["q"].cx, controllers[0].parameters_except_time.cx) out = controllers[0].cx.zeros((3, 1)) for i in range(1, len(controllers)): com_i = controllers[i].model.center_of_mass()( - controllers[i].states["q"].cx, controllers[i].parameters.cx + controllers[i].states["q"].cx, controllers[i].parameters_except_time.cx ) out += com_0 - com_i @@ -298,13 +298,13 @@ def com_velocity_equality(penalty, controllers: list[PenaltyController]): MultinodePenaltyFunctions.Functions._prepare_controller_cx(penalty, controllers) com_dot_0 = controllers[0].model.center_of_mass_velocity()( - controllers[0].states["q"].cx, controllers[0].states["qdot"].cx, controllers[0].parameters.cx + controllers[0].states["q"].cx, controllers[0].states["qdot"].cx, controllers[0].parameters_except_time.cx ) out = controllers[0].cx.zeros((3, 1)) for i in range(1, len(controllers)): com_dot_i = controllers[i].model.center_of_mass_velocity()( - controllers[i].states["q"].cx, controllers[i].states["qdot"].cx, controllers[i].parameters.cx + controllers[i].states["q"].cx, controllers[i].states["qdot"].cx, controllers[i].parameters_except_time.cx ) out += com_dot_0 - com_dot_i diff --git a/bioptim/limits/penalty.py b/bioptim/limits/penalty.py index e8f200217..78cd089f6 100644 --- a/bioptim/limits/penalty.py +++ b/bioptim/limits/penalty.py @@ -125,7 +125,7 @@ def minimize_power( return controls * controller.qdot elif key_control == "muscles": muscles_dot = controller.model.muscle_velocity()( - controller.q, controller.qdot, controller.parameters.cx + controller.q, controller.qdot, controller.parameters_except_time.cx ) return controls * muscles_dot @@ -297,11 +297,11 @@ def minimize_markers( CX_eye(4) if reference_jcs is None else controller.model.homogeneous_matrices_in_global(reference_jcs, inverse=True)( - controller.q, controller.parameters.cx + controller.q, controller.parameters_except_time.cx ) ) - markers = controller.model.markers()(controller.q, controller.parameters.cx) + markers = controller.model.markers()(controller.q, controller.parameters_except_time.cx) markers_in_jcs = [] for i in range(markers.shape[1]): marker_in_jcs = jcs_t @ vertcat(markers[:, i], 1) @@ -346,7 +346,7 @@ def minimize_markers_velocity( # Add the penalty in the requested reference frame. None for global markers = horzcat( *controller.model.marker_velocities(reference_index=reference_jcs)( - controller.q, controller.qdot, controller.parameters.cx + controller.q, controller.qdot, controller.parameters_except_time.cx ) ) @@ -388,7 +388,7 @@ def minimize_markers_acceleration( markers = horzcat( *controller.model.marker_accelerations(reference_index=reference_jcs)( - controller.q, controller.qdot, qddot, controller.parameters.cx + controller.q, controller.qdot, qddot, controller.parameters_except_time.cx ) ) @@ -433,8 +433,8 @@ def superimpose_markers( penalty.quadratic = True if penalty.quadratic is None else penalty.quadratic diff_markers = controller.model.marker(second_marker_idx)( - controller.q, controller.parameters.cx - ) - controller.model.marker(first_marker_idx)(controller.q, controller.parameters.cx) + controller.q, controller.parameters_except_time.cx + ) - controller.model.marker(first_marker_idx)(controller.q, controller.parameters_except_time.cx) return diff_markers @@ -477,7 +477,7 @@ def superimpose_markers_velocity( penalty.quadratic = True if penalty.quadratic is None else penalty.quadratic marker_velocity = controller.model.marker_velocities()( - controller.q, controller.qdot, controller.parameters.cx + controller.q, controller.qdot, controller.parameters_except_time.cx ) marker_1 = marker_velocity[first_marker_idx][:] marker_2 = marker_velocity[second_marker_idx][:] @@ -607,9 +607,9 @@ def minimize_predicted_com_height(_: PenaltyOption, controller: PenaltyControlle """ g = controller.model.gravity()["o0"][2] - com = controller.model.center_of_mass()(controller.q, controller.parameters.cx) + com = controller.model.center_of_mass()(controller.q, controller.parameters_except_time.cx) com_dot = controller.model.center_of_mass_velocity()( - controller.q, controller.qdot, controller.parameters.cx + controller.q, controller.qdot, controller.parameters_except_time.cx ) com_height = (com_dot[2] * com_dot[2]) / (2 * -g) + com[2] return com_height @@ -635,7 +635,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, controller.parameters.cx) + return controller.model.center_of_mass()(controller.q, controller.parameters_except_time.cx) @staticmethod def minimize_com_velocity(penalty: PenaltyOption, controller: PenaltyController, axes: tuple | list = None): @@ -658,7 +658,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, controller.qdot, controller.parameters.cx) + return controller.model.center_of_mass_velocity()(controller.q, controller.qdot, controller.parameters_except_time.cx) @staticmethod def minimize_com_acceleration(penalty: PenaltyOption, controller: PenaltyController, axes: tuple | list = None): @@ -684,7 +684,7 @@ def minimize_com_acceleration(penalty: PenaltyOption, controller: PenaltyControl qddot = PenaltyFunctionAbstract._get_qddot(controller, "cx") marker = controller.model.center_of_mass_acceleration()( - controller.q, controller.qdot, qddot, controller.parameters.cx + controller.q, controller.qdot, qddot, controller.parameters_except_time.cx ) return marker @@ -708,7 +708,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, controller.qdot, controller.parameters.cx) + return controller.model.angular_momentum()(controller.q, controller.qdot, controller.parameters_except_time.cx) @staticmethod def minimize_linear_momentum(penalty: PenaltyOption, controller: PenaltyController, axes: tuple | list = None): @@ -731,7 +731,7 @@ def minimize_linear_momentum(penalty: PenaltyOption, controller: PenaltyControll penalty.quadratic = True if penalty.quadratic is None else penalty.quadratic com_velocity = controller.model.center_of_mass_velocity()( - controller.q, controller.qdot, controller.parameters.cx + controller.q, controller.qdot, controller.parameters_except_time.cx ) mass = controller.model.mass()["o0"] linear_momentum_cx = com_velocity * mass @@ -867,7 +867,7 @@ def minimize_soft_contact_forces( force_idx.append(4 + (6 * i_sc)) force_idx.append(5 + (6 * i_sc)) soft_contact_force = controller.get_nlp.soft_contact_forces_func( - controller.time.cx, controller.states.cx_start, controller.controls.cx_start, controller.parameters.cx + controller.time.cx, controller.states.cx_start, controller.controls.cx_start, controller.parameters_except_time.cx ) return soft_contact_force[force_idx] @@ -907,9 +907,9 @@ def track_segment_with_custom_rt( "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, controller.parameters.cx + controller.q, controller.parameters_except_time.cx )[:3, :3].T - r_rt = controller.model.rt(rt_idx)(controller.q, controller.parameters.cx)[:3, :3] + r_rt = controller.model.rt(rt_idx)(controller.q, controller.parameters_except_time.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) @@ -951,7 +951,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, controller.parameters.cx) + marker = controller.model.marker(marker_idx, segment_idx)(controller.q, controller.parameters_except_time.cx) # To align an axis, the other must be equal to 0 if not penalty.rows_is_set: @@ -1002,7 +1002,7 @@ def minimize_segment_rotation( 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, controller.parameters.cx + controller.q, controller.parameters_except_time.cx )[:3, :3] angles_segment = controller.model.rotation_matrix_to_euler_angles(sequence)(jcs_segment) @@ -1049,7 +1049,7 @@ def minimize_segment_velocity( ) model: BiorbdModel = controller.model segment_angular_velocity = model.segment_angular_velocity(segment_idx)( - controller.q, controller.qdot, controller.parameters.cx + controller.q, controller.qdot, controller.parameters_except_time.cx ) if axes is None: @@ -1118,16 +1118,16 @@ def track_vector_orientations_from_markers( ) vector_0_marker_0_position = controller.model.marker(vector_0_marker_0_idx)( - controller.q, controller.parameters.cx + controller.q, controller.parameters_except_time.cx ) vector_0_marker_1_position = controller.model.marker(vector_0_marker_1_idx)( - controller.q, controller.parameters.cx + controller.q, controller.parameters_except_time.cx ) vector_1_marker_0_position = controller.model.marker(vector_1_marker_0_idx)( - controller.q, controller.parameters.cx + controller.q, controller.parameters_except_time.cx ) vector_1_marker_1_position = controller.model.marker(vector_1_marker_1_idx)( - controller.q, controller.parameters.cx + controller.q, controller.parameters_except_time.cx ) vector_0 = vector_0_marker_1_position - vector_0_marker_0_position diff --git a/bioptim/limits/penalty_controller.py b/bioptim/limits/penalty_controller.py index 4cb2deadb..3e5847514 100644 --- a/bioptim/limits/penalty_controller.py +++ b/bioptim/limits/penalty_controller.py @@ -161,10 +161,10 @@ def dt(self) -> OptimizationVariable: """ tp = OptimizationVariableList(self._nlp.cx, self._nlp.phase_dynamics == PhaseDynamics.SHARED_DURING_THE_PHASE) - n_val = self._nlp.dt_mx.shape[0] + n_val = self._nlp.dt.shape[0] tp.append( "dt", - mx=self._nlp.dt_mx, + mx=None, # self._nlp.dt_mx, cx=[self._nlp.dt, self._nlp.dt, self._nlp.dt], bimapping=BiMapping(to_second=range(n_val), to_first=range(n_val)), ) @@ -177,10 +177,10 @@ def time(self) -> OptimizationVariable: """ tp = OptimizationVariableList(self._nlp.cx, self._nlp.phase_dynamics == PhaseDynamics.SHARED_DURING_THE_PHASE) - n_val = self._nlp.time_mx.shape[0] + n_val = self._nlp.time_cx.shape[0] tp.append( "time", - mx=self._nlp.time_mx, + mx=None, #self._nlp.time_mx, cx=[self._nlp.time_cx, self._nlp.time_cx, self._nlp.time_cx], bimapping=BiMapping(to_second=range(n_val), to_first=range(n_val)), ) @@ -375,6 +375,17 @@ def parameters(self) -> OptimizationVariableList: """ return self._nlp.parameters + @property + def parameters_except_time(self) -> OptimizationVariableList: + """ + Return the parameters + + Returns + ------- + The parameters + """ + return self._nlp.parameters_except_time + @property def parameters_scaled(self) -> OptimizationVariableList: """ diff --git a/bioptim/limits/phase_transition.py b/bioptim/limits/phase_transition.py index 185c685e8..d8ae34320 100644 --- a/bioptim/limits/phase_transition.py +++ b/bioptim/limits/phase_transition.py @@ -274,7 +274,7 @@ def impact(transition, controllers: list[PenaltyController, PenaltyController]): # Todo scaled? q_pre = pre.states["q"].cx qdot_pre = pre.states["qdot"].cx - qdot_impact = post.model.qdot_from_impact()(q_pre, qdot_pre, pre.parameters.cx) + qdot_impact = post.model.qdot_from_impact()(q_pre, qdot_pre, pre.parameters_except_time.cx) val = [] cx_start = [] diff --git a/bioptim/optimization/non_linear_program.py b/bioptim/optimization/non_linear_program.py index 37c06ec7d..13228b390 100644 --- a/bioptim/optimization/non_linear_program.py +++ b/bioptim/optimization/non_linear_program.py @@ -194,8 +194,10 @@ def __init__(self, phase_dynamics: PhaseDynamics, use_sx: bool): from ..optimization.parameters import ParameterContainer self.parameters = ParameterContainer(use_sx=use_sx) + parameters_except_time = ParameterContainer(use_sx=use_sx) self.algebraic_states = OptimizationVariableContainer(self.phase_dynamics) self.integrated_values = OptimizationVariableContainer(self.phase_dynamics) + self.numerical_timeseries = OptimizationVariableContainer(self.phase_dynamics) def initialize(self, cx: MX | SX | Callable = None): """ diff --git a/bioptim/optimization/optimal_control_program.py b/bioptim/optimization/optimal_control_program.py index 24994f893..e3bd952a6 100644 --- a/bioptim/optimization/optimal_control_program.py +++ b/bioptim/optimization/optimal_control_program.py @@ -626,6 +626,7 @@ def _prepare_dynamics(self): for i in range(self.n_phases): self.nlp[i].initialize(self.cx) self.nlp[i].parameters = self.parameters # This should be remove when phase parameters will be implemented + self.nlp[i].parameters_except_time = self.parameters_except_time self.nlp[i].numerical_data_timeseries = self.nlp[i].dynamics_type.numerical_data_timeseries ConfigureProblem.initialize(self, self.nlp[i]) self.nlp[i].ode_solver.prepare_dynamic_integrator(self, self.nlp[i]) @@ -1064,6 +1065,9 @@ def _declare_parameters(self, parameters: ParameterList): self.parameters = ParameterContainer(use_sx=(True if self.cx == SX else False)) self.parameters.initialize(parameters) + # The version without time will not be updated later when time is declared + self.parameters_except_time = ParameterContainer(use_sx=(True if self.cx == SX else False)) + self.parameters_except_time.initialize(parameters) def update_bounds( self, @@ -1647,7 +1651,7 @@ def define_parameters_phase_time( dt_bounds = {} dt_initial_guess = {} dt_cx = [] - dt_mx = [] + # dt_mx = [] for i_phase in range(self.n_phases): if i_phase == self.time_phase_mapping.to_second.map_idx[i_phase]: dt = self.phase_time[i_phase] / self.nlp[i_phase].ns @@ -1655,7 +1659,7 @@ def define_parameters_phase_time( dt_initial_guess[f"dt_phase_{i_phase}"] = dt dt_cx.append(self.dt_parameter[self.time_phase_mapping.to_second.map_idx[i_phase]].cx) - dt_mx.append(self.dt_parameter[self.time_phase_mapping.to_second.map_idx[i_phase]].mx) + # dt_mx.append(self.dt_parameter[self.time_phase_mapping.to_second.map_idx[i_phase]].mx) has_penalty = define_parameters_phase_time(self, objective_functions) define_parameters_phase_time(self, constraints, has_penalty) @@ -1663,11 +1667,11 @@ def define_parameters_phase_time( # Add to the nlp NLP.add(self, "time_index", self.time_phase_mapping.to_second.map_idx, True) NLP.add(self, "time_cx", self.cx.sym("time", 1, 1), True) - NLP.add(self, "time_mx", MX.sym("time", 1, 1), True) + # NLP.add(self, "time_mx", MX.sym("time", 1, 1), True) NLP.add(self, "dt", dt_cx, False) - NLP.add(self, "dt_mx", dt_mx, False) + # NLP.add(self, "dt_mx", dt_mx, False) NLP.add(self, "tf", [nlp.dt * max(nlp.ns, 1) for nlp in self.nlp], False) - NLP.add(self, "tf_mx", [nlp.dt_mx * max(nlp.ns, 1) for nlp in self.nlp], False) + # NLP.add(self, "tf_mx", [nlp.dt_mx * max(nlp.ns, 1) for nlp in self.nlp], False) self.dt_parameter_bounds = Bounds( "dt_bounds", diff --git a/tests/shard3/test_graph.py b/tests/shard3/test_graph.py index cde4014f4..cbc4049f5 100644 --- a/tests/shard3/test_graph.py +++ b/tests/shard3/test_graph.py @@ -43,7 +43,7 @@ def custom_func_track_markers(controller: PenaltyController, first_marker: str, # Get the index of the markers from their name marker_0_idx = controller.model.marker_index(first_marker) marker_1_idx = controller.model.marker_index(second_marker) - markers = controller.model.markers()(controller.q, controller.parameters.cx) + markers = controller.model.markers()(controller.q, controller.parameters_except_time.cx) return markers[:, marker_1_idx] - markers[:, marker_0_idx] From 5e5e5516605f4cac72122641d8b0d6474a827fc5 Mon Sep 17 00:00:00 2001 From: eve-mac Date: Sat, 21 Sep 2024 10:23:08 -0400 Subject: [PATCH 022/108] blacked --- bioptim/dynamics/dynamics_functions.py | 52 +++++++++++++++---- bioptim/limits/constraints.py | 18 +++++-- bioptim/limits/multinode_penalty.py | 12 +++-- bioptim/limits/penalty.py | 17 ++++-- bioptim/limits/penalty_controller.py | 6 ++- bioptim/optimization/optimization_variable.py | 2 +- bioptim/optimization/parameters.py | 2 +- 7 files changed, 84 insertions(+), 25 deletions(-) diff --git a/bioptim/dynamics/dynamics_functions.py b/bioptim/dynamics/dynamics_functions.py index d2d9833fa..c9e3f1838 100644 --- a/bioptim/dynamics/dynamics_functions.py +++ b/bioptim/dynamics/dynamics_functions.py @@ -145,7 +145,11 @@ 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, nlp.parameters_except_time.cx) if with_passive_torque else tau + tau = ( + tau + nlp.model.passive_joint_torque()(q, qdot, nlp.parameters_except_time.cx) + if with_passive_torque + else tau + ) tau = tau + nlp.model.ligament_joint_torque()(q, qdot, nlp.parameters_except_time.cx) if with_ligament else tau tau = tau - nlp.model.friction_coefficients @ qdot if with_friction else tau @@ -598,7 +602,11 @@ 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, nlp.parameters_except_time.cx) if with_passive_torque else tau + tau = ( + tau + nlp.model.passive_joint_torque()(q, qdot, nlp.parameters_except_time.cx) + if with_passive_torque + else tau + ) tau = tau + nlp.model.ligament_joint_torque()(q, qdot, nlp.parameters_except_time.cx) if with_ligament else tau dq = DynamicsFunctions.compute_qdot(nlp, q, qdot) @@ -673,7 +681,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, nlp.parameters_except_time.cx) if with_passive_torque else tau + tau = ( + tau + nlp.model.passive_joint_torque()(q, qdot, nlp.parameters_except_time.cx) + if with_passive_torque + else tau + ) tau = tau + nlp.model.ligament_joint_torque()(q, qdot, nlp.parameters_except_time.cx) if with_ligament else tau external_forces = [] if external_forces is None else external_forces @@ -727,7 +739,11 @@ def forces_from_torque_activation_driven( 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, nlp.parameters_except_time.cx) - tau = tau + nlp.model.passive_joint_torque()(q, qdot, nlp.parameters_except_time.cx) if with_passive_torque else tau + tau = ( + tau + nlp.model.passive_joint_torque()(q, qdot, nlp.parameters_except_time.cx) + if with_passive_torque + else tau + ) tau = tau + nlp.model.ligament_joint_torque()(q, qdot, nlp.parameters_except_time.cx) if with_ligament else tau external_forces = [] if external_forces is None else external_forces @@ -831,7 +847,11 @@ 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, nlp.parameters_except_time.cx) if with_passive_torque else tau + tau = ( + tau + nlp.model.passive_joint_torque()(q, qdot, nlp.parameters_except_time.cx) + if with_passive_torque + else tau + ) tau = tau + nlp.model.ligament_joint_torque()(q, qdot, nlp.parameters_except_time.cx) if with_ligament else tau dq = DynamicsFunctions.compute_qdot(nlp, q, qdot) @@ -932,7 +952,11 @@ 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, nlp.parameters_except_time.cx) if with_passive_torque else tau + tau = ( + tau + nlp.model.passive_joint_torque()(q, qdot, nlp.parameters_except_time.cx) + if with_passive_torque + else tau + ) tau = tau + nlp.model.ligament_joint_torque()(q, qdot, nlp.parameters_except_time.cx) if with_ligament else tau external_forces = [] if external_forces is None else external_forces @@ -983,7 +1007,9 @@ def joints_acceleration_driven( qdot = nlp.get_var_from_states_or_controls("qdot", states, controls) qddot_joints = nlp.get_var_from_states_or_controls("qddot", states, controls) - qddot_root = nlp.model.forward_dynamics_free_floating_base()(q, qdot, qddot_joints, nlp.parameters_except_time.cx) + qddot_root = nlp.model.forward_dynamics_free_floating_base()( + q, qdot, qddot_joints, nlp.parameters_except_time.cx + ) qddot_reordered = nlp.model.reorder_qddot_root_joints(qddot_root, qddot_joints) qdot_mapped = nlp.variable_mappings["qdot"].to_first.map(qdot) @@ -1113,7 +1139,9 @@ 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, [], nlp.parameters_except_time.cx) + qddot = nlp.model.forward_dynamics(with_contact=with_contact)( + q, qdot, tau, [], nlp.parameters_except_time.cx + ) return qdot_var_mapping.map(qddot) else: qddot = nlp.model.forward_dynamics(with_contact=with_contact)( @@ -1154,7 +1182,9 @@ def inverse_dynamics( """ if external_forces is None: - tau = nlp.model.inverse_dynamics(with_contact=with_contact)(q, qdot, qddot, [], nlp.parameters_except_time.cx) + tau = nlp.model.inverse_dynamics(with_contact=with_contact)( + q, qdot, qddot, [], nlp.parameters_except_time.cx + ) else: if "tau" in nlp.states: tau_shape = nlp.states["tau"].cx.shape[0] @@ -1266,6 +1296,8 @@ 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, nlp.parameters_except_time.cx) + qddot_u = nlp.model.partitioned_forward_dynamics( + q_u, qdot_u, tau, external_forces, nlp.parameters_except_time.cx + ) return DynamicsEvaluation(dxdt=vertcat(qdot_u, qddot_u), defects=None) diff --git a/bioptim/limits/constraints.py b/bioptim/limits/constraints.py index c4c2c2e7c..8d2e2914a 100644 --- a/bioptim/limits/constraints.py +++ b/bioptim/limits/constraints.py @@ -412,7 +412,10 @@ def qddot_equals_forward_dynamics( tau = controller.states["tau"].cx if "tau" in controller.states else controller.tau tau = tau + passive_torque if with_passive_torque else tau tau = ( - tau + controller.model.ligament_joint_torque()(controller.q, controller.qdot, controller.parameters_except_time.cx) + tau + + controller.model.ligament_joint_torque()( + controller.q, controller.qdot, controller.parameters_except_time.cx + ) if with_ligament else tau ) @@ -460,7 +463,10 @@ def tau_equals_inverse_dynamics( ) tau = tau + passive_torque if with_passive_torque else tau tau = ( - tau + controller.model.ligament_joint_torque()(controller.q, controller.qdot, controller.parameters_except_time.cx) + tau + + controller.model.ligament_joint_torque()( + controller.q, controller.qdot, controller.parameters_except_time.cx + ) if with_ligament else tau ) @@ -548,7 +554,9 @@ def tau_from_muscle_equal_inverse_dynamics( muscle_tau = muscle_tau + passive_torque if with_passive_torque else muscle_tau muscle_tau = ( muscle_tau - + controller.model.ligament_joint_torque(controller.q, controller.qdot, controller.parameters_except_time.cx) + + controller.model.ligament_joint_torque( + controller.q, controller.qdot, controller.parameters_except_time.cx + ) if with_ligament else muscle_tau ) @@ -561,7 +569,9 @@ 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()(controller.q, controller.qdot, qddot, controller.parameters_except_time.cx) + tau_id = controller.model.inverse_dynamics()( + controller.q, controller.qdot, qddot, controller.parameters_except_time.cx + ) return tau_id - muscle_tau diff --git a/bioptim/limits/multinode_penalty.py b/bioptim/limits/multinode_penalty.py index 608119dc3..3a3629def 100644 --- a/bioptim/limits/multinode_penalty.py +++ b/bioptim/limits/multinode_penalty.py @@ -267,7 +267,9 @@ def com_equality(penalty, controllers: list[PenaltyController]): MultinodePenaltyFunctions.Functions._prepare_controller_cx(penalty, controllers) - com_0 = controllers[0].model.center_of_mass()(controllers[0].states["q"].cx, controllers[0].parameters_except_time.cx) + com_0 = controllers[0].model.center_of_mass()( + controllers[0].states["q"].cx, controllers[0].parameters_except_time.cx + ) out = controllers[0].cx.zeros((3, 1)) for i in range(1, len(controllers)): @@ -298,13 +300,17 @@ def com_velocity_equality(penalty, controllers: list[PenaltyController]): MultinodePenaltyFunctions.Functions._prepare_controller_cx(penalty, controllers) com_dot_0 = controllers[0].model.center_of_mass_velocity()( - controllers[0].states["q"].cx, controllers[0].states["qdot"].cx, controllers[0].parameters_except_time.cx + controllers[0].states["q"].cx, + controllers[0].states["qdot"].cx, + controllers[0].parameters_except_time.cx, ) out = controllers[0].cx.zeros((3, 1)) for i in range(1, len(controllers)): com_dot_i = controllers[i].model.center_of_mass_velocity()( - controllers[i].states["q"].cx, controllers[i].states["qdot"].cx, controllers[i].parameters_except_time.cx + controllers[i].states["q"].cx, + controllers[i].states["qdot"].cx, + controllers[i].parameters_except_time.cx, ) out += com_dot_0 - com_dot_i diff --git a/bioptim/limits/penalty.py b/bioptim/limits/penalty.py index 78cd089f6..4c6f43a7b 100644 --- a/bioptim/limits/penalty.py +++ b/bioptim/limits/penalty.py @@ -658,7 +658,9 @@ 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, controller.qdot, controller.parameters_except_time.cx) + return controller.model.center_of_mass_velocity()( + controller.q, controller.qdot, controller.parameters_except_time.cx + ) @staticmethod def minimize_com_acceleration(penalty: PenaltyOption, controller: PenaltyController, axes: tuple | list = None): @@ -708,7 +710,9 @@ 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, controller.qdot, controller.parameters_except_time.cx) + return controller.model.angular_momentum()( + controller.q, controller.qdot, controller.parameters_except_time.cx + ) @staticmethod def minimize_linear_momentum(penalty: PenaltyOption, controller: PenaltyController, axes: tuple | list = None): @@ -867,7 +871,10 @@ def minimize_soft_contact_forces( force_idx.append(4 + (6 * i_sc)) force_idx.append(5 + (6 * i_sc)) soft_contact_force = controller.get_nlp.soft_contact_forces_func( - controller.time.cx, controller.states.cx_start, controller.controls.cx_start, controller.parameters_except_time.cx + controller.time.cx, + controller.states.cx_start, + controller.controls.cx_start, + controller.parameters_except_time.cx, ) return soft_contact_force[force_idx] @@ -951,7 +958,9 @@ 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, controller.parameters_except_time.cx) + marker = controller.model.marker(marker_idx, segment_idx)( + controller.q, controller.parameters_except_time.cx + ) # To align an axis, the other must be equal to 0 if not penalty.rows_is_set: diff --git a/bioptim/limits/penalty_controller.py b/bioptim/limits/penalty_controller.py index 3e5847514..26648f4b3 100644 --- a/bioptim/limits/penalty_controller.py +++ b/bioptim/limits/penalty_controller.py @@ -134,7 +134,9 @@ def t_span(self) -> OptimizationVariable: tp = OptimizationVariableList(self._nlp.cx, self._nlp.phase_dynamics == PhaseDynamics.SHARED_DURING_THE_PHASE) n_val = cx.shape[0] - tp.append("t_span", mx=None, cx=[cx, cx, cx], bimapping=BiMapping(to_second=range(n_val), to_first=range(n_val))) + tp.append( + "t_span", mx=None, cx=[cx, cx, cx], bimapping=BiMapping(to_second=range(n_val), to_first=range(n_val)) + ) return tp["t_span"] @property @@ -180,7 +182,7 @@ def time(self) -> OptimizationVariable: n_val = self._nlp.time_cx.shape[0] tp.append( "time", - mx=None, #self._nlp.time_mx, + mx=None, # self._nlp.time_mx, cx=[self._nlp.time_cx, self._nlp.time_cx, self._nlp.time_cx], bimapping=BiMapping(to_second=range(n_val), to_first=range(n_val)), ) diff --git a/bioptim/optimization/optimization_variable.py b/bioptim/optimization/optimization_variable.py index 64c5765f8..888be16c7 100644 --- a/bioptim/optimization/optimization_variable.py +++ b/bioptim/optimization/optimization_variable.py @@ -37,7 +37,7 @@ class OptimizationVariable: def __init__( self, name: str, - mx: MX, # remove Charbie + mx: MX, # remove Charbie cx_start: list | None, index: range | list, mapping: BiMapping = None, diff --git a/bioptim/optimization/parameters.py b/bioptim/optimization/parameters.py index f6b7e3d38..6427773ab 100644 --- a/bioptim/optimization/parameters.py +++ b/bioptim/optimization/parameters.py @@ -105,6 +105,7 @@ def apply_parameter(self, model): 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): """ A list of Parameters. @@ -339,4 +340,3 @@ def cx_end(self): @property def mx(self): return self.unscaled.mx - From 5656f86682da3925d8498c5df79739a558dc8649 Mon Sep 17 00:00:00 2001 From: eve-mac Date: Mon, 23 Sep 2024 08:21:58 -0400 Subject: [PATCH 023/108] woups --- .../examples/muscle_driven_ocp/muscle_excitations_tracker.py | 2 +- bioptim/optimization/non_linear_program.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/bioptim/examples/muscle_driven_ocp/muscle_excitations_tracker.py b/bioptim/examples/muscle_driven_ocp/muscle_excitations_tracker.py index fd372bf1c..75807414e 100644 --- a/bioptim/examples/muscle_driven_ocp/muscle_excitations_tracker.py +++ b/bioptim/examples/muscle_driven_ocp/muscle_excitations_tracker.py @@ -202,7 +202,7 @@ def dyn_interface(t, x, u): def add_to_data(i, q): X[:, i] = q - markers[:, :, i] = horzcat(*bio_model.markers()(q[:n_q], [])) + markers[:, :, i] = bio_model.markers()(q[:n_q], []) x_init = np.array([0.0] * n_q + [0.0] * n_qdot + [0.5] * n_mus) add_to_data(0, x_init) diff --git a/bioptim/optimization/non_linear_program.py b/bioptim/optimization/non_linear_program.py index 13228b390..7d2f74de9 100644 --- a/bioptim/optimization/non_linear_program.py +++ b/bioptim/optimization/non_linear_program.py @@ -194,7 +194,7 @@ def __init__(self, phase_dynamics: PhaseDynamics, use_sx: bool): from ..optimization.parameters import ParameterContainer self.parameters = ParameterContainer(use_sx=use_sx) - parameters_except_time = ParameterContainer(use_sx=use_sx) + self.parameters_except_time = ParameterContainer(use_sx=use_sx) self.algebraic_states = OptimizationVariableContainer(self.phase_dynamics) self.integrated_values = OptimizationVariableContainer(self.phase_dynamics) self.numerical_timeseries = OptimizationVariableContainer(self.phase_dynamics) From b4d7185444450962b2c47440dc28ded14b2f9ac4 Mon Sep 17 00:00:00 2001 From: eve-mac Date: Mon, 23 Sep 2024 09:56:40 -0400 Subject: [PATCH 024/108] tests start to look good --- bioptim/dynamics/integrator.py | 4 +- .../muscle_activations_tracker.py | 2 +- ...arm_reaching_torque_driven_collocations.py | 4 +- .../arm_reaching_torque_driven_explicit.py | 10 ++-- .../arm_reaching_torque_driven_implicit.py | 4 +- .../stochastic_optimal_control/common.py | 4 +- bioptim/limits/constraints.py | 4 +- bioptim/limits/penalty.py | 9 ++-- bioptim/limits/penalty_controller.py | 16 +++--- bioptim/models/biorbd/biorbd_model.py | 49 ++++++++++++++----- bioptim/models/biorbd/multi_biorbd_model.py | 23 +++++++-- bioptim/models/protocols/biomodel.py | 22 +++++++-- tests/shard1/test_biorbd_multi_model.py | 2 +- ...st_global_stochastic_except_collocation.py | 2 +- tests/shard6/test_time_dependent_problems.py | 2 +- 15 files changed, 105 insertions(+), 52 deletions(-) diff --git a/bioptim/dynamics/integrator.py b/bioptim/dynamics/integrator.py index 81b60aac0..de5df59d6 100644 --- a/bioptim/dynamics/integrator.py +++ b/bioptim/dynamics/integrator.py @@ -312,7 +312,7 @@ def dxdt( t = self.t_span_sym[0] + self._integration_time * (i - 1) x[:, i] = self.next_x(t, x[:, i - 1], u, p, a, d) if self.model.nb_quaternions > 0: - x[:, i] = self.model.normalize_state_quaternions(x[:, i]) + x[:self.model.nb_q, i] = self.model.normalize_state_quaternions()(x[:self.model.nb_q, i]) return x[:, -1], x @@ -534,7 +534,7 @@ def dxdt( ) if self.model.nb_quaternions > 0: - x_prev[:, 1] = self.model.normalize_state_quaternions(x_prev[:, 1]) + x_prev[:self.model.nb_q, 1] = self.model.normalize_state_quaternions(x_prev[:self.model.nb_q, 1]) return x_prev[:, 1], x_prev diff --git a/bioptim/examples/muscle_driven_ocp/muscle_activations_tracker.py b/bioptim/examples/muscle_driven_ocp/muscle_activations_tracker.py index 056847daf..e32a97f06 100644 --- a/bioptim/examples/muscle_driven_ocp/muscle_activations_tracker.py +++ b/bioptim/examples/muscle_driven_ocp/muscle_activations_tracker.py @@ -199,7 +199,7 @@ def dyn_interface(t, x, u): def add_to_data(i, q): X[:, i] = q - markers[:, :, i] = horzcat(*bio_model.markers()(q[0:n_q], [])) + markers[:, :, i] = bio_model.markers()(q[0:n_q], []) x_init = np.array([0] * n_q + [0] * n_qdot) add_to_data(0, x_init) diff --git a/bioptim/examples/stochastic_optimal_control/arm_reaching_torque_driven_collocations.py b/bioptim/examples/stochastic_optimal_control/arm_reaching_torque_driven_collocations.py index a7420c7c3..d449eb384 100644 --- a/bioptim/examples/stochastic_optimal_control/arm_reaching_torque_driven_collocations.py +++ b/bioptim/examples/stochastic_optimal_control/arm_reaching_torque_driven_collocations.py @@ -47,8 +47,8 @@ def sensory_reference( """ q = states[nlp.states["q"].index] qdot = states[nlp.states["qdot"].index] - hand_pos = nlp.model.markers()(q, [])[2][:2] - hand_vel = nlp.model.marker_velocities()(q, qdot, [])[2][:2] + hand_pos = nlp.model.marker(2)(q, [])[:2] + hand_vel = nlp.model.marker_velocity(2)(q, qdot, [])[:2] hand_pos_velo = cas.vertcat(hand_pos, hand_vel) return hand_pos_velo diff --git a/bioptim/examples/stochastic_optimal_control/arm_reaching_torque_driven_explicit.py b/bioptim/examples/stochastic_optimal_control/arm_reaching_torque_driven_explicit.py index 19fdbcd65..5817d10cb 100644 --- a/bioptim/examples/stochastic_optimal_control/arm_reaching_torque_driven_explicit.py +++ b/bioptim/examples/stochastic_optimal_control/arm_reaching_torque_driven_explicit.py @@ -154,8 +154,8 @@ def sensory_reference( """ q = states[nlp.states["q"].index] qdot = states[nlp.states["qdot"].index] - hand_pos = nlp.model.markers(q)[2][:2] - hand_vel = nlp.model.marker_velocities(q, qdot)[2][:2] + hand_pos = nlp.model.marker(2)(q, [])[:2] + hand_vel = nlp.model.marker_velocity(2)(q, qdot, [])[:2] hand_pos_velo = cas.vertcat(hand_pos, hand_vel) return hand_pos_velo @@ -254,15 +254,13 @@ def reach_target_consistently(controllers: list[PenaltyController]) -> cas.MX: This is a multi-node constraint because the covariance matrix depends on all the precedent nodes, but it only applies at the END node. """ - - # Charbie todo remove these symbolics q_sym = cas.MX.sym("q_sym", controllers[-1].states["q"].cx.shape[0]) qdot_sym = cas.MX.sym("qdot_sym", controllers[-1].states["qdot"].cx.shape[0]) cov_sym = cas.MX.sym("cov", controllers[-1].integrated_values.cx.shape[0]) cov_matrix = StochasticBioModel.reshape_to_matrix(cov_sym, controllers[-1].model.matrix_shape_cov) - hand_pos = controllers[0].model.markers(q_sym)[2][:2] - hand_vel = controllers[0].model.marker_velocities(q_sym, qdot_sym)[2][:2] + hand_pos = controllers[0].model.marker(2)(q_sym, [])[:2] + hand_vel = controllers[0].model.marker_velocity(2)(q_sym, qdot_sym, [])[:2] jac_marker_q = cas.jacobian(hand_pos, q_sym) jac_marker_qdot = cas.jacobian(hand_vel, cas.vertcat(q_sym, qdot_sym)) diff --git a/bioptim/examples/stochastic_optimal_control/arm_reaching_torque_driven_implicit.py b/bioptim/examples/stochastic_optimal_control/arm_reaching_torque_driven_implicit.py index aa03d8102..922af3851 100644 --- a/bioptim/examples/stochastic_optimal_control/arm_reaching_torque_driven_implicit.py +++ b/bioptim/examples/stochastic_optimal_control/arm_reaching_torque_driven_implicit.py @@ -59,8 +59,8 @@ def sensory_reference( """ q = states[nlp.states["q"].index] qdot = states[nlp.states["qdot"].index] - hand_pos = nlp.model.markers(q)[2][:2] - hand_vel = nlp.model.marker_velocities(q, qdot)[2][:2] + hand_pos = nlp.model.marker(2)(q, [])[:2] + hand_vel = nlp.model.marker_velocity(2)(q, qdot, [])[:2] hand_pos_velo = cas.vertcat(hand_pos, hand_vel) return hand_pos_velo diff --git a/bioptim/examples/stochastic_optimal_control/common.py b/bioptim/examples/stochastic_optimal_control/common.py index e26f17f59..b57a7ea30 100644 --- a/bioptim/examples/stochastic_optimal_control/common.py +++ b/bioptim/examples/stochastic_optimal_control/common.py @@ -35,8 +35,8 @@ def dynamics_torque_driven_with_feedbacks( tau_force_field = get_force_field(q, nlp.model.force_field_magnitude) torques_computed = tau + tau_feedback + motor_noise + tau_force_field - mass_matrix = nlp.model.mass_matrix(q) - non_linear_effects = nlp.model.non_linear_effects(q, qdot) + mass_matrix = nlp.model.mass_matrix()(q, []) + non_linear_effects = nlp.model.non_linear_effects()(q, qdot, []) return cas.inv(mass_matrix) @ (torques_computed - non_linear_effects - nlp.model.friction_coefficients @ qdot) diff --git a/bioptim/limits/constraints.py b/bioptim/limits/constraints.py index 8d2e2914a..f1157a6e5 100644 --- a/bioptim/limits/constraints.py +++ b/bioptim/limits/constraints.py @@ -302,8 +302,8 @@ def torque_max_from_q_and_qdot( raise ValueError("min_torque cannot be negative in tau_max_from_actuators") bound = controller.model.tau_max()(controller.q, controller.qdot, controller.parameters_except_time.cx) - min_bound = controller.tau.mapping.to_first.map(bound[1]) - max_bound = controller.tau.mapping.to_first.map(bound[0]) + min_bound = controller.controls["tau"].mapping.to_first.map(bound[1]) + max_bound = controller.controls["tau"].mapping.to_first.map(bound[0]) if min_torque: min_bound = if_else(lt(min_bound, min_torque), min_torque, min_bound) max_bound = if_else(lt(max_bound, min_torque), min_torque, max_bound) diff --git a/bioptim/limits/penalty.py b/bioptim/limits/penalty.py index 4c6f43a7b..d51a8e60f 100644 --- a/bioptim/limits/penalty.py +++ b/bioptim/limits/penalty.py @@ -345,7 +345,7 @@ def minimize_markers_velocity( # Add the penalty in the requested reference frame. None for global markers = horzcat( - *controller.model.marker_velocities(reference_index=reference_jcs)( + *controller.model.markers_velocities(reference_index=reference_jcs)( controller.q, controller.qdot, controller.parameters_except_time.cx ) ) @@ -387,7 +387,7 @@ def minimize_markers_acceleration( qddot = PenaltyFunctionAbstract._get_qddot(controller, "cx") markers = horzcat( - *controller.model.marker_accelerations(reference_index=reference_jcs)( + *controller.model.markers_accelerations(reference_index=reference_jcs)( controller.q, controller.qdot, qddot, controller.parameters_except_time.cx ) ) @@ -476,7 +476,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()( + marker_velocity = controller.model.markers_velocities()( controller.q, controller.qdot, controller.parameters_except_time.cx ) marker_1 = marker_velocity[first_marker_idx][:] @@ -1454,6 +1454,7 @@ def _get_qddot(controller: PenaltyController, attribute: str): # raise ValueError("atrribute should be either mx or cx_start") if "qddot" not in controller.states and "qddot" not in controller.controls: + source_qdot = controller.states if "qdot" in controller.states else controller.controls return controller.dynamics( getattr(controller.time, attribute), getattr(controller.states, attribute), @@ -1461,7 +1462,7 @@ def _get_qddot(controller: PenaltyController, attribute: str): getattr(controller.parameters, attribute), getattr(controller.algebraic_states, attribute), getattr(controller.numerical_timeseries, attribute), - )[controller.qdot.index, :] + )[source_qdot["qdot"].index, :] source = controller.states if "qddot" in controller.states else controller.controls return getattr(source["qddot"], attribute) diff --git a/bioptim/limits/penalty_controller.py b/bioptim/limits/penalty_controller.py index 26648f4b3..f833420de 100644 --- a/bioptim/limits/penalty_controller.py +++ b/bioptim/limits/penalty_controller.py @@ -149,7 +149,7 @@ def phases_dt(self) -> OptimizationVariable: n_val = self.ocp.dt_parameter.cx.shape[0] tp.append( "phases_dt", - mx=self.ocp.dt_parameter.mx, + mx=None, # self.ocp.dt_parameter.mx, cx=[self.ocp.dt_parameter.cx, self.ocp.dt_parameter.cx, self.ocp.dt_parameter.cx], bimapping=BiMapping(to_second=range(n_val), to_first=range(n_val)), ) @@ -195,10 +195,10 @@ def tf(self) -> OptimizationVariable: """ tp = OptimizationVariableList(self._nlp.cx, self._nlp.phase_dynamics == PhaseDynamics.SHARED_DURING_THE_PHASE) - n_val = self._nlp.tf_mx.shape[0] + n_val = self._nlp.tf.shape[0] tp.append( "tf", - mx=self._nlp.tf_mx, + mx=None, # self._nlp.tf_mx, cx=[self._nlp.tf, self._nlp.tf, self._nlp.tf], bimapping=BiMapping(to_second=range(n_val), to_first=range(n_val)), ) @@ -408,14 +408,14 @@ def q(self) -> OptimizationVariable: return self.states["q"].mapping.to_second.map(self.states["q"].cx) elif "q_roots" in self.states and "q_joints" in self.states: # TODO: add mapping for q_roots and q_joints - cx_start = vertcat(self.states["q_roots"].cx_start, self.states["q_joints"].cx_start) + cx_start = vertcat(self.states["q_roots"].cx, self.states["q_joints"].cx) q_parent_list = OptimizationVariableList( self._nlp.cx, self._nlp.phase_dynamics == PhaseDynamics.SHARED_DURING_THE_PHASE ) q_parent_list._cx_start = cx_start q = OptimizationVariable( name="q", - mx=vertcat(self.states["q_roots"].mx, self.states["q_joints"].mx), + mx=None, # vertcat(self.states["q_roots"].mx, self.states["q_joints"].mx), cx_start=cx_start, index=[i for i in range(self.states["q_roots"].shape + self.states["q_joints"].shape)], mapping=BiMapping( @@ -424,7 +424,7 @@ def q(self) -> OptimizationVariable: ), parent_list=q_parent_list, ) - return q + return q.cx else: raise RuntimeError("q is not defined in the states") @@ -441,7 +441,7 @@ def qdot(self) -> OptimizationVariable: qdot_parent_list._cx_start = cx_start qdot = OptimizationVariable( name="qdot", - mx=vertcat(self.states["qdot_roots"].mx, self.states["qdot_joints"].mx), + mx=None, # vertcat(self.states["qdot_roots"].mx, self.states["qdot_joints"].mx), cx_start=cx_start, index=[i for i in range(self.states["qdot_roots"].shape + self.states["qdot_joints"].shape)], mapping=BiMapping( @@ -450,7 +450,7 @@ def qdot(self) -> OptimizationVariable: ), parent_list=qdot_parent_list, ) - return qdot + return qdot.cx @property def tau(self) -> OptimizationVariable: diff --git a/bioptim/models/biorbd/biorbd_model.py b/bioptim/models/biorbd/biorbd_model.py index c99a25fd6..9af21a894 100644 --- a/bioptim/models/biorbd/biorbd_model.py +++ b/bioptim/models/biorbd/biorbd_model.py @@ -595,7 +595,7 @@ def rigid_contact_index(self, contact_index) -> tuple: """ return self.model.rigidContacts()[contact_index].availableAxesIndices() - def marker_velocities(self, reference_index=None) -> list[MX]: + def markers_velocities(self, reference_index=None) -> list[MX]: if reference_index is None: biorbd_return = [ m.to_mx() @@ -608,23 +608,36 @@ def marker_velocities(self, reference_index=None) -> list[MX]: else: biorbd_return = [] - homogeneous_matrix_transposed = self.biorbd_homogeneous_matrices_in_global( + homogeneous_matrix_transposed = self.homogeneous_matrices_in_global(segment_idx=reference_index, + inverse=True)( GeneralizedCoordinates(self.q), - reference_index, - inverse=True, ) + # TODO: Check and fix this portion of code for m in self.model.markersVelocity(GeneralizedCoordinates(self.q), GeneralizedVelocity(self.qdot)): if m.applyRT(homogeneous_matrix_transposed) is None: biorbd_return.append(m.to_mx()) casadi_fun = Function( - "marker_velocities", + "markers_velocities", [self.q, self.qdot, self.parameters], biorbd_return, ) return casadi_fun - def marker_accelerations(self, reference_index=None) -> list[MX]: + def marker_velocity(self, marker_index: int) -> list[MX]: + biorbd_return = self.model.markersVelocity( + GeneralizedCoordinates(self.q), + GeneralizedVelocity(self.qdot), + True, + )[marker_index].to_mx() + casadi_fun = Function( + "marker_velocity", + [self.q, self.qdot, self.parameters], + [biorbd_return], + ) + return casadi_fun + + def markers_accelerations(self, reference_index=None) -> list[MX]: if reference_index is None: biorbd_return = [ m.to_mx() @@ -637,11 +650,11 @@ def marker_accelerations(self, reference_index=None) -> list[MX]: ] else: + # TODO: Check and fix this portion of code biorbd_return = [] - homogeneous_matrix_transposed = self.biorbd_homogeneous_matrices_in_global( + homogeneous_matrix_transposed = self.homogeneous_matrices_in_global(segment_idx=reference_index, + inverse=True,)( GeneralizedCoordinates(self.q), - reference_index, - inverse=True, ) for m in self.model.markersAcceleration( GeneralizedCoordinates(self.q), @@ -652,12 +665,26 @@ def marker_accelerations(self, reference_index=None) -> list[MX]: biorbd_return.append(m.to_mx()) casadi_fun = Function( - "marker_accelerations", + "markers_accelerations", [self.q, self.qdot, self.qddot, self.parameters], biorbd_return, ) return casadi_fun + def marker_acceleration(self, marker_index: int) -> list[MX]: + biorbd_return = self.model.markerAcceleration( + GeneralizedCoordinates(self.q), + GeneralizedVelocity(self.qdot), + GeneralizedAcceleration(self.qddot), + True, + )[marker_index].to_mx() + casadi_fun = Function( + "marker_acceleration", + [self.q, self.qdot, self.qddot, self.parameters], + [biorbd_return], + ) + return casadi_fun + def tau_max(self) -> tuple[MX, MX]: self.model.closeActuator() q_biorbd = GeneralizedCoordinates(self.q) @@ -766,7 +793,7 @@ def normalize_state_quaternions(self) -> Function: casadi_fun = Function( "soft_contact_forces", - [self.q, self.parameters], + [self.q], [biorbd_return], ) return casadi_fun diff --git a/bioptim/models/biorbd/multi_biorbd_model.py b/bioptim/models/biorbd/multi_biorbd_model.py index fa124413e..07611587e 100644 --- a/bioptim/models/biorbd/multi_biorbd_model.py +++ b/bioptim/models/biorbd/multi_biorbd_model.py @@ -712,18 +712,33 @@ def rigid_contact_index(self, contact_index) -> tuple: # Note: may not work if the contact_index is not in the first model return model_selected.rigid_contact_index(contact_index) - def marker_velocities(self, reference_index=None) -> Function: + def markers_velocities(self, reference_index=None) -> Function: if reference_index is not None: - raise RuntimeError("marker_velocities is not implemented yet with reference_index for MultiBiorbdModel") + raise RuntimeError("markers_velocities is not implemented yet with reference_index for MultiBiorbdModel") biorbd_return = [] for i, model in enumerate(self.models): q_model = self.q[self.variable_index("q", i)] qdot_model = self.qdot[self.variable_index("qdot", i)] - biorbd_return += [model.marker_velocities(reference_index)(q_model, qdot_model)] + biorbd_return += [model.markers_velocities(reference_index)(q_model, qdot_model)] biorbd_return = [item for sublist in biorbd_return for item in sublist] casadi_fun = Function( - "marker_velocities", + "markers_velocities", + [self.q, self.qdot], + [horzcat(*biorbd_return)], + ) + return casadi_fun + + + def marker_velocity(self, marker_index: int) -> Function: + biorbd_return = [] + for i, model in enumerate(self.models): + q_model = self.q[self.variable_index("q", i)] + qdot_model = self.qdot[self.variable_index("qdot", i)] + biorbd_return += [model.marker_velocity(marker_index)(q_model, qdot_model)] + biorbd_return = [item for sublist in biorbd_return for item in sublist] + casadi_fun = Function( + "marker_velocity", [self.q, self.qdot], [horzcat(*biorbd_return)], ) diff --git a/bioptim/models/protocols/biomodel.py b/bioptim/models/protocols/biomodel.py index 9fc63d89e..faa8e10c8 100644 --- a/bioptim/models/protocols/biomodel.py +++ b/bioptim/models/protocols/biomodel.py @@ -252,15 +252,27 @@ def nb_rigid_contacts(self) -> int: """Get the number of rigid contacts""" return -1 - def marker_velocities(self, reference_index=None) -> list[MX]: + def markers_velocities(self, reference_index=None) -> list[MX]: """ - Get the marker velocities of the model + Get the marker velocities of the model, in the reference frame number reference_index args: q, qdot """ - def marker_accelerations(self, reference_index=None) -> list[MX]: + def marker_velocity(self, marker_index=None) -> list[MX]: """ - Get the marker accelerations of the model + Get the velocity of one marker from the model + args: q, qdot + """ + + def markers_accelerations(self, reference_index=None) -> list[MX]: + """ + Get the marker accelerations of the model, in the reference frame number reference_index + args: q, qdot, qddot + """ + + def marker_acceleration(self, marker_index=None) -> list[MX]: + """ + Get the acceleration of one marker from the model args: q, qdot, qddot """ @@ -290,7 +302,7 @@ def soft_contact_forces(self) -> Function: def normalize_state_quaternions(self) -> Function: """ Normalize the quaternions of the state - args: x (The state to normalize) + args: q (The joint generalized coordinates to normalize) """ def contact_forces(self) -> Function: diff --git a/tests/shard1/test_biorbd_multi_model.py b/tests/shard1/test_biorbd_multi_model.py index a9c76fad6..6043ddc94 100644 --- a/tests/shard1/test_biorbd_multi_model.py +++ b/tests/shard1/test_biorbd_multi_model.py @@ -292,7 +292,7 @@ def test_biorbd_model(): assert models.marker_index("marker_3") == 2 - markers_velocities = models.marker_velocities()( + markers_velocities = models.markers_velocities()( np.array([1, 2.1, 3, 4.1, 5, 6.1]), np.array([1, 2.1, 3, 4.1, 5, 6]), ) diff --git a/tests/shard6/test_global_stochastic_except_collocation.py b/tests/shard6/test_global_stochastic_except_collocation.py index 98c3fae01..80dc47e3b 100644 --- a/tests/shard6/test_global_stochastic_except_collocation.py +++ b/tests/shard6/test_global_stochastic_except_collocation.py @@ -268,7 +268,7 @@ def test_arm_reaching_torque_driven_explicit(use_sx): bioptim_folder = os.path.dirname(ocp_module.__file__) if use_sx: - with pytest.raises(RuntimeError, match=".*eval_sx not defined for LinsolQr"): + with pytest.raises(NotImplementedError, match="Wrong number or type of arguments for overloaded function 'MX_set'"): ocp = ocp_module.prepare_socp( biorbd_model_path=bioptim_folder + "/models/LeuvenArmModel.bioMod", final_time=final_time, diff --git a/tests/shard6/test_time_dependent_problems.py b/tests/shard6/test_time_dependent_problems.py index 3928f1e30..71cc4cac5 100644 --- a/tests/shard6/test_time_dependent_problems.py +++ b/tests/shard6/test_time_dependent_problems.py @@ -72,7 +72,7 @@ def time_dynamic( # You can directly call biorbd function (as for ddq) or call bioptim accessor (as for dq) dq = DynamicsFunctions.compute_qdot(nlp, q, qdot) - ddq = nlp.model.forward_dynamics(q, qdot, tau, []) + ddq = nlp.model.forward_dynamics(with_contact=False)(q, qdot, tau, [], []) return DynamicsEvaluation(dxdt=vertcat(dq, ddq), defects=None) From 9d7e8d3b189f3bef0565bb243eebb071c1eb6f83 Mon Sep 17 00:00:00 2001 From: eve-mac Date: Mon, 23 Sep 2024 09:57:00 -0400 Subject: [PATCH 025/108] blacked --- bioptim/dynamics/integrator.py | 4 +-- bioptim/models/biorbd/biorbd_model.py | 29 ++++++++++--------- bioptim/models/biorbd/multi_biorbd_model.py | 1 - ...st_global_stochastic_except_collocation.py | 4 ++- 4 files changed, 21 insertions(+), 17 deletions(-) diff --git a/bioptim/dynamics/integrator.py b/bioptim/dynamics/integrator.py index de5df59d6..924aa4ecb 100644 --- a/bioptim/dynamics/integrator.py +++ b/bioptim/dynamics/integrator.py @@ -312,7 +312,7 @@ def dxdt( t = self.t_span_sym[0] + self._integration_time * (i - 1) x[:, i] = self.next_x(t, x[:, i - 1], u, p, a, d) if self.model.nb_quaternions > 0: - x[:self.model.nb_q, i] = self.model.normalize_state_quaternions()(x[:self.model.nb_q, i]) + x[: self.model.nb_q, i] = self.model.normalize_state_quaternions()(x[: self.model.nb_q, i]) return x[:, -1], x @@ -534,7 +534,7 @@ def dxdt( ) if self.model.nb_quaternions > 0: - x_prev[:self.model.nb_q, 1] = self.model.normalize_state_quaternions(x_prev[:self.model.nb_q, 1]) + x_prev[: self.model.nb_q, 1] = self.model.normalize_state_quaternions(x_prev[: self.model.nb_q, 1]) return x_prev[:, 1], x_prev diff --git a/bioptim/models/biorbd/biorbd_model.py b/bioptim/models/biorbd/biorbd_model.py index 9af21a894..b22181863 100644 --- a/bioptim/models/biorbd/biorbd_model.py +++ b/bioptim/models/biorbd/biorbd_model.py @@ -608,8 +608,9 @@ def markers_velocities(self, reference_index=None) -> list[MX]: else: biorbd_return = [] - homogeneous_matrix_transposed = self.homogeneous_matrices_in_global(segment_idx=reference_index, - inverse=True)( + homogeneous_matrix_transposed = self.homogeneous_matrices_in_global( + segment_idx=reference_index, inverse=True + )( GeneralizedCoordinates(self.q), ) # TODO: Check and fix this portion of code @@ -626,10 +627,10 @@ def markers_velocities(self, reference_index=None) -> list[MX]: def marker_velocity(self, marker_index: int) -> list[MX]: biorbd_return = self.model.markersVelocity( - GeneralizedCoordinates(self.q), - GeneralizedVelocity(self.qdot), - True, - )[marker_index].to_mx() + GeneralizedCoordinates(self.q), + GeneralizedVelocity(self.qdot), + True, + )[marker_index].to_mx() casadi_fun = Function( "marker_velocity", [self.q, self.qdot, self.parameters], @@ -652,8 +653,10 @@ def markers_accelerations(self, reference_index=None) -> list[MX]: else: # TODO: Check and fix this portion of code biorbd_return = [] - homogeneous_matrix_transposed = self.homogeneous_matrices_in_global(segment_idx=reference_index, - inverse=True,)( + homogeneous_matrix_transposed = self.homogeneous_matrices_in_global( + segment_idx=reference_index, + inverse=True, + )( GeneralizedCoordinates(self.q), ) for m in self.model.markersAcceleration( @@ -673,11 +676,11 @@ def markers_accelerations(self, reference_index=None) -> list[MX]: def marker_acceleration(self, marker_index: int) -> list[MX]: biorbd_return = self.model.markerAcceleration( - GeneralizedCoordinates(self.q), - GeneralizedVelocity(self.qdot), - GeneralizedAcceleration(self.qddot), - True, - )[marker_index].to_mx() + GeneralizedCoordinates(self.q), + GeneralizedVelocity(self.qdot), + GeneralizedAcceleration(self.qddot), + True, + )[marker_index].to_mx() casadi_fun = Function( "marker_acceleration", [self.q, self.qdot, self.qddot, self.parameters], diff --git a/bioptim/models/biorbd/multi_biorbd_model.py b/bioptim/models/biorbd/multi_biorbd_model.py index 07611587e..d420fe9dd 100644 --- a/bioptim/models/biorbd/multi_biorbd_model.py +++ b/bioptim/models/biorbd/multi_biorbd_model.py @@ -729,7 +729,6 @@ def markers_velocities(self, reference_index=None) -> Function: ) return casadi_fun - def marker_velocity(self, marker_index: int) -> Function: biorbd_return = [] for i, model in enumerate(self.models): diff --git a/tests/shard6/test_global_stochastic_except_collocation.py b/tests/shard6/test_global_stochastic_except_collocation.py index 80dc47e3b..0380a1628 100644 --- a/tests/shard6/test_global_stochastic_except_collocation.py +++ b/tests/shard6/test_global_stochastic_except_collocation.py @@ -268,7 +268,9 @@ def test_arm_reaching_torque_driven_explicit(use_sx): bioptim_folder = os.path.dirname(ocp_module.__file__) if use_sx: - with pytest.raises(NotImplementedError, match="Wrong number or type of arguments for overloaded function 'MX_set'"): + with pytest.raises( + NotImplementedError, match="Wrong number or type of arguments for overloaded function 'MX_set'" + ): ocp = ocp_module.prepare_socp( biorbd_model_path=bioptim_folder + "/models/LeuvenArmModel.bioMod", final_time=final_time, From dd183903c27989f93d595a7361e3962045acb52a Mon Sep 17 00:00:00 2001 From: eve-mac Date: Mon, 23 Sep 2024 12:53:20 -0400 Subject: [PATCH 026/108] SOCP tests --- .../example_continuity_as_objective.py | 2 +- .../arm_reaching_muscle_driven.py | 9 +- .../arm_reaching_torque_driven_explicit.py | 4 +- .../examples/torque_driven_ocp/spring_load.py | 2 +- bioptim/limits/constraints.py | 28 ++-- bioptim/limits/multinode_penalty.py | 14 +- bioptim/models/biorbd/multi_biorbd_model.py | 122 ++++++++++-------- bioptim/models/holonomic_constraints.py | 2 +- ...st_global_stochastic_except_collocation.py | 14 -- 9 files changed, 94 insertions(+), 103 deletions(-) diff --git a/bioptim/examples/getting_started/example_continuity_as_objective.py b/bioptim/examples/getting_started/example_continuity_as_objective.py index 55a28e1ec..3e74ee469 100644 --- a/bioptim/examples/getting_started/example_continuity_as_objective.py +++ b/bioptim/examples/getting_started/example_continuity_as_objective.py @@ -44,7 +44,7 @@ def out_of_sphere(controller: PenaltyController, y, z): q = controller.states["q"].cx - marker_q = controller.model.markers()(q, controller.parameters_except_time.cx)[1] + marker_q = controller.model.marker(1)(q, controller.parameters_except_time.cx) distance = sqrt((y - marker_q[1]) ** 2 + (z - marker_q[2]) ** 2) diff --git a/bioptim/examples/stochastic_optimal_control/arm_reaching_muscle_driven.py b/bioptim/examples/stochastic_optimal_control/arm_reaching_muscle_driven.py index 92833482b..1b489f209 100644 --- a/bioptim/examples/stochastic_optimal_control/arm_reaching_muscle_driven.py +++ b/bioptim/examples/stochastic_optimal_control/arm_reaching_muscle_driven.py @@ -181,7 +181,7 @@ def minimize_uncertainty(controllers: list[PenaltyController], key: str) -> cas. def get_cov_mat(nlp, node_index): - dt = nlp.dt_mx + dt = nlp.dt nlp.states.node_index = node_index - 1 nlp.controls.node_index = node_index - 1 @@ -194,7 +194,7 @@ def get_cov_mat(nlp, node_index): sensory_noise = nlp.parameters["sensory_noise"].cx motor_noise = nlp.parameters["motor_noise"].cx sigma_w = cas.vertcat(sensory_noise, motor_noise) * CX_eye(6) - cov_sym = cas.MX.sym("cov", nlp.integrated_values.cx.shape[0]) + cov_sym = nlp.cx.sym("cov", nlp.integrated_values.cx.shape[0]) cov_matrix = StochasticBioModel.reshape_to_matrix(cov_sym, nlp.model.matrix_shape_cov) dx = stochastic_forward_dynamics( @@ -302,12 +302,13 @@ def expected_feedback_effort(controllers: list[PenaltyController], sensory_noise Magnitude of the sensory noise. """ dt = controllers[0].dt.cx - sensory_noise_matrix = sensory_noise_magnitude * cas.MX_eye(4) + CX_eye = cas.MX_eye if controllers[0].cx == cas.MX else cas.SX_eye + sensory_noise_matrix = sensory_noise_magnitude * CX_eye(4) # create the casadi function to be evaluated # Get the symbolic variables ref = controllers[0].algebraic_states["ref"].cx - cov_sym = cas.MX.sym("cov", controllers[0].integrated_values.cx.shape[0]) + cov_sym = controllers[0].cx.sym("cov", controllers[0].integrated_values.cx.shape[0]) cov_matrix = StochasticBioModel.reshape_to_matrix(cov_sym, controllers[0].model.matrix_shape_cov) k = controllers[0].algebraic_states["k"].cx diff --git a/bioptim/examples/stochastic_optimal_control/arm_reaching_torque_driven_explicit.py b/bioptim/examples/stochastic_optimal_control/arm_reaching_torque_driven_explicit.py index 5817d10cb..e44c410df 100644 --- a/bioptim/examples/stochastic_optimal_control/arm_reaching_torque_driven_explicit.py +++ b/bioptim/examples/stochastic_optimal_control/arm_reaching_torque_driven_explicit.py @@ -183,11 +183,11 @@ def get_cov_mat(nlp, node_index, use_sx): M_matrix = StochasticBioModel.reshape_to_matrix(nlp.algebraic_states["m"].cx, nlp.model.matrix_shape_m) - CX_eye = cas.SX_eye if use_sx else cas.DM_eye + CX_eye = cas.SX_eye if use_sx else cas.MX_eye sensory_noise = nlp.parameters["sensory_noise"].cx motor_noise = nlp.parameters["motor_noise"].cx sigma_w = cas.vertcat(sensory_noise, motor_noise) * CX_eye(cas.vertcat(sensory_noise, motor_noise).shape[0]) - cov_sym = cas.MX.sym("cov", nlp.integrated_values.cx.shape[0]) + cov_sym = nlp.cx.sym("cov", nlp.integrated_values.cx.shape[0]) cov_matrix = StochasticBioModel.reshape_to_matrix(cov_sym, nlp.model.matrix_shape_cov) dx = stochastic_forward_dynamics( diff --git a/bioptim/examples/torque_driven_ocp/spring_load.py b/bioptim/examples/torque_driven_ocp/spring_load.py index a53573751..4d839df9d 100644 --- a/bioptim/examples/torque_driven_ocp/spring_load.py +++ b/bioptim/examples/torque_driven_ocp/spring_load.py @@ -151,7 +151,7 @@ def custom_dynamic( stiffness = 100 force_vector[5] = -sign(q[0]) * stiffness * q[0] ** 2 # traction-compression spring - qddot = nlp.model.forward_dynamics(q, qdot, tau, force_vector) + qddot = nlp.model.forward_dynamics(with_contact=False)(q, qdot, tau, force_vector, []) return DynamicsEvaluation(dxdt=vertcat(qdot, qddot), defects=None) diff --git a/bioptim/limits/constraints.py b/bioptim/limits/constraints.py index f1157a6e5..e240b5850 100644 --- a/bioptim/limits/constraints.py +++ b/bioptim/limits/constraints.py @@ -683,27 +683,21 @@ def stochastic_df_dx_implicit( controller.algebraic_states["a"].cx, controller.model.matrix_shape_a ) - q_roots = controller.states["q_roots"].cx - q_joints = controller.states["q_joints"].cx - qdot_roots = controller.states["qdot_roots"].cx - qdot_joints = controller.states["qdot_joints"].cx - tau_joints = controller.controls["tau_joints"].cx - - # q_root = MX.sym("q_root", nb_root, 1) - # q_joints = MX.sym("q_joints", nu, 1) - # qdot_root = MX.sym("qdot_root", nb_root, 1) - # qdot_joints = MX.sym("qdot_joints", nu, 1) - # tau_joints = MX.sym("tau_joints", nu, 1) - # algebraic_states_sym = MX.sym("algebraic_states_sym", controller.algebraic_states.shape, 1) - # numerical_timeseries_sym = MX.sym("numerical_timeseries_sym", controller.numerical_timeseries.shape, 1) + q_roots = controller.cx.sym("q_roots", nb_root, 1) + q_joints = controller.cx.sym("q_joints", nu, 1) + qdot_roots = controller.cx.sym("qdot_roots", nb_root, 1) + qdot_joints = controller.cx.sym("qdot_joints", nu, 1) + tau_joints = controller.cx.sym("tau_joints", nu, 1) + algebraic_states_sym = controller.cx.sym("algebraic_states_sym", controller.algebraic_states.shape, 1) + numerical_timeseries_sym = controller.cx.sym("numerical_timeseries_sym", controller.numerical_timeseries.shape, 1) dx = controller.extra_dynamics(0)( controller.t_span.cx, vertcat(q_roots, q_joints, qdot_roots, qdot_joints), # States tau_joints, controller.parameters.cx, - controller.algebraic_states.cx, - controller.numerical_timeseries.cx, + algebraic_states_sym, + numerical_timeseries_sym, ) non_root_index = list(range(nb_root, nb_root + nu)) + list( @@ -719,8 +713,8 @@ def stochastic_df_dx_implicit( qdot_joints, tau_joints, controller.parameters.cx, - controller.algebraic_states.cx, - controller.numerical_timeseries.cx, + algebraic_states_sym, + numerical_timeseries_sym, ], [jacobian(dx[non_root_index], vertcat(q_joints, qdot_joints))], ) diff --git a/bioptim/limits/multinode_penalty.py b/bioptim/limits/multinode_penalty.py index 3a3629def..813a46468 100644 --- a/bioptim/limits/multinode_penalty.py +++ b/bioptim/limits/multinode_penalty.py @@ -557,13 +557,13 @@ def stochastic_df_dw_implicit( controllers[0].algebraic_states["c"].cx, controllers[0].model.matrix_shape_c ) - q_root = MX.sym("q_root", nb_root, 1) - q_joints = MX.sym("q_joints", nu, 1) - qdot_root = MX.sym("qdot_root", nb_root, 1) - qdot_joints = MX.sym("qdot_joints", nu, 1) - tau_joints = MX.sym("tau_joints", nu, 1) - algebraic_states_sym = MX.sym("algebraic_states_sym", controllers[0].algebraic_states.shape, 1) - numerical_timeseries_sym = MX.sym("numerical_timeseries_sym", controllers[0].numerical_timeseries.shape, 1) + q_root = controllers[0].cx.sym("q_root", nb_root, 1) + q_joints = controllers[0].cx.sym("q_joints", nu, 1) + qdot_root = controllers[0].cx.sym("qdot_root", nb_root, 1) + qdot_joints = controllers[0].cx.sym("qdot_joints", nu, 1) + tau_joints = controllers[0].cx.sym("tau_joints", nu, 1) + algebraic_states_sym = controllers[0].cx.sym("algebraic_states_sym", controllers[0].algebraic_states.shape, 1) + numerical_timeseries_sym = controllers[0].cx.sym("numerical_timeseries_sym", controllers[0].numerical_timeseries.shape, 1) dx = controllers[0].extra_dynamics(0)( controllers[0].time.cx, diff --git a/bioptim/models/biorbd/multi_biorbd_model.py b/bioptim/models/biorbd/multi_biorbd_model.py index d420fe9dd..66410ca60 100644 --- a/bioptim/models/biorbd/multi_biorbd_model.py +++ b/bioptim/models/biorbd/multi_biorbd_model.py @@ -51,6 +51,9 @@ def __init__( self.models.append(BiorbdModel(model)) elif isinstance(model, BiorbdModel): self.models.append(model) + if model.parameters is not None: + raise NotImplementedError( + "MultiBiorbdModel does not handle parameters yet. Please use BiorbdModel instead.") else: raise ValueError("The models should be of type 'str', 'biorbd.Model' or 'bioptim.BiorbdModel'") @@ -76,6 +79,8 @@ def __init__( self.qddot_joints = MX.sym("qddot_joints_mx", self.nb_qddot - self.nb_root, 1) self.tau = MX.sym("tau_mx", self.nb_tau, 1) self.muscle = MX.sym("muscle_mx", self.nb_muscles, 1) + # TODO: parameters should be handled model by model + self.parameters = MX.sym("parameters_to_be_implemented", 0, 1) def __getitem__(self, index): return self.models[index] @@ -230,10 +235,10 @@ def nb_extra_models(self) -> int: @property def gravity(self) -> Function: - biorbd_return = vertcat(*(model.gravity()["o0"] for model in self.models)) + biorbd_return = vertcat(*(model.gravity()(self.parameters)["o0"] for model in self.models)) casadi_fun = Function( "gravity", - [MX()], + [self.parameters], [biorbd_return], ) return casadi_fun @@ -286,25 +291,25 @@ def segments(self) -> tuple[biorbd.Segment, ...]: def homogeneous_matrices_in_global(self, segment_idx, inverse=False) -> Function: local_segment_id, model_id = self.local_variable_id("segment", segment_idx) q_model = self.models[model_id].q - biorbd_return = self.models[model_id].homogeneous_matrices_in_global(local_segment_id, inverse)(q_model) + biorbd_return = self.models[model_id].homogeneous_matrices_in_global(local_segment_id, inverse)(q_model, self.parameters) casadi_fun = Function( "homogeneous_matrices_in_global", - [self.models[model_id].q], + [self.models[model_id].q, self.parameters], [biorbd_return], ) return casadi_fun def homogeneous_matrices_in_child(self, segment_id) -> Function: local_id, model_id = self.local_variable_id("segment", segment_id) - casadi_fun = self.models[model_id].homogeneous_matrices_in_child(local_id) + casadi_fun = self.models[model_id].homogeneous_matrices_in_child(local_id)(self.parameters) return casadi_fun @property def mass(self) -> Function: - biorbd_return = vertcat(*(model.mass()["o0"] for model in self.models)) + biorbd_return = vertcat(*(model.mass()(self.parameters)["o0"] for model in self.models)) casadi_fun = Function( "mass", - [MX()], + [self.parameters], [biorbd_return], ) return casadi_fun @@ -313,10 +318,10 @@ def center_of_mass(self) -> Function: biorbd_return = MX() for i, model in enumerate(self.models): q_model = self.q[self.variable_index("q", i)] - biorbd_return = vertcat(biorbd_return, model.center_of_mass()(q_model)) + biorbd_return = vertcat(biorbd_return, model.center_of_mass()(q_model, self.parameters)) casadi_fun = Function( "center_of_mass", - [self.q], + [self.q, self.parameters], [biorbd_return], ) return casadi_fun @@ -328,11 +333,11 @@ def center_of_mass_velocity(self) -> Function: qdot_model = self.qdot[self.variable_index("qdot", i)] biorbd_return = vertcat( biorbd_return, - model.center_of_mass_velocity()(q_model, qdot_model), + model.center_of_mass_velocity()(q_model, qdot_model, self.parameters), ) casadi_fun = Function( "center_of_mass_velocity", - [self.q, self.qdot], + [self.q, self.qdot, self.parameters], [biorbd_return], ) return casadi_fun @@ -345,11 +350,11 @@ def center_of_mass_acceleration(self) -> Function: qddot_model = self.qddot[self.variable_index("qddot", i)] biorbd_return = vertcat( biorbd_return, - model.center_of_mass_acceleration()(q_model, qdot_model, qddot_model), + model.center_of_mass_acceleration()(q_model, qdot_model, qddot_model, self.parameters), ) 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 @@ -358,10 +363,10 @@ def mass_matrix(self) -> Function: biorbd_return = [] for i, model in enumerate(self.models): q_model = self.q[self.variable_index("q", i)] - biorbd_return += [model.mass_matrix()(q_model)] + biorbd_return += [model.mass_matrix()(q_model, self.parameters)] casadi_fun = Function( "mass_matrix", - [self.q], + [self.q, self.parameters], biorbd_return, ) return casadi_fun @@ -371,10 +376,10 @@ def non_linear_effects(self) -> Function: for i, model in enumerate(self.models): q_model = self.q[self.variable_index("q", i)] qdot_model = self.qdot[self.variable_index("qdot", i)] - biorbd_return += [model.non_linear_effects()(q_model, qdot_model)] + biorbd_return += [model.non_linear_effects()(q_model, qdot_model, self.parameters)] casadi_fun = Function( "non_linear_effects", - [self.q, self.qdot], + [self.q, self.qdot, self.parameters], biorbd_return, ) return casadi_fun @@ -386,11 +391,11 @@ def angular_momentum(self) -> Function: qdot_model = self.qdot[self.variable_index("qdot", i)] biorbd_return = vertcat( biorbd_return, - model.angular_momentum()(q_model, qdot_model), + model.angular_momentum()(q_model, qdot_model, self.parameters), ) casadi_fun = Function( "angular_momentum", - [self.q, self.qdot], + [self.q, self.qdot, self.parameters], [biorbd_return], ) return casadi_fun @@ -402,11 +407,11 @@ def reshape_qdot(self, k_stab=1) -> Function: qdot_model = self.qdot[self.variable_index("qdot", i)] biorbd_return = vertcat( biorbd_return, - model.reshape_qdot(k_stab)(q_model, qdot_model), + model.reshape_qdot(k_stab)(q_model, qdot_model, self.parameters), ) casadi_fun = Function( "reshape_qdot", - [self.q, self.qdot], + [self.q, self.qdot, self.parameters], [biorbd_return], ) return casadi_fun @@ -418,11 +423,11 @@ def segment_angular_velocity(self, idx) -> Function: qdot_model = self.qdot[self.variable_index("qdot", i)] biorbd_return = vertcat( biorbd_return, - model.segment_angular_velocity(idx)(q_model, qdot_model), + model.segment_angular_velocity(idx)(q_model, qdot_model, self.parameters), ) casadi_fun = Function( "segment_angular_velocity", - [self.q, self.qdot], + [self.q, self.qdot, self.parameters], [biorbd_return], ) return casadi_fun @@ -474,11 +479,12 @@ def torque(self) -> Function: tau_activations_model, q_model, qdot_model, + self.parameters, ), ) casadi_fun = Function( "torque", - [self.muscle, self.q, self.qdot], + [self.muscle, self.q, self.qdot, self.parameters], [biorbd_return], ) return casadi_fun @@ -495,11 +501,12 @@ def forward_dynamics_free_floating_base(self) -> Function: q_model, qdot_model, qddot_joints_model, + self.parameters, ), ) 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 @@ -536,11 +543,12 @@ def forward_dynamics(self, with_contact) -> Function: qdot_model, tau_model, [], + self.parameters, ), ) casadi_fun = Function( "forward_dynamics", - [self.q, self.qdot, self.tau], + [self.q, self.qdot, self.tau, [], self.parameters], [biorbd_return], ) return casadi_fun @@ -555,11 +563,11 @@ def inverse_dynamics(self) -> Function: qddot_model = self.qddot[self.variable_index("qddot", i)] biorbd_return = vertcat( biorbd_return, - model.inverse_dynamics()(q_model, qdot_model, qddot_model, []), + model.inverse_dynamics()(q_model, qdot_model, qddot_model, self.parameters), ) casadi_fun = Function( "inverse_dynamics", - [self.q, self.qdot, self.qddot], + [self.q, self.qdot, self.qddot, self.parameters], [biorbd_return], ) return casadi_fun @@ -578,11 +586,12 @@ def contact_forces_from_constrained_forward_dynamics(self) -> Function: qdot_model, tau_model, [], + self.parameters, ), ) casadi_fun = Function( "contact_forces_from_constrained_forward_dynamics", - [self.q, self.qdot, self.tau], + [self.q, self.qdot, self.tau, [], self.parameters], [biorbd_return], ) return casadi_fun @@ -597,11 +606,12 @@ def qdot_from_impact(self) -> Function: model.qdot_from_impact()( q_model, qdot_pre_impact_model, + self.parameters, ), ) casadi_fun = Function( "qdot_from_impact", - [self.q, self.qdot], + [self.q, self.qdot, self.parameters], [biorbd_return], ) return casadi_fun @@ -615,7 +625,7 @@ def muscle_activation_dot(self) -> Function: biorbd_return = vertcat(biorbd_return, model.model.activationDot(muscle_states).to_mx()) casadi_fun = Function( "muscle_activation_dot", - [self.muscle], + [self.muscle, self.parameters], [biorbd_return], ) return casadi_fun @@ -625,7 +635,7 @@ def muscle_joint_torque(self) -> Function: for i, model in enumerate(self.models): muscles_states = model.model.stateSet() # still call from Biorbd for k in range(model.nb_muscles): - muscles_states[k].setActivation(self.activations[k]) + muscles_states[k].setActivation(self.muscle[k]) q_model = self.q[self.variable_index("q", i)] qdot_model = self.qdot[self.variable_index("qdot", i)] biorbd_return = vertcat( @@ -633,7 +643,7 @@ def muscle_joint_torque(self) -> Function: ) 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 @@ -642,11 +652,11 @@ def markers(self) -> Function: biorbd_return = [] for i, model in enumerate(self.models): q_model = self.q[self.variable_index("q", i)] - biorbd_return += [model.markers()(q_model)] + biorbd_return += [model.markers()(q_model, self.parameters)] biorbd_return = [item for sublist in biorbd_return for item in sublist] casadi_fun = Function( "markers", - [self.q], + [self.q, self.parameters], [horzcat(*biorbd_return)], ) return casadi_fun @@ -666,10 +676,10 @@ def marker_index(self, name): def marker(self, index, reference_segment_index=None) -> Function: local_marker_id, model_id = self.local_variable_id("markers", index) q_model = self.q[self.variable_index("q", model_id)] - biorbd_return = self.models[model_id].marker(local_marker_id, reference_segment_index)(q_model) + biorbd_return = self.models[model_id].marker(local_marker_id, reference_segment_index)(q_model, self.parameters) casadi_fun = Function( "marker", - [self.q], + [self.q, self.parameters], [biorbd_return], ) return casadi_fun @@ -720,11 +730,11 @@ def markers_velocities(self, reference_index=None) -> Function: for i, model in enumerate(self.models): q_model = self.q[self.variable_index("q", i)] qdot_model = self.qdot[self.variable_index("qdot", i)] - biorbd_return += [model.markers_velocities(reference_index)(q_model, qdot_model)] + biorbd_return += [model.markers_velocities(reference_index)(q_model, qdot_model, self.parameters)] biorbd_return = [item for sublist in biorbd_return for item in sublist] casadi_fun = Function( "markers_velocities", - [self.q, self.qdot], + [self.q, self.qdot, self.parameters], [horzcat(*biorbd_return)], ) return casadi_fun @@ -734,11 +744,11 @@ def marker_velocity(self, marker_index: int) -> Function: for i, model in enumerate(self.models): q_model = self.q[self.variable_index("q", i)] qdot_model = self.qdot[self.variable_index("qdot", i)] - biorbd_return += [model.marker_velocity(marker_index)(q_model, qdot_model)] + biorbd_return += [model.marker_velocity(marker_index)(q_model, qdot_model, self.parameters)] biorbd_return = [item for sublist in biorbd_return for item in sublist] casadi_fun = Function( "marker_velocity", - [self.q, self.qdot], + [self.q, self.qdot, self.parameters], [horzcat(*biorbd_return)], ) return casadi_fun @@ -749,12 +759,12 @@ def tau_max(self) -> Function: for i, model in enumerate(self.models): q_model = self.q[self.variable_index("q", i)] qdot_model = self.qdot[self.variable_index("qdot", i)] - torque_max, torque_min = model.tau_max()(q_model, qdot_model) + torque_max, torque_min = model.tau_max()(q_model, qdot_model, self.parameters) out_max = vertcat(out_max, torque_max) out_min = vertcat(out_min, torque_min) casadi_fun = Function( "tau_max", - [self.q, self.qdot], + [self.q, self.qdot, self.parameters], [out_max, out_min], ) return casadi_fun @@ -770,11 +780,11 @@ def rigid_contact_acceleration(self, contact_index, contact_axis) -> Function: qdot_model = self.qdot[self.variable_index("qdot", model_idx)] qddot_model = self.qddot[self.variable_index("qddot", model_idx)] biorbd_return = model_selected.rigid_contact_acceleration(contact_index, contact_axis)( - q_model, qdot_model, qddot_model + q_model, qdot_model, qddot_model, self.parameters ) 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 @@ -787,16 +797,16 @@ def nb_dof(self) -> int: def marker_names(self) -> tuple[str, ...]: return tuple([name for model in self.models for name in model.marker_names]) - def soft_contact_forces(self, q, qdot) -> Function: + def soft_contact_forces(self) -> Function: biorbd_return = MX() for i, model in enumerate(self.models): - q_model = q[self.variable_index("q", i)] - qdot_model = qdot[self.variable_index("qdot", i)] - soft_contact_forces = model.soft_contact_forces()(q_model, qdot_model) + q_model = self.q[self.variable_index("q", i)] + qdot_model = self.qdot[self.variable_index("qdot", i)] + soft_contact_forces = model.soft_contact_forces()(q_model, qdot_model, self.parameters) biorbd_return = vertcat(biorbd_return, soft_contact_forces) casadi_fun = Function( "soft_contact_forces", - [self.q, self.qdot], + [self.q, self.qdot, self.parameters], [biorbd_return], ) return casadi_fun @@ -825,11 +835,11 @@ def contact_forces(self) -> Function: qdot_model = self.qdot[self.variable_index("qdot", i)] tau_model = self.tau[self.variable_index("tau", i)] - contact_forces = model.contact_forces()(q_model, qdot_model, tau_model, []) + contact_forces = model.contact_forces()(q_model, qdot_model, tau_model, self.parameters) biorbd_return = vertcat(biorbd_return, contact_forces) casadi_fun = Function( "contact_forces", - [self.q, self.qdot, self.tau], + [self.q, self.qdot, self.tau, self.parameters], [biorbd_return], ) return casadi_fun @@ -839,10 +849,10 @@ def passive_joint_torque(self) -> Function: for i, model in enumerate(self.models): q_model = self.q[self.variable_index("q", i)] qdot_model = self.qdot[self.variable_index("qdot", i)] - biorbd_return = vertcat(biorbd_return, model.passive_joint_torque()(q_model, qdot_model)) + biorbd_return = vertcat(biorbd_return, model.passive_joint_torque()(q_model, qdot_model, self.parameters)) casadi_fun = Function( "passive_joint_torque", - [self.q, self.qdot], + [self.q, self.qdot, self.parameters], [biorbd_return], ) return casadi_fun @@ -852,10 +862,10 @@ def ligament_joint_torque(self) -> Function: for i, model in enumerate(self.models): q_model = self.q[self.variable_index("q", i)] qdot_model = self.qdot[self.variable_index("qdot", i)] - biorbd_return = vertcat(biorbd_return, model.ligament_joint_torque()(q_model, qdot_model)) + biorbd_return = vertcat(biorbd_return, model.ligament_joint_torque()(q_model, qdot_model, self.parameters)) 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/holonomic_constraints.py b/bioptim/models/holonomic_constraints.py index e9a4f48ab..b1749adcd 100644 --- a/bioptim/models/holonomic_constraints.py +++ b/bioptim/models/holonomic_constraints.py @@ -63,7 +63,7 @@ def superimpose_markers( # if local frame is provided, the markers are expressed in the same local frame if local_frame_index is not None: - jcs_t = biorbd_model.homogeneous_matrices_in_global(segment_idx=local_frame_index, inverse=True)(q_sym) + jcs_t = biorbd_model.homogeneous_matrices_in_global(segment_idx=local_frame_index, inverse=True)(q_sym, []) marker_1_sym = (jcs_t @ vertcat(marker_1_sym, 1))[:3] marker_2_sym = (jcs_t @ vertcat(marker_2_sym, 1))[:3] diff --git a/tests/shard6/test_global_stochastic_except_collocation.py b/tests/shard6/test_global_stochastic_except_collocation.py index 0380a1628..71d48ca5e 100644 --- a/tests/shard6/test_global_stochastic_except_collocation.py +++ b/tests/shard6/test_global_stochastic_except_collocation.py @@ -28,20 +28,6 @@ def test_arm_reaching_muscle_driven(use_sx): wPqdot_magnitude = DM(np.array([wPqdot_std**2 / dt, wPqdot_std**2 / dt])) sensory_noise_magnitude = vertcat(wPq_magnitude, wPqdot_magnitude) - if use_sx: - with pytest.raises(RuntimeError, match=".*eval_sx not defined for LinsolQr"): - ocp = ocp_module.prepare_socp( - final_time=final_time, - n_shooting=n_shooting, - hand_final_position=hand_final_position, - motor_noise_magnitude=motor_noise_magnitude, - sensory_noise_magnitude=sensory_noise_magnitude, - force_field_magnitude=force_field_magnitude, - example_type=example_type, - use_sx=use_sx, - ) - return - ocp = ocp_module.prepare_socp( final_time=final_time, n_shooting=n_shooting, From bfdb7810af60d11dc6e9ffdfee3b8870fffbecc8 Mon Sep 17 00:00:00 2001 From: eve-mac Date: Mon, 23 Sep 2024 13:08:44 -0400 Subject: [PATCH 027/108] completely removed mx --- bioptim/dynamics/configure_new_variable.py | 58 ------------------- .../muscle_activations_tracker.py | 6 -- .../arm_reaching_torque_driven_explicit.py | 5 -- bioptim/limits/constraints.py | 2 +- bioptim/limits/penalty_controller.py | 9 +-- bioptim/optimization/optimization_variable.py | 21 +++---- bioptim/optimization/parameters.py | 2 +- tests/shard4/test_penalty.py | 2 +- tests/shard6/test_unit_solver_interface.py | 4 +- tests/utils.py | 5 -- 10 files changed, 15 insertions(+), 99 deletions(-) diff --git a/bioptim/dynamics/configure_new_variable.py b/bioptim/dynamics/configure_new_variable.py index 733e14a7f..9a4f2e364 100644 --- a/bioptim/dynamics/configure_new_variable.py +++ b/bioptim/dynamics/configure_new_variable.py @@ -120,12 +120,6 @@ def __init__( self.copy_states_dot = False self.copy_controls = False - # # todo: Charbie - # self.mx_states = None - # self.mx_states_dot = None - # self.mx_controls = None - # self.mx_algebraic_states = None - self._check_combine_state_control_plot() if _manage_fatigue_to_new_variable(name, name_elements, ocp, nlp, as_states, as_controls, fatigue): @@ -329,54 +323,6 @@ def _declare_variable_scaling(self): self.name, scaling=np.ones(len(self.nlp.variable_mappings[self.name].to_first.map_idx)) ) - # def _use_copy(self): - # """Use of states[0] and controls[0] is permitted since nlp.phase_dynamics - # is PhaseDynamics.SHARED_DURING_THE_PHASE""" - # # todo: Charbie - # self.mx_states = ( - # [] if not self.copy_states else [self.ocp.nlp[self.nlp.use_states_from_phase_idx].states[0][self.name].mx] - # ) - # self.mx_states_dot = ( - # [] - # if not self.copy_states_dot - # else [self.ocp.nlp[self.nlp.use_states_dot_from_phase_idx].states_dot[0][self.name].mx] - # ) - # self.mx_controls = ( - # [] - # if not self.copy_controls - # else [self.ocp.nlp[self.nlp.use_controls_from_phase_idx].controls[0][self.name].mx] - # ) - # self.mx_algebraic_states = ( - # [] - # if not self.copy_algebraic_states - # else [self.ocp.nlp[self.nlp.use_states_from_phase_idx].algebraic_states[0][self.name].mx] - # ) - # - # # todo: if mapping on variables, what do we do with mapping on the nodes - # for i in self.nlp.variable_mappings[self.name].to_second.map_idx: - # var_name = ( - # f"{'-' if np.sign(i) < 0 else ''}{self.name}_{self.name_elements[abs(i)]}_MX" - # if i is not None - # else "zero" - # ) - # - # if not self.copy_states: - # self.mx_states.append(MX.sym(var_name, 1, 1)) - # - # if not self.copy_states_dot: - # self.mx_states_dot.append(MX.sym(var_name, 1, 1)) - # - # if not self.copy_controls: - # self.mx_controls.append(MX.sym(var_name, 1, 1)) - # - # self.mx_algebraic_states.append(MX.sym(var_name, 1, 1)) - # - # # todo: Charbie - # self.mx_states = vertcat(*self.mx_states) - # self.mx_states_dot = vertcat(*self.mx_states_dot) - # self.mx_controls = vertcat(*self.mx_controls) - # self.mx_algebraic_states = vertcat(*self.mx_algebraic_states) - def _declare_auto_axes_idx(self): """Declare the axes index if not already declared""" if not self.axes_idx: @@ -415,7 +361,6 @@ def _declare_cx_and_plot(self): self.name, cx[0], cx_scaled[0], - None, # self.mx_states, self.nlp.variable_mappings[self.name], node_index, ) @@ -450,7 +395,6 @@ def _declare_cx_and_plot(self): self.name, cx[0], cx_scaled[0], - None, # self.mx_controls, self.nlp.variable_mappings[self.name], node_index, ) @@ -492,7 +436,6 @@ def _declare_cx_and_plot(self): self.name, cx[0], cx_scaled[0], - None, # self.mx_states_dot, self.nlp.variable_mappings[self.name], node_index, ) @@ -517,7 +460,6 @@ def _declare_cx_and_plot(self): self.name, cx[0], cx_scaled[0], - None, # self.mx_states, self.nlp.variable_mappings[self.name], node_index, ) diff --git a/bioptim/examples/muscle_driven_ocp/muscle_activations_tracker.py b/bioptim/examples/muscle_driven_ocp/muscle_activations_tracker.py index e32a97f06..3f79c7dd8 100644 --- a/bioptim/examples/muscle_driven_ocp/muscle_activations_tracker.py +++ b/bioptim/examples/muscle_driven_ocp/muscle_activations_tracker.py @@ -110,7 +110,6 @@ def generate_data( name="q", cx=[symbolic_q, symbolic_q, symbolic_q], cx_scaled=[symbolic_q, symbolic_q, symbolic_q], - mx=symbolic_q, mapping=nlp.variable_mappings["q"], node_index=node_index, ) @@ -118,7 +117,6 @@ def generate_data( name="qdot", cx=[symbolic_qdot, symbolic_qdot, symbolic_qdot], cx_scaled=[symbolic_qdot, symbolic_qdot, symbolic_qdot], - mx=symbolic_qdot, mapping=nlp.variable_mappings["qdot"], node_index=node_index, ) @@ -127,7 +125,6 @@ def generate_data( name="qdot", cx=[symbolic_qdot, symbolic_qdot, symbolic_qdot], cx_scaled=[symbolic_qdot, symbolic_qdot, symbolic_qdot], - mx=symbolic_qdot, mapping=nlp.variable_mappings["qdot"], node_index=node_index, ) @@ -135,7 +132,6 @@ def generate_data( name="qddot", cx=[symbolic_qddot, symbolic_qddot, symbolic_qddot], cx_scaled=[symbolic_qddot, symbolic_qddot, symbolic_qddot], - mx=symbolic_qddot, mapping=nlp.variable_mappings["qddot"], node_index=node_index, ) @@ -145,7 +141,6 @@ def generate_data( name="tau", cx=[symbolic_tau, symbolic_tau, symbolic_tau], cx_scaled=[symbolic_tau, symbolic_tau, symbolic_tau], - mx=symbolic_tau, mapping=nlp.variable_mappings["tau"], node_index=node_index, ) @@ -153,7 +148,6 @@ def generate_data( name="muscles", cx=[symbolic_mus, symbolic_mus, symbolic_mus], cx_scaled=[symbolic_mus, symbolic_mus, symbolic_mus], - mx=symbolic_mus, mapping=nlp.variable_mappings["muscles"], node_index=node_index, ) diff --git a/bioptim/examples/stochastic_optimal_control/arm_reaching_torque_driven_explicit.py b/bioptim/examples/stochastic_optimal_control/arm_reaching_torque_driven_explicit.py index e44c410df..e5c7229a5 100644 --- a/bioptim/examples/stochastic_optimal_control/arm_reaching_torque_driven_explicit.py +++ b/bioptim/examples/stochastic_optimal_control/arm_reaching_torque_driven_explicit.py @@ -200,11 +200,6 @@ def get_cov_mat(nlp, node_index, use_sx): nlp, with_noise=True, ) - # dx.dxdt = cas.Function( - # "tp", - # [nlp.states.cx, nlp.controls.cx, nlp.parameters.cx, nlp.algebraic_states.cx, nlp.numerical_timeseries.cx], - # [dx.dxdt], - # )(nlp.states.cx, nlp.controls.cx, nlp.parameters.cx, nlp.algebraic_states.cx, nlp.numerical_timeseries.cx) ddx_dwm = cas.jacobian(dx.dxdt, cas.vertcat(sensory_noise, motor_noise)) dg_dw = -ddx_dwm * dt diff --git a/bioptim/limits/constraints.py b/bioptim/limits/constraints.py index e240b5850..d5fe4ec5c 100644 --- a/bioptim/limits/constraints.py +++ b/bioptim/limits/constraints.py @@ -566,7 +566,7 @@ def tau_from_muscle_equal_inverse_dynamics( raise NotImplementedError( "This implicit constraint tau_from_muscle_equal_inverse_dynamics is not implemented yet with external forces" ) - # Todo: add fext tau_id = nlp.model.inverse_dynamics(q, qdot, qddot, fext).to_mx() + # Todo: add fext tau_id = nlp.model.inverse_dynamics()(q, qdot, qddot, fext) # fext need to be a mx tau_id = controller.model.inverse_dynamics()( diff --git a/bioptim/limits/penalty_controller.py b/bioptim/limits/penalty_controller.py index f833420de..39c52b331 100644 --- a/bioptim/limits/penalty_controller.py +++ b/bioptim/limits/penalty_controller.py @@ -129,13 +129,12 @@ def t_span(self) -> OptimizationVariable: ------- """ - # mx = vertcat(self.time.mx, self.dt.mx) cx = vertcat(self.time.cx, self.dt.cx) tp = OptimizationVariableList(self._nlp.cx, self._nlp.phase_dynamics == PhaseDynamics.SHARED_DURING_THE_PHASE) n_val = cx.shape[0] tp.append( - "t_span", mx=None, cx=[cx, cx, cx], bimapping=BiMapping(to_second=range(n_val), to_first=range(n_val)) + "t_span", cx=[cx, cx, cx], bimapping=BiMapping(to_second=range(n_val), to_first=range(n_val)) ) return tp["t_span"] @@ -149,7 +148,6 @@ def phases_dt(self) -> OptimizationVariable: n_val = self.ocp.dt_parameter.cx.shape[0] tp.append( "phases_dt", - mx=None, # self.ocp.dt_parameter.mx, cx=[self.ocp.dt_parameter.cx, self.ocp.dt_parameter.cx, self.ocp.dt_parameter.cx], bimapping=BiMapping(to_second=range(n_val), to_first=range(n_val)), ) @@ -166,7 +164,6 @@ def dt(self) -> OptimizationVariable: n_val = self._nlp.dt.shape[0] tp.append( "dt", - mx=None, # self._nlp.dt_mx, cx=[self._nlp.dt, self._nlp.dt, self._nlp.dt], bimapping=BiMapping(to_second=range(n_val), to_first=range(n_val)), ) @@ -182,7 +179,6 @@ def time(self) -> OptimizationVariable: n_val = self._nlp.time_cx.shape[0] tp.append( "time", - mx=None, # self._nlp.time_mx, cx=[self._nlp.time_cx, self._nlp.time_cx, self._nlp.time_cx], bimapping=BiMapping(to_second=range(n_val), to_first=range(n_val)), ) @@ -198,7 +194,6 @@ def tf(self) -> OptimizationVariable: n_val = self._nlp.tf.shape[0] tp.append( "tf", - mx=None, # self._nlp.tf_mx, cx=[self._nlp.tf, self._nlp.tf, self._nlp.tf], bimapping=BiMapping(to_second=range(n_val), to_first=range(n_val)), ) @@ -415,7 +410,6 @@ def q(self) -> OptimizationVariable: q_parent_list._cx_start = cx_start q = OptimizationVariable( name="q", - mx=None, # vertcat(self.states["q_roots"].mx, self.states["q_joints"].mx), cx_start=cx_start, index=[i for i in range(self.states["q_roots"].shape + self.states["q_joints"].shape)], mapping=BiMapping( @@ -441,7 +435,6 @@ def qdot(self) -> OptimizationVariable: qdot_parent_list._cx_start = cx_start qdot = OptimizationVariable( name="qdot", - mx=None, # vertcat(self.states["qdot_roots"].mx, self.states["qdot_joints"].mx), cx_start=cx_start, index=[i for i in range(self.states["qdot_roots"].shape + self.states["qdot_joints"].shape)], mapping=BiMapping( diff --git a/bioptim/optimization/optimization_variable.py b/bioptim/optimization/optimization_variable.py index 888be16c7..352728dd2 100644 --- a/bioptim/optimization/optimization_variable.py +++ b/bioptim/optimization/optimization_variable.py @@ -37,7 +37,6 @@ class OptimizationVariable: def __init__( self, name: str, - mx: MX, # remove Charbie cx_start: list | None, index: range | list, mapping: BiMapping = None, @@ -56,7 +55,6 @@ def __init__( The list the OptimizationVariable is in """ self.name: str = name - # self.mx: MX = mx self.original_cx: list = cx_start self.index: range | list = index self.mapping: BiMapping = mapping @@ -166,7 +164,7 @@ class OptimizationVariableList: ------- __getitem__(self, item: int | str) Get a specific variable in the list, whether by name or by index - append(self, name: str, cx: list, mx: MX, bimapping: BiMapping) + append(self, name: str, cx: list, bimapping: BiMapping) Add a new variable to the list cx(self) The cx of all elements together (starting point) @@ -210,7 +208,7 @@ def __getitem__(self, item: int | str | list | range): index = [] for elt in self.elements: index.extend(list(elt.index)) - return OptimizationVariable("all", None, self.cx_start, index, None, self) + return OptimizationVariable("all", self.cx_start, index, None, self) for elt in self.elements: if item == elt.name: @@ -224,7 +222,7 @@ def __getitem__(self, item: int | str | list | range): for elt in self.elements: if elt.name in item: index.extend(list(elt.index)) - return OptimizationVariable("some", None, None, index) + return OptimizationVariable("some", None, index) else: raise ValueError("OptimizationVariableList can be sliced with int, list, range or str only") @@ -257,7 +255,7 @@ def current_cx_to_get(self, index: int): else: self._current_cx_to_get = index if index != -1 else 2 - def append_fake(self, name: str, index: MX | SX | list, mx: MX, bimapping: BiMapping): + def append_fake(self, name: str, index: MX | SX | list, bimapping: BiMapping): """ Add a new variable to the fake list which add something without changing the size of the normal elements @@ -271,9 +269,9 @@ def append_fake(self, name: str, index: MX | SX | list, mx: MX, bimapping: BiMap The Mapping of the MX against CX """ - self.fake_elements.append(OptimizationVariable(name, None, None, index, bimapping, self)) + self.fake_elements.append(OptimizationVariable(name, None, index, bimapping, self)) - def append(self, name: str, cx: list, mx: MX, bimapping: BiMapping): + def append(self, name: str, cx: list, bimapping: BiMapping): """ Add a new variable to the list @@ -300,7 +298,7 @@ def append(self, name: str, cx: list, mx: MX, bimapping: BiMapping): else: self._cx_intermediates[i] = vertcat(self._cx_intermediates[i], c) - self.elements.append(OptimizationVariable(name, None, cx, index, bimapping, parent_list=self)) + self.elements.append(OptimizationVariable(name, cx, index, bimapping, parent_list=self)) def append_from_scaled( self, @@ -336,7 +334,7 @@ def append_from_scaled( self._cx_intermediates[i] = vertcat(self._cx_intermediates[i], c) var = scaled_optimization_variable[name] - self.elements.append(OptimizationVariable(name, None, cx, var.index, var.mapping, self)) + self.elements.append(OptimizationVariable(name, cx, var.index, var.mapping, self)) @property def cx(self): @@ -559,7 +557,6 @@ def append( name: str, cx: list, cx_scaled: list, - mx: MX, # remove Charbie mapping: BiMapping, node_index: int, ): @@ -579,7 +576,7 @@ def append( node_index The index of the node for the scaled variable """ - self._scaled[node_index].append(name, cx_scaled, None, mapping) + self._scaled[node_index].append(name, cx_scaled, mapping) self._unscaled[node_index].append_from_scaled(name, cx, self._scaled[node_index]) def __contains__(self, item: str): diff --git a/bioptim/optimization/parameters.py b/bioptim/optimization/parameters.py index 6427773ab..52eb91b9e 100644 --- a/bioptim/optimization/parameters.py +++ b/bioptim/optimization/parameters.py @@ -44,7 +44,7 @@ def __init__( parent_list: ParameterList The list the OptimizationVariable is in """ - super(Parameter, self).__init__(name, mx, cx_start, index, mapping, parent_list) + super(Parameter, self).__init__(name, cx_start, index, mapping, parent_list) self.function = function self.size = size self.cx_type = cx_type diff --git a/tests/shard4/test_penalty.py b/tests/shard4/test_penalty.py index a397ff154..8779a1e02 100644 --- a/tests/shard4/test_penalty.py +++ b/tests/shard4/test_penalty.py @@ -1428,7 +1428,7 @@ def test_PenaltyFunctionAbstract_get_node(node, ns, phase_dynamics): nlp.A = np.linspace(0, 0, ns + 1) nlp.A_scaled = nlp.A tp = OptimizationVariableList(MX, phase_dynamics=phase_dynamics) - tp.append(name="param", cx=[MX(), MX(), MX()], mx=MX(), bimapping=BiMapping([], [])) + tp.append(name="param", cx=[MX(), MX(), MX()], bimapping=BiMapping([], [])) nlp.parameters = tp["param"] pn = [] diff --git a/tests/shard6/test_unit_solver_interface.py b/tests/shard6/test_unit_solver_interface.py index 6b7899f62..cc65a2a21 100644 --- a/tests/shard6/test_unit_solver_interface.py +++ b/tests/shard6/test_unit_solver_interface.py @@ -18,7 +18,7 @@ def nlp_sx(): @pytest.fixture def nlp_mx(): # Create a dummy NonLinearProgram object with necessary attributes - nlp = NonLinearProgram(None) + nlp = NonLinearProgram(PhaseDynamics.SHARED_DURING_THE_PHASE, use_sx=False) nlp.X = [MX(np.array([[1], [2], [3]]))] nlp.X_scaled = [MX(np.array([[4], [5], [6]]))] # Add more attributes as needed @@ -34,6 +34,6 @@ def nlp_control_sx(): @pytest.fixture def nlp_control_mx(): - nlp = NonLinearProgram(PhaseDynamics.SHARED_DURING_THE_PHASE) + nlp = NonLinearProgram(PhaseDynamics.SHARED_DURING_THE_PHASE, use_sx=False) nlp.U_scaled = [MX(np.array([[1], [2], [3]]))] return nlp diff --git a/tests/utils.py b/tests/utils.py index ea8603471..fb45d2b62 100644 --- a/tests/utils.py +++ b/tests/utils.py @@ -197,15 +197,10 @@ def initialize_numerical_timeseries(nlp, dynamics): f"{key}_phase{nlp.phase_idx}_{i_component}_cx", variable_shape[0], ) - mx = MX.sym( - f"{key}_phase{nlp.phase_idx}_{i_component}_mx", - variable_shape[0], - ) numerical_timeseries.append( name=f"{key}_{i_component}", cx=[cx, cx, cx], - mx=mx, bimapping=BiMapping( Mapping(list(range(variable_shape[0]))), Mapping(list(range(variable_shape[0]))) ), From c78098e5bbe2cf71d9a2f66275e974c0356457dd Mon Sep 17 00:00:00 2001 From: eve-mac Date: Mon, 23 Sep 2024 13:08:57 -0400 Subject: [PATCH 028/108] blacked --- bioptim/limits/constraints.py | 4 +++- bioptim/limits/multinode_penalty.py | 8 ++++++-- bioptim/limits/penalty_controller.py | 4 +--- bioptim/models/biorbd/multi_biorbd_model.py | 7 +++++-- 4 files changed, 15 insertions(+), 8 deletions(-) diff --git a/bioptim/limits/constraints.py b/bioptim/limits/constraints.py index d5fe4ec5c..3dcfddf35 100644 --- a/bioptim/limits/constraints.py +++ b/bioptim/limits/constraints.py @@ -689,7 +689,9 @@ def stochastic_df_dx_implicit( qdot_joints = controller.cx.sym("qdot_joints", nu, 1) tau_joints = controller.cx.sym("tau_joints", nu, 1) algebraic_states_sym = controller.cx.sym("algebraic_states_sym", controller.algebraic_states.shape, 1) - numerical_timeseries_sym = controller.cx.sym("numerical_timeseries_sym", controller.numerical_timeseries.shape, 1) + numerical_timeseries_sym = controller.cx.sym( + "numerical_timeseries_sym", controller.numerical_timeseries.shape, 1 + ) dx = controller.extra_dynamics(0)( controller.t_span.cx, diff --git a/bioptim/limits/multinode_penalty.py b/bioptim/limits/multinode_penalty.py index 813a46468..6f0222558 100644 --- a/bioptim/limits/multinode_penalty.py +++ b/bioptim/limits/multinode_penalty.py @@ -562,8 +562,12 @@ def stochastic_df_dw_implicit( qdot_root = controllers[0].cx.sym("qdot_root", nb_root, 1) qdot_joints = controllers[0].cx.sym("qdot_joints", nu, 1) tau_joints = controllers[0].cx.sym("tau_joints", nu, 1) - algebraic_states_sym = controllers[0].cx.sym("algebraic_states_sym", controllers[0].algebraic_states.shape, 1) - numerical_timeseries_sym = controllers[0].cx.sym("numerical_timeseries_sym", controllers[0].numerical_timeseries.shape, 1) + algebraic_states_sym = controllers[0].cx.sym( + "algebraic_states_sym", controllers[0].algebraic_states.shape, 1 + ) + numerical_timeseries_sym = controllers[0].cx.sym( + "numerical_timeseries_sym", controllers[0].numerical_timeseries.shape, 1 + ) dx = controllers[0].extra_dynamics(0)( controllers[0].time.cx, diff --git a/bioptim/limits/penalty_controller.py b/bioptim/limits/penalty_controller.py index 39c52b331..c8b2b57dc 100644 --- a/bioptim/limits/penalty_controller.py +++ b/bioptim/limits/penalty_controller.py @@ -133,9 +133,7 @@ def t_span(self) -> OptimizationVariable: tp = OptimizationVariableList(self._nlp.cx, self._nlp.phase_dynamics == PhaseDynamics.SHARED_DURING_THE_PHASE) n_val = cx.shape[0] - tp.append( - "t_span", cx=[cx, cx, cx], bimapping=BiMapping(to_second=range(n_val), to_first=range(n_val)) - ) + tp.append("t_span", cx=[cx, cx, cx], bimapping=BiMapping(to_second=range(n_val), to_first=range(n_val))) return tp["t_span"] @property diff --git a/bioptim/models/biorbd/multi_biorbd_model.py b/bioptim/models/biorbd/multi_biorbd_model.py index 66410ca60..644cee3bc 100644 --- a/bioptim/models/biorbd/multi_biorbd_model.py +++ b/bioptim/models/biorbd/multi_biorbd_model.py @@ -53,7 +53,8 @@ def __init__( self.models.append(model) if model.parameters is not None: raise NotImplementedError( - "MultiBiorbdModel does not handle parameters yet. Please use BiorbdModel instead.") + "MultiBiorbdModel does not handle parameters yet. Please use BiorbdModel instead." + ) else: raise ValueError("The models should be of type 'str', 'biorbd.Model' or 'bioptim.BiorbdModel'") @@ -291,7 +292,9 @@ def segments(self) -> tuple[biorbd.Segment, ...]: def homogeneous_matrices_in_global(self, segment_idx, inverse=False) -> Function: local_segment_id, model_id = self.local_variable_id("segment", segment_idx) q_model = self.models[model_id].q - biorbd_return = self.models[model_id].homogeneous_matrices_in_global(local_segment_id, inverse)(q_model, self.parameters) + biorbd_return = self.models[model_id].homogeneous_matrices_in_global(local_segment_id, inverse)( + q_model, self.parameters + ) casadi_fun = Function( "homogeneous_matrices_in_global", [self.models[model_id].q, self.parameters], From f275729882ea005954d20ad9d2168319cae82e3f Mon Sep 17 00:00:00 2001 From: eve-mac Date: Mon, 23 Sep 2024 14:03:16 -0400 Subject: [PATCH 029/108] ok --- bioptim/dynamics/configure_new_variable.py | 2 +- bioptim/dynamics/configure_problem.py | 7 +++++-- .../muscle_driven_ocp/muscle_excitations_tracker.py | 7 ------- bioptim/optimization/optimal_control_program.py | 5 ----- 4 files changed, 6 insertions(+), 15 deletions(-) diff --git a/bioptim/dynamics/configure_new_variable.py b/bioptim/dynamics/configure_new_variable.py index 9a4f2e364..8cb0997b3 100644 --- a/bioptim/dynamics/configure_new_variable.py +++ b/bioptim/dynamics/configure_new_variable.py @@ -640,4 +640,4 @@ def append_faked_optim_var(name: str, optim_var, keys: list): to_second.extend(list(np.array(optim_var[key].mapping.to_second.map_idx) + len(to_second))) to_first.extend(list(np.array(optim_var[key].mapping.to_first.map_idx) + len(to_first))) - optim_var.append_fake(name, index, None, BiMapping(to_second, to_first)) + optim_var.append_fake(name, index, BiMapping(to_second, to_first)) diff --git a/bioptim/dynamics/configure_problem.py b/bioptim/dynamics/configure_problem.py index f8e943d5e..424ec6a24 100644 --- a/bioptim/dynamics/configure_problem.py +++ b/bioptim/dynamics/configure_problem.py @@ -1496,7 +1496,11 @@ def configure_integrated_value( initial_vector = StochasticBioModel.reshape_to_vector(initial_matrix) cx_scaled_next_formatted = [initial_vector for _ in range(n_cx)] nlp.integrated_values.append( - name, cx_scaled_next_formatted, cx_scaled_next_formatted, initial_matrix, dummy_mapping, 0 + name=name, + cx=cx_scaled_next_formatted, + cx_scaled=[initial_matrix for i in range(n_cx)], # Only the first value is used + mapping=dummy_mapping, + node_index=0 ) for node_index in range(1, nlp.ns + 1): # cannot use phase_dynamics == PhaseDynamics.SHARED_DURING_THE_PHASE cx_scaled_next = nlp.integrated_value_functions[name](nlp, node_index) @@ -1504,7 +1508,6 @@ def configure_integrated_value( nlp.integrated_values.append( name, cx_scaled_next_formatted, - cx_scaled_next_formatted, cx_scaled_next, dummy_mapping, node_index, diff --git a/bioptim/examples/muscle_driven_ocp/muscle_excitations_tracker.py b/bioptim/examples/muscle_driven_ocp/muscle_excitations_tracker.py index 75807414e..9644a0727 100644 --- a/bioptim/examples/muscle_driven_ocp/muscle_excitations_tracker.py +++ b/bioptim/examples/muscle_driven_ocp/muscle_excitations_tracker.py @@ -114,7 +114,6 @@ def generate_data( name="q", cx=[symbolic_q, symbolic_q, symbolic_q], cx_scaled=[symbolic_q, symbolic_q, symbolic_q], - mx=symbolic_q, mapping=nlp.variable_mappings["q"], node_index=node_index, ) @@ -122,7 +121,6 @@ def generate_data( name="qdot", cx=[symbolic_qdot, symbolic_qdot, symbolic_qdot], cx_scaled=[symbolic_qdot, symbolic_qdot, symbolic_qdot], - mx=symbolic_qdot, mapping=nlp.variable_mappings["qdot"], node_index=node_index, ) @@ -130,7 +128,6 @@ def generate_data( name="muscles", cx=[symbolic_mus_states, symbolic_mus_states, symbolic_mus_states], cx_scaled=[symbolic_mus_states, symbolic_mus_states, symbolic_mus_states], - mx=symbolic_mus_states, mapping=nlp.variable_mappings["muscles"], node_index=node_index, ) @@ -139,7 +136,6 @@ def generate_data( name="tau", cx=[symbolic_tau, symbolic_tau, symbolic_tau], cx_scaled=[symbolic_tau, symbolic_tau, symbolic_tau], - mx=symbolic_tau, mapping=nlp.variable_mappings["tau"], node_index=node_index, ) @@ -147,7 +143,6 @@ def generate_data( name="muscles", cx=[symbolic_mus_controls, symbolic_mus_controls, symbolic_mus_controls], cx_scaled=[symbolic_mus_controls, symbolic_mus_controls, symbolic_mus_controls], - mx=symbolic_mus_controls, mapping=nlp.variable_mappings["muscles"], node_index=node_index, ) @@ -155,7 +150,6 @@ def generate_data( name="qdot", cx=[symbolic_qdot, symbolic_qdot, symbolic_qdot], cx_scaled=[symbolic_qdot, symbolic_qdot, symbolic_qdot], - mx=symbolic_qdot, mapping=nlp.variable_mappings["qdot"], node_index=node_index, ) @@ -163,7 +157,6 @@ def generate_data( name="qddot", cx=[symbolic_qddot, symbolic_qddot, symbolic_qddot], cx_scaled=[symbolic_qddot, symbolic_qddot, symbolic_qddot], - mx=symbolic_qddot, mapping=nlp.variable_mappings["qddot"], node_index=node_index, ) diff --git a/bioptim/optimization/optimal_control_program.py b/bioptim/optimization/optimal_control_program.py index e3bd952a6..470cb023b 100644 --- a/bioptim/optimization/optimal_control_program.py +++ b/bioptim/optimization/optimal_control_program.py @@ -1704,15 +1704,10 @@ def _define_numerical_timeseries(self, dynamics): f"{key}_phase{i_phase}_{i_component}_cx", variable_shape[0], ) - mx = MX.sym( - f"{key}_phase{i_phase}_{i_component}_mx", - variable_shape[0], - ) numerical_timeseries[-1].append( name=f"{key}_{i_component}", cx=[cx, cx, cx], - mx=mx, bimapping=BiMapping( Mapping(list(range(variable_shape[0]))), Mapping(list(range(variable_shape[0]))) ), From 133dd7410a7922c8ae6f0658cf9eaca6390a6da7 Mon Sep 17 00:00:00 2001 From: eve-mac Date: Mon, 23 Sep 2024 18:31:55 -0400 Subject: [PATCH 030/108] made requested changes except f_ext --- bioptim/dynamics/configure_new_variable.py | 3 - bioptim/dynamics/configure_problem.py | 2 +- bioptim/dynamics/dynamics_functions.py | 58 +- .../custom_package/custom_dynamics.py | 2 +- .../getting_started/custom_constraint.py | 2 +- .../getting_started/custom_dynamics.py | 2 +- .../getting_started/custom_objectives.py | 2 +- .../getting_started/custom_parameters.py | 4 +- .../example_continuity_as_objective.py | 2 +- .../muscle_activations_tracker.py | 19 +- .../muscle_excitations_tracker.py | 18 +- .../arm_reaching_muscle_driven.py | 8 +- .../track_markers_2D_pendulum.py | 3 +- bioptim/examples/track/optimal_estimation.py | 3 +- bioptim/limits/constraints.py | 50 +- bioptim/limits/multinode_penalty.py | 8 +- bioptim/limits/penalty.py | 50 +- bioptim/limits/penalty_controller.py | 21 +- bioptim/limits/phase_transition.py | 2 +- bioptim/models/biorbd/biorbd_model.py | 4 +- bioptim/models/biorbd/multi_biorbd_model.py | 38 +- bioptim/optimization/non_linear_program.py | 86 +-- .../optimization/optimal_control_program.py | 9 - bioptim/optimization/parameters.py | 1 + tests/shard1/test_biorbd_model_size.py | 631 ------------------ tests/shard1/test_biorbd_multi_model.py | 95 ++- tests/shard1/test_dynamics.py | 44 +- tests/shard3/test_graph.py | 2 +- tests/shard3/test_ligaments.py | 16 +- tests/shard3/test_passive_torque.py | 16 +- tests/shard4/test_penalty.py | 6 +- 31 files changed, 234 insertions(+), 973 deletions(-) delete mode 100644 tests/shard1/test_biorbd_model_size.py diff --git a/bioptim/dynamics/configure_new_variable.py b/bioptim/dynamics/configure_new_variable.py index 8cb0997b3..4a7368f57 100644 --- a/bioptim/dynamics/configure_new_variable.py +++ b/bioptim/dynamics/configure_new_variable.py @@ -134,7 +134,6 @@ def __init__( self._declare_initial_guess() self._declare_variable_scaling() - # self._use_copy() # plot self.legend = None @@ -631,12 +630,10 @@ def append_faked_optim_var(name: str, optim_var, keys: list): """ index = [] - # mx = MX() to_second = [] to_first = [] for key in keys: index.extend(list(optim_var[key].index)) - # mx = vertcat(mx, optim_var[key].mx) to_second.extend(list(np.array(optim_var[key].mapping.to_second.map_idx) + len(to_second))) to_first.extend(list(np.array(optim_var[key].mapping.to_first.map_idx) + len(to_first))) diff --git a/bioptim/dynamics/configure_problem.py b/bioptim/dynamics/configure_problem.py index 424ec6a24..5ee7d11fe 100644 --- a/bioptim/dynamics/configure_problem.py +++ b/bioptim/dynamics/configure_problem.py @@ -1353,7 +1353,7 @@ def configure_soft_contact_function(ocp, nlp): q = nlp.states["q"].mapping.to_second.map(nlp.states["q"].cx) qdot = nlp.states["qdot"].mapping.to_second.map(nlp.states["qdot"].cx) - global_soft_contact_force_func = nlp.model.soft_contact_forces()(q, qdot, nlp.parameters_except_time.cx) + 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 c9e3f1838..317ba78b9 100644 --- a/bioptim/dynamics/dynamics_functions.py +++ b/bioptim/dynamics/dynamics_functions.py @@ -146,11 +146,11 @@ def torque_driven( tau = DynamicsFunctions.__get_fatigable_tau(nlp, states, controls, fatigue) tau = ( - tau + nlp.model.passive_joint_torque()(q, qdot, nlp.parameters_except_time.cx) + 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_except_time.cx) if with_ligament 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 ( @@ -262,7 +262,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, nlp.parameters_except_time.cx) + tau_joints + nlp.model.passive_joint_torque()(q_full, qdot_full, nlp.parameters.cx) if with_passive_torque else tau_joints ) @@ -532,13 +532,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, nlp.parameters_except_time.cx) + tau = nlp.model.torque()(tau_activation, q, qdot, nlp.parameters.cx) if with_passive_torque: - tau += nlp.model.passive_joint_torque()(q, qdot, nlp.parameters_except_time.cx) + 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, nlp.parameters_except_time.cx) + 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) @@ -603,11 +603,11 @@ def torque_derivative_driven( tau = DynamicsFunctions.get(nlp.states["tau"], states) tau = ( - tau + nlp.model.passive_joint_torque()(q, qdot, nlp.parameters_except_time.cx) + 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_except_time.cx) if with_ligament 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) @@ -682,14 +682,14 @@ def forces_from_torque_driven( 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, nlp.parameters_except_time.cx) + 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_except_time.cx) if with_ligament 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, nlp.parameters_except_time.cx) + return nlp.model.contact_forces()(q, qdot, tau, external_forces, nlp.parameters.cx) @staticmethod def forces_from_torque_activation_driven( @@ -738,16 +738,16 @@ 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, nlp.parameters_except_time.cx) + tau = nlp.model.torque()(tau_activations, q, qdot, nlp.parameters.cx) tau = ( - tau + nlp.model.passive_joint_torque()(q, qdot, nlp.parameters_except_time.cx) + 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_except_time.cx) if with_ligament 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, nlp.parameters_except_time.cx) + return nlp.model.contact_forces()(q, qdot, tau, external_forces, nlp.parameters.cx) @staticmethod def muscles_driven( @@ -848,11 +848,11 @@ def muscles_driven( tau = muscles_tau + residual_tau if residual_tau is not None else muscles_tau tau = ( - tau + nlp.model.passive_joint_torque()(q, qdot, nlp.parameters_except_time.cx) + 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_except_time.cx) if with_ligament 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) @@ -953,14 +953,14 @@ def forces_from_muscle_driven( tau = muscles_tau + residual_tau if residual_tau is not None else muscles_tau tau = ( - tau + nlp.model.passive_joint_torque()(q, qdot, nlp.parameters_except_time.cx) + 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_except_time.cx) if with_ligament 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, nlp.parameters_except_time.cx) + return nlp.model.contact_forces()(q, qdot, tau, external_forces, nlp.parameters.cx) @staticmethod def joints_acceleration_driven( @@ -1008,7 +1008,7 @@ def joints_acceleration_driven( qddot_joints = nlp.get_var_from_states_or_controls("qddot", states, controls) qddot_root = nlp.model.forward_dynamics_free_floating_base()( - q, qdot, qddot_joints, nlp.parameters_except_time.cx + q, qdot, qddot_joints, nlp.parameters.cx ) qddot_reordered = nlp.model.reorder_qddot_root_joints(qddot_root, qddot_joints) @@ -1098,7 +1098,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, nlp.parameters_except_time.cx)) + return mapping.to_first.map(nlp.model.reshape_qdot()(q, qdot, nlp.parameters.cx)) @staticmethod def forward_dynamics( @@ -1140,12 +1140,12 @@ def forward_dynamics( if external_forces is None: qddot = nlp.model.forward_dynamics(with_contact=with_contact)( - q, qdot, tau, [], nlp.parameters_except_time.cx + 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, nlp.parameters_except_time.cx + q, qdot, tau, external_forces, nlp.parameters.cx ) return qdot_var_mapping.map(qddot) @@ -1183,7 +1183,7 @@ def inverse_dynamics( if external_forces is None: tau = nlp.model.inverse_dynamics(with_contact=with_contact)( - q, qdot, qddot, [], nlp.parameters_except_time.cx + q, qdot, qddot, [], nlp.parameters.cx ) else: if "tau" in nlp.states: @@ -1195,7 +1195,7 @@ def inverse_dynamics( 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], nlp.parameters_except_time.cx + 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. @@ -1216,7 +1216,7 @@ def compute_muscle_dot(nlp, muscle_excitations: MX | SX): The derivative of muscle activations """ - return nlp.model.muscle_activation_dot()(muscle_excitations, nlp.parameters_except_time.cx) + return nlp.model.muscle_activation_dot()(muscle_excitations, nlp.parameters.cx) @staticmethod def compute_tau_from_muscle( @@ -1253,7 +1253,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, nlp.parameters_except_time.cx) + return nlp.model.muscle_joint_torque()(activations, q, qdot, nlp.parameters.cx) @staticmethod def holonomic_torque_driven( @@ -1297,7 +1297,7 @@ def holonomic_torque_driven( 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, nlp.parameters_except_time.cx + 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/custom_model/custom_package/custom_dynamics.py b/bioptim/examples/custom_model/custom_package/custom_dynamics.py index 207b85f34..341e90faf 100644 --- a/bioptim/examples/custom_model/custom_package/custom_dynamics.py +++ b/bioptim/examples/custom_model/custom_package/custom_dynamics.py @@ -40,7 +40,7 @@ def custom_dynamics( """ return DynamicsEvaluation( - dxdt=vertcat(states[1], nlp.model.forward_dynamics(states[0], states[1], controls[0], None)), defects=None + dxdt=vertcat(states[1], nlp.model.forward_dynamics(with_contact=False)(states[0], states[1], controls[0], [])), defects=None ) diff --git a/bioptim/examples/getting_started/custom_constraint.py b/bioptim/examples/getting_started/custom_constraint.py index 3d07f2171..4f20b822f 100644 --- a/bioptim/examples/getting_started/custom_constraint.py +++ b/bioptim/examples/getting_started/custom_constraint.py @@ -53,7 +53,7 @@ def custom_func_track_markers(controller: PenaltyController, first_marker: str, marker_1_idx = controller.model.marker_index(second_marker) # compute the position of the markers using the markers function from the BioModel (here a BiorbdModel) - markers = controller.model.markers()(controller.states["q"].cx, controller.parameters_except_time.cx) + markers = controller.model.markers()(controller.states["q"].cx, controller.parameters.cx) markers_diff = markers[marker_1_idx] - markers[marker_0_idx] return markers_diff diff --git a/bioptim/examples/getting_started/custom_dynamics.py b/bioptim/examples/getting_started/custom_dynamics.py index 8e68ac9a0..2e53a5a30 100644 --- a/bioptim/examples/getting_started/custom_dynamics.py +++ b/bioptim/examples/getting_started/custom_dynamics.py @@ -74,7 +74,7 @@ def custom_dynamics( # You can directly call biorbd function (as for ddq) or call bioptim accessor (as for dq) dq = DynamicsFunctions.compute_qdot(nlp, q, qdot) * my_additional_factor - ddq = nlp.model.forward_dynamics()(q, qdot, tau, [], nlp.parameters_except_time.cx) + ddq = nlp.model.forward_dynamics()(q, qdot, tau, [], nlp.parameters.cx) # the user has to choose if want to return the explicit dynamics dx/dt = f(x,u,p) # as the first argument of DynamicsEvaluation or diff --git a/bioptim/examples/getting_started/custom_objectives.py b/bioptim/examples/getting_started/custom_objectives.py index acf9d08fd..d26403b22 100644 --- a/bioptim/examples/getting_started/custom_objectives.py +++ b/bioptim/examples/getting_started/custom_objectives.py @@ -52,7 +52,7 @@ def custom_func_track_markers(controller: PenaltyController, first_marker: str, marker_1_idx = controller.model.marker_index(second_marker) # Convert the function to the required format and then subtract - markers = controller.model.markers()(controller.states["q"].cx, controller.parameters_except_time.cx) + markers = controller.model.markers()(controller.states["q"].cx, controller.parameters.cx) markers_diff = markers[marker_1_idx] - markers[marker_0_idx] return markers_diff diff --git a/bioptim/examples/getting_started/custom_parameters.py b/bioptim/examples/getting_started/custom_parameters.py index dcca8cb3e..aab3038c0 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.model.setGravity(value) + bio_model.set_gravity(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.model.segments()[0].characteristics().setMass(value) + bio_model.segments[0].characteristics().setMass(value) def my_target_function(controller: PenaltyController, key: str) -> MX: diff --git a/bioptim/examples/getting_started/example_continuity_as_objective.py b/bioptim/examples/getting_started/example_continuity_as_objective.py index 3e74ee469..2768a0bf8 100644 --- a/bioptim/examples/getting_started/example_continuity_as_objective.py +++ b/bioptim/examples/getting_started/example_continuity_as_objective.py @@ -44,7 +44,7 @@ def out_of_sphere(controller: PenaltyController, y, z): q = controller.states["q"].cx - marker_q = controller.model.marker(1)(q, controller.parameters_except_time.cx) + marker_q = controller.model.marker(1)(q, controller.parameters.cx) distance = sqrt((y - marker_q[1]) ** 2 + (z - marker_q[2]) ** 2) diff --git a/bioptim/examples/muscle_driven_ocp/muscle_activations_tracker.py b/bioptim/examples/muscle_driven_ocp/muscle_activations_tracker.py index 3f79c7dd8..ee8661334 100644 --- a/bioptim/examples/muscle_driven_ocp/muscle_activations_tracker.py +++ b/bioptim/examples/muscle_driven_ocp/muscle_activations_tracker.py @@ -12,7 +12,7 @@ import biorbd_casadi as biorbd import numpy as np -from casadi import MX, vertcat, horzcat +from casadi import MX, vertcat, Function from matplotlib import pyplot as plt from scipy.integrate import solve_ivp @@ -159,9 +159,12 @@ def generate_data( symbolic_states = vertcat(*(symbolic_q, symbolic_qdot)) symbolic_controls = vertcat(*(symbolic_tau, symbolic_mus)) if use_residual_torque else vertcat(symbolic_mus) - dynamics_func = biorbd.to_casadi_func( + dynamics_func = Function( "ForwardDyn", - dyn_func( + [symbolic_states, + symbolic_controls, + symbolic_parameters], + [dyn_func( time=symbolic_time, states=symbolic_states, controls=symbolic_controls, @@ -171,12 +174,7 @@ def generate_data( nlp=nlp, with_contact=False, rigidbody_dynamics=RigidBodyDynamics.ODE, - ).dxdt, - symbolic_states, - symbolic_controls, - symbolic_parameters, - nlp, - False, + ).dxdt] ) def dyn_interface(t, x, u): @@ -361,10 +359,9 @@ def main(): markers = np.ndarray((3, n_mark, q.shape[1])) symbolic_states = MX.sym("x", n_q, 1) - markers_func = biorbd.to_casadi_func("ForwardKin", bio_model.markers, symbolic_states) for i in range(n_frames): - markers[:, :, i] = markers_func(q[:, i]) + markers[:, :, i] = bio_model.markers()(q[:, i]) plt.figure("Markers") n_steps_ode = ocp.nlp[0].ode_solver.steps + 1 if ocp.nlp[0].ode_solver.is_direct_collocation else 1 diff --git a/bioptim/examples/muscle_driven_ocp/muscle_excitations_tracker.py b/bioptim/examples/muscle_driven_ocp/muscle_excitations_tracker.py index 9644a0727..1a7a0d903 100644 --- a/bioptim/examples/muscle_driven_ocp/muscle_excitations_tracker.py +++ b/bioptim/examples/muscle_driven_ocp/muscle_excitations_tracker.py @@ -12,7 +12,7 @@ import biorbd_casadi as biorbd import numpy as np -from casadi import MX, SX, vertcat, horzcat +from casadi import MX, SX, vertcat, horzcat, Function from matplotlib import pyplot as plt from scipy.integrate import solve_ivp @@ -161,9 +161,13 @@ def generate_data( node_index=node_index, ) - dynamics_func = biorbd.to_casadi_func( + dynamics_func = Function( "ForwardDyn", - DynamicsFunctions.muscles_driven( + [symbolic_time, + symbolic_states, + symbolic_controls, + symbolic_parameters], + [DynamicsFunctions.muscles_driven( time=symbolic_time, states=symbolic_states, controls=symbolic_controls, @@ -173,13 +177,7 @@ def generate_data( nlp=nlp, with_contact=False, rigidbody_dynamics=RigidBodyDynamics.ODE, - ).dxdt, - symbolic_time, - symbolic_states, - symbolic_controls, - symbolic_parameters, - nlp, - False, + ).dxdt], ) def dyn_interface(t, x, u): diff --git a/bioptim/examples/stochastic_optimal_control/arm_reaching_muscle_driven.py b/bioptim/examples/stochastic_optimal_control/arm_reaching_muscle_driven.py index 1b489f209..a74c3a2ed 100644 --- a/bioptim/examples/stochastic_optimal_control/arm_reaching_muscle_driven.py +++ b/bioptim/examples/stochastic_optimal_control/arm_reaching_muscle_driven.py @@ -209,12 +209,6 @@ def get_cov_mat(nlp, node_index): with_noise=True, ) - # dx.dxdt = cas.Function( - # "tp", - # [nlp.states.mx, nlp.controls.mx, nlp.parameters.mx, nlp.algebraic_states.mx, nlp.numerical_timeseries.mx], - # [dx.dxdt], - # )(nlp.states.cx, nlp.controls.cx, nlp.parameters.cx, nlp.algebraic_states.cx, nlp.numerical_timeseries.cx) - ddx_dwm = cas.jacobian(dx.dxdt, cas.vertcat(sensory_noise, motor_noise)) dg_dw = -ddx_dwm * dt ddx_dx = cas.jacobian(dx.dxdt, nlp.states.cx) @@ -222,7 +216,7 @@ def get_cov_mat(nlp, node_index): p_next = m_matrix @ (dg_dx @ cov_matrix @ dg_dx.T + dg_dw @ sigma_w @ dg_dw.T) @ m_matrix.T - parameters = nlp.parameters_except_time.cx + parameters = nlp.parameters.cx parameters[nlp.parameters["sensory_noise"].index] = nlp.model.sensory_noise_magnitude parameters[nlp.parameters["motor_noise"].index] = nlp.model.motor_noise_magnitude diff --git a/bioptim/examples/torque_driven_ocp/track_markers_2D_pendulum.py b/bioptim/examples/torque_driven_ocp/track_markers_2D_pendulum.py index 9837d4d45..bb4853d87 100644 --- a/bioptim/examples/torque_driven_ocp/track_markers_2D_pendulum.py +++ b/bioptim/examples/torque_driven_ocp/track_markers_2D_pendulum.py @@ -171,10 +171,9 @@ def main(): x = np.concatenate((q, qdot)) symbolic_states = MX.sym("q", n_q, 1) - markers_fun = biorbd.to_casadi_func("ForwardKin", bio_model.markers, symbolic_states) markers_ref = np.zeros((3, n_marker, n_shooting + 1)) for i in range(n_shooting + 1): - markers_ref[:, :, i] = markers_fun(x[:n_q, i]) + markers_ref[:, :, i] = bio_model.markers()(x[:n_q, i]) tau_ref = tau[:, :-1] ocp = prepare_ocp( diff --git a/bioptim/examples/track/optimal_estimation.py b/bioptim/examples/track/optimal_estimation.py index fc37cfa48..88e6973f7 100644 --- a/bioptim/examples/track/optimal_estimation.py +++ b/bioptim/examples/track/optimal_estimation.py @@ -224,10 +224,9 @@ def main(): n_marker = model.nbMarkers() symbolic_states = MX.sym("q", n_q, 1) - markers_fun = biorbd.to_casadi_func("ForwardKin", model.markers, symbolic_states) markers_ref = np.zeros((3, n_marker, n_shooting + 1)) for i_node in range(n_shooting + 1): - markers_ref[:, :, i_node] = markers_fun(q[:, i_node]) + markers_ref[:, :, i_node] = model.markers()(q[:, i_node]) ocp = prepare_optimal_estimation( biorbd_model_path=biorbd_model_path, diff --git a/bioptim/limits/constraints.py b/bioptim/limits/constraints.py index 3dcfddf35..20261bfdc 100644 --- a/bioptim/limits/constraints.py +++ b/bioptim/limits/constraints.py @@ -301,7 +301,7 @@ def torque_max_from_q_and_qdot( if min_torque and min_torque < 0: raise ValueError("min_torque cannot be negative in tau_max_from_actuators") - bound = controller.model.tau_max()(controller.q, controller.qdot, controller.parameters_except_time.cx) + bound = controller.model.tau_max()(controller.q, controller.qdot, controller.parameters.cx) min_bound = controller.controls["tau"].mapping.to_first.map(bound[1]) max_bound = controller.controls["tau"].mapping.to_first.map(bound[0]) if min_torque: @@ -407,23 +407,23 @@ def qddot_equals_forward_dynamics( """ passive_torque = controller.model.passive_joint_torque()( - controller.q, controller.qdot, controller.parameters_except_time.cx + controller.q, controller.qdot, controller.parameters.cx ) tau = controller.states["tau"].cx if "tau" in controller.states else controller.tau tau = tau + passive_torque if with_passive_torque else tau tau = ( tau + controller.model.ligament_joint_torque()( - controller.q, controller.qdot, controller.parameters_except_time.cx + controller.q, controller.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 ? + # TODO: add external_forces qddot_fd = controller.model.forward_dynamics(with_contact=with_contact)( - controller.q, controller.qdot, tau, [], controller.parameters_except_time.cx + controller.q, controller.qdot, tau, [], controller.parameters.cx ) return qddot - qddot_fd @@ -459,30 +459,34 @@ def tau_equals_inverse_dynamics( tau = controller.states["tau"].cx if "tau" in controller.states else controller.tau qddot = controller.states["qddot"].cx if "qddot" in controller.states else controller.controls["qddot"].cx passive_torque = controller.model.passive_joint_torque()( - controller.q, controller.qdot, controller.parameters_except_time.cx + controller.q, controller.qdot, controller.parameters.cx ) tau = tau + passive_torque if with_passive_torque else tau tau = ( tau + controller.model.ligament_joint_torque()( - controller.q, controller.qdot, controller.parameters_except_time.cx + controller.q, controller.qdot, controller.parameters.cx ) if with_ligament else tau ) - if "fext" in controller.controls: - f_ext = controller.controls["fext"].cx - elif "fext" in controller.states: - f_ext = controller.states["fext"].cx - elif "external_forces" in controller.get_nlp.numerical_timeseries: - f_ext = controller.numerical_timeseries["external_forces"].cx - else: - raise ValueError("External forces must be provided") + if controller.get_nlp.numerical_timeseries: + # TODO: deal with external forces + raise NotImplementedError( + "This implicit constraint tau_equals_inverse_dynamics is not implemented yet with external forces" + ) + # Todo: add fext tau_id = nlp.model.inverse_dynamics()(q, qdot, qddot, fext) - tau_id = controller.model.inverse_dynamics(with_contact=with_contact)( - controller.q, controller.qdot, qddot, f_ext, controller.parameters_except_time.cx - ) + if with_contact: + # todo: this should be done internally in BiorbdModel + f_contact = ( + controller.controls["fext"].cx if "fext" in controller.controls else controller.states["fext"].cx + ) + f_contact_vec = controller.model.reshape_fext_to_fcontact(f_contact) + else: + f_contact_vec = [] + tau_id = controller.model.inverse_dynamics(with_contact=with_contact)(controller.q, controller.qdot, qddot, f_contact_vec, controller.parameters.cx) var = [] var.extend([controller.states[key] for key in controller.states]) @@ -544,18 +548,18 @@ def tau_from_muscle_equal_inverse_dynamics( muscle_activations = controller.controls["muscles"].cx muscles_states = controller.model.state_set() passive_torque = controller.model.passive_joint_torque()( - controller.q, controller.qdot, controller.parameters_except_time.cx + controller.q, controller.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, controller.q, controller.qdot, controller.parameters_except_time.cx + muscles_states, controller.q, controller.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( - controller.q, controller.qdot, controller.parameters_except_time.cx + controller.q, controller.qdot, controller.parameters.cx ) if with_ligament else muscle_tau @@ -570,7 +574,7 @@ def tau_from_muscle_equal_inverse_dynamics( # fext need to be a mx tau_id = controller.model.inverse_dynamics()( - controller.q, controller.qdot, qddot, controller.parameters_except_time.cx + controller.q, controller.qdot, qddot, controller.parameters.cx ) return tau_id - muscle_tau @@ -597,7 +601,7 @@ def implicit_soft_contact_forces(_: Constraint, controller: PenaltyController, * force_idx.append(5 + (6 * i_sc)) soft_contact_all = controller.get_nlp.soft_contact_forces_func( - controller.states.cx, controller.controls.cx, controller.parameters_except_time.cx + controller.states.cx, controller.controls.cx, controller.parameters.cx ) soft_contact_force = soft_contact_all[force_idx] diff --git a/bioptim/limits/multinode_penalty.py b/bioptim/limits/multinode_penalty.py index 6f0222558..88edd0584 100644 --- a/bioptim/limits/multinode_penalty.py +++ b/bioptim/limits/multinode_penalty.py @@ -268,13 +268,13 @@ def com_equality(penalty, controllers: list[PenaltyController]): MultinodePenaltyFunctions.Functions._prepare_controller_cx(penalty, controllers) com_0 = controllers[0].model.center_of_mass()( - controllers[0].states["q"].cx, controllers[0].parameters_except_time.cx + controllers[0].states["q"].cx, controllers[0].parameters.cx ) out = controllers[0].cx.zeros((3, 1)) for i in range(1, len(controllers)): com_i = controllers[i].model.center_of_mass()( - controllers[i].states["q"].cx, controllers[i].parameters_except_time.cx + controllers[i].states["q"].cx, controllers[i].parameters.cx ) out += com_0 - com_i @@ -302,7 +302,7 @@ def com_velocity_equality(penalty, controllers: list[PenaltyController]): com_dot_0 = controllers[0].model.center_of_mass_velocity()( controllers[0].states["q"].cx, controllers[0].states["qdot"].cx, - controllers[0].parameters_except_time.cx, + controllers[0].parameters.cx, ) out = controllers[0].cx.zeros((3, 1)) @@ -310,7 +310,7 @@ def com_velocity_equality(penalty, controllers: list[PenaltyController]): com_dot_i = controllers[i].model.center_of_mass_velocity()( controllers[i].states["q"].cx, controllers[i].states["qdot"].cx, - controllers[i].parameters_except_time.cx, + controllers[i].parameters.cx, ) out += com_dot_0 - com_dot_i diff --git a/bioptim/limits/penalty.py b/bioptim/limits/penalty.py index d51a8e60f..df4a6ec94 100644 --- a/bioptim/limits/penalty.py +++ b/bioptim/limits/penalty.py @@ -125,7 +125,7 @@ def minimize_power( return controls * controller.qdot elif key_control == "muscles": muscles_dot = controller.model.muscle_velocity()( - controller.q, controller.qdot, controller.parameters_except_time.cx + controller.q, controller.qdot, controller.parameters.cx ) return controls * muscles_dot @@ -297,11 +297,11 @@ def minimize_markers( CX_eye(4) if reference_jcs is None else controller.model.homogeneous_matrices_in_global(reference_jcs, inverse=True)( - controller.q, controller.parameters_except_time.cx + controller.q, controller.parameters.cx ) ) - markers = controller.model.markers()(controller.q, controller.parameters_except_time.cx) + markers = controller.model.markers()(controller.q, controller.parameters.cx) markers_in_jcs = [] for i in range(markers.shape[1]): marker_in_jcs = jcs_t @ vertcat(markers[:, i], 1) @@ -346,7 +346,7 @@ def minimize_markers_velocity( # Add the penalty in the requested reference frame. None for global markers = horzcat( *controller.model.markers_velocities(reference_index=reference_jcs)( - controller.q, controller.qdot, controller.parameters_except_time.cx + controller.q, controller.qdot, controller.parameters.cx ) ) @@ -388,7 +388,7 @@ def minimize_markers_acceleration( markers = horzcat( *controller.model.markers_accelerations(reference_index=reference_jcs)( - controller.q, controller.qdot, qddot, controller.parameters_except_time.cx + controller.q, controller.qdot, qddot, controller.parameters.cx ) ) @@ -433,8 +433,8 @@ def superimpose_markers( penalty.quadratic = True if penalty.quadratic is None else penalty.quadratic diff_markers = controller.model.marker(second_marker_idx)( - controller.q, controller.parameters_except_time.cx - ) - controller.model.marker(first_marker_idx)(controller.q, controller.parameters_except_time.cx) + controller.q, controller.parameters.cx + ) - controller.model.marker(first_marker_idx)(controller.q, controller.parameters.cx) return diff_markers @@ -477,7 +477,7 @@ def superimpose_markers_velocity( penalty.quadratic = True if penalty.quadratic is None else penalty.quadratic marker_velocity = controller.model.markers_velocities()( - controller.q, controller.qdot, controller.parameters_except_time.cx + controller.q, controller.qdot, controller.parameters.cx ) marker_1 = marker_velocity[first_marker_idx][:] marker_2 = marker_velocity[second_marker_idx][:] @@ -607,9 +607,9 @@ def minimize_predicted_com_height(_: PenaltyOption, controller: PenaltyControlle """ g = controller.model.gravity()["o0"][2] - com = controller.model.center_of_mass()(controller.q, controller.parameters_except_time.cx) + com = controller.model.center_of_mass()(controller.q, controller.parameters.cx) com_dot = controller.model.center_of_mass_velocity()( - controller.q, controller.qdot, controller.parameters_except_time.cx + controller.q, controller.qdot, controller.parameters.cx ) com_height = (com_dot[2] * com_dot[2]) / (2 * -g) + com[2] return com_height @@ -635,7 +635,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, controller.parameters_except_time.cx) + return controller.model.center_of_mass()(controller.q, controller.parameters.cx) @staticmethod def minimize_com_velocity(penalty: PenaltyOption, controller: PenaltyController, axes: tuple | list = None): @@ -659,7 +659,7 @@ def minimize_com_velocity(penalty: PenaltyOption, controller: PenaltyController, penalty.quadratic = True if penalty.quadratic is None else penalty.quadratic return controller.model.center_of_mass_velocity()( - controller.q, controller.qdot, controller.parameters_except_time.cx + controller.q, controller.qdot, controller.parameters.cx ) @staticmethod @@ -686,7 +686,7 @@ def minimize_com_acceleration(penalty: PenaltyOption, controller: PenaltyControl qddot = PenaltyFunctionAbstract._get_qddot(controller, "cx") marker = controller.model.center_of_mass_acceleration()( - controller.q, controller.qdot, qddot, controller.parameters_except_time.cx + controller.q, controller.qdot, qddot, controller.parameters.cx ) return marker @@ -711,7 +711,7 @@ def minimize_angular_momentum(penalty: PenaltyOption, controller: PenaltyControl penalty.quadratic = True if penalty.quadratic is None else penalty.quadratic return controller.model.angular_momentum()( - controller.q, controller.qdot, controller.parameters_except_time.cx + controller.q, controller.qdot, controller.parameters.cx ) @staticmethod @@ -735,7 +735,7 @@ def minimize_linear_momentum(penalty: PenaltyOption, controller: PenaltyControll penalty.quadratic = True if penalty.quadratic is None else penalty.quadratic com_velocity = controller.model.center_of_mass_velocity()( - controller.q, controller.qdot, controller.parameters_except_time.cx + controller.q, controller.qdot, controller.parameters.cx ) mass = controller.model.mass()["o0"] linear_momentum_cx = com_velocity * mass @@ -874,7 +874,7 @@ def minimize_soft_contact_forces( controller.time.cx, controller.states.cx_start, controller.controls.cx_start, - controller.parameters_except_time.cx, + controller.parameters.cx, ) return soft_contact_force[force_idx] @@ -914,9 +914,9 @@ def track_segment_with_custom_rt( "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, controller.parameters_except_time.cx + controller.q, controller.parameters.cx )[:3, :3].T - r_rt = controller.model.rt(rt_idx)(controller.q, controller.parameters_except_time.cx)[:3, :3] + r_rt = controller.model.rt(rt_idx)(controller.q, 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) @@ -959,7 +959,7 @@ def track_marker_with_segment_axis( # Get the marker in rt reference frame marker = controller.model.marker(marker_idx, segment_idx)( - controller.q, controller.parameters_except_time.cx + controller.q, controller.parameters.cx ) # To align an axis, the other must be equal to 0 @@ -1011,7 +1011,7 @@ def minimize_segment_rotation( 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, controller.parameters_except_time.cx + controller.q, controller.parameters.cx )[:3, :3] angles_segment = controller.model.rotation_matrix_to_euler_angles(sequence)(jcs_segment) @@ -1058,7 +1058,7 @@ def minimize_segment_velocity( ) model: BiorbdModel = controller.model segment_angular_velocity = model.segment_angular_velocity(segment_idx)( - controller.q, controller.qdot, controller.parameters_except_time.cx + controller.q, controller.qdot, controller.parameters.cx ) if axes is None: @@ -1127,16 +1127,16 @@ def track_vector_orientations_from_markers( ) vector_0_marker_0_position = controller.model.marker(vector_0_marker_0_idx)( - controller.q, controller.parameters_except_time.cx + controller.q, controller.parameters.cx ) vector_0_marker_1_position = controller.model.marker(vector_0_marker_1_idx)( - controller.q, controller.parameters_except_time.cx + controller.q, controller.parameters.cx ) vector_1_marker_0_position = controller.model.marker(vector_1_marker_0_idx)( - controller.q, controller.parameters_except_time.cx + controller.q, controller.parameters.cx ) vector_1_marker_1_position = controller.model.marker(vector_1_marker_1_idx)( - controller.q, controller.parameters_except_time.cx + controller.q, controller.parameters.cx ) vector_0 = vector_0_marker_1_position - vector_0_marker_0_position diff --git a/bioptim/limits/penalty_controller.py b/bioptim/limits/penalty_controller.py index c8b2b57dc..98eecd16c 100644 --- a/bioptim/limits/penalty_controller.py +++ b/bioptim/limits/penalty_controller.py @@ -96,10 +96,6 @@ def get_nlp(self): def cx(self) -> MX | SX | Callable: return self._nlp.cx - @property - def to_casadi_func(self) -> Callable: - return self._nlp.to_casadi_func - @property def control_type(self) -> ControlType: return self._nlp.control_type @@ -370,17 +366,6 @@ def parameters(self) -> OptimizationVariableList: """ return self._nlp.parameters - @property - def parameters_except_time(self) -> OptimizationVariableList: - """ - Return the parameters - - Returns - ------- - The parameters - """ - return self._nlp.parameters_except_time - @property def parameters_scaled(self) -> OptimizationVariableList: """ @@ -396,7 +381,7 @@ def parameters_scaled(self) -> OptimizationVariableList: return MX() if type(self._nlp.parameters.scaled) == DM else self._nlp.parameters.scaled @property - def q(self) -> OptimizationVariable: + def q(self) -> MX | SX: if "q" in self.states: return self.states["q"].mapping.to_second.map(self.states["q"].cx) elif "q_roots" in self.states and "q_joints" in self.states: @@ -421,7 +406,7 @@ def q(self) -> OptimizationVariable: raise RuntimeError("q is not defined in the states") @property - def qdot(self) -> OptimizationVariable: + def qdot(self) -> MX | SX: if "qdot" in self.states: return self.states["qdot"].mapping.to_second.map(self.states["qdot"].cx) elif "qdot_roots" in self.states and "qdot_joints" in self.states: @@ -444,7 +429,7 @@ def qdot(self) -> OptimizationVariable: return qdot.cx @property - def tau(self) -> OptimizationVariable: + def tau(self) -> MX | SX: if "tau" in self.controls: return self.controls["tau"].mapping.to_second.map(self.controls["tau"].cx) elif "tau_joints" in self.controls: diff --git a/bioptim/limits/phase_transition.py b/bioptim/limits/phase_transition.py index d8ae34320..185c685e8 100644 --- a/bioptim/limits/phase_transition.py +++ b/bioptim/limits/phase_transition.py @@ -274,7 +274,7 @@ def impact(transition, controllers: list[PenaltyController, PenaltyController]): # Todo scaled? q_pre = pre.states["q"].cx qdot_pre = pre.states["qdot"].cx - qdot_impact = post.model.qdot_from_impact()(q_pre, qdot_pre, pre.parameters_except_time.cx) + qdot_impact = post.model.qdot_from_impact()(q_pre, qdot_pre, pre.parameters.cx) val = [] cx_start = [] diff --git a/bioptim/models/biorbd/biorbd_model.py b/bioptim/models/biorbd/biorbd_model.py index b22181863..0250675c6 100644 --- a/bioptim/models/biorbd/biorbd_model.py +++ b/bioptim/models/biorbd/biorbd_model.py @@ -48,6 +48,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)) + # TODO: remove mx (the MX parameters should be created inside the BiorbdModel) self.parameters = parameters.mx if parameters else MX() @property @@ -137,8 +138,7 @@ def rotation_matrix_to_euler_angles(self, sequence) -> Function: Returns the rotation matrix to euler angles function. """ r = MX.sym("r_mx", 3, 3) - # @Pariterre: is this the right order? - r_matrix = biorbd.Rotation(r[0, 0], r[0, 1], r[0, 2], r[0, 0], r[0, 1], r[0, 2], r[0, 0], r[0, 1], r[0, 2]) + r_matrix = biorbd.Rotation(r[0, 0], r[0, 1], r[0, 2], r[1, 0], r[1, 1], r[1, 2], r[2, 0], r[2, 1], r[2, 2]) biorbd_return = biorbd.Rotation.toEulerAngles(r_matrix, sequence).to_mx() casadi_fun = Function( "rotation_matrix_to_euler_angles", diff --git a/bioptim/models/biorbd/multi_biorbd_model.py b/bioptim/models/biorbd/multi_biorbd_model.py index 644cee3bc..f55166139 100644 --- a/bioptim/models/biorbd/multi_biorbd_model.py +++ b/bioptim/models/biorbd/multi_biorbd_model.py @@ -236,7 +236,17 @@ def nb_extra_models(self) -> int: @property def gravity(self) -> Function: - biorbd_return = vertcat(*(model.gravity()(self.parameters)["o0"] for model in self.models)) + for i, model in enumerate(self.models): + if i == 0: + if self.parameters.shape[0] == 0: + biorbd_return = model.gravity()["o0"] + else: + biorbd_return = model.gravity()(self.parameters)["o0"] + else: + if self.parameters.shape[0] == 0: + biorbd_return = vertcat(biorbd_return, model.gravity()["o0"]) + else: + biorbd_return = vertcat(biorbd_return, model.gravity()(self.parameters)["o0"]) casadi_fun = Function( "gravity", [self.parameters], @@ -309,7 +319,17 @@ def homogeneous_matrices_in_child(self, segment_id) -> Function: @property def mass(self) -> Function: - biorbd_return = vertcat(*(model.mass()(self.parameters)["o0"] for model in self.models)) + for i, model in enumerate(self.models): + if i == 0: + if self.parameters.shape[0] == 0: + biorbd_return = model.mass()["o0"] + else: + biorbd_return = model.mass()(self.parameters)["o0"] + else: + if self.parameters.shape[0] == 0: + biorbd_return = vertcat(biorbd_return, model.mass()["o0"]) + else: + biorbd_return = vertcat(biorbd_return, model.mass()(self.parameters)["o0"]) casadi_fun = Function( "mass", [self.parameters], @@ -473,7 +493,8 @@ def nb_muscles(self) -> int: def torque(self) -> Function: biorbd_return = MX() for i, model in enumerate(self.models): - tau_activations_model = self.muscle[self.variable_index("tau", i)] + model.model.closeActuator() + tau_activations_model = self.tau[self.variable_index("tau", i)] q_model = self.q[self.variable_index("q", i)] qdot_model = self.qdot[self.variable_index("qdot", i)] biorbd_return = vertcat( @@ -487,7 +508,7 @@ def torque(self) -> Function: ) casadi_fun = Function( "torque", - [self.muscle, self.q, self.qdot, self.parameters], + [self.tau, self.q, self.qdot, self.parameters], [biorbd_return], ) return casadi_fun @@ -566,11 +587,11 @@ def inverse_dynamics(self) -> Function: qddot_model = self.qddot[self.variable_index("qddot", i)] biorbd_return = vertcat( biorbd_return, - model.inverse_dynamics()(q_model, qdot_model, qddot_model, self.parameters), + model.inverse_dynamics()(q_model, qdot_model, qddot_model, [], self.parameters), ) casadi_fun = Function( "inverse_dynamics", - [self.q, self.qdot, self.qddot, self.parameters], + [self.q, self.qdot, self.qddot, [], self.parameters], [biorbd_return], ) return casadi_fun @@ -656,7 +677,6 @@ def markers(self) -> Function: for i, model in enumerate(self.models): q_model = self.q[self.variable_index("q", i)] biorbd_return += [model.markers()(q_model, self.parameters)] - biorbd_return = [item for sublist in biorbd_return for item in sublist] casadi_fun = Function( "markers", [self.q, self.parameters], @@ -838,11 +858,11 @@ def contact_forces(self) -> Function: qdot_model = self.qdot[self.variable_index("qdot", i)] tau_model = self.tau[self.variable_index("tau", i)] - contact_forces = model.contact_forces()(q_model, qdot_model, tau_model, self.parameters) + contact_forces = model.contact_forces()(q_model, qdot_model, tau_model, [], self.parameters) biorbd_return = vertcat(biorbd_return, contact_forces) casadi_fun = Function( "contact_forces", - [self.q, self.qdot, self.tau, self.parameters], + [self.q, self.qdot, self.tau, [], self.parameters], [biorbd_return], ) return casadi_fun diff --git a/bioptim/optimization/non_linear_program.py b/bioptim/optimization/non_linear_program.py index 7d2f74de9..a745dd26e 100644 --- a/bioptim/optimization/non_linear_program.py +++ b/bioptim/optimization/non_linear_program.py @@ -75,8 +75,6 @@ class NonLinearProgram: The mapping for the plots tf: float The time stamp of the end of the phase - tf_mx: - The time stamp of the end of the phase variable_mappings: BiMappingList The list of mapping for all the variables u_bounds = Bounds() @@ -122,10 +120,6 @@ class NonLinearProgram: Add a new element to the nlp of the format 'nlp.param_name = param' or 'nlp.name["param_name"] = param' add_path_condition(ocp: OptimalControlProgram, var: Any, path_name: str, type_option: Any, type_list: Any) Interface to add for PathCondition classes - add_casadi_func(self, name: str, function: Callable, *all_param: Any) -> casadi.Function: - Add to the pool of declared casadi function. If the function already exists, it is skipped - to_casadi_func - Converts a symbolic expression into a casadi function node_time(self, node_idx: int) Gives the time for a specific index """ @@ -182,22 +176,18 @@ def __init__(self, phase_dynamics: PhaseDynamics, use_sx: bool): self.phase_dynamics = phase_dynamics self.time_index = None self.time_cx = None - self.time_mx = None self.dt = None - self.dt_mx = None self.tf = None - self.tf_mx = None self.states = OptimizationVariableContainer(self.phase_dynamics) self.states_dot = OptimizationVariableContainer(self.phase_dynamics) self.controls = OptimizationVariableContainer(self.phase_dynamics) + self.numerical_data_timeseries = OptimizationVariableContainer(self.phase_dynamics) # parameters is currently a clone of ocp.parameters, but should hold phase parameters from ..optimization.parameters import ParameterContainer self.parameters = ParameterContainer(use_sx=use_sx) - self.parameters_except_time = ParameterContainer(use_sx=use_sx) self.algebraic_states = OptimizationVariableContainer(self.phase_dynamics) self.integrated_values = OptimizationVariableContainer(self.phase_dynamics) - self.numerical_timeseries = OptimizationVariableContainer(self.phase_dynamics) def initialize(self, cx: MX | SX | Callable = None): """ @@ -398,80 +388,6 @@ def __setattr(nlp, name: str | None, param_name: str, param: Any): else: getattr(nlp, name)[param_name] = param - def add_casadi_func(self, name: str, function: Callable | SX | MX, *all_param: Any) -> casadi.Function: - """ - Add to the pool of declared casadi function. If the function already exists, it is skipped - - Parameters - ---------- - name: str - The unique name of the function to add to the casadi functions pool - function: Callable | SX | MX - The biorbd function to add - all_param: dict - Any parameters to pass to the biorbd function - """ - - if name in self.casadi_func: - return self.casadi_func[name] - else: - mx = [var.mx if isinstance(var, OptimizationVariable) else var for var in all_param] - self.casadi_func[name] = self.to_casadi_func(name, function, *mx) - return self.casadi_func[name] - - @staticmethod - def to_casadi_func(name, symbolic_expression: MX | SX | Callable, *all_param, expand=True) -> Function: - """ - Converts a symbolic expression into a casadi function - - Parameters - ---------- - name: str - The name of the function - symbolic_expression: MX | SX | Callable - The symbolic expression to be converted, also support Callables - all_param: Any - Any parameters to pass to the biorbd function - expand: bool - If the function should be expanded - - Returns - ------- - The converted function - - """ - cx_param = [] - for p in all_param: - if isinstance(p, (MX, SX)): - cx_param.append(p) - - if isinstance(symbolic_expression, (MX, SX, Function)): - func_evaluated = symbolic_expression - else: - func_evaluated = symbolic_expression(*all_param) - if isinstance(func_evaluated, (list, tuple)): - func_evaluated = horzcat(*[val if isinstance(val, MX) else val.to_mx() for val in func_evaluated]) - elif not isinstance(func_evaluated, MX): - func_evaluated = func_evaluated.to_mx() - func = Function(name, cx_param, [func_evaluated]) - - if expand: - try: - func = func.expand() - except Exception as me: - raise RuntimeError( - f"An error occurred while executing the 'expand()' function for {name}. Please review the following " - "casadi error message for more details.\n" - "Several factors could be causing this issue. If you are creating your own casadi function, " - "it is possible that you have free variables. Another possibility, if you are using a predefined " - "function, the error might be due to the inability to use expand=True at all. In that case, try " - "adding expand=False to the dynamics or the penalty.\n" - "Original casadi error message:\n" - f"{me}" - ) - - return func.expand() if expand else func - def node_time(self, node_idx: int): """ Gives the time for a specific index diff --git a/bioptim/optimization/optimal_control_program.py b/bioptim/optimization/optimal_control_program.py index 470cb023b..c857029f0 100644 --- a/bioptim/optimization/optimal_control_program.py +++ b/bioptim/optimization/optimal_control_program.py @@ -626,7 +626,6 @@ def _prepare_dynamics(self): for i in range(self.n_phases): self.nlp[i].initialize(self.cx) self.nlp[i].parameters = self.parameters # This should be remove when phase parameters will be implemented - self.nlp[i].parameters_except_time = self.parameters_except_time self.nlp[i].numerical_data_timeseries = self.nlp[i].dynamics_type.numerical_data_timeseries ConfigureProblem.initialize(self, self.nlp[i]) self.nlp[i].ode_solver.prepare_dynamic_integrator(self, self.nlp[i]) @@ -1065,9 +1064,6 @@ def _declare_parameters(self, parameters: ParameterList): self.parameters = ParameterContainer(use_sx=(True if self.cx == SX else False)) self.parameters.initialize(parameters) - # The version without time will not be updated later when time is declared - self.parameters_except_time = ParameterContainer(use_sx=(True if self.cx == SX else False)) - self.parameters_except_time.initialize(parameters) def update_bounds( self, @@ -1651,7 +1647,6 @@ def define_parameters_phase_time( dt_bounds = {} dt_initial_guess = {} dt_cx = [] - # dt_mx = [] for i_phase in range(self.n_phases): if i_phase == self.time_phase_mapping.to_second.map_idx[i_phase]: dt = self.phase_time[i_phase] / self.nlp[i_phase].ns @@ -1659,7 +1654,6 @@ def define_parameters_phase_time( dt_initial_guess[f"dt_phase_{i_phase}"] = dt dt_cx.append(self.dt_parameter[self.time_phase_mapping.to_second.map_idx[i_phase]].cx) - # dt_mx.append(self.dt_parameter[self.time_phase_mapping.to_second.map_idx[i_phase]].mx) has_penalty = define_parameters_phase_time(self, objective_functions) define_parameters_phase_time(self, constraints, has_penalty) @@ -1667,11 +1661,8 @@ def define_parameters_phase_time( # Add to the nlp NLP.add(self, "time_index", self.time_phase_mapping.to_second.map_idx, True) NLP.add(self, "time_cx", self.cx.sym("time", 1, 1), True) - # NLP.add(self, "time_mx", MX.sym("time", 1, 1), True) NLP.add(self, "dt", dt_cx, False) - # NLP.add(self, "dt_mx", dt_mx, False) NLP.add(self, "tf", [nlp.dt * max(nlp.ns, 1) for nlp in self.nlp], False) - # NLP.add(self, "tf_mx", [nlp.dt_mx * max(nlp.ns, 1) for nlp in self.nlp], False) self.dt_parameter_bounds = Bounds( "dt_bounds", diff --git a/bioptim/optimization/parameters.py b/bioptim/optimization/parameters.py index 52eb91b9e..55d3e2044 100644 --- a/bioptim/optimization/parameters.py +++ b/bioptim/optimization/parameters.py @@ -66,6 +66,7 @@ def __init__( @property def mx(self): + # TODO: this should removed and placed in the BiorbdModel return self._mx @property diff --git a/tests/shard1/test_biorbd_model_size.py b/tests/shard1/test_biorbd_model_size.py deleted file mode 100644 index f8e7b5913..000000000 --- a/tests/shard1/test_biorbd_model_size.py +++ /dev/null @@ -1,631 +0,0 @@ -import pytest -from bioptim import BiorbdModel -from casadi import MX -from tests.utils import TestUtils - -bioptim_folder = TestUtils.bioptim_folder() - - -@pytest.fixture -def model(): - return - - -# Pariterre: Can I remove this file all together? - - -def generate_q_vectors(model): - q_valid = MX([0.1] * model.nb_q) - q_too_large = MX([0.1] * (model.nb_q + 1)) - return q_valid, q_too_large - - -def generate_q_and_qdot_vectors(model): - q_valid = MX([0.1] * model.nb_q) - qdot_valid = MX([0.1] * model.nb_qdot) - q_too_large = MX([0.1] * (model.nb_q + 1)) - qdot_too_large = MX([0.1] * (model.nb_qdot + 1)) - - return q_valid, qdot_valid, q_too_large, qdot_too_large - - -def generate_q_qdot_qddot_vectors(model, root_dynamics=False): - q_valid = MX([0.1] * model.nb_q) - qdot_valid = MX([0.1] * model.nb_qdot) - nb_qddot = model.nb_qddot - model.nb_root if root_dynamics else model.nb_qddot - qddot_valid = MX([0.1] * nb_qddot) - - q_too_large = MX([0.1] * (model.nb_q + 1)) - qdot_too_large = MX([0.1] * (model.nb_qdot + 1)) - qddot_too_large = MX([0.1] * (model.nb_qddot + 1)) - - return ( - q_valid, - qdot_valid, - qddot_valid, - q_too_large, - qdot_too_large, - qddot_too_large, - ) - - -def generate_tau_activations_vectors(model): - tau_activations_valid = MX([0.1] * model.nb_tau) - tau_activations_too_large = MX([0.1] * (model.nb_tau + 1)) - return tau_activations_valid, tau_activations_too_large - - -def generate_muscle_vectors(model): - muscle_valid = MX([0.1] * model.nb_muscles) - muscle_too_large = MX([0.1] * (model.nb_muscles + 1)) - return muscle_valid, muscle_too_large - - -def test_center_of_mass_valid_and_too_large_q_input(model): - biorbd_model_path = ( - bioptim_folder + "/examples/muscle_driven_with_contact/models/2segments_4dof_2contacts_1muscle.bioMod" - ) - model = BiorbdModel(biorbd_model_path) - q_valid, q_too_large = generate_q_vectors(model) - - # q valid - model.center_of_mass()(q_valid) - # q not valid - with pytest.raises(ValueError, match="Length of q size should be: 4, but got: 5"): - model.center_of_mass(q_too_large) - - -def test_center_of_mass_velocity_valid_and_too_large_q_or_qdot_input(model): - biorbd_model_path = ( - bioptim_folder + "/examples/muscle_driven_with_contact/models/2segments_4dof_2contacts_1muscle.bioMod" - ) - model = BiorbdModel(biorbd_model_path) - q_valid, qdot_valid, q_too_large, qdot_too_large = generate_q_and_qdot_vectors(model) - - # q and qdot valid - model.center_of_mass_velocity()(q_valid, qdot_valid) - # qdot valid but q not valid - with pytest.raises(ValueError, match="Length of q size should be: 4, but got: 5"): - model.center_of_mass_velocity()(q_too_large, qdot_valid) - # q valid but qdot not valid - with pytest.raises(ValueError, match="Length of qdot size should be: 4, but got: 5"): - model.center_of_mass_velocity()(q_valid, qdot_too_large) - - -def test_center_of_mass_acceleration_valid_and_too_large_q_or_qdot_or_qddot_input( - model, -): - biorbd_model_path = ( - bioptim_folder + "/examples/muscle_driven_with_contact/models/2segments_4dof_2contacts_1muscle.bioMod" - ) - model = BiorbdModel(biorbd_model_path) - ( - q_valid, - qdot_valid, - qddot_valid, - q_too_large, - qdot_too_large, - qddot_too_large, - ) = generate_q_qdot_qddot_vectors(model) - - # q, qdot and qddot valid - model.center_of_mass_acceleration()(q_valid, qdot_valid, qddot_valid) - # qdot and qddot valid but q not valid - with pytest.raises(ValueError, match="Length of q size should be: 4, but got: 5"): - model.center_of_mass_acceleration()(q_too_large, qdot_valid, qddot_valid) - # q and qddot valid but qdot not valid - with pytest.raises(ValueError, match="Length of qdot size should be: 4, but got: 5"): - model.center_of_mass_acceleration()(q_valid, qdot_too_large, qddot_valid) - # q and qdot valid but qddot not valid - with pytest.raises(ValueError, match="Length of qddot size should be: 4, but got: 5"): - model.center_of_mass_acceleration()(q_valid, qdot_valid, qddot_too_large) - - -def test_body_rotation_rate_valid_and_too_large_q_or_qdot_input(model): - biorbd_model_path = ( - bioptim_folder + "/examples/muscle_driven_with_contact/models/2segments_4dof_2contacts_1muscle.bioMod" - ) - model = BiorbdModel(biorbd_model_path) - q_valid, qdot_valid, q_too_large, qdot_too_large = generate_q_and_qdot_vectors(model) - - # q and qdot valid - model.body_rotation_rate()(q_valid, qdot_valid) - # qdot valid but q not valid - with pytest.raises(ValueError, match="Length of q size should be: 4, but got: 5"): - model.body_rotation_rate()(q_too_large, qdot_valid) - # q valid but qdot not valid - with pytest.raises(ValueError, match="Length of qdot size should be: 4, but got: 5"): - model.body_rotation_rate()(q_valid, qdot_too_large) - - -def test_mass_matrix_valid_and_too_large_q_input(model): - biorbd_model_path = ( - bioptim_folder + "/examples/muscle_driven_with_contact/models/2segments_4dof_2contacts_1muscle.bioMod" - ) - model = BiorbdModel(biorbd_model_path) - q_valid, q_too_large = generate_q_vectors(model) - - # q valid - model.mass_matrix()(q_valid) - # q not valid - with pytest.raises(ValueError, match="Length of q size should be: 4, but got: 5"): - model.mass_matrix()(q_too_large) - - -def test_non_linear_effects_valid_and_too_large_q_or_qdot_input(model): - biorbd_model_path = ( - bioptim_folder + "/examples/muscle_driven_with_contact/models/2segments_4dof_2contacts_1muscle.bioMod" - ) - model = BiorbdModel(biorbd_model_path) - q_valid, qdot_valid, q_too_large, qdot_too_large = generate_q_and_qdot_vectors(model) - - # q and qdot valid - model.non_linear_effects()(q_valid, qdot_valid) - # qdot valid but q not valid - with pytest.raises(ValueError, match="Length of q size should be: 4, but got: 5"): - model.non_linear_effects()(q_too_large, qdot_valid) - # q valid but qdot not valid - with pytest.raises(ValueError, match="Length of qdot size should be: 4, but got: 5"): - model.non_linear_effects()(q_valid, qdot_too_large) - - -def test_angular_momentum_valid_and_too_large_q_or_qdot_input(model): - biorbd_model_path = ( - bioptim_folder + "/examples/muscle_driven_with_contact/models/2segments_4dof_2contacts_1muscle.bioMod" - ) - model = BiorbdModel(biorbd_model_path) - q_valid, qdot_valid, q_too_large, qdot_too_large = generate_q_and_qdot_vectors(model) - - # q and qdot valid - model.angular_momentum()(q_valid, qdot_valid) - # qdot valid but q not valid - with pytest.raises(ValueError, match="Length of q size should be: 4, but got: 5"): - model.angular_momentum()(q_too_large, qdot_valid) - # q valid but qdot not valid - with pytest.raises(ValueError, match="Length of qdot size should be: 4, but got: 5"): - model.angular_momentum()(q_valid, qdot_too_large) - - -def test_reshape_qdot_valid_and_too_large_q_or_qdot_input(model): - biorbd_model_path = ( - bioptim_folder + "/examples/muscle_driven_with_contact/models/2segments_4dof_2contacts_1muscle.bioMod" - ) - model = BiorbdModel(biorbd_model_path) - q_valid, qdot_valid, q_too_large, qdot_too_large = generate_q_and_qdot_vectors(model) - - # q and qdot valid - model.reshape_qdot()(q_valid, qdot_valid) - # qdot valid but q not valid - with pytest.raises(ValueError, match="Length of q size should be: 4, but got: 5"): - model.reshape_qdot()(q_too_large, qdot_valid) - # q valid but qdot not valid - with pytest.raises(ValueError, match="Length of qdot size should be: 4, but got: 5"): - model.reshape_qdot()(q_valid, qdot_too_large) - - -def test_segment_angular_velocity_valid_and_too_large_q_or_qdot_input(model): - biorbd_model_path = ( - bioptim_folder + "/examples/muscle_driven_with_contact/models/2segments_4dof_2contacts_1muscle.bioMod" - ) - model = BiorbdModel(biorbd_model_path) - q_valid, qdot_valid, q_too_large, qdot_too_large = generate_q_and_qdot_vectors(model) - idx = 1 - # q and qdot valid - model.segment_angular_velocity(idx)(q_valid, qdot_valid) - # qdot valid but q not valid - with pytest.raises(ValueError, match="Length of q size should be: 4, but got: 5"): - model.segment_angular_velocity(idx)(q_too_large, qdot_valid) - # q valid but qdot not valid - with pytest.raises(ValueError, match="Length of qdot size should be: 4, but got: 5"): - model.segment_angular_velocity(idx)(q_valid, qdot_too_large) - - -def test_forward_dynamics_free_floating_base_valid_and_too_large_q_or_qdot_or_qddot_joints_input( - model, -): - biorbd_model_path = ( - bioptim_folder + "/examples/muscle_driven_with_contact/models/2segments_4dof_2contacts_1muscle.bioMod" - ) - model = BiorbdModel(biorbd_model_path) - ( - q_valid, - qdot_valid, - qddot_joints_valid, - q_too_large, - qdot_too_large, - qddot_joints_too_large, - ) = generate_q_qdot_qddot_vectors(model, root_dynamics=True) - - # q, qdot and qddot_joints valid - model.forward_dynamics_free_floating_base()(q_valid, qdot_valid, qddot_joints_valid) - # qdot and qddot_joints valid but q not valid - with pytest.raises(ValueError, match="Length of q size should be: 4, but got: 5"): - model.forward_dynamics_free_floating_base()(q_too_large, qdot_valid, qddot_joints_valid) - # q and qddot_joints valid but qdot not valid - with pytest.raises(ValueError, match="Length of qdot size should be: 4, but got: 5"): - model.forward_dynamics_free_floating_base()(q_valid, qdot_too_large, qddot_joints_valid) - # q and qdot valid but qddot_joints not valid - with pytest.raises(ValueError, match="Length of qddot_joints size should be: 1, but got: 5"): - model.forward_dynamics_free_floating_base()(q_valid, qdot_valid, qddot_joints_too_large) - - -def test_forward_dynamics_valid_and_too_large_q_or_qdot_or_tau_activations_input(model): - biorbd_model_path = ( - bioptim_folder + "/examples/muscle_driven_with_contact/models/2segments_4dof_2contacts_1muscle.bioMod" - ) - model = BiorbdModel(biorbd_model_path) - q_valid, qdot_valid, q_too_large, qdot_too_large = generate_q_and_qdot_vectors(model) - tau_valid, tau_too_large = generate_tau_activations_vectors(model) - - # q, qdot and tau valid - model.forward_dynamics()(q_valid, qdot_valid, tau_valid, []) - # qdot and tau valid but q not valid - with pytest.raises(ValueError, match="Length of q size should be: 4, but got: 5"): - model.forward_dynamics()(q_too_large, qdot_valid, tau_valid, []) - # q and tau valid but qdot not valid - with pytest.raises(ValueError, match="Length of qdot size should be: 4, but got: 5"): - model.forward_dynamics()(q_valid, qdot_too_large, tau_valid, []) - # q and qdot valid but tau not valid - with pytest.raises(ValueError, match="Length of tau size should be: 4, but got: 5"): - model.forward_dynamics()(q_valid, qdot_valid, tau_too_large, []) - - -def test_constrained_forward_dynamics_valid_and_too_large_q_or_qdot_or_tau_input(model): - biorbd_model_path = ( - bioptim_folder + "/examples/muscle_driven_with_contact/models/2segments_4dof_2contacts_1muscle.bioMod" - ) - model = BiorbdModel(biorbd_model_path) - q_valid, qdot_valid, q_too_large, qdot_too_large = generate_q_and_qdot_vectors(model) - tau_valid, tau_too_large = generate_tau_activations_vectors(model) - - # q, qdot and tau valid - model.forward_dynamics(with_contact=True)(q_valid, qdot_valid, tau_valid) - # qdot and tau valid but q not valid - with pytest.raises(ValueError, match="Length of q size should be: 4, but got: 5"): - model.forward_dynamics(with_contact=True)(q_too_large, qdot_valid, tau_valid) - # q and tau valid but qdot not valid - with pytest.raises(ValueError, match="Length of qdot size should be: 4, but got: 5"): - model.forward_dynamics(with_contact=True)(q_valid, qdot_too_large, tau_valid) - # q and qdot valid but tau not valid - with pytest.raises(ValueError, match="Length of tau size should be: 4, but got: 5"): - model.forward_dynamics(with_contact=True)(q_valid, qdot_valid, tau_too_large) - - -def test_inverse_dynamics_valid_and_too_large_q_or_qdot_or_qddot_input(model): - biorbd_model_path = ( - bioptim_folder + "/examples/muscle_driven_with_contact/models/2segments_4dof_2contacts_1muscle.bioMod" - ) - model = BiorbdModel(biorbd_model_path) - ( - q_valid, - qdot_valid, - qddot_valid, - q_too_large, - qdot_too_large, - qddot_too_large, - ) = generate_q_qdot_qddot_vectors(model) - - # q, qdot and qddot valid - model.inverse_dynamics(q_valid, qdot_valid, qddot_valid) - # qdot and qddot valid but q not valid - with pytest.raises(ValueError, match="Length of q size should be: 4, but got: 5"): - model.inverse_dynamics(q_too_large, qdot_valid, qddot_valid) - # q and qddot valid but qdot not valid - with pytest.raises(ValueError, match="Length of qdot size should be: 4, but got: 5"): - model.inverse_dynamics(q_valid, qdot_too_large, qddot_valid) - # q and qdot valid but qddot not valid - with pytest.raises(ValueError, match="Length of qddot size should be: 4, but got: 5"): - model.inverse_dynamics(q_valid, qdot_valid, qddot_too_large) - - -def test_contact_forces_from_constrained_forward_dynamics_valid_and_too_large_q_or_qdot_or_tau_input( - model, -): - biorbd_model_path = ( - bioptim_folder + "/examples/muscle_driven_with_contact/models/2segments_4dof_2contacts_1muscle.bioMod" - ) - model = BiorbdModel(biorbd_model_path) - q_valid, qdot_valid, q_too_large, qdot_too_large = generate_q_and_qdot_vectors(model) - tau_valid, tau_too_large = generate_tau_activations_vectors(model) - - # q, qdot and tau valid - model.contact_forces_from_constrained_forward_dynamics(q_valid, qdot_valid, tau_valid) - # qdot and tau valid but q not valid - with pytest.raises(ValueError, match="Length of q size should be: 4, but got: 5"): - model.contact_forces_from_constrained_forward_dynamics(q_too_large, qdot_valid, tau_valid) - # q and tau valid but qdot not valid - with pytest.raises(ValueError, match="Length of qdot size should be: 4, but got: 5"): - model.contact_forces_from_constrained_forward_dynamics(q_valid, qdot_too_large, tau_valid) - # q and qdot valid but tau not valid - with pytest.raises(ValueError, match="Length of tau size should be: 4, but got: 5"): - model.contact_forces_from_constrained_forward_dynamics(q_valid, qdot_valid, tau_too_large) - - -def test_qdot_from_impact_valid_and_too_large_q_or_qdot_input(model): - biorbd_model_path = ( - bioptim_folder + "/examples/muscle_driven_with_contact/models/2segments_4dof_2contacts_1muscle.bioMod" - ) - model = BiorbdModel(biorbd_model_path) - q_valid, qdot_valid, q_too_large, qdot_too_large = generate_q_and_qdot_vectors(model) - - # q and qdot valid - model.qdot_from_impact(q_valid, qdot_valid) - # qdot valid but q not valid - with pytest.raises(ValueError, match="Length of q size should be: 4, but got: 5"): - model.qdot_from_impact(q_too_large, qdot_valid) - # q valid but qdot not valid - with pytest.raises(ValueError, match="Length of qdot size should be: 4, but got: 5"): - model.qdot_from_impact(q_valid, qdot_too_large) - - -def test_muscle_activation_dot_valid_and_too_large_q_input(model): - biorbd_model_path = ( - bioptim_folder + "/examples/muscle_driven_with_contact/models/2segments_4dof_2contacts_1muscle.bioMod" - ) - model = BiorbdModel(biorbd_model_path) - muscle_valid, muscle_too_large = generate_muscle_vectors(model) - - # muscle valid - model.muscle_activation_dot(muscle_valid) - # muscle not valid - with pytest.raises(ValueError, match="Length of muscle size should be: 1, but got: 2"): - model.muscle_activation_dot(muscle_too_large) - - -def test_muscle_length_jacobian_valid_and_too_large_q_input(model): - biorbd_model_path = ( - bioptim_folder + "/examples/muscle_driven_with_contact/models/2segments_4dof_2contacts_1muscle.bioMod" - ) - model = BiorbdModel(biorbd_model_path) - q_valid, q_too_large = generate_q_vectors(model) - - # q valid - model.muscle_length_jacobian(q_valid) - # q not valid - with pytest.raises(ValueError, match="Length of q size should be: 4, but got: 5"): - model.muscle_length_jacobian(q_too_large) - - -def test_muscle_velocity_valid_and_too_large_q_or_qdot_input(model): - biorbd_model_path = ( - bioptim_folder + "/examples/muscle_driven_with_contact/models/2segments_4dof_2contacts_1muscle.bioMod" - ) - model = BiorbdModel(biorbd_model_path) - q_valid, qdot_valid, q_too_large, qdot_too_large = generate_q_and_qdot_vectors(model) - - # q and qdot valid - model.muscle_velocity(q_valid, qdot_valid) - # qdot valid but q not valid - with pytest.raises(ValueError, match="Length of q size should be: 4, but got: 5"): - model.muscle_velocity(q_too_large, qdot_valid) - # q valid but qdot not valid - with pytest.raises(ValueError, match="Length of qdot size should be: 4, but got: 5"): - model.muscle_velocity(q_valid, qdot_too_large) - - -def test_muscle_joint_torque_valid_and_too_large_q_or_qdot_or_qddot_input(model): - biorbd_model_path = ( - bioptim_folder + "/examples/muscle_driven_with_contact/models/2segments_4dof_2contacts_1muscle.bioMod" - ) - model = BiorbdModel(biorbd_model_path) - ( - q_valid, - qdot_valid, - qddot_valid, - q_too_large, - qdot_too_large, - qddot_too_large, - ) = generate_q_qdot_qddot_vectors(model) - muscle_valid, muscle_too_large = generate_muscle_vectors(model) - - # q, qdot and qddot valid - model.muscle_joint_torque(muscle_valid, q_valid, qdot_valid) - # qdot and qddot valid but q not valid - with pytest.raises(ValueError, match="Length of q size should be: 4, but got: 5"): - model.muscle_joint_torque(muscle_valid, q_too_large, qdot_valid) - # q and qddot valid but qdot not valid - with pytest.raises(ValueError, match="Length of qdot size should be: 4, but got: 5"): - model.muscle_joint_torque(muscle_valid, q_valid, qdot_too_large) - # q and qdot valid but qddot not valid - with pytest.raises(ValueError, match="Length of muscle size should be: 1, but got: 2"): - model.muscle_joint_torque(muscle_too_large, q_valid, qdot_valid) - - -def test_markers_valid_and_too_large_q_input(model): - biorbd_model_path = ( - bioptim_folder + "/examples/muscle_driven_with_contact/models/2segments_4dof_2contacts_1muscle.bioMod" - ) - model = BiorbdModel(biorbd_model_path) - q_valid, q_too_large = generate_q_vectors(model) - - # q valid - model.markers(q_valid) - # q not valid - with pytest.raises(ValueError, match="Length of q size should be: 4, but got: 5"): - model.markers(q_too_large) - - -def test_marker_valid_and_too_large_q_input(model): - biorbd_model_path = ( - bioptim_folder + "/examples/muscle_driven_with_contact/models/2segments_4dof_2contacts_1muscle.bioMod" - ) - model = BiorbdModel(biorbd_model_path) - q_valid, q_too_large = generate_q_vectors(model) - - # q valid - model.marker(q_valid, 1) - # q not valid - with pytest.raises(ValueError, match="Length of q size should be: 4, but got: 5"): - model.marker(q_too_large, 1) - - -def test_marker_velocities_valid_and_too_large_q_or_qdot_input(model): - biorbd_model_path = ( - bioptim_folder + "/examples/muscle_driven_with_contact/models/2segments_4dof_2contacts_1muscle.bioMod" - ) - model = BiorbdModel(biorbd_model_path) - q_valid, qdot_valid, q_too_large, qdot_too_large = generate_q_and_qdot_vectors(model) - - # q and qdot valid - model.marker_velocities(q_valid, qdot_valid) - # qdot valid but q not valid - with pytest.raises(ValueError, match="Length of q size should be: 4, but got: 5"): - model.marker_velocities(q_too_large, qdot_valid) - # q valid but qdot not valid - with pytest.raises(ValueError, match="Length of qdot size should be: 4, but got: 5"): - model.marker_velocities(q_valid, qdot_too_large) - - -def test_marker_accelerations_valid_and_too_large_q_or_qdot_or_qddot_input(model): - biorbd_model_path = ( - bioptim_folder + "/examples/muscle_driven_with_contact/models/2segments_4dof_2contacts_1muscle.bioMod" - ) - model = BiorbdModel(biorbd_model_path) - ( - q_valid, - qdot_valid, - qddot_valid, - q_too_large, - qdot_too_large, - qddot_too_large, - ) = generate_q_qdot_qddot_vectors(model) - - # q, qdot and qddot valid - model.marker_accelerations(q_valid, qdot_valid, qddot_valid) - # qdot and qddot valid but q not valid - with pytest.raises(ValueError, match="Length of q size should be: 4, but got: 5"): - model.marker_accelerations(q_too_large, qdot_valid, qddot_valid) - # q and qddot valid but qdot not valid - with pytest.raises(ValueError, match="Length of qdot size should be: 4, but got: 5"): - model.marker_accelerations(q_valid, qdot_too_large, qddot_valid) - # q and qdot valid but qddot not valid - with pytest.raises(ValueError, match="Length of qddot size should be: 4, but got: 5"): - model.marker_accelerations(q_valid, qdot_valid, qddot_too_large) - - -def test_tau_max_valid_and_too_large_q_or_qdot_input(model): - biorbd_model_path = bioptim_folder + "/examples/optimal_time_ocp/models/cube.bioMod" - model = BiorbdModel(biorbd_model_path) - q_valid, qdot_valid, q_too_large, qdot_too_large = generate_q_and_qdot_vectors(model) - - # q and qdot valid - model.tau_max(q_valid, qdot_valid) - # qdot valid but q not valid - with pytest.raises(ValueError, match="Length of q size should be: 3, but got: 4"): - model.tau_max(q_too_large, qdot_valid) - # q valid but qdot not valid - with pytest.raises(ValueError, match="Length of qdot size should be: 3, but got: 4"): - model.tau_max(q_valid, qdot_too_large) - - -def test_rigid_contact_acceleration_valid_and_too_large_q_or_qdot_or_qddot_input(model): - biorbd_model_path = ( - bioptim_folder + "/examples/muscle_driven_with_contact/models/2segments_4dof_2contacts_1muscle.bioMod" - ) - model = BiorbdModel(biorbd_model_path) - ( - q_valid, - qdot_valid, - qddot_valid, - q_too_large, - qdot_too_large, - qddot_too_large, - ) = generate_q_qdot_qddot_vectors(model) - - # q, qdot and qddot valid - model.rigid_contact_acceleration(q_valid, qdot_valid, qddot_valid, 0, 0) - # qdot and qddot valid but q not valid - with pytest.raises(ValueError, match="Length of q size should be: 4, but got: 5"): - model.rigid_contact_acceleration(q_too_large, qdot_valid, qddot_valid, 0, 0) - # q and qddot valid but qdot not valid - with pytest.raises(ValueError, match="Length of qdot size should be: 4, but got: 5"): - model.rigid_contact_acceleration(q_valid, qdot_too_large, qddot_valid, 0, 0) - # q and qdot valid but qddot not valid - with pytest.raises(ValueError, match="Length of qddot size should be: 4, but got: 5"): - model.rigid_contact_acceleration(q_valid, qdot_valid, qddot_too_large, 0, 0) - - -def test_markers_jacobian_valid_and_too_large_q_input(model): - biorbd_model_path = ( - bioptim_folder + "/examples/muscle_driven_with_contact/models/2segments_4dof_2contacts_1muscle.bioMod" - ) - model = BiorbdModel(biorbd_model_path) - q_valid, q_too_large = generate_q_vectors(model) - - # q valid - model.markers_jacobian(q_valid) - # q not valid - with pytest.raises(ValueError, match="Length of q size should be: 4, but got: 5"): - model.markers_jacobian(q_too_large) - - -def test_soft_contact_forces_valid_and_too_large_q_or_qdot_input(model): - biorbd_model_path = ( - bioptim_folder + "/examples/muscle_driven_with_contact/models/2segments_4dof_2contacts_1muscle.bioMod" - ) - model = BiorbdModel(biorbd_model_path) - q_valid, qdot_valid, q_too_large, qdot_too_large = generate_q_and_qdot_vectors(model) - - # q and qdot valid - model.soft_contact_forces(q_valid, qdot_valid) - # qdot valid but q not valid - with pytest.raises(ValueError, match="Length of q size should be: 4, but got: 5"): - model.soft_contact_forces(q_too_large, qdot_valid) - # q valid but qdot not valid - with pytest.raises(ValueError, match="Length of qdot size should be: 4, but got: 5"): - model.soft_contact_forces(q_valid, qdot_too_large) - - -def test_contact_forces_valid_and_too_large_q_or_qdot_or_tau_input(model): - biorbd_model_path = ( - bioptim_folder + "/examples/muscle_driven_with_contact/models/2segments_4dof_2contacts_1muscle.bioMod" - ) - model = BiorbdModel(biorbd_model_path) - q_valid, qdot_valid, q_too_large, qdot_too_large = generate_q_and_qdot_vectors(model) - tau_valid, tau_too_large = generate_tau_activations_vectors(model) - - # q, qdot and tau valid - model.contact_forces(q_valid, qdot_valid, tau_valid) - # qdot and tau valid but q not valid - with pytest.raises(ValueError, match="Length of q size should be: 4, but got: 5"): - model.contact_forces(q_too_large, qdot_valid, tau_valid) - # q and tau valid but qdot not valid - with pytest.raises(ValueError, match="Length of qdot size should be: 4, but got: 5"): - model.contact_forces(q_valid, qdot_too_large, tau_valid) - # q and qdot valid but tau not valid - with pytest.raises(ValueError, match="Length of tau size should be: 4, but got: 5"): - model.contact_forces(q_valid, qdot_valid, tau_too_large) - - -def test_passive_joint_torque_valid_and_too_large_q_or_qdot_input(model): - biorbd_model_path = ( - bioptim_folder + "/examples/muscle_driven_with_contact/models/2segments_4dof_2contacts_1muscle.bioMod" - ) - model = BiorbdModel(biorbd_model_path) - q_valid, qdot_valid, q_too_large, qdot_too_large = generate_q_and_qdot_vectors(model) - - # q and qdot valid - model.passive_joint_torque(q_valid, qdot_valid) - # qdot valid but q not valid - with pytest.raises(ValueError, match="Length of q size should be: 4, but got: 5"): - model.passive_joint_torque(q_too_large, qdot_valid) - # q valid but qdot not valid - with pytest.raises(ValueError, match="Length of qdot size should be: 4, but got: 5"): - model.passive_joint_torque(q_valid, qdot_too_large) - - -def test_ligament_joint_torque_valid_and_too_large_q_or_qdot_input(model): - biorbd_model_path = ( - bioptim_folder + "/examples/muscle_driven_with_contact/models/2segments_4dof_2contacts_1muscle.bioMod" - ) - model = BiorbdModel(biorbd_model_path) - q_valid, qdot_valid, q_too_large, qdot_too_large = generate_q_and_qdot_vectors(model) - - # q and qdot valid - model.ligament_joint_torque(q_valid, qdot_valid) - # qdot valid but q not valid - with pytest.raises(ValueError, match="Length of q size should be: 4, but got: 5"): - model.ligament_joint_torque(q_too_large, qdot_valid) - # q valid but qdot not valid - with pytest.raises(ValueError, match="Length of qdot size should be: 4, but got: 5"): - model.ligament_joint_torque(q_valid, qdot_too_large) diff --git a/tests/shard1/test_biorbd_multi_model.py b/tests/shard1/test_biorbd_multi_model.py index 6043ddc94..b0b80a752 100644 --- a/tests/shard1/test_biorbd_multi_model.py +++ b/tests/shard1/test_biorbd_multi_model.py @@ -106,7 +106,7 @@ def test_biorbd_model(): assert isinstance(models.segments[0], biorbd.biorbd.Segment) TestUtils.assert_equal( - models.homogeneous_matrices_in_global(0, 0)(np.array([1, 2, 3])), + models.homogeneous_matrices_in_global(segment_idx=0, inverse=False)(np.array([1, 2, 3]), []), np.array( [ [1.0, 0.0, 0.0, 0.0], @@ -117,17 +117,17 @@ def test_biorbd_model(): ), ) TestUtils.assert_equal( - models.homogeneous_matrices_in_child(4)()["o0"], + models.homogeneous_matrices_in_child(segment_id=4), np.array([[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 1.0, -1.0], [0.0, 0.0, 0.0, 1.0]]), ) - TestUtils.assert_equal(models.mass()["o0"], np.array([3, 3]).reshape(2, 1)) + TestUtils.assert_equal(models.mass([]), np.array([3, 3]).reshape(2, 1)) TestUtils.assert_equal( - models.center_of_mass()(np.array([1, 2, 3, 4, 5, 6])), + models.center_of_mass()(np.array([1, 2, 3, 4, 5, 6]), []), np.array([-5.000000e-04, 8.433844e-01, -1.764446e-01, -5.000000e-04, -3.232674e-01, 1.485815e00]).reshape(6, 1), ) TestUtils.assert_equal( - models.center_of_mass_velocity()(np.array([1, 2.1, 3, 4.1, 5, 6.1]), np.array([1, 2.1, 3, 4.1, 5, 6])), + models.center_of_mass_velocity()(np.array([1, 2.1, 3, 4.1, 5, 6.1]), np.array([1, 2.1, 3, 4.1, 5, 6]), []), np.array([0.0, 0.425434, 0.638069, 0.0, -12.293126, 0.369492]).reshape(6, 1), ) TestUtils.assert_equal( @@ -135,11 +135,12 @@ def test_biorbd_model(): np.array([1, 2.1, 3, 4.1, 5, 6.1]), np.array([1, 2.1, 3, 4.1, 5, 6]), np.array([0.1, 0.2, 0.3, 0.4, 0.5, 0.6]), + [], ), np.array([0.0, 0.481652, 6.105858, 0.0, -33.566971, -126.795179]).reshape(6, 1), ) - mass_matrices = models.mass_matrix()(np.array([1, 2.1, 3, 4.1, 5, 6.1])) + mass_matrices = models.mass_matrix()(np.array([1, 2.1, 3, 4.1, 5, 6.1]), []) assert len(mass_matrices) == 2 TestUtils.assert_equal( mass_matrices[0], @@ -149,52 +150,52 @@ def test_biorbd_model(): [3.783457e-01, 9.999424e-01, -2.881681e-05], [4.243336e-01, -2.881681e-05, 9.543311e-01], ] - ), - ) + )) TestUtils.assert_equal( mass_matrices[1], np.array([[9.313616, 5.580191, 2.063886], [5.580191, 4.791997, 1.895999], [2.063886, 1.895999, 0.945231]]), ) nonlinear_effects = models.non_linear_effects()( - np.array([1, 2.1, 3, 4.1, 5, 6.1]), np.array([1, 2.1, 3, 4.1, 5, 6]) + np.array([1, 2.1, 3, 4.1, 5, 6.1]), np.array([1, 2.1, 3, 4.1, 5, 6]), [] ) assert len(nonlinear_effects) == 2 TestUtils.assert_equal(nonlinear_effects[0], np.array([38.817401, -1.960653, -1.259441]).reshape(3, 1)) TestUtils.assert_equal(nonlinear_effects[1], np.array([322.060726, -22.147881, -20.660836]).reshape(3, 1)) TestUtils.assert_equal( - models.angular_momentum()(np.array([1, 2.1, 3, 4.1, 5, 6.1]), np.array([1, 2.1, 3, 4.1, 5, 6])), + models.angular_momentum()(np.array([1, 2.1, 3, 4.1, 5, 6.1]), np.array([1, 2.1, 3, 4.1, 5, 6]), []), np.array([3.001448e00, 0.000000e00, -2.168404e-19, 2.514126e01, 3.252607e-19, 0.000000e00]).reshape(6, 1), decimal=5, ) TestUtils.assert_equal( - models.reshape_qdot(1)(np.array([1, 2.1, 3, 4.1, 5, 6.1]), np.array([1, 2.1, 3, 4.1, 5, 6])), + models.reshape_qdot(1)(np.array([1, 2.1, 3, 4.1, 5, 6.1]), np.array([1, 2.1, 3, 4.1, 5, 6]), []), np.array([1.0, 2.1, 3.0, 4.1, 5.0, 6.0]).reshape(6, 1), ) TestUtils.assert_equal( - models.segment_angular_velocity(1)(np.array([1, 2.1, 3, 4.1, 5, 6.1]), np.array([1, 2.1, 3, 4.1, 5, 6])), + models.segment_angular_velocity(1)(np.array([1, 2.1, 3, 4.1, 5, 6.1]), np.array([1, 2.1, 3, 4.1, 5, 6]), []), np.array([3.1, 0.0, 0.0, 9.1, 0.0, 0.0]).reshape(6, 1), ) assert models.soft_contact(0, 0) == [] # TODO: Fix soft contact (biorbd call error) - # with pytest.raises(RuntimeError, match="Close the actuator model before calling torqueMax"): - # models.torque()( - # np.array([3.1, 0.0, 0.0, 9.1, 0.0, 0.0]), - # np.array([3.1, 0.0, 0.0, 9.1, 0.0, 0.0]), - # np.array([3.1, 0.0, 0.0, 9.1, 0.0, 0.0]), - # ) # TODO: Fix torque (Close the actuator model before calling torqueMax) + with pytest.raises(RuntimeError, match="All dof must have their actuators set"): + models.torque()( + np.array([3.1, 0.0, 0.0, 9.1, 0.0, 0.0]), + np.array([3.1, 0.0, 0.0, 9.1, 0.0, 0.0]), + np.array([3.1, 0.0, 0.0, 9.1, 0.0, 0.0]), + ) # TODO: Add a model with actuator to properly test TestUtils.assert_equal( models.forward_dynamics_free_floating_base()( np.array([1, 2.1, 3, 4.1, 5, 6.1]), np.array([1, 2.1, 3, 4.1, 5, 6]), - np.array([3.1, 0.0, 9.1, 0.0]), + np.array([3.1, 0.0, 0.0, 9.1]), + [] ), - np.array([-14.750679, -40.031762]).reshape(2, 1), + np.array([-14.750679, -36.596107]).reshape(2, 1), ) TestUtils.assert_equal( @@ -202,6 +203,8 @@ def test_biorbd_model(): np.array([1, 2.1, 3, 4.1, 5, 6.1]), np.array([1, 2.1, 3, 4.1, 5, 6]), np.array([3.1, 1, 2, 9.1, 1, 2]), + [], + [] ), np.array([-16.092093, 9.049853, 10.570878, -121.783105, 154.820759, -20.664452]).reshape(6, 1), ) @@ -211,24 +214,19 @@ def test_biorbd_model(): np.array([1, 2.1, 3, 4.1, 5, 6.1]), np.array([1, 2.1, 3, 4.1, 5, 6]), np.array([3.1, 1, 2, 9.1, 1, 2]), + [], + [] ), np.array([-16.092093, 9.049853, 10.570878, -121.783105, 154.820759, -20.664452]).reshape(6, 1), ) - with pytest.raises(RuntimeError, match="Incorrect number of inputs: Expected 3, got 4"): - # Because external_forces are not implemented - models.forward_dynamics(with_contact=True)( - np.array([1, 2.1, 3, 4.1, 5, 6.1]), - np.array([1, 2.1, 3, 4.1, 5, 6]), - np.array([3.1, 1, 2, 9.1, 1, 2]), - np.array([3.1, 1, 2, 9.1, 1, 2]), - ) - TestUtils.assert_equal( models.inverse_dynamics()( np.array([1, 2.1, 3, 4.1, 5, 6.1]), np.array([1, 2.1, 3, 4.1, 5, 6]), np.array([3.1, 1, 2, 9.1, 1, 2]), + [], + [], ), np.array([4.844876e01, 2.121037e-01, 1.964626e00, 4.165226e02, 3.721585e01, 1.906986e00]).reshape(6, 1), decimal=5, @@ -239,23 +237,17 @@ def test_biorbd_model(): np.array([1, 2.1, 3, 4.1, 5, 6.1]), np.array([1, 2.1, 3, 4.1, 5, 6]), np.array([3.1, 1, 2, 9.1, 1, 2]), + [], + [], ), np.array([0.0, 0.0]).reshape(2, 1), ) - with pytest.raises(RuntimeError, match="Incorrect number of inputs: Expected 3, got 4"): - # Because external_forces are not implemented - models.contact_forces_from_constrained_forward_dynamics()( - np.array([1, 2.1, 3, 4.1, 5, 6.1]), - np.array([1, 2.1, 3, 4.1, 5, 6]), - np.array([3.1, 1, 2, 9.1, 1, 2]), - np.array([3.1, 1, 2, 9.1, 1, 2]), - ) - TestUtils.assert_equal( models.qdot_from_impact()( np.array([1, 2.1, 3, 4.1, 5, 6.1]), np.array([1, 2.1, 3, 4.1, 5, 6]), + [], ), np.array([1.0, 2.1, 3.0, 4.1, 5.0, 6.0]).reshape(6, 1), ) @@ -263,6 +255,7 @@ def test_biorbd_model(): TestUtils.assert_equal( models.muscle_activation_dot()( [], # There is no muscle in the models + [], ), np.array([], dtype=np.float64).reshape(0, 1), ) @@ -272,6 +265,7 @@ def test_biorbd_model(): np.array([]), # There is no muscle in the models np.array([1, 2.1, 3, 4.1, 5, 6.1]), np.array([1, 2.1, 3, 4.1, 5, 6]), + [], ), np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]).reshape(6, 1), ) @@ -279,6 +273,7 @@ def test_biorbd_model(): TestUtils.assert_equal( models.markers()( np.array([1, 2.1, 3, 4.1, 5, 6.1]), + [], )[:, 0], np.array([0.0, 0.0, 0.0]).reshape(3, 1), ) @@ -286,6 +281,7 @@ def test_biorbd_model(): TestUtils.assert_equal( models.marker(index=1)( np.array([1, 2.1, 3, 4.1, 5, 6.1]), + [], ), np.array([0.0, 0.841471, -0.540302]).reshape(3, 1), ) @@ -295,6 +291,7 @@ def test_biorbd_model(): markers_velocities = models.markers_velocities()( np.array([1, 2.1, 3, 4.1, 5, 6.1]), np.array([1, 2.1, 3, 4.1, 5, 6]), + [], ) TestUtils.assert_equal( @@ -310,23 +307,25 @@ def test_biorbd_model(): models.tau_max()( np.array([1, 2.1, 3, 4.1, 5, 6.1]), np.array([1, 2.1, 3, 4.1, 5, 6]), - ) # TODO: add an actuator model (AnaisFarr will do it when her PR will be merged) + [], + ) # TODO: add an actuator model # TODO: add a model with a contact to test this function # rigid_contact_acceleration = models.rigid_contact_acceleration(q, qdot, qddot, 0, 0) TestUtils.assert_equal( - models.soft_contact_forces( + models.soft_contact_forces()( np.array([1, 2.1, 3, 4.1, 5, 6.1]), np.array([1, 2.1, 3, 4.1, 5, 6]), + [], ), - np.array([], dtype=np.float64), + np.array([], dtype=np.float64).reshape(0, 1), ) with pytest.raises( NotImplementedError, match="reshape_fext_to_fcontact is not implemented yet for MultiBiorbdModel" ): - models.reshape_fext_to_fcontact()(np.array([1, 2.1, 3, 4.1, 5, 6.1])) + models.reshape_fext_to_fcontact()(np.array([1, 2.1, 3, 4.1, 5, 6.1]), []) # this test doesn't properly test the function, but it's the best we can do for now # we should add a quaternion to the model to test it @@ -343,23 +342,17 @@ def test_biorbd_model(): np.array([1, 2.1, 3, 4.1, 5, 6.1]), np.array([1, 2.1, 3, 4.1, 5, 6]), np.array([3.1, 1, 2, 9.1, 1, 2]), + [], + [], ), np.array([0.0, 0.0]).reshape(2, 1), ) - # Because external_forces are not implemented yet - with pytest.raises(RuntimeError, match="Incorrect number of inputs: Expected 3, got 4"): - models.contact_forces()( - np.array([1, 2.1, 3, 4.1, 5, 6.1]), - np.array([1, 2.1, 3, 4.1, 5, 6]), - np.array([3.1, 1, 2, 9.1, 1, 2]), - np.array([3.1, 1, 2, 9.1, 1, 2]), - ) - TestUtils.assert_equal( models.passive_joint_torque()( np.array([1, 2.1, 3, 4.1, 5, 6.1]), np.array([1, 2.1, 3, 4.1, 5, 6]), + [], ), np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]).reshape(6, 1), ) diff --git a/tests/shard1/test_dynamics.py b/tests/shard1/test_dynamics.py index f0460bd1c..76644127a 100644 --- a/tests/shard1/test_dynamics.py +++ b/tests/shard1/test_dynamics.py @@ -48,7 +48,7 @@ def __init__(self, nlp, use_sx): ) def test_torque_driven(with_contact, with_external_force, cx, rigidbody_dynamics, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(True if cx == SX else False)) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx==SX) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod", segments_to_apply_external_forces=["Seg0"], @@ -125,7 +125,7 @@ def test_torque_driven(with_contact, with_external_force, cx, rigidbody_dynamics 0, ] - ocp = OptimalControlProgram(nlp, use_sx=(True if cx == SX else False)) + ocp = OptimalControlProgram(nlp, use_sx=(cx==SX) nlp.control_type = ControlType.CONSTANT NonLinearProgram.add( ocp, @@ -270,7 +270,7 @@ def test_torque_driven(with_contact, with_external_force, cx, rigidbody_dynamics @pytest.mark.parametrize("with_contact", [False, True]) def test_torque_driven_implicit(with_contact, cx, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(True if cx == SX else False)) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx==SX) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod" ) @@ -287,7 +287,7 @@ def test_torque_driven_implicit(with_contact, cx, phase_dynamics): nlp.u_scaling = VariableScalingList() nlp.a_scaling = VariableScalingList() - ocp = OptimalControlProgram(nlp, use_sx=(True if cx == SX else False)) + ocp = OptimalControlProgram(nlp, use_sx=(cx==SX) nlp.control_type = ControlType.CONSTANT NonLinearProgram.add( @@ -348,7 +348,7 @@ def test_torque_driven_implicit(with_contact, cx, phase_dynamics): @pytest.mark.parametrize("implicit_contact", [False, True]) def test_torque_driven_soft_contacts_dynamics(with_contact, cx, implicit_contact, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(True if cx == SX else False)) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx==SX) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod" ) @@ -365,7 +365,7 @@ def test_torque_driven_soft_contacts_dynamics(with_contact, cx, implicit_contact nlp.u_scaling = VariableScalingList() nlp.a_scaling = VariableScalingList() - ocp = OptimalControlProgram(nlp, use_sx=(True if cx == SX else False)) + ocp = OptimalControlProgram(nlp, use_sx=(cx==SX) nlp.control_type = ControlType.CONSTANT NonLinearProgram.add( @@ -427,7 +427,7 @@ def test_torque_driven_soft_contacts_dynamics(with_contact, cx, implicit_contact @pytest.mark.parametrize("with_contact", [False, True]) def test_torque_derivative_driven(with_contact, with_external_force, cx, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(True if cx == SX else False)) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx==SX) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod", segments_to_apply_external_forces=["Seg0"], @@ -444,7 +444,7 @@ def test_torque_derivative_driven(with_contact, with_external_force, cx, phase_d nlp.u_scaling = VariableScalingList() nlp.a_scaling = VariableScalingList() - ocp = OptimalControlProgram(nlp, use_sx=(True if cx == SX else False)) + ocp = OptimalControlProgram(nlp, use_sx=(cx==SX) nlp.control_type = ControlType.CONSTANT external_forces = None @@ -635,7 +635,7 @@ def test_torque_derivative_driven(with_contact, with_external_force, cx, phase_d @pytest.mark.parametrize("with_contact", [False, True]) def test_torque_derivative_driven_implicit(with_contact, cx, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(True if cx == SX else False)) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx==SX) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod" ) @@ -652,7 +652,7 @@ def test_torque_derivative_driven_implicit(with_contact, cx, phase_dynamics): nlp.u_scaling = VariableScalingList() nlp.a_scaling = VariableScalingList() - ocp = OptimalControlProgram(nlp, use_sx=(True if cx == SX else False)) + ocp = OptimalControlProgram(nlp, use_sx=(cx==SX) nlp.control_type = ControlType.CONSTANT NonLinearProgram.add( ocp, @@ -745,7 +745,7 @@ def test_torque_derivative_driven_implicit(with_contact, cx, phase_dynamics): @pytest.mark.parametrize("implicit_contact", [False, True]) def test_torque_derivative_driven_soft_contacts_dynamics(with_contact, cx, implicit_contact, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(True if cx == SX else False)) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx==SX) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod" ) @@ -762,7 +762,7 @@ def test_torque_derivative_driven_soft_contacts_dynamics(with_contact, cx, impli nlp.u_scaling = VariableScalingList() nlp.a_scaling = VariableScalingList() - ocp = OptimalControlProgram(nlp, use_sx=(True if cx == SX else False)) + ocp = OptimalControlProgram(nlp, use_sx=(cx==SX) nlp.control_type = ControlType.CONSTANT NonLinearProgram.add( ocp, @@ -935,7 +935,7 @@ def test_implicit_dynamics_errors(dynamics, phase_dynamics): @pytest.mark.parametrize("with_contact", [False, True]) def test_torque_activation_driven(with_contact, with_external_force, cx, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(True if cx == SX else False)) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx==SX) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod", segments_to_apply_external_forces=["Seg0"], @@ -1012,7 +1012,7 @@ def test_torque_activation_driven(with_contact, with_external_force, cx, phase_d 0, ] - ocp = OptimalControlProgram(nlp, use_sx=(True if cx == SX else False)) + ocp = OptimalControlProgram(nlp, use_sx=(cx==SX) nlp.control_type = ControlType.CONSTANT NonLinearProgram.add( ocp, @@ -1116,7 +1116,7 @@ def test_torque_activation_driven_with_residual_torque( with_residual_torque, with_external_force, with_passive_torque, cx, phase_dynamics ): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(True if cx == SX else False)) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx==SX) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/torque_driven_ocp/models/2segments_2dof_2contacts.bioMod", segments_to_apply_external_forces=["Seg0"], @@ -1192,7 +1192,7 @@ def test_torque_activation_driven_with_residual_torque( 0, ] - ocp = OptimalControlProgram(nlp, use_sx=(True if cx == SX else False)) + ocp = OptimalControlProgram(nlp, use_sx=(cx==SX) nlp.control_type = ControlType.CONSTANT NonLinearProgram.add( ocp, @@ -1296,7 +1296,7 @@ def test_torque_activation_driven_with_residual_torque( @pytest.mark.parametrize("cx", [MX, SX]) def test_torque_driven_free_floating_base(cx, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(True if cx == SX else False)) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx==SX) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod" ) @@ -1313,7 +1313,7 @@ def test_torque_driven_free_floating_base(cx, phase_dynamics): nlp.u_scaling = VariableScalingList() nlp.a_scaling = VariableScalingList() - ocp = OptimalControlProgram(nlp, use_sx=(True if cx == SX else False)) + ocp = OptimalControlProgram(nlp, use_sx=(cx==SX) nlp.control_type = ControlType.CONSTANT NonLinearProgram.add( ocp, @@ -1362,7 +1362,7 @@ def test_muscle_driven( with_excitations, with_contact, with_residual_torque, with_external_force, rigidbody_dynamics, cx, phase_dynamics ): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(True if cx == SX else False)) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx==SX) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/muscle_driven_ocp/models/arm26_with_contact.bioMod", segments_to_apply_external_forces=["r_ulna_radius_hand_rotation1"], @@ -1440,7 +1440,7 @@ def test_muscle_driven( 0, ] - ocp = OptimalControlProgram(nlp, use_sx=(True if cx == SX else False)) + ocp = OptimalControlProgram(nlp, use_sx=(cx==SX) nlp.control_type = ControlType.CONSTANT NonLinearProgram.add( ocp, @@ -1932,7 +1932,7 @@ def test_muscle_driven( @pytest.mark.parametrize("rigid_body_dynamics", RigidBodyDynamics) def test_joints_acceleration_driven(cx, rigid_body_dynamics, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(True if cx == SX else False)) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx==SX) nlp.model = BiorbdModel(TestUtils.bioptim_folder() + "/examples/getting_started/models/double_pendulum.bioMod") nlp.ns = 5 @@ -1948,7 +1948,7 @@ def test_joints_acceleration_driven(cx, rigid_body_dynamics, phase_dynamics): nlp.u_scaling = VariableScalingList() nlp.a_scaling = VariableScalingList() - ocp = OptimalControlProgram(nlp, use_sx=(True if cx == SX else False)) + ocp = OptimalControlProgram(nlp, use_sx=(cx==SX) nlp.control_type = ControlType.CONSTANT NonLinearProgram.add( diff --git a/tests/shard3/test_graph.py b/tests/shard3/test_graph.py index cbc4049f5..cde4014f4 100644 --- a/tests/shard3/test_graph.py +++ b/tests/shard3/test_graph.py @@ -43,7 +43,7 @@ def custom_func_track_markers(controller: PenaltyController, first_marker: str, # Get the index of the markers from their name marker_0_idx = controller.model.marker_index(first_marker) marker_1_idx = controller.model.marker_index(second_marker) - markers = controller.model.markers()(controller.q, controller.parameters_except_time.cx) + markers = controller.model.markers()(controller.q, controller.parameters.cx) return markers[:, marker_1_idx] - markers[:, marker_0_idx] diff --git a/tests/shard3/test_ligaments.py b/tests/shard3/test_ligaments.py index b0c0f323b..00b278f9c 100644 --- a/tests/shard3/test_ligaments.py +++ b/tests/shard3/test_ligaments.py @@ -41,7 +41,7 @@ def __init__(self, nlp, use_sx): @pytest.mark.parametrize("with_ligament", [False, True]) def test_torque_driven_with_ligament(with_ligament, cx, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(True if cx == SX else False)) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx==SX) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/torque_driven_ocp/models/mass_point_with_ligament.bioMod" ) @@ -57,7 +57,7 @@ def test_torque_driven_with_ligament(with_ligament, cx, phase_dynamics): nlp.x_bounds = np.zeros((nlp.model.nb_q * 3, 1)) nlp.u_bounds = np.zeros((nlp.model.nb_q, 1)) - ocp = OptimalControlProgram(nlp, use_sx=(True if cx == SX else False)) + ocp = OptimalControlProgram(nlp, use_sx=(cx==SX) nlp.control_type = ControlType.CONSTANT NonLinearProgram.add( ocp, @@ -105,7 +105,7 @@ def test_torque_driven_with_ligament(with_ligament, cx, phase_dynamics): @pytest.mark.parametrize("with_ligament", [False, True]) def test_torque_derivative_driven_with_ligament(with_ligament, cx, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(True if cx == SX else False)) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx==SX) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/torque_driven_ocp/models/mass_point_with_ligament.bioMod" ) @@ -121,7 +121,7 @@ def test_torque_derivative_driven_with_ligament(with_ligament, cx, phase_dynamic nlp.x_bounds = np.zeros((nlp.model.nb_q * 3, 1)) nlp.u_bounds = np.zeros((nlp.model.nb_q, 1)) - ocp = OptimalControlProgram(nlp, use_sx=(True if cx == SX else False)) + ocp = OptimalControlProgram(nlp, use_sx=(cx==SX) nlp.control_type = ControlType.CONSTANT NonLinearProgram.add( @@ -171,7 +171,7 @@ def test_torque_derivative_driven_with_ligament(with_ligament, cx, phase_dynamic @pytest.mark.parametrize("with_ligament", [False, True]) def test_torque_activation_driven_with_ligament(with_ligament, cx, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(True if cx == SX else False)) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx==SX) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/torque_driven_ocp/models/mass_point_with_ligament.bioMod" ) @@ -186,7 +186,7 @@ def test_torque_activation_driven_with_ligament(with_ligament, cx, phase_dynamic nlp.a_scaling = VariableScalingList() nlp.x_bounds = np.zeros((nlp.model.nb_q * 2, 1)) nlp.u_bounds = np.zeros((nlp.model.nb_q, 1)) - ocp = OptimalControlProgram(nlp, use_sx=(True if cx == SX else False)) + ocp = OptimalControlProgram(nlp, use_sx=(cx==SX) nlp.control_type = ControlType.CONSTANT NonLinearProgram.add( ocp, @@ -235,7 +235,7 @@ def test_torque_activation_driven_with_ligament(with_ligament, cx, phase_dynamic @pytest.mark.parametrize("with_ligament", [False, True]) def test_muscle_driven_with_ligament(with_ligament, cx, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(True if cx == SX else False)) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx==SX) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/muscle_driven_ocp/models/arm26_with_ligament.bioMod" ) @@ -251,7 +251,7 @@ def test_muscle_driven_with_ligament(with_ligament, cx, phase_dynamics): nlp.x_bounds = np.zeros((nlp.model.nb_q * 2 + nlp.model.nb_muscles, 1)) nlp.u_bounds = np.zeros((nlp.model.nb_muscles, 1)) - ocp = OptimalControlProgram(nlp, use_sx=(True if cx == SX else False)) + ocp = OptimalControlProgram(nlp, use_sx=(cx==SX) nlp.control_type = ControlType.CONSTANT NonLinearProgram.add( ocp, diff --git a/tests/shard3/test_passive_torque.py b/tests/shard3/test_passive_torque.py index 1aa782ee4..8c7a1fe43 100644 --- a/tests/shard3/test_passive_torque.py +++ b/tests/shard3/test_passive_torque.py @@ -42,7 +42,7 @@ def __init__(self, nlp, use_sx): @pytest.mark.parametrize("rigidbody_dynamics", [RigidBodyDynamics.ODE]) def test_torque_driven_with_passive_torque(with_passive_torque, cx, rigidbody_dynamics, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(True if cx == SX else False)) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx==SX) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod" ) @@ -58,7 +58,7 @@ def test_torque_driven_with_passive_torque(with_passive_torque, cx, rigidbody_dy nlp.x_bounds = np.zeros((nlp.model.nb_q * 3, 1)) nlp.u_bounds = np.zeros((nlp.model.nb_q, 1)) - ocp = OptimalControlProgram(nlp, use_sx=(True if cx == SX else False)) + ocp = OptimalControlProgram(nlp, use_sx=(cx==SX) nlp.control_type = ControlType.CONSTANT NonLinearProgram.add( ocp, @@ -129,7 +129,7 @@ def test_torque_driven_with_passive_torque(with_passive_torque, cx, rigidbody_dy @pytest.mark.parametrize("with_passive_torque", [False, True]) def test_torque_derivative_driven_with_passive_torque(with_passive_torque, cx, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(True if cx == SX else False)) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx==SX) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod" ) @@ -145,7 +145,7 @@ def test_torque_derivative_driven_with_passive_torque(with_passive_torque, cx, p nlp.x_bounds = np.zeros((nlp.model.nb_q * 3, 1)) nlp.u_bounds = np.zeros((nlp.model.nb_q, 1)) - ocp = OptimalControlProgram(nlp, use_sx=(True if cx == SX else False)) + ocp = OptimalControlProgram(nlp, use_sx=(cx==SX) nlp.control_type = ControlType.CONSTANT NonLinearProgram.add( @@ -226,7 +226,7 @@ def test_torque_derivative_driven_with_passive_torque(with_passive_torque, cx, p @pytest.mark.parametrize("with_residual_torque", [False, True]) def test_torque_activation_driven_with_passive_torque(with_passive_torque, with_residual_torque, cx, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(True if cx == SX else False)) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx==SX) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod" ) @@ -240,7 +240,7 @@ def test_torque_activation_driven_with_passive_torque(with_passive_torque, with_ nlp.u_scaling = VariableScalingList() nlp.x_bounds = np.zeros((nlp.model.nb_q * 2, 1)) nlp.u_bounds = np.zeros((nlp.model.nb_q, 1)) - ocp = OptimalControlProgram(nlp, use_sx=(True if cx == SX else False)) + ocp = OptimalControlProgram(nlp, use_sx=(cx==SX) nlp.control_type = ControlType.CONSTANT NonLinearProgram.add( ocp, @@ -347,7 +347,7 @@ def test_torque_activation_driven_with_passive_torque(with_passive_torque, with_ @pytest.mark.parametrize("rigidbody_dynamics", [RigidBodyDynamics.ODE]) def test_muscle_driven_with_passive_torque(with_passive_torque, rigidbody_dynamics, cx, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(True if cx == SX else False)) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx==SX) nlp.model = BiorbdModel(TestUtils.bioptim_folder() + "/examples/muscle_driven_ocp/models/arm26_with_contact.bioMod") nlp.ns = 5 nlp.cx = cx @@ -361,7 +361,7 @@ def test_muscle_driven_with_passive_torque(with_passive_torque, rigidbody_dynami nlp.x_bounds = np.zeros((nlp.model.nb_q * 2 + nlp.model.nb_muscles, 1)) nlp.u_bounds = np.zeros((nlp.model.nb_muscles, 1)) - ocp = OptimalControlProgram(nlp, use_sx=(True if cx == SX else False)) + ocp = OptimalControlProgram(nlp, use_sx=(cx==SX) nlp.control_type = ControlType.CONSTANT NonLinearProgram.add( ocp, diff --git a/tests/shard4/test_penalty.py b/tests/shard4/test_penalty.py index 8779a1e02..59b448bc4 100644 --- a/tests/shard4/test_penalty.py +++ b/tests/shard4/test_penalty.py @@ -1,5 +1,5 @@ import pytest -from casadi import DM, MX, vertcat, horzcat +from casadi import DM, MX, vertcat, horzcat, Function import numpy as np import numpy.testing as npt from bioptim import ( @@ -112,9 +112,7 @@ def get_penalty_value(ocp, penalty, t, phases_dt, x, u, p, a, d): ocp.nlp[0].numerical_timeseries.cx if ocp.nlp[0].numerical_timeseries.cx.shape != (0, 0) else ocp.cx(0, 0) ) - return ocp.nlp[0].to_casadi_func( - "penalty", val, time, phases_dt_cx, states, controls, parameters, algebraic_states, numerical_timeseries - )(t, phases_dt, x[0], u[0], p, a, d) + return Function("penalty", [time, phases_dt_cx, states, controls, parameters, algebraic_states, numerical_timeseries], [val])(t, phases_dt, x[0], u[0], p, a, d) def test_penalty_targets_shapes(): From 80cdbee8d48e7459661aa1bf913c182c92a6cf3c Mon Sep 17 00:00:00 2001 From: eve-mac Date: Mon, 23 Sep 2024 18:32:31 -0400 Subject: [PATCH 031/108] blacked --- bioptim/dynamics/configure_problem.py | 2 +- bioptim/dynamics/dynamics_functions.py | 52 ++++--------------- .../custom_package/custom_dynamics.py | 3 +- .../muscle_activations_tracker.py | 28 +++++----- .../muscle_excitations_tracker.py | 29 +++++------ bioptim/limits/constraints.py | 22 +++----- bioptim/limits/multinode_penalty.py | 4 +- bioptim/limits/penalty.py | 12 ++--- tests/shard1/test_biorbd_multi_model.py | 12 ++--- tests/shard4/test_penalty.py | 4 +- 10 files changed, 60 insertions(+), 108 deletions(-) diff --git a/bioptim/dynamics/configure_problem.py b/bioptim/dynamics/configure_problem.py index 5ee7d11fe..66f3beb99 100644 --- a/bioptim/dynamics/configure_problem.py +++ b/bioptim/dynamics/configure_problem.py @@ -1500,7 +1500,7 @@ def configure_integrated_value( cx=cx_scaled_next_formatted, cx_scaled=[initial_matrix for i in range(n_cx)], # Only the first value is used mapping=dummy_mapping, - node_index=0 + node_index=0, ) for node_index in range(1, nlp.ns + 1): # cannot use phase_dynamics == PhaseDynamics.SHARED_DURING_THE_PHASE cx_scaled_next = nlp.integrated_value_functions[name](nlp, node_index) diff --git a/bioptim/dynamics/dynamics_functions.py b/bioptim/dynamics/dynamics_functions.py index 317ba78b9..eb560f97c 100644 --- a/bioptim/dynamics/dynamics_functions.py +++ b/bioptim/dynamics/dynamics_functions.py @@ -145,11 +145,7 @@ 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, nlp.parameters.cx) - if with_passive_torque - 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 @@ -602,11 +598,7 @@ 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, nlp.parameters.cx) - if with_passive_torque - 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) @@ -681,11 +673,7 @@ 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, nlp.parameters.cx) - if with_passive_torque - 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 @@ -739,11 +727,7 @@ def forces_from_torque_activation_driven( 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, 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.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 @@ -847,11 +831,7 @@ 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, nlp.parameters.cx) - if with_passive_torque - 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) @@ -952,11 +932,7 @@ 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, nlp.parameters.cx) - if with_passive_torque - 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 @@ -1007,9 +983,7 @@ def joints_acceleration_driven( qdot = nlp.get_var_from_states_or_controls("qdot", states, controls) qddot_joints = nlp.get_var_from_states_or_controls("qddot", states, controls) - qddot_root = nlp.model.forward_dynamics_free_floating_base()( - q, qdot, qddot_joints, nlp.parameters.cx - ) + qddot_root = nlp.model.forward_dynamics_free_floating_base()(q, qdot, qddot_joints, nlp.parameters.cx) qddot_reordered = nlp.model.reorder_qddot_root_joints(qddot_root, qddot_joints) qdot_mapped = nlp.variable_mappings["qdot"].to_first.map(qdot) @@ -1139,9 +1113,7 @@ 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, [], nlp.parameters.cx - ) + 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)( @@ -1182,9 +1154,7 @@ def inverse_dynamics( """ if external_forces is None: - tau = nlp.model.inverse_dynamics(with_contact=with_contact)( - q, qdot, qddot, [], nlp.parameters.cx - ) + 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] @@ -1296,8 +1266,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, nlp.parameters.cx - ) + 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/custom_model/custom_package/custom_dynamics.py b/bioptim/examples/custom_model/custom_package/custom_dynamics.py index 341e90faf..a667d8a83 100644 --- a/bioptim/examples/custom_model/custom_package/custom_dynamics.py +++ b/bioptim/examples/custom_model/custom_package/custom_dynamics.py @@ -40,7 +40,8 @@ def custom_dynamics( """ return DynamicsEvaluation( - dxdt=vertcat(states[1], nlp.model.forward_dynamics(with_contact=False)(states[0], states[1], controls[0], [])), defects=None + dxdt=vertcat(states[1], nlp.model.forward_dynamics(with_contact=False)(states[0], states[1], controls[0], [])), + defects=None, ) diff --git a/bioptim/examples/muscle_driven_ocp/muscle_activations_tracker.py b/bioptim/examples/muscle_driven_ocp/muscle_activations_tracker.py index ee8661334..2b9c5cbbb 100644 --- a/bioptim/examples/muscle_driven_ocp/muscle_activations_tracker.py +++ b/bioptim/examples/muscle_driven_ocp/muscle_activations_tracker.py @@ -161,20 +161,20 @@ def generate_data( dynamics_func = Function( "ForwardDyn", - [symbolic_states, - symbolic_controls, - symbolic_parameters], - [dyn_func( - time=symbolic_time, - states=symbolic_states, - controls=symbolic_controls, - parameters=symbolic_parameters, - algebraic_states=MX(), - numerical_timeseries=MX(), - nlp=nlp, - with_contact=False, - rigidbody_dynamics=RigidBodyDynamics.ODE, - ).dxdt] + [symbolic_states, symbolic_controls, symbolic_parameters], + [ + dyn_func( + time=symbolic_time, + states=symbolic_states, + controls=symbolic_controls, + parameters=symbolic_parameters, + algebraic_states=MX(), + numerical_timeseries=MX(), + nlp=nlp, + with_contact=False, + rigidbody_dynamics=RigidBodyDynamics.ODE, + ).dxdt + ], ) def dyn_interface(t, x, u): diff --git a/bioptim/examples/muscle_driven_ocp/muscle_excitations_tracker.py b/bioptim/examples/muscle_driven_ocp/muscle_excitations_tracker.py index 1a7a0d903..61ad22f51 100644 --- a/bioptim/examples/muscle_driven_ocp/muscle_excitations_tracker.py +++ b/bioptim/examples/muscle_driven_ocp/muscle_excitations_tracker.py @@ -163,21 +163,20 @@ def generate_data( dynamics_func = Function( "ForwardDyn", - [symbolic_time, - symbolic_states, - symbolic_controls, - symbolic_parameters], - [DynamicsFunctions.muscles_driven( - time=symbolic_time, - states=symbolic_states, - controls=symbolic_controls, - parameters=symbolic_parameters, - algebraic_states=MX(), - numerical_timeseries=MX(), - nlp=nlp, - with_contact=False, - rigidbody_dynamics=RigidBodyDynamics.ODE, - ).dxdt], + [symbolic_time, symbolic_states, symbolic_controls, symbolic_parameters], + [ + DynamicsFunctions.muscles_driven( + time=symbolic_time, + states=symbolic_states, + controls=symbolic_controls, + parameters=symbolic_parameters, + algebraic_states=MX(), + numerical_timeseries=MX(), + nlp=nlp, + with_contact=False, + rigidbody_dynamics=RigidBodyDynamics.ODE, + ).dxdt + ], ) def dyn_interface(t, x, u): diff --git a/bioptim/limits/constraints.py b/bioptim/limits/constraints.py index 20261bfdc..fae03ddae 100644 --- a/bioptim/limits/constraints.py +++ b/bioptim/limits/constraints.py @@ -412,10 +412,7 @@ def qddot_equals_forward_dynamics( tau = controller.states["tau"].cx if "tau" in controller.states else controller.tau tau = tau + passive_torque if with_passive_torque else tau tau = ( - tau - + controller.model.ligament_joint_torque()( - controller.q, controller.qdot, controller.parameters.cx - ) + tau + controller.model.ligament_joint_torque()(controller.q, controller.qdot, controller.parameters.cx) if with_ligament else tau ) @@ -463,10 +460,7 @@ def tau_equals_inverse_dynamics( ) tau = tau + passive_torque if with_passive_torque else tau tau = ( - tau - + controller.model.ligament_joint_torque()( - controller.q, controller.qdot, controller.parameters.cx - ) + tau + controller.model.ligament_joint_torque()(controller.q, controller.qdot, controller.parameters.cx) if with_ligament else tau ) @@ -486,7 +480,9 @@ def tau_equals_inverse_dynamics( f_contact_vec = controller.model.reshape_fext_to_fcontact(f_contact) else: f_contact_vec = [] - tau_id = controller.model.inverse_dynamics(with_contact=with_contact)(controller.q, controller.qdot, qddot, f_contact_vec, controller.parameters.cx) + tau_id = controller.model.inverse_dynamics(with_contact=with_contact)( + controller.q, controller.qdot, qddot, f_contact_vec, controller.parameters.cx + ) var = [] var.extend([controller.states[key] for key in controller.states]) @@ -558,9 +554,7 @@ def tau_from_muscle_equal_inverse_dynamics( muscle_tau = muscle_tau + passive_torque if with_passive_torque else muscle_tau muscle_tau = ( muscle_tau - + controller.model.ligament_joint_torque( - controller.q, controller.qdot, controller.parameters.cx - ) + + controller.model.ligament_joint_torque(controller.q, controller.qdot, controller.parameters.cx) if with_ligament else muscle_tau ) @@ -573,9 +567,7 @@ def tau_from_muscle_equal_inverse_dynamics( # Todo: add fext tau_id = nlp.model.inverse_dynamics()(q, qdot, qddot, fext) # fext need to be a mx - tau_id = controller.model.inverse_dynamics()( - controller.q, controller.qdot, qddot, controller.parameters.cx - ) + tau_id = controller.model.inverse_dynamics()(controller.q, controller.qdot, qddot, controller.parameters.cx) return tau_id - muscle_tau diff --git a/bioptim/limits/multinode_penalty.py b/bioptim/limits/multinode_penalty.py index 88edd0584..1300c5f37 100644 --- a/bioptim/limits/multinode_penalty.py +++ b/bioptim/limits/multinode_penalty.py @@ -267,9 +267,7 @@ def com_equality(penalty, controllers: list[PenaltyController]): MultinodePenaltyFunctions.Functions._prepare_controller_cx(penalty, controllers) - com_0 = controllers[0].model.center_of_mass()( - controllers[0].states["q"].cx, controllers[0].parameters.cx - ) + com_0 = controllers[0].model.center_of_mass()(controllers[0].states["q"].cx, controllers[0].parameters.cx) out = controllers[0].cx.zeros((3, 1)) for i in range(1, len(controllers)): diff --git a/bioptim/limits/penalty.py b/bioptim/limits/penalty.py index df4a6ec94..20f5fe347 100644 --- a/bioptim/limits/penalty.py +++ b/bioptim/limits/penalty.py @@ -658,9 +658,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, controller.qdot, controller.parameters.cx - ) + return controller.model.center_of_mass_velocity()(controller.q, controller.qdot, controller.parameters.cx) @staticmethod def minimize_com_acceleration(penalty: PenaltyOption, controller: PenaltyController, axes: tuple | list = None): @@ -710,9 +708,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, controller.qdot, controller.parameters.cx - ) + return controller.model.angular_momentum()(controller.q, controller.qdot, controller.parameters.cx) @staticmethod def minimize_linear_momentum(penalty: PenaltyOption, controller: PenaltyController, axes: tuple | list = None): @@ -958,9 +954,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, controller.parameters.cx - ) + marker = controller.model.marker(marker_idx, segment_idx)(controller.q, controller.parameters.cx) # To align an axis, the other must be equal to 0 if not penalty.rows_is_set: diff --git a/tests/shard1/test_biorbd_multi_model.py b/tests/shard1/test_biorbd_multi_model.py index b0b80a752..4061b4c04 100644 --- a/tests/shard1/test_biorbd_multi_model.py +++ b/tests/shard1/test_biorbd_multi_model.py @@ -150,7 +150,8 @@ def test_biorbd_model(): [3.783457e-01, 9.999424e-01, -2.881681e-05], [4.243336e-01, -2.881681e-05, 9.543311e-01], ] - )) + ), + ) TestUtils.assert_equal( mass_matrices[1], np.array([[9.313616, 5.580191, 2.063886], [5.580191, 4.791997, 1.895999], [2.063886, 1.895999, 0.945231]]), @@ -190,10 +191,7 @@ def test_biorbd_model(): TestUtils.assert_equal( models.forward_dynamics_free_floating_base()( - np.array([1, 2.1, 3, 4.1, 5, 6.1]), - np.array([1, 2.1, 3, 4.1, 5, 6]), - np.array([3.1, 0.0, 0.0, 9.1]), - [] + np.array([1, 2.1, 3, 4.1, 5, 6.1]), np.array([1, 2.1, 3, 4.1, 5, 6]), np.array([3.1, 0.0, 0.0, 9.1]), [] ), np.array([-14.750679, -36.596107]).reshape(2, 1), ) @@ -204,7 +202,7 @@ def test_biorbd_model(): np.array([1, 2.1, 3, 4.1, 5, 6]), np.array([3.1, 1, 2, 9.1, 1, 2]), [], - [] + [], ), np.array([-16.092093, 9.049853, 10.570878, -121.783105, 154.820759, -20.664452]).reshape(6, 1), ) @@ -215,7 +213,7 @@ def test_biorbd_model(): np.array([1, 2.1, 3, 4.1, 5, 6]), np.array([3.1, 1, 2, 9.1, 1, 2]), [], - [] + [], ), np.array([-16.092093, 9.049853, 10.570878, -121.783105, 154.820759, -20.664452]).reshape(6, 1), ) diff --git a/tests/shard4/test_penalty.py b/tests/shard4/test_penalty.py index 59b448bc4..9dcedee31 100644 --- a/tests/shard4/test_penalty.py +++ b/tests/shard4/test_penalty.py @@ -112,7 +112,9 @@ def get_penalty_value(ocp, penalty, t, phases_dt, x, u, p, a, d): ocp.nlp[0].numerical_timeseries.cx if ocp.nlp[0].numerical_timeseries.cx.shape != (0, 0) else ocp.cx(0, 0) ) - return Function("penalty", [time, phases_dt_cx, states, controls, parameters, algebraic_states, numerical_timeseries], [val])(t, phases_dt, x[0], u[0], p, a, d) + return Function( + "penalty", [time, phases_dt_cx, states, controls, parameters, algebraic_states, numerical_timeseries], [val] + )(t, phases_dt, x[0], u[0], p, a, d) def test_penalty_targets_shapes(): From 9d40d5c9f6f04a2e47d2a87257764a8a905f1ca4 Mon Sep 17 00:00:00 2001 From: Pariterre Date: Wed, 25 Sep 2024 13:23:54 -0400 Subject: [PATCH 032/108] Added a TODO --- bioptim/examples/getting_started/custom_parameters.py | 8 ++++++++ .../muscle_driven_ocp/muscle_excitations_tracker.py | 1 - 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/bioptim/examples/getting_started/custom_parameters.py b/bioptim/examples/getting_started/custom_parameters.py index aab3038c0..20d9039e4 100644 --- a/bioptim/examples/getting_started/custom_parameters.py +++ b/bioptim/examples/getting_started/custom_parameters.py @@ -165,6 +165,14 @@ def prepare_ocp( scaling=g_scaling, # The scaling of the parameter extra_value=1, # You can define as many extra arguments as you want ) + # TODO: Add the capability to send values to the parameter so instead of being a MX it is a float + # This would look more or less like the following (with the values the dispatch function similar to the one in Penalty). + # This would simultanously solve the dispatching of all forces and the phase parameters + # parameters.add( + # "gravity_xyz", # The name of the parameter + # my_parameter_function, # The function that modifies the biorbd model + # values=lambda phase_idx, node_idx=... + # ) # Give the parameter some min and max bounds parameter_bounds.add("gravity_xyz", min_bound=min_g, max_bound=max_g, interpolation=InterpolationType.CONSTANT) diff --git a/bioptim/examples/muscle_driven_ocp/muscle_excitations_tracker.py b/bioptim/examples/muscle_driven_ocp/muscle_excitations_tracker.py index 61ad22f51..089c8f276 100644 --- a/bioptim/examples/muscle_driven_ocp/muscle_excitations_tracker.py +++ b/bioptim/examples/muscle_driven_ocp/muscle_excitations_tracker.py @@ -10,7 +10,6 @@ import platform -import biorbd_casadi as biorbd import numpy as np from casadi import MX, SX, vertcat, horzcat, Function from matplotlib import pyplot as plt From e05d6d5515cc28043f61b196cfb89981d1af405a Mon Sep 17 00:00:00 2001 From: eve-mac Date: Thu, 26 Sep 2024 18:47:02 -0400 Subject: [PATCH 033/108] translational_forces and muscles --- bioptim/dynamics/configure_problem.py | 7 ++ bioptim/dynamics/dynamics_functions.py | 89 ++++++++++++++------- bioptim/limits/constraints.py | 5 +- bioptim/models/biorbd/biorbd_model.py | 65 ++++++++++----- bioptim/models/biorbd/multi_biorbd_model.py | 10 ++- bioptim/models/protocols/biomodel.py | 2 +- tests/shard1/test_biorbd_multi_model.py | 2 + tests/shard1/test_dynamics.py | 44 +++++----- 8 files changed, 148 insertions(+), 76 deletions(-) diff --git a/bioptim/dynamics/configure_problem.py b/bioptim/dynamics/configure_problem.py index 66f3beb99..fadaf8593 100644 --- a/bioptim/dynamics/configure_problem.py +++ b/bioptim/dynamics/configure_problem.py @@ -197,6 +197,7 @@ def torque_driven( rigidbody_dynamics, soft_contacts_dynamics, nlp.model.nb_soft_contacts, nlp.phase_idx ) external_forces = None + translational_forces = None if numerical_data_timeseries is not None: for key in numerical_data_timeseries.keys(): if key == "external_forces": @@ -204,6 +205,11 @@ def torque_driven( external_forces = nlp.numerical_timeseries[0].cx for i in range(1, numerical_data_timeseries[key].shape[1]): external_forces = horzcat(external_forces, nlp.numerical_timeseries[i].cx) + elif key == "translational_forces": + _check_numerical_timeseries_format(numerical_data_timeseries[key], nlp.ns, nlp.phase_idx) + translational_forces = nlp.numerical_timeseries[0].cx + for i in range(1, numerical_data_timeseries[key].shape[1]): + translational_forces = horzcat(translational_forces, nlp.numerical_timeseries[i].cx) # Declared rigidbody states and controls ConfigureProblem.configure_q(ocp, nlp, as_states=True, as_controls=False) @@ -291,6 +297,7 @@ def torque_driven( with_ligament=with_ligament, with_friction=with_friction, external_forces=external_forces, + translational_forces=translational_forces, ) # Configure the contact forces diff --git a/bioptim/dynamics/dynamics_functions.py b/bioptim/dynamics/dynamics_functions.py index eb560f97c..af7b24560 100644 --- a/bioptim/dynamics/dynamics_functions.py +++ b/bioptim/dynamics/dynamics_functions.py @@ -34,7 +34,7 @@ class DynamicsFunctions: Easy accessor to derivative of q forward_dynamics(nlp: NonLinearProgram, q: MX | SX, qdot: MX | SX, tau: MX | SX, with_contact: bool): Easy accessor to derivative of qdot - compute_muscle_dot(nlp: NonLinearProgram, muscle_excitations: MX | SX): + compute_muscle_dot(nlp: NonLinearProgram, muscle_excitations: MX | SX, muscle_activations: MX | SX): Easy accessor to derivative of muscle activations compute_tau_from_muscle(nlp: NonLinearProgram, q: MX | SX, qdot: MX | SX, muscle_activations: MX | SX): Easy accessor to tau computed from muscles @@ -98,6 +98,7 @@ def torque_driven( rigidbody_dynamics: RigidBodyDynamics, fatigue: FatigueList, external_forces: np.ndarray = None, + translational_forces: np.ndarray = None, ) -> DynamicsEvaluation: """ Forward dynamics driven by joint torques, optional external forces can be declared. @@ -132,6 +133,8 @@ def torque_driven( A list of fatigue elements external_forces: np.ndarray The external forces + translational_forces: np.ndarray + The translational forces Returns ---------- @@ -166,7 +169,7 @@ def torque_driven( dxdt[nlp.states["qdot"].index, :] = qddot dxdt[nlp.states["qddot"].index, :] = DynamicsFunctions.get(nlp.controls["qdddot"], controls) else: - ddq = DynamicsFunctions.forward_dynamics(nlp, q, qdot, tau, with_contact, external_forces) + ddq = DynamicsFunctions.forward_dynamics(nlp, q, qdot, tau, with_contact, external_forces, translational_forces) dxdt = nlp.cx(nlp.states.shape, ddq.shape[1]) dxdt[nlp.states["q"].index, :] = horzcat(*[dq for _ in range(ddq.shape[1])]) dxdt[nlp.states["qdot"].index, :] = ddq @@ -181,7 +184,7 @@ def torque_driven( ): if not with_contact and fatigue is None: qddot = DynamicsFunctions.get(nlp.states_dot["qddot"], nlp.states_dot.scaled.cx) - tau_id = DynamicsFunctions.inverse_dynamics(nlp, q, qdot, qddot, with_contact, external_forces) + tau_id = DynamicsFunctions.inverse_dynamics(nlp, q, qdot, qddot, with_contact, external_forces, translational_forces) defects = nlp.cx(dq.shape[0] + tau_id.shape[0], tau_id.shape[1]) dq_defects = [] @@ -637,6 +640,7 @@ def forces_from_torque_driven( with_passive_torque: bool = False, with_ligament: bool = False, external_forces: np.ndarray = None, + translational_forces: np.ndarray = None, ) -> MX | SX: """ Contact forces of a forward dynamics driven by joint torques with contact constraints. @@ -663,6 +667,8 @@ def forces_from_torque_driven( If the dynamic with ligament should be used external_forces: np.ndarray The external forces + translational_forces: np.ndarray + The translational forces Returns ---------- @@ -677,7 +683,8 @@ def forces_from_torque_driven( 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, nlp.parameters.cx) + translational_forces = [] if translational_forces is None else translational_forces + return nlp.model.contact_forces()(q, qdot, tau, external_forces, translational_forces, nlp.parameters.cx) @staticmethod def forces_from_torque_activation_driven( @@ -691,6 +698,7 @@ def forces_from_torque_activation_driven( with_passive_torque: bool = False, with_ligament: bool = False, external_forces: np.ndarray = None, + translational_forces: np.ndarray = None, ) -> MX | SX: """ Contact forces of a forward dynamics driven by joint torques with contact constraints. @@ -717,6 +725,8 @@ def forces_from_torque_activation_driven( If the dynamic with ligament should be used external_forces: np.ndarray The external forces + translational_forces: np.ndarray + The translational forces Returns ---------- @@ -731,7 +741,8 @@ def forces_from_torque_activation_driven( 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, nlp.parameters.cx) + translational_forces = [] if translational_forces is None else translational_forces + return nlp.model.contact_forces()(q, qdot, tau, external_forces, translational_forces, nlp.parameters.cx) @staticmethod def muscles_driven( @@ -749,6 +760,7 @@ def muscles_driven( with_residual_torque: bool = False, fatigue=None, external_forces: np.ndarray = None, + translational_forces: np.ndarray = None, ) -> DynamicsEvaluation: """ Forward dynamics driven by muscle. @@ -783,6 +795,8 @@ def muscles_driven( If the dynamic should be added with residual torques external_forces: np.ndarray The external forces + translational_forces: np.ndarray + The translational forces Returns ---------- @@ -850,7 +864,7 @@ def muscles_driven( has_excitation = True if "muscles" in nlp.states else False if has_excitation: mus_excitations = DynamicsFunctions.get(nlp.controls["muscles"], controls) - dmus = DynamicsFunctions.compute_muscle_dot(nlp, mus_excitations) + dmus = DynamicsFunctions.compute_muscle_dot(nlp, mus_excitations, mus_activations) dxdt[nlp.states["muscles"].index, :] = horzcat(*[dmus for _ in range(ddq.shape[1])]) if fatigue is not None and "muscles" in fatigue: @@ -863,7 +877,7 @@ def muscles_driven( ): if not with_contact and fatigue is None: qddot = DynamicsFunctions.get(nlp.states_dot["qddot"], nlp.states_dot.cx) - tau_id = DynamicsFunctions.inverse_dynamics(nlp, q, qdot, qddot, with_contact, external_forces) + tau_id = DynamicsFunctions.inverse_dynamics(nlp, q, qdot, qddot, with_contact, external_forces, translational_forces) defects = nlp.cx(dq.shape[0] + tau_id.shape[0], tau_id.shape[1]) dq_defects = [] @@ -893,6 +907,7 @@ def forces_from_muscle_driven( with_passive_torque: bool = False, with_ligament: bool = False, external_forces: list = None, + translational_forces: list = None, ) -> MX | SX: """ Contact forces of a forward dynamics driven by muscles activations and joint torques with contact constraints. @@ -919,6 +934,9 @@ def forces_from_muscle_driven( If the dynamic with ligament should be used external_forces: list[Any] The external forces + translational_forces: list[Any] + The translational forces + Returns ---------- MX.sym | SX.sym @@ -936,7 +954,8 @@ def forces_from_muscle_driven( 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, nlp.parameters.cx) + translational_forces = [] if translational_forces is None else translational_forces + return nlp.model.contact_forces()(q, qdot, tau, external_forces, translational_forces, nlp.parameters.cx) @staticmethod def joints_acceleration_driven( @@ -999,7 +1018,7 @@ def joints_acceleration_driven( qddot_root_defects = DynamicsFunctions.get(nlp.states_dot["qddot_roots"], nlp.states_dot.cx) qddot_defects_reordered = nlp.model.reorder_qddot_root_joints(qddot_root_defects, qddot_joints) - floating_base_constraint = nlp.model.inverse_dynamics(q, qdot, qddot_defects_reordered)[: nlp.model.nb_root] + floating_base_constraint = nlp.model.inverse_dynamics(q, qdot, qddot_defects_reordered, [], [])[: nlp.model.nb_root] defects = nlp.cx(qdot_mapped.shape[0] + qddot_root_mapped.shape[0] + qddot_joints_mapped.shape[0], 1) @@ -1082,6 +1101,7 @@ def forward_dynamics( tau: MX | SX, with_contact: bool, external_forces: list = None, + translational_forces: list = None, ): """ Easy accessor to derivative of qdot @@ -1100,6 +1120,8 @@ def forward_dynamics( If the dynamics with contact should be used external_forces: list[] The external forces + translational_forces: list[] + The translational forces Returns ------- The derivative of qdot @@ -1112,14 +1134,13 @@ def forward_dynamics( else: 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, [], nlp.parameters.cx) - return qdot_var_mapping.map(qddot) - else: - qddot = nlp.model.forward_dynamics(with_contact=with_contact)( - q, qdot, tau, external_forces, nlp.parameters.cx - ) - return qdot_var_mapping.map(qddot) + external_forces = [] if external_forces is None else external_forces + translational_forces = [] if translational_forces is None else translational_forces + qddot = nlp.model.forward_dynamics(with_contact=with_contact)( + q, qdot, tau, external_forces, translational_forces, nlp.parameters.cx, + ) + return qdot_var_mapping.map(qddot) + @staticmethod def inverse_dynamics( @@ -1129,6 +1150,7 @@ def inverse_dynamics( qddot: MX | SX, with_contact: bool, external_forces: MX = None, + translational_forces: MX = None, ): """ Easy accessor to torques from inverse dynamics @@ -1147,14 +1169,15 @@ def inverse_dynamics( If the dynamics with contact should be used external_forces: MX The external forces + translational_forces: MX + The translational forces Returns ------- Torques in tau """ - - if external_forces is None: - tau = nlp.model.inverse_dynamics(with_contact=with_contact)(q, qdot, qddot, [], nlp.parameters.cx) + if external_forces is None and translational_forces is None: + 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] @@ -1163,14 +1186,21 @@ def inverse_dynamics( else: 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], nlp.parameters.cx - ) + + if external_forces is not None: + for i in range(external_forces.shape[1]): + tau[:, i] = nlp.model.inverse_dynamics(with_contact=with_contact)( + q, qdot, qddot, external_forces[:, i], [], nlp.parameters.cx + ) + elif translational_forces is not None: + for i in range(translational_forces.shape[1]): + tau[:, i] = nlp.model.inverse_dynamics(with_contact=with_contact)( + q, qdot, qddot, [], translational_forces[:, i], nlp.parameters.cx + ) return tau # We ignore on purpose the mapping to keep zeros in the defects of the dynamic. @staticmethod - def compute_muscle_dot(nlp, muscle_excitations: MX | SX): + def compute_muscle_dot(nlp, muscle_excitations: MX | SX, muscle_activations: MX | SX): """ Easy accessor to derivative of muscle activations @@ -1180,13 +1210,15 @@ def compute_muscle_dot(nlp, muscle_excitations: MX | SX): The phase of the program muscle_excitations: MX | SX The value of muscle_excitations from "get" + muscle_activations: MX | SX + The value of muscle_activations from "get" Returns ------- The derivative of muscle activations """ - return nlp.model.muscle_activation_dot()(muscle_excitations, nlp.parameters.cx) + return nlp.model.muscle_activation_dot()(muscle_excitations, muscle_activations, nlp.parameters.cx) @staticmethod def compute_tau_from_muscle( @@ -1235,6 +1267,7 @@ def holonomic_torque_driven( numerical_timeseries, nlp, external_forces: list = None, + translational_forces: list = None, ) -> DynamicsEvaluation: """ The custom dynamics function that provides the derivative of the states: dxdt = f(t, x, u, p, a, d) @@ -1257,6 +1290,8 @@ def holonomic_torque_driven( A reference to the phase external_forces: list[Any] The external forces + translational_forces: list[Any] + The translational forces Returns ------- @@ -1266,6 +1301,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, nlp.parameters.cx) + qddot_u = nlp.model.partitioned_forward_dynamics(q_u, qdot_u, tau, external_forces, translational_forces, nlp.parameters.cx) return DynamicsEvaluation(dxdt=vertcat(qdot_u, qddot_u), defects=None) diff --git a/bioptim/limits/constraints.py b/bioptim/limits/constraints.py index fae03ddae..7fcd52f53 100644 --- a/bioptim/limits/constraints.py +++ b/bioptim/limits/constraints.py @@ -468,12 +468,13 @@ def tau_equals_inverse_dynamics( if controller.get_nlp.numerical_timeseries: # TODO: deal with external forces raise NotImplementedError( - "This implicit constraint tau_equals_inverse_dynamics is not implemented yet with external forces" + "This implicit constraint tau_equals_inverse_dynamics is not implemented yet with numerical_timeseries (external_forces or translational_forces)" ) # Todo: add fext tau_id = nlp.model.inverse_dynamics()(q, qdot, qddot, fext) if with_contact: # todo: this should be done internally in BiorbdModel + # Charbie: todo f_contact = ( controller.controls["fext"].cx if "fext" in controller.controls else controller.states["fext"].cx ) @@ -567,7 +568,7 @@ def tau_from_muscle_equal_inverse_dynamics( # Todo: add fext tau_id = nlp.model.inverse_dynamics()(q, qdot, qddot, fext) # fext need to be a mx - tau_id = controller.model.inverse_dynamics()(controller.q, controller.qdot, qddot, controller.parameters.cx) + tau_id = controller.model.inverse_dynamics()(controller.q, controller.qdot, qddot, [], [], controller.parameters.cx) return tau_id - muscle_tau diff --git a/bioptim/models/biorbd/biorbd_model.py b/bioptim/models/biorbd/biorbd_model.py index 0250675c6..796b2d1a7 100644 --- a/bioptim/models/biorbd/biorbd_model.py +++ b/bioptim/models/biorbd/biorbd_model.py @@ -28,6 +28,7 @@ def __init__( bio_model: str | biorbd.Model, friction_coefficients: np.ndarray = None, segments_to_apply_external_forces: list[str] = [], + segments_to_apply_translational_forces: list[str] = [], parameters: ParameterList = None, ): if not isinstance(bio_model, str) and not isinstance(bio_model, biorbd.Model): @@ -39,6 +40,9 @@ def __init__( parameters[param_key].apply_parameter(self) self._friction_coefficients = friction_coefficients self._segments_to_apply_external_forces = segments_to_apply_external_forces + self._segments_to_apply_translational_forces = segments_to_apply_translational_forces + if len(segments_to_apply_external_forces) > 0 and len(segments_to_apply_translational_forces) > 0: + raise ValueError("You can't apply both external_forces and translational_forces at the same time") # Declaration of MX variables of the right shape for the creation of CasADi Functions self.q = MX.sym("q_mx", self.nb_q, 1) @@ -47,7 +51,9 @@ def __init__( self.qddot_joints = MX.sym("qddot_joints_mx", self.nb_qddot - self.nb_root, 1) self.tau = MX.sym("tau_mx", self.nb_tau, 1) self.muscle = MX.sym("muscle_mx", self.nb_muscles, 1) + self.activations = MX.sym("activations_mx", self.nb_muscles, 1) self.external_forces = MX.sym("external_forces_mx", 9, len(segments_to_apply_external_forces)) + self.translational_forces = MX.sym("translational_forces_mx", 6, len(segments_to_apply_translational_forces)) # TODO: remove mx (the MX parameters should be created inside the BiorbdModel) self.parameters = parameters.mx if parameters else MX() @@ -388,11 +394,13 @@ def forward_dynamics_free_floating_base(self) -> Function: def reorder_qddot_root_joints(qddot_root, qddot_joints) -> MX | SX: return vertcat(qddot_root, qddot_joints) - def _dispatch_forces(self, external_forces: MX): + def _dispatch_forces(self, external_forces: MX, translational_forces: MX): - if external_forces is not None: + external_forces_set = self.model.externalForceSet() + + if external_forces.shape[1] > 0: if not isinstance(external_forces, MX): - raise ValueError("external_forces should be a numpy array of shape 9 x nb_forces.") + raise ValueError("external_forces should be of shape 9 x nb_forces.") if external_forces.shape[0] != 9: raise ValueError( f"external_forces has {external_forces.shape[0]} rows, it should have 9 rows (Mx, My, Mz, Fx, Fy, Fz, Px, Py, Pz). You should provide the moments, forces and points of application." @@ -402,19 +410,35 @@ def _dispatch_forces(self, external_forces: MX): f"external_forces has {external_forces.shape[1]} columns and {len(self._segments_to_apply_external_forces)} segments to apply forces on, they should have the same length." ) - external_forces_set = self.model.externalForceSet() - - if external_forces is not None: + # Add the external forces for i_element in range(external_forces.shape[1]): name = self._segments_to_apply_external_forces[i_element] values = external_forces[:6, i_element] point_of_application = external_forces[6:9, i_element] external_forces_set.add(name, values, point_of_application) + elif translational_forces.shape[1] > 0: + if not isinstance(translational_forces, MX): + raise ValueError("translational_forces should be of shape 6 x nb_forces.") + if translational_forces.shape[0] != 6: + raise ValueError( + f"translational_forces has {translational_forces.shape[0]} rows, it should have 6 rows (Fx, Fy, Fz, Px, Py, Pz). You should provide the forces and points of application.") + if len(self._segments_to_apply_external_forces) != translational_forces.shape[1]: + raise ValueError( + f"translational_forces has {translational_forces.shape[1]} columns and {len(self._segments_to_apply_external_forces)} segments to apply forces on, they should have the same length." + ) + + # Add the translational forces + for i_elements in range(translational_forces.shape[1]): + name = self._segments_to_apply_external_forces[i_elements] + values = translational_forces[:3, i_elements] + point_of_application = translational_forces[3:6, i_elements] + external_forces_set.addTranslationalForce(values, name, point_of_application) + return external_forces_set def forward_dynamics(self, with_contact: bool = False) -> Function: - external_forces_set = self._dispatch_forces(self.external_forces) + external_forces_set = self._dispatch_forces(self.external_forces, self.translational_forces) q_biorbd = GeneralizedCoordinates(self.q) qdot_biorbd = GeneralizedVelocity(self.qdot) @@ -426,14 +450,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.parameters], + [self.q, self.qdot, self.tau, self.external_forces, self.translational_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.parameters], + [self.q, self.qdot, self.tau, self.external_forces, self.translational_forces, self.parameters], [biorbd_return], ) return casadi_fun @@ -441,11 +465,11 @@ def forward_dynamics(self, with_contact: bool = False) -> Function: def inverse_dynamics(self, with_contact: bool = False) -> Function: # @ipuch: I do not understand what is happening here? Do we have f_ext or it is just the contact forces? if with_contact: - f_ext = self.reshape_fext_to_fcontact(self.external_forces) + external_forces = self.reshape_fext_to_fcontact(self.external_forces) else: - f_ext = self.external_forces + external_forces = self.external_forces - external_forces_set = self._dispatch_forces(f_ext) + external_forces_set = self._dispatch_forces(external_forces, self.translational_forces) q_biorbd = GeneralizedCoordinates(self.q) qdot_biorbd = GeneralizedVelocity(self.qdot) @@ -453,13 +477,13 @@ 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.parameters], + [self.q, self.qdot, self.qddot, self.external_forces, self.translational_forces, self.parameters], [biorbd_return], ) return casadi_fun def contact_forces_from_constrained_forward_dynamics(self) -> Function: - external_forces_set = self._dispatch_forces(self.external_forces) + external_forces_set = self._dispatch_forces(self.external_forces, self.translational_forces) q_biorbd = GeneralizedCoordinates(self.q) qdot_biorbd = GeneralizedVelocity(self.qdot) @@ -469,7 +493,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.parameters], + [self.q, self.qdot, self.tau, self.external_forces, self.translational_forces, self.parameters], [biorbd_return], ) return casadi_fun @@ -486,15 +510,14 @@ def qdot_from_impact(self) -> Function: return casadi_fun def muscle_activation_dot(self) -> Function: - # @Pariterre: there is a problem with the statesSet :/ - muscle_excitation_biorbd = self.muscle muscle_states = self.model.stateSet() for k in range(self.model.nbMuscles()): - muscle_states[k].setExcitation(muscle_excitation_biorbd[k]) + muscle_states[k].setActivation(self.activations[k]) + muscle_states[k].setExcitation(self.muscle[k]) biorbd_return = self.model.activationDot(muscle_states).to_mx() casadi_fun = Function( "muscle_activation_dot", - [self.muscle, self.parameters], + [self.muscle, self.activations, self.parameters], [biorbd_return], ) return casadi_fun @@ -814,11 +837,11 @@ def get_quaternion_idx(self) -> list[list[int]]: def contact_forces(self) -> Function: force = self.contact_forces_from_constrained_forward_dynamics()( - self.q, self.qdot, self.tau, self.external_forces, self.parameters + self.q, self.qdot, self.tau, self.external_forces, self.translational_forces, self.parameters ) casadi_fun = Function( "contact_forces", - [self.q, self.qdot, self.tau, self.external_forces, self.parameters], + [self.q, self.qdot, self.tau, self.external_forces, self.translational_forces, self.parameters], [force], ) return casadi_fun diff --git a/bioptim/models/biorbd/multi_biorbd_model.py b/bioptim/models/biorbd/multi_biorbd_model.py index f55166139..25fb4bf6c 100644 --- a/bioptim/models/biorbd/multi_biorbd_model.py +++ b/bioptim/models/biorbd/multi_biorbd_model.py @@ -80,6 +80,7 @@ def __init__( self.qddot_joints = MX.sym("qddot_joints_mx", self.nb_qddot - self.nb_root, 1) self.tau = MX.sym("tau_mx", self.nb_tau, 1) self.muscle = MX.sym("muscle_mx", self.nb_muscles, 1) + self.activations = MX.sym("activations_mx", self.nb_muscles, 1) # TODO: parameters should be handled model by model self.parameters = MX.sym("parameters_to_be_implemented", 0, 1) @@ -642,14 +643,17 @@ def qdot_from_impact(self) -> Function: def muscle_activation_dot(self) -> Function: biorbd_return = MX() + n_muscles = 0 for model in self.models: - muscle_states = model.model.stateSet() # still call from Biorbd + muscle_states = model.model.stateSet() for k in range(model.nb_muscles): - muscle_states[k].setExcitation(self.muscle[k]) + muscle_states[k].setActivation(self.activations[k + n_muscles]) + muscle_states[k].setExcitation(self.muscle[k + n_muscles]) biorbd_return = vertcat(biorbd_return, model.model.activationDot(muscle_states).to_mx()) + n_muscles += model.nb_muscles casadi_fun = Function( "muscle_activation_dot", - [self.muscle, self.parameters], + [self.muscle, self.activations, self.parameters], [biorbd_return], ) return casadi_fun diff --git a/bioptim/models/protocols/biomodel.py b/bioptim/models/protocols/biomodel.py index faa8e10c8..7cf646a12 100644 --- a/bioptim/models/protocols/biomodel.py +++ b/bioptim/models/protocols/biomodel.py @@ -206,7 +206,7 @@ def qdot_from_impact(self) -> Function: def muscle_activation_dot(self) -> Function: """ Get the activation derivative of the muscles states - args: muscle_excitations + args: muscle_excitations, muscle_activations """ def muscle_joint_torque(self) -> Function: diff --git a/tests/shard1/test_biorbd_multi_model.py b/tests/shard1/test_biorbd_multi_model.py index 4061b4c04..0f7c26088 100644 --- a/tests/shard1/test_biorbd_multi_model.py +++ b/tests/shard1/test_biorbd_multi_model.py @@ -225,6 +225,7 @@ def test_biorbd_model(): np.array([3.1, 1, 2, 9.1, 1, 2]), [], [], + [], ), np.array([4.844876e01, 2.121037e-01, 1.964626e00, 4.165226e02, 3.721585e01, 1.906986e00]).reshape(6, 1), decimal=5, @@ -254,6 +255,7 @@ def test_biorbd_model(): models.muscle_activation_dot()( [], # There is no muscle in the models [], + [], ), np.array([], dtype=np.float64).reshape(0, 1), ) diff --git a/tests/shard1/test_dynamics.py b/tests/shard1/test_dynamics.py index 76644127a..d0b97ef1c 100644 --- a/tests/shard1/test_dynamics.py +++ b/tests/shard1/test_dynamics.py @@ -48,7 +48,7 @@ def __init__(self, nlp, use_sx): ) def test_torque_driven(with_contact, with_external_force, cx, rigidbody_dynamics, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx==SX) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx==SX)) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod", segments_to_apply_external_forces=["Seg0"], @@ -125,7 +125,7 @@ def test_torque_driven(with_contact, with_external_force, cx, rigidbody_dynamics 0, ] - ocp = OptimalControlProgram(nlp, use_sx=(cx==SX) + ocp = OptimalControlProgram(nlp, use_sx=(cx==SX)) nlp.control_type = ControlType.CONSTANT NonLinearProgram.add( ocp, @@ -270,7 +270,7 @@ def test_torque_driven(with_contact, with_external_force, cx, rigidbody_dynamics @pytest.mark.parametrize("with_contact", [False, True]) def test_torque_driven_implicit(with_contact, cx, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx==SX) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx==SX)) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod" ) @@ -287,7 +287,7 @@ def test_torque_driven_implicit(with_contact, cx, phase_dynamics): nlp.u_scaling = VariableScalingList() nlp.a_scaling = VariableScalingList() - ocp = OptimalControlProgram(nlp, use_sx=(cx==SX) + ocp = OptimalControlProgram(nlp, use_sx=(cx==SX)) nlp.control_type = ControlType.CONSTANT NonLinearProgram.add( @@ -348,7 +348,7 @@ def test_torque_driven_implicit(with_contact, cx, phase_dynamics): @pytest.mark.parametrize("implicit_contact", [False, True]) def test_torque_driven_soft_contacts_dynamics(with_contact, cx, implicit_contact, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx==SX) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx==SX)) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod" ) @@ -365,7 +365,7 @@ def test_torque_driven_soft_contacts_dynamics(with_contact, cx, implicit_contact nlp.u_scaling = VariableScalingList() nlp.a_scaling = VariableScalingList() - ocp = OptimalControlProgram(nlp, use_sx=(cx==SX) + ocp = OptimalControlProgram(nlp, use_sx=(cx==SX)) nlp.control_type = ControlType.CONSTANT NonLinearProgram.add( @@ -427,7 +427,7 @@ def test_torque_driven_soft_contacts_dynamics(with_contact, cx, implicit_contact @pytest.mark.parametrize("with_contact", [False, True]) def test_torque_derivative_driven(with_contact, with_external_force, cx, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx==SX) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx==SX)) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod", segments_to_apply_external_forces=["Seg0"], @@ -444,7 +444,7 @@ def test_torque_derivative_driven(with_contact, with_external_force, cx, phase_d nlp.u_scaling = VariableScalingList() nlp.a_scaling = VariableScalingList() - ocp = OptimalControlProgram(nlp, use_sx=(cx==SX) + ocp = OptimalControlProgram(nlp, use_sx=(cx==SX)) nlp.control_type = ControlType.CONSTANT external_forces = None @@ -635,7 +635,7 @@ def test_torque_derivative_driven(with_contact, with_external_force, cx, phase_d @pytest.mark.parametrize("with_contact", [False, True]) def test_torque_derivative_driven_implicit(with_contact, cx, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx==SX) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx==SX)) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod" ) @@ -652,7 +652,7 @@ def test_torque_derivative_driven_implicit(with_contact, cx, phase_dynamics): nlp.u_scaling = VariableScalingList() nlp.a_scaling = VariableScalingList() - ocp = OptimalControlProgram(nlp, use_sx=(cx==SX) + ocp = OptimalControlProgram(nlp, use_sx=(cx==SX)) nlp.control_type = ControlType.CONSTANT NonLinearProgram.add( ocp, @@ -745,7 +745,7 @@ def test_torque_derivative_driven_implicit(with_contact, cx, phase_dynamics): @pytest.mark.parametrize("implicit_contact", [False, True]) def test_torque_derivative_driven_soft_contacts_dynamics(with_contact, cx, implicit_contact, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx==SX) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx==SX)) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod" ) @@ -762,7 +762,7 @@ def test_torque_derivative_driven_soft_contacts_dynamics(with_contact, cx, impli nlp.u_scaling = VariableScalingList() nlp.a_scaling = VariableScalingList() - ocp = OptimalControlProgram(nlp, use_sx=(cx==SX) + ocp = OptimalControlProgram(nlp, use_sx=(cx==SX)) nlp.control_type = ControlType.CONSTANT NonLinearProgram.add( ocp, @@ -935,7 +935,7 @@ def test_implicit_dynamics_errors(dynamics, phase_dynamics): @pytest.mark.parametrize("with_contact", [False, True]) def test_torque_activation_driven(with_contact, with_external_force, cx, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx==SX) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx==SX)) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod", segments_to_apply_external_forces=["Seg0"], @@ -1012,7 +1012,7 @@ def test_torque_activation_driven(with_contact, with_external_force, cx, phase_d 0, ] - ocp = OptimalControlProgram(nlp, use_sx=(cx==SX) + ocp = OptimalControlProgram(nlp, use_sx=(cx==SX)) nlp.control_type = ControlType.CONSTANT NonLinearProgram.add( ocp, @@ -1116,7 +1116,7 @@ def test_torque_activation_driven_with_residual_torque( with_residual_torque, with_external_force, with_passive_torque, cx, phase_dynamics ): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx==SX) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx==SX)) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/torque_driven_ocp/models/2segments_2dof_2contacts.bioMod", segments_to_apply_external_forces=["Seg0"], @@ -1192,7 +1192,7 @@ def test_torque_activation_driven_with_residual_torque( 0, ] - ocp = OptimalControlProgram(nlp, use_sx=(cx==SX) + ocp = OptimalControlProgram(nlp, use_sx=(cx==SX)) nlp.control_type = ControlType.CONSTANT NonLinearProgram.add( ocp, @@ -1296,7 +1296,7 @@ def test_torque_activation_driven_with_residual_torque( @pytest.mark.parametrize("cx", [MX, SX]) def test_torque_driven_free_floating_base(cx, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx==SX) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx==SX)) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod" ) @@ -1313,7 +1313,7 @@ def test_torque_driven_free_floating_base(cx, phase_dynamics): nlp.u_scaling = VariableScalingList() nlp.a_scaling = VariableScalingList() - ocp = OptimalControlProgram(nlp, use_sx=(cx==SX) + ocp = OptimalControlProgram(nlp, use_sx=(cx==SX)) nlp.control_type = ControlType.CONSTANT NonLinearProgram.add( ocp, @@ -1362,7 +1362,7 @@ def test_muscle_driven( with_excitations, with_contact, with_residual_torque, with_external_force, rigidbody_dynamics, cx, phase_dynamics ): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx==SX) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx==SX)) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/muscle_driven_ocp/models/arm26_with_contact.bioMod", segments_to_apply_external_forces=["r_ulna_radius_hand_rotation1"], @@ -1440,7 +1440,7 @@ def test_muscle_driven( 0, ] - ocp = OptimalControlProgram(nlp, use_sx=(cx==SX) + ocp = OptimalControlProgram(nlp, use_sx=(cx==SX)) nlp.control_type = ControlType.CONSTANT NonLinearProgram.add( ocp, @@ -1932,7 +1932,7 @@ def test_muscle_driven( @pytest.mark.parametrize("rigid_body_dynamics", RigidBodyDynamics) def test_joints_acceleration_driven(cx, rigid_body_dynamics, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx==SX) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx==SX)) nlp.model = BiorbdModel(TestUtils.bioptim_folder() + "/examples/getting_started/models/double_pendulum.bioMod") nlp.ns = 5 @@ -1948,7 +1948,7 @@ def test_joints_acceleration_driven(cx, rigid_body_dynamics, phase_dynamics): nlp.u_scaling = VariableScalingList() nlp.a_scaling = VariableScalingList() - ocp = OptimalControlProgram(nlp, use_sx=(cx==SX) + ocp = OptimalControlProgram(nlp, use_sx=(cx==SX)) nlp.control_type = ControlType.CONSTANT NonLinearProgram.add( From 7eae07ac592e05aafc2199756eae9afac21c01b5 Mon Sep 17 00:00:00 2001 From: eve-mac Date: Thu, 26 Sep 2024 18:49:21 -0400 Subject: [PATCH 034/108] consequences for being lazy )))))))) --- bioptim/dynamics/dynamics_functions.py | 28 ++++++++++++---- bioptim/limits/constraints.py | 4 ++- bioptim/models/biorbd/biorbd_model.py | 3 +- tests/shard1/test_dynamics.py | 44 +++++++++++++------------- tests/shard3/test_ligaments.py | 16 +++++----- tests/shard3/test_passive_torque.py | 16 +++++----- 6 files changed, 64 insertions(+), 47 deletions(-) diff --git a/bioptim/dynamics/dynamics_functions.py b/bioptim/dynamics/dynamics_functions.py index af7b24560..c7c1367cc 100644 --- a/bioptim/dynamics/dynamics_functions.py +++ b/bioptim/dynamics/dynamics_functions.py @@ -169,7 +169,9 @@ def torque_driven( dxdt[nlp.states["qdot"].index, :] = qddot dxdt[nlp.states["qddot"].index, :] = DynamicsFunctions.get(nlp.controls["qdddot"], controls) else: - ddq = DynamicsFunctions.forward_dynamics(nlp, q, qdot, tau, with_contact, external_forces, translational_forces) + ddq = DynamicsFunctions.forward_dynamics( + nlp, q, qdot, tau, with_contact, external_forces, translational_forces + ) dxdt = nlp.cx(nlp.states.shape, ddq.shape[1]) dxdt[nlp.states["q"].index, :] = horzcat(*[dq for _ in range(ddq.shape[1])]) dxdt[nlp.states["qdot"].index, :] = ddq @@ -184,7 +186,9 @@ def torque_driven( ): if not with_contact and fatigue is None: qddot = DynamicsFunctions.get(nlp.states_dot["qddot"], nlp.states_dot.scaled.cx) - tau_id = DynamicsFunctions.inverse_dynamics(nlp, q, qdot, qddot, with_contact, external_forces, translational_forces) + tau_id = DynamicsFunctions.inverse_dynamics( + nlp, q, qdot, qddot, with_contact, external_forces, translational_forces + ) defects = nlp.cx(dq.shape[0] + tau_id.shape[0], tau_id.shape[1]) dq_defects = [] @@ -877,7 +881,9 @@ def muscles_driven( ): if not with_contact and fatigue is None: qddot = DynamicsFunctions.get(nlp.states_dot["qddot"], nlp.states_dot.cx) - tau_id = DynamicsFunctions.inverse_dynamics(nlp, q, qdot, qddot, with_contact, external_forces, translational_forces) + tau_id = DynamicsFunctions.inverse_dynamics( + nlp, q, qdot, qddot, with_contact, external_forces, translational_forces + ) defects = nlp.cx(dq.shape[0] + tau_id.shape[0], tau_id.shape[1]) dq_defects = [] @@ -1018,7 +1024,9 @@ def joints_acceleration_driven( qddot_root_defects = DynamicsFunctions.get(nlp.states_dot["qddot_roots"], nlp.states_dot.cx) qddot_defects_reordered = nlp.model.reorder_qddot_root_joints(qddot_root_defects, qddot_joints) - floating_base_constraint = nlp.model.inverse_dynamics(q, qdot, qddot_defects_reordered, [], [])[: nlp.model.nb_root] + floating_base_constraint = nlp.model.inverse_dynamics(q, qdot, qddot_defects_reordered, [], [])[ + : nlp.model.nb_root + ] defects = nlp.cx(qdot_mapped.shape[0] + qddot_root_mapped.shape[0] + qddot_joints_mapped.shape[0], 1) @@ -1137,11 +1145,15 @@ def forward_dynamics( external_forces = [] if external_forces is None else external_forces translational_forces = [] if translational_forces is None else translational_forces qddot = nlp.model.forward_dynamics(with_contact=with_contact)( - q, qdot, tau, external_forces, translational_forces, nlp.parameters.cx, + q, + qdot, + tau, + external_forces, + translational_forces, + nlp.parameters.cx, ) return qdot_var_mapping.map(qddot) - @staticmethod def inverse_dynamics( nlp, @@ -1301,6 +1313,8 @@ 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, translational_forces, nlp.parameters.cx) + qddot_u = nlp.model.partitioned_forward_dynamics( + q_u, qdot_u, tau, external_forces, translational_forces, nlp.parameters.cx + ) return DynamicsEvaluation(dxdt=vertcat(qdot_u, qddot_u), defects=None) diff --git a/bioptim/limits/constraints.py b/bioptim/limits/constraints.py index 7fcd52f53..9e740f813 100644 --- a/bioptim/limits/constraints.py +++ b/bioptim/limits/constraints.py @@ -568,7 +568,9 @@ def tau_from_muscle_equal_inverse_dynamics( # Todo: add fext tau_id = nlp.model.inverse_dynamics()(q, qdot, qddot, fext) # fext need to be a mx - tau_id = controller.model.inverse_dynamics()(controller.q, controller.qdot, qddot, [], [], controller.parameters.cx) + tau_id = controller.model.inverse_dynamics()( + controller.q, controller.qdot, qddot, [], [], controller.parameters.cx + ) return tau_id - muscle_tau diff --git a/bioptim/models/biorbd/biorbd_model.py b/bioptim/models/biorbd/biorbd_model.py index 796b2d1a7..15dccad48 100644 --- a/bioptim/models/biorbd/biorbd_model.py +++ b/bioptim/models/biorbd/biorbd_model.py @@ -422,7 +422,8 @@ def _dispatch_forces(self, external_forces: MX, translational_forces: MX): raise ValueError("translational_forces should be of shape 6 x nb_forces.") if translational_forces.shape[0] != 6: raise ValueError( - f"translational_forces has {translational_forces.shape[0]} rows, it should have 6 rows (Fx, Fy, Fz, Px, Py, Pz). You should provide the forces and points of application.") + f"translational_forces has {translational_forces.shape[0]} rows, it should have 6 rows (Fx, Fy, Fz, Px, Py, Pz). You should provide the forces and points of application." + ) if len(self._segments_to_apply_external_forces) != translational_forces.shape[1]: raise ValueError( f"translational_forces has {translational_forces.shape[1]} columns and {len(self._segments_to_apply_external_forces)} segments to apply forces on, they should have the same length." diff --git a/tests/shard1/test_dynamics.py b/tests/shard1/test_dynamics.py index d0b97ef1c..298ce80b3 100644 --- a/tests/shard1/test_dynamics.py +++ b/tests/shard1/test_dynamics.py @@ -48,7 +48,7 @@ def __init__(self, nlp, use_sx): ) def test_torque_driven(with_contact, with_external_force, cx, rigidbody_dynamics, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx==SX)) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx == SX)) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod", segments_to_apply_external_forces=["Seg0"], @@ -125,7 +125,7 @@ def test_torque_driven(with_contact, with_external_force, cx, rigidbody_dynamics 0, ] - ocp = OptimalControlProgram(nlp, use_sx=(cx==SX)) + ocp = OptimalControlProgram(nlp, use_sx=(cx == SX)) nlp.control_type = ControlType.CONSTANT NonLinearProgram.add( ocp, @@ -270,7 +270,7 @@ def test_torque_driven(with_contact, with_external_force, cx, rigidbody_dynamics @pytest.mark.parametrize("with_contact", [False, True]) def test_torque_driven_implicit(with_contact, cx, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx==SX)) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx == SX)) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod" ) @@ -287,7 +287,7 @@ def test_torque_driven_implicit(with_contact, cx, phase_dynamics): nlp.u_scaling = VariableScalingList() nlp.a_scaling = VariableScalingList() - ocp = OptimalControlProgram(nlp, use_sx=(cx==SX)) + ocp = OptimalControlProgram(nlp, use_sx=(cx == SX)) nlp.control_type = ControlType.CONSTANT NonLinearProgram.add( @@ -348,7 +348,7 @@ def test_torque_driven_implicit(with_contact, cx, phase_dynamics): @pytest.mark.parametrize("implicit_contact", [False, True]) def test_torque_driven_soft_contacts_dynamics(with_contact, cx, implicit_contact, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx==SX)) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx == SX)) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod" ) @@ -365,7 +365,7 @@ def test_torque_driven_soft_contacts_dynamics(with_contact, cx, implicit_contact nlp.u_scaling = VariableScalingList() nlp.a_scaling = VariableScalingList() - ocp = OptimalControlProgram(nlp, use_sx=(cx==SX)) + ocp = OptimalControlProgram(nlp, use_sx=(cx == SX)) nlp.control_type = ControlType.CONSTANT NonLinearProgram.add( @@ -427,7 +427,7 @@ def test_torque_driven_soft_contacts_dynamics(with_contact, cx, implicit_contact @pytest.mark.parametrize("with_contact", [False, True]) def test_torque_derivative_driven(with_contact, with_external_force, cx, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx==SX)) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx == SX)) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod", segments_to_apply_external_forces=["Seg0"], @@ -444,7 +444,7 @@ def test_torque_derivative_driven(with_contact, with_external_force, cx, phase_d nlp.u_scaling = VariableScalingList() nlp.a_scaling = VariableScalingList() - ocp = OptimalControlProgram(nlp, use_sx=(cx==SX)) + ocp = OptimalControlProgram(nlp, use_sx=(cx == SX)) nlp.control_type = ControlType.CONSTANT external_forces = None @@ -635,7 +635,7 @@ def test_torque_derivative_driven(with_contact, with_external_force, cx, phase_d @pytest.mark.parametrize("with_contact", [False, True]) def test_torque_derivative_driven_implicit(with_contact, cx, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx==SX)) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx == SX)) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod" ) @@ -652,7 +652,7 @@ def test_torque_derivative_driven_implicit(with_contact, cx, phase_dynamics): nlp.u_scaling = VariableScalingList() nlp.a_scaling = VariableScalingList() - ocp = OptimalControlProgram(nlp, use_sx=(cx==SX)) + ocp = OptimalControlProgram(nlp, use_sx=(cx == SX)) nlp.control_type = ControlType.CONSTANT NonLinearProgram.add( ocp, @@ -745,7 +745,7 @@ def test_torque_derivative_driven_implicit(with_contact, cx, phase_dynamics): @pytest.mark.parametrize("implicit_contact", [False, True]) def test_torque_derivative_driven_soft_contacts_dynamics(with_contact, cx, implicit_contact, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx==SX)) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx == SX)) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod" ) @@ -762,7 +762,7 @@ def test_torque_derivative_driven_soft_contacts_dynamics(with_contact, cx, impli nlp.u_scaling = VariableScalingList() nlp.a_scaling = VariableScalingList() - ocp = OptimalControlProgram(nlp, use_sx=(cx==SX)) + ocp = OptimalControlProgram(nlp, use_sx=(cx == SX)) nlp.control_type = ControlType.CONSTANT NonLinearProgram.add( ocp, @@ -935,7 +935,7 @@ def test_implicit_dynamics_errors(dynamics, phase_dynamics): @pytest.mark.parametrize("with_contact", [False, True]) def test_torque_activation_driven(with_contact, with_external_force, cx, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx==SX)) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx == SX)) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod", segments_to_apply_external_forces=["Seg0"], @@ -1012,7 +1012,7 @@ def test_torque_activation_driven(with_contact, with_external_force, cx, phase_d 0, ] - ocp = OptimalControlProgram(nlp, use_sx=(cx==SX)) + ocp = OptimalControlProgram(nlp, use_sx=(cx == SX)) nlp.control_type = ControlType.CONSTANT NonLinearProgram.add( ocp, @@ -1116,7 +1116,7 @@ def test_torque_activation_driven_with_residual_torque( with_residual_torque, with_external_force, with_passive_torque, cx, phase_dynamics ): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx==SX)) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx == SX)) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/torque_driven_ocp/models/2segments_2dof_2contacts.bioMod", segments_to_apply_external_forces=["Seg0"], @@ -1192,7 +1192,7 @@ def test_torque_activation_driven_with_residual_torque( 0, ] - ocp = OptimalControlProgram(nlp, use_sx=(cx==SX)) + ocp = OptimalControlProgram(nlp, use_sx=(cx == SX)) nlp.control_type = ControlType.CONSTANT NonLinearProgram.add( ocp, @@ -1296,7 +1296,7 @@ def test_torque_activation_driven_with_residual_torque( @pytest.mark.parametrize("cx", [MX, SX]) def test_torque_driven_free_floating_base(cx, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx==SX)) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx == SX)) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod" ) @@ -1313,7 +1313,7 @@ def test_torque_driven_free_floating_base(cx, phase_dynamics): nlp.u_scaling = VariableScalingList() nlp.a_scaling = VariableScalingList() - ocp = OptimalControlProgram(nlp, use_sx=(cx==SX)) + ocp = OptimalControlProgram(nlp, use_sx=(cx == SX)) nlp.control_type = ControlType.CONSTANT NonLinearProgram.add( ocp, @@ -1362,7 +1362,7 @@ def test_muscle_driven( with_excitations, with_contact, with_residual_torque, with_external_force, rigidbody_dynamics, cx, phase_dynamics ): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx==SX)) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx == SX)) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/muscle_driven_ocp/models/arm26_with_contact.bioMod", segments_to_apply_external_forces=["r_ulna_radius_hand_rotation1"], @@ -1440,7 +1440,7 @@ def test_muscle_driven( 0, ] - ocp = OptimalControlProgram(nlp, use_sx=(cx==SX)) + ocp = OptimalControlProgram(nlp, use_sx=(cx == SX)) nlp.control_type = ControlType.CONSTANT NonLinearProgram.add( ocp, @@ -1932,7 +1932,7 @@ def test_muscle_driven( @pytest.mark.parametrize("rigid_body_dynamics", RigidBodyDynamics) def test_joints_acceleration_driven(cx, rigid_body_dynamics, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx==SX)) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx == SX)) nlp.model = BiorbdModel(TestUtils.bioptim_folder() + "/examples/getting_started/models/double_pendulum.bioMod") nlp.ns = 5 @@ -1948,7 +1948,7 @@ def test_joints_acceleration_driven(cx, rigid_body_dynamics, phase_dynamics): nlp.u_scaling = VariableScalingList() nlp.a_scaling = VariableScalingList() - ocp = OptimalControlProgram(nlp, use_sx=(cx==SX)) + ocp = OptimalControlProgram(nlp, use_sx=(cx == SX)) nlp.control_type = ControlType.CONSTANT NonLinearProgram.add( diff --git a/tests/shard3/test_ligaments.py b/tests/shard3/test_ligaments.py index 00b278f9c..f228a10de 100644 --- a/tests/shard3/test_ligaments.py +++ b/tests/shard3/test_ligaments.py @@ -41,7 +41,7 @@ def __init__(self, nlp, use_sx): @pytest.mark.parametrize("with_ligament", [False, True]) def test_torque_driven_with_ligament(with_ligament, cx, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx==SX) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx == SX)) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/torque_driven_ocp/models/mass_point_with_ligament.bioMod" ) @@ -57,7 +57,7 @@ def test_torque_driven_with_ligament(with_ligament, cx, phase_dynamics): nlp.x_bounds = np.zeros((nlp.model.nb_q * 3, 1)) nlp.u_bounds = np.zeros((nlp.model.nb_q, 1)) - ocp = OptimalControlProgram(nlp, use_sx=(cx==SX) + ocp = OptimalControlProgram(nlp, use_sx=(cx == SX)) nlp.control_type = ControlType.CONSTANT NonLinearProgram.add( ocp, @@ -105,7 +105,7 @@ def test_torque_driven_with_ligament(with_ligament, cx, phase_dynamics): @pytest.mark.parametrize("with_ligament", [False, True]) def test_torque_derivative_driven_with_ligament(with_ligament, cx, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx==SX) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx == SX)) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/torque_driven_ocp/models/mass_point_with_ligament.bioMod" ) @@ -121,7 +121,7 @@ def test_torque_derivative_driven_with_ligament(with_ligament, cx, phase_dynamic nlp.x_bounds = np.zeros((nlp.model.nb_q * 3, 1)) nlp.u_bounds = np.zeros((nlp.model.nb_q, 1)) - ocp = OptimalControlProgram(nlp, use_sx=(cx==SX) + ocp = OptimalControlProgram(nlp, use_sx=(cx == SX)) nlp.control_type = ControlType.CONSTANT NonLinearProgram.add( @@ -171,7 +171,7 @@ def test_torque_derivative_driven_with_ligament(with_ligament, cx, phase_dynamic @pytest.mark.parametrize("with_ligament", [False, True]) def test_torque_activation_driven_with_ligament(with_ligament, cx, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx==SX) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx == SX)) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/torque_driven_ocp/models/mass_point_with_ligament.bioMod" ) @@ -186,7 +186,7 @@ def test_torque_activation_driven_with_ligament(with_ligament, cx, phase_dynamic nlp.a_scaling = VariableScalingList() nlp.x_bounds = np.zeros((nlp.model.nb_q * 2, 1)) nlp.u_bounds = np.zeros((nlp.model.nb_q, 1)) - ocp = OptimalControlProgram(nlp, use_sx=(cx==SX) + ocp = OptimalControlProgram(nlp, use_sx=(cx == SX)) nlp.control_type = ControlType.CONSTANT NonLinearProgram.add( ocp, @@ -235,7 +235,7 @@ def test_torque_activation_driven_with_ligament(with_ligament, cx, phase_dynamic @pytest.mark.parametrize("with_ligament", [False, True]) def test_muscle_driven_with_ligament(with_ligament, cx, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx==SX) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx == SX)) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/muscle_driven_ocp/models/arm26_with_ligament.bioMod" ) @@ -251,7 +251,7 @@ def test_muscle_driven_with_ligament(with_ligament, cx, phase_dynamics): nlp.x_bounds = np.zeros((nlp.model.nb_q * 2 + nlp.model.nb_muscles, 1)) nlp.u_bounds = np.zeros((nlp.model.nb_muscles, 1)) - ocp = OptimalControlProgram(nlp, use_sx=(cx==SX) + ocp = OptimalControlProgram(nlp, use_sx=(cx == SX)) nlp.control_type = ControlType.CONSTANT NonLinearProgram.add( ocp, diff --git a/tests/shard3/test_passive_torque.py b/tests/shard3/test_passive_torque.py index 8c7a1fe43..95a8c592b 100644 --- a/tests/shard3/test_passive_torque.py +++ b/tests/shard3/test_passive_torque.py @@ -42,7 +42,7 @@ def __init__(self, nlp, use_sx): @pytest.mark.parametrize("rigidbody_dynamics", [RigidBodyDynamics.ODE]) def test_torque_driven_with_passive_torque(with_passive_torque, cx, rigidbody_dynamics, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx==SX) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx == SX)) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod" ) @@ -58,7 +58,7 @@ def test_torque_driven_with_passive_torque(with_passive_torque, cx, rigidbody_dy nlp.x_bounds = np.zeros((nlp.model.nb_q * 3, 1)) nlp.u_bounds = np.zeros((nlp.model.nb_q, 1)) - ocp = OptimalControlProgram(nlp, use_sx=(cx==SX) + ocp = OptimalControlProgram(nlp, use_sx=(cx == SX)) nlp.control_type = ControlType.CONSTANT NonLinearProgram.add( ocp, @@ -129,7 +129,7 @@ def test_torque_driven_with_passive_torque(with_passive_torque, cx, rigidbody_dy @pytest.mark.parametrize("with_passive_torque", [False, True]) def test_torque_derivative_driven_with_passive_torque(with_passive_torque, cx, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx==SX) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx == SX)) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod" ) @@ -145,7 +145,7 @@ def test_torque_derivative_driven_with_passive_torque(with_passive_torque, cx, p nlp.x_bounds = np.zeros((nlp.model.nb_q * 3, 1)) nlp.u_bounds = np.zeros((nlp.model.nb_q, 1)) - ocp = OptimalControlProgram(nlp, use_sx=(cx==SX) + ocp = OptimalControlProgram(nlp, use_sx=(cx == SX)) nlp.control_type = ControlType.CONSTANT NonLinearProgram.add( @@ -226,7 +226,7 @@ def test_torque_derivative_driven_with_passive_torque(with_passive_torque, cx, p @pytest.mark.parametrize("with_residual_torque", [False, True]) def test_torque_activation_driven_with_passive_torque(with_passive_torque, with_residual_torque, cx, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx==SX) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx == SX)) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod" ) @@ -240,7 +240,7 @@ def test_torque_activation_driven_with_passive_torque(with_passive_torque, with_ nlp.u_scaling = VariableScalingList() nlp.x_bounds = np.zeros((nlp.model.nb_q * 2, 1)) nlp.u_bounds = np.zeros((nlp.model.nb_q, 1)) - ocp = OptimalControlProgram(nlp, use_sx=(cx==SX) + ocp = OptimalControlProgram(nlp, use_sx=(cx == SX)) nlp.control_type = ControlType.CONSTANT NonLinearProgram.add( ocp, @@ -347,7 +347,7 @@ def test_torque_activation_driven_with_passive_torque(with_passive_torque, with_ @pytest.mark.parametrize("rigidbody_dynamics", [RigidBodyDynamics.ODE]) def test_muscle_driven_with_passive_torque(with_passive_torque, rigidbody_dynamics, cx, phase_dynamics): # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx==SX) + nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx == SX)) nlp.model = BiorbdModel(TestUtils.bioptim_folder() + "/examples/muscle_driven_ocp/models/arm26_with_contact.bioMod") nlp.ns = 5 nlp.cx = cx @@ -361,7 +361,7 @@ def test_muscle_driven_with_passive_torque(with_passive_torque, rigidbody_dynami nlp.x_bounds = np.zeros((nlp.model.nb_q * 2 + nlp.model.nb_muscles, 1)) nlp.u_bounds = np.zeros((nlp.model.nb_muscles, 1)) - ocp = OptimalControlProgram(nlp, use_sx=(cx==SX) + ocp = OptimalControlProgram(nlp, use_sx=(cx == SX)) nlp.control_type = ControlType.CONSTANT NonLinearProgram.add( ocp, From 72c6cfcb869ec71d11c6988c0ff608d5e36c4c2e Mon Sep 17 00:00:00 2001 From: eve-mac Date: Thu, 26 Sep 2024 21:05:33 -0400 Subject: [PATCH 035/108] Fixed tests (few remaining) --- README.md | 12 +-- bioptim/dynamics/configure_problem.py | 6 +- .../custom_model/custom_package/my_model.py | 13 +++- .../getting_started/custom_constraint.py | 2 +- .../getting_started/custom_dynamics.py | 2 +- .../getting_started/custom_objectives.py | 2 +- .../examples/moving_horizon_estimation/mhe.py | 2 +- bioptim/examples/track/track_segment_on_rt.py | 2 +- bioptim/limits/constraints.py | 9 +-- bioptim/limits/penalty.py | 47 +++++------ bioptim/models/biorbd/biorbd_model.py | 78 ++++++++++--------- bioptim/models/biorbd/multi_biorbd_model.py | 11 +-- bioptim/models/holonomic_constraints.py | 2 +- bioptim/models/protocols/biomodel.py | 11 +-- bioptim/optimization/optimization_variable.py | 5 +- tests/shard1/test_biorbd_multi_model.py | 2 +- tests/shard3/test_global_torque_driven_ocp.py | 3 - tests/shard4/test_penalty.py | 7 +- 18 files changed, 108 insertions(+), 108 deletions(-) diff --git a/README.md b/README.md index 50dc4c0b9..19c3f3a7c 100644 --- a/README.md +++ b/README.md @@ -1226,11 +1226,11 @@ This constraint assumes that the normal forces is positive (that is having an ad - **TRACK_COM_VELOCITY** — Constraints the center of mass velocity toward a target. The extra parameter `axis_to_track: Axis = (Axis.X, Axis.Y, Axis.Z)` can be provided to specify the axes along which the velocity should be tracked. - **TRACK_CONTACT_FORCES** — Tracks the non-acceleration point reaction forces toward a target. - **TRACK_LINEAR_MOMENTUM** — Constraints the linear momentum toward a target. The extra parameter `axis_to_track: Axis = (Axis.X, Axis.Y, Axis.Z)` can be sent to specify the axes along which the momentum should be tracked. -- **TRACK_MARKER_WITH_SEGMENT_AXIS** — Tracks a marker using a segment, that is aligning an axis toward the marker. The extra parameters `marker_idx: int`, `segment_idx: int`, and `axis: Axis` must be passed to the `Constraint` constructor +- **TRACK_MARKER_WITH_SEGMENT_AXIS** — Tracks a marker using a segment, that is aligning an axis toward the marker. The extra parameters `marker_idx: int`, `segment_index: int`, and `axis: Axis` must be passed to the `Constraint` constructor - **TRACK_MARKERS_VELOCITY** — Tracks the skin marker velocities toward a target. - **TRACK_MARKERS** — Tracks the skin markers toward a target. The extra parameter `axis_to_track: Axis = (Axis.X, Axis.Y, Axis.Z)` can be provided to specify the axes along which the markers should be tracked. - **TRACK_MUSCLES_CONTROL** — Tracks the muscles (part of the control variables) toward a target. -- **TRACK_SEGMENT_WITH_CUSTOM_RT** —Links a segment with an RT (for instance, an Inertial Measurement Unit). It does so by computing the homogenous transformation between the segment and the RT and then converting this to Euler angles. The extra parameters `segment_idx: int` and `rt_idx: int` must be passed to the `Constraint` constructor. +- **TRACK_SEGMENT_WITH_CUSTOM_RT** —Links a segment with an RT (for instance, an Inertial Measurement Unit). It does so by computing the homogenous transformation between the segment and the RT and then converting this to Euler angles. The extra parameters `segment_index: int` and `rt_index: int` must be passed to the `Constraint` constructor. - **TRACK_STATE** — Tracks the state's variable toward a target. - **TRACK_TORQUE** — Tracks the generalized forces (part of the control variables) toward a target. - **CUSTOM** — The user should not directly send CUSTOM, but the user should pass the custom_constraint function directly. You can look at Constraint and ConstraintList sections for more information about how to define custom constraints. @@ -1322,11 +1322,11 @@ Here a list of objective function with its type (Lagrange and/or Mayer) in alpha - **SUPERIMPOSE_MARKERS** (Lagrange and Mayer) — Tracks one marker with another one. The extra parameters `first_marker_idx: int` and `second_marker_idx: int` informs which markers are to be superimposed - **TRACK_ALL_CONTROLS (Lagrange)** — Tracks all the control variables toward a target. - **TRACK_CONTACT_FORCES** (Lagrange) — Tracks the non-acceleration points of the reaction forces toward a target. -- **TRACK_MARKER_WITH_SEGMENT_AXIS** (Lagrange and Mayer) — Minimizes the distance between a marker and an axis of a segment, that is aligning an axis toward the marker. The extra parameters `marker_idx: int`, `segment_idx: int` and `axis: Axis` must be passed to the `Objective` constructor +- **TRACK_MARKER_WITH_SEGMENT_AXIS** (Lagrange and Mayer) — Minimizes the distance between a marker and an axis of a segment, that is aligning an axis toward the marker. The extra parameters `marker_idx: int`, `segment_index: int` and `axis: Axis` must be passed to the `Objective` constructor - **TRACK_MARKERS_VELOCITY or TRACK_MARKERS_ACCELERATION** (Lagrange and Mayer) — Tracks the marker velocities or accelerations toward a target. - **TRACK_MARKERS** (Lagrange and Mayer) — Tracks the skin markers towards a target. The extra parameter `axis_to_track: Axis = (Axis.X, Axis.Y, Axis.Z)` can be sent to specify the axes along which the markers should be tracked. - **TRACK_MUSCLES_CONTROL** (Lagrange) — Tracks the muscles' controls (part of the control variables) toward a target. -- **TRACK_SEGMENT_WITH_CUSTOM_RT** (Lagrange and Mayer) — Minimizes the distance between a segment and an RT (for instance, an Inertial Measurement Unit). It does so by computing the homogenous transformation between the segment and the RT and then converting this to Euler angles. The extra parameters `segment_idx: int` and `rt_idx: int` must be passed to the `Objective` constructor. +- **TRACK_SEGMENT_WITH_CUSTOM_RT** (Lagrange and Mayer) — Minimizes the distance between a segment and an RT (for instance, an Inertial Measurement Unit). It does so by computing the homogenous transformation between the segment and the RT and then converting this to Euler angles. The extra parameters `segment_index: int` and `rt_index: int` must be passed to the `Objective` constructor. - **TRACK_SOFT_CONTACT_FORCES** (Lagrange) — Tracks the external forces induced by soft contacts toward a target. - **TRACK_STATE** (Lagrange and Mayer) — Tracks the state variable toward a target. - **TRACK_TORQUE** (Lagrange — Tracks the generalized forces (part of the control variables) toward a target. @@ -2366,7 +2366,7 @@ definition of the constraints of the problem: ```python constraints = ConstraintList() constraints.add( -ConstraintFcn.TRACK_MARKER_WITH_SEGMENT_AXIS, node=Node.ALL, marker_idx=1, segment_idx=2, axis=Axis.X +ConstraintFcn.TRACK_MARKER_WITH_SEGMENT_AXIS, node=Node.ALL, marker_idx=1, segment_index=2, axis=Axis.X ) ``` @@ -2380,7 +2380,7 @@ the rest is fully optimized. It is designed to show how to use the tracking RT f any RT (for instance, Inertial Measurement Unit [IMU]) with a body segment. To implement this tracking task, we use the `ConstraintFcn.TRACK_SEGMENT_WITH_CUSTOM_RT` constraint function, which -minimizes the distance between a segment and an RT. The extra parameters `segment_idx: int` and `rt_idx: int` must be +minimizes the distance between a segment and an RT. The extra parameters `segment_index: int` and `rt_index: int` must be passed to the Objective constructor. ### The [track_vector_orientation.py](./bioptim/examples/track/track_vector_orientation.py) file diff --git a/bioptim/dynamics/configure_problem.py b/bioptim/dynamics/configure_problem.py index fadaf8593..d9af2ec25 100644 --- a/bioptim/dynamics/configure_problem.py +++ b/bioptim/dynamics/configure_problem.py @@ -1357,11 +1357,7 @@ def configure_soft_contact_function(ocp, nlp): A reference to the phase """ component_list = ["Mx", "My", "Mz", "Fx", "Fy", "Fz"] - - q = nlp.states["q"].mapping.to_second.map(nlp.states["q"].cx) - qdot = nlp.states["qdot"].mapping.to_second.map(nlp.states["qdot"].cx) - 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 + nlp.soft_contact_forces_func = nlp.model.soft_contact_forces() for i_sc in range(nlp.model.nb_soft_contacts): all_soft_contact_names = [] diff --git a/bioptim/examples/custom_model/custom_package/my_model.py b/bioptim/examples/custom_model/custom_package/my_model.py index 8ee6c8e4b..c737ccae3 100644 --- a/bioptim/examples/custom_model/custom_package/my_model.py +++ b/bioptim/examples/custom_model/custom_package/my_model.py @@ -6,7 +6,7 @@ from typing import Callable import numpy as np -from casadi import sin, MX +from casadi import sin, MX, Function from typing import Callable @@ -20,6 +20,9 @@ def __init__(self): # custom values for the example self.com = MX(np.array([-0.0005, 0.0688, -0.9542])) self.inertia = MX(0.0391) + self.q = MX.sym("q", 1) + self.qdot = MX.sym("qdot", 1) + self.tau = MX.sym("tau", 1) # ---- Absolutely needed methods ---- # def serialize(self) -> tuple[Callable, dict]: @@ -57,7 +60,7 @@ def mass(self): def name_dof(self): return ["rotx"] - def forward_dynamics(self, q, qdot, tau, fext=None, f_contacts=None): + def forward_dynamics(self, with_contact=False) -> Function: # This is where you can implement your own forward dynamics # with casadi it your are dealing with mechanical systems d = 0 # damping @@ -65,7 +68,11 @@ def forward_dynamics(self, q, qdot, tau, fext=None, f_contacts=None): I = self.inertia m = self.mass g = 9.81 - return 1 / (I + m * L**2) * (-qdot[0] * d - g * m * L * sin(q[0]) + tau[0]) + casadi_return = 1 / (I + m * L**2) * (-self.qdot * d - g * m * L * sin(self.q) + self.tau) + casadi_fun = Function("forward_dynamics", + [self.q, self.qdot, self.tau, MX()], + [casadi_return]) + return casadi_fun # def system_dynamics(self, *args): # This is where you can implement your system dynamics with casadi if you are dealing with other systems diff --git a/bioptim/examples/getting_started/custom_constraint.py b/bioptim/examples/getting_started/custom_constraint.py index 4f20b822f..25520f97e 100644 --- a/bioptim/examples/getting_started/custom_constraint.py +++ b/bioptim/examples/getting_started/custom_constraint.py @@ -54,7 +54,7 @@ def custom_func_track_markers(controller: PenaltyController, first_marker: str, # compute the position of the markers using the markers function from the BioModel (here a BiorbdModel) markers = controller.model.markers()(controller.states["q"].cx, controller.parameters.cx) - markers_diff = markers[marker_1_idx] - markers[marker_0_idx] + markers_diff = markers[:, marker_1_idx] - markers[:, marker_0_idx] return markers_diff diff --git a/bioptim/examples/getting_started/custom_dynamics.py b/bioptim/examples/getting_started/custom_dynamics.py index 2e53a5a30..228c211ef 100644 --- a/bioptim/examples/getting_started/custom_dynamics.py +++ b/bioptim/examples/getting_started/custom_dynamics.py @@ -74,7 +74,7 @@ def custom_dynamics( # You can directly call biorbd function (as for ddq) or call bioptim accessor (as for dq) dq = DynamicsFunctions.compute_qdot(nlp, q, qdot) * my_additional_factor - ddq = nlp.model.forward_dynamics()(q, qdot, tau, [], nlp.parameters.cx) + ddq = nlp.model.forward_dynamics()(q, qdot, tau, [], [], nlp.parameters.cx) # the user has to choose if want to return the explicit dynamics dx/dt = f(x,u,p) # as the first argument of DynamicsEvaluation or diff --git a/bioptim/examples/getting_started/custom_objectives.py b/bioptim/examples/getting_started/custom_objectives.py index d26403b22..ef585a2b2 100644 --- a/bioptim/examples/getting_started/custom_objectives.py +++ b/bioptim/examples/getting_started/custom_objectives.py @@ -53,7 +53,7 @@ def custom_func_track_markers(controller: PenaltyController, first_marker: str, # Convert the function to the required format and then subtract markers = controller.model.markers()(controller.states["q"].cx, controller.parameters.cx) - markers_diff = markers[marker_1_idx] - markers[marker_0_idx] + markers_diff = markers[:, marker_1_idx] - markers[:, marker_0_idx] return markers_diff diff --git a/bioptim/examples/moving_horizon_estimation/mhe.py b/bioptim/examples/moving_horizon_estimation/mhe.py index d992aad8e..60a3c6ea0 100644 --- a/bioptim/examples/moving_horizon_estimation/mhe.py +++ b/bioptim/examples/moving_horizon_estimation/mhe.py @@ -47,7 +47,7 @@ def states_to_markers(bio_model, states): 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 qddot_func = bio_model.forward_dynamics() diff --git a/bioptim/examples/track/track_segment_on_rt.py b/bioptim/examples/track/track_segment_on_rt.py index 319606b4a..2dfad0559 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_idx=0) + constraints.add(ConstraintFcn.TRACK_SEGMENT_WITH_CUSTOM_RT, node=Node.ALL, segment="seg_rt", rt_index=0) # Path constraint x_bounds = BoundsList() diff --git a/bioptim/limits/constraints.py b/bioptim/limits/constraints.py index 9e740f813..a16edffdb 100644 --- a/bioptim/limits/constraints.py +++ b/bioptim/limits/constraints.py @@ -420,7 +420,7 @@ def qddot_equals_forward_dynamics( qddot = controller.controls["qddot"].cx if "qddot" in controller.controls else controller.states["qddot"].cx # TODO: add external_forces qddot_fd = controller.model.forward_dynamics(with_contact=with_contact)( - controller.q, controller.qdot, tau, [], controller.parameters.cx + controller.q, controller.qdot, tau, [], [], controller.parameters.cx ) return qddot - qddot_fd @@ -474,15 +474,14 @@ def tau_equals_inverse_dynamics( if with_contact: # todo: this should be done internally in BiorbdModel - # Charbie: todo f_contact = ( controller.controls["fext"].cx if "fext" in controller.controls else controller.states["fext"].cx ) - f_contact_vec = controller.model.reshape_fext_to_fcontact(f_contact) + f_contact_vec = controller.model.reshape_fext_to_fcontact(f_contact, controller.parameters.cx) else: f_contact_vec = [] tau_id = controller.model.inverse_dynamics(with_contact=with_contact)( - controller.q, controller.qdot, qddot, f_contact_vec, controller.parameters.cx + controller.q, controller.qdot, qddot, f_contact_vec, [], controller.parameters.cx ) var = [] @@ -515,7 +514,7 @@ def implicit_marker_acceleration( # TODO get the index of the marker contact_acceleration = controller.model.rigid_contact_acceleration(contact_index, contact_axis)( - controller.q, controller.qdot, qddot + controller.q, controller.qdot, qddot, controller.parameters.cx ) return contact_acceleration diff --git a/bioptim/limits/penalty.py b/bioptim/limits/penalty.py index 20f5fe347..51740900e 100644 --- a/bioptim/limits/penalty.py +++ b/bioptim/limits/penalty.py @@ -867,9 +867,8 @@ def minimize_soft_contact_forces( force_idx.append(4 + (6 * i_sc)) force_idx.append(5 + (6 * i_sc)) soft_contact_force = controller.get_nlp.soft_contact_forces_func( - controller.time.cx, - controller.states.cx_start, - controller.controls.cx_start, + controller.q, + controller.qdot, controller.parameters.cx, ) return soft_contact_force[force_idx] @@ -879,7 +878,7 @@ def track_segment_with_custom_rt( penalty: PenaltyOption, controller: PenaltyController, segment: int | str, - rt_idx: int, + rt_index: int, sequence: str = "zyx", ): """ @@ -894,28 +893,24 @@ def track_segment_with_custom_rt( The penalty node elements segment: int | str The name or index of the segment - rt_idx: int + rt_index: int The index of the RT in the bioMod sequence: str The sequence of the euler angles (default="zyx") """ - from ..models.biorbd.biorbd_model import BiorbdModel penalty.quadratic = True if penalty.quadratic is None else penalty.quadratic segment_index = controller.model.segment_index(segment) if isinstance(segment, str) else segment - if not isinstance(controller.model, BiorbdModel): - 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)( + r_seg = controller.model.homogeneous_matrices_in_global(segment_index=segment_index)( controller.q, controller.parameters.cx - )[:3, :3].T - r_rt = controller.model.rt(rt_idx)(controller.q, controller.parameters.cx)[:3, :3] - # @Pariterre: why was this sequence is fixed? + )[:3, :3] + r_seg_transposed = r_seg.T + r_rt = controller.model.rt(rt_index=rt_index)(controller.q, controller.parameters.cx)[:3, :3] + # @Pariterre: this is suspicious and it breaks the tests! - angles_diff = controller.model.rotation_matrix_to_euler_angles(sequence)(r_seg_transposed * r_rt) + angles_diff = controller.model.rotation_matrix_to_euler_angles(sequence=sequence)(r_seg_transposed * r_rt) return angles_diff @@ -951,10 +946,10 @@ def track_marker_with_segment_axis( penalty.quadratic = True if penalty.quadratic is None else penalty.quadratic marker_idx = controller.model.marker_index(marker) if isinstance(marker, str) else marker - segment_idx = controller.model.segment_index(segment) if isinstance(segment, str) else segment + segment_index = 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, controller.parameters.cx) + # Get the marker in segment_index reference frame + marker = controller.model.marker(marker_idx, segment_index)(controller.q, controller.parameters.cx) # To align an axis, the other must be equal to 0 if not penalty.rows_is_set: @@ -999,12 +994,12 @@ def minimize_segment_rotation( penalty.quadratic = True if penalty.quadratic is None else penalty.quadratic - segment_idx = controller.model.segment_index(segment) if isinstance(segment, str) else segment + segment_index = controller.model.segment_index(segment) if isinstance(segment, str) else segment 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)( + jcs_segment = controller.model.homogeneous_matrices_in_global(segment_index)( controller.q, controller.parameters.cx )[:3, :3] angles_segment = controller.model.rotation_matrix_to_euler_angles(sequence)(jcs_segment) @@ -1044,14 +1039,14 @@ def minimize_segment_velocity( penalty.quadratic = True if penalty.quadratic is None else penalty.quadratic - segment_idx = controller.model.segment_index(segment) if isinstance(segment, str) else segment + segment_index = controller.model.segment_index(segment) if isinstance(segment, str) else segment if not isinstance(controller.model, BiorbdModel): raise NotImplementedError( "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)( + segment_angular_velocity = model.segment_angular_velocity(segment_index)( controller.q, controller.qdot, controller.parameters.cx ) @@ -1120,16 +1115,16 @@ def track_vector_orientations_from_markers( else vector_1_marker_1 ) - vector_0_marker_0_position = controller.model.marker(vector_0_marker_0_idx)( + vector_0_marker_0_position = controller.model.marker(index=vector_0_marker_0_idx)( controller.q, controller.parameters.cx ) - vector_0_marker_1_position = controller.model.marker(vector_0_marker_1_idx)( + vector_0_marker_1_position = controller.model.marker(index=vector_0_marker_1_idx)( controller.q, controller.parameters.cx ) - vector_1_marker_0_position = controller.model.marker(vector_1_marker_0_idx)( + vector_1_marker_0_position = controller.model.marker(index=vector_1_marker_0_idx)( controller.q, controller.parameters.cx ) - vector_1_marker_1_position = controller.model.marker(vector_1_marker_1_idx)( + vector_1_marker_1_position = controller.model.marker(index=vector_1_marker_1_idx)( controller.q, controller.parameters.cx ) diff --git a/bioptim/models/biorbd/biorbd_model.py b/bioptim/models/biorbd/biorbd_model.py index 15dccad48..e64259995 100644 --- a/bioptim/models/biorbd/biorbd_model.py +++ b/bioptim/models/biorbd/biorbd_model.py @@ -139,12 +139,14 @@ def nb_root(self) -> int: def segments(self) -> tuple[biorbd.Segment]: return self.model.segments() - def rotation_matrix_to_euler_angles(self, sequence) -> Function: + def rotation_matrix_to_euler_angles(self, sequence: str) -> Function: """ Returns the rotation matrix to euler angles function. """ r = MX.sym("r_mx", 3, 3) - r_matrix = biorbd.Rotation(r[0, 0], r[0, 1], r[0, 2], r[1, 0], r[1, 1], r[1, 2], r[2, 0], r[2, 1], r[2, 2]) + r_matrix = biorbd.Rotation(r[0, 0], r[0, 1], r[0, 2], + r[1, 0], r[1, 1], r[1, 2], + r[2, 0], r[2, 1], r[2, 2]) biorbd_return = biorbd.Rotation.toEulerAngles(r_matrix, sequence).to_mx() casadi_fun = Function( "rotation_matrix_to_euler_angles", @@ -153,12 +155,13 @@ def rotation_matrix_to_euler_angles(self, sequence) -> Function: ) return casadi_fun - def homogeneous_matrices_in_global(self, segment_idx, inverse=False) -> Function: + def homogeneous_matrices_in_global(self, segment_index: int, inverse=False) -> Function: """ Returns the roto-translation matrix of the segment in the global reference frame. """ - jcs = self.model.globalJCS(GeneralizedCoordinates(self.q), segment_idx).to_mx() - biorbd_return = jcs.T if inverse else jcs + q_biorbd = GeneralizedCoordinates(self.q) + jcs = self.model.globalJCS(q_biorbd, segment_index) + biorbd_return = jcs.transpose().to_mx() if inverse else jcs.to_mx() casadi_fun = Function( "homogeneous_matrices_in_global", [self.q, self.parameters], @@ -308,26 +311,13 @@ def segment_angular_velocity(self, idx) -> Function: ) return casadi_fun - def segment_orientation(self, idx) -> Function: + def segment_orientation(self, idx: int, sequence: str = "xyz") -> Function: """ Returns the angular position of the segment in the global reference frame. """ q_biorbd = GeneralizedCoordinates(self.q) - rotation_matrix = self.homogeneous_matrices_in_global(q_biorbd, idx)[:3, :3] - biorbd_return = biorbd.Rotation.toEulerAngles( - biorbd.Rotation( - rotation_matrix[0, 0], - rotation_matrix[0, 1], - rotation_matrix[0, 2], - rotation_matrix[1, 0], - rotation_matrix[1, 1], - rotation_matrix[1, 2], - rotation_matrix[2, 0], - rotation_matrix[2, 1], - rotation_matrix[2, 2], - ), - "xyz", - ).to_mx() + rotation_matrix = self.homogeneous_matrices_in_global(idx)(q_biorbd, self.parameters)[:3, :3] + biorbd_return = self.rotation_matrix_to_euler_angles(sequence=sequence)(rotation_matrix) casadi_fun = Function( "segment_orientation", [self.q, self.parameters], @@ -464,9 +454,8 @@ def forward_dynamics(self, with_contact: bool = False) -> Function: return casadi_fun def inverse_dynamics(self, with_contact: bool = False) -> Function: - # @ipuch: I do not understand what is happening here? Do we have f_ext or it is just the contact forces? if with_contact: - external_forces = self.reshape_fext_to_fcontact(self.external_forces) + external_forces = self.reshape_fext_to_fcontact(self.external_forces, self.parameters) else: external_forces = self.external_forces @@ -574,12 +563,14 @@ def nb_markers(self) -> int: def marker_index(self, name): return biorbd.marker_index(self.model, name) - def marker(self, index, reference_segment_index=None) -> Function: - marker = self.model.marker(GeneralizedCoordinates(self.q), index) + def marker(self, index: int, reference_segment_index: int = None) -> Function: + q_biorbd = GeneralizedCoordinates(self.q) + marker = self.model.marker(q_biorbd, index) if reference_segment_index is not None: - global_homogeneous_matrix = self.model.globalJCS(GeneralizedCoordinates(self.q), reference_segment_index) - marker.applyRT(global_homogeneous_matrix.transpose()) - biorbd_return = marker.to_mx() + global_homogeneous_matrix = self.model.globalJCS(q_biorbd, reference_segment_index) + biorbd_return = global_homogeneous_matrix.transpose().to_mx() @ vertcat(marker.to_mx(), 1) + else: + biorbd_return = marker.to_mx() casadi_fun = Function( "marker", [self.q, self.parameters], @@ -633,14 +624,15 @@ def markers_velocities(self, reference_index=None) -> list[MX]: else: biorbd_return = [] homogeneous_matrix_transposed = self.homogeneous_matrices_in_global( - segment_idx=reference_index, inverse=True + segment_index=reference_index, inverse=True )( GeneralizedCoordinates(self.q), ) - # TODO: Check and fix this portion of code for m in self.model.markersVelocity(GeneralizedCoordinates(self.q), GeneralizedVelocity(self.qdot)): if m.applyRT(homogeneous_matrix_transposed) is None: biorbd_return.append(m.to_mx()) + else: + biorbd_return.append(m.applyRT(homogeneous_matrix_transposed).to_mx()) casadi_fun = Function( "markers_velocities", @@ -678,7 +670,7 @@ def markers_accelerations(self, reference_index=None) -> list[MX]: # TODO: Check and fix this portion of code biorbd_return = [] homogeneous_matrix_transposed = self.homogeneous_matrices_in_global( - segment_idx=reference_index, + segment_index=reference_index, inverse=True, )( GeneralizedCoordinates(self.q), @@ -690,6 +682,8 @@ def markers_accelerations(self, reference_index=None) -> list[MX]: ): if m.applyRT(homogeneous_matrix_transposed) is None: biorbd_return.append(m.to_mx()) + else: + biorbd_return.append(m.applyRT(homogeneous_matrix_transposed).to_mx()) casadi_fun = Function( "markers_accelerations", @@ -770,7 +764,7 @@ def soft_contact_forces(self) -> Function: ) return casadi_fun - def reshape_fext_to_fcontact(self, fext: MX | SX) -> list: + def reshape_fext_to_fcontact(self, fext: MX | SX, parameters: MX | SX) -> list: if len(self._segments_to_apply_external_forces) == 0: parent_name = [] for i in range(self.nb_rigid_contacts): @@ -780,24 +774,34 @@ def reshape_fext_to_fcontact(self, fext: MX | SX) -> list: ] self._segments_to_apply_external_forces = parent_name - fext_sym = MX.sym("Fext", fext.shape[0], fext.shape[1]) + fext_sym = MX.sym("Fext", 9, fext.shape[1]) count = 0 f_contact_vec = MX() for i in range(self.nb_rigid_contacts): contact = self.model.rigidContact(i) - tp = MX.zeros(6) + tp = MX.zeros(9) used_axes = [i for i, val in enumerate(contact.axes()) if val] n_contacts = len(used_axes) tp[used_axes] = fext_sym[count : count + n_contacts] - tp[3:] = contact.to_mx() + tp[3:6] = contact.to_mx() f_contact_vec = horzcat(f_contact_vec, tp) count += n_contacts + fext_reshaped = type(fext).zeros(9, fext.shape[1]) + if fext.shape[0] == 6: + fext_reshaped[:6, :] = fext + elif fext.shape[0] == 3: + fext_reshaped[3:6, :] = fext + elif fext.shape[0] == 9: + fext_reshaped = fext[:, :] + else: + raise ValueError("fext must be of size 3, 6 or 9") + casadi_fun_evaluated = Function( "reshape_fext_to_fcontact", [fext_sym, self.parameters], [f_contact_vec], - )(fext) + )(fext_reshaped, parameters) return casadi_fun_evaluated def normalize_state_quaternions(self) -> Function: @@ -819,7 +823,7 @@ def normalize_state_quaternions(self) -> Function: biorbd_return[quat_idx[j][3]] = quaternion[0] casadi_fun = Function( - "soft_contact_forces", + "normalize_state_quaternions", [self.q], [biorbd_return], ) diff --git a/bioptim/models/biorbd/multi_biorbd_model.py b/bioptim/models/biorbd/multi_biorbd_model.py index 25fb4bf6c..4690bb089 100644 --- a/bioptim/models/biorbd/multi_biorbd_model.py +++ b/bioptim/models/biorbd/multi_biorbd_model.py @@ -300,8 +300,8 @@ def segments(self) -> tuple[biorbd.Segment, ...]: out += (seg,) return out - def homogeneous_matrices_in_global(self, segment_idx, inverse=False) -> Function: - local_segment_id, model_id = self.local_variable_id("segment", segment_idx) + def homogeneous_matrices_in_global(self, segment_index, inverse=False) -> Function: + local_segment_id, model_id = self.local_variable_id("segment", segment_index) q_model = self.models[model_id].q biorbd_return = self.models[model_id].homogeneous_matrices_in_global(local_segment_id, inverse)( q_model, self.parameters @@ -568,12 +568,13 @@ def forward_dynamics(self, with_contact) -> Function: qdot_model, tau_model, [], + [], self.parameters, ), ) casadi_fun = Function( "forward_dynamics", - [self.q, self.qdot, self.tau, [], self.parameters], + [self.q, self.qdot, self.tau, [], [], self.parameters], [biorbd_return], ) return casadi_fun @@ -588,11 +589,11 @@ def inverse_dynamics(self) -> Function: qddot_model = self.qddot[self.variable_index("qddot", i)] biorbd_return = vertcat( biorbd_return, - model.inverse_dynamics()(q_model, qdot_model, qddot_model, [], self.parameters), + model.inverse_dynamics()(q_model, qdot_model, qddot_model, [], [], self.parameters), ) casadi_fun = Function( "inverse_dynamics", - [self.q, self.qdot, self.qddot, [], self.parameters], + [self.q, self.qdot, self.qddot, [], [], self.parameters], [biorbd_return], ) return casadi_fun diff --git a/bioptim/models/holonomic_constraints.py b/bioptim/models/holonomic_constraints.py index b1749adcd..7260adbc7 100644 --- a/bioptim/models/holonomic_constraints.py +++ b/bioptim/models/holonomic_constraints.py @@ -63,7 +63,7 @@ def superimpose_markers( # if local frame is provided, the markers are expressed in the same local frame if local_frame_index is not None: - jcs_t = biorbd_model.homogeneous_matrices_in_global(segment_idx=local_frame_index, inverse=True)(q_sym, []) + jcs_t = biorbd_model.homogeneous_matrices_in_global(segment_index=local_frame_index, inverse=True)(q_sym, []) marker_1_sym = (jcs_t @ vertcat(marker_1_sym, 1))[:3] marker_2_sym = (jcs_t @ vertcat(marker_2_sym, 1))[:3] diff --git a/bioptim/models/protocols/biomodel.py b/bioptim/models/protocols/biomodel.py index 7cf646a12..e341fed30 100644 --- a/bioptim/models/protocols/biomodel.py +++ b/bioptim/models/protocols/biomodel.py @@ -85,13 +85,10 @@ def segments(self) -> tuple: """Get all segments""" return () - def homogeneous_matrices_in_child(self, segment_id) -> Function: + def rotation_matrix_to_euler_angles(self, sequence: str) -> tuple: """ - Get the homogeneous matrices of one segment in its parent frame, - such as: P_R1 = T_R1_R2 * P_R2 - with P_R1 the position of any point P in the segment R1 frame, - with P_R2 the position of any point P in the segment R2 frame, - T_R1_R2 the homogeneous matrix that transform any point in R2 frame to R1 frame. + Get the Euler angles from a rotation matrix, in the sequence specified + args: rotation matrix """ @property @@ -99,7 +96,7 @@ def mass(self) -> Function: """Get the mass of the model""" return Function("F", [], []) - def rt(self, rt_idx) -> Function: + def rt(self, rt_index) -> Function: """ Get the rototrans matrix of an object (e.g., an IMU) that is placed on the model args: q diff --git a/bioptim/optimization/optimization_variable.py b/bioptim/optimization/optimization_variable.py index 352728dd2..c59e05660 100644 --- a/bioptim/optimization/optimization_variable.py +++ b/bioptim/optimization/optimization_variable.py @@ -287,7 +287,10 @@ def append(self, name: str, cx: list, bimapping: BiMapping): raise NotImplementedError("cx should be of dimension 2 (start, [mid], end)") index = range(self._cx_start.shape[0], self._cx_start.shape[0] + cx[0].shape[0]) - self._cx_start = vertcat(self._cx_start, cx[0]) + if self._cx_start.shape[0] == 0: + self._cx_start = cx[0] + else: + self._cx_start = vertcat(self._cx_start, cx[0]) if len(cx) > 2: self._cx_mid = vertcat(self._cx_mid, cx[(len(cx) - 1) // 2]) self._cx_end = vertcat(self._cx_end, cx[-1]) diff --git a/tests/shard1/test_biorbd_multi_model.py b/tests/shard1/test_biorbd_multi_model.py index 0f7c26088..8da733076 100644 --- a/tests/shard1/test_biorbd_multi_model.py +++ b/tests/shard1/test_biorbd_multi_model.py @@ -106,7 +106,7 @@ def test_biorbd_model(): assert isinstance(models.segments[0], biorbd.biorbd.Segment) TestUtils.assert_equal( - models.homogeneous_matrices_in_global(segment_idx=0, inverse=False)(np.array([1, 2, 3]), []), + models.homogeneous_matrices_in_global(segment_index=0, inverse=False)(np.array([1, 2, 3]), []), np.array( [ [1.0, 0.0, 0.0, 0.0], diff --git a/tests/shard3/test_global_torque_driven_ocp.py b/tests/shard3/test_global_torque_driven_ocp.py index 94b8e1f47..1b4ecb075 100644 --- a/tests/shard3/test_global_torque_driven_ocp.py +++ b/tests/shard3/test_global_torque_driven_ocp.py @@ -679,9 +679,6 @@ def test_example_multi_biorbd_model(phase_dynamics): from bioptim.examples.torque_driven_ocp import example_multi_biorbd_model as ocp_module bioptim_folder = os.path.dirname(ocp_module.__file__) - - # Define the problem - biorbd_model_path = bioptim_folder + "/models/triple_pendulum.bioMod" biorbd_model_path_modified_inertia = bioptim_folder + "/models/triple_pendulum_modified_inertia.bioMod" diff --git a/tests/shard4/test_penalty.py b/tests/shard4/test_penalty.py index 9dcedee31..77d617d72 100644 --- a/tests/shard4/test_penalty.py +++ b/tests/shard4/test_penalty.py @@ -52,7 +52,7 @@ def prepare_test_ocp( elif with_contact: bio_model = BiorbdModel( bioptim_folder + "/examples/muscle_driven_with_contact/models/2segments_4dof_2contacts_1muscle.bioMod", - # segments_to_apply_external_forces=["", ""], # TODO: Charbie add the proper segments + segments_to_apply_external_forces=["Seg1", "Seg1"], ) dynamics = DynamicsList() rigidbody_dynamics = RigidBodyDynamics.DAE_INVERSE_DYNAMICS if implicit else RigidBodyDynamics.ODE @@ -952,6 +952,7 @@ def test_penalty_minimize_comddot(value, penalty_origin, implicit, phase_dynamic else: res = get_penalty_value(ocp, penalty, t, phases_dt, x, u, [], [], []) + expected = np.array([[0], [-0.0008324], [0.002668]]) expected = np.array([[0], [-0.0008324], [0.002668]]) if value == -10: expected = np.array([[0], [-17.5050533], [-18.2891901]]) @@ -975,9 +976,9 @@ def test_penalty_track_segment_with_custom_rt(penalty_origin, value, phase_dynam penalty_type = penalty_origin.TRACK_SEGMENT_WITH_CUSTOM_RT if isinstance(penalty_type, (ObjectiveFcn.Lagrange, ObjectiveFcn.Mayer)): - penalty = Objective(penalty_type, segment="ground", rt_idx=0) + penalty = Objective(penalty_type, segment="ground", rt_index=0) else: - penalty = Constraint(penalty_type, segment="ground", rt_idx=0) + penalty = Constraint(penalty_type, segment="ground", rt_index=0) res = get_penalty_value(ocp, penalty, t, phases_dt, x, u, p, a, d) expected = np.array([[0], [0.1], [0]]) From 073cdf31dd6b5dc5ae0e316aee24484aa7bde73f Mon Sep 17 00:00:00 2001 From: eve-mac Date: Thu, 26 Sep 2024 21:05:44 -0400 Subject: [PATCH 036/108] blacked --- bioptim/examples/custom_model/custom_package/my_model.py | 4 +--- bioptim/models/biorbd/biorbd_model.py | 4 +--- bioptim/models/holonomic_constraints.py | 4 +++- 3 files changed, 5 insertions(+), 7 deletions(-) diff --git a/bioptim/examples/custom_model/custom_package/my_model.py b/bioptim/examples/custom_model/custom_package/my_model.py index c737ccae3..8c80bbef8 100644 --- a/bioptim/examples/custom_model/custom_package/my_model.py +++ b/bioptim/examples/custom_model/custom_package/my_model.py @@ -69,9 +69,7 @@ def forward_dynamics(self, with_contact=False) -> Function: m = self.mass g = 9.81 casadi_return = 1 / (I + m * L**2) * (-self.qdot * d - g * m * L * sin(self.q) + self.tau) - casadi_fun = Function("forward_dynamics", - [self.q, self.qdot, self.tau, MX()], - [casadi_return]) + casadi_fun = Function("forward_dynamics", [self.q, self.qdot, self.tau, MX()], [casadi_return]) return casadi_fun # def system_dynamics(self, *args): diff --git a/bioptim/models/biorbd/biorbd_model.py b/bioptim/models/biorbd/biorbd_model.py index e64259995..400f5f53f 100644 --- a/bioptim/models/biorbd/biorbd_model.py +++ b/bioptim/models/biorbd/biorbd_model.py @@ -144,9 +144,7 @@ def rotation_matrix_to_euler_angles(self, sequence: str) -> Function: Returns the rotation matrix to euler angles function. """ r = MX.sym("r_mx", 3, 3) - r_matrix = biorbd.Rotation(r[0, 0], r[0, 1], r[0, 2], - r[1, 0], r[1, 1], r[1, 2], - r[2, 0], r[2, 1], r[2, 2]) + r_matrix = biorbd.Rotation(r[0, 0], r[0, 1], r[0, 2], r[1, 0], r[1, 1], r[1, 2], r[2, 0], r[2, 1], r[2, 2]) biorbd_return = biorbd.Rotation.toEulerAngles(r_matrix, sequence).to_mx() casadi_fun = Function( "rotation_matrix_to_euler_angles", diff --git a/bioptim/models/holonomic_constraints.py b/bioptim/models/holonomic_constraints.py index 7260adbc7..c11815051 100644 --- a/bioptim/models/holonomic_constraints.py +++ b/bioptim/models/holonomic_constraints.py @@ -63,7 +63,9 @@ def superimpose_markers( # if local frame is provided, the markers are expressed in the same local frame if local_frame_index is not None: - jcs_t = biorbd_model.homogeneous_matrices_in_global(segment_index=local_frame_index, inverse=True)(q_sym, []) + jcs_t = biorbd_model.homogeneous_matrices_in_global(segment_index=local_frame_index, inverse=True)( + q_sym, [] + ) marker_1_sym = (jcs_t @ vertcat(marker_1_sym, 1))[:3] marker_2_sym = (jcs_t @ vertcat(marker_2_sym, 1))[:3] From 7673fa0a9f21fa96a5a19d8b175311aefd235055 Mon Sep 17 00:00:00 2001 From: eve-mac Date: Fri, 27 Sep 2024 09:15:39 -0400 Subject: [PATCH 037/108] named the in and out in Function --- bioptim/models/biorbd/biorbd_model.py | 87 ++++++++++++++++++++++++++- 1 file changed, 86 insertions(+), 1 deletion(-) diff --git a/bioptim/models/biorbd/biorbd_model.py b/bioptim/models/biorbd/biorbd_model.py index 400f5f53f..b7c0fb410 100644 --- a/bioptim/models/biorbd/biorbd_model.py +++ b/bioptim/models/biorbd/biorbd_model.py @@ -93,6 +93,8 @@ def gravity(self) -> Function: "gravity", [self.parameters], [biorbd_return], + ["parameters"], + ["gravity"], ) return casadi_fun @@ -150,6 +152,8 @@ def rotation_matrix_to_euler_angles(self, sequence: str) -> Function: "rotation_matrix_to_euler_angles", [r], [biorbd_return], + ["Rotation matrix"], + ["Euler angles"], ) return casadi_fun @@ -164,6 +168,8 @@ def homogeneous_matrices_in_global(self, segment_index: int, inverse=False) -> F "homogeneous_matrices_in_global", [self.q, self.parameters], [biorbd_return], + ["q", "parameters"], + ["Joint coordinate system RT matrix in global"], ) return casadi_fun @@ -178,6 +184,8 @@ def homogeneous_matrices_in_child(self, segment_id) -> Function: "homogeneous_matrices_in_child", [self.parameters], [biorbd_return], + ["parameters"], + ["Joint coordinate system RT matrix in local"], ) return casadi_fun @@ -193,6 +201,8 @@ def mass(self) -> Function: "mass", [self.parameters], [biorbd_return], + ["parameters"], + ["mass"], ) return casadi_fun @@ -203,6 +213,8 @@ def rt(self, rt_index) -> Function: "rt", [self.q, self.parameters], [biorbd_return], + ["q", "parameters"], + ["RT matrix"], ) return casadi_fun @@ -213,6 +225,8 @@ def center_of_mass(self) -> Function: "center_of_mass", [self.q, self.parameters], [biorbd_return], + ["q", "parameters"], + ["Center of mass"], ) return casadi_fun @@ -224,6 +238,8 @@ def center_of_mass_velocity(self) -> Function: "center_of_mass_velocity", [self.q, self.qdot, self.parameters], [biorbd_return], + ["q", "qdot", "parameters"], + ["Center of mass velocity"], ) return casadi_fun @@ -236,6 +252,8 @@ def center_of_mass_acceleration(self) -> Function: "center_of_mass_acceleration", [self.q, self.qdot, self.qddot, self.parameters], [biorbd_return], + ["q", "qdot", "qddot", "parameters"], + ["Center of mass acceleration"], ) return casadi_fun @@ -247,6 +265,8 @@ def body_rotation_rate(self) -> Function: "body_rotation_rate", [self.q, self.qdot, self.parameters], [biorbd_return], + ["q", "qdot", "parameters"], + ["Body rotation rate"], ) return casadi_fun @@ -257,6 +277,8 @@ def mass_matrix(self) -> Function: "mass_matrix", [self.q, self.parameters], [biorbd_return], + ["q", "parameters"], + ["Mass matrix"], ) return casadi_fun @@ -268,6 +290,8 @@ def non_linear_effects(self) -> Function: "non_linear_effects", [self.q, self.qdot, self.parameters], [biorbd_return], + ["q", "qdot", "parameters"], + ["Non linear effects"], ) return casadi_fun @@ -279,6 +303,8 @@ def angular_momentum(self) -> Function: "angular_momentum", [self.q, self.qdot, self.parameters], [biorbd_return], + ["q", "qdot", "parameters"], + ["Angular momentum"], ) return casadi_fun @@ -292,6 +318,8 @@ def reshape_qdot(self, k_stab=1) -> Function: "reshape_qdot", [self.q, self.qdot, self.parameters], [biorbd_return], + ["q", "qdot", "parameters"], + ["Reshaped qdot"], ) return casadi_fun @@ -306,6 +334,8 @@ def segment_angular_velocity(self, idx) -> Function: "segment_angular_velocity", [self.q, self.qdot, self.parameters], [biorbd_return], + ["q", "qdot", "parameters"], + ["Segment angular velocity"], ) return casadi_fun @@ -320,6 +350,8 @@ def segment_orientation(self, idx: int, sequence: str = "xyz") -> Function: "segment_orientation", [self.q, self.parameters], [biorbd_return], + ["q", "parameters"], + ["Segment orientation"], ) return casadi_fun @@ -363,6 +395,8 @@ def torque(self) -> Function: "torque_activation", [self.tau, self.q, self.qdot, self.parameters], [biorbd_return], + ["tau", "q", "qdot", "parameters"], + ["Torque from tau activations"], ) return casadi_fun @@ -375,6 +409,8 @@ def forward_dynamics_free_floating_base(self) -> Function: "forward_dynamics_free_floating_base", [self.q, self.qdot, self.qddot_joints, self.parameters], [biorbd_return], + ["q", "qdot", "qddot_joints", "parameters"], + ["qddot_root and qddot_joints"], ) return casadi_fun @@ -441,6 +477,8 @@ def forward_dynamics(self, with_contact: bool = False) -> Function: "constrained_forward_dynamics", [self.q, self.qdot, self.tau, self.external_forces, self.translational_forces, self.parameters], [biorbd_return], + ["q", "qdot", "tau", "external_forces", "translational_forces", "parameters"], + ["qddot"], ) else: biorbd_return = self.model.ForwardDynamics(q_biorbd, qdot_biorbd, tau_biorbd, external_forces_set).to_mx() @@ -448,6 +486,8 @@ def forward_dynamics(self, with_contact: bool = False) -> Function: "forward_dynamics", [self.q, self.qdot, self.tau, self.external_forces, self.translational_forces, self.parameters], [biorbd_return], + ["q", "qdot", "tau", "external_forces", "translational_forces", "parameters"], + ["qddot"], ) return casadi_fun @@ -467,6 +507,8 @@ def inverse_dynamics(self, with_contact: bool = False) -> Function: "inverse_dynamics", [self.q, self.qdot, self.qddot, self.external_forces, self.translational_forces, self.parameters], [biorbd_return], + ["q", "qdot", "qddot", "external_forces", "translational_forces", "parameters"], + ["tau"], ) return casadi_fun @@ -483,6 +525,8 @@ def contact_forces_from_constrained_forward_dynamics(self) -> Function: "contact_forces_from_constrained_forward_dynamics", [self.q, self.qdot, self.tau, self.external_forces, self.translational_forces, self.parameters], [biorbd_return], + ["q", "qdot", "tau", "external_forces", "translational_forces", "parameters"], + ["contact_forces"], ) return casadi_fun @@ -494,6 +538,8 @@ def qdot_from_impact(self) -> Function: "qdot_from_impact", [self.q, self.qdot, self.parameters], [biorbd_return], + ["q", "qdot", "parameters"], + ["qdot post impact"], ) return casadi_fun @@ -507,6 +553,8 @@ def muscle_activation_dot(self) -> Function: "muscle_activation_dot", [self.muscle, self.activations, self.parameters], [biorbd_return], + ["muscle_excitation", "muscle_activation", "parameters"], + ["muscle_activation_dot"], ) return casadi_fun @@ -517,6 +565,8 @@ def muscle_length_jacobian(self) -> Function: "muscle_length_jacobian", [self.q, self.parameters], [biorbd_return], + ["q", "parameters"], + ["muscle_length_jacobian"], ) return casadi_fun @@ -527,6 +577,8 @@ def muscle_velocity(self) -> Function: "muscle_velocity", [self.q, self.qdot, self.parameters], [biorbd_return], + ["q", "qdot", "parameters"], + ["muscle_velocity"], ) return casadi_fun @@ -542,6 +594,8 @@ def muscle_joint_torque(self) -> Function: "muscle_joint_torque", [self.muscle, self.q, self.qdot, self.parameters], [biorbd_return], + ["muscle_activation", "q", "qdot", "parameters"], + ["muscle_joint_torque"], ) return casadi_fun @@ -551,6 +605,8 @@ def markers(self) -> list[MX]: "markers", [self.q, self.parameters], [biorbd_return], + ["q", "parameters"], + ["markers"], ) return casadi_fun @@ -573,6 +629,8 @@ def marker(self, index: int, reference_segment_index: int = None) -> Function: "marker", [self.q, self.parameters], [biorbd_return], + ["q", "parameters"], + ["marker"], ) return casadi_fun @@ -636,6 +694,8 @@ def markers_velocities(self, reference_index=None) -> list[MX]: "markers_velocities", [self.q, self.qdot, self.parameters], biorbd_return, + ["q", "qdot", "parameters"], + ["markers_velocities"], ) return casadi_fun @@ -649,6 +709,8 @@ def marker_velocity(self, marker_index: int) -> list[MX]: "marker_velocity", [self.q, self.qdot, self.parameters], [biorbd_return], + ["q", "qdot", "parameters"], + ["marker_velocity"], ) return casadi_fun @@ -665,7 +727,6 @@ def markers_accelerations(self, reference_index=None) -> list[MX]: ] else: - # TODO: Check and fix this portion of code biorbd_return = [] homogeneous_matrix_transposed = self.homogeneous_matrices_in_global( segment_index=reference_index, @@ -687,6 +748,8 @@ def markers_accelerations(self, reference_index=None) -> list[MX]: "markers_accelerations", [self.q, self.qdot, self.qddot, self.parameters], biorbd_return, + ["q", "qdot", "qddot", "parameters"], + ["markers_accelerations"], ) return casadi_fun @@ -701,6 +764,8 @@ def marker_acceleration(self, marker_index: int) -> list[MX]: "marker_acceleration", [self.q, self.qdot, self.qddot, self.parameters], [biorbd_return], + ["q", "qdot", "qddot", "parameters"], + ["marker_acceleration"], ) return casadi_fun @@ -713,6 +778,8 @@ def tau_max(self) -> tuple[MX, MX]: "tau_max", [self.q, self.qdot, self.parameters], [torque_max.to_mx(), torque_min.to_mx()], + ["q", "qdot", "parameters"], + ["tau_max", "tau_min"], ) return casadi_fun @@ -727,6 +794,8 @@ def rigid_contact_acceleration(self, contact_index, contact_axis) -> Function: "rigid_contact_acceleration", [self.q, self.qdot, self.qddot, self.parameters], [biorbd_return], + ["q", "qdot", "qddot", "parameters"], + ["rigid_contact_acceleration"], ) return casadi_fun @@ -736,6 +805,8 @@ def markers_jacobian(self) -> list[MX]: "markers_jacobian", [self.q, self.parameters], biorbd_return, + ["q", "parameters"], + ["markers_jacobian"], ) return casadi_fun @@ -759,6 +830,8 @@ def soft_contact_forces(self) -> Function: "soft_contact_forces", [self.q, self.qdot, self.parameters], [biorbd_return], + ["q", "qdot", "parameters"], + ["soft_contact_forces"], ) return casadi_fun @@ -799,6 +872,8 @@ def reshape_fext_to_fcontact(self, fext: MX | SX, parameters: MX | SX) -> list: "reshape_fext_to_fcontact", [fext_sym, self.parameters], [f_contact_vec], + ["external_forces", "parameters"], + ["contact_forces"], )(fext_reshaped, parameters) return casadi_fun_evaluated @@ -824,6 +899,8 @@ def normalize_state_quaternions(self) -> Function: "normalize_state_quaternions", [self.q], [biorbd_return], + ["q"], + ["q_normalized"], ) return casadi_fun @@ -846,6 +923,8 @@ def contact_forces(self) -> Function: "contact_forces", [self.q, self.qdot, self.tau, self.external_forces, self.translational_forces, self.parameters], [force], + ["q", "qdot", "tau", "external_forces", "translational_forces", "parameters"], + ["contact_forces"], ) return casadi_fun @@ -857,6 +936,8 @@ def passive_joint_torque(self) -> Function: "passive_joint_torque", [self.q, self.qdot, self.parameters], [biorbd_return], + ["q", "qdot", "parameters"], + ["passive_joint_torque"], ) return casadi_fun @@ -868,6 +949,8 @@ def ligament_joint_torque(self) -> Function: "ligament_joint_torque", [self.q, self.qdot, self.parameters], [biorbd_return], + ["q", "qdot", "parameters"], + ["ligament_joint_torque"], ) return casadi_fun @@ -936,6 +1019,8 @@ def lagrangian(self) -> Function: "lagrangian", [self.q, self.qdot], [biorbd_return], + ["q", "qdot"], + ["lagrangian"], ) return casadi_fun From 4e1c11aea847a767750b5d818e749ef11fca644f Mon Sep 17 00:00:00 2001 From: eve-mac Date: Fri, 27 Sep 2024 10:36:50 -0400 Subject: [PATCH 038/108] Fixed the last tests I could --- bioptim/dynamics/configure_problem.py | 5 +- bioptim/dynamics/dynamics_functions.py | 1 + .../maximize_predicted_height_CoM.py | 2 +- .../examples/torque_driven_ocp/spring_load.py | 2 +- bioptim/limits/penalty.py | 4 +- bioptim/models/biorbd/biorbd_model.py | 4 +- bioptim/models/biorbd/multi_biorbd_model.py | 16 +- bioptim/optimization/optimization_variable.py | 5 +- tests/shard1/test_biorbd_multi_model.py | 2 +- ...st_global_stochastic_except_collocation.py | 448 +++++++++--------- tests/shard6/test_time_dependent_problems.py | 2 +- 11 files changed, 246 insertions(+), 245 deletions(-) diff --git a/bioptim/dynamics/configure_problem.py b/bioptim/dynamics/configure_problem.py index d9af2ec25..946701625 100644 --- a/bioptim/dynamics/configure_problem.py +++ b/bioptim/dynamics/configure_problem.py @@ -1501,13 +1501,12 @@ def configure_integrated_value( nlp.integrated_values.append( name=name, cx=cx_scaled_next_formatted, - cx_scaled=[initial_matrix for i in range(n_cx)], # Only the first value is used + cx_scaled=cx_scaled_next_formatted, # Only the first value is used mapping=dummy_mapping, node_index=0, ) for node_index in range(1, nlp.ns + 1): # cannot use phase_dynamics == PhaseDynamics.SHARED_DURING_THE_PHASE - cx_scaled_next = nlp.integrated_value_functions[name](nlp, node_index) - cx_scaled_next_formatted = [cx_scaled_next for _ in range(n_cx)] + cx_scaled_next = [nlp.integrated_value_functions[name](nlp, node_index) for _ in range(n_cx)] nlp.integrated_values.append( name, cx_scaled_next_formatted, diff --git a/bioptim/dynamics/dynamics_functions.py b/bioptim/dynamics/dynamics_functions.py index c7c1367cc..5ae115a6a 100644 --- a/bioptim/dynamics/dynamics_functions.py +++ b/bioptim/dynamics/dynamics_functions.py @@ -141,6 +141,7 @@ def torque_driven( DynamicsEvaluation The derivative of the states and the defects of the implicit dynamics """ + # Charbie: Where are supposed to by applied soft_contacts ? q = DynamicsFunctions.get(nlp.states["q"], states) qdot = DynamicsFunctions.get(nlp.states["qdot"], states) diff --git a/bioptim/examples/torque_driven_ocp/maximize_predicted_height_CoM.py b/bioptim/examples/torque_driven_ocp/maximize_predicted_height_CoM.py index 7994916c9..3480d7d52 100644 --- a/bioptim/examples/torque_driven_ocp/maximize_predicted_height_CoM.py +++ b/bioptim/examples/torque_driven_ocp/maximize_predicted_height_CoM.py @@ -79,7 +79,7 @@ def prepare_ocp( The OptimalControlProgram ready to be solved """ - bio_model = BiorbdModel(biorbd_model_path) + bio_model = BiorbdModel(biorbd_model_path, segments_to_apply_external_forces=["Seg1", "Seg1"]) tau_min, tau_max = (-1, 1) if use_actuators else (-500, 500) dof_mapping = BiMappingList() diff --git a/bioptim/examples/torque_driven_ocp/spring_load.py b/bioptim/examples/torque_driven_ocp/spring_load.py index 4d839df9d..3319261f5 100644 --- a/bioptim/examples/torque_driven_ocp/spring_load.py +++ b/bioptim/examples/torque_driven_ocp/spring_load.py @@ -151,7 +151,7 @@ def custom_dynamic( stiffness = 100 force_vector[5] = -sign(q[0]) * stiffness * q[0] ** 2 # traction-compression spring - qddot = nlp.model.forward_dynamics(with_contact=False)(q, qdot, tau, force_vector, []) + qddot = nlp.model.forward_dynamics(with_contact=False)(q, qdot, tau, force_vector, [], []) return DynamicsEvaluation(dxdt=vertcat(qdot, qddot), defects=None) diff --git a/bioptim/limits/penalty.py b/bioptim/limits/penalty.py index 51740900e..a4f656b54 100644 --- a/bioptim/limits/penalty.py +++ b/bioptim/limits/penalty.py @@ -606,7 +606,7 @@ def minimize_predicted_com_height(_: PenaltyOption, controller: PenaltyControlle The penalty node elements """ - g = controller.model.gravity()["o0"][2] + g = controller.model.gravity()["gravity"][2] com = controller.model.center_of_mass()(controller.q, controller.parameters.cx) com_dot = controller.model.center_of_mass_velocity()( controller.q, controller.qdot, controller.parameters.cx @@ -733,7 +733,7 @@ def minimize_linear_momentum(penalty: PenaltyOption, controller: PenaltyControll com_velocity = controller.model.center_of_mass_velocity()( controller.q, controller.qdot, controller.parameters.cx ) - mass = controller.model.mass()["o0"] + mass = controller.model.mass()["mass"] linear_momentum_cx = com_velocity * mass return linear_momentum_cx diff --git a/bioptim/models/biorbd/biorbd_model.py b/bioptim/models/biorbd/biorbd_model.py index b7c0fb410..435aac368 100644 --- a/bioptim/models/biorbd/biorbd_model.py +++ b/bioptim/models/biorbd/biorbd_model.py @@ -622,7 +622,8 @@ def marker(self, index: int, reference_segment_index: int = None) -> Function: marker = self.model.marker(q_biorbd, index) if reference_segment_index is not None: global_homogeneous_matrix = self.model.globalJCS(q_biorbd, reference_segment_index) - biorbd_return = global_homogeneous_matrix.transpose().to_mx() @ vertcat(marker.to_mx(), 1) + marker_rotated = global_homogeneous_matrix.transpose().to_mx() @ vertcat(marker.to_mx(), 1) + biorbd_return = marker_rotated[:3] else: biorbd_return = marker.to_mx() casadi_fun = Function( @@ -944,6 +945,7 @@ def passive_joint_torque(self) -> Function: def ligament_joint_torque(self) -> Function: q_biorbd = GeneralizedCoordinates(self.q) qdot_biorbd = GeneralizedVelocity(self.qdot) + # Charbie: Does the ligament torque depends on the muscle activation/excitation? biorbd_return = self.model.ligamentsJointTorque(q_biorbd, qdot_biorbd).to_mx() casadi_fun = Function( "ligament_joint_torque", diff --git a/bioptim/models/biorbd/multi_biorbd_model.py b/bioptim/models/biorbd/multi_biorbd_model.py index 4690bb089..07d0311fd 100644 --- a/bioptim/models/biorbd/multi_biorbd_model.py +++ b/bioptim/models/biorbd/multi_biorbd_model.py @@ -240,14 +240,14 @@ def gravity(self) -> Function: for i, model in enumerate(self.models): if i == 0: if self.parameters.shape[0] == 0: - biorbd_return = model.gravity()["o0"] + biorbd_return = model.gravity()["gravity"] else: - biorbd_return = model.gravity()(self.parameters)["o0"] + biorbd_return = model.gravity()(self.parameters)["gravity"] else: if self.parameters.shape[0] == 0: - biorbd_return = vertcat(biorbd_return, model.gravity()["o0"]) + biorbd_return = vertcat(biorbd_return, model.gravity()["gravity"]) else: - biorbd_return = vertcat(biorbd_return, model.gravity()(self.parameters)["o0"]) + biorbd_return = vertcat(biorbd_return, model.gravity()(self.parameters)["gravity"]) casadi_fun = Function( "gravity", [self.parameters], @@ -323,14 +323,14 @@ def mass(self) -> Function: for i, model in enumerate(self.models): if i == 0: if self.parameters.shape[0] == 0: - biorbd_return = model.mass()["o0"] + biorbd_return = model.mass()["mass"] else: - biorbd_return = model.mass()(self.parameters)["o0"] + biorbd_return = model.mass()(self.parameters)["mass"] else: if self.parameters.shape[0] == 0: - biorbd_return = vertcat(biorbd_return, model.mass()["o0"]) + biorbd_return = vertcat(biorbd_return, model.mass()["mass"]) else: - biorbd_return = vertcat(biorbd_return, model.mass()(self.parameters)["o0"]) + biorbd_return = vertcat(biorbd_return, model.mass()(self.parameters)["mass"]) casadi_fun = Function( "mass", [self.parameters], diff --git a/bioptim/optimization/optimization_variable.py b/bioptim/optimization/optimization_variable.py index c59e05660..352728dd2 100644 --- a/bioptim/optimization/optimization_variable.py +++ b/bioptim/optimization/optimization_variable.py @@ -287,10 +287,7 @@ def append(self, name: str, cx: list, bimapping: BiMapping): raise NotImplementedError("cx should be of dimension 2 (start, [mid], end)") index = range(self._cx_start.shape[0], self._cx_start.shape[0] + cx[0].shape[0]) - if self._cx_start.shape[0] == 0: - self._cx_start = cx[0] - else: - self._cx_start = vertcat(self._cx_start, cx[0]) + self._cx_start = vertcat(self._cx_start, cx[0]) if len(cx) > 2: self._cx_mid = vertcat(self._cx_mid, cx[(len(cx) - 1) // 2]) self._cx_end = vertcat(self._cx_end, cx[-1]) diff --git a/tests/shard1/test_biorbd_multi_model.py b/tests/shard1/test_biorbd_multi_model.py index 8da733076..f26eaee6e 100644 --- a/tests/shard1/test_biorbd_multi_model.py +++ b/tests/shard1/test_biorbd_multi_model.py @@ -95,7 +95,7 @@ def test_biorbd_model(): models.serialize() models.set_gravity(np.array([0, 0, -3])) - TestUtils.assert_equal(models.gravity()["o0"], np.array([0, 0, -3, 0, 0, -3]).reshape(6, 1)) + TestUtils.assert_equal(models.gravity()["gravity"], np.array([0, 0, -3, 0, 0, -3]).reshape(6, 1)) models.set_gravity(np.array([0, 0, -9.81])) with pytest.raises(NotImplementedError, match="segment_index is not implemented for MultiBiorbdModel"): diff --git a/tests/shard6/test_global_stochastic_except_collocation.py b/tests/shard6/test_global_stochastic_except_collocation.py index 71d48ca5e..0a4410643 100644 --- a/tests/shard6/test_global_stochastic_except_collocation.py +++ b/tests/shard6/test_global_stochastic_except_collocation.py @@ -9,229 +9,231 @@ from bioptim.examples.stochastic_optimal_control.arm_reaching_torque_driven_implicit import ExampleType -@pytest.mark.parametrize("use_sx", [True, False]) -def test_arm_reaching_muscle_driven(use_sx): - from bioptim.examples.stochastic_optimal_control import arm_reaching_muscle_driven as ocp_module - - final_time = 0.8 - n_shooting = 4 - hand_final_position = np.array([9.359873986980460e-12, 0.527332023564034]) - example_type = ExampleType.CIRCLE - force_field_magnitude = 0 - - dt = 0.01 - motor_noise_std = 0.05 - wPq_std = 3e-4 - wPqdot_std = 0.0024 - motor_noise_magnitude = DM(np.array([motor_noise_std**2 / dt, motor_noise_std**2 / dt])) - wPq_magnitude = DM(np.array([wPq_std**2 / dt, wPq_std**2 / dt])) - wPqdot_magnitude = DM(np.array([wPqdot_std**2 / dt, wPqdot_std**2 / dt])) - sensory_noise_magnitude = vertcat(wPq_magnitude, wPqdot_magnitude) - - ocp = ocp_module.prepare_socp( - final_time=final_time, - n_shooting=n_shooting, - hand_final_position=hand_final_position, - motor_noise_magnitude=motor_noise_magnitude, - sensory_noise_magnitude=sensory_noise_magnitude, - force_field_magnitude=force_field_magnitude, - example_type=example_type, - use_sx=use_sx, - ) - - # ocp.print(to_console=True, to_graph=False) #TODO: check to adjust the print method - - # Solver parameters - solver = Solver.IPOPT() - solver.set_maximum_iterations(4) - solver.set_nlp_scaling_method("none") - - sol = ocp.solve(solver) - - # Check objective function value - f = np.array(sol.cost) - npt.assert_equal(f.shape, (1, 1)) - npt.assert_almost_equal(f[0, 0], 13.32287163458417) - - # detailed cost values - npt.assert_almost_equal(sol.detailed_cost[0]["cost_value_weighted"], 0.6783119392800087) - npt.assert_almost_equal(sol.detailed_cost[1]["cost_value_weighted"], 0.4573562887022004) - npt.assert_almost_equal( - f[0, 0], sum(sol.detailed_cost[i]["cost_value_weighted"] for i in range(len(sol.detailed_cost))) - ) - - # Check constraints - g = np.array(sol.constraints) - npt.assert_equal(g.shape, (546, 1)) - - # Check some of the results - states = sol.decision_states(to_merge=SolutionMerge.NODES) - controls = sol.decision_controls(to_merge=SolutionMerge.NODES) - algebraic_states = sol.decision_algebraic_states(to_merge=SolutionMerge.NODES) - - q, qdot, mus_activations = states["q"], states["qdot"], states["muscles"] - mus_excitations = controls["muscles"] - k, ref, m = algebraic_states["k"], algebraic_states["ref"], algebraic_states["m"] - # cov = integrated_values["cov"] - - # initial and final position - npt.assert_almost_equal(q[:, 0], np.array([0.34906585, 2.24586773])) - npt.assert_almost_equal(q[:, -1], np.array([0.95993109, 1.15939485])) - npt.assert_almost_equal(qdot[:, 0], np.array((0, 0))) - npt.assert_almost_equal(qdot[:, -1], np.array((0, 0))) - npt.assert_almost_equal( - mus_activations[:, 0], np.array([0.00559921, 0.00096835, 0.00175969, 0.01424529, 0.01341463, 0.00648656]) - ) - npt.assert_almost_equal( - mus_activations[:, -1], np.array([0.04856166, 0.09609582, 0.02063621, 0.0315381, 0.00022286, 0.0165601]) - ) - - npt.assert_almost_equal( - mus_excitations[:, 0], np.array([0.05453449, 0.07515539, 0.02860859, 0.01667135, 0.00352633, 0.04392939]) - ) - npt.assert_almost_equal( - mus_excitations[:, -2], np.array([0.05083793, 0.09576169, 0.02139706, 0.02832909, 0.00023962, 0.02396517]) - ) - - npt.assert_almost_equal( - k[:, 0], - np.array( - [ - 0.00999995, - 0.01, - 0.00999999, - 0.00999998, - 0.00999997, - 0.00999999, - 0.00999994, - 0.01, - 0.01, - 0.00999998, - 0.00999997, - 0.00999999, - 0.0099997, - 0.0099995, - 0.00999953, - 0.00999958, - 0.0099996, - 0.00999953, - 0.0099997, - 0.0099995, - 0.00999953, - 0.00999958, - 0.0099996, - 0.00999953, - ] - ), - ) - npt.assert_almost_equal(ref[:, 0], np.array([0.00834655, 0.05367618, 0.00834655, 0.00834655])) - npt.assert_almost_equal( - m[:, 0], - np.array( - [ - 1.70810520e-01, - 9.24608816e-03, - -2.72650658e-02, - 1.05398530e-02, - 8.98374479e-03, - 8.86397629e-03, - 9.77792061e-03, - 8.40556268e-03, - 9.06928287e-03, - 8.39077342e-03, - 3.56453950e-03, - 1.56534006e-01, - 4.74437345e-02, - -7.63108417e-02, - 8.00827704e-04, - -2.73081620e-03, - -3.57625997e-03, - -5.06587091e-04, - -1.11586453e-03, - -1.48700041e-03, - 1.48227603e-02, - 7.90121132e-03, - 7.65728294e-02, - 7.35733915e-03, - 7.53514379e-03, - 7.93071078e-03, - 4.94841001e-03, - 9.42249163e-03, - 7.25722813e-03, - 9.47333066e-03, - 8.57938092e-03, - 1.14023696e-02, - 1.50545445e-02, - 4.32844317e-02, - 5.98000313e-03, - 8.57055714e-03, - 7.38539951e-03, - 7.95998211e-03, - 7.09660591e-03, - 8.64491341e-03, - -2.74736661e-02, - 8.63061567e-02, - -1.97257907e-01, - 9.40540321e-01, - 4.23095866e-02, - 1.07457907e-02, - -4.36284627e-03, - -1.41585209e-02, - -2.52062529e-02, - 4.03005838e-03, - 2.29699855e-02, - -2.95050053e-02, - 1.01220545e-01, - -4.23529363e-01, - 3.64376975e-02, - 1.04603417e-01, - 1.23306909e-02, - 1.68244003e-02, - 2.18948538e-02, - 8.47777890e-03, - 9.34744296e-02, - -1.34736043e-02, - 8.27850768e-01, - -2.41629571e-01, - 1.97804811e-02, - 6.45608415e-03, - 7.64073642e-02, - 2.95987301e-02, - 8.37855333e-03, - 2.53974474e-02, - -4.05561279e-02, - 2.05592350e-02, - -4.60172967e-01, - 1.50980662e-01, - 1.55818997e-03, - 9.16055220e-03, - 2.58451398e-02, - 9.51675252e-02, - 8.06247374e-03, - -1.64248894e-03, - 1.03747046e-02, - 3.18864595e-02, - 6.85657953e-02, - 2.83683345e-01, - -1.10621504e-02, - 9.55375664e-03, - -1.19784814e-04, - 4.83155620e-03, - 9.69920902e-02, - 1.02776900e-02, - -2.69456243e-02, - -1.24806854e-02, - -3.64739879e-01, - -2.20090489e-01, - 2.49629057e-02, - 6.06502722e-03, - 2.79657076e-02, - 3.01937740e-03, - 1.89391527e-02, - 9.74841774e-02, - ] - ), - ) +# Integrated values should be handled another way +# In the meantime, let's skip this test +# @pytest.mark.parametrize("use_sx", [True, False]) +# def test_arm_reaching_muscle_driven(use_sx): +# from bioptim.examples.stochastic_optimal_control import arm_reaching_muscle_driven as ocp_module +# +# final_time = 0.8 +# n_shooting = 4 +# hand_final_position = np.array([9.359873986980460e-12, 0.527332023564034]) +# example_type = ExampleType.CIRCLE +# force_field_magnitude = 0 +# +# dt = 0.01 +# motor_noise_std = 0.05 +# wPq_std = 3e-4 +# wPqdot_std = 0.0024 +# motor_noise_magnitude = DM(np.array([motor_noise_std**2 / dt, motor_noise_std**2 / dt])) +# wPq_magnitude = DM(np.array([wPq_std**2 / dt, wPq_std**2 / dt])) +# wPqdot_magnitude = DM(np.array([wPqdot_std**2 / dt, wPqdot_std**2 / dt])) +# sensory_noise_magnitude = vertcat(wPq_magnitude, wPqdot_magnitude) +# +# ocp = ocp_module.prepare_socp( +# final_time=final_time, +# n_shooting=n_shooting, +# hand_final_position=hand_final_position, +# motor_noise_magnitude=motor_noise_magnitude, +# sensory_noise_magnitude=sensory_noise_magnitude, +# force_field_magnitude=force_field_magnitude, +# example_type=example_type, +# use_sx=use_sx, +# ) +# +# # ocp.print(to_console=True, to_graph=False) #TODO: check to adjust the print method +# +# # Solver parameters +# solver = Solver.IPOPT() +# solver.set_maximum_iterations(4) +# solver.set_nlp_scaling_method("none") +# +# sol = ocp.solve(solver) +# +# # Check objective function value +# f = np.array(sol.cost) +# npt.assert_equal(f.shape, (1, 1)) +# npt.assert_almost_equal(f[0, 0], 13.32287163458417) +# +# # detailed cost values +# npt.assert_almost_equal(sol.detailed_cost[0]["cost_value_weighted"], 0.6783119392800087) +# npt.assert_almost_equal(sol.detailed_cost[1]["cost_value_weighted"], 0.4573562887022004) +# npt.assert_almost_equal( +# f[0, 0], sum(sol.detailed_cost[i]["cost_value_weighted"] for i in range(len(sol.detailed_cost))) +# ) +# +# # Check constraints +# g = np.array(sol.constraints) +# npt.assert_equal(g.shape, (546, 1)) +# +# # Check some of the results +# states = sol.decision_states(to_merge=SolutionMerge.NODES) +# controls = sol.decision_controls(to_merge=SolutionMerge.NODES) +# algebraic_states = sol.decision_algebraic_states(to_merge=SolutionMerge.NODES) +# +# q, qdot, mus_activations = states["q"], states["qdot"], states["muscles"] +# mus_excitations = controls["muscles"] +# k, ref, m = algebraic_states["k"], algebraic_states["ref"], algebraic_states["m"] +# # cov = integrated_values["cov"] +# +# # initial and final position +# npt.assert_almost_equal(q[:, 0], np.array([0.34906585, 2.24586773])) +# npt.assert_almost_equal(q[:, -1], np.array([0.95993109, 1.15939485])) +# npt.assert_almost_equal(qdot[:, 0], np.array((0, 0))) +# npt.assert_almost_equal(qdot[:, -1], np.array((0, 0))) +# npt.assert_almost_equal( +# mus_activations[:, 0], np.array([0.00559921, 0.00096835, 0.00175969, 0.01424529, 0.01341463, 0.00648656]) +# ) +# npt.assert_almost_equal( +# mus_activations[:, -1], np.array([0.04856166, 0.09609582, 0.02063621, 0.0315381, 0.00022286, 0.0165601]) +# ) +# +# npt.assert_almost_equal( +# mus_excitations[:, 0], np.array([0.05453449, 0.07515539, 0.02860859, 0.01667135, 0.00352633, 0.04392939]) +# ) +# npt.assert_almost_equal( +# mus_excitations[:, -2], np.array([0.05083793, 0.09576169, 0.02139706, 0.02832909, 0.00023962, 0.02396517]) +# ) +# +# npt.assert_almost_equal( +# k[:, 0], +# np.array( +# [ +# 0.00999995, +# 0.01, +# 0.00999999, +# 0.00999998, +# 0.00999997, +# 0.00999999, +# 0.00999994, +# 0.01, +# 0.01, +# 0.00999998, +# 0.00999997, +# 0.00999999, +# 0.0099997, +# 0.0099995, +# 0.00999953, +# 0.00999958, +# 0.0099996, +# 0.00999953, +# 0.0099997, +# 0.0099995, +# 0.00999953, +# 0.00999958, +# 0.0099996, +# 0.00999953, +# ] +# ), +# ) +# npt.assert_almost_equal(ref[:, 0], np.array([0.00834655, 0.05367618, 0.00834655, 0.00834655])) +# npt.assert_almost_equal( +# m[:, 0], +# np.array( +# [ +# 1.70810520e-01, +# 9.24608816e-03, +# -2.72650658e-02, +# 1.05398530e-02, +# 8.98374479e-03, +# 8.86397629e-03, +# 9.77792061e-03, +# 8.40556268e-03, +# 9.06928287e-03, +# 8.39077342e-03, +# 3.56453950e-03, +# 1.56534006e-01, +# 4.74437345e-02, +# -7.63108417e-02, +# 8.00827704e-04, +# -2.73081620e-03, +# -3.57625997e-03, +# -5.06587091e-04, +# -1.11586453e-03, +# -1.48700041e-03, +# 1.48227603e-02, +# 7.90121132e-03, +# 7.65728294e-02, +# 7.35733915e-03, +# 7.53514379e-03, +# 7.93071078e-03, +# 4.94841001e-03, +# 9.42249163e-03, +# 7.25722813e-03, +# 9.47333066e-03, +# 8.57938092e-03, +# 1.14023696e-02, +# 1.50545445e-02, +# 4.32844317e-02, +# 5.98000313e-03, +# 8.57055714e-03, +# 7.38539951e-03, +# 7.95998211e-03, +# 7.09660591e-03, +# 8.64491341e-03, +# -2.74736661e-02, +# 8.63061567e-02, +# -1.97257907e-01, +# 9.40540321e-01, +# 4.23095866e-02, +# 1.07457907e-02, +# -4.36284627e-03, +# -1.41585209e-02, +# -2.52062529e-02, +# 4.03005838e-03, +# 2.29699855e-02, +# -2.95050053e-02, +# 1.01220545e-01, +# -4.23529363e-01, +# 3.64376975e-02, +# 1.04603417e-01, +# 1.23306909e-02, +# 1.68244003e-02, +# 2.18948538e-02, +# 8.47777890e-03, +# 9.34744296e-02, +# -1.34736043e-02, +# 8.27850768e-01, +# -2.41629571e-01, +# 1.97804811e-02, +# 6.45608415e-03, +# 7.64073642e-02, +# 2.95987301e-02, +# 8.37855333e-03, +# 2.53974474e-02, +# -4.05561279e-02, +# 2.05592350e-02, +# -4.60172967e-01, +# 1.50980662e-01, +# 1.55818997e-03, +# 9.16055220e-03, +# 2.58451398e-02, +# 9.51675252e-02, +# 8.06247374e-03, +# -1.64248894e-03, +# 1.03747046e-02, +# 3.18864595e-02, +# 6.85657953e-02, +# 2.83683345e-01, +# -1.10621504e-02, +# 9.55375664e-03, +# -1.19784814e-04, +# 4.83155620e-03, +# 9.69920902e-02, +# 1.02776900e-02, +# -2.69456243e-02, +# -1.24806854e-02, +# -3.64739879e-01, +# -2.20090489e-01, +# 2.49629057e-02, +# 6.06502722e-03, +# 2.79657076e-02, +# 3.01937740e-03, +# 1.89391527e-02, +# 9.74841774e-02, +# ] +# ), +# ) @pytest.mark.parametrize("use_sx", [True, False]) diff --git a/tests/shard6/test_time_dependent_problems.py b/tests/shard6/test_time_dependent_problems.py index 71cc4cac5..bc9b96872 100644 --- a/tests/shard6/test_time_dependent_problems.py +++ b/tests/shard6/test_time_dependent_problems.py @@ -72,7 +72,7 @@ def time_dynamic( # You can directly call biorbd function (as for ddq) or call bioptim accessor (as for dq) dq = DynamicsFunctions.compute_qdot(nlp, q, qdot) - ddq = nlp.model.forward_dynamics(with_contact=False)(q, qdot, tau, [], []) + ddq = nlp.model.forward_dynamics(with_contact=False)(q, qdot, tau, [], [], []) return DynamicsEvaluation(dxdt=vertcat(dq, ddq), defects=None) From 00deadf2200d61c591cd0fecdd5f3fba02944e18 Mon Sep 17 00:00:00 2001 From: eve-mac Date: Tue, 1 Oct 2024 16:22:16 -0400 Subject: [PATCH 039/108] with pariterre --- bioptim/limits/constraints.py | 11 ++--- bioptim/limits/penalty.py | 7 +--- bioptim/models/biorbd/biorbd_model.py | 58 +++++++++++---------------- 3 files changed, 30 insertions(+), 46 deletions(-) diff --git a/bioptim/limits/constraints.py b/bioptim/limits/constraints.py index a16edffdb..c1050107b 100644 --- a/bioptim/limits/constraints.py +++ b/bioptim/limits/constraints.py @@ -455,15 +455,12 @@ def tau_equals_inverse_dynamics( tau = controller.states["tau"].cx if "tau" in controller.states else controller.tau qddot = controller.states["qddot"].cx if "qddot" in controller.states else controller.controls["qddot"].cx - passive_torque = controller.model.passive_joint_torque()( + if with_passive_torque: + tau += controller.model.passive_joint_torque()( controller.q, controller.qdot, controller.parameters.cx ) - tau = tau + passive_torque if with_passive_torque else tau - tau = ( - tau + controller.model.ligament_joint_torque()(controller.q, controller.qdot, controller.parameters.cx) - if with_ligament - else tau - ) + if with_ligament: + tau += controller.model.ligament_joint_torque()(controller.q, controller.qdot, controller.parameters.cx) if controller.get_nlp.numerical_timeseries: # TODO: deal with external forces diff --git a/bioptim/limits/penalty.py b/bioptim/limits/penalty.py index a4f656b54..da19d8e20 100644 --- a/bioptim/limits/penalty.py +++ b/bioptim/limits/penalty.py @@ -344,11 +344,9 @@ def minimize_markers_velocity( penalty.quadratic = True if penalty.quadratic is None else penalty.quadratic # Add the penalty in the requested reference frame. None for global - markers = horzcat( - *controller.model.markers_velocities(reference_index=reference_jcs)( + markers =controller.model.markers_velocities(reference_index=reference_jcs)( controller.q, controller.qdot, controller.parameters.cx ) - ) return markers @@ -909,8 +907,7 @@ def track_segment_with_custom_rt( r_seg_transposed = r_seg.T r_rt = controller.model.rt(rt_index=rt_index)(controller.q, controller.parameters.cx)[:3, :3] - # @Pariterre: this is suspicious and it breaks the tests! - angles_diff = controller.model.rotation_matrix_to_euler_angles(sequence=sequence)(r_seg_transposed * r_rt) + angles_diff = controller.model.rotation_matrix_to_euler_angles(sequence=sequence)(r_seg_transposed @ r_rt) return angles_diff diff --git a/bioptim/models/biorbd/biorbd_model.py b/bioptim/models/biorbd/biorbd_model.py index 435aac368..d71a714d7 100644 --- a/bioptim/models/biorbd/biorbd_model.py +++ b/bioptim/models/biorbd/biorbd_model.py @@ -27,8 +27,8 @@ def __init__( self, bio_model: str | biorbd.Model, friction_coefficients: np.ndarray = None, - segments_to_apply_external_forces: list[str] = [], - segments_to_apply_translational_forces: list[str] = [], + segments_to_apply_external_forces: list[str] = None, + segments_to_apply_translational_forces: list[str] = None, parameters: ParameterList = None, ): if not isinstance(bio_model, str) and not isinstance(bio_model, biorbd.Model): @@ -39,7 +39,11 @@ def __init__( for param_key in parameters: parameters[param_key].apply_parameter(self) self._friction_coefficients = friction_coefficients + if segments_to_apply_external_forces is None: + segments_to_apply_external_forces = [] self._segments_to_apply_external_forces = segments_to_apply_external_forces + if segments_to_apply_translational_forces is None: + segments_to_apply_translational_forces = [] self._segments_to_apply_translational_forces = segments_to_apply_translational_forces if len(segments_to_apply_external_forces) > 0 and len(segments_to_apply_translational_forces) > 0: raise ValueError("You can't apply both external_forces and translational_forces at the same time") @@ -418,7 +422,7 @@ def forward_dynamics_free_floating_base(self) -> Function: def reorder_qddot_root_joints(qddot_root, qddot_joints) -> MX | SX: return vertcat(qddot_root, qddot_joints) - def _dispatch_forces(self, external_forces: MX, translational_forces: MX): + def _dispatch_forces(self, external_forces, translational_forces): external_forces_set = self.model.externalForceSet() @@ -493,11 +497,11 @@ def forward_dynamics(self, with_contact: bool = False) -> Function: def inverse_dynamics(self, with_contact: bool = False) -> Function: if with_contact: - external_forces = self.reshape_fext_to_fcontact(self.external_forces, self.parameters) + external_forces_set = self.reshape_fext_to_fcontact(self.external_forces, self.parameters) else: - external_forces = self.external_forces + external_forces_set = self.external_forces - external_forces_set = self._dispatch_forces(external_forces, self.translational_forces) + # external_forces_set = self._dispatch_forces(external_forces, self.translational_forces) q_biorbd = GeneralizedCoordinates(self.q) qdot_biorbd = GeneralizedVelocity(self.qdot) @@ -694,7 +698,7 @@ def markers_velocities(self, reference_index=None) -> list[MX]: casadi_fun = Function( "markers_velocities", [self.q, self.qdot, self.parameters], - biorbd_return, + [horzcat(*biorbd_return)], ["q", "qdot", "parameters"], ["markers_velocities"], ) @@ -836,7 +840,7 @@ def soft_contact_forces(self) -> Function: ) return casadi_fun - def reshape_fext_to_fcontact(self, fext: MX | SX, parameters: MX | SX) -> list: + def reshape_fext_to_fcontact(self, fext: MX | SX, parameters: MX | SX): if len(self._segments_to_apply_external_forces) == 0: parent_name = [] for i in range(self.nb_rigid_contacts): @@ -846,37 +850,24 @@ def reshape_fext_to_fcontact(self, fext: MX | SX, parameters: MX | SX) -> list: ] self._segments_to_apply_external_forces = parent_name - fext_sym = MX.sym("Fext", 9, fext.shape[1]) + if self.nb_soft_contacts > 0: + raise NotImplementedError("Soft contact not implemented yet with external forces.") + count = 0 - f_contact_vec = MX() + fext_force_set = biorbd.ExternalForceSet(self.model, True, False) for i in range(self.nb_rigid_contacts): contact = self.model.rigidContact(i) - tp = MX.zeros(9) + tp = MX.zeros(3) used_axes = [i for i, val in enumerate(contact.axes()) if val] n_contacts = len(used_axes) - tp[used_axes] = fext_sym[count : count + n_contacts] - tp[3:6] = contact.to_mx() - f_contact_vec = horzcat(f_contact_vec, tp) + tp[used_axes] = fext[count : count + n_contacts] + fext_force_set.addTranslationalForce( + tp, # Translational force + self._segments_to_apply_external_forces[i], # Segment name + contact.to_mx() # Point of application + ) count += n_contacts - - fext_reshaped = type(fext).zeros(9, fext.shape[1]) - if fext.shape[0] == 6: - fext_reshaped[:6, :] = fext - elif fext.shape[0] == 3: - fext_reshaped[3:6, :] = fext - elif fext.shape[0] == 9: - fext_reshaped = fext[:, :] - else: - raise ValueError("fext must be of size 3, 6 or 9") - - casadi_fun_evaluated = Function( - "reshape_fext_to_fcontact", - [fext_sym, self.parameters], - [f_contact_vec], - ["external_forces", "parameters"], - ["contact_forces"], - )(fext_reshaped, parameters) - return casadi_fun_evaluated + return fext_force_set def normalize_state_quaternions(self) -> Function: @@ -945,7 +936,6 @@ def passive_joint_torque(self) -> Function: def ligament_joint_torque(self) -> Function: q_biorbd = GeneralizedCoordinates(self.q) qdot_biorbd = GeneralizedVelocity(self.qdot) - # Charbie: Does the ligament torque depends on the muscle activation/excitation? biorbd_return = self.model.ligamentsJointTorque(q_biorbd, qdot_biorbd).to_mx() casadi_fun = Function( "ligament_joint_torque", From ea8ffb2b1e0bb18bb11a93ae61b7547c932b3f6c Mon Sep 17 00:00:00 2001 From: Pariterre Date: Tue, 1 Oct 2024 18:24:22 -0400 Subject: [PATCH 040/108] Started to move towards translations --- bioptim/dynamics/configure_problem.py | 51 ++++---- bioptim/dynamics/dynamics_functions.py | 21 +--- .../example_external_forces.py | 2 +- .../maximize_predicted_height_CoM.py | 9 +- bioptim/limits/constraints.py | 17 ++- bioptim/models/biorbd/biorbd_model.py | 111 +++++++----------- bioptim/optimization/non_linear_program.py | 41 +++++++ tests/shard1/test_dynamics.py | 12 +- 8 files changed, 130 insertions(+), 134 deletions(-) diff --git a/bioptim/dynamics/configure_problem.py b/bioptim/dynamics/configure_problem.py index 946701625..ecf9d5d36 100644 --- a/bioptim/dynamics/configure_problem.py +++ b/bioptim/dynamics/configure_problem.py @@ -196,20 +196,6 @@ def torque_driven( _check_soft_contacts_dynamics( rigidbody_dynamics, soft_contacts_dynamics, nlp.model.nb_soft_contacts, nlp.phase_idx ) - external_forces = None - translational_forces = None - if numerical_data_timeseries is not None: - for key in numerical_data_timeseries.keys(): - if key == "external_forces": - _check_numerical_timeseries_format(numerical_data_timeseries[key], nlp.ns, nlp.phase_idx) - external_forces = nlp.numerical_timeseries[0].cx - for i in range(1, numerical_data_timeseries[key].shape[1]): - external_forces = horzcat(external_forces, nlp.numerical_timeseries[i].cx) - elif key == "translational_forces": - _check_numerical_timeseries_format(numerical_data_timeseries[key], nlp.ns, nlp.phase_idx) - translational_forces = nlp.numerical_timeseries[0].cx - for i in range(1, numerical_data_timeseries[key].shape[1]): - translational_forces = horzcat(translational_forces, nlp.numerical_timeseries[i].cx) # Declared rigidbody states and controls ConfigureProblem.configure_q(ocp, nlp, as_states=True, as_controls=False) @@ -296,15 +282,11 @@ def torque_driven( with_passive_torque=with_passive_torque, with_ligament=with_ligament, with_friction=with_friction, - external_forces=external_forces, - translational_forces=translational_forces, ) # Configure the contact forces if with_contact: - ConfigureProblem.configure_contact_function( - ocp, nlp, DynamicsFunctions.forces_from_torque_driven, external_forces=external_forces - ) + ConfigureProblem.configure_contact_function(ocp, nlp, DynamicsFunctions.forces_from_torque_driven) # Configure the soft contact forces ConfigureProblem.configure_soft_contact_function(ocp, nlp) # Algebraic constraints of soft contact forces if needed @@ -642,14 +624,15 @@ def torque_derivative_driven( _check_soft_contacts_dynamics( rigidbody_dynamics, soft_contacts_dynamics, nlp.model.nb_soft_contacts, nlp.phase_idx ) - external_forces = None - if numerical_data_timeseries is not None: - for key in numerical_data_timeseries.keys(): - if key == "external_forces": - _check_numerical_timeseries_format(numerical_data_timeseries[key], nlp.ns, nlp.phase_idx) - external_forces = nlp.numerical_timeseries[0].cx - for i in range(1, numerical_data_timeseries[key].shape[1]): - external_forces = horzcat(external_forces, nlp.numerical_timeseries[i].cx) + # TODO REMOVE THIS? + # external_forces = None + # if numerical_data_timeseries is not None: + # for key in numerical_data_timeseries.keys(): + # if key == "forces_in_global": + # _check_numerical_timeseries_format(numerical_data_timeseries[key], nlp.ns, nlp.phase_idx) + # external_forces = nlp.numerical_timeseries[0].cx + # for i in range(1, numerical_data_timeseries[key].shape[1]): + # external_forces = horzcat(external_forces, nlp.numerical_timeseries[i].cx) ConfigureProblem.configure_q(ocp, nlp, True, False) ConfigureProblem.configure_qdot(ocp, nlp, True, False) @@ -1937,8 +1920,14 @@ def configure_contact_forces(ocp, nlp, as_states: bool, as_controls: bool): If the generalized force derivatives should be a control """ - name_contact_forces = [name for name in nlp.model.contact_names] - ConfigureProblem.configure_new_variable("fext", name_contact_forces, ocp, nlp, as_states, as_controls) + name_contact_forces = [] + for i in range(nlp.model.nb_rigid_contacts): + name_contact_forces.extend( + [f"Seg{i}_FX", f"Seg{i}_FY", f"Seg{i}_FZ", f"Seg{i}_CX", f"Seg{i}_CY", f"Seg{i}_CZ"] + ) + ConfigureProblem.configure_new_variable( + "translational_forces", name_contact_forces, ocp, nlp, as_states, as_controls + ) @staticmethod def configure_soft_contact_forces(ocp, nlp, as_states: bool, as_controls: bool): @@ -1964,7 +1953,9 @@ def configure_soft_contact_forces(ocp, nlp, as_states: bool, as_controls: bool): if nlp.model.soft_contact_name(ii) not in name_soft_contact_forces ] ) - ConfigureProblem.configure_new_variable("fext", name_soft_contact_forces, ocp, nlp, as_states, as_controls) + ConfigureProblem.configure_new_variable( + "forces_in_global", name_soft_contact_forces, ocp, nlp, as_states, as_controls + ) @staticmethod def configure_muscles(ocp, nlp, as_states: bool, as_controls: bool, fatigue: FatigueList = None): diff --git a/bioptim/dynamics/dynamics_functions.py b/bioptim/dynamics/dynamics_functions.py index 5ae115a6a..6ba7fdb2a 100644 --- a/bioptim/dynamics/dynamics_functions.py +++ b/bioptim/dynamics/dynamics_functions.py @@ -97,8 +97,6 @@ def torque_driven( with_friction: bool, rigidbody_dynamics: RigidBodyDynamics, fatigue: FatigueList, - external_forces: np.ndarray = None, - translational_forces: np.ndarray = None, ) -> DynamicsEvaluation: """ Forward dynamics driven by joint torques, optional external forces can be declared. @@ -170,9 +168,9 @@ def torque_driven( dxdt[nlp.states["qdot"].index, :] = qddot dxdt[nlp.states["qddot"].index, :] = DynamicsFunctions.get(nlp.controls["qdddot"], controls) else: - ddq = DynamicsFunctions.forward_dynamics( - nlp, q, qdot, tau, with_contact, external_forces, translational_forces - ) + external_forces = nlp.get_external_forces(states, controls, algebraic_states, numerical_timeseries) + + ddq = DynamicsFunctions.forward_dynamics(nlp, q, qdot, tau, with_contact, external_forces) dxdt = nlp.cx(nlp.states.shape, ddq.shape[1]) dxdt[nlp.states["q"].index, :] = horzcat(*[dq for _ in range(ddq.shape[1])]) dxdt[nlp.states["qdot"].index, :] = ddq @@ -644,8 +642,6 @@ def forces_from_torque_driven( nlp, with_passive_torque: bool = False, with_ligament: bool = False, - external_forces: np.ndarray = None, - translational_forces: np.ndarray = None, ) -> MX | SX: """ Contact forces of a forward dynamics driven by joint torques with contact constraints. @@ -687,9 +683,9 @@ def forces_from_torque_driven( 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 - translational_forces = [] if translational_forces is None else translational_forces - return nlp.model.contact_forces()(q, qdot, tau, external_forces, translational_forces, nlp.parameters.cx) + external_forces = nlp.get_external_forces(states, controls, algebraic_states, numerical_timeseries) + + return nlp.model.contact_forces()(q, qdot, tau, external_forces, nlp.parameters.cx) @staticmethod def forces_from_torque_activation_driven( @@ -1110,7 +1106,6 @@ def forward_dynamics( tau: MX | SX, with_contact: bool, external_forces: list = None, - translational_forces: list = None, ): """ Easy accessor to derivative of qdot @@ -1129,8 +1124,6 @@ def forward_dynamics( If the dynamics with contact should be used external_forces: list[] The external forces - translational_forces: list[] - The translational forces Returns ------- The derivative of qdot @@ -1144,13 +1137,11 @@ 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 external_forces = [] if external_forces is None else external_forces - translational_forces = [] if translational_forces is None else translational_forces qddot = nlp.model.forward_dynamics(with_contact=with_contact)( q, qdot, tau, external_forces, - translational_forces, nlp.parameters.cx, ) return qdot_var_mapping.map(qddot) diff --git a/bioptim/examples/getting_started/example_external_forces.py b/bioptim/examples/getting_started/example_external_forces.py index f53467c18..6e0eb0292 100644 --- a/bioptim/examples/getting_started/example_external_forces.py +++ b/bioptim/examples/getting_started/example_external_forces.py @@ -104,7 +104,7 @@ def prepare_ocp( DynamicsFcn.TORQUE_DRIVEN, expand_dynamics=expand_dynamics, phase_dynamics=phase_dynamics, - numerical_data_timeseries={"external_forces": external_forces}, # the key word "external_forces" must be used + numerical_data_timeseries={"forces_in_global": external_forces}, # the key word "forces_in_global" must be used ) # Constraints diff --git a/bioptim/examples/torque_driven_ocp/maximize_predicted_height_CoM.py b/bioptim/examples/torque_driven_ocp/maximize_predicted_height_CoM.py index 3480d7d52..123b378dd 100644 --- a/bioptim/examples/torque_driven_ocp/maximize_predicted_height_CoM.py +++ b/bioptim/examples/torque_driven_ocp/maximize_predicted_height_CoM.py @@ -79,7 +79,7 @@ def prepare_ocp( The OptimalControlProgram ready to be solved """ - bio_model = BiorbdModel(biorbd_model_path, segments_to_apply_external_forces=["Seg1", "Seg1"]) + bio_model = BiorbdModel(biorbd_model_path, segments_to_apply_translational_forces=["Seg1", "Seg1"]) tau_min, tau_max = (-1, 1) if use_actuators else (-500, 500) dof_mapping = BiMappingList() @@ -152,7 +152,12 @@ def prepare_ocp( u_bounds["qddot"] = [tau_min] * bio_model.nb_qddot, [tau_max] * bio_model.nb_qddot elif rigidbody_dynamics == RigidBodyDynamics.DAE_INVERSE_DYNAMICS: u_bounds["qddot"] = [tau_min] * bio_model.nb_qddot, [tau_max] * bio_model.nb_qddot - u_bounds["fext"] = [tau_min] * bio_model.nb_contacts, [tau_max] * bio_model.nb_contacts + + min_forces_seg1 = [0, -tau_min, -tau_min, 0, 0, 0] + min_forces_seg2 = [0, 0, -tau_min, 0, 0, 0] + max_forces_seg1 = [0, tau_max, tau_max, 0, 0, 0] + max_forces_seg2 = [0, 0, tau_max, 0, 0, 0] + u_bounds["translational_forces"] = min_forces_seg1 + min_forces_seg2, max_forces_seg1 + max_forces_seg2 return OptimalControlProgram( bio_model, diff --git a/bioptim/limits/constraints.py b/bioptim/limits/constraints.py index c1050107b..67d750b98 100644 --- a/bioptim/limits/constraints.py +++ b/bioptim/limits/constraints.py @@ -420,7 +420,7 @@ def qddot_equals_forward_dynamics( qddot = controller.controls["qddot"].cx if "qddot" in controller.controls else controller.states["qddot"].cx # TODO: add external_forces qddot_fd = controller.model.forward_dynamics(with_contact=with_contact)( - controller.q, controller.qdot, tau, [], [], controller.parameters.cx + controller.q, controller.qdot, tau, [], controller.parameters.cx ) return qddot - qddot_fd @@ -456,9 +456,7 @@ def tau_equals_inverse_dynamics( tau = controller.states["tau"].cx if "tau" in controller.states else controller.tau qddot = controller.states["qddot"].cx if "qddot" in controller.states else controller.controls["qddot"].cx if with_passive_torque: - tau += controller.model.passive_joint_torque()( - controller.q, controller.qdot, controller.parameters.cx - ) + tau += controller.model.passive_joint_torque()(controller.q, controller.qdot, controller.parameters.cx) if with_ligament: tau += controller.model.ligament_joint_torque()(controller.q, controller.qdot, controller.parameters.cx) @@ -467,18 +465,19 @@ def tau_equals_inverse_dynamics( raise NotImplementedError( "This implicit constraint tau_equals_inverse_dynamics is not implemented yet with numerical_timeseries (external_forces or translational_forces)" ) - # Todo: add fext tau_id = nlp.model.inverse_dynamics()(q, qdot, qddot, fext) + # Todo: add forces_in_global tau_id = nlp.model.inverse_dynamics()(q, qdot, qddot, forces_in_global) if with_contact: # todo: this should be done internally in BiorbdModel - f_contact = ( - controller.controls["fext"].cx if "fext" in controller.controls else controller.states["fext"].cx + f_contact_vec = ( + controller.controls["translational_forces"].cx + if "translational_forces" in controller.controls + else controller.states["translational_forces"].cx ) - f_contact_vec = controller.model.reshape_fext_to_fcontact(f_contact, controller.parameters.cx) else: f_contact_vec = [] tau_id = controller.model.inverse_dynamics(with_contact=with_contact)( - controller.q, controller.qdot, qddot, f_contact_vec, [], controller.parameters.cx + controller.q, controller.qdot, qddot, f_contact_vec, controller.parameters.cx ) var = [] diff --git a/bioptim/models/biorbd/biorbd_model.py b/bioptim/models/biorbd/biorbd_model.py index d71a714d7..3a73cb9f9 100644 --- a/bioptim/models/biorbd/biorbd_model.py +++ b/bioptim/models/biorbd/biorbd_model.py @@ -27,7 +27,7 @@ def __init__( self, bio_model: str | biorbd.Model, friction_coefficients: np.ndarray = None, - segments_to_apply_external_forces: list[str] = None, + segments_to_apply_forces_in_global: list[str] = None, segments_to_apply_translational_forces: list[str] = None, parameters: ParameterList = None, ): @@ -39,14 +39,12 @@ def __init__( for param_key in parameters: parameters[param_key].apply_parameter(self) self._friction_coefficients = friction_coefficients - if segments_to_apply_external_forces is None: - segments_to_apply_external_forces = [] - self._segments_to_apply_external_forces = segments_to_apply_external_forces + if segments_to_apply_forces_in_global is None: + segments_to_apply_forces_in_global = [] + self._segments_to_apply_forces_in_global = segments_to_apply_forces_in_global if segments_to_apply_translational_forces is None: segments_to_apply_translational_forces = [] self._segments_to_apply_translational_forces = segments_to_apply_translational_forces - if len(segments_to_apply_external_forces) > 0 and len(segments_to_apply_translational_forces) > 0: - raise ValueError("You can't apply both external_forces and translational_forces at the same time") # Declaration of MX variables of the right shape for the creation of CasADi Functions self.q = MX.sym("q_mx", self.nb_q, 1) @@ -56,8 +54,13 @@ def __init__( self.tau = MX.sym("tau_mx", self.nb_tau, 1) self.muscle = MX.sym("muscle_mx", self.nb_muscles, 1) self.activations = MX.sym("activations_mx", self.nb_muscles, 1) - self.external_forces = MX.sym("external_forces_mx", 9, len(segments_to_apply_external_forces)) + + self.forces_in_global = MX.sym("forces_in_global", 9, len(segments_to_apply_forces_in_global)) self.translational_forces = MX.sym("translational_forces_mx", 6, len(segments_to_apply_translational_forces)) + self.external_forces = vertcat( + self.forces_in_global.reshape((-1, 1)), self.translational_forces.reshape((-1, 1)) + ) + # TODO: remove mx (the MX parameters should be created inside the BiorbdModel) self.parameters = parameters.mx if parameters else MX() @@ -422,27 +425,27 @@ def forward_dynamics_free_floating_base(self) -> Function: def reorder_qddot_root_joints(qddot_root, qddot_joints) -> MX | SX: return vertcat(qddot_root, qddot_joints) - def _dispatch_forces(self, external_forces, translational_forces): + def _dispatch_forces(self, forces_in_global, translational_forces): external_forces_set = self.model.externalForceSet() - if external_forces.shape[1] > 0: - if not isinstance(external_forces, MX): - raise ValueError("external_forces should be of shape 9 x nb_forces.") - if external_forces.shape[0] != 9: + if forces_in_global.shape[1] > 0: + if not isinstance(forces_in_global, MX): + raise ValueError("forces_in_global should be of shape 9 x nb_forces.") + if forces_in_global.shape[0] != 9: raise ValueError( - f"external_forces has {external_forces.shape[0]} rows, it should have 9 rows (Mx, My, Mz, Fx, Fy, Fz, Px, Py, Pz). You should provide the moments, forces and points of application." + f"forces_in_global has {forces_in_global.shape[0]} rows, it should have 9 rows (Mx, My, Mz, Fx, Fy, Fz, Px, Py, Pz). You should provide the moments, forces and points of application." ) - if len(self._segments_to_apply_external_forces) != external_forces.shape[1]: + if len(self._segments_to_apply_forces_in_global) != forces_in_global.shape[1]: raise ValueError( - f"external_forces has {external_forces.shape[1]} columns and {len(self._segments_to_apply_external_forces)} segments to apply forces on, they should have the same length." + f"forces_in_global has {forces_in_global.shape[1]} columns and {len(self._segments_to_apply_forces_in_global)} segments to apply forces_in_global on, they should have the same length." ) # Add the external forces - for i_element in range(external_forces.shape[1]): - name = self._segments_to_apply_external_forces[i_element] - values = external_forces[:6, i_element] - point_of_application = external_forces[6:9, i_element] + for i_element in range(forces_in_global.shape[1]): + name = self._segments_to_apply_forces_in_global[i_element] + values = forces_in_global[:6, i_element] + point_of_application = forces_in_global[6:9, i_element] external_forces_set.add(name, values, point_of_application) elif translational_forces.shape[1] > 0: @@ -452,14 +455,14 @@ def _dispatch_forces(self, external_forces, translational_forces): raise ValueError( f"translational_forces has {translational_forces.shape[0]} rows, it should have 6 rows (Fx, Fy, Fz, Px, Py, Pz). You should provide the forces and points of application." ) - if len(self._segments_to_apply_external_forces) != translational_forces.shape[1]: + if len(self._segments_to_apply_translational_forces) != translational_forces.shape[1]: raise ValueError( - f"translational_forces has {translational_forces.shape[1]} columns and {len(self._segments_to_apply_external_forces)} segments to apply forces on, they should have the same length." + f"translational_forces has {translational_forces.shape[1]} columns and {len(self._segments_to_apply_translational_forces)} segments to apply forces on, they should have the same length." ) # Add the translational forces for i_elements in range(translational_forces.shape[1]): - name = self._segments_to_apply_external_forces[i_elements] + name = self._segments_to_apply_translational_forces[i_elements] values = translational_forces[:3, i_elements] point_of_application = translational_forces[3:6, i_elements] external_forces_set.addTranslationalForce(values, name, point_of_application) @@ -467,7 +470,7 @@ def _dispatch_forces(self, external_forces, translational_forces): return external_forces_set def forward_dynamics(self, with_contact: bool = False) -> Function: - external_forces_set = self._dispatch_forces(self.external_forces, self.translational_forces) + external_forces_set = self._dispatch_forces(self.forces_in_global, self.translational_forces) q_biorbd = GeneralizedCoordinates(self.q) qdot_biorbd = GeneralizedVelocity(self.qdot) @@ -479,29 +482,24 @@ 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.translational_forces, self.parameters], + [self.q, self.qdot, self.tau, self.external_forces, self.parameters], [biorbd_return], - ["q", "qdot", "tau", "external_forces", "translational_forces", "parameters"], + ["q", "qdot", "tau", "external_forces", "parameters"], ["qddot"], ) 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.translational_forces, self.parameters], + [self.q, self.qdot, self.tau, self.external_forces, self.parameters], [biorbd_return], - ["q", "qdot", "tau", "external_forces", "translational_forces", "parameters"], + ["q", "qdot", "tau", "external_forces", "parameters"], ["qddot"], ) return casadi_fun def inverse_dynamics(self, with_contact: bool = False) -> Function: - if with_contact: - external_forces_set = self.reshape_fext_to_fcontact(self.external_forces, self.parameters) - else: - external_forces_set = self.external_forces - - # external_forces_set = self._dispatch_forces(external_forces, self.translational_forces) + external_forces_set = self._dispatch_forces(self.forces_in_global, self.translational_forces) q_biorbd = GeneralizedCoordinates(self.q) qdot_biorbd = GeneralizedVelocity(self.qdot) @@ -509,15 +507,15 @@ 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.translational_forces, self.parameters], + [self.q, self.qdot, self.qddot, self.external_forces, self.parameters], [biorbd_return], - ["q", "qdot", "qddot", "external_forces", "translational_forces", "parameters"], + ["q", "qdot", "qddot", "external_forces", "parameters"], ["tau"], ) return casadi_fun def contact_forces_from_constrained_forward_dynamics(self) -> Function: - external_forces_set = self._dispatch_forces(self.external_forces, self.translational_forces) + external_forces_set = self._dispatch_forces(self.forces_in_global, self.translational_forces) q_biorbd = GeneralizedCoordinates(self.q) qdot_biorbd = GeneralizedVelocity(self.qdot) @@ -527,9 +525,9 @@ 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.translational_forces, self.parameters], + [self.q, self.qdot, self.tau, self.external_forces, self.parameters], [biorbd_return], - ["q", "qdot", "tau", "external_forces", "translational_forces", "parameters"], + ["q", "qdot", "tau", "external_forces", "parameters"], ["contact_forces"], ) return casadi_fun @@ -840,35 +838,6 @@ def soft_contact_forces(self) -> Function: ) return casadi_fun - def reshape_fext_to_fcontact(self, fext: MX | SX, parameters: MX | SX): - if len(self._segments_to_apply_external_forces) == 0: - parent_name = [] - for i in range(self.nb_rigid_contacts): - contact = self.model.rigidContact(i) - parent_name += [ - self.model.segment(self.model.getBodyRbdlIdToBiorbdId(contact.parentId())).name().to_string() - ] - self._segments_to_apply_external_forces = parent_name - - if self.nb_soft_contacts > 0: - raise NotImplementedError("Soft contact not implemented yet with external forces.") - - count = 0 - fext_force_set = biorbd.ExternalForceSet(self.model, True, False) - for i in range(self.nb_rigid_contacts): - contact = self.model.rigidContact(i) - tp = MX.zeros(3) - used_axes = [i for i, val in enumerate(contact.axes()) if val] - n_contacts = len(used_axes) - tp[used_axes] = fext[count : count + n_contacts] - fext_force_set.addTranslationalForce( - tp, # Translational force - self._segments_to_apply_external_forces[i], # Segment name - contact.to_mx() # Point of application - ) - count += n_contacts - return fext_force_set - def normalize_state_quaternions(self) -> Function: quat_idx = self.get_quaternion_idx() @@ -909,13 +878,13 @@ def get_quaternion_idx(self) -> list[list[int]]: def contact_forces(self) -> Function: force = self.contact_forces_from_constrained_forward_dynamics()( - self.q, self.qdot, self.tau, self.external_forces, self.translational_forces, self.parameters + self.q, self.qdot, self.tau, self.external_forces, self.parameters ) casadi_fun = Function( "contact_forces", - [self.q, self.qdot, self.tau, self.external_forces, self.translational_forces, self.parameters], + [self.q, self.qdot, self.tau, self.external_forces, self.parameters], [force], - ["q", "qdot", "tau", "external_forces", "translational_forces", "parameters"], + ["q", "qdot", "tau", "external_forces", "parameters"], ["contact_forces"], ) return casadi_fun @@ -1016,7 +985,7 @@ def lagrangian(self) -> Function: ) return casadi_fun - def partitioned_forward_dynamics(self, external_forces=None, f_contacts=None, q_v_init=None): + def partitioned_forward_dynamics(self, forces_in_global=None, f_contacts=None, q_v_init=None): raise NotImplementedError("partitioned_forward_dynamics is not implemented for BiorbdModel") @staticmethod diff --git a/bioptim/optimization/non_linear_program.py b/bioptim/optimization/non_linear_program.py index a745dd26e..d9c77d653 100644 --- a/bioptim/optimization/non_linear_program.py +++ b/bioptim/optimization/non_linear_program.py @@ -450,3 +450,44 @@ def get_var_from_states_or_controls( else: raise RuntimeError(f"{key} not found in states or controls") return out + + def get_external_forces( + self, states: MX.sym, controls: MX.sym, algebraic_states: MX.sym, numerical_timeseries: MX.sym + ): + external_forces = self.cx(0, 1) + + if "forces_in_global" in self.states: + external_forces = vertcat(external_forces, DynamicsFunctions.get(self.states["forces_in_global"], states)) + if "forces_in_global" in self.algebraic_states: + external_forces = vertcat( + external_forces, DynamicsFunctions.get(self.algebraic_states["forces_in_global"], algebraic_states) + ) + if "forces_in_global" in self.controls: + external_forces = vertcat( + external_forces, DynamicsFunctions.get(self.controls["forces_in_global"], controls) + ) + if "forces_in_global" in self.numerical_timeseries: + external_forces = vertcat( + external_forces, + DynamicsFunctions.get(self.numerical_timeseries["forces_in_global"], numerical_timeseries), + ) + + if "translational_forces" in self.states: + external_forces = vertcat( + external_forces, DynamicsFunctions.get(self.states["translational_forces"], states) + ) + if "translational_forces" in self.algebraic_states: + external_forces = vertcat( + external_forces, DynamicsFunctions.get(self.algebraic_states["translational_forces"], algebraic_states) + ) + if "translational_forces" in self.controls: + external_forces = vertcat( + external_forces, DynamicsFunctions.get(self.controls["translational_forces"], controls) + ) + if "translational_forces" in self.numerical_timeseries: + external_forces = vertcat( + external_forces, + DynamicsFunctions.get(self.numerical_timeseries["translational_forces"], numerical_timeseries), + ) + + return external_forces diff --git a/tests/shard1/test_dynamics.py b/tests/shard1/test_dynamics.py index 298ce80b3..2e5287831 100644 --- a/tests/shard1/test_dynamics.py +++ b/tests/shard1/test_dynamics.py @@ -51,7 +51,7 @@ def test_torque_driven(with_contact, with_external_force, cx, rigidbody_dynamics nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx == SX)) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod", - segments_to_apply_external_forces=["Seg0"], + segments_to_apply_forces_in_global=["Seg0"], ) nlp.ns = 5 nlp.cx = cx @@ -136,7 +136,7 @@ def test_torque_driven(with_contact, with_external_force, cx, rigidbody_dynamics rigidbody_dynamics=rigidbody_dynamics, expand_dynamics=True, phase_dynamics=phase_dynamics, - numerical_data_timeseries=({"external_forces": external_forces} if external_forces is not None else None), + numerical_data_timeseries=({"forces_in_global": external_forces} if external_forces is not None else None), ), False, ) @@ -514,7 +514,7 @@ def test_torque_derivative_driven(with_contact, with_external_force, cx, phase_d with_contact=with_contact, expand_dynamics=True, phase_dynamics=phase_dynamics, - numerical_data_timeseries=({"external_forces": external_forces} if external_forces is not None else None), + numerical_data_timeseries=({"forces_in_global": external_forces} if external_forces is not None else None), ), False, ) @@ -1022,7 +1022,7 @@ def test_torque_activation_driven(with_contact, with_external_force, cx, phase_d with_contact=with_contact, expand_dynamics=True, phase_dynamics=phase_dynamics, - numerical_data_timeseries=({"external_forces": external_forces} if external_forces is not None else None), + numerical_data_timeseries=({"forces_in_global": external_forces} if external_forces is not None else None), ), False, ) @@ -1202,7 +1202,7 @@ def test_torque_activation_driven_with_residual_torque( with_residual_torque=with_residual_torque, expand_dynamics=True, phase_dynamics=phase_dynamics, - numerical_data_timeseries=({"external_forces": external_forces} if external_forces is not None else None), + numerical_data_timeseries=({"forces_in_global": external_forces} if external_forces is not None else None), ), False, ) @@ -1453,7 +1453,7 @@ def test_muscle_driven( rigidbody_dynamics=rigidbody_dynamics, expand_dynamics=True, phase_dynamics=phase_dynamics, - numerical_data_timeseries=({"external_forces": external_forces} if external_forces is not None else None), + numerical_data_timeseries=({"forces_in_global": external_forces} if external_forces is not None else None), ), False, ) From 0fd63364cb031c73d3aff00985b27ed5c9c2a9a3 Mon Sep 17 00:00:00 2001 From: eve-mac Date: Sat, 5 Oct 2024 10:20:14 -0400 Subject: [PATCH 041/108] continued + removed self.translation... --- bioptim/dynamics/configure_problem.py | 34 +----- bioptim/dynamics/dynamics_functions.py | 98 +++++---------- .../getting_started/custom_dynamics.py | 2 +- .../examples/torque_driven_ocp/spring_load.py | 2 +- bioptim/models/biorbd/biorbd_model.py | 115 ++++++++++-------- tests/shard6/test_time_dependent_problems.py | 2 +- 6 files changed, 96 insertions(+), 157 deletions(-) diff --git a/bioptim/dynamics/configure_problem.py b/bioptim/dynamics/configure_problem.py index ecf9d5d36..5558504d7 100644 --- a/bioptim/dynamics/configure_problem.py +++ b/bioptim/dynamics/configure_problem.py @@ -624,15 +624,6 @@ def torque_derivative_driven( _check_soft_contacts_dynamics( rigidbody_dynamics, soft_contacts_dynamics, nlp.model.nb_soft_contacts, nlp.phase_idx ) - # TODO REMOVE THIS? - # external_forces = None - # if numerical_data_timeseries is not None: - # for key in numerical_data_timeseries.keys(): - # if key == "forces_in_global": - # _check_numerical_timeseries_format(numerical_data_timeseries[key], nlp.ns, nlp.phase_idx) - # external_forces = nlp.numerical_timeseries[0].cx - # for i in range(1, numerical_data_timeseries[key].shape[1]): - # external_forces = horzcat(external_forces, nlp.numerical_timeseries[i].cx) ConfigureProblem.configure_q(ocp, nlp, True, False) ConfigureProblem.configure_qdot(ocp, nlp, True, False) @@ -662,7 +653,6 @@ def torque_derivative_driven( rigidbody_dynamics=rigidbody_dynamics, with_passive_torque=with_passive_torque, with_ligament=with_ligament, - external_forces=external_forces, ) if with_contact: @@ -670,7 +660,6 @@ def torque_derivative_driven( ocp, nlp, DynamicsFunctions.forces_from_torque_driven, - external_forces=external_forces, ) ConfigureProblem.configure_soft_contact_function(ocp, nlp) @@ -716,14 +705,6 @@ def torque_activations_driven( """ _check_contacts_in_biorbd_model(with_contact, nlp.model.nb_contacts, nlp.phase_idx) - external_forces = None - if numerical_data_timeseries is not None: - for key in numerical_data_timeseries.keys(): - if key == "external_forces": - _check_numerical_timeseries_format(numerical_data_timeseries[key], nlp.ns, nlp.phase_idx) - external_forces = nlp.numerical_timeseries[0].cx - for i in range(1, numerical_data_timeseries[key].shape[1]): - external_forces = horzcat(external_forces, nlp.numerical_timeseries[i].cx) ConfigureProblem.configure_q(ocp, nlp, True, False) ConfigureProblem.configure_qdot(ocp, nlp, True, False) @@ -743,12 +724,11 @@ def torque_activations_driven( with_passive_torque=with_passive_torque, with_residual_torque=with_residual_torque, with_ligament=with_ligament, - external_forces=external_forces, ) if with_contact: ConfigureProblem.configure_contact_function( - ocp, nlp, DynamicsFunctions.forces_from_torque_activation_driven, external_forces=external_forces + ocp, nlp, DynamicsFunctions.forces_from_torque_activation_driven ) ConfigureProblem.configure_soft_contact_function(ocp, nlp) @@ -852,14 +832,7 @@ def muscle_driven( A list of values to pass to the dynamics at each node. Experimental external forces should be included here. """ _check_contacts_in_biorbd_model(with_contact, nlp.model.nb_contacts, nlp.phase_idx) - external_forces = None - if numerical_data_timeseries is not None: - for key in numerical_data_timeseries.keys(): - if key == "external_forces": - _check_numerical_timeseries_format(numerical_data_timeseries[key], nlp.ns, nlp.phase_idx) - external_forces = nlp.numerical_timeseries[0].cx - for i in range(1, numerical_data_timeseries[key].shape[1]): - external_forces = horzcat(external_forces, nlp.numerical_timeseries[i].cx) + if fatigue is not None and "tau" in fatigue and not with_residual_torque: raise RuntimeError("Residual torques need to be used to apply fatigue on torques") @@ -898,12 +871,11 @@ def muscle_driven( with_passive_torque=with_passive_torque, with_ligament=with_ligament, rigidbody_dynamics=rigidbody_dynamics, - external_forces=external_forces, ) if with_contact: ConfigureProblem.configure_contact_function( - ocp, nlp, DynamicsFunctions.forces_from_muscle_driven, external_forces=external_forces + ocp, nlp, DynamicsFunctions.forces_from_muscle_driven, ) ConfigureProblem.configure_soft_contact_function(ocp, nlp) diff --git a/bioptim/dynamics/dynamics_functions.py b/bioptim/dynamics/dynamics_functions.py index 6ba7fdb2a..7a4c1941d 100644 --- a/bioptim/dynamics/dynamics_functions.py +++ b/bioptim/dynamics/dynamics_functions.py @@ -17,11 +17,11 @@ class DynamicsFunctions: custom(states: MX.sym, controls: MX.sym, parameters: MX.sym, nlp: NonLinearProgram) -> MX Interface to custom dynamic function provided by the user torque_driven(states: MX.sym, controls: MX.sym, parameters: MX.sym, nlp, with_contact: bool) - Forward dynamics driven by joint torques, optional external forces can be declared. + Forward dynamics driven by joint torques torque_activations_driven(states: MX.sym, controls: MX.sym, parameters: MX.sym, nlp, with_contact) -> MX: Forward dynamics driven by joint torques activations. torque_derivative_driven(states: MX.sym, controls: MX.sym, parameters: MX.sym, nlp, with_contact: bool) -> MX: - Forward dynamics driven by joint torques, optional external forces can be declared. + Forward dynamics driven by joint torques derivatives forces_from_torque_driven(states: MX.sym, controls: MX.sym, parameters: MX.sym, nlp) -> MX: Contact forces of a forward dynamics driven by joint torques with contact constraints. muscles_driven(states: MX.sym, controls: MX.sym, parameters: MX.sym, nlp, with_contact: bool) -> MX: @@ -99,7 +99,7 @@ def torque_driven( fatigue: FatigueList, ) -> DynamicsEvaluation: """ - Forward dynamics driven by joint torques, optional external forces can be declared. + Forward dynamics driven by joint torques Parameters ---------- @@ -129,17 +129,12 @@ def torque_driven( which rigidbody dynamics should be used fatigue : FatigueList A list of fatigue elements - external_forces: np.ndarray - The external forces - translational_forces: np.ndarray - The translational forces Returns ---------- DynamicsEvaluation The derivative of the states and the defects of the implicit dynamics """ - # Charbie: Where are supposed to by applied soft_contacts ? q = DynamicsFunctions.get(nlp.states["q"], states) qdot = DynamicsFunctions.get(nlp.states["qdot"], states) @@ -185,8 +180,9 @@ def torque_driven( ): if not with_contact and fatigue is None: qddot = DynamicsFunctions.get(nlp.states_dot["qddot"], nlp.states_dot.scaled.cx) + external_forces = nlp.get_external_forces(states, controls, algebraic_states, numerical_timeseries) tau_id = DynamicsFunctions.inverse_dynamics( - nlp, q, qdot, qddot, with_contact, external_forces, translational_forces + nlp, q, qdot, qddot, with_contact, external_forces ) defects = nlp.cx(dq.shape[0] + tau_id.shape[0], tau_id.shape[1]) @@ -220,7 +216,7 @@ def torque_driven_free_floating_base( with_friction: bool, ) -> DynamicsEvaluation: """ - Forward dynamics driven by joint torques without actuation of the free floating base, optional external forces can be declared. + Forward dynamics driven by joint torques without actuation of the free floating base Parameters ---------- @@ -273,7 +269,7 @@ def torque_driven_free_floating_base( tau_full = vertcat(nlp.cx.zeros(nlp.model.nb_root), tau_joints) - ddq = DynamicsFunctions.forward_dynamics(nlp, q_full, qdot_full, tau_full, with_contact=False) + ddq = DynamicsFunctions.forward_dynamics(nlp, q_full, qdot_full, tau_full, with_contact=False, external_forces=None) dxdt = nlp.cx(n_q + n_qdot, ddq.shape[1]) dxdt[:n_q, :] = horzcat(*[dq for _ in range(ddq.shape[1])]) dxdt[n_q:, :] = ddq @@ -295,7 +291,7 @@ def stochastic_torque_driven( with_friction: bool, ) -> DynamicsEvaluation: """ - Forward dynamics subject to motor and sensory noise driven by torques, optional external forces can be declared. + Forward dynamics subject to motor and sensory noise driven by torques Parameters ---------- @@ -344,7 +340,7 @@ def stochastic_torque_driven( tau = tau - nlp.model.friction_coefficients @ qdot if with_friction else tau dq = DynamicsFunctions.compute_qdot(nlp, q, qdot) - ddq = DynamicsFunctions.forward_dynamics(nlp, q, qdot, tau, with_contact) + ddq = DynamicsFunctions.forward_dynamics(nlp, q, qdot, tau, with_contact, external_forces=None) dxdt = nlp.cx(nlp.states.shape, ddq.shape[1]) dxdt[nlp.states["q"].index, :] = horzcat(*[dq for _ in range(ddq.shape[1])]) dxdt[nlp.states["qdot"].index, :] = ddq @@ -363,7 +359,7 @@ def stochastic_torque_driven_free_floating_base( with_friction: bool, ) -> DynamicsEvaluation: """ - Forward dynamics subject to motor and sensory noise driven by joint torques, optional external forces can be declared. + Forward dynamics subject to motor and sensory noise driven by joint torques Parameters ---------- @@ -417,7 +413,7 @@ def stochastic_torque_driven_free_floating_base( tau_full = vertcat(nlp.cx.zeros(nlp.model.nb_root), tau_joints) dq = DynamicsFunctions.compute_qdot(nlp, q_full, qdot_full) - ddq = DynamicsFunctions.forward_dynamics(nlp, q_full, qdot_full, tau_full, with_contact=False) + ddq = DynamicsFunctions.forward_dynamics(nlp, q_full, qdot_full, tau_full, with_contact=False, external_forces=None) dxdt = nlp.cx(nlp.states.shape, ddq.shape[1]) dxdt[:n_q, :] = horzcat(*[dq for _ in range(ddq.shape[1])]) dxdt[n_q:, :] = ddq @@ -492,7 +488,6 @@ def torque_activations_driven( with_passive_torque: bool, with_residual_torque: bool, with_ligament: bool, - external_forces: np.ndarray = None, ): """ Forward dynamics driven by joint torques activations. @@ -521,8 +516,6 @@ def torque_activations_driven( If the dynamic should be added with residual torques with_ligament: bool If the dynamic with ligament should be used - external_forces: np.ndarray - The external forces Returns ---------- @@ -542,6 +535,8 @@ def torque_activations_driven( if with_ligament: tau += nlp.model.ligament_joint_torque()(q, qdot, nlp.parameters.cx) + external_forces = nlp.get_external_forces(time, states, controls, parameters, algebraic_states, numerical_timeseries) + dq = DynamicsFunctions.compute_qdot(nlp, q, qdot) ddq = DynamicsFunctions.forward_dynamics(nlp, q, qdot, tau, with_contact, external_forces) @@ -562,10 +557,9 @@ def torque_derivative_driven( with_contact: bool, with_passive_torque: bool, with_ligament: bool, - external_forces: np.ndarray = None, ) -> DynamicsEvaluation: """ - Forward dynamics driven by joint torques, optional external forces can be declared. + Forward dynamics driven by joint torques derivatives Parameters ---------- @@ -591,8 +585,6 @@ def torque_derivative_driven( If the dynamic with passive torque should be used with_ligament: bool If the dynamic with ligament should be used - external_forces: np.ndarray - The external forces Returns ---------- @@ -623,6 +615,7 @@ def torque_derivative_driven( dxdt[nlp.states["qddot"].index, :] = dddq dxdt[nlp.states["tau"].index, :] = dtau else: + external_forces = nlp.get_external_forces(time, states, controls, parameters, algebraic_states, numerical_timeseries) ddq = DynamicsFunctions.forward_dynamics(nlp, q, qdot, tau, with_contact, external_forces) dxdt = nlp.cx(nlp.states.shape, ddq.shape[1]) dxdt[nlp.states["q"].index, :] = horzcat(*[dq for _ in range(ddq.shape[1])]) @@ -666,10 +659,6 @@ def forces_from_torque_driven( If the dynamic with passive torque should be used with_ligament: bool If the dynamic with ligament should be used - external_forces: np.ndarray - The external forces - translational_forces: np.ndarray - The translational forces Returns ---------- @@ -698,8 +687,6 @@ def forces_from_torque_activation_driven( nlp, with_passive_torque: bool = False, with_ligament: bool = False, - external_forces: np.ndarray = None, - translational_forces: np.ndarray = None, ) -> MX | SX: """ Contact forces of a forward dynamics driven by joint torques with contact constraints. @@ -724,10 +711,6 @@ def forces_from_torque_activation_driven( If the dynamic with passive torque should be used with_ligament: bool If the dynamic with ligament should be used - external_forces: np.ndarray - The external forces - translational_forces: np.ndarray - The translational forces Returns ---------- @@ -741,9 +724,8 @@ def forces_from_torque_activation_driven( 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 - translational_forces = [] if translational_forces is None else translational_forces - return nlp.model.contact_forces()(q, qdot, tau, external_forces, translational_forces, nlp.parameters.cx) + external_forces = nlp.get_external_forces(states, controls, algebraic_states, numerical_timeseries) + return nlp.model.contact_forces()(q, qdot, tau, external_forces, nlp.parameters.cx) @staticmethod def muscles_driven( @@ -760,8 +742,6 @@ def muscles_driven( rigidbody_dynamics: RigidBodyDynamics = RigidBodyDynamics.ODE, with_residual_torque: bool = False, fatigue=None, - external_forces: np.ndarray = None, - translational_forces: np.ndarray = None, ) -> DynamicsEvaluation: """ Forward dynamics driven by muscle. @@ -794,10 +774,6 @@ def muscles_driven( To define fatigue elements with_residual_torque: bool If the dynamic should be added with residual torques - external_forces: np.ndarray - The external forces - translational_forces: np.ndarray - The translational forces Returns ---------- @@ -857,6 +833,7 @@ def muscles_driven( dxdt[nlp.states["q"].index, :] = dq dxdt[nlp.states["qdot"].index, :] = DynamicsFunctions.get(nlp.controls["qddot"], controls) else: + external_forces = nlp.get_external_forces(states, controls, algebraic_states, numerical_timeseries) ddq = DynamicsFunctions.forward_dynamics(nlp, q, qdot, tau, with_contact, external_forces) dxdt = nlp.cx(nlp.states.shape, ddq.shape[1]) dxdt[nlp.states["q"].index, :] = horzcat(*[dq for _ in range(ddq.shape[1])]) @@ -878,8 +855,9 @@ def muscles_driven( ): if not with_contact and fatigue is None: qddot = DynamicsFunctions.get(nlp.states_dot["qddot"], nlp.states_dot.cx) + external_forces = nlp.get_external_forces(states, controls, algebraic_states, numerical_timeseries) tau_id = DynamicsFunctions.inverse_dynamics( - nlp, q, qdot, qddot, with_contact, external_forces, translational_forces + nlp, q, qdot, qddot, with_contact, external_forces ) defects = nlp.cx(dq.shape[0] + tau_id.shape[0], tau_id.shape[1]) @@ -909,8 +887,6 @@ def forces_from_muscle_driven( nlp, with_passive_torque: bool = False, with_ligament: bool = False, - external_forces: list = None, - translational_forces: list = None, ) -> MX | SX: """ Contact forces of a forward dynamics driven by muscles activations and joint torques with contact constraints. @@ -935,10 +911,6 @@ def forces_from_muscle_driven( If the dynamic with passive torque should be used with_ligament: bool If the dynamic with ligament should be used - external_forces: list[Any] - The external forces - translational_forces: list[Any] - The translational forces Returns ---------- @@ -956,9 +928,8 @@ def forces_from_muscle_driven( 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 - translational_forces = [] if translational_forces is None else translational_forces - return nlp.model.contact_forces()(q, qdot, tau, external_forces, translational_forces, nlp.parameters.cx) + external_forces = nlp.get_external_forces(states, controls, algebraic_states, numerical_timeseries) + return nlp.model.contact_forces()(q, qdot, tau, external_forces, nlp.parameters.cx) @staticmethod def joints_acceleration_driven( @@ -1154,7 +1125,6 @@ def inverse_dynamics( qddot: MX | SX, with_contact: bool, external_forces: MX = None, - translational_forces: MX = None, ): """ Easy accessor to torques from inverse dynamics @@ -1173,15 +1143,13 @@ def inverse_dynamics( If the dynamics with contact should be used external_forces: MX The external forces - translational_forces: MX - The translational forces Returns ------- Torques in tau """ - if external_forces is None and translational_forces is None: - tau = nlp.model.inverse_dynamics(with_contact=with_contact)(q, qdot, qddot, [], [], nlp.parameters.cx) + if external_forces is None: + 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] @@ -1191,16 +1159,13 @@ def inverse_dynamics( tau_shape = nlp.model.nb_tau tau = nlp.cx(tau_shape, nlp.ns) + # @pariterre not sure of this part if external_forces is not None: for i in range(external_forces.shape[1]): tau[:, i] = nlp.model.inverse_dynamics(with_contact=with_contact)( - q, qdot, qddot, external_forces[:, i], [], nlp.parameters.cx - ) - elif translational_forces is not None: - for i in range(translational_forces.shape[1]): - tau[:, i] = nlp.model.inverse_dynamics(with_contact=with_contact)( - q, qdot, qddot, [], translational_forces[:, i], nlp.parameters.cx + 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 @@ -1270,8 +1235,6 @@ def holonomic_torque_driven( algebraic_states, numerical_timeseries, nlp, - external_forces: list = None, - translational_forces: list = None, ) -> DynamicsEvaluation: """ The custom dynamics function that provides the derivative of the states: dxdt = f(t, x, u, p, a, d) @@ -1292,10 +1255,6 @@ def holonomic_torque_driven( The numerical timeseries of the system nlp: NonLinearProgram A reference to the phase - external_forces: list[Any] - The external forces - translational_forces: list[Any] - The translational forces Returns ------- @@ -1305,8 +1264,9 @@ 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) + external_forces = nlp.get_external_forces(states, controls, algebraic_states, numerical_timeseries) qddot_u = nlp.model.partitioned_forward_dynamics( - q_u, qdot_u, tau, external_forces, translational_forces, nlp.parameters.cx + 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_dynamics.py b/bioptim/examples/getting_started/custom_dynamics.py index 228c211ef..2e53a5a30 100644 --- a/bioptim/examples/getting_started/custom_dynamics.py +++ b/bioptim/examples/getting_started/custom_dynamics.py @@ -74,7 +74,7 @@ def custom_dynamics( # You can directly call biorbd function (as for ddq) or call bioptim accessor (as for dq) dq = DynamicsFunctions.compute_qdot(nlp, q, qdot) * my_additional_factor - ddq = nlp.model.forward_dynamics()(q, qdot, tau, [], [], nlp.parameters.cx) + ddq = nlp.model.forward_dynamics()(q, qdot, tau, [], nlp.parameters.cx) # the user has to choose if want to return the explicit dynamics dx/dt = f(x,u,p) # as the first argument of DynamicsEvaluation or diff --git a/bioptim/examples/torque_driven_ocp/spring_load.py b/bioptim/examples/torque_driven_ocp/spring_load.py index 3319261f5..4d839df9d 100644 --- a/bioptim/examples/torque_driven_ocp/spring_load.py +++ b/bioptim/examples/torque_driven_ocp/spring_load.py @@ -151,7 +151,7 @@ def custom_dynamic( stiffness = 100 force_vector[5] = -sign(q[0]) * stiffness * q[0] ** 2 # traction-compression spring - qddot = nlp.model.forward_dynamics(with_contact=False)(q, qdot, tau, force_vector, [], []) + qddot = nlp.model.forward_dynamics(with_contact=False)(q, qdot, tau, force_vector, []) return DynamicsEvaluation(dxdt=vertcat(qdot, qddot), defects=None) diff --git a/bioptim/models/biorbd/biorbd_model.py b/bioptim/models/biorbd/biorbd_model.py index 3a73cb9f9..357374cfe 100644 --- a/bioptim/models/biorbd/biorbd_model.py +++ b/bioptim/models/biorbd/biorbd_model.py @@ -55,15 +55,20 @@ def __init__( self.muscle = MX.sym("muscle_mx", self.nb_muscles, 1) self.activations = MX.sym("activations_mx", self.nb_muscles, 1) - self.forces_in_global = MX.sym("forces_in_global", 9, len(segments_to_apply_forces_in_global)) - self.translational_forces = MX.sym("translational_forces_mx", 6, len(segments_to_apply_translational_forces)) - self.external_forces = vertcat( - self.forces_in_global.reshape((-1, 1)), self.translational_forces.reshape((-1, 1)) - ) + self.external_forces = MX.sym("external_forces_mx", 9*self.nb_forces_in_global + 6*self.nb_translational_forces, 1) + self.external_forces_set = self._dispatch_forces() # TODO: remove mx (the MX parameters should be created inside the BiorbdModel) self.parameters = parameters.mx if parameters else MX() + @property + def nb_forces_in_global(self) -> int: + return len(self._segments_to_apply_forces_in_global) + + @property + def nb_translational_forces(self) -> int: + return len(self._segments_to_apply_translational_forces) + @property def name(self) -> str: # parse the path and split to get the .bioMod name @@ -425,52 +430,56 @@ def forward_dynamics_free_floating_base(self) -> Function: def reorder_qddot_root_joints(qddot_root, qddot_joints) -> MX | SX: return vertcat(qddot_root, qddot_joints) - def _dispatch_forces(self, forces_in_global, translational_forces): - - external_forces_set = self.model.externalForceSet() - - if forces_in_global.shape[1] > 0: - if not isinstance(forces_in_global, MX): - raise ValueError("forces_in_global should be of shape 9 x nb_forces.") - if forces_in_global.shape[0] != 9: - raise ValueError( - f"forces_in_global has {forces_in_global.shape[0]} rows, it should have 9 rows (Mx, My, Mz, Fx, Fy, Fz, Px, Py, Pz). You should provide the moments, forces and points of application." - ) - if len(self._segments_to_apply_forces_in_global) != forces_in_global.shape[1]: - raise ValueError( - f"forces_in_global has {forces_in_global.shape[1]} columns and {len(self._segments_to_apply_forces_in_global)} segments to apply forces_in_global on, they should have the same length." - ) + def _dispatch_forces(self): - # Add the external forces - for i_element in range(forces_in_global.shape[1]): - name = self._segments_to_apply_forces_in_global[i_element] - values = forces_in_global[:6, i_element] - point_of_application = forces_in_global[6:9, i_element] - external_forces_set.add(name, values, point_of_application) - - elif translational_forces.shape[1] > 0: - if not isinstance(translational_forces, MX): - raise ValueError("translational_forces should be of shape 6 x nb_forces.") - if translational_forces.shape[0] != 6: - raise ValueError( - f"translational_forces has {translational_forces.shape[0]} rows, it should have 6 rows (Fx, Fy, Fz, Px, Py, Pz). You should provide the forces and points of application." - ) - if len(self._segments_to_apply_translational_forces) != translational_forces.shape[1]: - raise ValueError( - f"translational_forces has {translational_forces.shape[1]} columns and {len(self._segments_to_apply_translational_forces)} segments to apply forces on, they should have the same length." - ) - - # Add the translational forces - for i_elements in range(translational_forces.shape[1]): - name = self._segments_to_apply_translational_forces[i_elements] - values = translational_forces[:3, i_elements] - point_of_application = translational_forces[3:6, i_elements] - external_forces_set.addTranslationalForce(values, name, point_of_application) - - return external_forces_set + if self.nb_forces_in_global == 0 and self.nb_translational_forces == 0: + return None + else: + external_forces_set = self.model.externalForceSet() + forces_in_global = self.external_forces[:9*self.nb_forces_in_global].reshape(9, self.nb_forces_in_global) + translational_forces = self.external_forces[9*self.nb_forces_in_global:].reshape(6, self.nb_translational_forces) + + if forces_in_global.shape[1] > 0: + if not isinstance(forces_in_global, MX): + raise ValueError("forces_in_global should be of shape 9 x nb_forces.") + if forces_in_global.shape[0] != 9: + raise ValueError( + f"forces_in_global has {forces_in_global.shape[0]} rows, it should have 9 rows (Mx, My, Mz, Fx, Fy, Fz, Px, Py, Pz). You should provide the moments, forces and points of application." + ) + if len(self._segments_to_apply_forces_in_global) != forces_in_global.shape[1]: + raise ValueError( + f"forces_in_global has {forces_in_global.shape[1]} columns and {len(self._segments_to_apply_forces_in_global)} segments to apply forces_in_global on, they should have the same length." + ) + + # Add the external forces + for i_element in range(forces_in_global.shape[1]): + name = self._segments_to_apply_forces_in_global[i_element] + values = forces_in_global[:6, i_element] + point_of_application = forces_in_global[6:9, i_element] + external_forces_set.add(name, values, point_of_application) + + elif translational_forces.shape[1] > 0: + if not isinstance(translational_forces, MX): + raise ValueError("translational_forces should be of shape 6 x nb_forces.") + if translational_forces.shape[0] != 6: + raise ValueError( + f"translational_forces has {translational_forces.shape[0]} rows, it should have 6 rows (Fx, Fy, Fz, Px, Py, Pz). You should provide the forces and points of application." + ) + if len(self._segments_to_apply_translational_forces) != translational_forces.shape[1]: + raise ValueError( + f"translational_forces has {translational_forces.shape[1]} columns and {len(self._segments_to_apply_translational_forces)} segments to apply forces on, they should have the same length." + ) + + # Add the translational forces + for i_elements in range(translational_forces.shape[1]): + name = self._segments_to_apply_translational_forces[i_elements] + values = translational_forces[:3, i_elements] + point_of_application = translational_forces[3:6, i_elements] + external_forces_set.addTranslationalForce(values, name, point_of_application) + + return external_forces_set def forward_dynamics(self, with_contact: bool = False) -> Function: - external_forces_set = self._dispatch_forces(self.forces_in_global, self.translational_forces) q_biorbd = GeneralizedCoordinates(self.q) qdot_biorbd = GeneralizedVelocity(self.qdot) @@ -478,7 +487,7 @@ def forward_dynamics(self, with_contact: bool = False) -> Function: if with_contact: biorbd_return = self.model.ForwardDynamicsConstraintsDirect( - q_biorbd, qdot_biorbd, tau_biorbd, external_forces_set + q_biorbd, qdot_biorbd, tau_biorbd, self.external_forces_set ).to_mx() casadi_fun = Function( "constrained_forward_dynamics", @@ -488,7 +497,7 @@ def forward_dynamics(self, with_contact: bool = False) -> Function: ["qddot"], ) else: - biorbd_return = self.model.ForwardDynamics(q_biorbd, qdot_biorbd, tau_biorbd, external_forces_set).to_mx() + biorbd_return = self.model.ForwardDynamics(q_biorbd, qdot_biorbd, tau_biorbd, self.external_forces_set).to_mx() casadi_fun = Function( "forward_dynamics", [self.q, self.qdot, self.tau, self.external_forces, self.parameters], @@ -499,12 +508,11 @@ def forward_dynamics(self, with_contact: bool = False) -> Function: return casadi_fun def inverse_dynamics(self, with_contact: bool = False) -> Function: - external_forces_set = self._dispatch_forces(self.forces_in_global, self.translational_forces) q_biorbd = GeneralizedCoordinates(self.q) qdot_biorbd = GeneralizedVelocity(self.qdot) qddot_biorbd = GeneralizedAcceleration(self.qddot) - biorbd_return = self.model.InverseDynamics(q_biorbd, qdot_biorbd, qddot_biorbd, external_forces_set).to_mx() + biorbd_return = self.model.InverseDynamics(q_biorbd, qdot_biorbd, qddot_biorbd, self.external_forces_set).to_mx() casadi_fun = Function( "inverse_dynamics", [self.q, self.qdot, self.qddot, self.external_forces, self.parameters], @@ -515,13 +523,12 @@ def inverse_dynamics(self, with_contact: bool = False) -> Function: return casadi_fun def contact_forces_from_constrained_forward_dynamics(self) -> Function: - external_forces_set = self._dispatch_forces(self.forces_in_global, self.translational_forces) q_biorbd = GeneralizedCoordinates(self.q) qdot_biorbd = GeneralizedVelocity(self.qdot) tau_biorbd = GeneralizedTorque(self.tau) biorbd_return = self.model.ContactForcesFromForwardDynamicsConstraintsDirect( - q_biorbd, qdot_biorbd, tau_biorbd, external_forces_set + q_biorbd, qdot_biorbd, tau_biorbd, self.external_forces_set ).to_mx() casadi_fun = Function( "contact_forces_from_constrained_forward_dynamics", diff --git a/tests/shard6/test_time_dependent_problems.py b/tests/shard6/test_time_dependent_problems.py index bc9b96872..71cc4cac5 100644 --- a/tests/shard6/test_time_dependent_problems.py +++ b/tests/shard6/test_time_dependent_problems.py @@ -72,7 +72,7 @@ def time_dynamic( # You can directly call biorbd function (as for ddq) or call bioptim accessor (as for dq) dq = DynamicsFunctions.compute_qdot(nlp, q, qdot) - ddq = nlp.model.forward_dynamics(with_contact=False)(q, qdot, tau, [], [], []) + ddq = nlp.model.forward_dynamics(with_contact=False)(q, qdot, tau, [], []) return DynamicsEvaluation(dxdt=vertcat(dq, ddq), defects=None) From c56e79c30d5421f1f5cd6e7861014ac3d5c85692 Mon Sep 17 00:00:00 2001 From: eve-mac Date: Sat, 5 Oct 2024 16:22:50 -0400 Subject: [PATCH 042/108] fixed dynamics except DAE_INVERSE --- bioptim/dynamics/dynamics_functions.py | 35 ++++++----- .../example_external_forces.py | 30 +++++----- bioptim/models/biorbd/biorbd_model.py | 60 +++++++++---------- bioptim/optimization/non_linear_program.py | 39 ++++++++---- tests/shard1/test_dynamics.py | 27 +++++++-- 5 files changed, 108 insertions(+), 83 deletions(-) diff --git a/bioptim/dynamics/dynamics_functions.py b/bioptim/dynamics/dynamics_functions.py index 7a4c1941d..a5b4b76b6 100644 --- a/bioptim/dynamics/dynamics_functions.py +++ b/bioptim/dynamics/dynamics_functions.py @@ -535,7 +535,7 @@ def torque_activations_driven( if with_ligament: tau += nlp.model.ligament_joint_torque()(q, qdot, nlp.parameters.cx) - external_forces = nlp.get_external_forces(time, states, controls, parameters, algebraic_states, numerical_timeseries) + external_forces = nlp.get_external_forces(states, controls, algebraic_states, numerical_timeseries) dq = DynamicsFunctions.compute_qdot(nlp, q, qdot) ddq = DynamicsFunctions.forward_dynamics(nlp, q, qdot, tau, with_contact, external_forces) @@ -615,7 +615,7 @@ def torque_derivative_driven( dxdt[nlp.states["qddot"].index, :] = dddq dxdt[nlp.states["tau"].index, :] = dtau else: - external_forces = nlp.get_external_forces(time, states, controls, parameters, algebraic_states, numerical_timeseries) + external_forces = nlp.get_external_forces(states, controls, algebraic_states, numerical_timeseries) ddq = DynamicsFunctions.forward_dynamics(nlp, q, qdot, tau, with_contact, external_forces) dxdt = nlp.cx(nlp.states.shape, ddq.shape[1]) dxdt[nlp.states["q"].index, :] = horzcat(*[dq for _ in range(ddq.shape[1])]) @@ -1151,20 +1151,23 @@ def inverse_dynamics( if external_forces is None: 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] - elif "tau" in nlp.controls: - tau_shape = nlp.controls["tau"].cx.shape[0] - else: - tau_shape = nlp.model.nb_tau - tau = nlp.cx(tau_shape, nlp.ns) - - # @pariterre not sure of this part - if external_forces is not None: - for i in range(external_forces.shape[1]): - tau[:, i] = nlp.model.inverse_dynamics(with_contact=with_contact)( - q, qdot, qddot, external_forces[:, i], nlp.parameters.cx - ) + # @ipuch not sure of this part + tau = nlp.model.inverse_dynamics(with_contact=with_contact)(q, qdot, qddot, external_forces, + nlp.parameters.cx) + + # if "tau" in nlp.states: + # tau_shape = nlp.states["tau"].cx.shape[0] + # elif "tau" in nlp.controls: + # tau_shape = nlp.controls["tau"].cx.shape[0] + # else: + # tau_shape = nlp.model.nb_tau + + # tau = nlp.cx(tau_shape, nlp.ns) + # if external_forces is not None: + # for i in range(external_forces.shape[1]): + # 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. diff --git a/bioptim/examples/getting_started/example_external_forces.py b/bioptim/examples/getting_started/example_external_forces.py index 6e0eb0292..2484dca66 100644 --- a/bioptim/examples/getting_started/example_external_forces.py +++ b/bioptim/examples/getting_started/example_external_forces.py @@ -69,9 +69,9 @@ def prepare_ocp( The OptimalControlProgram ready to be solved """ - bio_model = BiorbdModel(biorbd_model_path, segments_to_apply_external_forces=["Seg1", "Test"]) - # segments_to_apply_external_forces is necessary to define the external forces. - # Please note that they should be declared in the same order as the external forces values bellow. + bio_model = BiorbdModel(biorbd_model_path, segments_to_apply_forces_in_global=["Seg1", "Test"]) + # segments_to_apply_forces_in_global is necessary to define the external forces. + # Please note that they should be declared in the same order as the external forces (in the global reference frame) values bellow. # Problem parameters n_shooting = 30 @@ -83,19 +83,19 @@ def prepare_ocp( # External forces (shape: 9 x nb_external_forces x (n_shooting_points+1)) # First components are the moments and forces - external_forces = np.zeros((9, 2, n_shooting + 1)) - external_forces[5, 0, :] = -2 - external_forces[5, 1, :] = 5 - external_forces[5, 0, 4] = -22 - external_forces[5, 1, 4] = 52 + forces_in_global = np.zeros((9, 2, n_shooting + 1)) + forces_in_global[5, 0, :] = -2 + forces_in_global[5, 1, :] = 5 + forces_in_global[5, 0, 4] = -22 + forces_in_global[5, 1, 4] = 52 if use_point_of_applications: # Last components are the point of application - external_forces[6, 0, :] = 0.05 - external_forces[7, 1, :] = 0.01 - external_forces[8, 0, :] = 0.007 - external_forces[6, 1, :] = -0.009 - external_forces[7, 0, :] = -0.05 - external_forces[8, 1, :] = -0.01 + forces_in_global[6, 0, :] = 0.05 + forces_in_global[7, 1, :] = 0.01 + forces_in_global[8, 0, :] = 0.007 + forces_in_global[6, 1, :] = -0.009 + forces_in_global[7, 0, :] = -0.05 + forces_in_global[8, 1, :] = -0.01 # Dynamics dynamics = DynamicsList() @@ -104,7 +104,7 @@ def prepare_ocp( DynamicsFcn.TORQUE_DRIVEN, expand_dynamics=expand_dynamics, phase_dynamics=phase_dynamics, - numerical_data_timeseries={"forces_in_global": external_forces}, # the key word "forces_in_global" must be used + numerical_data_timeseries={"forces_in_global": forces_in_global}, # the key word "forces_in_global" must be used ) # Constraints diff --git a/bioptim/models/biorbd/biorbd_model.py b/bioptim/models/biorbd/biorbd_model.py index 357374cfe..8eb6664a0 100644 --- a/bioptim/models/biorbd/biorbd_model.py +++ b/bioptim/models/biorbd/biorbd_model.py @@ -6,7 +6,7 @@ GeneralizedTorque, GeneralizedAcceleration, ) -from casadi import SX, MX, vertcat, horzcat, norm_fro, Function +from casadi import SX, MX, vertcat, horzcat, norm_fro, Function, reshape from typing import Callable from ..utils import _var_mapping, bounds_from_ranges @@ -436,21 +436,10 @@ def _dispatch_forces(self): return None else: external_forces_set = self.model.externalForceSet() - forces_in_global = self.external_forces[:9*self.nb_forces_in_global].reshape(9, self.nb_forces_in_global) - translational_forces = self.external_forces[9*self.nb_forces_in_global:].reshape(6, self.nb_translational_forces) + forces_in_global = reshape(self.external_forces[:9*self.nb_forces_in_global], (9, self.nb_forces_in_global)) + translational_forces = reshape(self.external_forces[9*self.nb_forces_in_global:], (6, self.nb_translational_forces)) if forces_in_global.shape[1] > 0: - if not isinstance(forces_in_global, MX): - raise ValueError("forces_in_global should be of shape 9 x nb_forces.") - if forces_in_global.shape[0] != 9: - raise ValueError( - f"forces_in_global has {forces_in_global.shape[0]} rows, it should have 9 rows (Mx, My, Mz, Fx, Fy, Fz, Px, Py, Pz). You should provide the moments, forces and points of application." - ) - if len(self._segments_to_apply_forces_in_global) != forces_in_global.shape[1]: - raise ValueError( - f"forces_in_global has {forces_in_global.shape[1]} columns and {len(self._segments_to_apply_forces_in_global)} segments to apply forces_in_global on, they should have the same length." - ) - # Add the external forces for i_element in range(forces_in_global.shape[1]): name = self._segments_to_apply_forces_in_global[i_element] @@ -459,17 +448,6 @@ def _dispatch_forces(self): external_forces_set.add(name, values, point_of_application) elif translational_forces.shape[1] > 0: - if not isinstance(translational_forces, MX): - raise ValueError("translational_forces should be of shape 6 x nb_forces.") - if translational_forces.shape[0] != 6: - raise ValueError( - f"translational_forces has {translational_forces.shape[0]} rows, it should have 6 rows (Fx, Fy, Fz, Px, Py, Pz). You should provide the forces and points of application." - ) - if len(self._segments_to_apply_translational_forces) != translational_forces.shape[1]: - raise ValueError( - f"translational_forces has {translational_forces.shape[1]} columns and {len(self._segments_to_apply_translational_forces)} segments to apply forces on, they should have the same length." - ) - # Add the translational forces for i_elements in range(translational_forces.shape[1]): name = self._segments_to_apply_translational_forces[i_elements] @@ -486,9 +464,14 @@ def forward_dynamics(self, with_contact: bool = False) -> Function: tau_biorbd = GeneralizedTorque(self.tau) if with_contact: - biorbd_return = self.model.ForwardDynamicsConstraintsDirect( - q_biorbd, qdot_biorbd, tau_biorbd, self.external_forces_set - ).to_mx() + if self.external_forces_set is None: + biorbd_return = self.model.ForwardDynamicsConstraintsDirect( + q_biorbd, qdot_biorbd, tau_biorbd + ).to_mx() + else: + biorbd_return = self.model.ForwardDynamicsConstraintsDirect( + q_biorbd, qdot_biorbd, tau_biorbd, self.external_forces_set + ).to_mx() casadi_fun = Function( "constrained_forward_dynamics", [self.q, self.qdot, self.tau, self.external_forces, self.parameters], @@ -497,7 +480,10 @@ def forward_dynamics(self, with_contact: bool = False) -> Function: ["qddot"], ) else: - biorbd_return = self.model.ForwardDynamics(q_biorbd, qdot_biorbd, tau_biorbd, self.external_forces_set).to_mx() + if self.external_forces_set is None: + biorbd_return = self.model.ForwardDynamics(q_biorbd, qdot_biorbd, tau_biorbd).to_mx() + else: + biorbd_return = self.model.ForwardDynamics(q_biorbd, qdot_biorbd, tau_biorbd, self.external_forces_set).to_mx() casadi_fun = Function( "forward_dynamics", [self.q, self.qdot, self.tau, self.external_forces, self.parameters], @@ -512,7 +498,10 @@ def inverse_dynamics(self, with_contact: bool = False) -> Function: q_biorbd = GeneralizedCoordinates(self.q) qdot_biorbd = GeneralizedVelocity(self.qdot) qddot_biorbd = GeneralizedAcceleration(self.qddot) - biorbd_return = self.model.InverseDynamics(q_biorbd, qdot_biorbd, qddot_biorbd, self.external_forces_set).to_mx() + if self.external_forces_set is None: + biorbd_return = self.model.InverseDynamics(q_biorbd, qdot_biorbd, qddot_biorbd).to_mx() + else: + biorbd_return = self.model.InverseDynamics(q_biorbd, qdot_biorbd, qddot_biorbd, self.external_forces_set).to_mx() casadi_fun = Function( "inverse_dynamics", [self.q, self.qdot, self.qddot, self.external_forces, self.parameters], @@ -527,9 +516,14 @@ def contact_forces_from_constrained_forward_dynamics(self) -> Function: q_biorbd = GeneralizedCoordinates(self.q) qdot_biorbd = GeneralizedVelocity(self.qdot) tau_biorbd = GeneralizedTorque(self.tau) - biorbd_return = self.model.ContactForcesFromForwardDynamicsConstraintsDirect( - q_biorbd, qdot_biorbd, tau_biorbd, self.external_forces_set - ).to_mx() + if self.external_forces_set is None: + biorbd_return = self.model.ContactForcesFromForwardDynamicsConstraintsDirect( + q_biorbd, qdot_biorbd, tau_biorbd + ).to_mx() + else: + biorbd_return = self.model.ContactForcesFromForwardDynamicsConstraintsDirect( + q_biorbd, qdot_biorbd, tau_biorbd, self.external_forces_set + ).to_mx() casadi_fun = Function( "contact_forces_from_constrained_forward_dynamics", [self.q, self.qdot, self.tau, self.external_forces, self.parameters], diff --git a/bioptim/optimization/non_linear_program.py b/bioptim/optimization/non_linear_program.py index d9c77d653..a640ffac7 100644 --- a/bioptim/optimization/non_linear_program.py +++ b/bioptim/optimization/non_linear_program.py @@ -458,36 +458,49 @@ def get_external_forces( if "forces_in_global" in self.states: external_forces = vertcat(external_forces, DynamicsFunctions.get(self.states["forces_in_global"], states)) - if "forces_in_global" in self.algebraic_states: - external_forces = vertcat( - external_forces, DynamicsFunctions.get(self.algebraic_states["forces_in_global"], algebraic_states) - ) if "forces_in_global" in self.controls: external_forces = vertcat( external_forces, DynamicsFunctions.get(self.controls["forces_in_global"], controls) ) - if "forces_in_global" in self.numerical_timeseries: + if "forces_in_global" in self.algebraic_states: external_forces = vertcat( - external_forces, - DynamicsFunctions.get(self.numerical_timeseries["forces_in_global"], numerical_timeseries), + external_forces, DynamicsFunctions.get(self.algebraic_states["forces_in_global"], algebraic_states) ) + component_numerical_timeseries = 0 + for key in self.numerical_timeseries.keys(): + if "forces_in_global" in key: + component_numerical_timeseries += 1 + if component_numerical_timeseries > 0: + for i_component in range(component_numerical_timeseries): + external_forces = vertcat( + external_forces, + DynamicsFunctions.get(self.numerical_timeseries[f"forces_in_global_{i_component}"], numerical_timeseries), + ) + if "translational_forces" in self.states: external_forces = vertcat( external_forces, DynamicsFunctions.get(self.states["translational_forces"], states) ) - if "translational_forces" in self.algebraic_states: - external_forces = vertcat( - external_forces, DynamicsFunctions.get(self.algebraic_states["translational_forces"], algebraic_states) - ) if "translational_forces" in self.controls: external_forces = vertcat( external_forces, DynamicsFunctions.get(self.controls["translational_forces"], controls) ) - if "translational_forces" in self.numerical_timeseries: + if "translational_forces" in self.algebraic_states: external_forces = vertcat( external_forces, - DynamicsFunctions.get(self.numerical_timeseries["translational_forces"], numerical_timeseries), + DynamicsFunctions.get(self.algebraic_states["translational_forces"], algebraic_states) ) + component_numerical_timeseries = 0 + for key in self.numerical_timeseries.keys(): + if "translational_forces" in key: + component_numerical_timeseries += 1 + if component_numerical_timeseries > 0: + for i_component in range(component_numerical_timeseries): + external_forces = vertcat( + external_forces, + DynamicsFunctions.get(self.numerical_timeseries[f"translational_forces{i_component}"], numerical_timeseries), + ) + return external_forces diff --git a/tests/shard1/test_dynamics.py b/tests/shard1/test_dynamics.py index 2e5287831..7c1f197d3 100644 --- a/tests/shard1/test_dynamics.py +++ b/tests/shard1/test_dynamics.py @@ -49,10 +49,23 @@ def __init__(self, nlp, use_sx): def test_torque_driven(with_contact, with_external_force, cx, rigidbody_dynamics, phase_dynamics): # Prepare the program nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx == SX)) + + segments_to_apply_forces_in_global = None + segments_to_apply_translational_forces = None + + if with_external_force: + segments_to_apply_forces_in_global = ["Seg0"] + + if rigidbody_dynamics == RigidBodyDynamics.DAE_INVERSE_DYNAMICS: + if with_contact: + segments_to_apply_translational_forces = ["Seg0", "Seg1"] + nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod", - segments_to_apply_forces_in_global=["Seg0"], + segments_to_apply_forces_in_global=segments_to_apply_forces_in_global, + segments_to_apply_translational_forces=segments_to_apply_translational_forces, ) + nlp.ns = 5 nlp.cx = cx nlp.time_cx = cx.sym("time", 1, 1) @@ -272,8 +285,10 @@ def test_torque_driven_implicit(with_contact, cx, phase_dynamics): # Prepare the program nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx == SX)) nlp.model = BiorbdModel( - TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod" + TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod", + segments_to_apply_translational_forces=["Seg0", "Seg1"] if with_contact else None ) + nlp.ns = 5 nlp.cx = cx nlp.time_cx = cx.sym("time", 1, 1) @@ -430,7 +445,7 @@ def test_torque_derivative_driven(with_contact, with_external_force, cx, phase_d nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx == SX)) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod", - segments_to_apply_external_forces=["Seg0"], + segments_to_apply_forces_in_global=["Seg0"], ) nlp.ns = 5 nlp.cx = cx @@ -938,7 +953,7 @@ def test_torque_activation_driven(with_contact, with_external_force, cx, phase_d nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx == SX)) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod", - segments_to_apply_external_forces=["Seg0"], + segments_to_apply_forces_in_global=["Seg0"], ) nlp.ns = 5 nlp.cx = cx @@ -1119,7 +1134,7 @@ def test_torque_activation_driven_with_residual_torque( nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx == SX)) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/torque_driven_ocp/models/2segments_2dof_2contacts.bioMod", - segments_to_apply_external_forces=["Seg0"], + segments_to_apply_forces_in_global=["Seg0"], ) nlp.ns = 5 nlp.cx = cx @@ -1365,7 +1380,7 @@ def test_muscle_driven( nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx == SX)) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/muscle_driven_ocp/models/arm26_with_contact.bioMod", - segments_to_apply_external_forces=["r_ulna_radius_hand_rotation1"], + segments_to_apply_forces_in_global=["r_ulna_radius_hand_rotation1"], ) nlp.ns = 5 nlp.cx = cx From 8f720e42f6b6d6207c8dd0868dd65d591866c024 Mon Sep 17 00:00:00 2001 From: eve-mac Date: Sat, 5 Oct 2024 16:23:16 -0400 Subject: [PATCH 043/108] blacked --- bioptim/dynamics/configure_problem.py | 4 +++- bioptim/dynamics/dynamics_functions.py | 25 +++++++++++----------- bioptim/limits/penalty.py | 6 +++--- bioptim/models/biorbd/biorbd_model.py | 24 ++++++++++++++------- bioptim/optimization/non_linear_program.py | 11 ++++++---- tests/shard1/test_dynamics.py | 2 +- 6 files changed, 42 insertions(+), 30 deletions(-) diff --git a/bioptim/dynamics/configure_problem.py b/bioptim/dynamics/configure_problem.py index 5558504d7..17994ce72 100644 --- a/bioptim/dynamics/configure_problem.py +++ b/bioptim/dynamics/configure_problem.py @@ -875,7 +875,9 @@ def muscle_driven( if with_contact: ConfigureProblem.configure_contact_function( - ocp, nlp, DynamicsFunctions.forces_from_muscle_driven, + ocp, + nlp, + DynamicsFunctions.forces_from_muscle_driven, ) ConfigureProblem.configure_soft_contact_function(ocp, nlp) diff --git a/bioptim/dynamics/dynamics_functions.py b/bioptim/dynamics/dynamics_functions.py index a5b4b76b6..080048fdc 100644 --- a/bioptim/dynamics/dynamics_functions.py +++ b/bioptim/dynamics/dynamics_functions.py @@ -181,9 +181,7 @@ def torque_driven( if not with_contact and fatigue is None: qddot = DynamicsFunctions.get(nlp.states_dot["qddot"], nlp.states_dot.scaled.cx) external_forces = nlp.get_external_forces(states, controls, algebraic_states, numerical_timeseries) - tau_id = DynamicsFunctions.inverse_dynamics( - nlp, q, qdot, qddot, with_contact, external_forces - ) + tau_id = DynamicsFunctions.inverse_dynamics(nlp, q, qdot, qddot, with_contact, external_forces) defects = nlp.cx(dq.shape[0] + tau_id.shape[0], tau_id.shape[1]) dq_defects = [] @@ -269,7 +267,9 @@ def torque_driven_free_floating_base( tau_full = vertcat(nlp.cx.zeros(nlp.model.nb_root), tau_joints) - ddq = DynamicsFunctions.forward_dynamics(nlp, q_full, qdot_full, tau_full, with_contact=False, external_forces=None) + ddq = DynamicsFunctions.forward_dynamics( + nlp, q_full, qdot_full, tau_full, with_contact=False, external_forces=None + ) dxdt = nlp.cx(n_q + n_qdot, ddq.shape[1]) dxdt[:n_q, :] = horzcat(*[dq for _ in range(ddq.shape[1])]) dxdt[n_q:, :] = ddq @@ -413,7 +413,9 @@ def stochastic_torque_driven_free_floating_base( tau_full = vertcat(nlp.cx.zeros(nlp.model.nb_root), tau_joints) dq = DynamicsFunctions.compute_qdot(nlp, q_full, qdot_full) - ddq = DynamicsFunctions.forward_dynamics(nlp, q_full, qdot_full, tau_full, with_contact=False, external_forces=None) + ddq = DynamicsFunctions.forward_dynamics( + nlp, q_full, qdot_full, tau_full, with_contact=False, external_forces=None + ) dxdt = nlp.cx(nlp.states.shape, ddq.shape[1]) dxdt[:n_q, :] = horzcat(*[dq for _ in range(ddq.shape[1])]) dxdt[n_q:, :] = ddq @@ -856,9 +858,7 @@ def muscles_driven( if not with_contact and fatigue is None: qddot = DynamicsFunctions.get(nlp.states_dot["qddot"], nlp.states_dot.cx) external_forces = nlp.get_external_forces(states, controls, algebraic_states, numerical_timeseries) - tau_id = DynamicsFunctions.inverse_dynamics( - nlp, q, qdot, qddot, with_contact, external_forces - ) + tau_id = DynamicsFunctions.inverse_dynamics(nlp, q, qdot, qddot, with_contact, external_forces) defects = nlp.cx(dq.shape[0] + tau_id.shape[0], tau_id.shape[1]) dq_defects = [] @@ -1152,8 +1152,9 @@ def inverse_dynamics( tau = nlp.model.inverse_dynamics(with_contact=with_contact)(q, qdot, qddot, [], nlp.parameters.cx) else: # @ipuch not sure of this part - tau = nlp.model.inverse_dynamics(with_contact=with_contact)(q, qdot, qddot, external_forces, - nlp.parameters.cx) + tau = nlp.model.inverse_dynamics(with_contact=with_contact)( + q, qdot, qddot, external_forces, nlp.parameters.cx + ) # if "tau" in nlp.states: # tau_shape = nlp.states["tau"].cx.shape[0] @@ -1268,8 +1269,6 @@ def holonomic_torque_driven( qdot_u = DynamicsFunctions.get(nlp.states["qdot_u"], states) tau = DynamicsFunctions.get(nlp.controls["tau"], controls) external_forces = nlp.get_external_forces(states, controls, algebraic_states, numerical_timeseries) - qddot_u = nlp.model.partitioned_forward_dynamics( - q_u, qdot_u, tau, external_forces, nlp.parameters.cx - ) + 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/limits/penalty.py b/bioptim/limits/penalty.py index da19d8e20..c9860ca06 100644 --- a/bioptim/limits/penalty.py +++ b/bioptim/limits/penalty.py @@ -344,9 +344,9 @@ def minimize_markers_velocity( penalty.quadratic = True if penalty.quadratic is None else penalty.quadratic # Add the penalty in the requested reference frame. None for global - markers =controller.model.markers_velocities(reference_index=reference_jcs)( - controller.q, controller.qdot, controller.parameters.cx - ) + markers = controller.model.markers_velocities(reference_index=reference_jcs)( + controller.q, controller.qdot, controller.parameters.cx + ) return markers diff --git a/bioptim/models/biorbd/biorbd_model.py b/bioptim/models/biorbd/biorbd_model.py index 8eb6664a0..1120af86f 100644 --- a/bioptim/models/biorbd/biorbd_model.py +++ b/bioptim/models/biorbd/biorbd_model.py @@ -55,7 +55,9 @@ def __init__( self.muscle = MX.sym("muscle_mx", self.nb_muscles, 1) self.activations = MX.sym("activations_mx", self.nb_muscles, 1) - self.external_forces = MX.sym("external_forces_mx", 9*self.nb_forces_in_global + 6*self.nb_translational_forces, 1) + self.external_forces = MX.sym( + "external_forces_mx", 9 * self.nb_forces_in_global + 6 * self.nb_translational_forces, 1 + ) self.external_forces_set = self._dispatch_forces() # TODO: remove mx (the MX parameters should be created inside the BiorbdModel) @@ -436,8 +438,12 @@ def _dispatch_forces(self): return None else: external_forces_set = self.model.externalForceSet() - forces_in_global = reshape(self.external_forces[:9*self.nb_forces_in_global], (9, self.nb_forces_in_global)) - translational_forces = reshape(self.external_forces[9*self.nb_forces_in_global:], (6, self.nb_translational_forces)) + forces_in_global = reshape( + self.external_forces[: 9 * self.nb_forces_in_global], (9, self.nb_forces_in_global) + ) + translational_forces = reshape( + self.external_forces[9 * self.nb_forces_in_global :], (6, self.nb_translational_forces) + ) if forces_in_global.shape[1] > 0: # Add the external forces @@ -465,9 +471,7 @@ def forward_dynamics(self, with_contact: bool = False) -> Function: if with_contact: if self.external_forces_set is None: - biorbd_return = self.model.ForwardDynamicsConstraintsDirect( - q_biorbd, qdot_biorbd, tau_biorbd - ).to_mx() + biorbd_return = self.model.ForwardDynamicsConstraintsDirect(q_biorbd, qdot_biorbd, tau_biorbd).to_mx() else: biorbd_return = self.model.ForwardDynamicsConstraintsDirect( q_biorbd, qdot_biorbd, tau_biorbd, self.external_forces_set @@ -483,7 +487,9 @@ def forward_dynamics(self, with_contact: bool = False) -> Function: if self.external_forces_set is None: biorbd_return = self.model.ForwardDynamics(q_biorbd, qdot_biorbd, tau_biorbd).to_mx() else: - biorbd_return = self.model.ForwardDynamics(q_biorbd, qdot_biorbd, tau_biorbd, self.external_forces_set).to_mx() + biorbd_return = self.model.ForwardDynamics( + q_biorbd, qdot_biorbd, tau_biorbd, self.external_forces_set + ).to_mx() casadi_fun = Function( "forward_dynamics", [self.q, self.qdot, self.tau, self.external_forces, self.parameters], @@ -501,7 +507,9 @@ def inverse_dynamics(self, with_contact: bool = False) -> Function: if self.external_forces_set is None: biorbd_return = self.model.InverseDynamics(q_biorbd, qdot_biorbd, qddot_biorbd).to_mx() else: - biorbd_return = self.model.InverseDynamics(q_biorbd, qdot_biorbd, qddot_biorbd, self.external_forces_set).to_mx() + biorbd_return = self.model.InverseDynamics( + q_biorbd, qdot_biorbd, qddot_biorbd, self.external_forces_set + ).to_mx() casadi_fun = Function( "inverse_dynamics", [self.q, self.qdot, self.qddot, self.external_forces, self.parameters], diff --git a/bioptim/optimization/non_linear_program.py b/bioptim/optimization/non_linear_program.py index a640ffac7..58d21faea 100644 --- a/bioptim/optimization/non_linear_program.py +++ b/bioptim/optimization/non_linear_program.py @@ -475,7 +475,9 @@ def get_external_forces( for i_component in range(component_numerical_timeseries): external_forces = vertcat( external_forces, - DynamicsFunctions.get(self.numerical_timeseries[f"forces_in_global_{i_component}"], numerical_timeseries), + DynamicsFunctions.get( + self.numerical_timeseries[f"forces_in_global_{i_component}"], numerical_timeseries + ), ) if "translational_forces" in self.states: @@ -488,8 +490,7 @@ def get_external_forces( ) if "translational_forces" in self.algebraic_states: external_forces = vertcat( - external_forces, - DynamicsFunctions.get(self.algebraic_states["translational_forces"], algebraic_states) + external_forces, DynamicsFunctions.get(self.algebraic_states["translational_forces"], algebraic_states) ) component_numerical_timeseries = 0 @@ -500,7 +501,9 @@ def get_external_forces( for i_component in range(component_numerical_timeseries): external_forces = vertcat( external_forces, - DynamicsFunctions.get(self.numerical_timeseries[f"translational_forces{i_component}"], numerical_timeseries), + DynamicsFunctions.get( + self.numerical_timeseries[f"translational_forces{i_component}"], numerical_timeseries + ), ) return external_forces diff --git a/tests/shard1/test_dynamics.py b/tests/shard1/test_dynamics.py index 7c1f197d3..5f4caa036 100644 --- a/tests/shard1/test_dynamics.py +++ b/tests/shard1/test_dynamics.py @@ -286,7 +286,7 @@ def test_torque_driven_implicit(with_contact, cx, phase_dynamics): nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx == SX)) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod", - segments_to_apply_translational_forces=["Seg0", "Seg1"] if with_contact else None + segments_to_apply_translational_forces=["Seg0", "Seg1"] if with_contact else None, ) nlp.ns = 5 From 91bc961dc33a54a629d970c918c1313b631f998d Mon Sep 17 00:00:00 2001 From: eve-mac Date: Mon, 7 Oct 2024 09:32:08 -0400 Subject: [PATCH 044/108] fixed the alg_states missing tests --- .../muscle_activations_tracker.py | 2 + .../muscle_excitations_tracker.py | 2 + bioptim/optimization/non_linear_program.py | 50 ++++++++++--------- 3 files changed, 30 insertions(+), 24 deletions(-) diff --git a/bioptim/examples/muscle_driven_ocp/muscle_activations_tracker.py b/bioptim/examples/muscle_driven_ocp/muscle_activations_tracker.py index 2b9c5cbbb..376a0e4c0 100644 --- a/bioptim/examples/muscle_driven_ocp/muscle_activations_tracker.py +++ b/bioptim/examples/muscle_driven_ocp/muscle_activations_tracker.py @@ -101,9 +101,11 @@ def generate_data( nlp.states = OptimizationVariableContainer(phase_dynamics=phase_dynamics) nlp.states_dot = OptimizationVariableContainer(phase_dynamics=phase_dynamics) nlp.controls = OptimizationVariableContainer(phase_dynamics=phase_dynamics) + nlp.algebraic_states = OptimizationVariableContainer(phase_dynamics=phase_dynamics) nlp.states.initialize_from_shooting(n_shooting, MX) nlp.states_dot.initialize_from_shooting(n_shooting, MX) nlp.controls.initialize_from_shooting(n_shooting, MX) + nlp.algebraic_states.initialize_from_shooting(n_shooting, MX) for node_index in range(n_shooting): nlp.states.append( diff --git a/bioptim/examples/muscle_driven_ocp/muscle_excitations_tracker.py b/bioptim/examples/muscle_driven_ocp/muscle_excitations_tracker.py index 089c8f276..77d7c17f8 100644 --- a/bioptim/examples/muscle_driven_ocp/muscle_excitations_tracker.py +++ b/bioptim/examples/muscle_driven_ocp/muscle_excitations_tracker.py @@ -104,9 +104,11 @@ def generate_data( nlp.states = OptimizationVariableContainer(phase_dynamics=phase_dynamics) nlp.states_dot = OptimizationVariableContainer(phase_dynamics=phase_dynamics) nlp.controls = OptimizationVariableContainer(phase_dynamics=phase_dynamics) + nlp.algebraic_states = OptimizationVariableContainer(phase_dynamics=phase_dynamics) nlp.states.initialize_from_shooting(n_shooting, MX) nlp.states_dot.initialize_from_shooting(n_shooting, MX) nlp.controls.initialize_from_shooting(n_shooting, MX) + nlp.algebraic_states.initialize_from_shooting(n_shooting, MX) for node_index in range(n_shooting): nlp.states.append( diff --git a/bioptim/optimization/non_linear_program.py b/bioptim/optimization/non_linear_program.py index 58d21faea..87d87645d 100644 --- a/bioptim/optimization/non_linear_program.py +++ b/bioptim/optimization/non_linear_program.py @@ -467,18 +467,19 @@ def get_external_forces( external_forces, DynamicsFunctions.get(self.algebraic_states["forces_in_global"], algebraic_states) ) - component_numerical_timeseries = 0 - for key in self.numerical_timeseries.keys(): - if "forces_in_global" in key: - component_numerical_timeseries += 1 - if component_numerical_timeseries > 0: - for i_component in range(component_numerical_timeseries): - external_forces = vertcat( - external_forces, - DynamicsFunctions.get( - self.numerical_timeseries[f"forces_in_global_{i_component}"], numerical_timeseries - ), - ) + if self.numerical_timeseries is not None: + component_numerical_timeseries = 0 + for key in self.numerical_timeseries.keys(): + if "forces_in_global" in key: + component_numerical_timeseries += 1 + if component_numerical_timeseries > 0: + for i_component in range(component_numerical_timeseries): + external_forces = vertcat( + external_forces, + DynamicsFunctions.get( + self.numerical_timeseries[f"forces_in_global_{i_component}"], numerical_timeseries + ), + ) if "translational_forces" in self.states: external_forces = vertcat( @@ -493,17 +494,18 @@ def get_external_forces( external_forces, DynamicsFunctions.get(self.algebraic_states["translational_forces"], algebraic_states) ) - component_numerical_timeseries = 0 - for key in self.numerical_timeseries.keys(): - if "translational_forces" in key: - component_numerical_timeseries += 1 - if component_numerical_timeseries > 0: - for i_component in range(component_numerical_timeseries): - external_forces = vertcat( - external_forces, - DynamicsFunctions.get( - self.numerical_timeseries[f"translational_forces{i_component}"], numerical_timeseries - ), - ) + if self.numerical_timeseries is not None: + component_numerical_timeseries = 0 + for key in self.numerical_timeseries.keys(): + if "translational_forces" in key: + component_numerical_timeseries += 1 + if component_numerical_timeseries > 0: + for i_component in range(component_numerical_timeseries): + external_forces = vertcat( + external_forces, + DynamicsFunctions.get( + self.numerical_timeseries[f"translational_forces{i_component}"], numerical_timeseries + ), + ) return external_forces From 07e8b6ef97f578ff142399cefe622176d74154ac Mon Sep 17 00:00:00 2001 From: eve-mac Date: Mon, 7 Oct 2024 11:53:35 -0400 Subject: [PATCH 045/108] fixed some tests --- bioptim/examples/getting_started/example_external_forces.py | 2 +- bioptim/examples/moving_horizon_estimation/mhe.py | 2 +- bioptim/examples/torque_driven_ocp/spring_load.py | 2 +- bioptim/models/biorbd/multi_biorbd_model.py | 3 +-- tests/shard4/test_penalty.py | 2 +- 5 files changed, 5 insertions(+), 6 deletions(-) diff --git a/bioptim/examples/getting_started/example_external_forces.py b/bioptim/examples/getting_started/example_external_forces.py index 2484dca66..38894b443 100644 --- a/bioptim/examples/getting_started/example_external_forces.py +++ b/bioptim/examples/getting_started/example_external_forces.py @@ -8,7 +8,7 @@ Please note that the point of application of the external forces are defined from the name of the segment in the bioMod. It is expected to act on a segment in the global_reference_frame. Bioptim expects an array of shape [9, nb_external_forces, n_shooting+1] where the three first components are the moments, the three next components are the forces and the three last components are the point of application (Mx, My, Mz, Fx, Fy, Fz, Px, Py, Pz) -You should also specify the name of the segments where the external forces are applied the list "segments_to_apply_external_forces". +You should also specify the name of the segments where the external forces are applied the list "segments_to_apply_forces_in_global". """ import platform diff --git a/bioptim/examples/moving_horizon_estimation/mhe.py b/bioptim/examples/moving_horizon_estimation/mhe.py index 60a3c6ea0..d992aad8e 100644 --- a/bioptim/examples/moving_horizon_estimation/mhe.py +++ b/bioptim/examples/moving_horizon_estimation/mhe.py @@ -47,7 +47,7 @@ def states_to_markers(bio_model, states): 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 qddot_func = bio_model.forward_dynamics() diff --git a/bioptim/examples/torque_driven_ocp/spring_load.py b/bioptim/examples/torque_driven_ocp/spring_load.py index 4d839df9d..47a9c8e05 100644 --- a/bioptim/examples/torque_driven_ocp/spring_load.py +++ b/bioptim/examples/torque_driven_ocp/spring_load.py @@ -182,7 +182,7 @@ def prepare_ocp( scenario=1, ): # BioModel path - m = BiorbdModel(biorbd_model_path, segments_to_apply_external_forces=["Point"]) + m = BiorbdModel(biorbd_model_path, segments_to_apply_forces_in_global=["Point"]) m.set_gravity(np.array((0, 0, 0))) weight = 1 diff --git a/bioptim/models/biorbd/multi_biorbd_model.py b/bioptim/models/biorbd/multi_biorbd_model.py index 07d0311fd..331a33ae9 100644 --- a/bioptim/models/biorbd/multi_biorbd_model.py +++ b/bioptim/models/biorbd/multi_biorbd_model.py @@ -568,13 +568,12 @@ def forward_dynamics(self, with_contact) -> Function: qdot_model, tau_model, [], - [], self.parameters, ), ) casadi_fun = Function( "forward_dynamics", - [self.q, self.qdot, self.tau, [], [], self.parameters], + [self.q, self.qdot, self.tau, [], self.parameters], [biorbd_return], ) return casadi_fun diff --git a/tests/shard4/test_penalty.py b/tests/shard4/test_penalty.py index 77d617d72..fd40b2968 100644 --- a/tests/shard4/test_penalty.py +++ b/tests/shard4/test_penalty.py @@ -52,7 +52,7 @@ def prepare_test_ocp( elif with_contact: bio_model = BiorbdModel( bioptim_folder + "/examples/muscle_driven_with_contact/models/2segments_4dof_2contacts_1muscle.bioMod", - segments_to_apply_external_forces=["Seg1", "Seg1"], + segments_to_apply_forces_in_global=["Seg1", "Seg1"], ) dynamics = DynamicsList() rigidbody_dynamics = RigidBodyDynamics.DAE_INVERSE_DYNAMICS if implicit else RigidBodyDynamics.ODE From dcb89cbe94d317fc0e0a9f179c16b61aeafbfae6 Mon Sep 17 00:00:00 2001 From: eve-mac Date: Fri, 11 Oct 2024 17:05:08 -0400 Subject: [PATCH 046/108] temporary commit for ipuch --- bioptim/dynamics/configure_problem.py | 10 +++++----- .../contact_forces_inequality_constraint_muscle.py | 7 +++++++ bioptim/models/biorbd/biorbd_model.py | 13 +++++++++++++ bioptim/models/biorbd/holonomic_biorbd_model.py | 6 +++--- 4 files changed, 28 insertions(+), 8 deletions(-) diff --git a/bioptim/dynamics/configure_problem.py b/bioptim/dynamics/configure_problem.py index 17994ce72..88d427fcd 100644 --- a/bioptim/dynamics/configure_problem.py +++ b/bioptim/dynamics/configure_problem.py @@ -1018,15 +1018,15 @@ def configure_qv(ocp, nlp, dyn_func: Callable, **extra_params): "qv_function", [ time_span_sym, - nlp.states.scaled.cx, - nlp.controls.scaled.cx, - nlp.parameters.scaled.cx, - nlp.algebraic_states.scaled.cx, + nlp.states.cx, + nlp.controls.cx, + nlp.parameters.cx, + nlp.algebraic_states.cx, nlp.numerical_timeseries.cx, ], [ dyn_func( - nlp.get_var_from_states_or_controls("q_u", nlp.states.scaled.cx, nlp.controls.scaled.cx), + nlp.get_var_from_states_or_controls("q_u", nlp.states.cx, nlp.controls.cx), ) ], ["t_span", "x", "u", "p", "a", "d"], diff --git a/bioptim/examples/muscle_driven_with_contact/contact_forces_inequality_constraint_muscle.py b/bioptim/examples/muscle_driven_with_contact/contact_forces_inequality_constraint_muscle.py index 44f5f46f2..fa17b4a09 100644 --- a/bioptim/examples/muscle_driven_with_contact/contact_forces_inequality_constraint_muscle.py +++ b/bioptim/examples/muscle_driven_with_contact/contact_forces_inequality_constraint_muscle.py @@ -29,6 +29,13 @@ def prepare_ocp(biorbd_model_path, phase_time, n_shooting, min_bound, max_bound, expand_dynamics=True): # --- Options --- # # BioModel path + + # TODO: Charbie + # force = ForcesList() + # force.add(Px=0, Py= 0, Pz= 0, Mx= 0, My= 0, Mz= 0 + # point_of_application_in_global=True, + # segment_name=) + bio_model = BiorbdModel(biorbd_model_path) tau_min, tau_max, tau_init = -500.0, 500.0, 0.0 activation_min, activation_max, activation_init = 0.0, 1.0, 0.5 diff --git a/bioptim/models/biorbd/biorbd_model.py b/bioptim/models/biorbd/biorbd_model.py index 1120af86f..aa5439103 100644 --- a/bioptim/models/biorbd/biorbd_model.py +++ b/bioptim/models/biorbd/biorbd_model.py @@ -31,6 +31,19 @@ def __init__( segments_to_apply_translational_forces: list[str] = None, parameters: ParameterList = None, ): + """ + + Parameters + ---------- + bio_model + friction_coefficients + TODO: @ipuch + segments_to_apply_forces_in_global: + Moments and forces (Mx, My Mz, Fx, Fy, Fz, Px, Py, Pz) expressed in the global reference frame will be applied at the global origin or the point of application expressed in the global reference frame to the segments listed here + segments_to_apply_translational_forces: + Forces (Fx, Fy, Fz, Px, Py, Pz) expressed in the local reference frame will be applied at the point of application expressed in the local reference frame of the segments listed here + parameters + """ 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'") diff --git a/bioptim/models/biorbd/holonomic_biorbd_model.py b/bioptim/models/biorbd/holonomic_biorbd_model.py index e199b32ef..61d7a6c54 100644 --- a/bioptim/models/biorbd/holonomic_biorbd_model.py +++ b/bioptim/models/biorbd/holonomic_biorbd_model.py @@ -5,7 +5,7 @@ GeneralizedCoordinates, GeneralizedVelocity, ) -from casadi import MX, DM, vertcat, horzcat, Function, solve, rootfinder, inv +from casadi import MX, SX, DM, vertcat, horzcat, Function, solve, rootfinder, inv from .biorbd_model import BiorbdModel from ..holonomic_constraints import HolonomicConstraintsList @@ -357,7 +357,7 @@ def check_state_v_size(self, state_v): if state_v.shape[0] != self.nb_dependent_joints: raise ValueError(f"Length of state v size should be: {self.nb_dependent_joints}. Got: {state_v.shape[0]}") - def compute_q_v(self, q_u: MX | DM, q_v_init: MX | DM = None) -> MX | DM: + def compute_q_v(self, q_u: MX | SX | DM, q_v_init: MX | SX | DM = None) -> MX | SX | DM: """ Compute the dependent joint positions from the independent joint positions. This function might be misleading because it can be used for numerical purpose with DM @@ -367,7 +367,7 @@ def compute_q_v(self, q_u: MX | DM, q_v_init: MX | DM = None) -> MX | DM: q = self.state_from_partition(q_u, decision_variables) mx_residuals = self.holonomic_constraints(q) - if isinstance(q_u, MX): + if isinstance(q_u, MX | SX): q_v_init = MX.zeros(self.nb_dependent_joints) if q_v_init is None else q_v_init ifcn_input = (q_v_init, q_u) residuals = Function( From 7d7f7c974da7689849da3a7f652ae1c13ff1090d Mon Sep 17 00:00:00 2001 From: eve-mac Date: Wed, 23 Oct 2024 15:04:57 -0400 Subject: [PATCH 047/108] fixed holonomic (@ipuch, could you please check carefully this commit?) --- bioptim/dynamics/configure_problem.py | 2 +- .../models/biorbd/holonomic_biorbd_model.py | 36 ++++++++----------- 2 files changed, 15 insertions(+), 23 deletions(-) diff --git a/bioptim/dynamics/configure_problem.py b/bioptim/dynamics/configure_problem.py index 88d427fcd..ce18ee063 100644 --- a/bioptim/dynamics/configure_problem.py +++ b/bioptim/dynamics/configure_problem.py @@ -1403,7 +1403,7 @@ def configure_new_variable( axes_idx: BiMapping The axes index to use for the plot """ - new_variable_config = NewVariableConfiguration( + NewVariableConfiguration( name, name_elements, ocp, diff --git a/bioptim/models/biorbd/holonomic_biorbd_model.py b/bioptim/models/biorbd/holonomic_biorbd_model.py index 61d7a6c54..b67f210b9 100644 --- a/bioptim/models/biorbd/holonomic_biorbd_model.py +++ b/bioptim/models/biorbd/holonomic_biorbd_model.py @@ -205,9 +205,9 @@ def partitioned_mass_matrix(self, q): return vertcat(first_line, second_line) def partitioned_non_linear_effect(self, q, qdot, f_ext=None, f_contacts=None): - if f_ext is not None: + if f_ext is not None and f_ext.shape[0] != 0: raise NotImplementedError("External forces are not implemented yet.") - if f_contacts is not None: + if f_contacts is not None and f_contacts.shape[0] != 0: raise NotImplementedError("Contact forces are not implemented yet.") external_forces_set = self.model.externalForceSet() non_linear_effect = self.model.NonLinearEffect(q, qdot, external_forces_set).to_mx() @@ -251,9 +251,9 @@ def partitioned_forward_dynamics( ROBOTRAN: a powerful symbolic gnerator of multibody models, Mech. Sci., 4, 199–219, https://doi.org/10.5194/ms-4-199-2013, 2013. """ - if external_forces is not None: + if external_forces is not None and external_forces.shape[0] != 0: raise NotImplementedError("External forces are not implemented yet.") - if f_contacts is not None: + if f_contacts is not None and f_contacts.shape[0] != 0: raise NotImplementedError("Contact forces are not implemented yet.") # compute q and qdot @@ -363,26 +363,18 @@ def compute_q_v(self, q_u: MX | SX | DM, q_v_init: MX | SX | DM = None) -> MX | This function might be misleading because it can be used for numerical purpose with DM or for symbolic purpose with MX. The return type is not enforced. """ - decision_variables = MX.sym("decision_variables", self.nb_dependent_joints) - q = self.state_from_partition(q_u, decision_variables) + decision_variables_sym = MX.sym("decision_variables_sym", self.nb_dependent_joints) + q_u_sym = MX.sym("q_u_sym", q_u.shape[0], q_u.shape[1]) + q = self.state_from_partition(q_u_sym, decision_variables_sym) mx_residuals = self.holonomic_constraints(q) - if isinstance(q_u, MX | SX): - q_v_init = MX.zeros(self.nb_dependent_joints) if q_v_init is None else q_v_init - ifcn_input = (q_v_init, q_u) - residuals = Function( - "final_states_residuals", - [decision_variables, q_u], - [mx_residuals], - ).expand() - else: - q_v_init = DM.zeros(self.nb_dependent_joints) if q_v_init is None else q_v_init - ifcn_input = (q_v_init,) - residuals = Function( - "final_states_residuals", - [decision_variables], - [mx_residuals], - ).expand() + q_v_init = MX.zeros(self.nb_dependent_joints) if q_v_init is None else q_v_init + ifcn_input = (q_v_init, q_u) + residuals = Function( + "final_states_residuals", + [decision_variables_sym, q_u_sym], + [mx_residuals], + ).expand() # Create an implicit function instance to solve the system of equations opts = {"abstol": self._newton_tol} From 80a98290524a423e7be4afae311147763d007cef Mon Sep 17 00:00:00 2001 From: eve-mac Date: Wed, 23 Oct 2024 18:32:09 -0400 Subject: [PATCH 048/108] first idea of an external_force interface --- bioptim/__init__.py | 8 ++ bioptim/dynamics/configure_problem.py | 25 +++++- .../example_external_forces.py | 73 ++++++++++++----- bioptim/misc/enums.py | 17 ++++ bioptim/models/biorbd/biorbd_model.py | 42 ++++++---- bioptim/optimization/non_linear_program.py | 81 +++++++------------ 6 files changed, 158 insertions(+), 88 deletions(-) diff --git a/bioptim/__init__.py b/bioptim/__init__.py index 99d9408cd..7360e0f1b 100644 --- a/bioptim/__init__.py +++ b/bioptim/__init__.py @@ -173,7 +173,9 @@ from .dynamics.fatigue.michaud_fatigue import MichaudFatigue, MichaudTauFatigue from .dynamics.fatigue.effort_perception import EffortPerception, TauEffortPerception from .dynamics.ode_solver import OdeSolver, OdeSolverBase + from .interfaces import Solver + from .models.biorbd.biorbd_model import BiorbdModel from .models.biorbd.multi_biorbd_model import MultiBiorbdModel from .models.biorbd.holonomic_biorbd_model import HolonomicBiorbdModel @@ -182,6 +184,7 @@ from .models.holonomic_constraints import HolonomicConstraintsFcn, HolonomicConstraintsList from .models.protocols.stochastic_biomodel import StochasticBioModel from .models.protocols.biomodel import BioModel + from .limits.constraints import ConstraintFcn, ConstraintList, Constraint, ParameterConstraintList from .limits.phase_transition import PhaseTransitionFcn, PhaseTransitionList, PhaseTransition from .limits.multinode_constraint import MultinodeConstraintFcn, MultinodeConstraintList, MultinodeConstraint @@ -191,6 +194,7 @@ from .limits.fatigue_path_conditions import FatigueBounds, FatigueInitialGuess from .limits.penalty_controller import PenaltyController from .limits.penalty_helpers import PenaltyHelpers + from .misc.enums import ( Axis, Node, @@ -209,8 +213,12 @@ MultiCyclicCycleSolutions, PhaseDynamics, OnlineOptim, + ReferenceFrame, + ExternalForcesType, ) from .misc.mapping import BiMappingList, BiMapping, Mapping, NodeMapping, NodeMappingList, SelectionMapping, Dependency +from .misc.external_forces import ExternalForces, ExternalForcesList + from .optimization.multi_start import MultiStart from .optimization.non_linear_program import NonLinearProgram from .optimization.optimal_control_program import OptimalControlProgram diff --git a/bioptim/dynamics/configure_problem.py b/bioptim/dynamics/configure_problem.py index ce18ee063..49e9cf360 100644 --- a/bioptim/dynamics/configure_problem.py +++ b/bioptim/dynamics/configure_problem.py @@ -15,10 +15,12 @@ RigidBodyDynamics, SoftContactDynamics, PhaseDynamics, + ReferenceFrame, ) from ..misc.fcn_enum import FcnEnum from ..misc.mapping import BiMapping, Mapping from ..misc.options import UniquePerPhaseOptionList, OptionGeneric +from ..misc.external_forces import ExternalForcesList from ..models.protocols.stochastic_biomodel import StochasticBioModel from ..optimization.problem_type import SocpType @@ -2050,6 +2052,7 @@ def __init__( state_continuity_weight: float | None = None, phase_dynamics: PhaseDynamics = PhaseDynamics.SHARED_DURING_THE_PHASE, numerical_data_timeseries: dict[str, np.ndarray] = None, + external_forces: ExternalForcesList = None, **extra_parameters: Any, ): """ @@ -2097,7 +2100,27 @@ def __init__( self.state_continuity_weight = state_continuity_weight self.phase_dynamics = phase_dynamics self.numerical_data_timeseries = numerical_data_timeseries - + self.external_forces_in_numerical_timeseries(external_forces) + + def external_forces_in_numerical_timeseries(self, external_forces): + if external_forces: + self.numerical_data_timeseries = {} if self.numerical_data_timeseries is None else self.numerical_data_timeseries + for key in external_forces.keys(): + data = None + force = external_forces[key] + if force.torque_data is not None: + data = force.torque_data + if force.linear_force_data is not None: + data = force.linear_force_data if data is None else np.concatenate((data, force.linear_force_data), axis=1) + if force.point_of_application is not None: + data = force.point_of_application if data is None else np.concatenate((data, force.point_of_application), axis=1) + + if force.force_reference_frame == ReferenceFrame.LOCAL: + self.numerical_data_timeseries["forces_in_local"] = data + elif force.point_of_application_reference_frame == ReferenceFrame.LOCAL: + self.numerical_data_timeseries["translational_forces"] = data + else: + self.numerical_data_timeseries["forces_in_global"] = data class DynamicsList(UniquePerPhaseOptionList): """ diff --git a/bioptim/examples/getting_started/example_external_forces.py b/bioptim/examples/getting_started/example_external_forces.py index 38894b443..5f1d3004c 100644 --- a/bioptim/examples/getting_started/example_external_forces.py +++ b/bioptim/examples/getting_started/example_external_forces.py @@ -30,6 +30,9 @@ OdeSolverBase, Solver, PhaseDynamics, + ExternalForcesList, + ExternalForcesType, + ReferenceFrame, ) @@ -69,34 +72,62 @@ def prepare_ocp( The OptimalControlProgram ready to be solved """ - bio_model = BiorbdModel(biorbd_model_path, segments_to_apply_forces_in_global=["Seg1", "Test"]) - # segments_to_apply_forces_in_global is necessary to define the external forces. - # Please note that they should be declared in the same order as the external forces (in the global reference frame) values bellow. - # Problem parameters n_shooting = 30 final_time = 2 + # TODO: add Seg1_torque = np.zeros((3, n_shooting + 1)) to test + # TODO test global et local + + # Linear external forces + Seg1_force = np.zeros((3, n_shooting + 1)) + Seg1_force[2, :] = -2 + Seg1_force[2, 4] = -22 + + Test_force = np.zeros((3, n_shooting + 1)) + Test_force[2, :] = 5 + Test_force[2, 4] = 52 + + if use_point_of_applications: + Seg1_point_of_application = np.zeros((3, n_shooting + 1)) + Seg1_point_of_application[0, :] = 0.05 + Seg1_point_of_application[1, :] = -0.05 + Seg1_point_of_application[2, :] = 0.007 + + Test_point_of_application = np.zeros((3, n_shooting + 1)) + Test_point_of_application[0, :] = -0.009 + Test_point_of_application[1, :] = 0.01 + Test_point_of_application[2, :] = -0.01 + else: + Seg1_point_of_application = None + Test_point_of_application = None + + external_forces = ExternalForcesList() + external_forces.add( + key="Seg1", # Name of the segment where the external force is applied + data=Seg1_force, # 3 x (n_shooting_points+1) array + force_type=ExternalForcesType.LINEAR_FORCE, # Type of the external force + force_reference_frame=ReferenceFrame.GLOBAL, # Reference frame of the external force + point_of_application=Seg1_point_of_application, # Position of the point of application + point_of_application_reference_frame=ReferenceFrame.GLOBAL, # Reference frame of the point of application + phase=0, # Pariterre, did we deal with phases already? + ) + external_forces.add( + key="Test", # Name of the segment where the external force is applied + data=Test_force, # 3 x (n_shooting_points+1) array + force_type=ExternalForcesType.LINEAR_FORCE, # Type of the external force + force_reference_frame=ReferenceFrame.GLOBAL, # Reference frame of the external force + point_of_application=Test_point_of_application, # Position of the point of application + point_of_application_reference_frame=ReferenceFrame.GLOBAL, # Reference frame of the point of application + phase=0, + ) + + bio_model = BiorbdModel(biorbd_model_path, external_forces=external_forces) + # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100) - # External forces (shape: 9 x nb_external_forces x (n_shooting_points+1)) - # First components are the moments and forces - forces_in_global = np.zeros((9, 2, n_shooting + 1)) - forces_in_global[5, 0, :] = -2 - forces_in_global[5, 1, :] = 5 - forces_in_global[5, 0, 4] = -22 - forces_in_global[5, 1, 4] = 52 - if use_point_of_applications: - # Last components are the point of application - forces_in_global[6, 0, :] = 0.05 - forces_in_global[7, 1, :] = 0.01 - forces_in_global[8, 0, :] = 0.007 - forces_in_global[6, 1, :] = -0.009 - forces_in_global[7, 0, :] = -0.05 - forces_in_global[8, 1, :] = -0.01 - # Dynamics dynamics = DynamicsList() dynamics.add( @@ -104,7 +135,7 @@ def prepare_ocp( DynamicsFcn.TORQUE_DRIVEN, expand_dynamics=expand_dynamics, phase_dynamics=phase_dynamics, - numerical_data_timeseries={"forces_in_global": forces_in_global}, # the key word "forces_in_global" must be used + external_forces=external_forces, ) # Constraints diff --git a/bioptim/misc/enums.py b/bioptim/misc/enums.py index fd93b6d89..188baf9b1 100644 --- a/bioptim/misc/enums.py +++ b/bioptim/misc/enums.py @@ -222,3 +222,20 @@ class MultiCyclicCycleSolutions(Enum): NONE = "none" FIRST_CYCLES = "first_cycles" ALL_CYCLES = "all_cycles" + + +class ReferenceFrame(Enum): + """ + Selection of reference frame + """ + + GLOBAL = "global" + LOCAL = "local" + +class ExternalForcesType(Enum): + """ + Selection of external forces type + """ + + LINEAR_FORCE = "linear_force" + TORQUE = "torque" \ No newline at end of file diff --git a/bioptim/models/biorbd/biorbd_model.py b/bioptim/models/biorbd/biorbd_model.py index aa5439103..19bd5cbca 100644 --- a/bioptim/models/biorbd/biorbd_model.py +++ b/bioptim/models/biorbd/biorbd_model.py @@ -13,6 +13,8 @@ from ...limits.path_conditions import Bounds from ...misc.mapping import BiMapping, BiMappingList from ...misc.utils import check_version +from ...misc.external_forces import ExternalForcesList, get_external_forces_segments +from ...misc.enums import ReferenceFrame from ...optimization.parameters import ParameterList check_version(biorbd, "1.11.1", "1.12.0") @@ -27,8 +29,7 @@ def __init__( self, bio_model: str | biorbd.Model, friction_coefficients: np.ndarray = None, - segments_to_apply_forces_in_global: list[str] = None, - segments_to_apply_translational_forces: list[str] = None, + external_forces: ExternalForcesList = None, parameters: ParameterList = None, ): """ @@ -37,11 +38,7 @@ def __init__( ---------- bio_model friction_coefficients - TODO: @ipuch - segments_to_apply_forces_in_global: - Moments and forces (Mx, My Mz, Fx, Fy, Fz, Px, Py, Pz) expressed in the global reference frame will be applied at the global origin or the point of application expressed in the global reference frame to the segments listed here - segments_to_apply_translational_forces: - Forces (Fx, Fy, Fz, Px, Py, Pz) expressed in the local reference frame will be applied at the point of application expressed in the local reference frame of the segments listed here + external_forces parameters """ if not isinstance(bio_model, str) and not isinstance(bio_model, biorbd.Model): @@ -52,11 +49,11 @@ def __init__( for param_key in parameters: parameters[param_key].apply_parameter(self) self._friction_coefficients = friction_coefficients - if segments_to_apply_forces_in_global is None: - segments_to_apply_forces_in_global = [] + + # TODO extend the interface to make conversions and allow all mixed cases in an unified way + segments_to_apply_forces_in_global, segments_to_apply_forces_in_local, segments_to_apply_translational_forces = get_external_forces_segments(external_forces) self._segments_to_apply_forces_in_global = segments_to_apply_forces_in_global - if segments_to_apply_translational_forces is None: - segments_to_apply_translational_forces = [] + self._segments_to_apply_forces_in_local = segments_to_apply_forces_in_local self._segments_to_apply_translational_forces = segments_to_apply_translational_forces # Declaration of MX variables of the right shape for the creation of CasADi Functions @@ -69,7 +66,7 @@ def __init__( self.activations = MX.sym("activations_mx", self.nb_muscles, 1) self.external_forces = MX.sym( - "external_forces_mx", 9 * self.nb_forces_in_global + 6 * self.nb_translational_forces, 1 + "external_forces_mx", 9 * self.nb_forces_in_global + 9 * self.nb_forces_in_local + 6 * self.nb_translational_forces, 1 ) self.external_forces_set = self._dispatch_forces() @@ -80,6 +77,10 @@ def __init__( def nb_forces_in_global(self) -> int: return len(self._segments_to_apply_forces_in_global) + @property + def nb_forces_in_local(self) -> int: + return len(self._segments_to_apply_forces_in_local) + @property def nb_translational_forces(self) -> int: return len(self._segments_to_apply_translational_forces) @@ -447,25 +448,36 @@ def reorder_qddot_root_joints(qddot_root, qddot_joints) -> MX | SX: def _dispatch_forces(self): - if self.nb_forces_in_global == 0 and self.nb_translational_forces == 0: + if self.nb_forces_in_global == 0 and self.nb_forces_in_local == 0 and self.nb_translational_forces == 0: return None else: external_forces_set = self.model.externalForceSet() forces_in_global = reshape( self.external_forces[: 9 * self.nb_forces_in_global], (9, self.nb_forces_in_global) ) + forces_in_local = reshape( + self.external_forces[9 * self.nb_forces_in_global : 9 * self.nb_forces_in_global + 9 * self.nb_forces_in_local], (9, self.nb_forces_in_local) + ) translational_forces = reshape( - self.external_forces[9 * self.nb_forces_in_global :], (6, self.nb_translational_forces) + self.external_forces[9 * self.nb_forces_in_global + 9 * self.nb_forces_in_local :], (6, self.nb_translational_forces) ) if forces_in_global.shape[1] > 0: - # Add the external forces + # Add the external forces in the global reference frame for i_element in range(forces_in_global.shape[1]): name = self._segments_to_apply_forces_in_global[i_element] values = forces_in_global[:6, i_element] point_of_application = forces_in_global[6:9, i_element] external_forces_set.add(name, values, point_of_application) + if forces_in_local.shape[1] > 0: + # Add the external forces in the local reference frame + for i_element in range(forces_in_local.shape[1]): + name = self._segments_to_apply_forces_in_local[i_element] + values = forces_in_local[:6, i_element] + point_of_application = forces_in_local[6:9, i_element] + external_forces_set.addInSegmentReferenceFrame(name, values, point_of_application) + elif translational_forces.shape[1] > 0: # Add the translational forces for i_elements in range(translational_forces.shape[1]): diff --git a/bioptim/optimization/non_linear_program.py b/bioptim/optimization/non_linear_program.py index 87d87645d..6f2780a9f 100644 --- a/bioptim/optimization/non_linear_program.py +++ b/bioptim/optimization/non_linear_program.py @@ -454,58 +454,37 @@ def get_var_from_states_or_controls( def get_external_forces( self, states: MX.sym, controls: MX.sym, algebraic_states: MX.sym, numerical_timeseries: MX.sym ): - external_forces = self.cx(0, 1) - if "forces_in_global" in self.states: - external_forces = vertcat(external_forces, DynamicsFunctions.get(self.states["forces_in_global"], states)) - if "forces_in_global" in self.controls: - external_forces = vertcat( - external_forces, DynamicsFunctions.get(self.controls["forces_in_global"], controls) - ) - if "forces_in_global" in self.algebraic_states: - external_forces = vertcat( - external_forces, DynamicsFunctions.get(self.algebraic_states["forces_in_global"], algebraic_states) - ) - - if self.numerical_timeseries is not None: - component_numerical_timeseries = 0 - for key in self.numerical_timeseries.keys(): - if "forces_in_global" in key: - component_numerical_timeseries += 1 - if component_numerical_timeseries > 0: - for i_component in range(component_numerical_timeseries): - external_forces = vertcat( - external_forces, - DynamicsFunctions.get( - self.numerical_timeseries[f"forces_in_global_{i_component}"], numerical_timeseries - ), - ) + def retrieve_forces(name: str, external_forces: MX): + if name in self.states: + external_forces = vertcat(external_forces, + DynamicsFunctions.get(self.states[name], states)) + if name in self.controls: + external_forces = vertcat( + external_forces, DynamicsFunctions.get(self.controls[name], controls) + ) + if name in self.algebraic_states: + external_forces = vertcat( + external_forces, DynamicsFunctions.get(self.algebraic_states[name], algebraic_states) + ) + if self.numerical_timeseries is not None: + component_numerical_timeseries = 0 + for key in self.numerical_timeseries.keys(): + if name in key: + component_numerical_timeseries += 1 + if component_numerical_timeseries > 0: + for i_component in range(component_numerical_timeseries): + external_forces = vertcat( + external_forces, + DynamicsFunctions.get( + self.numerical_timeseries[f"{name}_{i_component}"], numerical_timeseries + ), + ) + return external_forces - if "translational_forces" in self.states: - external_forces = vertcat( - external_forces, DynamicsFunctions.get(self.states["translational_forces"], states) - ) - if "translational_forces" in self.controls: - external_forces = vertcat( - external_forces, DynamicsFunctions.get(self.controls["translational_forces"], controls) - ) - if "translational_forces" in self.algebraic_states: - external_forces = vertcat( - external_forces, DynamicsFunctions.get(self.algebraic_states["translational_forces"], algebraic_states) - ) - - if self.numerical_timeseries is not None: - component_numerical_timeseries = 0 - for key in self.numerical_timeseries.keys(): - if "translational_forces" in key: - component_numerical_timeseries += 1 - if component_numerical_timeseries > 0: - for i_component in range(component_numerical_timeseries): - external_forces = vertcat( - external_forces, - DynamicsFunctions.get( - self.numerical_timeseries[f"translational_forces{i_component}"], numerical_timeseries - ), - ) + external_forces = self.cx(0, 1) + external_forces = retrieve_forces("forces_in_global", external_forces) + external_forces = retrieve_forces("forces_in_local", external_forces) + external_forces = retrieve_forces("translational_forces", external_forces) return external_forces From a7f55832ab56b70b0283894e40b54e014d68d7b7 Mon Sep 17 00:00:00 2001 From: eve-mac Date: Thu, 24 Oct 2024 08:38:05 -0400 Subject: [PATCH 049/108] test external forces work --- bioptim/dynamics/configure_problem.py | 29 ++++++++++++++++++++------- 1 file changed, 22 insertions(+), 7 deletions(-) diff --git a/bioptim/dynamics/configure_problem.py b/bioptim/dynamics/configure_problem.py index 49e9cf360..f331a9fe5 100644 --- a/bioptim/dynamics/configure_problem.py +++ b/bioptim/dynamics/configure_problem.py @@ -2106,21 +2106,36 @@ def external_forces_in_numerical_timeseries(self, external_forces): if external_forces: self.numerical_data_timeseries = {} if self.numerical_data_timeseries is None else self.numerical_data_timeseries for key in external_forces.keys(): - data = None force = external_forces[key] if force.torque_data is not None: - data = force.torque_data + data = force.torque_data.reshape(3, 1, -1) + else: + data = np.zeros((3, 1, force.len)) if force.linear_force_data is not None: - data = force.linear_force_data if data is None else np.concatenate((data, force.linear_force_data), axis=1) + data = np.concatenate((data, force.linear_force_data.reshape(3, 1, -1)), axis=0) + else: + data = np.concatenate((data, np.zeros((3, 1, force.len))), axis=0) if force.point_of_application is not None: - data = force.point_of_application if data is None else np.concatenate((data, force.point_of_application), axis=1) + data = np.concatenate((data, force.point_of_application.reshape(3, 1, -1)), axis=0) + else: + data = np.concatenate((data, np.zeros((3, 1, force.len))), axis=0) if force.force_reference_frame == ReferenceFrame.LOCAL: - self.numerical_data_timeseries["forces_in_local"] = data + if "forces_in_local" in self.numerical_data_timeseries: + self.numerical_data_timeseries["forces_in_local"] = np.concatenate((self.numerical_data_timeseries["forces_in_local"], data), axis=1) + else: + self.numerical_data_timeseries["forces_in_local"] = data elif force.point_of_application_reference_frame == ReferenceFrame.LOCAL: - self.numerical_data_timeseries["translational_forces"] = data + # exclude the torques for translational forces + if "translational_forces" in self.numerical_data_timeseries: + self.numerical_data_timeseries["translational_forces"] = np.concatenate((self.numerical_data_timeseries["translational_forces"], data[3:, :, :]), axis=1) + else: + self.numerical_data_timeseries["translational_forces"] = data[3:, :, ] else: - self.numerical_data_timeseries["forces_in_global"] = data + if "forces_in_global" in self.numerical_data_timeseries: + self.numerical_data_timeseries["forces_in_global"] = np.concatenate((self.numerical_data_timeseries["forces_in_global"], data), axis=1) + else: + self.numerical_data_timeseries["forces_in_global"] = data class DynamicsList(UniquePerPhaseOptionList): """ From e49f1d5730478ad8a9e43d77eeb16a23d72b96d4 Mon Sep 17 00:00:00 2001 From: eve-mac Date: Thu, 24 Oct 2024 11:13:50 -0400 Subject: [PATCH 050/108] added tests --- .../example_external_forces.py | 19 +- tests/shard3/test_external_forces.py | 400 ++++++++++++++++++ 2 files changed, 409 insertions(+), 10 deletions(-) create mode 100644 tests/shard3/test_external_forces.py diff --git a/bioptim/examples/getting_started/example_external_forces.py b/bioptim/examples/getting_started/example_external_forces.py index 5f1d3004c..6929354e3 100644 --- a/bioptim/examples/getting_started/example_external_forces.py +++ b/bioptim/examples/getting_started/example_external_forces.py @@ -41,7 +41,10 @@ def prepare_ocp( ode_solver: OdeSolverBase = OdeSolver.RK4(), expand_dynamics: bool = True, phase_dynamics: PhaseDynamics = PhaseDynamics.ONE_PER_NODE, + force_type: ExternalForcesType = ExternalForcesType.LINEAR_FORCE, + force_reference_frame: ReferenceFrame = ReferenceFrame.GLOBAL, use_point_of_applications: bool = False, + point_of_application_reference_frame: ReferenceFrame = ReferenceFrame.GLOBAL, n_threads: int = 1, use_sx: bool = False, ) -> OptimalControlProgram: @@ -76,9 +79,6 @@ def prepare_ocp( n_shooting = 30 final_time = 2 - # TODO: add Seg1_torque = np.zeros((3, n_shooting + 1)) to test - # TODO test global et local - # Linear external forces Seg1_force = np.zeros((3, n_shooting + 1)) Seg1_force[2, :] = -2 @@ -106,19 +106,19 @@ def prepare_ocp( external_forces.add( key="Seg1", # Name of the segment where the external force is applied data=Seg1_force, # 3 x (n_shooting_points+1) array - force_type=ExternalForcesType.LINEAR_FORCE, # Type of the external force - force_reference_frame=ReferenceFrame.GLOBAL, # Reference frame of the external force + force_type=force_type, # Type of the external force (ExternalForcesType.LINEAR_FORCE) + force_reference_frame=force_reference_frame, # Reference frame of the external force (ReferenceFrame.GLOBAL) point_of_application=Seg1_point_of_application, # Position of the point of application - point_of_application_reference_frame=ReferenceFrame.GLOBAL, # Reference frame of the point of application + point_of_application_reference_frame=point_of_application_reference_frame, # Reference frame of the point of application (ReferenceFrame.GLOBAL) phase=0, # Pariterre, did we deal with phases already? ) external_forces.add( key="Test", # Name of the segment where the external force is applied data=Test_force, # 3 x (n_shooting_points+1) array - force_type=ExternalForcesType.LINEAR_FORCE, # Type of the external force - force_reference_frame=ReferenceFrame.GLOBAL, # Reference frame of the external force + force_type=force_type, # Type of the external force (ExternalForcesType.LINEAR_FORCE) + force_reference_frame=force_reference_frame, # Reference frame of the external force (ReferenceFrame.GLOBAL) point_of_application=Test_point_of_application, # Position of the point of application - point_of_application_reference_frame=ReferenceFrame.GLOBAL, # Reference frame of the point of application + point_of_application_reference_frame=point_of_application_reference_frame, # Reference frame of the point of application (ReferenceFrame.GLOBAL) phase=0, ) @@ -131,7 +131,6 @@ def prepare_ocp( # Dynamics dynamics = DynamicsList() dynamics.add( - # This must be PhaseDynamics.ONE_PER_NODE since external forces change at each node within the phase DynamicsFcn.TORQUE_DRIVEN, expand_dynamics=expand_dynamics, phase_dynamics=phase_dynamics, diff --git a/tests/shard3/test_external_forces.py b/tests/shard3/test_external_forces.py new file mode 100644 index 000000000..4a3219bbe --- /dev/null +++ b/tests/shard3/test_external_forces.py @@ -0,0 +1,400 @@ +import os + +import numpy as np +import numpy.testing as npt +import pytest + +from bioptim import ( + PhaseDynamics, + SolutionMerge, + ExternalForcesType, + ReferenceFrame, + BiorbdModel, + Node, + OptimalControlProgram, + DynamicsList, + DynamicsFcn, + ExternalForcesList, + ObjectiveList, + ObjectiveFcn, + ConstraintList, + ConstraintFcn, + BoundsList, +) + + +@pytest.mark.parametrize("phase_dynamics", [PhaseDynamics.SHARED_DURING_THE_PHASE, PhaseDynamics.ONE_PER_NODE]) +@pytest.mark.parametrize("use_sx", [True, False]) +@pytest.mark.parametrize("force_type", [ExternalForcesType.LINEAR_FORCE, ExternalForcesType.TORQUE]) +@pytest.mark.parametrize("force_reference_frame", [ReferenceFrame.GLOBAL, ReferenceFrame.LOCAL]) +@pytest.mark.parametrize("use_point_of_applications", [True, False]) +@pytest.mark.parametrize("point_of_application_reference_frame", [ReferenceFrame.GLOBAL, ReferenceFrame.LOCAL]) +def test_example_external_forces(phase_dynamics, + use_sx, + force_type, + force_reference_frame, + use_point_of_applications, + point_of_application_reference_frame): + from bioptim.examples.getting_started import example_external_forces as ocp_module + + bioptim_folder = os.path.dirname(ocp_module.__file__) + + if use_point_of_applications == False: + point_of_application_reference_frame = None + + if force_type == ExternalForcesType.LINEAR_FORCE and force_reference_frame == ReferenceFrame.GLOBAL and point_of_application_reference_frame == ReferenceFrame.GLOBAL: + # This combination is already tested in test_global_getting_started.py + return + + # Errors for some combinations + if force_type == ExternalForcesType.TORQUE and use_point_of_applications: + with pytest.raises( + ValueError, + match="Point of application cannot be used with ExternalForcesType.TORQUE", + ): + ocp = ocp_module.prepare_ocp( + biorbd_model_path=bioptim_folder + "/models/cube_with_forces.bioMod", + phase_dynamics=phase_dynamics, + force_type=force_type, + force_reference_frame=force_reference_frame, + use_point_of_applications=use_point_of_applications, + point_of_application_reference_frame=point_of_application_reference_frame, + use_sx=use_sx, + ) + return + + if force_reference_frame == ReferenceFrame.LOCAL and point_of_application_reference_frame == ReferenceFrame.GLOBAL: + with pytest.raises( + NotImplementedError, + match="External forces in local reference frame cannot have a point of application in the global reference frame yet", + ): + ocp = ocp_module.prepare_ocp( + biorbd_model_path=bioptim_folder + "/models/cube_with_forces.bioMod", + phase_dynamics=phase_dynamics, + force_type=force_type, + force_reference_frame=force_reference_frame, + use_point_of_applications=use_point_of_applications, + point_of_application_reference_frame=point_of_application_reference_frame, + use_sx=use_sx, + ) + return + + if force_type == ExternalForcesType.TORQUE and force_reference_frame == ReferenceFrame.LOCAL: + with pytest.raises( + ValueError, + match="External torques are defined in global reference frame", + ): + ocp = ocp_module.prepare_ocp( + biorbd_model_path=bioptim_folder + "/models/cube_with_forces.bioMod", + phase_dynamics=phase_dynamics, + force_type=force_type, + force_reference_frame=force_reference_frame, + use_point_of_applications=use_point_of_applications, + point_of_application_reference_frame=point_of_application_reference_frame, + use_sx=use_sx, + ) + return + + ocp = ocp_module.prepare_ocp( + biorbd_model_path=bioptim_folder + "/models/cube_with_forces.bioMod", + phase_dynamics=phase_dynamics, + force_type=force_type, + force_reference_frame=force_reference_frame, + use_point_of_applications=use_point_of_applications, + point_of_application_reference_frame=point_of_application_reference_frame, + use_sx=use_sx, + ) + sol = ocp.solve() + + # Check objective function value + f = np.array(sol.cost) + npt.assert_equal(f.shape, (1, 1)) + + # Check constraints + g = np.array(sol.constraints) + npt.assert_equal(g.shape, (246, 1)) + npt.assert_almost_equal(g, np.zeros((246, 1))) + + # Check some of the results + states = sol.decision_states(to_merge=SolutionMerge.NODES) + controls = sol.decision_controls(to_merge=SolutionMerge.NODES) + q, qdot, tau = states["q"], states["qdot"], controls["tau"] + + # initial and final velocities + npt.assert_almost_equal(qdot[:, 0], np.array([0.0, 0.0, 0.0, 0.0]), decimal=5) + npt.assert_almost_equal(qdot[:, -1], np.array([0.0, 0.0, 0.0, 0.0]), decimal=5) + + if force_type == ExternalForcesType.TORQUE: + + npt.assert_almost_equal(f[0, 0], 19847.887805189126) + + # initial and final controls + npt.assert_almost_equal(tau[:, 0], np.array([2.03776708e-09, 1.27132259e+01, 2.32230666e-27, 0.0])) + npt.assert_almost_equal(tau[:, 10], np.array([-8.23139024e-10, 1.07110012e+01, 5.45884198e-26, 0.0])) + npt.assert_almost_equal(tau[:, 20], np.array([-6.72563418e-10, 8.70877651e+00, -2.09699944e-26, 0.0])) + npt.assert_almost_equal(tau[:, -1], np.array([2.03777147e-09, 6.90677426e+00, -1.66232727e-25, 0.0])) + + # initial and final position + npt.assert_almost_equal(q[:, 0], np.array([-4.69166539e-15, 6.03679344e-16, -1.43453710e-17, 0.0]), decimal=5) + npt.assert_almost_equal(q[:, -1], np.array([-4.69169398e-15, 2.00000000e+00, -1.43453709e-17, 0.0]), decimal=5) + + # how the force is stored + data = ocp.nlp[0].numerical_data_timeseries["forces_in_global"] + npt.assert_equal(data.shape, (9, 2, 31)) + npt.assert_almost_equal(data[:, 0, 0], np.array([0., 0., -2., 0., 0., 0., 0., 0., 0.])) + + else: + if force_reference_frame == ReferenceFrame.GLOBAL: + if use_point_of_applications: + if point_of_application_reference_frame == ReferenceFrame.LOCAL: + npt.assert_almost_equal(f[0, 0], 7067.851604540217) + + # initial and final controls + npt.assert_almost_equal(tau[:, 0], np.array([2.03776698e-09, 6.98419368e+00, -8.87085933e-09, 0.0])) + npt.assert_almost_equal(tau[:, 10], np.array([-8.23139073e-10, 6.24337052e+00, -9.08634557e-09, 0.0])) + npt.assert_almost_equal(tau[:, 20], np.array([-6.72563331e-10, 5.50254736e+00, -9.36203643e-09, 0.00])) + npt.assert_almost_equal(tau[:, -1], np.array([2.03777157e-09, 4.83580652e+00, -9.46966399e-09, 0.0])) + + # initial and final position + npt.assert_almost_equal(q[:, 0], np.array([-4.69166257e-15, 6.99774238e-16, -1.15797965e+00, 0.0]), + decimal=5) + npt.assert_almost_equal(q[:, -1], np.array([-4.69169076e-15, 2.00000000e+00, -1.15744926e+00, 0.0]), + decimal=5) + + # how the force is stored + data = ocp.nlp[0].numerical_data_timeseries["translational_forces"] + npt.assert_equal(data.shape, (6, 2, 31)) + npt.assert_almost_equal(data[:, 0, 0], np.array([0., 0., -2., 0.05, -0.05, 0.007])) + + else: + npt.assert_almost_equal(f[0, 0], 7067.851604540217) + + # initial and final controls + npt.assert_almost_equal(tau[:, 0], np.array([1.53525114e-09, 6.98419368e+00, -4.06669623e-10, 0.0])) + npt.assert_almost_equal(tau[:, 10], np.array([-6.45031008e-10, 6.24337052e+00, -4.06669624e-10, 0.0])) + npt.assert_almost_equal(tau[:, 20], np.array([-5.51259131e-10, 5.50254736e+00, -4.06669632e-10, 0.0])) + npt.assert_almost_equal(tau[:, -1], np.array([1.64434894e-09, 4.83580652e+00, -4.06669638e-10, 0.0])) + + # initial and final position + npt.assert_almost_equal(q[:, 0], np.array([-3.41506199e-15, 6.99773953e-16, -4.11684795e-12, 0.0]), decimal=5) + npt.assert_almost_equal(q[:, -1], np.array([-3.84536375e-15, 2.00000000e+00, 6.27099006e-11, 0.0]), decimal=5) + + # how the force is stored + data = ocp.nlp[0].numerical_data_timeseries["forces_in_global"] + npt.assert_equal(data.shape, (9, 2, 31)) + npt.assert_almost_equal(data[:, 0, 0], np.array([0., 0., 0., 0., 0., -2., 0., 0., 0.])) + + else: + if use_point_of_applications: + if point_of_application_reference_frame == ReferenceFrame.LOCAL: + npt.assert_almost_equal(f[0, 0], 7076.043800572127) + + # initial and final controls + npt.assert_almost_equal(tau[:, 0], np.array([0.01922272, 6.98428645, -0.28099094, 0.])) + npt.assert_almost_equal(tau[:, 10], np.array([4.40289150e-03, 6.24343465e+00, -2.16557082e-01, 0.0])) + npt.assert_almost_equal(tau[:, 20], np.array([-0.01041693, 5.50258285, -0.14650525, 0.])) + npt.assert_almost_equal(tau[:, -1], np.array([-0.02375477, 4.83581623, -0.12679319, 0.])) + + # initial and final position + npt.assert_almost_equal(q[:, 0], np.array([-4.65561476e-15, 7.00215056e-16, -1.34683990e-03, 0.0]), + decimal=5) + npt.assert_almost_equal(q[:, -1], np.array([-4.76138727e-15, 2.00000000e+00, 2.00652560e-03, 0.0]), + decimal=5) + + # how the force is stored + data = ocp.nlp[0].numerical_data_timeseries["forces_in_local"] + npt.assert_equal(data.shape, (9, 2, 31)) + npt.assert_almost_equal(data[:, 0, 0], np.array([0., 0., 0., 0., 0., -2., 0.05, -0.05, + 0.007])) + else: + npt.assert_almost_equal(f[0, 0], 7067.851604540217) + + # initial and final controls + npt.assert_almost_equal(tau[:, 0], np.array([1.92280974e-09, 6.98419368e+00, -4.00749051e-10, 0.0])) + npt.assert_almost_equal(tau[:, 10], np.array([-8.87099972e-10, 6.24337052e+00, 2.22483147e-10, 0.0])) + npt.assert_almost_equal(tau[:, 20], np.array([-6.85527926e-10, 5.50254736e+00, 1.70072550e-10, 0.0])) + npt.assert_almost_equal(tau[:, -1], np.array([2.07070380e-09, 4.83580652e+00, -3.91202705e-10, 0.0])) + + # initial and final position + npt.assert_almost_equal(q[:, 0], np.array([-4.64041726e-15, 6.99774094e-16, 4.75924448e-11, 0.0]), + decimal=5) + npt.assert_almost_equal(q[:, -1], np.array([-4.74298258e-15, 2.00000000e+00, 3.79354612e-11, 0.0]), + decimal=5) + + # how the force is stored + data = ocp.nlp[0].numerical_data_timeseries["forces_in_local"] + npt.assert_equal(data.shape, (9, 2, 31)) + npt.assert_almost_equal(data[:, 0, 0], np.array([0., 0., 0., 0., 0., -2., 0.0, 0.0, + 0.0])) + + # detailed cost values + npt.assert_almost_equal(sol.detailed_cost[0]["cost_value_weighted"], f[0, 0]) + + + +def prepare_ocp( + biorbd_model_path: str = "models/cube_with_forces.bioMod", +) -> OptimalControlProgram: + + # Problem parameters + n_shooting = 30 + final_time = 2 + + # Linear external forces + Seg1_force = np.zeros((3, n_shooting + 1)) + Seg1_force[2, :] = -2 + Seg1_force[2, 4] = -22 + + Test_force = np.zeros((3, n_shooting + 1)) + Test_force[2, :] = 5 + Test_force[2, 4] = 52 + + # Point of application + Seg1_point_of_application = np.zeros((3, n_shooting + 1)) + Seg1_point_of_application[0, :] = 0.05 + Seg1_point_of_application[1, :] = -0.05 + Seg1_point_of_application[2, :] = 0.007 + + Test_point_of_application = np.zeros((3, n_shooting + 1)) + Test_point_of_application[0, :] = -0.009 + Test_point_of_application[1, :] = 0.01 + Test_point_of_application[2, :] = -0.01 + + # Torques external forces + Seg1_torque = np.zeros((3, n_shooting + 1)) + Seg1_torque[1, :] = 1.8 + Seg1_torque[1, 4] = 18 + + Test_torque = np.zeros((3, n_shooting + 1)) + Test_torque[1, :] = -1.8 + Test_torque[1, 4] = -18 + + external_forces = ExternalForcesList() + external_forces.add( + key="Seg1", + data=Seg1_force, + force_type=ExternalForcesType.LINEAR_FORCE, + force_reference_frame=ReferenceFrame.GLOBAL, + point_of_application=Seg1_point_of_application, + point_of_application_reference_frame=ReferenceFrame.GLOBAL, + phase=0, + ) + external_forces.add( + key="Seg1", + data=Seg1_torque, + force_type=ExternalForcesType.TORQUE, + force_reference_frame=ReferenceFrame.GLOBAL, + point_of_application=None, + point_of_application_reference_frame=None, + phase=0, + ) + external_forces.add( + key="Test", + data=Test_force, + force_type=ExternalForcesType.LINEAR_FORCE, + force_reference_frame=ReferenceFrame.LOCAL, + point_of_application=Test_point_of_application, + point_of_application_reference_frame=ReferenceFrame.LOCAL, + phase=0, + ) + external_forces.add( + key="Test", + data=Test_torque, + force_type=ExternalForcesType.TORQUE, + force_reference_frame=ReferenceFrame.GLOBAL, + point_of_application=None, + point_of_application_reference_frame=None, + phase=0, + ) + + bio_model = BiorbdModel(biorbd_model_path, external_forces=external_forces) + + # Add objective functions + objective_functions = ObjectiveList() + objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100) + + # Dynamics + dynamics = DynamicsList() + dynamics.add( + DynamicsFcn.TORQUE_DRIVEN, + external_forces=external_forces, + ) + + # Constraints + constraints = ConstraintList() + constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.START, first_marker="m0", second_marker="m1") + constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m2") + + # Path constraint + x_bounds = BoundsList() + x_bounds["q"] = bio_model.bounds_from_ranges("q") + x_bounds["q"][3, [0, -1]] = 0 + x_bounds["qdot"] = bio_model.bounds_from_ranges("qdot") + x_bounds["qdot"][:3, [0, -1]] = 0 + + # Define control path constraint + u_bounds = BoundsList() + tau_min, tau_max = -100, 100 + u_bounds["tau"] = [tau_min] * bio_model.nb_tau, [tau_max] * bio_model.nb_tau + + return OptimalControlProgram( + bio_model, + dynamics, + n_shooting, + final_time, + x_bounds=x_bounds, + u_bounds=u_bounds, + objective_functions=objective_functions, + constraints=constraints, + ) + +def test_example_external_forces_all_at_once(): + + from bioptim.examples.getting_started import example_external_forces as ocp_module + bioptim_folder = os.path.dirname(ocp_module.__file__) + + ocp = prepare_ocp( + biorbd_model_path=bioptim_folder + "/models/cube_with_forces.bioMod", + ) + sol = ocp.solve() + + # Check objective function value + f = np.array(sol.cost) + npt.assert_equal(f.shape, (1, 1)) + npt.assert_almost_equal(f[0, 0], 7067.8516045402175) + + # Check constraints + g = np.array(sol.constraints) + npt.assert_equal(g.shape, (246, 1)) + npt.assert_almost_equal(g, np.zeros((246, 1))) + + # Check some of the results + states = sol.decision_states(to_merge=SolutionMerge.NODES) + controls = sol.decision_controls(to_merge=SolutionMerge.NODES) + q, qdot, tau = states["q"], states["qdot"], controls["tau"] + + # initial and final controls + npt.assert_almost_equal(tau[:, 0], np.array([1.56834240e-09, 6.98419368e+00, -1.49703626e-10, 0.0])) + npt.assert_almost_equal(tau[:, 10], np.array([-7.45768885e-10, 6.24337052e+00, 6.62627954e-10, 0.0])) + npt.assert_almost_equal(tau[:, 20], np.array([-5.73998856e-10, 5.50254736e+00, 5.21623414e-10, 0.0])) + npt.assert_almost_equal(tau[:, -1], np.array([1.79690124e-09, 4.83580652e+00, -2.97742253e-10, 0.0])) + + # initial and final position + npt.assert_almost_equal(q[:, 0], np.array([-4.07966993e-15, 6.99774100e-16, 3.12712716e-11, 0.0]), decimal=5) + npt.assert_almost_equal(q[:, -1], np.array([-4.35983004e-15, 2.00000000e+00, 2.69788862e-11, 0.0]), decimal=5) + + # initial and final velocities + npt.assert_almost_equal(qdot[:, 0], np.array([0.0, 0.0, 0.0, 0.0]), decimal=5) + npt.assert_almost_equal(qdot[:, -1], np.array([0.0, 0.0, 0.0, 0.0]), decimal=5) + + # how the force is stored + data_Seg1 = ocp.nlp[0].numerical_data_timeseries["forces_in_global"] + npt.assert_equal(data_Seg1.shape, (9, 1, 31)) + npt.assert_almost_equal(data_Seg1[:, 0, 0], np.array([0. , 1.8, 0. , 0. , 0. , -2. , 0. , 0. , 0.])) + + data_Test = ocp.nlp[0].numerical_data_timeseries["forces_in_local"] + npt.assert_equal(data_Test.shape, (9, 1, 31)) + npt.assert_almost_equal(data_Test[:, 0, 0], np.array([0. , -1.8, 0. , 0. , 0. , 5. , 0. , 0. , 0.])) + + # detailed cost values + npt.assert_almost_equal(sol.detailed_cost[0]["cost_value_weighted"], f[0, 0]) From 7fd0713e00479c9335f3831f318abf08f526cab3 Mon Sep 17 00:00:00 2001 From: eve-mac Date: Thu, 24 Oct 2024 11:14:36 -0400 Subject: [PATCH 051/108] blacked --- bioptim/dynamics/configure_problem.py | 22 +++++++++++++++++----- bioptim/misc/enums.py | 3 ++- bioptim/models/biorbd/biorbd_model.py | 18 ++++++++++++++---- bioptim/optimization/non_linear_program.py | 7 ++----- 4 files changed, 35 insertions(+), 15 deletions(-) diff --git a/bioptim/dynamics/configure_problem.py b/bioptim/dynamics/configure_problem.py index f331a9fe5..e30e34f89 100644 --- a/bioptim/dynamics/configure_problem.py +++ b/bioptim/dynamics/configure_problem.py @@ -2104,7 +2104,9 @@ def __init__( def external_forces_in_numerical_timeseries(self, external_forces): if external_forces: - self.numerical_data_timeseries = {} if self.numerical_data_timeseries is None else self.numerical_data_timeseries + self.numerical_data_timeseries = ( + {} if self.numerical_data_timeseries is None else self.numerical_data_timeseries + ) for key in external_forces.keys(): force = external_forces[key] if force.torque_data is not None: @@ -2122,21 +2124,31 @@ def external_forces_in_numerical_timeseries(self, external_forces): if force.force_reference_frame == ReferenceFrame.LOCAL: if "forces_in_local" in self.numerical_data_timeseries: - self.numerical_data_timeseries["forces_in_local"] = np.concatenate((self.numerical_data_timeseries["forces_in_local"], data), axis=1) + self.numerical_data_timeseries["forces_in_local"] = np.concatenate( + (self.numerical_data_timeseries["forces_in_local"], data), axis=1 + ) else: self.numerical_data_timeseries["forces_in_local"] = data elif force.point_of_application_reference_frame == ReferenceFrame.LOCAL: # exclude the torques for translational forces if "translational_forces" in self.numerical_data_timeseries: - self.numerical_data_timeseries["translational_forces"] = np.concatenate((self.numerical_data_timeseries["translational_forces"], data[3:, :, :]), axis=1) + self.numerical_data_timeseries["translational_forces"] = np.concatenate( + (self.numerical_data_timeseries["translational_forces"], data[3:, :, :]), axis=1 + ) else: - self.numerical_data_timeseries["translational_forces"] = data[3:, :, ] + self.numerical_data_timeseries["translational_forces"] = data[ + 3:, + :, + ] else: if "forces_in_global" in self.numerical_data_timeseries: - self.numerical_data_timeseries["forces_in_global"] = np.concatenate((self.numerical_data_timeseries["forces_in_global"], data), axis=1) + self.numerical_data_timeseries["forces_in_global"] = np.concatenate( + (self.numerical_data_timeseries["forces_in_global"], data), axis=1 + ) else: self.numerical_data_timeseries["forces_in_global"] = data + class DynamicsList(UniquePerPhaseOptionList): """ A list of Dynamics if more than one is required, typically when more than one phases are declared diff --git a/bioptim/misc/enums.py b/bioptim/misc/enums.py index 188baf9b1..79ad0a7e0 100644 --- a/bioptim/misc/enums.py +++ b/bioptim/misc/enums.py @@ -232,10 +232,11 @@ class ReferenceFrame(Enum): GLOBAL = "global" LOCAL = "local" + class ExternalForcesType(Enum): """ Selection of external forces type """ LINEAR_FORCE = "linear_force" - TORQUE = "torque" \ No newline at end of file + TORQUE = "torque" diff --git a/bioptim/models/biorbd/biorbd_model.py b/bioptim/models/biorbd/biorbd_model.py index 19bd5cbca..523bc127f 100644 --- a/bioptim/models/biorbd/biorbd_model.py +++ b/bioptim/models/biorbd/biorbd_model.py @@ -51,7 +51,11 @@ def __init__( self._friction_coefficients = friction_coefficients # TODO extend the interface to make conversions and allow all mixed cases in an unified way - segments_to_apply_forces_in_global, segments_to_apply_forces_in_local, segments_to_apply_translational_forces = get_external_forces_segments(external_forces) + ( + segments_to_apply_forces_in_global, + segments_to_apply_forces_in_local, + segments_to_apply_translational_forces, + ) = get_external_forces_segments(external_forces) self._segments_to_apply_forces_in_global = segments_to_apply_forces_in_global self._segments_to_apply_forces_in_local = segments_to_apply_forces_in_local self._segments_to_apply_translational_forces = segments_to_apply_translational_forces @@ -66,7 +70,9 @@ def __init__( self.activations = MX.sym("activations_mx", self.nb_muscles, 1) self.external_forces = MX.sym( - "external_forces_mx", 9 * self.nb_forces_in_global + 9 * self.nb_forces_in_local + 6 * self.nb_translational_forces, 1 + "external_forces_mx", + 9 * self.nb_forces_in_global + 9 * self.nb_forces_in_local + 6 * self.nb_translational_forces, + 1, ) self.external_forces_set = self._dispatch_forces() @@ -456,10 +462,14 @@ def _dispatch_forces(self): self.external_forces[: 9 * self.nb_forces_in_global], (9, self.nb_forces_in_global) ) forces_in_local = reshape( - self.external_forces[9 * self.nb_forces_in_global : 9 * self.nb_forces_in_global + 9 * self.nb_forces_in_local], (9, self.nb_forces_in_local) + self.external_forces[ + 9 * self.nb_forces_in_global : 9 * self.nb_forces_in_global + 9 * self.nb_forces_in_local + ], + (9, self.nb_forces_in_local), ) translational_forces = reshape( - self.external_forces[9 * self.nb_forces_in_global + 9 * self.nb_forces_in_local :], (6, self.nb_translational_forces) + self.external_forces[9 * self.nb_forces_in_global + 9 * self.nb_forces_in_local :], + (6, self.nb_translational_forces), ) if forces_in_global.shape[1] > 0: diff --git a/bioptim/optimization/non_linear_program.py b/bioptim/optimization/non_linear_program.py index 6f2780a9f..399cc8e03 100644 --- a/bioptim/optimization/non_linear_program.py +++ b/bioptim/optimization/non_linear_program.py @@ -457,12 +457,9 @@ def get_external_forces( def retrieve_forces(name: str, external_forces: MX): if name in self.states: - external_forces = vertcat(external_forces, - DynamicsFunctions.get(self.states[name], states)) + external_forces = vertcat(external_forces, DynamicsFunctions.get(self.states[name], states)) if name in self.controls: - external_forces = vertcat( - external_forces, DynamicsFunctions.get(self.controls[name], controls) - ) + external_forces = vertcat(external_forces, DynamicsFunctions.get(self.controls[name], controls)) if name in self.algebraic_states: external_forces = vertcat( external_forces, DynamicsFunctions.get(self.algebraic_states[name], algebraic_states) From 113ad5470d34fdce27a05946ff230c94e9b32d81 Mon Sep 17 00:00:00 2001 From: eve-mac Date: Thu, 24 Oct 2024 11:57:27 -0400 Subject: [PATCH 052/108] removed DAE (RigidBodyDynamics) --- README.md | 12 - bioptim/__init__.py | 1 - bioptim/dynamics/configure_problem.py | 121 +-- bioptim/dynamics/dynamics_functions.py | 172 +--- bioptim/examples/__main__.py | 1 - .../example_implicit_dynamics.py | 246 ----- .../muscle_activations_tracker.py | 2 - .../muscle_excitations_tracker.py | 2 - .../torque_driven_ocp/example_soft_contact.py | 3 - .../maximize_predicted_height_CoM.py | 15 - .../ocp_mass_with_ligament.py | 11 - .../pendulum_with_passive_torque.py | 10 - bioptim/misc/enums.py | 9 - tests/shard1/test__global_plots.py | 47 +- tests/shard1/test_dynamics.py | 934 +++--------------- tests/shard1/test_dynamics_units.py | 7 +- tests/shard1/test_enums.py | 30 +- tests/shard1/test_prepare_all_examples.py | 3 - ...t_global_torque_driven_with_contact_ocp.py | 22 +- tests/shard3/test_ligaments.py | 68 +- tests/shard3/test_passive_torque.py | 210 ++-- tests/shard4/test_penalty.py | 2 - 22 files changed, 263 insertions(+), 1665 deletions(-) delete mode 100644 bioptim/examples/getting_started/example_implicit_dynamics.py diff --git a/README.md b/README.md index 19c3f3a7c..3436c650a 100644 --- a/README.md +++ b/README.md @@ -175,7 +175,6 @@ As a tour guide that uses this binder, you can watch the `bioptim` workshop that - [CostType](#enum-costtype) - [SolutionIntegrator](#enum-solutionintegrator) - [QuadratureRule](#enum-quadraturerule) - - [RigidBodyDynamics](#enum-rigidbodydynamics) - [SoftContactDynamics](#enum-softcontactdynamics) - [DefectType](#enum-defecttype) @@ -1746,14 +1745,6 @@ The type of integration used to integrate the cost function terms of Lagrange: - APPROXIMATE_TRAPEZOIDAL: The integral is approximated by a trapezoidal rule using the state at the beginning of the next interval. - TRAPEZOIDAL: The integral is approximated by a trapezoidal rule using the state at the end of the current interval. -### Enum: RigidBodyDynamics -The type of transcription of any dynamics (e.g., rigidbody_dynamics or soft_contact_dynamics): -- ODE: the dynamics is handled explicitly in the continuity constraint of the ordinary differential equation of the Direct Multiple Shooting approach. -- DAE_INVERSE_DYNAMICS: it adds an extra control *qddot* to respect inverse dynamics on nodes; this is a DAE-constrained OCP. -- DAE_FORWARD_DYNAMICS: it adds an extra control *qddot* to respect forward dynamics on nodes; this is a DAE-constrained OCP. -- DAE_INVERSE_DYNAMICS_JERK: it adds an extra control *qdddot* and an extra state *qddot* to respect inverse dynamics on nodes; this is a DAE-constrained OCP. -- DAE_FORWARD_DYNAMICS_JERK: it adds an extra control *qdddot* and an extra state *qddot* to respect forward dynamics on nodes; this is a DAE-constrained OCP. - ### Enum: SoftContactDynamics The type of transcription of any dynamics (e.g., rigidbody_dynamics or soft_contact_dynamics): - ODE: soft contact dynamics is handled explicitly. @@ -1910,9 +1901,6 @@ example, a spring) can be found at 'examples/torque_driven_ocp/spring_load.py' `Bioptim` expects `external_forces` to be a np.ndarray [6 x n x n_shooting], where the six components are [Mx, My, Mz, Fx, Fy, Fz], expressed at the origin of the global reference frame for each node. -### The [example_implicit_dynamics.py](./bioptim/examples/getting_started/example_implicit_dynamics.py) file -*#TODO* - ### The [example_inequality_constraint.py](./bioptim/examples/getting_started/example_inequality_constraint.py) file This example mimics what a jumper does when maximizing the predicted height of the center of mass at the peak of an aerial phase. It does so with a simplistic two segments model. It is a clone of 'torque_driven_ocp/maximize_predicted_height_CoM.py' using diff --git a/bioptim/__init__.py b/bioptim/__init__.py index 7360e0f1b..c7b35e1d1 100644 --- a/bioptim/__init__.py +++ b/bioptim/__init__.py @@ -206,7 +206,6 @@ VariableType, SolutionIntegrator, QuadratureRule, - RigidBodyDynamics, SoftContactDynamics, DefectType, MagnitudeType, diff --git a/bioptim/dynamics/configure_problem.py b/bioptim/dynamics/configure_problem.py index e30e34f89..ea71b00d7 100644 --- a/bioptim/dynamics/configure_problem.py +++ b/bioptim/dynamics/configure_problem.py @@ -12,7 +12,6 @@ PlotType, Node, ConstraintType, - RigidBodyDynamics, SoftContactDynamics, PhaseDynamics, ReferenceFrame, @@ -162,7 +161,6 @@ def torque_driven( with_passive_torque: bool = False, with_ligament: bool = False, with_friction: bool = False, - rigidbody_dynamics: RigidBodyDynamics = RigidBodyDynamics.ODE, soft_contacts_dynamics: SoftContactDynamics = SoftContactDynamics.ODE, fatigue: FatigueList = None, numerical_data_timeseries: dict[str, np.ndarray] = None, @@ -184,8 +182,6 @@ def torque_driven( If the dynamic with ligament should be used with_friction: bool If the dynamic with joint friction should be used (friction = coefficients * qdot) - rigidbody_dynamics: RigidBodyDynamics - which rigidbody dynamics should be used soft_contacts_dynamics: SoftContactDynamics which soft contact dynamic should be used fatigue: FatigueList @@ -196,75 +192,14 @@ def torque_driven( _check_contacts_in_biorbd_model(with_contact, nlp.model.nb_contacts, nlp.phase_idx) _check_soft_contacts_dynamics( - rigidbody_dynamics, soft_contacts_dynamics, nlp.model.nb_soft_contacts, nlp.phase_idx + soft_contacts_dynamics, nlp.model.nb_soft_contacts, nlp.phase_idx ) # Declared rigidbody states and controls ConfigureProblem.configure_q(ocp, nlp, as_states=True, as_controls=False) ConfigureProblem.configure_qdot(ocp, nlp, as_states=True, as_controls=False, as_states_dot=True) ConfigureProblem.configure_tau(ocp, nlp, as_states=False, as_controls=True, fatigue=fatigue) - - if ( - rigidbody_dynamics == RigidBodyDynamics.DAE_FORWARD_DYNAMICS - or rigidbody_dynamics == RigidBodyDynamics.DAE_INVERSE_DYNAMICS - ): - ConfigureProblem.configure_qddot(ocp, nlp, False, True, True) - elif ( - rigidbody_dynamics == RigidBodyDynamics.DAE_FORWARD_DYNAMICS_JERK - or rigidbody_dynamics == RigidBodyDynamics.DAE_INVERSE_DYNAMICS_JERK - ): - ConfigureProblem.configure_qddot(ocp, nlp, True, False, True) - ConfigureProblem.configure_qdddot(ocp, nlp, False, True) - else: - ConfigureProblem.configure_qddot(ocp, nlp, False, False, True) - - # Algebraic constraints of rigidbody dynamics if needed - if ( - rigidbody_dynamics == RigidBodyDynamics.DAE_INVERSE_DYNAMICS - or rigidbody_dynamics == RigidBodyDynamics.DAE_INVERSE_DYNAMICS_JERK - ): - ocp.implicit_constraints.add( - ImplicitConstraintFcn.TAU_EQUALS_INVERSE_DYNAMICS, - node=Node.ALL_SHOOTING, - penalty_type=ConstraintType.IMPLICIT, - phase=nlp.phase_idx, - with_contact=with_contact, - with_passive_torque=with_passive_torque, - with_ligament=with_ligament, - with_friction=with_friction, - ) - if with_contact: - # qddot is continuous with RigidBodyDynamics.DAE_INVERSE_DYNAMICS_JERK - # so the consistency constraint of the marker acceleration can only be set to zero - # at the first shooting node - node = Node.ALL_SHOOTING if rigidbody_dynamics == RigidBodyDynamics.DAE_INVERSE_DYNAMICS else Node.ALL - ConfigureProblem.configure_contact_forces(ocp, nlp, False, True) - for ii in range(nlp.model.nb_rigid_contacts): - for jj in nlp.model.rigid_contact_index(ii): - ocp.implicit_constraints.add( - ImplicitConstraintFcn.CONTACT_ACCELERATION_EQUALS_ZERO, - with_contact=with_contact, - contact_index=ii, - contact_axis=jj, - node=node, - constraint_type=ConstraintType.IMPLICIT, - phase=nlp.phase_idx, - ) - if ( - rigidbody_dynamics == RigidBodyDynamics.DAE_FORWARD_DYNAMICS - or rigidbody_dynamics == RigidBodyDynamics.DAE_FORWARD_DYNAMICS_JERK - ): - # contacts forces are directly handled with this constraint - ocp.implicit_constraints.add( - ImplicitConstraintFcn.QDDOT_EQUALS_FORWARD_DYNAMICS, - node=Node.ALL_SHOOTING, - constraint_type=ConstraintType.IMPLICIT, - with_contact=with_contact, - phase=nlp.phase_idx, - with_passive_torque=with_passive_torque, - with_ligament=with_ligament, - with_friction=with_friction, - ) + ConfigureProblem.configure_qddot(ocp, nlp, False, False, True) # Declared soft contacts controls if soft_contacts_dynamics == SoftContactDynamics.CONSTRAINT: @@ -280,7 +215,6 @@ def torque_driven( DynamicsFunctions.torque_driven, with_contact=with_contact, fatigue=fatigue, - rigidbody_dynamics=rigidbody_dynamics, with_passive_torque=with_passive_torque, with_ligament=with_ligament, with_friction=with_friction, @@ -591,7 +525,6 @@ def torque_derivative_driven( with_contact=False, with_passive_torque: bool = False, with_ligament: bool = False, - rigidbody_dynamics: RigidBodyDynamics = RigidBodyDynamics.ODE, soft_contacts_dynamics: SoftContactDynamics = SoftContactDynamics.ODE, numerical_data_timeseries: dict[str, np.ndarray] = None, ): @@ -610,8 +543,6 @@ def torque_derivative_driven( If the dynamic with passive torque should be used with_ligament: bool If the dynamic with ligament should be used - rigidbody_dynamics: RigidBodyDynamics - which rigidbody dynamics should be used soft_contacts_dynamics: SoftContactDynamics which soft contact dynamic should be used numerical_data_timeseries: dict[str, np.ndarray] @@ -620,11 +551,8 @@ def torque_derivative_driven( """ _check_contacts_in_biorbd_model(with_contact, nlp.model.nb_contacts, nlp.phase_idx) - if rigidbody_dynamics not in (RigidBodyDynamics.DAE_INVERSE_DYNAMICS, RigidBodyDynamics.ODE): - raise NotImplementedError("TORQUE_DERIVATIVE_DRIVEN cannot be used with this enum RigidBodyDynamics yet") - _check_soft_contacts_dynamics( - rigidbody_dynamics, soft_contacts_dynamics, nlp.model.nb_soft_contacts, nlp.phase_idx + soft_contacts_dynamics, nlp.model.nb_soft_contacts, nlp.phase_idx ) ConfigureProblem.configure_q(ocp, nlp, True, False) @@ -632,15 +560,6 @@ def torque_derivative_driven( ConfigureProblem.configure_tau(ocp, nlp, True, False) ConfigureProblem.configure_taudot(ocp, nlp, False, True) - if rigidbody_dynamics == RigidBodyDynamics.DAE_INVERSE_DYNAMICS: - ConfigureProblem.configure_qddot(ocp, nlp, True, False) - ConfigureProblem.configure_qdddot(ocp, nlp, False, True) - ocp.implicit_constraints.add( - ImplicitConstraintFcn.TAU_EQUALS_INVERSE_DYNAMICS, - node=Node.ALL_SHOOTING, - penalty_type=ConstraintType.IMPLICIT, - phase=nlp.phase_idx, - ) if soft_contacts_dynamics == SoftContactDynamics.CONSTRAINT: ConfigureProblem.configure_soft_contact_forces(ocp, nlp, False, True) @@ -652,7 +571,6 @@ def torque_derivative_driven( nlp, DynamicsFunctions.torque_derivative_driven, with_contact=with_contact, - rigidbody_dynamics=rigidbody_dynamics, with_passive_torque=with_passive_torque, with_ligament=with_ligament, ) @@ -738,7 +656,6 @@ def torque_activations_driven( def joints_acceleration_driven( ocp, nlp, - rigidbody_dynamics: RigidBodyDynamics = RigidBodyDynamics.ODE, numerical_data_timeseries: dict[str, np.ndarray] = None, ): """ @@ -751,14 +668,9 @@ def joints_acceleration_driven( A reference to the ocp nlp: NonLinearProgram A reference to the phase - rigidbody_dynamics: RigidBodyDynamics - which rigidbody dynamics should be used numerical_data_timeseries: dict[str, np.ndarray] A list of values to pass to the dynamics at each node. Experimental external forces should be included here. """ - if rigidbody_dynamics != RigidBodyDynamics.ODE: - raise NotImplementedError("Implicit dynamics not implemented yet.") - ConfigureProblem.configure_q(ocp, nlp, as_states=True, as_controls=False) ConfigureProblem.configure_qdot(ocp, nlp, as_states=True, as_controls=False, as_states_dot=True) # Configure qddot joints @@ -800,7 +712,6 @@ def muscle_driven( with_contact: bool = False, with_passive_torque: bool = False, with_ligament: bool = False, - rigidbody_dynamics: RigidBodyDynamics = RigidBodyDynamics.ODE, numerical_data_timeseries: dict[str, np.ndarray] = None, ): """ @@ -828,8 +739,6 @@ def muscle_driven( If the dynamic with passive torque should be used with_ligament: bool If the dynamic with ligament should be used - rigidbody_dynamics: RigidBodyDynamics - which rigidbody dynamics should be used numerical_data_timeseries: dict[str, np.ndarray] A list of values to pass to the dynamics at each node. Experimental external forces should be included here. """ @@ -838,9 +747,6 @@ def muscle_driven( if fatigue is not None and "tau" in fatigue and not with_residual_torque: raise RuntimeError("Residual torques need to be used to apply fatigue on torques") - if rigidbody_dynamics not in (RigidBodyDynamics.DAE_INVERSE_DYNAMICS, RigidBodyDynamics.ODE): - raise NotImplementedError("MUSCLE_DRIVEN cannot be used with this enum RigidBodyDynamics yet") - ConfigureProblem.configure_q(ocp, nlp, True, False) ConfigureProblem.configure_qdot(ocp, nlp, True, False, True) ConfigureProblem.configure_qddot(ocp, nlp, False, False, True) @@ -849,17 +755,6 @@ def muscle_driven( ConfigureProblem.configure_tau(ocp, nlp, False, True, fatigue=fatigue) ConfigureProblem.configure_muscles(ocp, nlp, with_excitations, True, fatigue=fatigue) - if rigidbody_dynamics == RigidBodyDynamics.DAE_INVERSE_DYNAMICS: - ConfigureProblem.configure_qddot(ocp, nlp, False, True) - ocp.implicit_constraints.add( - ImplicitConstraintFcn.TAU_FROM_MUSCLE_EQUAL_INVERSE_DYNAMICS, - node=Node.ALL_SHOOTING, - penalty_type=ConstraintType.IMPLICIT, - phase=nlp.phase_idx, - with_passive_torque=with_passive_torque, - with_ligament=with_ligament, - ) - if nlp.dynamics_type.dynamic_function: ConfigureProblem.configure_dynamics_function(ocp, nlp, DynamicsFunctions.custom) else: @@ -872,7 +767,6 @@ def muscle_driven( with_residual_torque=with_residual_torque, with_passive_torque=with_passive_torque, with_ligament=with_ligament, - rigidbody_dynamics=rigidbody_dynamics, ) if with_contact: @@ -2203,7 +2097,6 @@ def _check_numerical_timeseries_format(numerical_timeseries: np.ndarray, n_shoot def _check_soft_contacts_dynamics( - rigidbody_dynamics: RigidBodyDynamics, soft_contacts_dynamics: SoftContactDynamics, nb_soft_contacts, phase_idx: int, @@ -2218,14 +2111,6 @@ def _check_soft_contacts_dynamics( f"SoftContactDynamics.CONSTRAINT or SoftContactDynamics.ODE." ) - if rigidbody_dynamics == RigidBodyDynamics.DAE_INVERSE_DYNAMICS: - if soft_contacts_dynamics == SoftContactDynamics.ODE: - raise ValueError( - f"Phase {phase_idx} has soft contacts but the rigidbody_dynamics is " - f"RigidBodyDynamics.DAE_INVERSE_DYNAMICS and soft_contacts_dynamics is SoftContactDynamics.ODE." - f"Please set soft_contacts_dynamics=SoftContactDynamics.CONSTRAINT" - ) - def _check_contacts_in_biorbd_model(with_contact: bool, nb_contacts: int, phase_idx: int): if with_contact and nb_contacts == 0: diff --git a/bioptim/dynamics/dynamics_functions.py b/bioptim/dynamics/dynamics_functions.py index 080048fdc..a48746709 100644 --- a/bioptim/dynamics/dynamics_functions.py +++ b/bioptim/dynamics/dynamics_functions.py @@ -3,7 +3,7 @@ from .dynamics_evaluation import DynamicsEvaluation from .fatigue.fatigue_dynamics import FatigueList -from ..misc.enums import RigidBodyDynamics, DefectType +from ..misc.enums import DefectType from ..misc.mapping import BiMapping from ..optimization.optimization_variable import OptimizationVariable @@ -95,7 +95,6 @@ def torque_driven( with_passive_torque: bool, with_ligament: bool, with_friction: bool, - rigidbody_dynamics: RigidBodyDynamics, fatigue: FatigueList, ) -> DynamicsEvaluation: """ @@ -125,8 +124,6 @@ def torque_driven( If the dynamic with ligament should be used with_friction: bool If the dynamic with friction should be used - rigidbody_dynamics: RigidBodyDynamics - which rigidbody dynamics should be used fatigue : FatigueList A list of fatigue elements @@ -146,59 +143,17 @@ def torque_driven( 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 ( - rigidbody_dynamics == RigidBodyDynamics.DAE_INVERSE_DYNAMICS - or rigidbody_dynamics == RigidBodyDynamics.DAE_FORWARD_DYNAMICS - ): - dxdt = nlp.cx(nlp.states.shape, 1) - dxdt[nlp.states["q"].index, :] = dq - dxdt[nlp.states["qdot"].index, :] = DynamicsFunctions.get(nlp.controls["qddot"], controls) - elif ( - rigidbody_dynamics == RigidBodyDynamics.DAE_INVERSE_DYNAMICS_JERK - or rigidbody_dynamics == RigidBodyDynamics.DAE_FORWARD_DYNAMICS_JERK - ): - dxdt = nlp.cx(nlp.states.shape, 1) - dxdt[nlp.states["q"].index, :] = dq - qddot = DynamicsFunctions.get(nlp.states["qddot"], states) - dxdt[nlp.states["qdot"].index, :] = qddot - dxdt[nlp.states["qddot"].index, :] = DynamicsFunctions.get(nlp.controls["qdddot"], controls) - else: - external_forces = nlp.get_external_forces(states, controls, algebraic_states, numerical_timeseries) + external_forces = nlp.get_external_forces(states, controls, algebraic_states, numerical_timeseries) - ddq = DynamicsFunctions.forward_dynamics(nlp, q, qdot, tau, with_contact, external_forces) - dxdt = nlp.cx(nlp.states.shape, ddq.shape[1]) - dxdt[nlp.states["q"].index, :] = horzcat(*[dq for _ in range(ddq.shape[1])]) - dxdt[nlp.states["qdot"].index, :] = ddq + ddq = DynamicsFunctions.forward_dynamics(nlp, q, qdot, tau, with_contact, external_forces) + dxdt = nlp.cx(nlp.states.shape, ddq.shape[1]) + dxdt[nlp.states["q"].index, :] = horzcat(*[dq for _ in range(ddq.shape[1])]) + dxdt[nlp.states["qdot"].index, :] = ddq if fatigue is not None and "tau" in fatigue: dxdt = fatigue["tau"].dynamics(dxdt, nlp, states, controls) - defects = None - # TODO: contacts and fatigue to be handled with implicit dynamics - if rigidbody_dynamics is not RigidBodyDynamics.ODE or ( - rigidbody_dynamics is RigidBodyDynamics.ODE and nlp.ode_solver.defects_type == DefectType.IMPLICIT - ): - if not with_contact and fatigue is None: - qddot = DynamicsFunctions.get(nlp.states_dot["qddot"], nlp.states_dot.scaled.cx) - external_forces = nlp.get_external_forces(states, controls, algebraic_states, numerical_timeseries) - tau_id = DynamicsFunctions.inverse_dynamics(nlp, q, qdot, qddot, with_contact, external_forces) - defects = nlp.cx(dq.shape[0] + tau_id.shape[0], tau_id.shape[1]) - - dq_defects = [] - for _ in range(tau_id.shape[1]): - dq_defects.append( - dq - - DynamicsFunctions.compute_qdot( - nlp, - q, - DynamicsFunctions.get(nlp.states_dot.scaled["qdot"], nlp.states_dot.scaled.cx), - ) - ) - defects[: dq.shape[0], :] = horzcat(*dq_defects) - # We modified on purpose the size of the tau to keep the zero in the defects in order to respect the dynamics - defects[dq.shape[0] :, :] = tau - tau_id - - return DynamicsEvaluation(dxdt, defects) + return DynamicsEvaluation(dxdt, None) @staticmethod def torque_driven_free_floating_base( @@ -555,7 +510,6 @@ def torque_derivative_driven( algebraic_states, numerical_timeseries, nlp, - rigidbody_dynamics: RigidBodyDynamics, with_contact: bool, with_passive_torque: bool, with_ligament: bool, @@ -579,8 +533,6 @@ def torque_derivative_driven( The numerical timeseries of the system nlp: NonLinearProgram The definition of the system - rigidbody_dynamics: RigidBodyDynamics - which rigidbody dynamics should be used with_contact: bool If the dynamic with contact should be used with_passive_torque: bool @@ -604,25 +556,12 @@ def torque_derivative_driven( dq = DynamicsFunctions.compute_qdot(nlp, q, qdot) dtau = DynamicsFunctions.get(nlp.controls["taudot"], controls) - if ( - rigidbody_dynamics == RigidBodyDynamics.DAE_INVERSE_DYNAMICS - or rigidbody_dynamics == RigidBodyDynamics.DAE_FORWARD_DYNAMICS - ): - ddq = DynamicsFunctions.get(nlp.states["qddot"], states) - dddq = DynamicsFunctions.get(nlp.controls["qdddot"], controls) - - dxdt = nlp.cx(nlp.states.shape, 1) - dxdt[nlp.states["q"].index, :] = dq - dxdt[nlp.states["qdot"].index, :] = ddq - dxdt[nlp.states["qddot"].index, :] = dddq - dxdt[nlp.states["tau"].index, :] = dtau - else: - external_forces = nlp.get_external_forces(states, controls, algebraic_states, numerical_timeseries) - ddq = DynamicsFunctions.forward_dynamics(nlp, q, qdot, tau, with_contact, external_forces) - dxdt = nlp.cx(nlp.states.shape, ddq.shape[1]) - dxdt[nlp.states["q"].index, :] = horzcat(*[dq for _ in range(ddq.shape[1])]) - dxdt[nlp.states["qdot"].index, :] = ddq - dxdt[nlp.states["tau"].index, :] = horzcat(*[dtau for _ in range(ddq.shape[1])]) + external_forces = nlp.get_external_forces(states, controls, algebraic_states, numerical_timeseries) + ddq = DynamicsFunctions.forward_dynamics(nlp, q, qdot, tau, with_contact, external_forces) + dxdt = nlp.cx(nlp.states.shape, ddq.shape[1]) + dxdt[nlp.states["q"].index, :] = horzcat(*[dq for _ in range(ddq.shape[1])]) + dxdt[nlp.states["qdot"].index, :] = ddq + dxdt[nlp.states["tau"].index, :] = horzcat(*[dtau for _ in range(ddq.shape[1])]) return DynamicsEvaluation(dxdt=dxdt, defects=None) @@ -741,7 +680,6 @@ def muscles_driven( with_contact: bool, with_passive_torque: bool = False, with_ligament: bool = False, - rigidbody_dynamics: RigidBodyDynamics = RigidBodyDynamics.ODE, with_residual_torque: bool = False, fatigue=None, ) -> DynamicsEvaluation: @@ -770,8 +708,6 @@ def muscles_driven( If the dynamic with passive torque should be used with_ligament: bool If the dynamic with ligament should be used - rigidbody_dynamics: RigidBodyDynamics - which rigidbody dynamics should be used fatigue: FatigueDynamicsList To define fatigue elements with_residual_torque: bool @@ -829,17 +765,11 @@ def muscles_driven( dq = DynamicsFunctions.compute_qdot(nlp, q, qdot) - if rigidbody_dynamics == RigidBodyDynamics.DAE_INVERSE_DYNAMICS: - ddq = DynamicsFunctions.get(nlp.controls["qddot"], controls) - dxdt = nlp.cx(nlp.states.shape, 1) - dxdt[nlp.states["q"].index, :] = dq - dxdt[nlp.states["qdot"].index, :] = DynamicsFunctions.get(nlp.controls["qddot"], controls) - else: - external_forces = nlp.get_external_forces(states, controls, algebraic_states, numerical_timeseries) - ddq = DynamicsFunctions.forward_dynamics(nlp, q, qdot, tau, with_contact, external_forces) - dxdt = nlp.cx(nlp.states.shape, ddq.shape[1]) - dxdt[nlp.states["q"].index, :] = horzcat(*[dq for _ in range(ddq.shape[1])]) - dxdt[nlp.states["qdot"].index, :] = ddq + external_forces = nlp.get_external_forces(states, controls, algebraic_states, numerical_timeseries) + ddq = DynamicsFunctions.forward_dynamics(nlp, q, qdot, tau, with_contact, external_forces) + dxdt = nlp.cx(nlp.states.shape, ddq.shape[1]) + dxdt[nlp.states["q"].index, :] = horzcat(*[dq for _ in range(ddq.shape[1])]) + dxdt[nlp.states["qdot"].index, :] = ddq has_excitation = True if "muscles" in nlp.states else False if has_excitation: @@ -850,31 +780,7 @@ def muscles_driven( if fatigue is not None and "muscles" in fatigue: dxdt = fatigue["muscles"].dynamics(dxdt, nlp, states, controls) - defects = None - # TODO: contacts and fatigue to be handled with implicit dynamics - if rigidbody_dynamics is not RigidBodyDynamics.ODE or ( - rigidbody_dynamics is RigidBodyDynamics.ODE and nlp.ode_solver.defects_type == DefectType.IMPLICIT - ): - if not with_contact and fatigue is None: - qddot = DynamicsFunctions.get(nlp.states_dot["qddot"], nlp.states_dot.cx) - external_forces = nlp.get_external_forces(states, controls, algebraic_states, numerical_timeseries) - tau_id = DynamicsFunctions.inverse_dynamics(nlp, q, qdot, qddot, with_contact, external_forces) - defects = nlp.cx(dq.shape[0] + tau_id.shape[0], tau_id.shape[1]) - - dq_defects = [] - for _ in range(tau_id.shape[1]): - dq_defects.append( - dq - - DynamicsFunctions.compute_qdot( - nlp, - q, - DynamicsFunctions.get(nlp.states_dot["qdot"], nlp.states_dot.cx), - ) - ) - defects[: dq.shape[0], :] = horzcat(*dq_defects) - defects[dq.shape[0] :, :] = tau - tau_id - - return DynamicsEvaluation(dxdt=dxdt, defects=defects) + return DynamicsEvaluation(dxdt=dxdt, defects=None) @staticmethod def forces_from_muscle_driven( @@ -940,7 +846,6 @@ def joints_acceleration_driven( algebraic_states, numerical_timeseries, nlp, - rigidbody_dynamics: RigidBodyDynamics = RigidBodyDynamics.ODE, ) -> DynamicsEvaluation: """ Forward dynamics driven by joints accelerations of a free floating body. @@ -961,17 +866,12 @@ def joints_acceleration_driven( The numerical timeseries of the system nlp: NonLinearProgram The definition of the system - rigidbody_dynamics: RigidBodyDynamics - which rigid body dynamics to use Returns ---------- MX.sym | SX.sym The derivative of states """ - if rigidbody_dynamics != RigidBodyDynamics.ODE: - raise NotImplementedError("Implicit dynamics not implemented yet.") - q = nlp.get_var_from_states_or_controls("q", states, controls) qdot = nlp.get_var_from_states_or_controls("qdot", states, controls) qddot_joints = nlp.get_var_from_states_or_controls("qddot", states, controls) @@ -981,40 +881,8 @@ def joints_acceleration_driven( qdot_mapped = nlp.variable_mappings["qdot"].to_first.map(qdot) qddot_mapped = nlp.variable_mappings["qdot"].to_first.map(qddot_reordered) - qddot_root_mapped = nlp.variable_mappings["qddot_roots"].to_first.map(qddot_root) - qddot_joints_mapped = nlp.variable_mappings["qddot_joints"].to_first.map(qddot_joints) - - # defects - defects = None - if rigidbody_dynamics is not RigidBodyDynamics.ODE or ( - rigidbody_dynamics is RigidBodyDynamics.ODE and nlp.ode_solver.defects_type == DefectType.IMPLICIT - ): - qddot_root_defects = DynamicsFunctions.get(nlp.states_dot["qddot_roots"], nlp.states_dot.cx) - qddot_defects_reordered = nlp.model.reorder_qddot_root_joints(qddot_root_defects, qddot_joints) - - floating_base_constraint = nlp.model.inverse_dynamics(q, qdot, qddot_defects_reordered, [], [])[ - : nlp.model.nb_root - ] - - defects = nlp.cx(qdot_mapped.shape[0] + qddot_root_mapped.shape[0] + qddot_joints_mapped.shape[0], 1) - - defects[: qdot_mapped.shape[0], :] = qdot_mapped - nlp.variable_mappings["qdot"].to_first.map( - DynamicsFunctions.compute_qdot( - nlp, q, DynamicsFunctions.get((nlp.states_dot["qdot"]), nlp.states_dot.cx) - ) - ) - - defects[qdot_mapped.shape[0] : (qdot_mapped.shape[0] + qddot_root_mapped.shape[0]), :] = ( - floating_base_constraint - ) - defects[(qdot_mapped.shape[0] + qddot_root_mapped.shape[0]) :, :] = ( - qddot_joints_mapped - - nlp.variable_mappings["qddot_joints"].to_first.map( - DynamicsFunctions.get(nlp.states_dot["qddot_joints"], nlp.states_dot.cx) - ) - ) - return DynamicsEvaluation(dxdt=vertcat(qdot_mapped, qddot_mapped), defects=defects) + return DynamicsEvaluation(dxdt=vertcat(qdot_mapped, qddot_mapped), defects=None) @staticmethod def get(var: OptimizationVariable, cx: MX | SX): diff --git a/bioptim/examples/__main__.py b/bioptim/examples/__main__.py index be7c0602a..42bb246aa 100644 --- a/bioptim/examples/__main__.py +++ b/bioptim/examples/__main__.py @@ -51,7 +51,6 @@ ("Example multiphase", "example_multiphase.py"), ("Example optimal time", "example_optimal_time.py"), ("Example simulation", "example_simulation.py"), - ("Example Implicit Dynamics", "example_implicit_dynamics.py"), ("Pendulum", "pendulum.py"), ] ), diff --git a/bioptim/examples/getting_started/example_implicit_dynamics.py b/bioptim/examples/getting_started/example_implicit_dynamics.py deleted file mode 100644 index ee9033dac..000000000 --- a/bioptim/examples/getting_started/example_implicit_dynamics.py +++ /dev/null @@ -1,246 +0,0 @@ -""" -A very simple yet meaningful optimal control program consisting in a pendulum starting downward and ending upward -while requiring the minimum of generalized forces. The solver is only allowed to move the pendulum sideways. - -This simple example is a good place to start investigating explicit and implicit dynamics. There are extra controls in -implicit dynamics which are joint acceleration qddot thus, u=[tau, qddot]^T. Also a dynamic constraints is enforced at -each shooting nodes such that inverse_dynamics(q,qdot,qddot) - tau = 0. - -Finally, once it finished optimizing, it animates the model using the optimal solution. -""" - -from bioptim import ( - BiorbdModel, - OptimalControlProgram, - DynamicsFcn, - Dynamics, - ObjectiveFcn, - OdeSolver, - OdeSolverBase, - CostType, - Solver, - BoundsList, - ObjectiveList, - RigidBodyDynamics, - Solution, - PhaseDynamics, - SolutionMerge, -) -import matplotlib.pyplot as plt -import numpy as np - - -def prepare_ocp( - biorbd_model_path: str, - final_time: float, - n_shooting: int, - ode_solver: OdeSolverBase = OdeSolver.RK1(n_integration_steps=1), - use_sx: bool = False, - n_threads: int = 1, - rigidbody_dynamics: RigidBodyDynamics = RigidBodyDynamics.ODE, - phase_dynamics: PhaseDynamics = PhaseDynamics.SHARED_DURING_THE_PHASE, - expand_dynamics: bool = True, -) -> OptimalControlProgram: - """ - The initialization of an ocp - - Parameters - ---------- - biorbd_model_path: str - The path to the biorbd model - final_time: float - The time in second required to perform the task - n_shooting: int - The number of shooting points to define int the direct multiple shooting program - ode_solver: OdeSolverBase = OdeSolver.RK4() - Which type of OdeSolver to use - use_sx: bool - If the SX variable should be used instead of MX (can be extensive on RAM) - n_threads: int - The number of threads to use in the paralleling (1 = no parallel computing) - rigidbody_dynamics: RigidBodyDynamics - rigidbody dynamics ODE or DAE - phase_dynamics: PhaseDynamics - If the dynamics equation within a phase is unique or changes at each node. - PhaseDynamics.SHARED_DURING_THE_PHASE is much faster, but lacks the capability to have changing dynamics within - a phase. A good example of when PhaseDynamics.ONE_PER_NODE should be used is when different external forces - are applied at each node - expand_dynamics: bool - If the dynamics function should be expanded. Please note, this will solve the problem faster, but will slow down - the declaration of the OCP, so it is a trade-off. Also depending on the solver, it may or may not work - (for instance IRK is not compatible with expanded dynamics) - - Returns - ------- - The OptimalControlProgram ready to be solved - """ - - bio_model = BiorbdModel(biorbd_model_path) - - objective_functions = ObjectiveList() - objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau") - - # Dynamics - dynamics = Dynamics( - DynamicsFcn.TORQUE_DRIVEN, - rigidbody_dynamics=rigidbody_dynamics, - expand_dynamics=expand_dynamics, - phase_dynamics=phase_dynamics, - ) - - # Path constraint - tau_min, tau_max, tau_init = -100.0, 100.0, 0.0 - - # Be careful to let the accelerations not to much bounded to find the same solution in implicit dynamics - qddot_min, qddot_max, qddot_init = ( - (-1000.0, 1000.0, 0.0) - if ( - rigidbody_dynamics == RigidBodyDynamics.DAE_INVERSE_DYNAMICS - or rigidbody_dynamics == RigidBodyDynamics.DAE_FORWARD_DYNAMICS - ) - else (0.0, 0.0, 0.0) - ) - - 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") - - # Initial guess - n_qddot = bio_model.nb_qddot - n_tau = bio_model.nb_tau - - # Define control path constraint - # There are extra controls in implicit dynamics which are joint acceleration qddot. - u_bounds = BoundsList() - u_bounds["tau"] = [tau_min] * n_tau, [tau_max] * n_tau - if ( - rigidbody_dynamics == RigidBodyDynamics.DAE_INVERSE_DYNAMICS - or rigidbody_dynamics == RigidBodyDynamics.DAE_FORWARD_DYNAMICS - ): - u_bounds["qddot"] = [qddot_min] * n_tau, [qddot_max] * n_tau - - u_bounds["tau"][1, :] = 0 # Prevent the model from actively rotate - - return OptimalControlProgram( - bio_model, - dynamics, - n_shooting, - final_time, - x_bounds=x_bounds, - u_bounds=u_bounds, - objective_functions=objective_functions, - ode_solver=ode_solver, - use_sx=use_sx, - n_threads=n_threads, - ) - - -def solve_ocp( - rigidbody_dynamics: RigidBodyDynamics, max_iter: int = 10000, model_path: str = "models/pendulum.bioMod" -) -> Solution: - """ - The initialization of ocp with implicit_dynamics as the only argument - - Parameters - ---------- - rigidbody_dynamics: RigidBodyDynamics - rigidbody dynamics DAE or ODE - max_iter: int - maximum iterations of the solver - model_path: str - The path to the biorbd model - Returns - ------- - The OptimalControlProgram ready to be solved - """ - n_shooting = 200 # The higher it is, the closer implicit and explicit solutions are. - ode_solver = OdeSolver.RK2(n_integration_steps=1) - time = 1 - - # --- Prepare the ocp with implicit dynamics --- # - ocp = prepare_ocp( - biorbd_model_path=model_path, - final_time=time, - n_shooting=n_shooting, - ode_solver=ode_solver, - rigidbody_dynamics=rigidbody_dynamics, - ) - - # --- Custom Plots --- # - ocp.add_plot_penalty(CostType.ALL) - - # --- Solve the ocp --- # - sol_opt = Solver.IPOPT(show_online_optim=False) - sol_opt.set_maximum_iterations(max_iter) - sol = ocp.solve(sol_opt) - - return sol - - -def prepare_plots(sol_implicit, sol_semi_explicit, sol_explicit): - plt.figure() - tau_ex = sol_explicit.decision_controls(to_merge=SolutionMerge.NODES)["tau"][0, :] - tau_sex = sol_semi_explicit.decision_controls(to_merge=SolutionMerge.NODES)["tau"][0, :] - tau_im = sol_implicit.decision_controls(to_merge=SolutionMerge.NODES)["tau"][0, :] - plt.plot(tau_ex, label="tau in explicit dynamics") - plt.plot(tau_sex, label="tau in semi-explicit dynamics") - plt.plot(tau_im, label="tau in implicit dynamics") - plt.xlabel("frames") - plt.ylabel("Torque (Nm)") - plt.legend() - - lalbels = ["explicit", "semi-explicit", "implicit"] - - plt.figure() - cost_ex = np.sum(sol_explicit.cost) - cost_sex = np.sum(sol_semi_explicit.cost) - cost_im = np.sum(sol_implicit.cost) - plt.bar([0, 1, 2], width=0.3, height=[cost_ex, cost_sex, cost_im]) - plt.xticks([0, 1, 2], lalbels) - plt.ylabel("weighted cost function") - - plt.figure() - time_ex = np.sum(sol_explicit.real_time_to_optimize) - time_sex = np.sum(sol_semi_explicit.real_time_to_optimize) - time_im = np.sum(sol_implicit.real_time_to_optimize) - plt.bar([0, 1, 2], width=0.3, height=[time_ex, time_sex, time_im]) - plt.xticks([0, 1, 2], lalbels) - plt.ylabel("time (s)") - - plt.show() - - -def main(): - """ - The pendulum runs two ocp with implicit and explicit dynamics and plot comparison for the results - """ - - # --- Prepare the ocp with implicit and explicit dynamics --- # - sol_implicit = solve_ocp(rigidbody_dynamics=RigidBodyDynamics.DAE_INVERSE_DYNAMICS) - sol_semi_explicit = solve_ocp(rigidbody_dynamics=RigidBodyDynamics.DAE_FORWARD_DYNAMICS) - sol_explicit = solve_ocp(rigidbody_dynamics=RigidBodyDynamics.ODE) - - # --- Show the results in a bioviz animation --- # - sol_implicit.print_cost() - # sol_implicit.animate(n_frames=100) - # sol_implicit.graphs() - - # --- Show the results in a bioviz animation --- # - sol_semi_explicit.print_cost() - # sol_semi_explicit.animate(n_frames=100) - # sol_semi_explicit.graphs() - - # --- Show the results in a bioviz animation --- # - sol_explicit.print_cost() - # sol_explicit.animate(n_frames=100) - # sol_explicit.graphs() - - # Tau are closer between implicit and explicit when the dynamic is more discretized, - # meaning the more n_shooting is high, the more tau are close. - prepare_plots(sol_implicit, sol_semi_explicit, sol_explicit) - - -if __name__ == "__main__": - main() diff --git a/bioptim/examples/muscle_driven_ocp/muscle_activations_tracker.py b/bioptim/examples/muscle_driven_ocp/muscle_activations_tracker.py index 376a0e4c0..9d3d2a03a 100644 --- a/bioptim/examples/muscle_driven_ocp/muscle_activations_tracker.py +++ b/bioptim/examples/muscle_driven_ocp/muscle_activations_tracker.py @@ -32,7 +32,6 @@ OdeSolverBase, Node, Solver, - RigidBodyDynamics, PhaseDynamics, SolutionMerge, ) @@ -174,7 +173,6 @@ def generate_data( numerical_timeseries=MX(), nlp=nlp, with_contact=False, - rigidbody_dynamics=RigidBodyDynamics.ODE, ).dxdt ], ) diff --git a/bioptim/examples/muscle_driven_ocp/muscle_excitations_tracker.py b/bioptim/examples/muscle_driven_ocp/muscle_excitations_tracker.py index 77d7c17f8..cdeb7f5a9 100644 --- a/bioptim/examples/muscle_driven_ocp/muscle_excitations_tracker.py +++ b/bioptim/examples/muscle_driven_ocp/muscle_excitations_tracker.py @@ -30,7 +30,6 @@ OdeSolverBase, Node, Solver, - RigidBodyDynamics, PhaseDynamics, SolutionMerge, ) @@ -175,7 +174,6 @@ def generate_data( numerical_timeseries=MX(), nlp=nlp, with_contact=False, - rigidbody_dynamics=RigidBodyDynamics.ODE, ).dxdt ], ) diff --git a/bioptim/examples/torque_driven_ocp/example_soft_contact.py b/bioptim/examples/torque_driven_ocp/example_soft_contact.py index 4e85ce26a..42f2c76f0 100644 --- a/bioptim/examples/torque_driven_ocp/example_soft_contact.py +++ b/bioptim/examples/torque_driven_ocp/example_soft_contact.py @@ -24,7 +24,6 @@ Shooting, Solution, SoftContactDynamics, - RigidBodyDynamics, SolutionIntegrator, PhaseDynamics, SolutionMerge, @@ -52,7 +51,6 @@ def prepare_single_shooting( # Dynamics dynamics = Dynamics( DynamicsFcn.TORQUE_DRIVEN, - rigidbody_dynamics=RigidBodyDynamics.ODE, soft_contacts_dynamics=SoftContactDynamics.ODE, ) @@ -151,7 +149,6 @@ def prepare_ocp( # Dynamics dynamics = Dynamics( DynamicsFcn.TORQUE_DRIVEN, - rigidbody_dynamics=RigidBodyDynamics.ODE, soft_contacts_dynamics=SoftContactDynamics.ODE, phase_dynamics=phase_dynamics, ) diff --git a/bioptim/examples/torque_driven_ocp/maximize_predicted_height_CoM.py b/bioptim/examples/torque_driven_ocp/maximize_predicted_height_CoM.py index 123b378dd..bb739c297 100644 --- a/bioptim/examples/torque_driven_ocp/maximize_predicted_height_CoM.py +++ b/bioptim/examples/torque_driven_ocp/maximize_predicted_height_CoM.py @@ -25,7 +25,6 @@ ConstraintFcn, Node, Solver, - RigidBodyDynamics, PhaseDynamics, ) @@ -38,7 +37,6 @@ def prepare_ocp( ode_solver: OdeSolverBase = OdeSolver.RK4(), objective_name: str = "MINIMIZE_PREDICTED_COM_HEIGHT", com_constraints: bool = False, - rigidbody_dynamics: RigidBodyDynamics = RigidBodyDynamics.ODE, phase_dynamics: PhaseDynamics = PhaseDynamics.SHARED_DURING_THE_PHASE, expand_dynamics: bool = True, ) -> OptimalControlProgram: @@ -62,8 +60,6 @@ def prepare_ocp( 'MINIMIZE_COM_POSITION' or 'MINIMIZE_COM_VELOCITY') com_constraints: bool If a constraint on the COM should be applied - rigidbody_dynamics: RigidBodyDynamics - which transcription of rigidbody dynamics is chosen phase_dynamics: PhaseDynamics If the dynamics equation within a phase is unique or changes at each node. PhaseDynamics.SHARED_DURING_THE_PHASE is much faster, but lacks the capability to have changing dynamics within @@ -108,7 +104,6 @@ def prepare_ocp( dynamics.add( DynamicsFcn.TORQUE_DRIVEN, with_contact=True, - rigidbody_dynamics=rigidbody_dynamics, expand_dynamics=expand_dynamics, phase_dynamics=phase_dynamics, ) @@ -148,16 +143,6 @@ def prepare_ocp( # Define control path constraint u_bounds = BoundsList() u_bounds["tau"] = [tau_min] * len(dof_mapping["tau"].to_first), [tau_max] * len(dof_mapping["tau"].to_first) - if rigidbody_dynamics == RigidBodyDynamics.DAE_FORWARD_DYNAMICS: - u_bounds["qddot"] = [tau_min] * bio_model.nb_qddot, [tau_max] * bio_model.nb_qddot - elif rigidbody_dynamics == RigidBodyDynamics.DAE_INVERSE_DYNAMICS: - u_bounds["qddot"] = [tau_min] * bio_model.nb_qddot, [tau_max] * bio_model.nb_qddot - - min_forces_seg1 = [0, -tau_min, -tau_min, 0, 0, 0] - min_forces_seg2 = [0, 0, -tau_min, 0, 0, 0] - max_forces_seg1 = [0, tau_max, tau_max, 0, 0, 0] - max_forces_seg2 = [0, 0, tau_max, 0, 0, 0] - u_bounds["translational_forces"] = min_forces_seg1 + min_forces_seg2, max_forces_seg1 + max_forces_seg2 return OptimalControlProgram( bio_model, diff --git a/bioptim/examples/torque_driven_ocp/ocp_mass_with_ligament.py b/bioptim/examples/torque_driven_ocp/ocp_mass_with_ligament.py index 92839e13d..70cf69f47 100644 --- a/bioptim/examples/torque_driven_ocp/ocp_mass_with_ligament.py +++ b/bioptim/examples/torque_driven_ocp/ocp_mass_with_ligament.py @@ -13,7 +13,6 @@ OdeSolver, BiorbdModel, DynamicsFcn, - RigidBodyDynamics, PhaseDynamics, ) @@ -22,7 +21,6 @@ def prepare_ocp( biorbd_model_path: str, use_sx: bool = False, ode_solver=OdeSolver.RK4(), - rigidbody_dynamics: RigidBodyDynamics = RigidBodyDynamics.ODE, phase_dynamics: PhaseDynamics = PhaseDynamics.SHARED_DURING_THE_PHASE, n_threads: int = 8, expand_dynamics: bool = True, @@ -38,8 +36,6 @@ def prepare_ocp( If the project should be build in mx [False] or sx [True] ode_solver: OdeSolverBase The type of integrator - rigidbody_dynamics: RigidBodyDynamics - The rigidbody dynamics to use phase_dynamics: PhaseDynamics If the dynamics equation within a phase is unique or changes at each node. PhaseDynamics.SHARED_DURING_THE_PHASE is much faster, but lacks the capability to have changing dynamics within @@ -73,7 +69,6 @@ def prepare_ocp( dynamics = DynamicsList() dynamics.add( DynamicsFcn.TORQUE_DRIVEN, - rigidbody_dynamics=rigidbody_dynamics, with_ligament=True, expand_dynamics=expand_dynamics, phase_dynamics=phase_dynamics, @@ -92,12 +87,6 @@ def prepare_ocp( u_bounds["tau"] = [tau_min] * bio_model.nb_tau, [tau_max] * bio_model.nb_tau u_init["tau"] = [tau_init] * bio_model.nb_tau - if ( - rigidbody_dynamics == RigidBodyDynamics.DAE_INVERSE_DYNAMICS - or rigidbody_dynamics == RigidBodyDynamics.DAE_FORWARD_DYNAMICS - ): - u_bounds["qddot"] = [qddot_min] * bio_model.nb_qddot, [qddot_max] * bio_model.nb_qddot - u_init["qddot"] = [qddot_init] * bio_model.nb_qddot return OptimalControlProgram( bio_model, diff --git a/bioptim/examples/torque_driven_ocp/pendulum_with_passive_torque.py b/bioptim/examples/torque_driven_ocp/pendulum_with_passive_torque.py index 54e402a35..076d00d29 100644 --- a/bioptim/examples/torque_driven_ocp/pendulum_with_passive_torque.py +++ b/bioptim/examples/torque_driven_ocp/pendulum_with_passive_torque.py @@ -28,7 +28,6 @@ def prepare_ocp( final_time: float, n_shooting: int, ode_solver: OdeSolverBase = OdeSolver.RK4(), - rigidbody_dynamics=RigidBodyDynamics.DAE_INVERSE_DYNAMICS, with_passive_torque=False, phase_dynamics: PhaseDynamics = PhaseDynamics.SHARED_DURING_THE_PHASE, expand_dynamics: bool = True, @@ -46,8 +45,6 @@ def prepare_ocp( The number of shooting points to define int the direct multiple shooting program ode_solver: OdeSolverBase = OdeSolver.RK4() Which type of OdeSolver to use - rigidbody_dynamics : RigidBodyDynamics - rigidbody dynamics DAE or ODE with_passive_torque: bool If the passive torque is used in dynamics phase_dynamics: PhaseDynamics @@ -73,7 +70,6 @@ def prepare_ocp( # Dynamics dynamics = Dynamics( DynamicsFcn.TORQUE_DRIVEN, - rigidbody_dynamics=rigidbody_dynamics, with_passive_torque=with_passive_torque, expand_dynamics=expand_dynamics, phase_dynamics=phase_dynamics, @@ -95,12 +91,6 @@ def prepare_ocp( u_bounds["tau"] = [tau_min] * bio_model.nb_tau, [tau_max] * bio_model.nb_tau u_bounds["tau"][1, :] = 0 # Prevent the model from actively rotate - if ( - rigidbody_dynamics == RigidBodyDynamics.DAE_FORWARD_DYNAMICS - or rigidbody_dynamics == RigidBodyDynamics.DAE_INVERSE_DYNAMICS - ): - u_bounds["qddot"] = [qddot_min] * bio_model.nb_qddot, [qddot_max] * bio_model.nb_qddot - return OptimalControlProgram( bio_model, dynamics, diff --git a/bioptim/misc/enums.py b/bioptim/misc/enums.py index 79ad0a7e0..82c5c9ee6 100644 --- a/bioptim/misc/enums.py +++ b/bioptim/misc/enums.py @@ -194,15 +194,6 @@ class SoftContactDynamics(Enum): ODE = "ode" CONSTRAINT = "constraint" - -class RigidBodyDynamics(Enum): - ODE = "ode" - DAE_INVERSE_DYNAMICS = "dae_inverse_dynamics" - DAE_FORWARD_DYNAMICS = "dae_forward_dynamics" - DAE_INVERSE_DYNAMICS_JERK = "dae_inverse_dynamics_jerk" - DAE_FORWARD_DYNAMICS_JERK = "dae_forward_dynamics_jerk" - - class DefectType(Enum): EXPLICIT = "explicit" IMPLICIT = "implicit" diff --git a/tests/shard1/test__global_plots.py b/tests/shard1/test__global_plots.py index 970fd09e4..abba43461 100644 --- a/tests/shard1/test__global_plots.py +++ b/tests/shard1/test__global_plots.py @@ -10,7 +10,7 @@ import sys from casadi import Function, MX -from bioptim import CostType, OdeSolver, Solver, RigidBodyDynamics, BiorbdModel, PhaseDynamics +from bioptim import CostType, OdeSolver, Solver, BiorbdModel, PhaseDynamics from bioptim.limits.penalty import PenaltyOption matplotlib.use("Agg") @@ -184,51 +184,6 @@ def test_add_new_plot(phase_dynamics): sol.graphs(automatically_organize=False) -@pytest.mark.parametrize("phase_dynamics", [PhaseDynamics.SHARED_DURING_THE_PHASE, PhaseDynamics.ONE_PER_NODE]) -@pytest.mark.parametrize( - "rigidbody_dynamics", - [RigidBodyDynamics.ODE, RigidBodyDynamics.DAE_FORWARD_DYNAMICS, RigidBodyDynamics.DAE_INVERSE_DYNAMICS], -) -def test_plot_graphs_for_implicit_constraints(rigidbody_dynamics, phase_dynamics): - from bioptim.examples.getting_started import example_implicit_dynamics as ocp_module - - bioptim_folder = os.path.dirname(ocp_module.__file__) - - ocp = ocp_module.prepare_ocp( - biorbd_model_path=bioptim_folder + "/models/pendulum.bioMod", - n_shooting=5, - final_time=1, - rigidbody_dynamics=rigidbody_dynamics, - phase_dynamics=phase_dynamics, - expand_dynamics=False, - ) - ocp.add_plot_penalty(CostType.ALL) - sol = ocp.solve() - if sys.platform != "linux": - sol.graphs(automatically_organize=False) - - -def test_implicit_example(): - from bioptim.examples.getting_started import example_implicit_dynamics as ocp_module - - bioptim_folder = os.path.dirname(ocp_module.__file__) - - sol_implicit = ocp_module.solve_ocp( - rigidbody_dynamics=RigidBodyDynamics.DAE_INVERSE_DYNAMICS, - max_iter=1, - model_path=bioptim_folder + "/models/pendulum.bioMod", - ) - sol_semi_explicit = ocp_module.solve_ocp( - rigidbody_dynamics=RigidBodyDynamics.DAE_FORWARD_DYNAMICS, - max_iter=1, - model_path=bioptim_folder + "/models/pendulum.bioMod", - ) - sol_explicit = ocp_module.solve_ocp( - rigidbody_dynamics=RigidBodyDynamics.ODE, max_iter=1, model_path=bioptim_folder + "/models/pendulum.bioMod" - ) - ocp_module.prepare_plots(sol_implicit, sol_semi_explicit, sol_explicit) - - @pytest.mark.parametrize("phase_dynamics", [PhaseDynamics.SHARED_DURING_THE_PHASE, PhaseDynamics.ONE_PER_NODE]) def test_console_objective_functions(phase_dynamics): # Load graphs_one_phase diff --git a/tests/shard1/test_dynamics.py b/tests/shard1/test_dynamics.py index 5f4caa036..68aa177bb 100644 --- a/tests/shard1/test_dynamics.py +++ b/tests/shard1/test_dynamics.py @@ -12,7 +12,6 @@ DynamicsFunctions, BiorbdModel, ControlType, - RigidBodyDynamics, NonLinearProgram, DynamicsFcn, Dynamics, @@ -42,11 +41,7 @@ def __init__(self, nlp, use_sx): @pytest.mark.parametrize("cx", [MX, SX]) @pytest.mark.parametrize("with_external_force", [False, True]) @pytest.mark.parametrize("with_contact", [False, True]) -@pytest.mark.parametrize( - "rigidbody_dynamics", - [RigidBodyDynamics.ODE, RigidBodyDynamics.DAE_FORWARD_DYNAMICS, RigidBodyDynamics.DAE_INVERSE_DYNAMICS], -) -def test_torque_driven(with_contact, with_external_force, cx, rigidbody_dynamics, phase_dynamics): +def test_torque_driven(with_contact, with_external_force, cx, phase_dynamics): # Prepare the program nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx == SX)) @@ -56,10 +51,6 @@ def test_torque_driven(with_contact, with_external_force, cx, rigidbody_dynamics if with_external_force: segments_to_apply_forces_in_global = ["Seg0"] - if rigidbody_dynamics == RigidBodyDynamics.DAE_INVERSE_DYNAMICS: - if with_contact: - segments_to_apply_translational_forces = ["Seg0", "Seg1"] - nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod", segments_to_apply_forces_in_global=segments_to_apply_forces_in_global, @@ -146,7 +137,6 @@ def test_torque_driven(with_contact, with_external_force, cx, rigidbody_dynamics Dynamics( DynamicsFcn.TORQUE_DRIVEN, with_contact=with_contact, - rigidbody_dynamics=rigidbody_dynamics, expand_dynamics=True, phase_dynamics=phase_dynamics, numerical_data_timeseries=({"forces_in_global": external_forces} if external_forces is not None else None), @@ -181,181 +171,44 @@ def test_torque_driven(with_contact, with_external_force, cx, rigidbody_dynamics time, states[:, 0], controls[:, 0], params[:, 0], algebraic_states[:, 0], numerical_timeseries ) ) - if rigidbody_dynamics == RigidBodyDynamics.ODE: - if with_contact: - contact_out = np.array( - nlp.contact_forces_func(time, states, controls, params, algebraic_states, numerical_timeseries) - ) - if with_external_force: - npt.assert_almost_equal( - x_out[:, 0], - [0.9695846, 0.9218742, 0.3886773, 0.5426961, -2.2030836, -0.3463042, 4.4577117, -3.5917074], - ) - npt.assert_almost_equal(contact_out[:, 0], [-14.3821076, 126.2899884, 4.1631847]) - - else: - npt.assert_almost_equal( - x_out[:, 0], - [0.6118529, 0.785176, 0.6075449, 0.8083973, -0.3214905, -0.1912131, 0.6507164, -0.2359716], - ) - npt.assert_almost_equal(contact_out[:, 0], [-2.444071, 128.8816865, 2.7245124]) - - else: - if with_external_force: - npt.assert_almost_equal( - x_out[:, 0], - [0.9695846, 0.9218742, 0.3886773, 0.5426961, -1.090359, -10.1284375, 4.8896337, 13.5217526], - ) - else: - npt.assert_almost_equal( - x_out[:, 0], - [ - 0.61185289, - 0.78517596, - 0.60754485, - 0.80839735, - -0.30241366, - -10.38503791, - 1.60445173, - 35.80238642, - ], - ) - elif rigidbody_dynamics == RigidBodyDynamics.DAE_FORWARD_DYNAMICS: - if with_contact: - contact_out = np.array( - nlp.contact_forces_func(time, states, controls, params, algebraic_states, numerical_timeseries) - ) - if with_external_force: - npt.assert_almost_equal( - x_out[:, 0], - [0.9695846, 0.9218742, 0.3886773, 0.5426961, 0.1195942, 0.4937956, 0.0314292, 0.2492922], - ) - npt.assert_almost_equal(contact_out[:, 0], [-14.3821076, 126.2899884, 4.1631847]) - else: - npt.assert_almost_equal( - x_out[:, 0], [0.6118529, 0.785176, 0.6075449, 0.8083973, 0.3886773, 0.5426961, 0.7722448, 0.7290072] - ) - npt.assert_almost_equal(contact_out[:, 0], [-2.444071, 128.8816865, 2.7245124]) - - else: - if with_external_force: - npt.assert_almost_equal( - x_out[:, 0], - [0.9695846, 0.9218742, 0.3886773, 0.5426961, 0.1195942, 0.4937956, 0.0314292, 0.2492922], - ) - else: - npt.assert_almost_equal( - x_out[:, 0], - [0.6118529, 0.785176, 0.6075449, 0.8083973, 0.3886773, 0.5426961, 0.7722448, 0.7290072], - ) - elif rigidbody_dynamics == RigidBodyDynamics.DAE_INVERSE_DYNAMICS: - if with_contact: - contact_out = np.array( - nlp.contact_forces_func(time, states, controls, params, algebraic_states, numerical_timeseries) - ) - if with_external_force: - npt.assert_almost_equal( - x_out[:, 0], - [0.9695846, 0.9218742, 0.3886773, 0.5426961, 0.1195942, 0.4937956, 0.0314292, 0.2492922], - ) - npt.assert_almost_equal(contact_out[:, 0], [-14.3821076, 126.2899884, 4.1631847]) - else: - npt.assert_almost_equal( - x_out[:, 0], [0.6118529, 0.785176, 0.6075449, 0.8083973, 0.3886773, 0.5426961, 0.7722448, 0.7290072] - ) - npt.assert_almost_equal(contact_out[:, 0], [-2.444071, 128.8816865, 2.7245124]) - - else: - if with_external_force: - npt.assert_almost_equal( - x_out[:, 0], - [0.9695846, 0.9218742, 0.3886773, 0.5426961, 0.1195942, 0.4937956, 0.0314292, 0.2492922], - ) - else: - npt.assert_almost_equal( - x_out[:, 0], - [0.6118529, 0.785176, 0.6075449, 0.8083973, 0.3886773, 0.5426961, 0.7722448, 0.7290072], - ) - - -@pytest.mark.parametrize("phase_dynamics", [PhaseDynamics.SHARED_DURING_THE_PHASE, PhaseDynamics.ONE_PER_NODE]) -@pytest.mark.parametrize("cx", [MX, SX]) -@pytest.mark.parametrize("with_contact", [False, True]) -def test_torque_driven_implicit(with_contact, cx, phase_dynamics): - # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx == SX)) - nlp.model = BiorbdModel( - TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod", - segments_to_apply_translational_forces=["Seg0", "Seg1"] if with_contact else None, - ) - - nlp.ns = 5 - nlp.cx = cx - nlp.time_cx = cx.sym("time", 1, 1) - nlp.dt = cx.sym("dt", 1, 1) - nlp.initialize(cx) - - nlp.x_bounds = np.zeros((nlp.model.nb_q * 3, 1)) - nlp.u_bounds = np.zeros((nlp.model.nb_q * 2, 1)) - nlp.x_scaling = VariableScalingList() - nlp.xdot_scaling = VariableScalingList() - nlp.u_scaling = VariableScalingList() - nlp.a_scaling = VariableScalingList() - - ocp = OptimalControlProgram(nlp, use_sx=(cx == SX)) - nlp.control_type = ControlType.CONSTANT - - NonLinearProgram.add( - ocp, - "dynamics_type", - Dynamics( - DynamicsFcn.TORQUE_DRIVEN, - with_contact=with_contact, - rigidbody_dynamics=RigidBodyDynamics.DAE_INVERSE_DYNAMICS, - expand_dynamics=True, - phase_dynamics=phase_dynamics, - ), - False, - ) - phase_index = [i for i in range(ocp.n_phases)] - NonLinearProgram.add(ocp, "phase_idx", phase_index, False) - use_states_from_phase_idx = [i for i in range(ocp.n_phases)] - use_states_dot_from_phase_idx = [i for i in range(ocp.n_phases)] - use_controls_from_phase_idx = [i for i in range(ocp.n_phases)] - NonLinearProgram.add(ocp, "use_states_from_phase_idx", use_states_from_phase_idx, False) - NonLinearProgram.add(ocp, "use_states_dot_from_phase_idx", use_states_dot_from_phase_idx, False) - NonLinearProgram.add(ocp, "use_controls_from_phase_idx", use_controls_from_phase_idx, False) - - # Prepare the dynamics - nlp.numerical_timeseries = TestUtils.initialize_numerical_timeseries(nlp, dynamics=nlp.dynamics_type) - ConfigureProblem.initialize(ocp, nlp) - - # Test the results - np.random.seed(42) - states = np.random.rand(nlp.states.shape, nlp.ns) - controls = np.random.rand(nlp.controls.shape, nlp.ns) - params = np.random.rand(nlp.parameters.shape, nlp.ns) - algebraic_states = np.random.rand(nlp.algebraic_states.shape, nlp.ns) - numerical_timeseries = [] - time = np.random.rand(2) - x_out = np.array(nlp.dynamics_func(time, states, controls, params, algebraic_states, numerical_timeseries)) - if with_contact: contact_out = np.array( nlp.contact_forces_func(time, states, controls, params, algebraic_states, numerical_timeseries) ) - npt.assert_almost_equal( - x_out[:, 0], [0.6118529, 0.785176, 0.6075449, 0.8083973, 0.3886773, 0.5426961, 0.7722448, 0.7290072] - ) + if with_external_force: + npt.assert_almost_equal( + x_out[:, 0], + [0.9695846, 0.9218742, 0.3886773, 0.5426961, -2.2030836, -0.3463042, 4.4577117, -3.5917074], + ) + npt.assert_almost_equal(contact_out[:, 0], [-14.3821076, 126.2899884, 4.1631847]) - npt.assert_almost_equal(contact_out[:, 0], [-2.444071, 128.8816865, 2.7245124]) + else: + npt.assert_almost_equal( + x_out[:, 0], + [0.6118529, 0.785176, 0.6075449, 0.8083973, -0.3214905, -0.1912131, 0.6507164, -0.2359716], + ) + npt.assert_almost_equal(contact_out[:, 0], [-2.444071, 128.8816865, 2.7245124]) else: - npt.assert_almost_equal( - x_out[:, 0], - [0.6118529, 0.785176, 0.6075449, 0.8083973, 0.3886773, 0.5426961, 0.7722448, 0.7290072], - ) - + if with_external_force: + npt.assert_almost_equal( + x_out[:, 0], + [0.9695846, 0.9218742, 0.3886773, 0.5426961, -1.090359, -10.1284375, 4.8896337, 13.5217526], + ) + else: + npt.assert_almost_equal( + x_out[:, 0], + [ + 0.61185289, + 0.78517596, + 0.60754485, + 0.80839735, + -0.30241366, + -10.38503791, + 1.60445173, + 35.80238642, + ], + ) @pytest.mark.parametrize("phase_dynamics", [PhaseDynamics.SHARED_DURING_THE_PHASE, PhaseDynamics.ONE_PER_NODE]) @pytest.mark.parametrize("cx", [MX, SX]) @@ -645,115 +498,6 @@ def test_torque_derivative_driven(with_contact, with_external_force, cx, phase_d ) -@pytest.mark.parametrize("phase_dynamics", [PhaseDynamics.SHARED_DURING_THE_PHASE, PhaseDynamics.ONE_PER_NODE]) -@pytest.mark.parametrize("cx", [MX, SX]) -@pytest.mark.parametrize("with_contact", [False, True]) -def test_torque_derivative_driven_implicit(with_contact, cx, phase_dynamics): - # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx == SX)) - nlp.model = BiorbdModel( - TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod" - ) - nlp.ns = 5 - nlp.cx = cx - nlp.time_cx = cx.sym("time", 1, 1) - nlp.dt = cx.sym("dt", 1, 1) - nlp.initialize(cx) - nlp.phase_idx = 0 - nlp.x_bounds = np.zeros((nlp.model.nb_q * 4, 1)) - nlp.u_bounds = np.zeros((nlp.model.nb_q, 2)) - nlp.x_scaling = VariableScalingList() - nlp.xdot_scaling = VariableScalingList() - nlp.u_scaling = VariableScalingList() - nlp.a_scaling = VariableScalingList() - - ocp = OptimalControlProgram(nlp, use_sx=(cx == SX)) - nlp.control_type = ControlType.CONSTANT - NonLinearProgram.add( - ocp, - "dynamics_type", - Dynamics( - DynamicsFcn.TORQUE_DERIVATIVE_DRIVEN, - with_contact=with_contact, - rigidbody_dynamics=RigidBodyDynamics.DAE_INVERSE_DYNAMICS, - expand_dynamics=True, - phase_dynamics=phase_dynamics, - ), - False, - ) - phase_index = [i for i in range(ocp.n_phases)] - NonLinearProgram.add(ocp, "phase_idx", phase_index, False) - use_states_from_phase_idx = [i for i in range(ocp.n_phases)] - use_states_dot_from_phase_idx = [i for i in range(ocp.n_phases)] - use_controls_from_phase_idx = [i for i in range(ocp.n_phases)] - NonLinearProgram.add(ocp, "use_states_from_phase_idx", use_states_from_phase_idx, False) - NonLinearProgram.add(ocp, "use_states_dot_from_phase_idx", use_states_dot_from_phase_idx, False) - NonLinearProgram.add(ocp, "use_controls_from_phase_idx", use_controls_from_phase_idx, False) - - # Prepare the dynamics - nlp.numerical_timeseries = TestUtils.initialize_numerical_timeseries(nlp, dynamics=nlp.dynamics_type) - ConfigureProblem.initialize(ocp, nlp) - - # Test the results - np.random.seed(42) - states = np.random.rand(nlp.states.shape, nlp.ns) - controls = np.random.rand(nlp.controls.shape, nlp.ns) - params = np.random.rand(nlp.parameters.shape, nlp.ns) - algebraic_states = np.random.rand(nlp.algebraic_states.shape, nlp.ns) - numerical_timeseries = [] - time = np.random.rand(2) - x_out = np.array(nlp.dynamics_func(time, states, controls, params, algebraic_states, numerical_timeseries)) - - if with_contact: - contact_out = np.array( - nlp.contact_forces_func(time, states, controls, params, algebraic_states, numerical_timeseries) - ) - npt.assert_almost_equal( - x_out[:, 0], - [ - 0.6118529, - 0.785176, - 0.6075449, - 0.8083973, - 0.3886773, - 0.5426961, - 0.7722448, - 0.7290072, - 0.8631034, - 0.3251833, - 0.1195942, - 0.4937956, - 0.0314292, - 0.2492922, - 0.2897515, - 0.8714606, - ], - ) - npt.assert_almost_equal(contact_out[:, 0], [-2.444071, 128.8816865, 2.7245124]) - else: - npt.assert_almost_equal( - x_out[:, 0], - [ - 0.6118529, - 0.785176, - 0.6075449, - 0.8083973, - 0.3886773, - 0.5426961, - 0.7722448, - 0.7290072, - 0.8631034, - 0.3251833, - 0.1195942, - 0.4937956, - 0.0314292, - 0.2492922, - 0.2897515, - 0.8714606, - ], - ) - - @pytest.mark.parametrize("phase_dynamics", [PhaseDynamics.SHARED_DURING_THE_PHASE, PhaseDynamics.ONE_PER_NODE]) @pytest.mark.parametrize("cx", [MX, SX]) @pytest.mark.parametrize("with_contact", [False, True]) @@ -900,50 +644,6 @@ def test_soft_contacts_dynamics_errors(dynamics, phase_dynamics): ConfigureProblem.initialize(ocp, nlp) -@pytest.mark.parametrize("phase_dynamics", [PhaseDynamics.SHARED_DURING_THE_PHASE, PhaseDynamics.ONE_PER_NODE]) -@pytest.mark.parametrize("dynamics", [DynamicsFcn.TORQUE_ACTIVATIONS_DRIVEN]) -def test_implicit_dynamics_errors(dynamics, phase_dynamics): - # Prepare the program - nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=False) - nlp.model = BiorbdModel( - TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod" - ) - nlp.ns = 5 - nlp.cx = MX - - nlp.u_bounds = np.zeros((nlp.model.nb_q * 4, 1)) - nlp.u_scaling = VariableScalingList() - - ocp = OptimalControlProgram(nlp, use_sx=True) - nlp.control_type = ControlType.CONSTANT - NonLinearProgram.add( - ocp, - "dynamics_type", - Dynamics( - dynamics, - rigidbody_dynamics=RigidBodyDynamics.DAE_INVERSE_DYNAMICS, - expand_dynamics=True, - phase_dynamics=phase_dynamics, - ), - False, - ) - phase_index = [i for i in range(ocp.n_phases)] - NonLinearProgram.add(ocp, "phase_idx", phase_index, False) - use_states_from_phase_idx = [i for i in range(ocp.n_phases)] - use_states_dot_from_phase_idx = [i for i in range(ocp.n_phases)] - use_controls_from_phase_idx = [i for i in range(ocp.n_phases)] - NonLinearProgram.add(ocp, "use_states_from_phase_idx", use_states_from_phase_idx, False) - NonLinearProgram.add(ocp, "use_states_dot_from_phase_idx", use_states_dot_from_phase_idx, False) - NonLinearProgram.add(ocp, "use_controls_from_phase_idx", use_controls_from_phase_idx, False) - - # Prepare the dynamics - with pytest.raises( - TypeError, - match=re.escape(f"{dynamics.name.lower()}() got an unexpected keyword argument " "'rigidbody_dynamics'"), - ): - ConfigureProblem.initialize(ocp, nlp) - - @pytest.mark.parametrize("phase_dynamics", [PhaseDynamics.SHARED_DURING_THE_PHASE, PhaseDynamics.ONE_PER_NODE]) @pytest.mark.parametrize("cx", [MX, SX]) @pytest.mark.parametrize("with_external_force", [False, True]) @@ -1372,9 +1072,8 @@ def test_torque_driven_free_floating_base(cx, phase_dynamics): @pytest.mark.parametrize("with_contact", [False, True]) @pytest.mark.parametrize("with_residual_torque", [False, True]) @pytest.mark.parametrize("with_excitations", [False, True]) -@pytest.mark.parametrize("rigidbody_dynamics", [RigidBodyDynamics.ODE, RigidBodyDynamics.DAE_INVERSE_DYNAMICS]) def test_muscle_driven( - with_excitations, with_contact, with_residual_torque, with_external_force, rigidbody_dynamics, cx, phase_dynamics + with_excitations, with_contact, with_residual_torque, with_external_force, cx, phase_dynamics ): # Prepare the program nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx == SX)) @@ -1465,7 +1164,6 @@ def test_muscle_driven( with_residual_torque=with_residual_torque, with_excitations=with_excitations, with_contact=with_contact, - rigidbody_dynamics=rigidbody_dynamics, expand_dynamics=True, phase_dynamics=phase_dynamics, numerical_data_timeseries=({"forces_in_global": external_forces} if external_forces is not None else None), @@ -1486,8 +1184,6 @@ def test_muscle_driven( np.random.rand(nlp.ns, 6) # just to make sure the next random is the same as before # Prepare the dynamics - if rigidbody_dynamics == RigidBodyDynamics.DAE_INVERSE_DYNAMICS: - pass nlp.numerical_timeseries = TestUtils.initialize_numerical_timeseries(nlp, dynamics=nlp.dynamics_type) ConfigureProblem.initialize(ocp, nlp) @@ -1505,447 +1201,122 @@ def test_muscle_driven( ) if with_contact: # Warning this test is a bit bogus, there since the model does not have contacts - if rigidbody_dynamics == RigidBodyDynamics.DAE_INVERSE_DYNAMICS: - if with_residual_torque: - if with_excitations: - if with_external_force: - npt.assert_almost_equal( - x_out[:, 0], - [ - 0.6625223, - 0.9695846, - 0.9218742, - 0.3232029, - 0.9624473, - 0.0368869, - -3.773906, - -8.3095101, - 5.9827416, - 4.9220243, - -19.5615453, - 9.336912, - ], - ) - else: - npt.assert_almost_equal( - x_out[:, 0], - [ - 0.183405, - 0.611853, - 0.785176, - 0.249292, - 0.289751, - 0.871461, - 8.606308, - 3.194336, - 29.740561, - -20.275423, - -23.246778, - -41.913501, - ], - decimal=6, - ) - else: - if with_external_force: - npt.assert_almost_equal( - x_out[:, 0], - [0.662522, 0.969585, 0.921874, 0.249292, 0.289751, 0.871461], - decimal=6, - ) - else: - npt.assert_almost_equal( - x_out[:, 0], - [0.183405, 0.611853, 0.785176, 0.729007, 0.863103, 0.325183], - decimal=6, - ) - - else: - if with_excitations: - if with_external_force: - npt.assert_almost_equal( - x_out[:, 0], - [ - 0.6625223, - 0.9695846, - 0.9218742, - 0.8074402, - 0.4271078, - 0.417411, - -7.2855306, - -1.6064349, - -30.7136058, - -19.1107728, - -25.7242266, - 55.3038169, - ], - ) - else: - npt.assert_almost_equal( - x_out[:, 0], - [ - 1.83404510e-01, - 6.11852895e-01, - 7.85175961e-01, - 1.19594246e-01, - 4.93795596e-01, - 3.14291857e-02, - -7.72228930e00, - -1.13759732e01, - 9.51906209e01, - 4.45077128e00, - -5.20261014e00, - -2.80864106e01, - ], - decimal=6, - ) - else: - if with_external_force: - npt.assert_almost_equal( - x_out[:, 0], - [0.6625223, 0.9695846, 0.9218742, 0.1195942, 0.4937956, 0.0314292], - ) - else: - npt.assert_almost_equal( - x_out[:, 0], - [0.183405, 0.611853, 0.785176, 0.388677, 0.542696, 0.772245], - decimal=6, - ) - else: - if with_residual_torque: - if with_excitations: - if with_external_force: - npt.assert_almost_equal( - x_out[:, 0], - [ - 0.6625223, - 0.9695846, - 0.9218742, - 0.2123157, - -29.9955403, - -37.8135747, - -3.773906, - -8.3095101, - 5.9827416, - 4.9220243, - -19.5615453, - 9.336912, - ], - ) - else: - npt.assert_almost_equal( - x_out[:, 0], - [ - 1.83404510e-01, - 6.11852895e-01, - 7.85175961e-01, - -3.94658983e00, - 1.23227027e02, - -4.38936797e02, - 8.60630831e00, - 3.19433638e00, - 2.97405608e01, - -2.02754226e01, - -2.32467778e01, - -4.19135012e01, - ], - decimal=6, - ) + if with_residual_torque: + if with_excitations: + if with_external_force: + npt.assert_almost_equal( + x_out[:, 0], + [ + 0.6625223, + 0.9695846, + 0.9218742, + 0.2123157, + -29.9955403, + -37.8135747, + -3.773906, + -8.3095101, + 5.9827416, + 4.9220243, + -19.5615453, + 9.336912, + ], + ) else: - if with_external_force: - npt.assert_almost_equal( - x_out[:, 0], - [0.662522, 0.969585, 0.921874, 1.151072, -56.094393, 49.109365], - decimal=6, - ) - else: - npt.assert_almost_equal( - x_out[:, 0], - [0.183405, 0.611853, 0.785176, -0.867138, 22.511947, -153.294775], - decimal=6, - ) - + npt.assert_almost_equal( + x_out[:, 0], + [ + 1.83404510e-01, + 6.11852895e-01, + 7.85175961e-01, + -3.94658983e00, + 1.23227027e02, + -4.38936797e02, + 8.60630831e00, + 3.19433638e00, + 2.97405608e01, + -2.02754226e01, + -2.32467778e01, + -4.19135012e01, + ], + decimal=6, + ) else: - if with_excitations: - if with_external_force: - npt.assert_almost_equal( - x_out[:, 0], - [ - 0.6625223, - 0.9695846, - 0.9218742, - 0.2684853, - -33.7252751, - -30.3079326, - -7.2855306, - -1.6064349, - -30.7136058, - -19.1107728, - -25.7242266, - 55.3038169, - ], - ) - else: - npt.assert_almost_equal( - x_out[:, 0], - [ - 1.83404510e-01, - 6.11852895e-01, - 7.85175961e-01, - -4.37708456e00, - 1.33221135e02, - -4.71307550e02, - -7.72228930e00, - -1.13759732e01, - 9.51906209e01, - 4.45077128e00, - -5.20261014e00, - -2.80864106e01, - ], - decimal=6, - ) - else: - if with_external_force: - npt.assert_almost_equal( - x_out[:, 0], - [0.6625223, 0.9695846, 0.9218742, 0.2684853, -33.7252751, -30.3079326], - ) - else: - npt.assert_almost_equal( - x_out[:, 0], - [ - 1.83404510e-01, - 6.11852895e-01, - 7.85175961e-01, - -4.37708456e00, - 1.33221135e02, - -4.71307550e02, - ], - decimal=6, - ) - else: - if rigidbody_dynamics == RigidBodyDynamics.DAE_INVERSE_DYNAMICS: - if with_residual_torque: - if with_excitations: - if with_external_force: - npt.assert_almost_equal( - x_out[:, 0], - [ - 0.6625223, - 0.9695846, - 0.9218742, - 0.3232029, - 0.9624473, - 0.0368869, - -3.773906, - -8.3095101, - 5.9827416, - 4.9220243, - -19.5615453, - 9.336912, - ], - ) - else: - npt.assert_almost_equal( - x_out[:, 0], - [ - 0.183405, - 0.611853, - 0.785176, - 0.249292, - 0.289751, - 0.871461, - 8.606308, - 3.194336, - 29.740561, - -20.275423, - -23.246778, - -41.913501, - ], - decimal=6, - ) + if with_external_force: + npt.assert_almost_equal( + x_out[:, 0], + [0.662522, 0.969585, 0.921874, 1.151072, -56.094393, 49.109365], + decimal=6, + ) else: - if with_external_force: - npt.assert_almost_equal( - x_out[:, 0], - [0.662522, 0.969585, 0.921874, 0.249292, 0.289751, 0.871461], - decimal=6, - ) - else: - npt.assert_almost_equal( - x_out[:, 0], - [0.183405, 0.611853, 0.785176, 0.729007, 0.863103, 0.325183], - decimal=6, - ) + npt.assert_almost_equal( + x_out[:, 0], + [0.18340451, 0.61185289, 0.78517596, -0.8671376, 22.51194682, -153.29477496], + decimal=6, + ) - else: - if with_excitations: - if with_external_force: - npt.assert_almost_equal( - x_out[:, 0], - [ - 0.6625223, - 0.9695846, - 0.9218742, - 0.8074402, - 0.4271078, - 0.417411, - -7.2855306, - -1.6064349, - -30.7136058, - -19.1107728, - -25.7242266, - 55.3038169, - ], - ) - else: - npt.assert_almost_equal( - x_out[:, 0], - [ - 1.83404510e-01, - 6.11852895e-01, - 7.85175961e-01, - 1.19594246e-01, - 4.93795596e-01, - 3.14291857e-02, - -7.72228930e00, - -1.13759732e01, - 9.51906209e01, - 4.45077128e00, - -5.20261014e00, - -2.80864106e01, - ], - decimal=6, - ) - else: - if with_external_force: - npt.assert_almost_equal( - x_out[:, 0], - [0.6625223, 0.9695846, 0.9218742, 0.1195942, 0.4937956, 0.0314292], - ) - else: - npt.assert_almost_equal( - x_out[:, 0], - [0.183405, 0.611853, 0.785176, 0.388677, 0.542696, 0.772245], - decimal=6, - ) else: - if with_residual_torque: - if with_excitations: - if with_external_force: - npt.assert_almost_equal( - x_out[:, 0], - [ - 0.6625223, - 0.9695846, - 0.9218742, - 0.2123157, - -29.9955403, - -37.8135747, - -3.773906, - -8.3095101, - 5.9827416, - 4.9220243, - -19.5615453, - 9.336912, - ], - ) - else: - npt.assert_almost_equal( - x_out[:, 0], - [ - 1.83404510e-01, - 6.11852895e-01, - 7.85175961e-01, - -3.94658983e00, - 1.23227027e02, - -4.38936797e02, - 8.60630831e00, - 3.19433638e00, - 2.97405608e01, - -2.02754226e01, - -2.32467778e01, - -4.19135012e01, - ], - decimal=6, - ) + if with_excitations: + if with_external_force: + npt.assert_almost_equal( + x_out[:, 0], + [ + 0.6625223, + 0.9695846, + 0.9218742, + 0.2684853, + -33.7252751, + -30.3079326, + -7.2855306, + -1.6064349, + -30.7136058, + -19.1107728, + -25.7242266, + 55.3038169, + ], + ) else: - if with_external_force: - npt.assert_almost_equal( - x_out[:, 0], - [0.662522, 0.969585, 0.921874, 1.151072, -56.094393, 49.109365], - decimal=6, - ) - else: - npt.assert_almost_equal( - x_out[:, 0], - [0.18340451, 0.61185289, 0.78517596, -0.8671376, 22.51194682, -153.29477496], - decimal=6, - ) - + npt.assert_almost_equal( + x_out[:, 0], + [ + 1.83404510e-01, + 6.11852895e-01, + 7.85175961e-01, + -4.37708456e00, + 1.33221135e02, + -4.71307550e02, + -7.72228930e00, + -1.13759732e01, + 9.51906209e01, + 4.45077128e00, + -5.20261014e00, + -2.80864106e01, + ], + decimal=6, + ) else: - if with_excitations: - if with_external_force: - npt.assert_almost_equal( - x_out[:, 0], - [ - 0.6625223, - 0.9695846, - 0.9218742, - 0.2684853, - -33.7252751, - -30.3079326, - -7.2855306, - -1.6064349, - -30.7136058, - -19.1107728, - -25.7242266, - 55.3038169, - ], - ) - else: - npt.assert_almost_equal( - x_out[:, 0], - [ - 1.83404510e-01, - 6.11852895e-01, - 7.85175961e-01, - -4.37708456e00, - 1.33221135e02, - -4.71307550e02, - -7.72228930e00, - -1.13759732e01, - 9.51906209e01, - 4.45077128e00, - -5.20261014e00, - -2.80864106e01, - ], - decimal=6, - ) + if with_external_force: + npt.assert_almost_equal( + x_out[:, 0], + [0.6625223, 0.9695846, 0.9218742, 0.2684853, -33.7252751, -30.3079326], + ) else: - if with_external_force: - npt.assert_almost_equal( - x_out[:, 0], - [0.6625223, 0.9695846, 0.9218742, 0.2684853, -33.7252751, -30.3079326], - ) - else: - npt.assert_almost_equal( - x_out[:, 0], - [ - 1.83404510e-01, - 6.11852895e-01, - 7.85175961e-01, - -4.37708456e00, - 1.33221135e02, - -4.71307550e02, - ], - decimal=6, - ) + npt.assert_almost_equal( + x_out[:, 0], + [ + 1.83404510e-01, + 6.11852895e-01, + 7.85175961e-01, + -4.37708456e00, + 1.33221135e02, + -4.71307550e02, + ], + decimal=6, + ) @pytest.mark.parametrize("phase_dynamics", [PhaseDynamics.SHARED_DURING_THE_PHASE, PhaseDynamics.ONE_PER_NODE]) @pytest.mark.parametrize("cx", [MX, SX]) -@pytest.mark.parametrize("rigid_body_dynamics", RigidBodyDynamics) -def test_joints_acceleration_driven(cx, rigid_body_dynamics, phase_dynamics): +def test_joints_acceleration_driven(cx, phase_dynamics): # Prepare the program nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx == SX)) nlp.model = BiorbdModel(TestUtils.bioptim_folder() + "/examples/getting_started/models/double_pendulum.bioMod") @@ -1971,7 +1342,6 @@ def test_joints_acceleration_driven(cx, rigid_body_dynamics, phase_dynamics): "dynamics_type", Dynamics( DynamicsFcn.JOINTS_ACCELERATION_DRIVEN, - rigidbody_dynamics=rigid_body_dynamics, expand_dynamics=True, phase_dynamics=phase_dynamics, ), @@ -1988,24 +1358,20 @@ def test_joints_acceleration_driven(cx, rigid_body_dynamics, phase_dynamics): NonLinearProgram.add(ocp, "use_controls_from_phase_idx", use_controls_from_phase_idx, False) # Prepare the dynamics - if rigid_body_dynamics != RigidBodyDynamics.ODE: - with pytest.raises(NotImplementedError, match=re.escape("Implicit dynamics not implemented yet.")): - ConfigureProblem.initialize(ocp, nlp) - else: - nlp.numerical_timeseries = TestUtils.initialize_numerical_timeseries(nlp, dynamics=nlp.dynamics_type) - ConfigureProblem.initialize(ocp, nlp) + nlp.numerical_timeseries = TestUtils.initialize_numerical_timeseries(nlp, dynamics=nlp.dynamics_type) + ConfigureProblem.initialize(ocp, nlp) + + # Test the results + states = np.random.rand(nlp.states.shape, nlp.ns) + controls = np.random.rand(nlp.controls.shape, nlp.ns) + params = np.random.rand(nlp.parameters.shape, nlp.ns) + algebraic_states = np.random.rand(nlp.algebraic_states.shape, nlp.ns) + numerical_timeseries = [] + time = np.random.rand(2) + x_out = np.array(nlp.dynamics_func(time, states, controls, params, algebraic_states, numerical_timeseries)) - # Test the results - states = np.random.rand(nlp.states.shape, nlp.ns) - controls = np.random.rand(nlp.controls.shape, nlp.ns) - params = np.random.rand(nlp.parameters.shape, nlp.ns) - algebraic_states = np.random.rand(nlp.algebraic_states.shape, nlp.ns) - numerical_timeseries = [] - time = np.random.rand(2) - x_out = np.array(nlp.dynamics_func(time, states, controls, params, algebraic_states, numerical_timeseries)) - - # obtained using Ipuch reference implementation. [https://github.com/Ipuch/OnDynamicsForSomersaults] - npt.assert_almost_equal(x_out[:, 0], [0.02058449, 0.18340451, -2.95556261, 0.61185289]) + # obtained using Ipuch reference implementation. [https://github.com/Ipuch/OnDynamicsForSomersaults] + npt.assert_almost_equal(x_out[:, 0], [0.02058449, 0.18340451, -2.95556261, 0.61185289]) @pytest.mark.parametrize("phase_dynamics", [PhaseDynamics.SHARED_DURING_THE_PHASE, PhaseDynamics.ONE_PER_NODE]) diff --git a/tests/shard1/test_dynamics_units.py b/tests/shard1/test_dynamics_units.py index 08a14a2bf..ba4537264 100644 --- a/tests/shard1/test_dynamics_units.py +++ b/tests/shard1/test_dynamics_units.py @@ -1,7 +1,7 @@ import numpy as np import pytest -from bioptim import RigidBodyDynamics, SoftContactDynamics +from bioptim import SoftContactDynamics from bioptim.dynamics.configure_problem import ( _check_numerical_timeseries_format, _check_soft_contacts_dynamics, @@ -38,11 +38,6 @@ def test_check_soft_contacts_dynamics_valid_ode(): _check_soft_contacts_dynamics("NotDAEInverseDynamics", SoftContactDynamics.ODE, 1, 0) -def test_check_soft_contacts_dynamics_invalid_rigid(): - with pytest.raises(ValueError): - _check_soft_contacts_dynamics(RigidBodyDynamics.DAE_INVERSE_DYNAMICS, SoftContactDynamics.ODE, 1, 0) - - # More tests for _check_external_forces_format def test_check_external_forces_format_none(): with pytest.raises( diff --git a/tests/shard1/test_enums.py b/tests/shard1/test_enums.py index 6b10c803a..017e46bdc 100644 --- a/tests/shard1/test_enums.py +++ b/tests/shard1/test_enums.py @@ -12,8 +12,9 @@ ControlType, SolutionIntegrator, QuadratureRule, - RigidBodyDynamics, SoftContactDynamics, + ExternalForcesType, + ReferenceFrame, ) from bioptim.misc.enums import SolverType, PenaltyType, ConstraintType @@ -161,17 +162,6 @@ def test_soft_contact_dynamics(): assert len(SoftContactDynamics) == 2 -def test_rigid_body_dynamics(): - assert RigidBodyDynamics.ODE.value == "ode" - assert RigidBodyDynamics.DAE_INVERSE_DYNAMICS.value == "dae_inverse_dynamics" - assert RigidBodyDynamics.DAE_FORWARD_DYNAMICS.value == "dae_forward_dynamics" - assert RigidBodyDynamics.DAE_INVERSE_DYNAMICS_JERK.value == "dae_inverse_dynamics_jerk" - assert RigidBodyDynamics.DAE_FORWARD_DYNAMICS_JERK.value == "dae_forward_dynamics_jerk" - - # verify the number of elements - assert len(RigidBodyDynamics) == 5 - - def test_defect_type(): assert DefectType.EXPLICIT.value == "explicit" assert DefectType.IMPLICIT.value == "implicit" @@ -196,3 +186,19 @@ def test_multi_cyclic_cycle_solutions(): # verify the number of elements assert len(MultiCyclicCycleSolutions) == 3 + + +def test_external_forces_type(): + assert ExternalForcesType.LINEAR_FORCE == "linear_force" + assert ExternalForcesType.TORQUE == "torque" + + # verify the number of elements + assert len(ExternalForcesType) == 2 + + +def test_reference_frame(): + assert ReferenceFrame.GLOBAL == "global" + assert ReferenceFrame.LOCAL == "local" + + # verify the number of elements + assert len(ReferenceFrame) == 2 \ No newline at end of file diff --git a/tests/shard1/test_prepare_all_examples.py b/tests/shard1/test_prepare_all_examples.py index 2e28ed44f..704657941 100644 --- a/tests/shard1/test_prepare_all_examples.py +++ b/tests/shard1/test_prepare_all_examples.py @@ -206,9 +206,6 @@ def test__getting_started__example_external_forces(): ) -# todo: Add example_implicit_dynamics.py? - - def test__getting_started__example_inequality_constraint(): from bioptim.examples.getting_started import ( example_inequality_constraint as ocp_module, diff --git a/tests/shard3/test_global_torque_driven_with_contact_ocp.py b/tests/shard3/test_global_torque_driven_with_contact_ocp.py index 00cae2b89..19ac2af47 100644 --- a/tests/shard3/test_global_torque_driven_with_contact_ocp.py +++ b/tests/shard3/test_global_torque_driven_with_contact_ocp.py @@ -140,11 +140,7 @@ def test_maximize_predicted_height_CoM_with_actuators(phase_dynamics): @pytest.mark.parametrize("phase_dynamics", [PhaseDynamics.SHARED_DURING_THE_PHASE]) -@pytest.mark.parametrize( - "rigidbody_dynamics", - [RigidBodyDynamics.ODE, RigidBodyDynamics.DAE_FORWARD_DYNAMICS, RigidBodyDynamics.DAE_INVERSE_DYNAMICS], -) -def test_maximize_predicted_height_CoM_rigidbody_dynamics(rigidbody_dynamics, phase_dynamics): +def test_maximize_predicted_height_CoM_rigidbody_dynamics(phase_dynamics): from bioptim.examples.torque_driven_ocp import maximize_predicted_height_CoM as ocp_module bioptim_folder = os.path.dirname(ocp_module.__file__) @@ -157,7 +153,6 @@ def test_maximize_predicted_height_CoM_rigidbody_dynamics(rigidbody_dynamics, ph n_shooting=20, use_actuators=False, ode_solver=ode_solver, - rigidbody_dynamics=rigidbody_dynamics, phase_dynamics=phase_dynamics, expand_dynamics=True, ) @@ -168,19 +163,8 @@ def test_maximize_predicted_height_CoM_rigidbody_dynamics(rigidbody_dynamics, ph # Check objective function value f = np.array(sol.cost) npt.assert_equal(f.shape, (1, 1)) - - if rigidbody_dynamics == RigidBodyDynamics.ODE: - npt.assert_almost_equal(f[0, 0], 0.8032447451950947) - elif rigidbody_dynamics == RigidBodyDynamics.DAE_FORWARD_DYNAMICS: - npt.assert_almost_equal(f[0, 0], 0.9695327421106931) - elif rigidbody_dynamics == RigidBodyDynamics.DAE_INVERSE_DYNAMICS: - npt.assert_almost_equal(f[0, 0], 1.691190510518052) + npt.assert_almost_equal(f[0, 0], 0.8032447451950947) # Check constraints g = np.array(sol.constraints) - if rigidbody_dynamics == RigidBodyDynamics.ODE: - npt.assert_equal(g.shape, (160, 1)) - elif rigidbody_dynamics == RigidBodyDynamics.DAE_FORWARD_DYNAMICS: - npt.assert_equal(g.shape, (240, 1)) - elif rigidbody_dynamics == RigidBodyDynamics.DAE_INVERSE_DYNAMICS: - npt.assert_equal(g.shape, (300, 1)) + npt.assert_equal(g.shape, (160, 1)) diff --git a/tests/shard3/test_ligaments.py b/tests/shard3/test_ligaments.py index f228a10de..0c11d608d 100644 --- a/tests/shard3/test_ligaments.py +++ b/tests/shard3/test_ligaments.py @@ -62,7 +62,7 @@ def test_torque_driven_with_ligament(with_ligament, cx, phase_dynamics): NonLinearProgram.add( ocp, "dynamics_type", - Dynamics(DynamicsFcn.TORQUE_DRIVEN, rigidbody_dynamics=RigidBodyDynamics.ODE, with_ligament=with_ligament), + Dynamics(DynamicsFcn.TORQUE_DRIVEN, with_ligament=with_ligament), False, ) phase_index = [i for i in range(ocp.n_phases)] @@ -258,7 +258,6 @@ def test_muscle_driven_with_ligament(with_ligament, cx, phase_dynamics): "dynamics_type", Dynamics( DynamicsFcn.MUSCLE_DRIVEN, - rigidbody_dynamics=RigidBodyDynamics.ODE, with_ligament=with_ligament, ), False, @@ -299,68 +298,3 @@ def test_muscle_driven_with_ligament(with_ligament, cx, phase_dynamics): [2.0584494e-02, 1.8340451e-01, -7.3880194e00, -9.0642142e01], decimal=6, ) - - -@pytest.mark.parametrize("phase_dynamics", [PhaseDynamics.SHARED_DURING_THE_PHASE, PhaseDynamics.ONE_PER_NODE]) -@pytest.mark.parametrize( - "rigidbody_dynamics", - [RigidBodyDynamics.DAE_FORWARD_DYNAMICS, RigidBodyDynamics.DAE_INVERSE_DYNAMICS], -) -def test_ocp_mass_ligament(rigidbody_dynamics, phase_dynamics): - from bioptim.examples.torque_driven_ocp import ocp_mass_with_ligament as ocp_module - - bioptim_folder = os.path.dirname(ocp_module.__file__) - - # Define the problem - biorbd_model_path = bioptim_folder + "/models/mass_point_with_ligament.bioMod" - - ocp = ocp_module.prepare_ocp( - biorbd_model_path, - rigidbody_dynamics=rigidbody_dynamics, - phase_dynamics=phase_dynamics, - n_threads=8 if phase_dynamics == PhaseDynamics.SHARED_DURING_THE_PHASE else 1, - expand_dynamics=True, - ) - solver = Solver.IPOPT() - - # solver.set_maximum_iterations(10) - sol = ocp.solve(solver) - - # Check some results - states = sol.decision_states(to_merge=SolutionMerge.NODES) - controls = sol.decision_controls(to_merge=SolutionMerge.NODES) - q, qdot, tau = states["q"], states["qdot"], controls["tau"] - - if rigidbody_dynamics == RigidBodyDynamics.DAE_INVERSE_DYNAMICS: - # initial and final position - npt.assert_almost_equal(q[:, 0], np.array([0.0])) - npt.assert_almost_equal(q[:, -1], np.array([0.0194773])) - # initial and final velocities - npt.assert_almost_equal(qdot[:, 0], np.array([0.0])) - npt.assert_almost_equal(qdot[:, -1], np.array([-2.3061592])) - # initial and final controls - npt.assert_almost_equal( - tau[:, 0], - np.array([2.158472e-16]), - decimal=6, - ) - npt.assert_almost_equal(tau[:, -1], np.array([1.423733e-17]), decimal=6) - - else: - # initial and final position - npt.assert_almost_equal(q[:, 0], np.array([0.0])) - npt.assert_almost_equal(q[:, -1], np.array([0.0194773])) - # initial and final velocities - npt.assert_almost_equal(qdot[:, 0], np.array([0.0])) - npt.assert_almost_equal(qdot[:, -1], np.array([-2.3061592])) - # initial and final controls - npt.assert_almost_equal( - tau[:, 0], - np.array([2.158472e-16]), - decimal=6, - ) - npt.assert_almost_equal( - tau[:, -1], - np.array([1.423733e-17]), - decimal=6, - ) diff --git a/tests/shard3/test_passive_torque.py b/tests/shard3/test_passive_torque.py index 95a8c592b..74591a605 100644 --- a/tests/shard3/test_passive_torque.py +++ b/tests/shard3/test_passive_torque.py @@ -39,8 +39,7 @@ def __init__(self, nlp, use_sx): @pytest.mark.parametrize("phase_dynamics", [PhaseDynamics.SHARED_DURING_THE_PHASE, PhaseDynamics.ONE_PER_NODE]) @pytest.mark.parametrize("cx", [MX, SX]) @pytest.mark.parametrize("with_passive_torque", [False, True]) -@pytest.mark.parametrize("rigidbody_dynamics", [RigidBodyDynamics.ODE]) -def test_torque_driven_with_passive_torque(with_passive_torque, cx, rigidbody_dynamics, phase_dynamics): +def test_torque_driven_with_passive_torque(with_passive_torque, cx, phase_dynamics): # Prepare the program nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx == SX)) nlp.model = BiorbdModel( @@ -65,7 +64,6 @@ def test_torque_driven_with_passive_torque(with_passive_torque, cx, rigidbody_dy "dynamics_type", Dynamics( DynamicsFcn.TORQUE_DRIVEN, - rigidbody_dynamics=rigidbody_dynamics, with_passive_torque=with_passive_torque, phase_dynamics=phase_dynamics, ), @@ -94,34 +92,16 @@ def test_torque_driven_with_passive_torque(with_passive_torque, cx, rigidbody_dy numerical_timeseries = [] time = np.random.rand(2) x_out = np.array(nlp.dynamics_func(time, states, controls, params, algebraic_states, numerical_timeseries)) - if rigidbody_dynamics == RigidBodyDynamics.ODE: - if with_passive_torque: - npt.assert_almost_equal( - x_out[:, 0], [0.6118529, 0.785176, 0.6075449, 0.8083973, -5.0261535, -10.5570666, 18.569191, 24.2237134] - ) - else: - npt.assert_almost_equal( - x_out[:, 0], - [0.61185289, 0.78517596, 0.60754485, 0.80839735, -0.30241366, -10.38503791, 1.60445173, 35.80238642], - ) - elif rigidbody_dynamics == RigidBodyDynamics.DAE_FORWARD_DYNAMICS: - if with_passive_torque: - npt.assert_almost_equal( - x_out[:, 0], [0.6118529, 0.785176, 0.6075449, 0.8083973, 0.3886773, 0.5426961, 0.7722448, 0.7290072] - ) - else: - npt.assert_almost_equal( - x_out[:, 0], [0.6118529, 0.785176, 0.6075449, 0.8083973, 0.3886773, 0.5426961, 0.7722448, 0.7290072] - ) - elif rigidbody_dynamics == RigidBodyDynamics.DAE_INVERSE_DYNAMICS: - if with_passive_torque: - npt.assert_almost_equal( - x_out[:, 0], [0.6118529, 0.785176, 0.6075449, 0.8083973, 0.3886773, 0.5426961, 0.7722448, 0.7290072] - ) - else: - npt.assert_almost_equal( - x_out[:, 0], [0.6118529, 0.785176, 0.6075449, 0.8083973, 0.3886773, 0.5426961, 0.7722448, 0.7290072] - ) + + if with_passive_torque: + npt.assert_almost_equal( + x_out[:, 0], [0.6118529, 0.785176, 0.6075449, 0.8083973, -5.0261535, -10.5570666, 18.569191, 24.2237134] + ) + else: + npt.assert_almost_equal( + x_out[:, 0], + [0.61185289, 0.78517596, 0.60754485, 0.80839735, -0.30241366, -10.38503791, 1.60445173, 35.80238642], + ) @pytest.mark.parametrize("phase_dynamics", [PhaseDynamics.SHARED_DURING_THE_PHASE, PhaseDynamics.ONE_PER_NODE]) @@ -344,8 +324,7 @@ def test_torque_activation_driven_with_passive_torque(with_passive_torque, with_ @pytest.mark.parametrize("phase_dynamics", [PhaseDynamics.SHARED_DURING_THE_PHASE, PhaseDynamics.ONE_PER_NODE]) @pytest.mark.parametrize("cx", [MX, SX]) @pytest.mark.parametrize("with_passive_torque", [False, True]) -@pytest.mark.parametrize("rigidbody_dynamics", [RigidBodyDynamics.ODE]) -def test_muscle_driven_with_passive_torque(with_passive_torque, rigidbody_dynamics, cx, phase_dynamics): +def test_muscle_driven_with_passive_torque(with_passive_torque, cx, phase_dynamics): # Prepare the program nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx == SX)) nlp.model = BiorbdModel(TestUtils.bioptim_folder() + "/examples/muscle_driven_ocp/models/arm26_with_contact.bioMod") @@ -368,7 +347,6 @@ def test_muscle_driven_with_passive_torque(with_passive_torque, rigidbody_dynami "dynamics_type", Dynamics( DynamicsFcn.MUSCLE_DRIVEN, - rigidbody_dynamics=rigidbody_dynamics, with_passive_torque=with_passive_torque, phase_dynamics=phase_dynamics, ), @@ -386,8 +364,6 @@ def test_muscle_driven_with_passive_torque(with_passive_torque, rigidbody_dynami np.random.seed(42) # Prepare the dynamics - if rigidbody_dynamics == RigidBodyDynamics.DAE_INVERSE_DYNAMICS: - pass nlp.numerical_timeseries = TestUtils.initialize_numerical_timeseries(nlp, dynamics=nlp.dynamics_type) ConfigureProblem.initialize(ocp, nlp) @@ -400,47 +376,30 @@ def test_muscle_driven_with_passive_torque(with_passive_torque, rigidbody_dynami time = np.random.rand(2) x_out = np.array(nlp.dynamics_func(time, states, controls, params, algebraic_states, numerical_timeseries)) - if rigidbody_dynamics == RigidBodyDynamics.DAE_INVERSE_DYNAMICS: - if with_passive_torque: - npt.assert_almost_equal( - x_out[:, 0], - [0.183405, 0.611853, 0.785176, 0.388677, 0.542696, 0.772245], - decimal=6, - ) - else: - npt.assert_almost_equal( - x_out[:, 0], - [0.183405, 0.611853, 0.785176, 0.388677, 0.542696, 0.772245], - decimal=6, - ) + if with_passive_torque: + npt.assert_almost_equal( + x_out[:, 0], + [ + 1.8340450985e-01, + 6.1185289472e-01, + 7.8517596139e-01, + -5.3408086130e00, + 1.6890917494e02, + -5.4766884856e02, + ], + decimal=6, + ) else: - if with_passive_torque: - npt.assert_almost_equal( - x_out[:, 0], - [ - 1.8340450985e-01, - 6.1185289472e-01, - 7.8517596139e-01, - -5.3408086130e00, - 1.6890917494e02, - -5.4766884856e02, - ], - decimal=6, - ) - else: - npt.assert_almost_equal( - x_out[:, 0], - [1.83404510e-01, 6.11852895e-01, 7.85175961e-01, -4.37708456e00, 1.33221135e02, -4.71307550e02], - decimal=6, - ) + npt.assert_almost_equal( + x_out[:, 0], + [1.83404510e-01, 6.11852895e-01, 7.85175961e-01, -4.37708456e00, 1.33221135e02, -4.71307550e02], + decimal=6, + ) @pytest.mark.parametrize("phase_dynamics", [PhaseDynamics.SHARED_DURING_THE_PHASE, PhaseDynamics.ONE_PER_NODE]) -@pytest.mark.parametrize( - "rigidbody_dynamics", [RigidBodyDynamics.DAE_FORWARD_DYNAMICS, RigidBodyDynamics.DAE_INVERSE_DYNAMICS] -) @pytest.mark.parametrize("with_passive_torque", [False, True]) -def test_pendulum_passive_torque(rigidbody_dynamics, with_passive_torque, phase_dynamics): +def test_pendulum_passive_torque(with_passive_torque, phase_dynamics): from bioptim.examples.torque_driven_ocp import pendulum_with_passive_torque as ocp_module bioptim_folder = os.path.dirname(ocp_module.__file__) @@ -454,7 +413,6 @@ def test_pendulum_passive_torque(rigidbody_dynamics, with_passive_torque, phase_ biorbd_model_path, final_time, n_shooting, - rigidbody_dynamics=rigidbody_dynamics, with_passive_torque=with_passive_torque, phase_dynamics=phase_dynamics, expand_dynamics=True, @@ -469,76 +427,40 @@ def test_pendulum_passive_torque(rigidbody_dynamics, with_passive_torque, phase_ controls = sol.decision_controls(to_merge=SolutionMerge.NODES) q, qdot, tau = states["q"], states["qdot"], controls["tau"] - if rigidbody_dynamics == RigidBodyDynamics.DAE_INVERSE_DYNAMICS: - if with_passive_torque: - # initial and final position - npt.assert_almost_equal(q[:, 0], np.array([0.0, 0.0])) - npt.assert_almost_equal(q[:, -1], np.array([0.0, 3.14])) - # initial and final velocities - npt.assert_almost_equal(qdot[:, 0], np.array([0.0, 0.0])) - npt.assert_almost_equal(qdot[:, -1], np.array([0.0, 0.0])) - # initial and final controls - npt.assert_almost_equal( - tau[:, 0], - np.array([-1.071535, 0.0]), - decimal=6, - ) - npt.assert_almost_equal(tau[:, -1], np.array([-19.422394, 0.0]), decimal=6) - - else: - # initial and final position - npt.assert_almost_equal(q[:, 0], np.array([0.0, 0.0])) - npt.assert_almost_equal(q[:, -1], np.array([0.0, 3.14])) - # initial and final velocities - npt.assert_almost_equal(qdot[:, 0], np.array([0.0, 0.0])) - npt.assert_almost_equal(qdot[:, -1], np.array([0.0, 0.0])) - # initial and final controls - npt.assert_almost_equal( - tau[:, 0], - np.array([2.531529, 0.0]), - decimal=6, - ) - npt.assert_almost_equal( - tau[:, -1], - np.array([-18.254416, 0.0]), - decimal=6, - ) + if with_passive_torque: + # initial and final position + npt.assert_almost_equal(q[:, 0], np.array([0.0, 0.0])) + npt.assert_almost_equal(q[:, -1], np.array([0.0, 3.14])) + # initial and final velocities + npt.assert_almost_equal(qdot[:, 0], np.array([0.0, 0.0])) + npt.assert_almost_equal(qdot[:, -1], np.array([0.0, 0.0])) + # initial and final controls + npt.assert_almost_equal( + tau[:, 0], + np.array([1.587319, 0.0]), + decimal=6, + ) + npt.assert_almost_equal( + tau[:, -1], + np.array([-39.19793, 0.0]), + decimal=6, + ) else: - if with_passive_torque: - # initial and final position - npt.assert_almost_equal(q[:, 0], np.array([0.0, 0.0])) - npt.assert_almost_equal(q[:, -1], np.array([0.0, 3.14])) - # initial and final velocities - npt.assert_almost_equal(qdot[:, 0], np.array([0.0, 0.0])) - npt.assert_almost_equal(qdot[:, -1], np.array([0.0, 0.0])) - # initial and final controls - npt.assert_almost_equal( - tau[:, 0], - np.array([1.587319, 0.0]), - decimal=6, - ) - npt.assert_almost_equal( - tau[:, -1], - np.array([-39.19793, 0.0]), - decimal=6, - ) - - else: - # initial and final position - npt.assert_almost_equal(q[:, 0], np.array([0.0, 0.0])) - npt.assert_almost_equal(q[:, -1], np.array([0.0, 3.14])) - # initial and final velocities - npt.assert_almost_equal(qdot[:, 0], np.array([0.0, 0.0])) - npt.assert_almost_equal(qdot[:, -1], np.array([0.0, 0.0])) - # initial and final controls - npt.assert_almost_equal( - tau[:, 0], - np.array([2.606971, 0.0]), - decimal=6, - ) - npt.assert_almost_equal( - tau[:, -1], - np.array([-24.611219, 0.0]), - decimal=6, - ) + # initial and final position + npt.assert_almost_equal(q[:, 0], np.array([0.0, 0.0])) + npt.assert_almost_equal(q[:, -1], np.array([0.0, 3.14])) + # initial and final velocities + npt.assert_almost_equal(qdot[:, 0], np.array([0.0, 0.0])) + npt.assert_almost_equal(qdot[:, -1], np.array([0.0, 0.0])) + # initial and final controls + npt.assert_almost_equal( + tau[:, 0], + np.array([2.606971, 0.0]), + decimal=6, + ) + npt.assert_almost_equal( + tau[:, -1], + np.array([-24.611219, 0.0]), + decimal=6, + ) diff --git a/tests/shard4/test_penalty.py b/tests/shard4/test_penalty.py index fd40b2968..1cf0a7c25 100644 --- a/tests/shard4/test_penalty.py +++ b/tests/shard4/test_penalty.py @@ -55,13 +55,11 @@ def prepare_test_ocp( segments_to_apply_forces_in_global=["Seg1", "Seg1"], ) dynamics = DynamicsList() - rigidbody_dynamics = RigidBodyDynamics.DAE_INVERSE_DYNAMICS if implicit else RigidBodyDynamics.ODE dynamics.add( DynamicsFcn.TORQUE_DRIVEN, with_contact=True, expand_dynamics=True, phase_dynamics=phase_dynamics, - rigidbody_dynamics=rigidbody_dynamics, ) elif with_actuator: bio_model = BiorbdModel(bioptim_folder + "/examples/torque_driven_ocp/models/cube.bioMod") From 4ee879c8923898b166a04178a5655b56e5bed1a0 Mon Sep 17 00:00:00 2001 From: eve-mac Date: Thu, 24 Oct 2024 14:54:48 -0400 Subject: [PATCH 053/108] fixed external_forces in tests/examples --- .../example_external_forces.py | 5 - .../maximize_predicted_height_CoM.py | 2 +- .../pendulum_with_passive_torque.py | 1 - .../examples/torque_driven_ocp/spring_load.py | 2 +- tests/shard1/test_dynamics.py | 327 +++++++++++------- ...t_global_torque_driven_with_contact_ocp.py | 2 +- tests/shard3/test_ligaments.py | 1 - tests/shard3/test_passive_torque.py | 1 - tests/shard4/test_penalty.py | 2 - 9 files changed, 203 insertions(+), 140 deletions(-) diff --git a/bioptim/examples/getting_started/example_external_forces.py b/bioptim/examples/getting_started/example_external_forces.py index 6929354e3..0c0c2f77a 100644 --- a/bioptim/examples/getting_started/example_external_forces.py +++ b/bioptim/examples/getting_started/example_external_forces.py @@ -4,11 +4,6 @@ The solver must minimize the force needed to lift the box while reaching the marker in time. It is designed to show how to use external forces. An example of external forces that depends on the state (for example a spring) can be found at 'examples/torque_driven_ocp/spring_load.py' - -Please note that the point of application of the external forces are defined from the name of the segment in the bioMod. -It is expected to act on a segment in the global_reference_frame. Bioptim expects an array of shape [9, nb_external_forces, n_shooting+1] -where the three first components are the moments, the three next components are the forces and the three last components are the point of application (Mx, My, Mz, Fx, Fy, Fz, Px, Py, Pz) -You should also specify the name of the segments where the external forces are applied the list "segments_to_apply_forces_in_global". """ import platform diff --git a/bioptim/examples/torque_driven_ocp/maximize_predicted_height_CoM.py b/bioptim/examples/torque_driven_ocp/maximize_predicted_height_CoM.py index bb739c297..32b3c431d 100644 --- a/bioptim/examples/torque_driven_ocp/maximize_predicted_height_CoM.py +++ b/bioptim/examples/torque_driven_ocp/maximize_predicted_height_CoM.py @@ -75,7 +75,7 @@ def prepare_ocp( The OptimalControlProgram ready to be solved """ - bio_model = BiorbdModel(biorbd_model_path, segments_to_apply_translational_forces=["Seg1", "Seg1"]) + bio_model = BiorbdModel(biorbd_model_path) tau_min, tau_max = (-1, 1) if use_actuators else (-500, 500) dof_mapping = BiMappingList() diff --git a/bioptim/examples/torque_driven_ocp/pendulum_with_passive_torque.py b/bioptim/examples/torque_driven_ocp/pendulum_with_passive_torque.py index 076d00d29..b7245ee3d 100644 --- a/bioptim/examples/torque_driven_ocp/pendulum_with_passive_torque.py +++ b/bioptim/examples/torque_driven_ocp/pendulum_with_passive_torque.py @@ -17,7 +17,6 @@ CostType, Solver, BiorbdModel, - RigidBodyDynamics, BoundsList, PhaseDynamics, ) diff --git a/bioptim/examples/torque_driven_ocp/spring_load.py b/bioptim/examples/torque_driven_ocp/spring_load.py index 47a9c8e05..960457851 100644 --- a/bioptim/examples/torque_driven_ocp/spring_load.py +++ b/bioptim/examples/torque_driven_ocp/spring_load.py @@ -182,7 +182,7 @@ def prepare_ocp( scenario=1, ): # BioModel path - m = BiorbdModel(biorbd_model_path, segments_to_apply_forces_in_global=["Point"]) + m = BiorbdModel(biorbd_model_path) m.set_gravity(np.array((0, 0, 0))) weight = 1 diff --git a/tests/shard1/test_dynamics.py b/tests/shard1/test_dynamics.py index 68aa177bb..5d6d53895 100644 --- a/tests/shard1/test_dynamics.py +++ b/tests/shard1/test_dynamics.py @@ -20,6 +20,9 @@ ParameterContainer, ParameterList, PhaseDynamics, + ExternalForcesList, + ExternalForcesType, + ReferenceFrame, ) from tests.utils import TestUtils @@ -44,36 +47,12 @@ def __init__(self, nlp, use_sx): def test_torque_driven(with_contact, with_external_force, cx, phase_dynamics): # Prepare the program nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx == SX)) - - segments_to_apply_forces_in_global = None - segments_to_apply_translational_forces = None - - if with_external_force: - segments_to_apply_forces_in_global = ["Seg0"] - - nlp.model = BiorbdModel( - TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod", - segments_to_apply_forces_in_global=segments_to_apply_forces_in_global, - segments_to_apply_translational_forces=segments_to_apply_translational_forces, - ) - nlp.ns = 5 - nlp.cx = cx - nlp.time_cx = cx.sym("time", 1, 1) - nlp.dt = cx.sym("dt", 1, 1) - nlp.initialize(cx) - - nlp.x_bounds = np.zeros((nlp.model.nb_q * 3, 1)) - nlp.u_bounds = np.zeros((nlp.model.nb_q, 1)) - nlp.x_scaling = VariableScalingList() - nlp.xdot_scaling = VariableScalingList() - nlp.u_scaling = VariableScalingList() - nlp.a_scaling = VariableScalingList() external_forces = None if with_external_force: - external_forces = np.zeros((9, 1, nlp.ns + 1)) - external_forces[:, 0, 0] = [ + external_forces_array = np.zeros((9, nlp.ns + 1)) + external_forces_array[:, 0] = [ 0.374540118847362, 0.950714306409916, 0.731993941811405, @@ -84,7 +63,7 @@ def test_torque_driven(with_contact, with_external_force, cx, phase_dynamics): 0, 0, ] - external_forces[:, 0, 1] = [ + external_forces_array[:, 1] = [ 0.058083612168199, 0.866176145774935, 0.601115011743209, @@ -95,7 +74,7 @@ def test_torque_driven(with_contact, with_external_force, cx, phase_dynamics): 0, 0, ] - external_forces[:, 0, 2] = [ + external_forces_array[:, 2] = [ 0.832442640800422, 0.212339110678276, 0.181824967207101, @@ -106,7 +85,7 @@ def test_torque_driven(with_contact, with_external_force, cx, phase_dynamics): 0, 0, ] - external_forces[:, 0, 3] = [ + external_forces_array[:, 3] = [ 0.431945018642116, 0.291229140198042, 0.611852894722379, @@ -117,7 +96,7 @@ def test_torque_driven(with_contact, with_external_force, cx, phase_dynamics): 0, 0, ] - external_forces[:, 0, 4] = [ + external_forces_array[:, 4] = [ 0.456069984217036, 0.785175961393014, 0.19967378215836, @@ -129,6 +108,36 @@ def test_torque_driven(with_contact, with_external_force, cx, phase_dynamics): 0, ] + external_forces = ExternalForcesList() + external_forces.add(key="Seg0", + data=external_forces_array[:3, :], + force_type=ExternalForcesType.TORQUE, + force_reference_frame=ReferenceFrame.GLOBAL, + phase=0) + external_forces.add(key="Seg0", + data=external_forces_array[3:6, :], + force_type=ExternalForcesType.LINEAR_FORCE, + force_reference_frame=ReferenceFrame.GLOBAL, + phase=0) + + + nlp.model = BiorbdModel( + TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod", + external_forces=external_forces, + ) + + nlp.cx = cx + nlp.time_cx = cx.sym("time", 1, 1) + nlp.dt = cx.sym("dt", 1, 1) + nlp.initialize(cx) + + nlp.x_bounds = np.zeros((nlp.model.nb_q * 3, 1)) + nlp.u_bounds = np.zeros((nlp.model.nb_q, 1)) + nlp.x_scaling = VariableScalingList() + nlp.xdot_scaling = VariableScalingList() + nlp.u_scaling = VariableScalingList() + nlp.a_scaling = VariableScalingList() + ocp = OptimalControlProgram(nlp, use_sx=(cx == SX)) nlp.control_type = ControlType.CONSTANT NonLinearProgram.add( @@ -139,8 +148,8 @@ def test_torque_driven(with_contact, with_external_force, cx, phase_dynamics): with_contact=with_contact, expand_dynamics=True, phase_dynamics=phase_dynamics, - numerical_data_timeseries=({"forces_in_global": external_forces} if external_forces is not None else None), - ), + external_forces=external_forces, + ), False, ) phase_index = [i for i in range(ocp.n_phases)] @@ -164,7 +173,7 @@ def test_torque_driven(with_contact, with_external_force, cx, phase_dynamics): controls = np.random.rand(nlp.controls.shape, nlp.ns) params = np.random.rand(nlp.parameters.shape, nlp.ns) algebraic_states = np.random.rand(nlp.algebraic_states.shape, nlp.ns) - numerical_timeseries = external_forces[:, 0, 0] if with_external_force else [] + numerical_timeseries = external_forces_array[:, 0] if with_external_force else [] time = np.random.rand(2) x_out = np.array( nlp.dynamics_func( @@ -296,29 +305,12 @@ def test_torque_driven_soft_contacts_dynamics(with_contact, cx, implicit_contact def test_torque_derivative_driven(with_contact, with_external_force, cx, phase_dynamics): # Prepare the program nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx == SX)) - nlp.model = BiorbdModel( - TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod", - segments_to_apply_forces_in_global=["Seg0"], - ) nlp.ns = 5 - nlp.cx = cx - nlp.time_cx = cx.sym("time", 1, 1) - nlp.dt = cx.sym("dt", 1, 1) - nlp.initialize(cx) - nlp.x_bounds = np.zeros((nlp.model.nb_q * 3, 1)) - nlp.u_bounds = np.zeros((nlp.model.nb_q, 1)) - nlp.x_scaling = VariableScalingList() - nlp.xdot_scaling = VariableScalingList() - nlp.u_scaling = VariableScalingList() - nlp.a_scaling = VariableScalingList() - - ocp = OptimalControlProgram(nlp, use_sx=(cx == SX)) - nlp.control_type = ControlType.CONSTANT external_forces = None if with_external_force: - external_forces = np.zeros((9, 1, nlp.ns + 1)) - external_forces[:, 0, 0] = [ + external_forces_array = np.zeros((9, nlp.ns + 1)) + external_forces_array[:, 0] = [ 0.3745401188473625, 0.9507143064099162, 0.7319939418114051, @@ -329,7 +321,7 @@ def test_torque_derivative_driven(with_contact, with_external_force, cx, phase_d 0, 0, ] - external_forces[:, 0, 1] = [ + external_forces_array[:, 1] = [ 0.05808361216819946, 0.8661761457749352, 0.6011150117432088, @@ -340,7 +332,7 @@ def test_torque_derivative_driven(with_contact, with_external_force, cx, phase_d 0, 0, ] - external_forces[:, 0, 2] = [ + external_forces_array[:, 2] = [ 0.8324426408004217, 0.21233911067827616, 0.18182496720710062, @@ -351,7 +343,7 @@ def test_torque_derivative_driven(with_contact, with_external_force, cx, phase_d 0, 0, ] - external_forces[:, 0, 3] = [ + external_forces_array[:, 3] = [ 0.43194501864211576, 0.2912291401980419, 0.6118528947223795, @@ -362,7 +354,7 @@ def test_torque_derivative_driven(with_contact, with_external_force, cx, phase_d 0, 0, ] - external_forces[:, 0, 4] = [ + external_forces_array[:, 4] = [ 0.45606998421703593, 0.7851759613930136, 0.19967378215835974, @@ -373,6 +365,37 @@ def test_torque_derivative_driven(with_contact, with_external_force, cx, phase_d 0, 0, ] + external_forces = ExternalForcesList() + external_forces.add( + key="Seg0", + data=external_forces_array[:3, :], + force_type=ExternalForcesType.TORQUE, + force_reference_frame=ReferenceFrame.GLOBAL, + phase=0) + external_forces.add( + key="Seg0", + data=external_forces_array[3:6, :], + force_type=ExternalForcesType.LINEAR_FORCE, + force_reference_frame=ReferenceFrame.GLOBAL, + phase=0) + + nlp.model = BiorbdModel( + TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod", + external_forces=external_forces, + ) + nlp.cx = cx + nlp.time_cx = cx.sym("time", 1, 1) + nlp.dt = cx.sym("dt", 1, 1) + nlp.initialize(cx) + nlp.x_bounds = np.zeros((nlp.model.nb_q * 3, 1)) + nlp.u_bounds = np.zeros((nlp.model.nb_q, 1)) + nlp.x_scaling = VariableScalingList() + nlp.xdot_scaling = VariableScalingList() + nlp.u_scaling = VariableScalingList() + nlp.a_scaling = VariableScalingList() + + ocp = OptimalControlProgram(nlp, use_sx=(cx == SX)) + nlp.control_type = ControlType.CONSTANT NonLinearProgram.add( ocp, @@ -382,7 +405,7 @@ def test_torque_derivative_driven(with_contact, with_external_force, cx, phase_d with_contact=with_contact, expand_dynamics=True, phase_dynamics=phase_dynamics, - numerical_data_timeseries=({"forces_in_global": external_forces} if external_forces is not None else None), + external_forces=external_forces, ), False, ) @@ -408,7 +431,7 @@ def test_torque_derivative_driven(with_contact, with_external_force, cx, phase_d controls = np.random.rand(nlp.controls.shape, nlp.ns) params = np.random.rand(nlp.parameters.shape, nlp.ns) algebraic_states = np.random.rand(nlp.algebraic_states.shape, nlp.ns) - numerical_timeseries = external_forces[:, 0, 0] if with_external_force else [] + numerical_timeseries = external_forces_array[:, 0] if with_external_force else [] time = np.random.rand(2) x_out = np.array( nlp.dynamics_func( @@ -651,27 +674,12 @@ def test_soft_contacts_dynamics_errors(dynamics, phase_dynamics): def test_torque_activation_driven(with_contact, with_external_force, cx, phase_dynamics): # Prepare the program nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx == SX)) - nlp.model = BiorbdModel( - TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod", - segments_to_apply_forces_in_global=["Seg0"], - ) nlp.ns = 5 - nlp.cx = cx - nlp.time_cx = cx.sym("time", 1, 1) - nlp.dt = cx.sym("dt", 1, 1) - nlp.initialize(cx) - - nlp.x_bounds = np.zeros((nlp.model.nb_q * 2, 1)) - nlp.u_bounds = np.zeros((nlp.model.nb_q, 1)) - nlp.x_scaling = VariableScalingList() - nlp.xdot_scaling = VariableScalingList() - nlp.u_scaling = VariableScalingList() - nlp.a_scaling = VariableScalingList() external_forces = None if with_external_force: - external_forces = np.zeros((9, 1, nlp.ns + 1)) - external_forces[:, 0, 0] = [ + external_forces_array = np.zeros((9, nlp.ns + 1)) + external_forces_array[:, 0] = [ 0.3745401188473625, 0.9507143064099162, 0.7319939418114051, @@ -682,7 +690,7 @@ def test_torque_activation_driven(with_contact, with_external_force, cx, phase_d 0, 0, ] - external_forces[:, 0, 1] = [ + external_forces_array[:, 1] = [ 0.05808361216819946, 0.8661761457749352, 0.6011150117432088, @@ -693,7 +701,7 @@ def test_torque_activation_driven(with_contact, with_external_force, cx, phase_d 0, 0, ] - external_forces[:, 0, 2] = [ + external_forces_array[:, 2] = [ 0.8324426408004217, 0.21233911067827616, 0.18182496720710062, @@ -704,7 +712,7 @@ def test_torque_activation_driven(with_contact, with_external_force, cx, phase_d 0, 0, ] - external_forces[:, 0, 3] = [ + external_forces_array[:, 3] = [ 0.43194501864211576, 0.2912291401980419, 0.6118528947223795, @@ -715,7 +723,7 @@ def test_torque_activation_driven(with_contact, with_external_force, cx, phase_d 0, 0, ] - external_forces[:, 0, 4] = [ + external_forces_array[:, 4] = [ 0.45606998421703593, 0.7851759613930136, 0.19967378215835974, @@ -726,6 +734,38 @@ def test_torque_activation_driven(with_contact, with_external_force, cx, phase_d 0, 0, ] + external_forces = ExternalForcesList() + external_forces.add( + key="Seg0", + data=external_forces_array[:3, :], + force_type=ExternalForcesType.TORQUE, + force_reference_frame=ReferenceFrame.GLOBAL, + phase=0, + ) + external_forces.add( + key="Seg0", + data=external_forces_array[3:6, :], + force_type=ExternalForcesType.LINEAR_FORCE, + force_reference_frame=ReferenceFrame.GLOBAL, + phase=0, + ) + + + nlp.model = BiorbdModel( + TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod", + external_forces=external_forces, + ) + nlp.cx = cx + nlp.time_cx = cx.sym("time", 1, 1) + nlp.dt = cx.sym("dt", 1, 1) + nlp.initialize(cx) + + nlp.x_bounds = np.zeros((nlp.model.nb_q * 2, 1)) + nlp.u_bounds = np.zeros((nlp.model.nb_q, 1)) + nlp.x_scaling = VariableScalingList() + nlp.xdot_scaling = VariableScalingList() + nlp.u_scaling = VariableScalingList() + nlp.a_scaling = VariableScalingList() ocp = OptimalControlProgram(nlp, use_sx=(cx == SX)) nlp.control_type = ControlType.CONSTANT @@ -737,7 +777,7 @@ def test_torque_activation_driven(with_contact, with_external_force, cx, phase_d with_contact=with_contact, expand_dynamics=True, phase_dynamics=phase_dynamics, - numerical_data_timeseries=({"forces_in_global": external_forces} if external_forces is not None else None), + external_forces=external_forces, ), False, ) @@ -762,7 +802,7 @@ def test_torque_activation_driven(with_contact, with_external_force, cx, phase_d controls = np.random.rand(nlp.controls.shape, nlp.ns) params = np.random.rand(nlp.parameters.shape, nlp.ns) algebraic_states = np.random.rand(nlp.algebraic_states.shape, nlp.ns) - numerical_timeseries = external_forces[:, 0, 0] if with_external_force else [] + numerical_timeseries = external_forces_array[:, 0] if with_external_force else [] time = np.random.rand(2) x_out = np.array( nlp.dynamics_func( @@ -832,26 +872,12 @@ def test_torque_activation_driven_with_residual_torque( ): # Prepare the program nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx == SX)) - nlp.model = BiorbdModel( - TestUtils.bioptim_folder() + "/examples/torque_driven_ocp/models/2segments_2dof_2contacts.bioMod", - segments_to_apply_forces_in_global=["Seg0"], - ) nlp.ns = 5 - nlp.cx = cx - nlp.time_cx = cx.sym("time", 1, 1) - nlp.dt = cx.sym("dt", 1, 1) - nlp.initialize(cx) - nlp.x_bounds = np.zeros((nlp.model.nb_q * 2, 1)) - nlp.u_bounds = np.zeros((nlp.model.nb_q, 1)) - nlp.x_scaling = VariableScalingList() - nlp.xdot_scaling = VariableScalingList() - nlp.u_scaling = VariableScalingList() - nlp.a_scaling = VariableScalingList() external_forces = None if with_external_force: - external_forces = np.zeros((9, 1, nlp.ns + 1)) - external_forces[:, 0, 0] = [ + external_forces_array = np.zeros((9, nlp.ns + 1)) + external_forces_array[:, 0] = [ 0.3745401188473625, 0.9507143064099162, 0.7319939418114051, @@ -862,7 +888,7 @@ def test_torque_activation_driven_with_residual_torque( 0, 0, ] - external_forces[:, 0, 1] = [ + external_forces_array[:, 1] = [ 0.05808361216819946, 0.8661761457749352, 0.6011150117432088, @@ -873,7 +899,7 @@ def test_torque_activation_driven_with_residual_torque( 0, 0, ] - external_forces[:, 0, 2] = [ + external_forces_array[:, 2] = [ 0.8324426408004217, 0.21233911067827616, 0.18182496720710062, @@ -884,7 +910,7 @@ def test_torque_activation_driven_with_residual_torque( 0, 0, ] - external_forces[:, 0, 3] = [ + external_forces_array[:, 3] = [ 0.43194501864211576, 0.2912291401980419, 0.6118528947223795, @@ -895,7 +921,7 @@ def test_torque_activation_driven_with_residual_torque( 0, 0, ] - external_forces[:, 0, 4] = [ + external_forces_array[:, 4] = [ 0.45606998421703593, 0.7851759613930136, 0.19967378215835974, @@ -906,6 +932,36 @@ def test_torque_activation_driven_with_residual_torque( 0, 0, ] + external_forces = ExternalForcesList() + external_forces.add( + key="Seg0", + data=external_forces_array[:3, :], + force_type=ExternalForcesType.TORQUE, + force_reference_frame=ReferenceFrame.GLOBAL, + phase=0, + ) + external_forces.add( + key="Seg0", + data=external_forces_array[3:6, :], + force_type=ExternalForcesType.LINEAR_FORCE, + force_reference_frame=ReferenceFrame.GLOBAL, + phase=0, + ) + + nlp.model = BiorbdModel( + TestUtils.bioptim_folder() + "/examples/torque_driven_ocp/models/2segments_2dof_2contacts.bioMod", + external_forces=external_forces, + ) + nlp.cx = cx + nlp.time_cx = cx.sym("time", 1, 1) + nlp.dt = cx.sym("dt", 1, 1) + nlp.initialize(cx) + nlp.x_bounds = np.zeros((nlp.model.nb_q * 2, 1)) + nlp.u_bounds = np.zeros((nlp.model.nb_q, 1)) + nlp.x_scaling = VariableScalingList() + nlp.xdot_scaling = VariableScalingList() + nlp.u_scaling = VariableScalingList() + nlp.a_scaling = VariableScalingList() ocp = OptimalControlProgram(nlp, use_sx=(cx == SX)) nlp.control_type = ControlType.CONSTANT @@ -917,7 +973,7 @@ def test_torque_activation_driven_with_residual_torque( with_residual_torque=with_residual_torque, expand_dynamics=True, phase_dynamics=phase_dynamics, - numerical_data_timeseries=({"forces_in_global": external_forces} if external_forces is not None else None), + external_forces=external_forces, ), False, ) @@ -942,7 +998,7 @@ def test_torque_activation_driven_with_residual_torque( controls = np.random.rand(nlp.controls.shape, nlp.ns) params = np.random.rand(nlp.parameters.shape, nlp.ns) algebraic_states = np.random.rand(nlp.algebraic_states.shape, nlp.ns) - numerical_timeseries = external_forces[:, 0, 0] if with_external_force else [] + numerical_timeseries = external_forces_array[:, 0] if with_external_force else [] time = np.random.rand(2) x_out = np.array( nlp.dynamics_func( @@ -1077,28 +1133,12 @@ def test_muscle_driven( ): # Prepare the program nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx == SX)) - nlp.model = BiorbdModel( - TestUtils.bioptim_folder() + "/examples/muscle_driven_ocp/models/arm26_with_contact.bioMod", - segments_to_apply_forces_in_global=["r_ulna_radius_hand_rotation1"], - ) nlp.ns = 5 - nlp.cx = cx - nlp.time_cx = cx.sym("time", 1, 1) - nlp.dt = cx.sym("dt", 1, 1) - nlp.initialize(cx) - - nlp.x_bounds = np.zeros((nlp.model.nb_q * 2 + nlp.model.nb_muscles, 1)) - nlp.u_bounds = np.zeros((nlp.model.nb_muscles, 1)) - nlp.x_scaling = VariableScalingList() - nlp.xdot_scaling = VariableScalingList() - nlp.u_scaling = VariableScalingList() - nlp.a_scaling = VariableScalingList() - nlp.phase_idx = 0 external_forces = None if with_external_force: - external_forces = np.zeros((9, 1, nlp.ns + 1)) - external_forces[:, 0, 0] = [ + external_forces_array = np.zeros((9, nlp.ns + 1)) + external_forces_array[:, 0] = [ 0.3745401188473625, 0.9507143064099162, 0.7319939418114051, @@ -1109,7 +1149,7 @@ def test_muscle_driven( 0, 0, ] - external_forces[:, 0, 1] = [ + external_forces_array[:, 1] = [ 0.05808361216819946, 0.8661761457749352, 0.6011150117432088, @@ -1120,7 +1160,7 @@ def test_muscle_driven( 0, 0, ] - external_forces[:, 0, 2] = [ + external_forces_array[:, 2] = [ 0.8324426408004217, 0.21233911067827616, 0.18182496720710062, @@ -1131,7 +1171,7 @@ def test_muscle_driven( 0, 0, ] - external_forces[:, 0, 3] = [ + external_forces_array[:, 3] = [ 0.43194501864211576, 0.2912291401980419, 0.6118528947223795, @@ -1142,7 +1182,7 @@ def test_muscle_driven( 0, 0, ] - external_forces[:, 0, 4] = [ + external_forces_array[:, 4] = [ 0.45606998421703593, 0.7851759613930136, 0.19967378215835974, @@ -1153,6 +1193,39 @@ def test_muscle_driven( 0, 0, ] + external_forces = ExternalForcesList() + external_forces.add( + key="r_ulna_radius_hand_rotation1", + data=external_forces_array[:3, :], + force_type=ExternalForcesType.TORQUE, + force_reference_frame=ReferenceFrame.GLOBAL, + phase=0, + ) + external_forces.add( + key="r_ulna_radius_hand_rotation1", + data=external_forces_array[3:6, :], + force_type=ExternalForcesType.LINEAR_FORCE, + force_reference_frame=ReferenceFrame.GLOBAL, + phase=0, + ) + + nlp.model = BiorbdModel( + TestUtils.bioptim_folder() + "/examples/muscle_driven_ocp/models/arm26_with_contact.bioMod", + external_forces=external_forces, + ) + + nlp.cx = cx + nlp.time_cx = cx.sym("time", 1, 1) + nlp.dt = cx.sym("dt", 1, 1) + nlp.initialize(cx) + + nlp.x_bounds = np.zeros((nlp.model.nb_q * 2 + nlp.model.nb_muscles, 1)) + nlp.u_bounds = np.zeros((nlp.model.nb_muscles, 1)) + nlp.x_scaling = VariableScalingList() + nlp.xdot_scaling = VariableScalingList() + nlp.u_scaling = VariableScalingList() + nlp.a_scaling = VariableScalingList() + nlp.phase_idx = 0 ocp = OptimalControlProgram(nlp, use_sx=(cx == SX)) nlp.control_type = ControlType.CONSTANT @@ -1166,7 +1239,7 @@ def test_muscle_driven( with_contact=with_contact, expand_dynamics=True, phase_dynamics=phase_dynamics, - numerical_data_timeseries=({"forces_in_global": external_forces} if external_forces is not None else None), + external_forces=external_forces, ), False, ) @@ -1192,7 +1265,7 @@ def test_muscle_driven( controls = np.random.rand(nlp.controls.shape, nlp.ns) params = np.random.rand(nlp.parameters.shape, nlp.ns) algebraic_states = np.random.rand(nlp.algebraic_states.shape, nlp.ns) - numerical_timeseries = external_forces[:, 0, 0] if with_external_force else [] + numerical_timeseries = external_forces_array[:, 0] if with_external_force else [] time = np.random.rand(2) x_out = np.array( nlp.dynamics_func( diff --git a/tests/shard3/test_global_torque_driven_with_contact_ocp.py b/tests/shard3/test_global_torque_driven_with_contact_ocp.py index 19ac2af47..486d82a73 100644 --- a/tests/shard3/test_global_torque_driven_with_contact_ocp.py +++ b/tests/shard3/test_global_torque_driven_with_contact_ocp.py @@ -11,7 +11,7 @@ import numpy as np import numpy.testing as npt -from bioptim import OdeSolver, RigidBodyDynamics, Solver, PhaseDynamics, SolutionMerge +from bioptim import OdeSolver, Solver, PhaseDynamics, SolutionMerge from tests.utils import TestUtils diff --git a/tests/shard3/test_ligaments.py b/tests/shard3/test_ligaments.py index 0c11d608d..4f18fa633 100644 --- a/tests/shard3/test_ligaments.py +++ b/tests/shard3/test_ligaments.py @@ -8,7 +8,6 @@ from bioptim import ( ConfigureProblem, ControlType, - RigidBodyDynamics, BiorbdModel, NonLinearProgram, DynamicsFcn, diff --git a/tests/shard3/test_passive_torque.py b/tests/shard3/test_passive_torque.py index 74591a605..e98a7f284 100644 --- a/tests/shard3/test_passive_torque.py +++ b/tests/shard3/test_passive_torque.py @@ -8,7 +8,6 @@ from bioptim import ( ConfigureProblem, ControlType, - RigidBodyDynamics, BiorbdModel, NonLinearProgram, DynamicsFcn, diff --git a/tests/shard4/test_penalty.py b/tests/shard4/test_penalty.py index 1cf0a7c25..e4621e852 100644 --- a/tests/shard4/test_penalty.py +++ b/tests/shard4/test_penalty.py @@ -17,7 +17,6 @@ MultinodeConstraint, MultinodeObjective, Node, - RigidBodyDynamics, ControlType, PhaseDynamics, ConstraintList, @@ -52,7 +51,6 @@ def prepare_test_ocp( elif with_contact: bio_model = BiorbdModel( bioptim_folder + "/examples/muscle_driven_with_contact/models/2segments_4dof_2contacts_1muscle.bioMod", - segments_to_apply_forces_in_global=["Seg1", "Seg1"], ) dynamics = DynamicsList() dynamics.add( From ef9638d620b9767142ca042bb4539797852f9a88 Mon Sep 17 00:00:00 2001 From: eve-mac Date: Thu, 24 Oct 2024 14:55:00 -0400 Subject: [PATCH 054/108] blacked --- bioptim/dynamics/configure_problem.py | 8 ++--- bioptim/misc/enums.py | 1 + tests/shard1/test_dynamics.py | 43 ++++++++++++++------------- tests/shard1/test_enums.py | 2 +- 4 files changed, 27 insertions(+), 27 deletions(-) diff --git a/bioptim/dynamics/configure_problem.py b/bioptim/dynamics/configure_problem.py index ea71b00d7..b7cf0b50d 100644 --- a/bioptim/dynamics/configure_problem.py +++ b/bioptim/dynamics/configure_problem.py @@ -191,9 +191,7 @@ def torque_driven( """ _check_contacts_in_biorbd_model(with_contact, nlp.model.nb_contacts, nlp.phase_idx) - _check_soft_contacts_dynamics( - soft_contacts_dynamics, nlp.model.nb_soft_contacts, nlp.phase_idx - ) + _check_soft_contacts_dynamics(soft_contacts_dynamics, nlp.model.nb_soft_contacts, nlp.phase_idx) # Declared rigidbody states and controls ConfigureProblem.configure_q(ocp, nlp, as_states=True, as_controls=False) @@ -551,9 +549,7 @@ def torque_derivative_driven( """ _check_contacts_in_biorbd_model(with_contact, nlp.model.nb_contacts, nlp.phase_idx) - _check_soft_contacts_dynamics( - soft_contacts_dynamics, nlp.model.nb_soft_contacts, nlp.phase_idx - ) + _check_soft_contacts_dynamics(soft_contacts_dynamics, nlp.model.nb_soft_contacts, nlp.phase_idx) ConfigureProblem.configure_q(ocp, nlp, True, False) ConfigureProblem.configure_qdot(ocp, nlp, True, False) diff --git a/bioptim/misc/enums.py b/bioptim/misc/enums.py index 82c5c9ee6..00d08699e 100644 --- a/bioptim/misc/enums.py +++ b/bioptim/misc/enums.py @@ -194,6 +194,7 @@ class SoftContactDynamics(Enum): ODE = "ode" CONSTRAINT = "constraint" + class DefectType(Enum): EXPLICIT = "explicit" IMPLICIT = "implicit" diff --git a/tests/shard1/test_dynamics.py b/tests/shard1/test_dynamics.py index 5d6d53895..8629c9cdb 100644 --- a/tests/shard1/test_dynamics.py +++ b/tests/shard1/test_dynamics.py @@ -109,22 +109,25 @@ def test_torque_driven(with_contact, with_external_force, cx, phase_dynamics): ] external_forces = ExternalForcesList() - external_forces.add(key="Seg0", - data=external_forces_array[:3, :], - force_type=ExternalForcesType.TORQUE, - force_reference_frame=ReferenceFrame.GLOBAL, - phase=0) - external_forces.add(key="Seg0", - data=external_forces_array[3:6, :], - force_type=ExternalForcesType.LINEAR_FORCE, - force_reference_frame=ReferenceFrame.GLOBAL, - phase=0) - + external_forces.add( + key="Seg0", + data=external_forces_array[:3, :], + force_type=ExternalForcesType.TORQUE, + force_reference_frame=ReferenceFrame.GLOBAL, + phase=0, + ) + external_forces.add( + key="Seg0", + data=external_forces_array[3:6, :], + force_type=ExternalForcesType.LINEAR_FORCE, + force_reference_frame=ReferenceFrame.GLOBAL, + phase=0, + ) nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod", external_forces=external_forces, - ) + ) nlp.cx = cx nlp.time_cx = cx.sym("time", 1, 1) @@ -149,7 +152,7 @@ def test_torque_driven(with_contact, with_external_force, cx, phase_dynamics): expand_dynamics=True, phase_dynamics=phase_dynamics, external_forces=external_forces, - ), + ), False, ) phase_index = [i for i in range(ocp.n_phases)] @@ -219,6 +222,7 @@ def test_torque_driven(with_contact, with_external_force, cx, phase_dynamics): ], ) + @pytest.mark.parametrize("phase_dynamics", [PhaseDynamics.SHARED_DURING_THE_PHASE, PhaseDynamics.ONE_PER_NODE]) @pytest.mark.parametrize("cx", [MX, SX]) @pytest.mark.parametrize("with_contact", [False, True]) @@ -371,14 +375,16 @@ def test_torque_derivative_driven(with_contact, with_external_force, cx, phase_d data=external_forces_array[:3, :], force_type=ExternalForcesType.TORQUE, force_reference_frame=ReferenceFrame.GLOBAL, - phase=0) + phase=0, + ) external_forces.add( key="Seg0", data=external_forces_array[3:6, :], force_type=ExternalForcesType.LINEAR_FORCE, force_reference_frame=ReferenceFrame.GLOBAL, - phase=0) - + phase=0, + ) + nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod", external_forces=external_forces, @@ -750,7 +756,6 @@ def test_torque_activation_driven(with_contact, with_external_force, cx, phase_d phase=0, ) - nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod", external_forces=external_forces, @@ -1128,9 +1133,7 @@ def test_torque_driven_free_floating_base(cx, phase_dynamics): @pytest.mark.parametrize("with_contact", [False, True]) @pytest.mark.parametrize("with_residual_torque", [False, True]) @pytest.mark.parametrize("with_excitations", [False, True]) -def test_muscle_driven( - with_excitations, with_contact, with_residual_torque, with_external_force, cx, phase_dynamics -): +def test_muscle_driven(with_excitations, with_contact, with_residual_torque, with_external_force, cx, phase_dynamics): # Prepare the program nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx == SX)) nlp.ns = 5 diff --git a/tests/shard1/test_enums.py b/tests/shard1/test_enums.py index 017e46bdc..9e16e35e7 100644 --- a/tests/shard1/test_enums.py +++ b/tests/shard1/test_enums.py @@ -201,4 +201,4 @@ def test_reference_frame(): assert ReferenceFrame.LOCAL == "local" # verify the number of elements - assert len(ReferenceFrame) == 2 \ No newline at end of file + assert len(ReferenceFrame) == 2 From 84f52872532c4e2496c7f56452b39bc49ec89832 Mon Sep 17 00:00:00 2001 From: eve-mac Date: Thu, 24 Oct 2024 15:32:55 -0400 Subject: [PATCH 055/108] gitignore --- .gitignore | 3 - bioptim/misc/external_forces.py | 146 ++++++++++++++++++++++++++++++++ 2 files changed, 146 insertions(+), 3 deletions(-) create mode 100644 bioptim/misc/external_forces.py diff --git a/.gitignore b/.gitignore index 998c6911d..02f28c707 100644 --- a/.gitignore +++ b/.gitignore @@ -86,9 +86,6 @@ venv.bak/ # pycharm .idea -# Personnal stuff -Misc/ - # Remove symbolic links to bioptim in examples and tests folder examples/*/biorbd_optim examples/biorbd_optim diff --git a/bioptim/misc/external_forces.py b/bioptim/misc/external_forces.py new file mode 100644 index 000000000..938df6e4a --- /dev/null +++ b/bioptim/misc/external_forces.py @@ -0,0 +1,146 @@ +from typing import Any + +import numpy as np + +from ..misc.options import OptionGeneric, OptionDict +from ..misc.enums import ExternalForcesType, ReferenceFrame + +class ExternalForces(OptionGeneric): + + def __init__( + self, + key: str, + linear_force_data: np.ndarray, + torque_data: np.ndarray, + force_reference_frame: ReferenceFrame, + point_of_application_reference_frame: ReferenceFrame, + point_of_application: np.ndarray = None, + **extra_parameters: Any, + ): + super(ExternalForces, self).__init__(**extra_parameters) + if self.list_index != -1 and self.list_index is not None: + raise NotImplementedError("All external forces must be declared, list_index cannot be used for now.") + + if linear_force_data is not None and linear_force_data.shape[0] != 3: + raise ValueError(f"External forces must have 3 rows, got {linear_force_data.shape[0]}") + + if torque_data is not None and torque_data.shape[0] != 3: + raise ValueError(f"External torques must have 3 rows, got {torque_data.shape[0]}") + + self.key = key + self.linear_force_data = linear_force_data + self.torque_data = torque_data + self.force_reference_frame = force_reference_frame + self.point_of_application = point_of_application + self.point_of_application_reference_frame = point_of_application_reference_frame + + @property + def len(self): + """ + Returns the number of nodes in the external forces + """ + if self.linear_force_data is not None: + return self.linear_force_data.shape[1] + elif self.torque_data is not None: + return self.torque_data.shape[1] + else: + raise ValueError("External forces must have either linear_force_data or torque_data defined") + +class ExternalForcesList(OptionDict): + + def __init__(self, *args, **kwargs): + super(ExternalForcesList, self).__init__(sub_type=ExternalForces) + + def add( + self, + key: str, + phase: int, + data: np.ndarray, + force_type: ExternalForcesType, + force_reference_frame: ReferenceFrame, + point_of_application: np.ndarray = None, + point_of_application_reference_frame: ReferenceFrame = None, + **extra_arguments: Any, + ): + + if key in self and self[key].phase == phase and self[key].force_type == force_type: + raise ValueError( + f"There should be only one external force with the same key, force_type and phase (key:{key}, force_type: {force_type}, phase:{phase} already exists)") + + if force_reference_frame not in [ReferenceFrame.GLOBAL, ReferenceFrame.LOCAL]: + raise ValueError( + f"External force reference frame must be of type ReferenceFrame.GLOBAL or ReferenceFrame.LOCAL, got{force_reference_frame}") + + if point_of_application_reference_frame not in [ReferenceFrame.GLOBAL, ReferenceFrame.LOCAL, None]: + raise ValueError( + f"Point of application reference frame must be of type ReferenceFrame.GLOBAL or ReferenceFrame.LOCAL, got{point_of_application_reference_frame}") + + if point_of_application is not None: + if point_of_application.shape[0] != 3: + raise ValueError(f"Point of application must have 3 rows, got {point_of_application.shape[0]}") + if force_type == ExternalForcesType.TORQUE: + raise ValueError("Point of application cannot be used with ExternalForcesType.TORQUE") + + if force_type == ExternalForcesType.TORQUE and force_reference_frame != ReferenceFrame.GLOBAL: + raise ValueError("External torques are defined in global reference frame") + + if force_type == ExternalForcesType.TORQUE and key in self.keys(): + # Do not change the reference frame of the linear force + force_reference_frame = self[key].force_reference_frame + + linear_force_data = None + if key in self.keys() and self[key].linear_force_data is not None: + if force_type == ExternalForcesType.LINEAR_FORCE: + raise ValueError(f"The linear force is already defined for {key}") + else: + linear_force_data = self[key].linear_force_data + elif force_type == ExternalForcesType.LINEAR_FORCE: + linear_force_data = data + + torque_data = None + if key in self.keys() and self[key].torque_data is not None: + if force_type == ExternalForcesType.TORQUE: + raise ValueError(f"The torque is already defined for {key}") + else: + torque_data = self[key].torque_data + elif force_type == ExternalForcesType.TORQUE: + torque_data = data + + super(ExternalForcesList, self)._add( + key=key, + phase=phase, + linear_force_data=linear_force_data, + torque_data=torque_data, + force_reference_frame=force_reference_frame, + point_of_application=point_of_application, + point_of_application_reference_frame=point_of_application_reference_frame, + **extra_arguments, + ) + + def print(self): + raise NotImplementedError("Printing of ExternalForcesList is not ready yet") + +def get_external_forces_segments(external_forces: ExternalForcesList): + + segments_to_apply_forces_in_global = [] + segments_to_apply_forces_in_local = [] + segments_to_apply_translational_forces = [] + if external_forces is not None: + for key in external_forces.keys(): + force = external_forces[key] + # Check sanity first + if force.force_reference_frame == ReferenceFrame.GLOBAL and force.point_of_application_reference_frame == ReferenceFrame.LOCAL and force.torque_data is not None: + raise NotImplementedError("External forces in global reference frame cannot have a point of application in the local reference frame and torques defined at the same time yet") + elif force.force_reference_frame == ReferenceFrame.LOCAL and force.point_of_application_reference_frame == ReferenceFrame.GLOBAL: + raise NotImplementedError("External forces in local reference frame cannot have a point of application in the global reference frame yet") + + if force.force_reference_frame == ReferenceFrame.GLOBAL and (force.point_of_application_reference_frame == ReferenceFrame.GLOBAL or force.point_of_application_reference_frame is None): + segments_to_apply_forces_in_global.append(force.key) + elif force.force_reference_frame == ReferenceFrame.LOCAL and (force.point_of_application_reference_frame == ReferenceFrame.LOCAL or force.point_of_application_reference_frame is None): + segments_to_apply_forces_in_local.append(force.key) + elif force.force_reference_frame == ReferenceFrame.GLOBAL and force.point_of_application_reference_frame == ReferenceFrame.LOCAL: + segments_to_apply_translational_forces.append(force.key) + else: + raise ValueError("This should not happen") + + return segments_to_apply_forces_in_global, segments_to_apply_forces_in_local, segments_to_apply_translational_forces \ No newline at end of file From 39c21bc1f99f9cc66490b545a816cc1d1259381c Mon Sep 17 00:00:00 2001 From: eve-mac Date: Thu, 24 Oct 2024 16:26:53 -0400 Subject: [PATCH 056/108] shard 1 (-acados, -vector_orientation) --- .../examples/torque_driven_ocp/spring_load.py | 3 +- bioptim/models/biorbd/biorbd_model.py | 35 +++++----- bioptim/models/biorbd/multi_biorbd_model.py | 64 +++++++++++++++++-- tests/shard1/test_biorbd_multi_model.py | 1 - tests/shard1/test_dynamics_units.py | 8 +-- tests/shard1/test_enums.py | 8 +-- 6 files changed, 90 insertions(+), 29 deletions(-) diff --git a/bioptim/examples/torque_driven_ocp/spring_load.py b/bioptim/examples/torque_driven_ocp/spring_load.py index 960457851..e51cbfbc1 100644 --- a/bioptim/examples/torque_driven_ocp/spring_load.py +++ b/bioptim/examples/torque_driven_ocp/spring_load.py @@ -181,8 +181,9 @@ def prepare_ocp( n_shooting: float = 30, scenario=1, ): + # BioModel path - m = BiorbdModel(biorbd_model_path) + m = BiorbdModel(biorbd_model_path, nb_supplementary_forces_in_global=1) m.set_gravity(np.array((0, 0, 0))) weight = 1 diff --git a/bioptim/models/biorbd/biorbd_model.py b/bioptim/models/biorbd/biorbd_model.py index 523bc127f..cf6e91bf2 100644 --- a/bioptim/models/biorbd/biorbd_model.py +++ b/bioptim/models/biorbd/biorbd_model.py @@ -31,16 +31,8 @@ def __init__( friction_coefficients: np.ndarray = None, external_forces: ExternalForcesList = None, parameters: ParameterList = None, + nb_supplementary_forces_in_global = 0, ): - """ - - Parameters - ---------- - bio_model - friction_coefficients - external_forces - parameters - """ 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'") @@ -60,6 +52,11 @@ def __init__( self._segments_to_apply_forces_in_local = segments_to_apply_forces_in_local self._segments_to_apply_translational_forces = segments_to_apply_translational_forces + # Declare the number of external forces that are not numerical values + if nb_supplementary_forces_in_global != 0 and (segments_to_apply_forces_in_global != [] or segments_to_apply_forces_in_local != [] or segments_to_apply_translational_forces != []): + raise ValueError( + "You cannot provide nb_supplementary_forces_in_global and segments_to_apply_forces_in_global/segments_to_apply_forces_in_local/segments_to_apply_translational_forces at the same time") + # Declaration of MX variables of the right shape for the creation of CasADi Functions self.q = MX.sym("q_mx", self.nb_q, 1) self.qdot = MX.sym("qdot_mx", self.nb_qdot, 1) @@ -69,12 +66,20 @@ def __init__( self.muscle = MX.sym("muscle_mx", self.nb_muscles, 1) self.activations = MX.sym("activations_mx", self.nb_muscles, 1) - self.external_forces = MX.sym( - "external_forces_mx", - 9 * self.nb_forces_in_global + 9 * self.nb_forces_in_local + 6 * self.nb_translational_forces, - 1, - ) - self.external_forces_set = self._dispatch_forces() + if nb_supplementary_forces_in_global != 0: + self.external_forces = MX.sym( + "supplementary_forces_in_global", + 9 * nb_supplementary_forces_in_global, + 1, + ) + self.external_forces_set = None + else: + self.external_forces = MX.sym( + "external_forces_mx", + 9 * self.nb_forces_in_global + 9 * self.nb_forces_in_local + 6 * self.nb_translational_forces, + 1, + ) + self.external_forces_set = self._dispatch_forces() # TODO: remove mx (the MX parameters should be created inside the BiorbdModel) self.parameters = parameters.mx if parameters else MX() diff --git a/bioptim/models/biorbd/multi_biorbd_model.py b/bioptim/models/biorbd/multi_biorbd_model.py index 331a33ae9..2d82fc60f 100644 --- a/bioptim/models/biorbd/multi_biorbd_model.py +++ b/bioptim/models/biorbd/multi_biorbd_model.py @@ -252,6 +252,8 @@ def gravity(self) -> Function: "gravity", [self.parameters], [biorbd_return], + ["parameters"], + ["gravity"], ) return casadi_fun @@ -310,6 +312,8 @@ def homogeneous_matrices_in_global(self, segment_index, inverse=False) -> Functi "homogeneous_matrices_in_global", [self.models[model_id].q, self.parameters], [biorbd_return], + ["q", "parameters"], + ["homogeneous_matrices_in_global"], ) return casadi_fun @@ -335,6 +339,8 @@ def mass(self) -> Function: "mass", [self.parameters], [biorbd_return], + ["parameters"], + ["mass"], ) return casadi_fun @@ -347,6 +353,8 @@ def center_of_mass(self) -> Function: "center_of_mass", [self.q, self.parameters], [biorbd_return], + ["q", "parameters"], + ["center_of_mass"], ) return casadi_fun @@ -363,6 +371,8 @@ def center_of_mass_velocity(self) -> Function: "center_of_mass_velocity", [self.q, self.qdot, self.parameters], [biorbd_return], + ["q", "qdot", "parameters"], + ["center_of_mass_velocity"], ) return casadi_fun @@ -380,6 +390,8 @@ def center_of_mass_acceleration(self) -> Function: "center_of_mass_acceleration", [self.q, self.qdot, self.qddot, self.parameters], [biorbd_return], + ["q", "qdot", "qddot", "parameters"], + ["center_of_mass_acceleration"], ) return casadi_fun @@ -421,6 +433,8 @@ def angular_momentum(self) -> Function: "angular_momentum", [self.q, self.qdot, self.parameters], [biorbd_return], + ["q", "qdot", "parameters"], + ["angular_momentum"], ) return casadi_fun @@ -437,6 +451,8 @@ def reshape_qdot(self, k_stab=1) -> Function: "reshape_qdot", [self.q, self.qdot, self.parameters], [biorbd_return], + ["q", "qdot", "parameters"], + ["qdot_reshaped"], ) return casadi_fun @@ -453,6 +469,8 @@ def segment_angular_velocity(self, idx) -> Function: "segment_angular_velocity", [self.q, self.qdot, self.parameters], [biorbd_return], + ["q", "qdot", "parameters"], + ["segment_angular_velocity"], ) return casadi_fun @@ -511,6 +529,8 @@ def torque(self) -> Function: "torque", [self.tau, self.q, self.qdot, self.parameters], [biorbd_return], + ["tau", "q", "qdot", "parameters"], + ["torque"], ) return casadi_fun @@ -533,6 +553,8 @@ def forward_dynamics_free_floating_base(self) -> Function: "forward_dynamics_free_floating_base", [self.q, self.qdot, self.qddot_joints, self.parameters], [biorbd_return], + ["q", "qdot", "qddot_joints", "parameters"], + ["qddot"], ) return casadi_fun @@ -550,6 +572,8 @@ def reorder_qddot_root_joints(self): "reorder_qddot_root_joints", [self.qddot_roots, self.qddot_joints], [biorbd_return], + ["qddot_roots", "qddot_joints"], + ["qddot"], ) return casadi_fun @@ -575,6 +599,8 @@ def forward_dynamics(self, with_contact) -> Function: "forward_dynamics", [self.q, self.qdot, self.tau, [], self.parameters], [biorbd_return], + ["q", "qdot", "tau", "external_forces([])", "parameters"], + ["qddot"], ) return casadi_fun @@ -588,12 +614,14 @@ def inverse_dynamics(self) -> Function: qddot_model = self.qddot[self.variable_index("qddot", i)] biorbd_return = vertcat( biorbd_return, - model.inverse_dynamics()(q_model, qdot_model, qddot_model, [], [], self.parameters), + model.inverse_dynamics()(q_model, qdot_model, qddot_model, [], self.parameters), ) casadi_fun = Function( "inverse_dynamics", - [self.q, self.qdot, self.qddot, [], [], self.parameters], + [self.q, self.qdot, self.qddot, [], self.parameters], [biorbd_return], + ["q", "qdot", "qddot", "external_forces([])", "parameters"], + ["tau"], ) return casadi_fun @@ -618,6 +646,8 @@ def contact_forces_from_constrained_forward_dynamics(self) -> Function: "contact_forces_from_constrained_forward_dynamics", [self.q, self.qdot, self.tau, [], self.parameters], [biorbd_return], + ["q", "qdot", "tau", "external_forces([])", "parameters"], + ["contact_forces"], ) return casadi_fun @@ -638,6 +668,8 @@ def qdot_from_impact(self) -> Function: "qdot_from_impact", [self.q, self.qdot, self.parameters], [biorbd_return], + ["q", "qdot_pre", "parameters"], + ["qdot_post"], ) return casadi_fun @@ -655,6 +687,8 @@ def muscle_activation_dot(self) -> Function: "muscle_activation_dot", [self.muscle, self.activations, self.parameters], [biorbd_return], + ["muscles", "activations", "parameters"], + ["muscle_activations_dot"], ) return casadi_fun @@ -673,6 +707,8 @@ def muscle_joint_torque(self) -> Function: "muscle_joint_torque", [self.muscle, self.q, self.qdot, self.parameters], [biorbd_return], + ["muscles", "q", "qdot", "parameters"], + ["muscle_joint_torque"], ) return casadi_fun @@ -685,6 +721,8 @@ def markers(self) -> Function: "markers", [self.q, self.parameters], [horzcat(*biorbd_return)], + ["q", "parameters"], + ["markers"], ) return casadi_fun @@ -708,6 +746,8 @@ def marker(self, index, reference_segment_index=None) -> Function: "marker", [self.q, self.parameters], [biorbd_return], + ["q", "parameters"], + ["marker"], ) return casadi_fun @@ -758,11 +798,12 @@ def markers_velocities(self, reference_index=None) -> Function: q_model = self.q[self.variable_index("q", i)] qdot_model = self.qdot[self.variable_index("qdot", i)] biorbd_return += [model.markers_velocities(reference_index)(q_model, qdot_model, self.parameters)] - biorbd_return = [item for sublist in biorbd_return for item in sublist] casadi_fun = Function( "markers_velocities", [self.q, self.qdot, self.parameters], [horzcat(*biorbd_return)], + ["q", "qdot", "parameters"], + ["markers_velocities"], ) return casadi_fun @@ -772,11 +813,12 @@ def marker_velocity(self, marker_index: int) -> Function: q_model = self.q[self.variable_index("q", i)] qdot_model = self.qdot[self.variable_index("qdot", i)] biorbd_return += [model.marker_velocity(marker_index)(q_model, qdot_model, self.parameters)] - biorbd_return = [item for sublist in biorbd_return for item in sublist] casadi_fun = Function( "marker_velocity", [self.q, self.qdot, self.parameters], [horzcat(*biorbd_return)], + ["q", "qdot", "parameters"], + ["marker_velocity"], ) return casadi_fun @@ -793,6 +835,8 @@ def tau_max(self) -> Function: "tau_max", [self.q, self.qdot, self.parameters], [out_max, out_min], + ["q", "qdot", "parameters"], + ["tau_max", "tau_min"], ) return casadi_fun @@ -813,6 +857,8 @@ def rigid_contact_acceleration(self, contact_index, contact_axis) -> Function: "rigid_contact_acceleration", [self.q, self.qdot, self.qddot, self.parameters], [biorbd_return], + ["q", "qdot", "qddot", "parameters"], + ["rigid_contact_acceleration"], ) return casadi_fun @@ -835,6 +881,8 @@ def soft_contact_forces(self) -> Function: "soft_contact_forces", [self.q, self.qdot, self.parameters], [biorbd_return], + ["q", "qdot", "parameters"], + ["soft_contact_forces"], ) return casadi_fun @@ -851,6 +899,8 @@ def normalize_state_quaternions(self) -> Function: "normalize_state_quaternions", [self.q], [biorbd_return], + ["q"], + ["q_normalized"], ) return casadi_fun @@ -868,6 +918,8 @@ def contact_forces(self) -> Function: "contact_forces", [self.q, self.qdot, self.tau, [], self.parameters], [biorbd_return], + ["q", "qdot", "tau", "external_forces", "parameters"], + ["contact_forces"], ) return casadi_fun @@ -881,6 +933,8 @@ def passive_joint_torque(self) -> Function: "passive_joint_torque", [self.q, self.qdot, self.parameters], [biorbd_return], + ["q", "qdot", "parameters"], + ["passive_joint_torque"], ) return casadi_fun @@ -894,6 +948,8 @@ def ligament_joint_torque(self) -> Function: "ligament_joint_torque", [self.q, self.qdot, self.parameters], [biorbd_return], + ["q", "qdot", "parameters"], + ["ligament_joint_torque"], ) return casadi_fun diff --git a/tests/shard1/test_biorbd_multi_model.py b/tests/shard1/test_biorbd_multi_model.py index f26eaee6e..3e8b8cc1d 100644 --- a/tests/shard1/test_biorbd_multi_model.py +++ b/tests/shard1/test_biorbd_multi_model.py @@ -225,7 +225,6 @@ def test_biorbd_model(): np.array([3.1, 1, 2, 9.1, 1, 2]), [], [], - [], ), np.array([4.844876e01, 2.121037e-01, 1.964626e00, 4.165226e02, 3.721585e01, 1.906986e00]).reshape(6, 1), decimal=5, diff --git a/tests/shard1/test_dynamics_units.py b/tests/shard1/test_dynamics_units.py index ba4537264..dba5f1cc3 100644 --- a/tests/shard1/test_dynamics_units.py +++ b/tests/shard1/test_dynamics_units.py @@ -35,7 +35,7 @@ def test_check_external_forces_format_invalid(): # Tests for _check_soft_contacts_dynamics def test_check_soft_contacts_dynamics_valid_ode(): - _check_soft_contacts_dynamics("NotDAEInverseDynamics", SoftContactDynamics.ODE, 1, 0) + _check_soft_contacts_dynamics(SoftContactDynamics.ODE, 1, 0) # More tests for _check_external_forces_format @@ -54,16 +54,16 @@ def test_check_external_forces_format_wrong_length(): # Tests for _check_soft_contacts_dynamics def test_check_soft_contacts_dynamics_valid_constraint(): - _check_soft_contacts_dynamics("SomeRigidBodyDynamics", SoftContactDynamics.CONSTRAINT, 1, 0) + _check_soft_contacts_dynamics(SoftContactDynamics.CONSTRAINT, 1, 0) def test_check_soft_contacts_dynamics_invalid_soft_contacts_dynamics(): with pytest.raises(ValueError): - _check_soft_contacts_dynamics("SomeRigidBodyDynamics", "InvalidSoftContactDynamics", 1, 0) + _check_soft_contacts_dynamics("InvalidSoftContactDynamics", 1, 0) def test_check_soft_contacts_dynamics_no_soft_contacts(): - _check_soft_contacts_dynamics("SomeRigidBodyDynamics", SoftContactDynamics.ODE, 0, 0) + _check_soft_contacts_dynamics(SoftContactDynamics.ODE, 0, 0) # Tests for _check_contacts_in_biorbd_model diff --git a/tests/shard1/test_enums.py b/tests/shard1/test_enums.py index 9e16e35e7..776c348d1 100644 --- a/tests/shard1/test_enums.py +++ b/tests/shard1/test_enums.py @@ -189,16 +189,16 @@ def test_multi_cyclic_cycle_solutions(): def test_external_forces_type(): - assert ExternalForcesType.LINEAR_FORCE == "linear_force" - assert ExternalForcesType.TORQUE == "torque" + assert ExternalForcesType.LINEAR_FORCE.value == 'linear_force' + assert ExternalForcesType.TORQUE.value == "torque" # verify the number of elements assert len(ExternalForcesType) == 2 def test_reference_frame(): - assert ReferenceFrame.GLOBAL == "global" - assert ReferenceFrame.LOCAL == "local" + assert ReferenceFrame.GLOBAL.value == "global" + assert ReferenceFrame.LOCAL.value == "local" # verify the number of elements assert len(ReferenceFrame) == 2 From e9b7afd2e15c8a7140717cd146e4011bb08713c9 Mon Sep 17 00:00:00 2001 From: eve-mac Date: Thu, 24 Oct 2024 16:52:30 -0400 Subject: [PATCH 057/108] shard 3 (changed values because implicit) --- .../shard3/test_muscle_driven_ocp_implicit.py | 98 ------------------- tests/shard3/test_passive_torque.py | 8 +- 2 files changed, 4 insertions(+), 102 deletions(-) delete mode 100644 tests/shard3/test_muscle_driven_ocp_implicit.py diff --git a/tests/shard3/test_muscle_driven_ocp_implicit.py b/tests/shard3/test_muscle_driven_ocp_implicit.py deleted file mode 100644 index 17535ce0d..000000000 --- a/tests/shard3/test_muscle_driven_ocp_implicit.py +++ /dev/null @@ -1,98 +0,0 @@ -""" -Test for file IO -""" - -import os -import pytest - -import numpy as np -import numpy.testing as npt -from bioptim import OdeSolver, DefectType, PhaseDynamics, SolutionMerge - -from tests.utils import TestUtils - - -@pytest.mark.parametrize("phase_dynamics", [PhaseDynamics.SHARED_DURING_THE_PHASE, PhaseDynamics.ONE_PER_NODE]) -@pytest.mark.parametrize("ode_solver", [OdeSolver.COLLOCATION, OdeSolver.IRK]) -def test_muscle_driven_ocp_implicit(ode_solver, phase_dynamics): - from bioptim.examples.muscle_driven_ocp import static_arm as ocp_module - - bioptim_folder = os.path.dirname(ocp_module.__file__) - - ode_solver_obj = ode_solver(defects_type=DefectType.IMPLICIT) - ocp = ocp_module.prepare_ocp( - bioptim_folder + "/models/arm26.bioMod", - final_time=0.1, - n_shooting=5, - weight=1, - ode_solver=ode_solver_obj, - phase_dynamics=phase_dynamics, - expand_dynamics=ode_solver != OdeSolver.IRK, - n_threads=1, - ) - sol = ocp.solve() - - # Check objective function value - f = np.array(sol.cost) - npt.assert_equal(f.shape, (1, 1)) - - # Check constraints - g = np.array(sol.constraints) - if ode_solver == OdeSolver.COLLOCATION: - npt.assert_equal(g.shape, (20 * 5, 1)) - npt.assert_almost_equal(g, np.zeros((20 * 5, 1)), decimal=5) - else: - npt.assert_equal(g.shape, (20, 1)) - npt.assert_almost_equal(g, np.zeros((20, 1)), decimal=5) - - # Check some of the results - states = sol.decision_states(to_merge=SolutionMerge.NODES) - controls = sol.decision_controls(to_merge=SolutionMerge.NODES) - q, qdot, tau, mus = states["q"], states["qdot"], controls["tau"], controls["muscles"] - - if ode_solver == OdeSolver.IRK: - npt.assert_almost_equal(f[0, 0], 0.12644299285122357) - - # initial and final position - npt.assert_almost_equal(q[:, 0], np.array([0.07, 1.4])) - npt.assert_almost_equal(q[:, -1], np.array([-0.19992522, 2.65885512])) - # initial and final velocities - npt.assert_almost_equal(qdot[:, 0], np.array([0.0, 0.0])) - npt.assert_almost_equal(qdot[:, -1], np.array([-2.31428244, 14.18136079])) - # initial and final controls - npt.assert_almost_equal(tau[:, 0], np.array([0.00799548, 0.02025833])) - npt.assert_almost_equal(tau[:, -1], np.array([0.00228284, 0.00281158])) - npt.assert_almost_equal( - mus[:, 0], - np.array([7.16894627e-06, 6.03295877e-01, 3.37029458e-01, 1.08379096e-05, 1.14087059e-05, 3.66744423e-01]), - ) - npt.assert_almost_equal( - mus[:, -1], - np.array([5.46688078e-05, 6.60548530e-03, 3.77595547e-03, 4.92828831e-04, 5.09444822e-04, 9.08082070e-03]), - ) - - elif ode_solver == OdeSolver.COLLOCATION: - npt.assert_almost_equal(f[0, 0], 0.12644297341855165) - - # initial and final position - npt.assert_almost_equal(q[:, 0], np.array([0.07, 1.4])) - npt.assert_almost_equal(q[:, -1], np.array([-0.19992534, 2.65884909])) - # initial and final velocities - npt.assert_almost_equal(qdot[:, 0], np.array([0.0, 0.0])) - npt.assert_almost_equal(qdot[:, -1], np.array([-2.3143106, 14.1812974])) - # initial and final controls - npt.assert_almost_equal(tau[:, 0], np.array([0.00799575, 0.02025812])) - npt.assert_almost_equal(tau[:, -1], np.array([0.00228286, 0.00281158])) - npt.assert_almost_equal( - mus[:, 0], - np.array([7.16887076e-06, 6.03293415e-01, 3.37026700e-01, 1.08380212e-05, 1.14088234e-05, 3.66740786e-01]), - ) - npt.assert_almost_equal( - mus[:, -1], - np.array([5.4664028e-05, 6.5610959e-03, 3.7092411e-03, 4.6592962e-04, 4.8159442e-04, 9.0543847e-03]), - ) - else: - raise ValueError("Test not ready") - - # simulate - TestUtils.simulate(sol, decimal_value=5) diff --git a/tests/shard3/test_passive_torque.py b/tests/shard3/test_passive_torque.py index e98a7f284..c606c957d 100644 --- a/tests/shard3/test_passive_torque.py +++ b/tests/shard3/test_passive_torque.py @@ -436,12 +436,12 @@ def test_pendulum_passive_torque(with_passive_torque, phase_dynamics): # initial and final controls npt.assert_almost_equal( tau[:, 0], - np.array([1.587319, 0.0]), + np.array([6.16172631, 0.0]), decimal=6, ) npt.assert_almost_equal( tau[:, -1], - np.array([-39.19793, 0.0]), + np.array([-11.82081071, 0.0]), decimal=6, ) @@ -455,11 +455,11 @@ def test_pendulum_passive_torque(with_passive_torque, phase_dynamics): # initial and final controls npt.assert_almost_equal( tau[:, 0], - np.array([2.606971, 0.0]), + np.array([6.015498, 0.0]), decimal=6, ) npt.assert_almost_equal( tau[:, -1], - np.array([-24.611219, 0.0]), + np.array([-13.68877181, 0.0]), decimal=6, ) From 0570dc83ac1e313b6fea80be5daa278f8b68d9e4 Mon Sep 17 00:00:00 2001 From: eve-mac Date: Thu, 24 Oct 2024 17:05:26 -0400 Subject: [PATCH 058/108] shard 4 (-soft contacts) --- bioptim/limits/penalty.py | 12 ++++-------- bioptim/models/biorbd/biorbd_model.py | 4 ++-- tests/shard4/test_penalty.py | 28 +++++++-------------------- 3 files changed, 13 insertions(+), 31 deletions(-) diff --git a/bioptim/limits/penalty.py b/bioptim/limits/penalty.py index c9860ca06..b6f75176c 100644 --- a/bioptim/limits/penalty.py +++ b/bioptim/limits/penalty.py @@ -384,11 +384,9 @@ def minimize_markers_acceleration( qddot = PenaltyFunctionAbstract._get_qddot(controller, "cx") - markers = horzcat( - *controller.model.markers_accelerations(reference_index=reference_jcs)( + markers =controller.model.markers_accelerations(reference_index=reference_jcs)( controller.q, controller.qdot, qddot, controller.parameters.cx ) - ) return markers @@ -474,11 +472,11 @@ 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.markers_velocities()( + marker_velocities = controller.model.markers_velocities()( controller.q, controller.qdot, controller.parameters.cx ) - marker_1 = marker_velocity[first_marker_idx][:] - marker_2 = marker_velocity[second_marker_idx][:] + marker_1 = marker_velocities[:, first_marker_idx] + marker_2 = marker_velocities[:, second_marker_idx] diff_markers = marker_2 - marker_1 @@ -1436,8 +1434,6 @@ def _get_qddot(controller: PenaltyController, attribute: str): attribute : str Specifies which attribute ('cx_start' or 'mx') to use for the extraction. """ - # if attribute not in ["mx", "cx_start"]: - # raise ValueError("atrribute should be either mx or cx_start") if "qddot" not in controller.states and "qddot" not in controller.controls: source_qdot = controller.states if "qdot" in controller.states else controller.controls diff --git a/bioptim/models/biorbd/biorbd_model.py b/bioptim/models/biorbd/biorbd_model.py index cf6e91bf2..36c8c7d93 100644 --- a/bioptim/models/biorbd/biorbd_model.py +++ b/bioptim/models/biorbd/biorbd_model.py @@ -799,7 +799,7 @@ def markers_accelerations(self, reference_index=None) -> list[MX]: casadi_fun = Function( "markers_accelerations", [self.q, self.qdot, self.qddot, self.parameters], - biorbd_return, + [horzcat(*biorbd_return)], ["q", "qdot", "qddot", "parameters"], ["markers_accelerations"], ) @@ -873,7 +873,7 @@ def soft_contact_forces(self) -> Function: biorbd_return = MX.zeros(self.nb_soft_contacts * 6, 1) for i_sc in range(self.nb_soft_contacts): soft_contact = self.soft_contact(i_sc) - + # @ipuch: please confirm / help with the following lines biorbd_return[i_sc * 6 : (i_sc + 1) * 6, :] = ( biorbd.SoftContactSphere(soft_contact).computeForceAtOrigin(self.model, q_biorbd, qdot_biorbd).to_mx() ) diff --git a/tests/shard4/test_penalty.py b/tests/shard4/test_penalty.py index e4621e852..a55aced3d 100644 --- a/tests/shard4/test_penalty.py +++ b/tests/shard4/test_penalty.py @@ -34,14 +34,11 @@ def prepare_test_ocp( with_muscles=False, with_contact=False, with_actuator=False, - implicit=False, use_sx=True, ): bioptim_folder = TestUtils.bioptim_folder() if with_muscles and with_contact or with_muscles and with_actuator or with_contact and with_actuator: raise RuntimeError("With muscles and with contact and with_actuator together is not defined") - if with_muscles and implicit or implicit and with_actuator: - raise RuntimeError("With muscles and implicit and with_actuator together is not defined") elif with_muscles: bio_model = BiorbdModel(bioptim_folder + "/examples/muscle_driven_ocp/models/arm26.bioMod") dynamics = DynamicsList() @@ -919,9 +916,8 @@ def test_penalty_minimize_linear_momentum(value, penalty_origin, use_sx, phase_d @pytest.mark.parametrize("phase_dynamics", [PhaseDynamics.SHARED_DURING_THE_PHASE, PhaseDynamics.ONE_PER_NODE]) @pytest.mark.parametrize("penalty_origin", [ObjectiveFcn.Lagrange, ObjectiveFcn.Mayer]) @pytest.mark.parametrize("value", [0.1, -10]) -@pytest.mark.parametrize("implicit", [True, False]) -def test_penalty_minimize_comddot(value, penalty_origin, implicit, phase_dynamics): - ocp = prepare_test_ocp(with_contact=True, implicit=implicit, phase_dynamics=phase_dynamics) +def test_penalty_minimize_comddot(value, penalty_origin, phase_dynamics): + ocp = prepare_test_ocp(with_contact=True, phase_dynamics=phase_dynamics) t = [0] phases_dt = [0.05] x = [DM.ones((8, 1)) * value] @@ -937,23 +933,13 @@ def test_penalty_minimize_comddot(value, penalty_origin, implicit, phase_dynamic else: penalty = Constraint(penalty_type) - if not implicit: - res = get_penalty_value(ocp, penalty, t, phases_dt, x, u, p, a, d) - - expected = np.array([[0.0], [-0.7168803], [-0.0740871]]) - if value == -10: - expected = np.array([[0.0], [1.455063], [16.3741091]]) - - npt.assert_almost_equal(res, expected) - else: - res = get_penalty_value(ocp, penalty, t, phases_dt, x, u, [], [], []) + res = get_penalty_value(ocp, penalty, t, phases_dt, x, u, p, a, d) - expected = np.array([[0], [-0.0008324], [0.002668]]) - expected = np.array([[0], [-0.0008324], [0.002668]]) - if value == -10: - expected = np.array([[0], [-17.5050533], [-18.2891901]]) + expected = np.array([[0.0], [-0.7168803], [-0.0740871]]) + if value == -10: + expected = np.array([[0.0], [1.455063], [16.3741091]]) - npt.assert_almost_equal(res, expected) + npt.assert_almost_equal(res, expected) @pytest.mark.parametrize("phase_dynamics", [PhaseDynamics.SHARED_DURING_THE_PHASE, PhaseDynamics.ONE_PER_NODE]) From fc2f80026e46aac8d56eedce4477508fb7c4708e Mon Sep 17 00:00:00 2001 From: eve-mac Date: Thu, 24 Oct 2024 17:06:53 -0400 Subject: [PATCH 059/108] shard 6 (removing explicit formulation) --- ...st_global_stochastic_except_collocation.py | 310 +++++++++--------- 1 file changed, 155 insertions(+), 155 deletions(-) diff --git a/tests/shard6/test_global_stochastic_except_collocation.py b/tests/shard6/test_global_stochastic_except_collocation.py index 0a4410643..a3ed1c4a6 100644 --- a/tests/shard6/test_global_stochastic_except_collocation.py +++ b/tests/shard6/test_global_stochastic_except_collocation.py @@ -234,161 +234,161 @@ # ] # ), # ) - - -@pytest.mark.parametrize("use_sx", [True, False]) -def test_arm_reaching_torque_driven_explicit(use_sx): - from bioptim.examples.stochastic_optimal_control import arm_reaching_torque_driven_explicit as ocp_module - - final_time = 0.8 - n_shooting = 4 - hand_final_position = np.array([9.359873986980460e-12, 0.527332023564034]) - - dt = 0.01 - motor_noise_std = 0.05 - wPq_std = 3e-4 - wPqdot_std = 0.0024 - motor_noise_magnitude = DM(np.array([motor_noise_std**2 / dt, motor_noise_std**2 / dt])) - wPq_magnitude = DM(np.array([wPq_std**2 / dt, wPq_std**2 / dt])) - wPqdot_magnitude = DM(np.array([wPqdot_std**2 / dt, wPqdot_std**2 / dt])) - sensory_noise_magnitude = vertcat(wPq_magnitude, wPqdot_magnitude) - - bioptim_folder = os.path.dirname(ocp_module.__file__) - - if use_sx: - with pytest.raises( - NotImplementedError, match="Wrong number or type of arguments for overloaded function 'MX_set'" - ): - ocp = ocp_module.prepare_socp( - biorbd_model_path=bioptim_folder + "/models/LeuvenArmModel.bioMod", - final_time=final_time, - n_shooting=n_shooting, - hand_final_position=hand_final_position, - motor_noise_magnitude=motor_noise_magnitude, - sensory_noise_magnitude=sensory_noise_magnitude, - use_sx=use_sx, - ) - return - - ocp = ocp_module.prepare_socp( - biorbd_model_path=bioptim_folder + "/models/LeuvenArmModel.bioMod", - final_time=final_time, - n_shooting=n_shooting, - hand_final_position=hand_final_position, - motor_noise_magnitude=motor_noise_magnitude, - sensory_noise_magnitude=sensory_noise_magnitude, - use_sx=use_sx, - ) - - # Solver parameters - solver = Solver.IPOPT() - solver.set_maximum_iterations(4) - solver.set_nlp_scaling_method("none") - - sol = ocp.solve(solver) - - # Check objective function value - f = np.array(sol.cost) - npt.assert_equal(f.shape, (1, 1)) - npt.assert_almost_equal(f[0, 0], 46.99030175091475) - - # detailed cost values - npt.assert_almost_equal(sol.detailed_cost[0]["cost_value_weighted"], 0.055578630313992475) - npt.assert_almost_equal(sol.detailed_cost[1]["cost_value_weighted"], 6.038226210163837) - npt.assert_almost_equal( - f[0, 0], sum(sol.detailed_cost[i]["cost_value_weighted"] for i in range(len(sol.detailed_cost))) - ) - - # Check constraints - g = np.array(sol.constraints) - npt.assert_equal(g.shape, (214, 1)) - - # Check some of the results - states = sol.decision_states(to_merge=SolutionMerge.NODES) - controls = sol.decision_controls(to_merge=SolutionMerge.NODES) - algebraic_states = sol.decision_algebraic_states(to_merge=SolutionMerge.NODES) - - q, qdot, qddot = states["q"], states["qdot"], states["qddot"] - qdddot, tau = controls["qdddot"], controls["tau"] - k, ref, m = algebraic_states["k"], algebraic_states["ref"], algebraic_states["m"] - ocp.nlp[0].integrated_values["cov"].cx - - # TODO Integrated value is not a proper way to go, it should be removed and recomputed at will - # cov = integrated_values["cov"] - - # initial and final position - npt.assert_almost_equal(q[:, 0], np.array([0.34906585, 2.24586773])) - npt.assert_almost_equal(q[:, -1], np.array([0.92702265, 1.27828413])) - npt.assert_almost_equal(qdot[:, 0], np.array([0, 0])) - npt.assert_almost_equal(qdot[:, -1], np.array([0, 0])) - npt.assert_almost_equal(qddot[:, 0], np.array([0, 0])) - npt.assert_almost_equal(qddot[:, -1], np.array([0, 0])) - - npt.assert_almost_equal(qdddot[:, 0], np.array([0.00124365, 0.00124365])) - npt.assert_almost_equal(qdddot[:, -2], np.array([0.00124365, 0.00124365])) - - npt.assert_almost_equal(tau[:, 0], np.array([0.36186712, -0.2368119])) - npt.assert_almost_equal(tau[:, -2], np.array([-0.35709778, 0.18867995])) - - npt.assert_almost_equal( - k[:, 0], - np.array( - [ - 0.13824554, - 0.54172046, - 0.05570321, - 0.25169273, - 0.00095407, - 0.00121309, - 0.00095146, - 0.00121091, - ] - ), - ) - npt.assert_almost_equal(ref[:, 0], np.array([0.02592847, 0.25028511, 0.00124365, 0.00124365])) - npt.assert_almost_equal( - m[:, 0], - np.array( - [ - 8.36639386e-01, - 1.14636589e-01, - -4.32594485e-01, - 1.10372277e00, - 4.73812392e-03, - 4.73812392e-03, - 8.01515210e-02, - 9.66785674e-01, - 7.40822199e-01, - 8.50818498e-01, - 6.74366790e-03, - 6.74366790e-03, - 7.92700393e-02, - -8.94683551e-03, - 7.86796476e-01, - -9.53722725e-02, - 6.55990825e-04, - 6.55990825e-04, - -8.94995258e-04, - 7.69438075e-02, - -2.33336654e-02, - 7.55054362e-01, - 1.59819032e-03, - 1.59819032e-03, - 1.24365477e-03, - 1.24365477e-03, - 1.24365477e-03, - 1.24365477e-03, - 8.76878178e-01, - 1.24365477e-03, - 1.24365477e-03, - 1.24365477e-03, - 1.24365477e-03, - 1.24365477e-03, - 1.24365477e-03, - 8.76878178e-01, - ] - ), - ) +# +# +# @pytest.mark.parametrize("use_sx", [True, False]) +# def test_arm_reaching_torque_driven_explicit(use_sx): +# from bioptim.examples.stochastic_optimal_control import arm_reaching_torque_driven_explicit as ocp_module +# +# final_time = 0.8 +# n_shooting = 4 +# hand_final_position = np.array([9.359873986980460e-12, 0.527332023564034]) +# +# dt = 0.01 +# motor_noise_std = 0.05 +# wPq_std = 3e-4 +# wPqdot_std = 0.0024 +# motor_noise_magnitude = DM(np.array([motor_noise_std**2 / dt, motor_noise_std**2 / dt])) +# wPq_magnitude = DM(np.array([wPq_std**2 / dt, wPq_std**2 / dt])) +# wPqdot_magnitude = DM(np.array([wPqdot_std**2 / dt, wPqdot_std**2 / dt])) +# sensory_noise_magnitude = vertcat(wPq_magnitude, wPqdot_magnitude) +# +# bioptim_folder = os.path.dirname(ocp_module.__file__) +# +# if use_sx: +# with pytest.raises( +# NotImplementedError, match="Wrong number or type of arguments for overloaded function 'MX_set'" +# ): +# ocp = ocp_module.prepare_socp( +# biorbd_model_path=bioptim_folder + "/models/LeuvenArmModel.bioMod", +# final_time=final_time, +# n_shooting=n_shooting, +# hand_final_position=hand_final_position, +# motor_noise_magnitude=motor_noise_magnitude, +# sensory_noise_magnitude=sensory_noise_magnitude, +# use_sx=use_sx, +# ) +# return +# +# ocp = ocp_module.prepare_socp( +# biorbd_model_path=bioptim_folder + "/models/LeuvenArmModel.bioMod", +# final_time=final_time, +# n_shooting=n_shooting, +# hand_final_position=hand_final_position, +# motor_noise_magnitude=motor_noise_magnitude, +# sensory_noise_magnitude=sensory_noise_magnitude, +# use_sx=use_sx, +# ) +# +# # Solver parameters +# solver = Solver.IPOPT() +# solver.set_maximum_iterations(4) +# solver.set_nlp_scaling_method("none") +# +# sol = ocp.solve(solver) +# +# # Check objective function value +# f = np.array(sol.cost) +# npt.assert_equal(f.shape, (1, 1)) +# npt.assert_almost_equal(f[0, 0], 46.99030175091475) +# +# # detailed cost values +# npt.assert_almost_equal(sol.detailed_cost[0]["cost_value_weighted"], 0.055578630313992475) +# npt.assert_almost_equal(sol.detailed_cost[1]["cost_value_weighted"], 6.038226210163837) +# npt.assert_almost_equal( +# f[0, 0], sum(sol.detailed_cost[i]["cost_value_weighted"] for i in range(len(sol.detailed_cost))) +# ) +# +# # Check constraints +# g = np.array(sol.constraints) +# npt.assert_equal(g.shape, (214, 1)) +# +# # Check some of the results +# states = sol.decision_states(to_merge=SolutionMerge.NODES) +# controls = sol.decision_controls(to_merge=SolutionMerge.NODES) +# algebraic_states = sol.decision_algebraic_states(to_merge=SolutionMerge.NODES) +# +# q, qdot, qddot = states["q"], states["qdot"], states["qddot"] +# qdddot, tau = controls["qdddot"], controls["tau"] +# k, ref, m = algebraic_states["k"], algebraic_states["ref"], algebraic_states["m"] +# ocp.nlp[0].integrated_values["cov"].cx +# +# # TODO Integrated value is not a proper way to go, it should be removed and recomputed at will +# # cov = integrated_values["cov"] +# +# # initial and final position +# npt.assert_almost_equal(q[:, 0], np.array([0.34906585, 2.24586773])) +# npt.assert_almost_equal(q[:, -1], np.array([0.92702265, 1.27828413])) +# npt.assert_almost_equal(qdot[:, 0], np.array([0, 0])) +# npt.assert_almost_equal(qdot[:, -1], np.array([0, 0])) +# npt.assert_almost_equal(qddot[:, 0], np.array([0, 0])) +# npt.assert_almost_equal(qddot[:, -1], np.array([0, 0])) +# +# npt.assert_almost_equal(qdddot[:, 0], np.array([0.00124365, 0.00124365])) +# npt.assert_almost_equal(qdddot[:, -2], np.array([0.00124365, 0.00124365])) +# +# npt.assert_almost_equal(tau[:, 0], np.array([0.36186712, -0.2368119])) +# npt.assert_almost_equal(tau[:, -2], np.array([-0.35709778, 0.18867995])) +# +# npt.assert_almost_equal( +# k[:, 0], +# np.array( +# [ +# 0.13824554, +# 0.54172046, +# 0.05570321, +# 0.25169273, +# 0.00095407, +# 0.00121309, +# 0.00095146, +# 0.00121091, +# ] +# ), +# ) +# npt.assert_almost_equal(ref[:, 0], np.array([0.02592847, 0.25028511, 0.00124365, 0.00124365])) +# npt.assert_almost_equal( +# m[:, 0], +# np.array( +# [ +# 8.36639386e-01, +# 1.14636589e-01, +# -4.32594485e-01, +# 1.10372277e00, +# 4.73812392e-03, +# 4.73812392e-03, +# 8.01515210e-02, +# 9.66785674e-01, +# 7.40822199e-01, +# 8.50818498e-01, +# 6.74366790e-03, +# 6.74366790e-03, +# 7.92700393e-02, +# -8.94683551e-03, +# 7.86796476e-01, +# -9.53722725e-02, +# 6.55990825e-04, +# 6.55990825e-04, +# -8.94995258e-04, +# 7.69438075e-02, +# -2.33336654e-02, +# 7.55054362e-01, +# 1.59819032e-03, +# 1.59819032e-03, +# 1.24365477e-03, +# 1.24365477e-03, +# 1.24365477e-03, +# 1.24365477e-03, +# 8.76878178e-01, +# 1.24365477e-03, +# 1.24365477e-03, +# 1.24365477e-03, +# 1.24365477e-03, +# 1.24365477e-03, +# 1.24365477e-03, +# 8.76878178e-01, +# ] +# ), +# ) @pytest.mark.parametrize("with_cholesky", [True, False]) From 02a70661aec1b64763653ce3d6949c0923044bb7 Mon Sep 17 00:00:00 2001 From: eve-mac Date: Thu, 24 Oct 2024 17:07:53 -0400 Subject: [PATCH 060/108] blacked --- bioptim/limits/penalty.py | 6 +++--- bioptim/models/biorbd/biorbd_model.py | 11 ++++++++--- tests/shard1/test_enums.py | 2 +- 3 files changed, 12 insertions(+), 7 deletions(-) diff --git a/bioptim/limits/penalty.py b/bioptim/limits/penalty.py index b6f75176c..87254a9dd 100644 --- a/bioptim/limits/penalty.py +++ b/bioptim/limits/penalty.py @@ -384,9 +384,9 @@ def minimize_markers_acceleration( qddot = PenaltyFunctionAbstract._get_qddot(controller, "cx") - markers =controller.model.markers_accelerations(reference_index=reference_jcs)( - controller.q, controller.qdot, qddot, controller.parameters.cx - ) + markers = controller.model.markers_accelerations(reference_index=reference_jcs)( + controller.q, controller.qdot, qddot, controller.parameters.cx + ) return markers diff --git a/bioptim/models/biorbd/biorbd_model.py b/bioptim/models/biorbd/biorbd_model.py index 36c8c7d93..5e9c79ba6 100644 --- a/bioptim/models/biorbd/biorbd_model.py +++ b/bioptim/models/biorbd/biorbd_model.py @@ -31,7 +31,7 @@ def __init__( friction_coefficients: np.ndarray = None, external_forces: ExternalForcesList = None, parameters: ParameterList = None, - nb_supplementary_forces_in_global = 0, + nb_supplementary_forces_in_global=0, ): 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'") @@ -53,9 +53,14 @@ def __init__( self._segments_to_apply_translational_forces = segments_to_apply_translational_forces # Declare the number of external forces that are not numerical values - if nb_supplementary_forces_in_global != 0 and (segments_to_apply_forces_in_global != [] or segments_to_apply_forces_in_local != [] or segments_to_apply_translational_forces != []): + if nb_supplementary_forces_in_global != 0 and ( + segments_to_apply_forces_in_global != [] + or segments_to_apply_forces_in_local != [] + or segments_to_apply_translational_forces != [] + ): raise ValueError( - "You cannot provide nb_supplementary_forces_in_global and segments_to_apply_forces_in_global/segments_to_apply_forces_in_local/segments_to_apply_translational_forces at the same time") + "You cannot provide nb_supplementary_forces_in_global and segments_to_apply_forces_in_global/segments_to_apply_forces_in_local/segments_to_apply_translational_forces at the same time" + ) # Declaration of MX variables of the right shape for the creation of CasADi Functions self.q = MX.sym("q_mx", self.nb_q, 1) diff --git a/tests/shard1/test_enums.py b/tests/shard1/test_enums.py index 776c348d1..901655a80 100644 --- a/tests/shard1/test_enums.py +++ b/tests/shard1/test_enums.py @@ -189,7 +189,7 @@ def test_multi_cyclic_cycle_solutions(): def test_external_forces_type(): - assert ExternalForcesType.LINEAR_FORCE.value == 'linear_force' + assert ExternalForcesType.LINEAR_FORCE.value == "linear_force" assert ExternalForcesType.TORQUE.value == "torque" # verify the number of elements From ed23461c1c4faeb1a56a65ee6c87e0a80095c253 Mon Sep 17 00:00:00 2001 From: eve-mac Date: Thu, 24 Oct 2024 19:56:58 -0400 Subject: [PATCH 061/108] forgot to remove implicit --- tests/shard4/test_penalty.py | 112 ++++++++++------------------------- 1 file changed, 31 insertions(+), 81 deletions(-) diff --git a/tests/shard4/test_penalty.py b/tests/shard4/test_penalty.py index a55aced3d..2068a7f71 100644 --- a/tests/shard4/test_penalty.py +++ b/tests/shard4/test_penalty.py @@ -396,9 +396,8 @@ def test_penalty_minimize_markers_velocity(penalty_origin, value, phase_dynamics @pytest.mark.parametrize("phase_dynamics", [PhaseDynamics.SHARED_DURING_THE_PHASE, PhaseDynamics.ONE_PER_NODE]) @pytest.mark.parametrize("penalty_origin", [ObjectiveFcn.Lagrange, ObjectiveFcn.Mayer]) @pytest.mark.parametrize("value", [0.1, -10]) -@pytest.mark.parametrize("implicit", [True, False]) -def test_penalty_minimize_markers_acceleration(penalty_origin, implicit, value, phase_dynamics): - ocp = prepare_test_ocp(implicit=implicit, phase_dynamics=phase_dynamics) +def test_penalty_minimize_markers_acceleration(penalty_origin, value, phase_dynamics): + ocp = prepare_test_ocp(phase_dynamics=phase_dynamics) t = [0] phases_dt = [0.05] x = [DM.ones((8, 1)) * value] @@ -414,49 +413,25 @@ def test_penalty_minimize_markers_acceleration(penalty_origin, implicit, value, else: penalty = Constraint(penalty_type) - if not implicit: - res = get_penalty_value(ocp, penalty, t, phases_dt, x, u, p, a, d) + res = get_penalty_value(ocp, penalty, t, phases_dt, x, u, p, a, d) + expected = np.array( + [ + [2.15106e-16, -0.00895171, -0.0189017, -0.00995004, 00, 00, 00, -0.00497502], + [0, 0, 0, 0, 00, 00, 00, 0], + [-9.81, -9.79905, -9.79805, -9.809, 00, 00, 00, 0.000499167], + ] + ) + if value == -10: expected = np.array( [ - [2.15106e-16, -0.00895171, -0.0189017, -0.00995004, 00, 00, 00, -0.00497502], - [0, 0, 0, 0, 00, 00, 00, 0], - [-9.81, -9.79905, -9.79805, -9.809, 00, 00, 00, 0.000499167], + [0.0, 138.309264, 222.2164169, 83.90715291, 0.0, 0.0, 0.0, 41.95357645], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [-9.81, -39.31504182, 15.08706927, 44.59211109, 0.0, 0.0, 0.0, 27.20105554], ] ) - if value == -10: - expected = np.array( - [ - [0.0, 138.309264, 222.2164169, 83.90715291, 0.0, 0.0, 0.0, 41.95357645], - [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], - [-9.81, -39.31504182, 15.08706927, 44.59211109, 0.0, 0.0, 0.0, 27.20105554], - ] - ) - npt.assert_almost_equal(res, expected, decimal=5) - else: - res = get_penalty_value(ocp, penalty, t, phases_dt, x, u, p, a, d) - - expected = np.array( - [ - [2.15105711e-16, -8.95170749e-03, -1.89017491e-02, -9.95004165e-03, 00, 00, 00, -4.97502083e-03], - [0, 0, 0, 0, 00, 00, 00, 0], - [-9.81, -9.79905162e00, -9.79805329e00, -9.80900167e00, 00, 00, 00, 4.99167083e-04], - ] - ) - if value == -10: - expected = np.array( - [ - [0.0, 138.309264, 222.2164169, 83.90715291, 0.0, 0.0, 0.0, 41.95357645], - [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], - [-9.81, -39.31504182, 15.08706927, 44.59211109, 0.0, 0.0, 0.0, 27.20105554], - ] - ) - npt.assert_almost_equal( - res, - expected, - decimal=5, - ) + npt.assert_almost_equal(res, expected, decimal=5) @pytest.mark.parametrize("phase_dynamics", [PhaseDynamics.SHARED_DURING_THE_PHASE, PhaseDynamics.ONE_PER_NODE]) @@ -508,9 +483,8 @@ def test_penalty_track_markers_velocity(penalty_origin, value, phase_dynamics): @pytest.mark.parametrize("phase_dynamics", [PhaseDynamics.SHARED_DURING_THE_PHASE, PhaseDynamics.ONE_PER_NODE]) @pytest.mark.parametrize("penalty_origin", [ObjectiveFcn.Lagrange, ObjectiveFcn.Mayer]) @pytest.mark.parametrize("value", [0.1, -10]) -@pytest.mark.parametrize("implicit", [True, False]) -def test_penalty_track_markers_acceleration(penalty_origin, value, implicit, phase_dynamics): - ocp = prepare_test_ocp(implicit=implicit, phase_dynamics=phase_dynamics) +def test_penalty_track_markers_acceleration(penalty_origin, value, phase_dynamics): + ocp = prepare_test_ocp(phase_dynamics=phase_dynamics) t = [0] phases_dt = [0.05] x = [DM.ones((8, 1)) * value] @@ -522,51 +496,27 @@ def test_penalty_track_markers_acceleration(penalty_origin, value, implicit, pha else: penalty = Constraint(penalty_type, target=np.ones((3, 7, 1)) * value) - if not implicit: - res = get_penalty_value(ocp, penalty, t, phases_dt, x, u, [], [], []) - - expected = np.array( - [ - [2.15106e-16, -0.00895171, -0.0189017, -0.00995004, 00, 00, 00, -0.00497502], - [0, 0, 0, 0, 00, 00, 00, 0], - [-9.81, -9.79905, -9.79805, -9.809, 00, 00, 00, 0.000499167], - ] - ) - if value == -10: - expected = np.array( - [ - [0.0, 138.309264, 222.2164169, 83.90715291, 0.0, 0.0, 0.0, 41.95357645], - [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], - [-9.81, -39.31504182, 15.08706927, 44.59211109, 0.0, 0.0, 0.0, 27.20105554], - ] - ) - - npt.assert_almost_equal(res, expected, decimal=5) - else: - res = get_penalty_value(ocp, penalty, t, phases_dt, x, u, [], [], []) + res = get_penalty_value(ocp, penalty, t, phases_dt, x, u, [], [], []) + expected = np.array( + [ + [2.15106e-16, -0.00895171, -0.0189017, -0.00995004, 00, 00, 00, -0.00497502], + [0, 0, 0, 0, 00, 00, 00, 0], + [-9.81, -9.79905, -9.79805, -9.809, 00, 00, 00, 0.000499167], + ] + ) + if value == -10: expected = np.array( [ - [2.15105711e-16, -8.95170749e-03, -1.89017491e-02, -9.95004165e-03, 00, 00, 00, -4.97502083e-03], - [0, 0, 0, 0, 00, 00, 00, 0], - [-9.81, -9.79905162e00, -9.79805329e00, -9.80900167e00, 00, 00, 00, 4.99167083e-04], + [0.0, 138.309264, 222.2164169, 83.90715291, 0.0, 0.0, 0.0, 41.95357645], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [-9.81, -39.31504182, 15.08706927, 44.59211109, 0.0, 0.0, 0.0, 27.20105554], ] ) - if value == -10: - expected = np.array( - [ - [0.0, 138.309264, 222.2164169, 83.90715291, 0.0, 0.0, 0.0, 41.95357645], - [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], - [-9.81, -39.31504182, 15.08706927, 44.59211109, 0.0, 0.0, 0.0, 27.20105554], - ] - ) - npt.assert_almost_equal( - res, - expected, - decimal=5, - ) + npt.assert_almost_equal(res, expected, decimal=5) + @pytest.mark.parametrize("phase_dynamics", [PhaseDynamics.SHARED_DURING_THE_PHASE, PhaseDynamics.ONE_PER_NODE]) @pytest.mark.parametrize("penalty_origin", [ObjectiveFcn.Lagrange, ObjectiveFcn.Mayer, ConstraintFcn]) @pytest.mark.parametrize("value", [0.1, -10]) From 1517dbeb3637ddad58226a9f83a513c8961e2571 Mon Sep 17 00:00:00 2001 From: eve-mac Date: Mon, 28 Oct 2024 10:10:57 -0400 Subject: [PATCH 062/108] some changes requested by ipuch --- bioptim/__init__.py | 4 +- bioptim/dynamics/configure_problem.py | 8 +- bioptim/dynamics/dynamics_functions.py | 4 +- .../example_external_forces.py | 14 +- .../holonomic_constraints/two_pendulums.py | 32 +-- bioptim/misc/enums.py | 6 +- bioptim/misc/external_forces.py | 105 +++---- bioptim/models/biorbd/biorbd_model.py | 8 +- .../models/biorbd/holonomic_biorbd_model.py | 266 +++++++++++------- bioptim/models/biorbd/multi_biorbd_model.py | 2 +- .../models/biorbd/variational_biorbd_model.py | 11 +- bioptim/models/protocols/biomodel.py | 2 +- .../models/protocols/holonomic_biomodel.py | 99 +------ tests/shard1/test_biorbd_model_holonomic.py | 39 +-- tests/shard1/test_dynamics.py | 44 ++- tests/shard1/test_enums.py | 9 +- tests/shard3/test_external_forces.py | 265 +++++++++++------ tests/shard4/test_penalty.py | 2 +- 18 files changed, 469 insertions(+), 451 deletions(-) diff --git a/bioptim/__init__.py b/bioptim/__init__.py index c7b35e1d1..a20e07f2d 100644 --- a/bioptim/__init__.py +++ b/bioptim/__init__.py @@ -213,10 +213,10 @@ PhaseDynamics, OnlineOptim, ReferenceFrame, - ExternalForcesType, + ExternalForceType, ) from .misc.mapping import BiMappingList, BiMapping, Mapping, NodeMapping, NodeMappingList, SelectionMapping, Dependency -from .misc.external_forces import ExternalForces, ExternalForcesList +from .misc.external_forces import ExternalForce, ExternalForces from .optimization.multi_start import MultiStart from .optimization.non_linear_program import NonLinearProgram diff --git a/bioptim/dynamics/configure_problem.py b/bioptim/dynamics/configure_problem.py index b7cf0b50d..02e94f6fd 100644 --- a/bioptim/dynamics/configure_problem.py +++ b/bioptim/dynamics/configure_problem.py @@ -19,7 +19,7 @@ from ..misc.fcn_enum import FcnEnum from ..misc.mapping import BiMapping, Mapping from ..misc.options import UniquePerPhaseOptionList, OptionGeneric -from ..misc.external_forces import ExternalForcesList +from ..misc.external_forces import ExternalForces from ..models.protocols.stochastic_biomodel import StochasticBioModel from ..optimization.problem_type import SocpType @@ -1942,7 +1942,7 @@ def __init__( state_continuity_weight: float | None = None, phase_dynamics: PhaseDynamics = PhaseDynamics.SHARED_DURING_THE_PHASE, numerical_data_timeseries: dict[str, np.ndarray] = None, - external_forces: ExternalForcesList = None, + external_forces: ExternalForces = None, **extra_parameters: Any, ): """ @@ -2003,8 +2003,8 @@ def external_forces_in_numerical_timeseries(self, external_forces): data = force.torque_data.reshape(3, 1, -1) else: data = np.zeros((3, 1, force.len)) - if force.linear_force_data is not None: - data = np.concatenate((data, force.linear_force_data.reshape(3, 1, -1)), axis=0) + if force.force_data is not None: + data = np.concatenate((data, force.force_data.reshape(3, 1, -1)), axis=0) else: data = np.concatenate((data, np.zeros((3, 1, force.len))), axis=0) if force.point_of_application is not None: diff --git a/bioptim/dynamics/dynamics_functions.py b/bioptim/dynamics/dynamics_functions.py index a48746709..cc56e41f3 100644 --- a/bioptim/dynamics/dynamics_functions.py +++ b/bioptim/dynamics/dynamics_functions.py @@ -1136,7 +1136,7 @@ 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) - external_forces = nlp.get_external_forces(states, controls, algebraic_states, numerical_timeseries) - qddot_u = nlp.model.partitioned_forward_dynamics(q_u, qdot_u, tau, external_forces, nlp.parameters.cx) + q_v_init = DM.zeros(nlp.model.nb_dependent_joints) # @ipuch: I'm not sure of this, where q_v_init is supposed to be if not zeros? + qddot_u = nlp.model.partitioned_forward_dynamics()(q_u, qdot_u, q_v_init, tau, nlp.parameters.cx) return DynamicsEvaluation(dxdt=vertcat(qdot_u, qddot_u), defects=None) diff --git a/bioptim/examples/getting_started/example_external_forces.py b/bioptim/examples/getting_started/example_external_forces.py index 0c0c2f77a..50f9dd06f 100644 --- a/bioptim/examples/getting_started/example_external_forces.py +++ b/bioptim/examples/getting_started/example_external_forces.py @@ -25,8 +25,8 @@ OdeSolverBase, Solver, PhaseDynamics, - ExternalForcesList, - ExternalForcesType, + ExternalForces, + ExternalForceType, ReferenceFrame, ) @@ -36,7 +36,7 @@ def prepare_ocp( ode_solver: OdeSolverBase = OdeSolver.RK4(), expand_dynamics: bool = True, phase_dynamics: PhaseDynamics = PhaseDynamics.ONE_PER_NODE, - force_type: ExternalForcesType = ExternalForcesType.LINEAR_FORCE, + force_type: ExternalForceType = ExternalForceType.FORCE, force_reference_frame: ReferenceFrame = ReferenceFrame.GLOBAL, use_point_of_applications: bool = False, point_of_application_reference_frame: ReferenceFrame = ReferenceFrame.GLOBAL, @@ -97,24 +97,22 @@ def prepare_ocp( Seg1_point_of_application = None Test_point_of_application = None - external_forces = ExternalForcesList() + external_forces = ExternalForces() external_forces.add( key="Seg1", # Name of the segment where the external force is applied data=Seg1_force, # 3 x (n_shooting_points+1) array - force_type=force_type, # Type of the external force (ExternalForcesType.LINEAR_FORCE) + force_type=force_type, # Type of the external force (ExternalForceType.FORCE) force_reference_frame=force_reference_frame, # Reference frame of the external force (ReferenceFrame.GLOBAL) point_of_application=Seg1_point_of_application, # Position of the point of application point_of_application_reference_frame=point_of_application_reference_frame, # Reference frame of the point of application (ReferenceFrame.GLOBAL) - phase=0, # Pariterre, did we deal with phases already? ) external_forces.add( key="Test", # Name of the segment where the external force is applied data=Test_force, # 3 x (n_shooting_points+1) array - force_type=force_type, # Type of the external force (ExternalForcesType.LINEAR_FORCE) + force_type=force_type, # Type of the external force (ExternalForceType.FORCE) force_reference_frame=force_reference_frame, # Reference frame of the external force (ReferenceFrame.GLOBAL) point_of_application=Test_point_of_application, # Position of the point of application point_of_application_reference_frame=point_of_application_reference_frame, # Reference frame of the point of application (ReferenceFrame.GLOBAL) - phase=0, ) bio_model = BiorbdModel(biorbd_model_path, external_forces=external_forces) diff --git a/bioptim/examples/holonomic_constraints/two_pendulums.py b/bioptim/examples/holonomic_constraints/two_pendulums.py index c998463b7..a2b32d924 100644 --- a/bioptim/examples/holonomic_constraints/two_pendulums.py +++ b/bioptim/examples/holonomic_constraints/two_pendulums.py @@ -8,7 +8,7 @@ import matplotlib.pyplot as plt import numpy as np -from casadi import MX, Function +from casadi import DM from bioptim import ( BiMappingList, @@ -60,36 +60,18 @@ def compute_all_states(sol, bio_model: HolonomicBiorbdModel): for i, dependent_joint_index in enumerate(bio_model.dependent_joint_index): tau[dependent_joint_index, :-1] = controls["tau"][i, :] - # Partitioned forward dynamics - q_u_sym = MX.sym("q_u_sym", bio_model.nb_independent_joints, 1) - qdot_u_sym = MX.sym("qdot_u_sym", bio_model.nb_independent_joints, 1) - tau_sym = MX.sym("tau_sym", bio_model.nb_tau, 1) - partitioned_forward_dynamics_func = Function( - "partitioned_forward_dynamics", - [q_u_sym, qdot_u_sym, tau_sym], - [bio_model.partitioned_forward_dynamics(q_u_sym, qdot_u_sym, tau_sym)], - ) - # Lagrangian multipliers - q_sym = MX.sym("q_sym", bio_model.nb_q, 1) - qdot_sym = MX.sym("qdot_sym", bio_model.nb_q, 1) - qddot_sym = MX.sym("qddot_sym", bio_model.nb_q, 1) - compute_lambdas_func = Function( - "compute_the_lagrangian_multipliers", - [q_sym, qdot_sym, qddot_sym, tau_sym], - [bio_model.compute_the_lagrangian_multipliers(q_sym, qdot_sym, qddot_sym, tau_sym)], - ) - + q_v_init = DM.zeros(bio_model.nb_dependent_joints) for i in range(n): - q_v_i = bio_model.compute_q_v(states["q_u"][:, i]).toarray() + q_v_i = bio_model.compute_q_v()(states["q_u"][:, i], q_v_init).toarray() q[:, i] = bio_model.state_from_partition(states["q_u"][:, i][:, np.newaxis], q_v_i).toarray().squeeze() - qdot[:, i] = bio_model.compute_qdot(q[:, i], states["qdot_u"][:, i]).toarray().squeeze() + qdot[:, i] = bio_model.compute_qdot()(q[:, i], states["qdot_u"][:, i]).toarray().squeeze() qddot_u_i = ( - partitioned_forward_dynamics_func(states["q_u"][:, i], states["qdot_u"][:, i], tau[:, i]) + bio_model.partitioned_forward_dynamics()(states["q_u"][:, i], states["qdot_u"][:, i], q_v_init, tau[:, i]) .toarray() .squeeze() ) - qddot[:, i] = bio_model.compute_qddot(q[:, i], qdot[:, i], qddot_u_i).toarray().squeeze() - lambdas[:, i] = compute_lambdas_func(q[:, i], qdot[:, i], qddot[:, i], tau[:, i]).toarray().squeeze() + qddot[:, i] = bio_model.compute_qddot()(q[:, i], qdot[:, i], qddot_u_i).toarray().squeeze() + lambdas[:, i] = bio_model.compute_the_lagrangian_multipliers()(states["q_u"][:, i][:, np.newaxis], states["qdot_u"][:, i], q_v_init[:, i], tau[:, i]).toarray().squeeze() return q, qdot, qddot, lambdas diff --git a/bioptim/misc/enums.py b/bioptim/misc/enums.py index 00d08699e..16a528ff6 100644 --- a/bioptim/misc/enums.py +++ b/bioptim/misc/enums.py @@ -225,10 +225,12 @@ class ReferenceFrame(Enum): LOCAL = "local" -class ExternalForcesType(Enum): +class ExternalForceType(Enum): """ Selection of external forces type """ - LINEAR_FORCE = "linear_force" + FORCE = "force" TORQUE = "torque" + TORQUE_AND_FORCE = "torque_and_force" + diff --git a/bioptim/misc/external_forces.py b/bioptim/misc/external_forces.py index 938df6e4a..53ecee7d2 100644 --- a/bioptim/misc/external_forces.py +++ b/bioptim/misc/external_forces.py @@ -3,32 +3,32 @@ import numpy as np from ..misc.options import OptionGeneric, OptionDict -from ..misc.enums import ExternalForcesType, ReferenceFrame +from ..misc.enums import ExternalForceType, ReferenceFrame -class ExternalForces(OptionGeneric): +class ExternalForce(OptionGeneric): def __init__( self, key: str, - linear_force_data: np.ndarray, + force_data: np.ndarray, torque_data: np.ndarray, force_reference_frame: ReferenceFrame, point_of_application_reference_frame: ReferenceFrame, point_of_application: np.ndarray = None, **extra_parameters: Any, ): - super(ExternalForces, self).__init__(**extra_parameters) + super(ExternalForce, self).__init__(**extra_parameters) if self.list_index != -1 and self.list_index is not None: raise NotImplementedError("All external forces must be declared, list_index cannot be used for now.") - if linear_force_data is not None and linear_force_data.shape[0] != 3: - raise ValueError(f"External forces must have 3 rows, got {linear_force_data.shape[0]}") + if force_data is not None and force_data.shape[0] != 3: + raise ValueError(f"External forces must have 3 rows, got {force_data.shape[0]}") if torque_data is not None and torque_data.shape[0] != 3: raise ValueError(f"External torques must have 3 rows, got {torque_data.shape[0]}") self.key = key - self.linear_force_data = linear_force_data + self.force_data = force_data self.torque_data = torque_data self.force_reference_frame = force_reference_frame self.point_of_application = point_of_application @@ -39,33 +39,32 @@ def len(self): """ Returns the number of nodes in the external forces """ - if self.linear_force_data is not None: - return self.linear_force_data.shape[1] + if self.force_data is not None: + return self.force_data.shape[1] elif self.torque_data is not None: return self.torque_data.shape[1] else: - raise ValueError("External forces must have either linear_force_data or torque_data defined") + raise ValueError("External forces must have either force_data or torque_data defined") -class ExternalForcesList(OptionDict): +class ExternalForces(OptionDict): def __init__(self, *args, **kwargs): - super(ExternalForcesList, self).__init__(sub_type=ExternalForces) + super(ExternalForces, self).__init__(sub_type=ExternalForce) def add( self, key: str, - phase: int, data: np.ndarray, - force_type: ExternalForcesType, + force_type: ExternalForceType, force_reference_frame: ReferenceFrame, point_of_application: np.ndarray = None, point_of_application_reference_frame: ReferenceFrame = None, **extra_arguments: Any, ): - if key in self and self[key].phase == phase and self[key].force_type == force_type: + if key in self and self[key].force_type == force_type: raise ValueError( - f"There should be only one external force with the same key, force_type and phase (key:{key}, force_type: {force_type}, phase:{phase} already exists)") + f"There should be only one external force with the same key, force_type (key:{key} and force_type: {force_type} already exists)") if force_reference_frame not in [ReferenceFrame.GLOBAL, ReferenceFrame.LOCAL]: raise ValueError( @@ -78,38 +77,46 @@ def add( if point_of_application is not None: if point_of_application.shape[0] != 3: raise ValueError(f"Point of application must have 3 rows, got {point_of_application.shape[0]}") - if force_type == ExternalForcesType.TORQUE: - raise ValueError("Point of application cannot be used with ExternalForcesType.TORQUE") - - if force_type == ExternalForcesType.TORQUE and force_reference_frame != ReferenceFrame.GLOBAL: - raise ValueError("External torques are defined in global reference frame") - - if force_type == ExternalForcesType.TORQUE and key in self.keys(): - # Do not change the reference frame of the linear force - force_reference_frame = self[key].force_reference_frame - - linear_force_data = None - if key in self.keys() and self[key].linear_force_data is not None: - if force_type == ExternalForcesType.LINEAR_FORCE: - raise ValueError(f"The linear force is already defined for {key}") + if force_type == ExternalForceType.TORQUE: + raise ValueError("Point of application cannot be used with ExternalForceType.TORQUE") + + if key in self.keys(): + # They must both be in the same reference frame + if force_reference_frame != self[key].force_reference_frame: + raise ValueError(f"External forces must be in the same reference frame, got {force_reference_frame} and {self[key].force_reference_frame}") + + force_data = None + if key in self.keys() and self[key].force_data is not None: + if force_type == ExternalForceType.FORCE: + raise ValueError(f"The force is already defined for {key}") else: - linear_force_data = self[key].linear_force_data - elif force_type == ExternalForcesType.LINEAR_FORCE: - linear_force_data = data + force_data = self[key].force_data + point_of_application = self[key].point_of_application + point_of_application_reference_frame = self[key].point_of_application_reference_frame + elif force_type == ExternalForceType.FORCE: + force_data = data torque_data = None if key in self.keys() and self[key].torque_data is not None: - if force_type == ExternalForcesType.TORQUE: + if force_type == ExternalForceType.TORQUE: raise ValueError(f"The torque is already defined for {key}") else: torque_data = self[key].torque_data - elif force_type == ExternalForcesType.TORQUE: + elif force_type == ExternalForceType.TORQUE: torque_data = data - super(ExternalForcesList, self)._add( + if force_type == ExternalForceType.TORQUE_AND_FORCE: + if force_data is not None: + raise ValueError(f"The force is already defined for {key}") + elif torque_data is not None: + raise ValueError(f"The torque is already defined for {key}") + else: + torque_data = data[:3, :] + force_data = data[3:, :] + + super(ExternalForces, self)._add( key=key, - phase=phase, - linear_force_data=linear_force_data, + force_data=force_data, torque_data=torque_data, force_reference_frame=force_reference_frame, point_of_application=point_of_application, @@ -118,28 +125,28 @@ def add( ) def print(self): - raise NotImplementedError("Printing of ExternalForcesList is not ready yet") + raise NotImplementedError("Printing of ExternalForces is not ready yet") -def get_external_forces_segments(external_forces: ExternalForcesList): +def get_external_forces_segments(external_forces: ExternalForces): segments_to_apply_forces_in_global = [] segments_to_apply_forces_in_local = [] segments_to_apply_translational_forces = [] if external_forces is not None: for key in external_forces.keys(): - force = external_forces[key] + force_torsor = external_forces[key] # Check sanity first - if force.force_reference_frame == ReferenceFrame.GLOBAL and force.point_of_application_reference_frame == ReferenceFrame.LOCAL and force.torque_data is not None: + if force_torsor.force_reference_frame == ReferenceFrame.GLOBAL and force_torsor.point_of_application_reference_frame == ReferenceFrame.LOCAL and force.torque_data is not None: raise NotImplementedError("External forces in global reference frame cannot have a point of application in the local reference frame and torques defined at the same time yet") - elif force.force_reference_frame == ReferenceFrame.LOCAL and force.point_of_application_reference_frame == ReferenceFrame.GLOBAL: + elif force_torsor.force_reference_frame == ReferenceFrame.LOCAL and force_torsor.point_of_application_reference_frame == ReferenceFrame.GLOBAL: raise NotImplementedError("External forces in local reference frame cannot have a point of application in the global reference frame yet") - if force.force_reference_frame == ReferenceFrame.GLOBAL and (force.point_of_application_reference_frame == ReferenceFrame.GLOBAL or force.point_of_application_reference_frame is None): - segments_to_apply_forces_in_global.append(force.key) - elif force.force_reference_frame == ReferenceFrame.LOCAL and (force.point_of_application_reference_frame == ReferenceFrame.LOCAL or force.point_of_application_reference_frame is None): - segments_to_apply_forces_in_local.append(force.key) - elif force.force_reference_frame == ReferenceFrame.GLOBAL and force.point_of_application_reference_frame == ReferenceFrame.LOCAL: - segments_to_apply_translational_forces.append(force.key) + if force_torsor.force_reference_frame == ReferenceFrame.GLOBAL and (force_torsor.point_of_application_reference_frame == ReferenceFrame.GLOBAL or force.point_of_application_reference_frame is None): + segments_to_apply_forces_in_global.append(force_torsor.key) + elif force_torsor.force_reference_frame == ReferenceFrame.LOCAL and (force_torsor.point_of_application_reference_frame == ReferenceFrame.LOCAL or force.point_of_application_reference_frame is None): + segments_to_apply_forces_in_local.append(force_torsor.key) + elif force_torsor.force_reference_frame == ReferenceFrame.GLOBAL and force_torsor.point_of_application_reference_frame == ReferenceFrame.LOCAL: + segments_to_apply_translational_forces.append(force_torsor.key) else: raise ValueError("This should not happen") diff --git a/bioptim/models/biorbd/biorbd_model.py b/bioptim/models/biorbd/biorbd_model.py index 5e9c79ba6..dcdca885f 100644 --- a/bioptim/models/biorbd/biorbd_model.py +++ b/bioptim/models/biorbd/biorbd_model.py @@ -13,8 +13,7 @@ from ...limits.path_conditions import Bounds from ...misc.mapping import BiMapping, BiMappingList from ...misc.utils import check_version -from ...misc.external_forces import ExternalForcesList, get_external_forces_segments -from ...misc.enums import ReferenceFrame +from ...misc.external_forces import ExternalForces, get_external_forces_segments from ...optimization.parameters import ParameterList check_version(biorbd, "1.11.1", "1.12.0") @@ -29,7 +28,7 @@ def __init__( self, bio_model: str | biorbd.Model, friction_coefficients: np.ndarray = None, - external_forces: ExternalForcesList = None, + external_forces: ExternalForces = None, parameters: ParameterList = None, nb_supplementary_forces_in_global=0, ): @@ -878,7 +877,6 @@ def soft_contact_forces(self) -> Function: biorbd_return = MX.zeros(self.nb_soft_contacts * 6, 1) for i_sc in range(self.nb_soft_contacts): soft_contact = self.soft_contact(i_sc) - # @ipuch: please confirm / help with the following lines biorbd_return[i_sc * 6 : (i_sc + 1) * 6, :] = ( biorbd.SoftContactSphere(soft_contact).computeForceAtOrigin(self.model, q_biorbd, qdot_biorbd).to_mx() ) @@ -1039,7 +1037,7 @@ def lagrangian(self) -> Function: ) return casadi_fun - def partitioned_forward_dynamics(self, forces_in_global=None, f_contacts=None, q_v_init=None): + def partitioned_forward_dynamics(self): raise NotImplementedError("partitioned_forward_dynamics is not implemented for BiorbdModel") @staticmethod diff --git a/bioptim/models/biorbd/holonomic_biorbd_model.py b/bioptim/models/biorbd/holonomic_biorbd_model.py index b67f210b9..60c9c1ed5 100644 --- a/bioptim/models/biorbd/holonomic_biorbd_model.py +++ b/bioptim/models/biorbd/holonomic_biorbd_model.py @@ -3,9 +3,8 @@ import biorbd_casadi as biorbd from biorbd_casadi import ( GeneralizedCoordinates, - GeneralizedVelocity, ) -from casadi import MX, SX, DM, vertcat, horzcat, Function, solve, rootfinder, inv +from casadi import MX, DM, vertcat, horzcat, Function, solve, rootfinder, inv from .biorbd_model import BiorbdModel from ..holonomic_constraints import HolonomicConstraintsList @@ -34,6 +33,16 @@ def __init__( self._dependent_joint_index = [] self._independent_joint_index = [i for i in range(self.nb_q)] + # Declaration of MX variables of the right shape for the creation of CasADi Functions + self.q_u = MX.sym("q_u_mx", self.nb_independent_joints, 1) + self.qdot_u = MX.sym("qdot_u_mx", self.nb_independent_joints, 1) + self.qddot_u = MX.sym("qddot_u_mx", self.nb_independent_joints, 1) + self.q_v = MX.sym("q_v_mx", self.nb_dependent_joints, 1) + self.qdot_v = MX.sym("qdot_v_mx", self.nb_dependent_joints, 1) + self.qddot_v = MX.sym("qddot_v_mx", self.nb_dependent_joints, 1) + self.q_v_init = MX.sym("q_v_init_mx", self.nb_dependent_joints, 1) + + def set_newton_tol(self, newton_tol: float): self._newton_tol = newton_tol @@ -93,8 +102,7 @@ def set_holonomic_configuration( self.check_dependant_jacobian() def check_dependant_jacobian(self): - q_test = MX.sym("q_test", self.nb_q) - partitioned_constraints_jacobian = self.partitioned_constraints_jacobian(q_test) + partitioned_constraints_jacobian = self.partitioned_constraints_jacobian(self.q) partitioned_constraints_jacobian_v = partitioned_constraints_jacobian[:, self.nb_independent_joints :] shape = partitioned_constraints_jacobian_v.shape if shape[0] != shape[1]: @@ -137,31 +145,22 @@ def nb_holonomic_constraints(self): def has_holonomic_constraints(self): return self.nb_holonomic_constraints > 0 - def holonomic_constraints(self, q: MX): + def holonomic_constraints(self, q: MX) -> MX: return vertcat(*[c(q) for c in self._holonomic_constraints]) - def holonomic_constraints_jacobian(self, q: MX): + def holonomic_constraints_jacobian(self, q: MX) -> MX: return vertcat(*[c(q) for c in self._holonomic_constraints_jacobians]) - def holonomic_constraints_derivative(self, q: MX, qdot: MX): + def holonomic_constraints_derivative(self, q: MX, qdot: MX) -> MX: return self.holonomic_constraints_jacobian(q) @ qdot - def holonomic_constraints_double_derivative(self, q: MX, qdot: MX, qddot: MX): + def holonomic_constraints_double_derivative(self, q: MX, qdot: MX, qddot: MX) -> MX: return vertcat(*[c(q, qdot, qddot) for c in self._holonomic_constraints_double_derivatives]) - def constrained_forward_dynamics(self, q, qdot, tau, external_forces=None, f_contacts=None) -> MX: - # @ipuch does this stays contrained_ - if external_forces is not None: - raise NotImplementedError("External forces are not implemented yet.") - if f_contacts is not None: - raise NotImplementedError("Contact forces are not implemented yet.") - external_forces_set = self.model.externalForceSet() - - q_biorbd = GeneralizedCoordinates(q) - qdot_biorbd = GeneralizedVelocity(qdot) + def constrained_forward_dynamics(self) -> Function: - mass_matrix = self.model.massMatrix(q_biorbd).to_mx() - constraints_jacobian = self.holonomic_constraints_jacobian(q) + mass_matrix = self.mass_matrix()(self.q, self.parameters) + constraints_jacobian = self.holonomic_constraints_jacobian(self.q) constraints_jacobian_transpose = constraints_jacobian.T # compute the matrix DAE @@ -175,12 +174,12 @@ def constrained_forward_dynamics(self, q, qdot, tau, external_forces=None, f_con ) # compute b vector - tau_augmented = tau - self.model.NonLinearEffect(q_biorbd, qdot_biorbd, external_forces_set).to_mx() + tau_augmented = self.tau - self.non_linear_effects()(self.q, self.qdot, self.parameters) - biais = -self.holonomic_constraints_jacobian(qdot) @ qdot + biais = -self.holonomic_constraints_jacobian(self.qdot) @ self.qdot if self.stabilization: - biais -= self.alpha * self.holonomic_constraints(q) + self.beta * self.holonomic_constraints_derivative( - q, qdot + biais -= self.alpha * self.holonomic_constraints(self.q) + self.beta * self.holonomic_constraints_derivative( + self.q, self.qdot ) tau_augmented = vertcat(tau_augmented, biais) @@ -188,12 +187,22 @@ def constrained_forward_dynamics(self, q, qdot, tau, external_forces=None, f_con # solve with casadi Ax = b x = solve(mass_matrix_augmented, tau_augmented, "symbolicqr") - return x[: self.nb_qddot] + biorbd_return = x[: self.nb_qddot] + + casadi_fun = Function( + "constrained_forward_dynamics", + [self.q, self.qdot, self.tau, self.parameters], + [biorbd_return], + ["q", "qdot", "tau", "parameters"], + ["qddot"], + ) + return casadi_fun - def partitioned_mass_matrix(self, q): + def partitioned_mass_matrix(self, q: MX) -> MX: # q_u: independent # q_v: dependent - mass_matrix = self.model.massMatrix(q).to_mx() + # @ Ipuch: either we send the parameters as an arg of the function or we return a Function... + mass_matrix = self.mass_matrix()(q, []) mass_matrix_uu = mass_matrix[self._independent_joint_index, self._independent_joint_index] mass_matrix_uv = mass_matrix[self._independent_joint_index, self._dependent_joint_index] mass_matrix_vu = mass_matrix[self._dependent_joint_index, self._independent_joint_index] @@ -204,11 +213,8 @@ def partitioned_mass_matrix(self, q): return vertcat(first_line, second_line) - def partitioned_non_linear_effect(self, q, qdot, f_ext=None, f_contacts=None): - if f_ext is not None and f_ext.shape[0] != 0: - raise NotImplementedError("External forces are not implemented yet.") - if f_contacts is not None and f_contacts.shape[0] != 0: - raise NotImplementedError("Contact forces are not implemented yet.") + def partitioned_non_linear_effect(self, q: MX, qdot: MX) -> MX: + # @Ipuch: Not sure external forces work external_forces_set = self.model.externalForceSet() non_linear_effect = self.model.NonLinearEffect(q, qdot, external_forces_set).to_mx() non_linear_effect_u = non_linear_effect[self._independent_joint_index] @@ -216,34 +222,32 @@ def partitioned_non_linear_effect(self, q, qdot, f_ext=None, f_contacts=None): return vertcat(non_linear_effect_u, non_linear_effect_v) - def partitioned_q(self, q): + def partitioned_q(self, q: MX) -> MX: q_u = q[self._independent_joint_index] q_v = q[self._dependent_joint_index] return vertcat(q_u, q_v) - def partitioned_qdot(self, qdot): + def partitioned_qdot(self, qdot: MX) -> MX: qdot_u = qdot[self._independent_joint_index] qdot_v = qdot[self._dependent_joint_index] return vertcat(qdot_u, qdot_v) - def partitioned_tau(self, tau): + def partitioned_tau(self, tau: MX) -> MX: tau_u = tau[self._independent_joint_index] tau_v = tau[self._dependent_joint_index] return vertcat(tau_u, tau_v) - def partitioned_constraints_jacobian(self, q): + def partitioned_constraints_jacobian(self, q: MX) -> MX: constrained_jacobian = self.holonomic_constraints_jacobian(q) constrained_jacobian_u = constrained_jacobian[:, self._independent_joint_index] constrained_jacobian_v = constrained_jacobian[:, self._dependent_joint_index] return horzcat(constrained_jacobian_u, constrained_jacobian_v) - def partitioned_forward_dynamics( - self, q_u, qdot_u, tau, external_forces=None, f_contacts=None, q_v_init=None - ) -> MX: + def partitioned_forward_dynamics(self) -> Function: """ Sources ------- @@ -251,14 +255,10 @@ def partitioned_forward_dynamics( ROBOTRAN: a powerful symbolic gnerator of multibody models, Mech. Sci., 4, 199–219, https://doi.org/10.5194/ms-4-199-2013, 2013. """ - if external_forces is not None and external_forces.shape[0] != 0: - raise NotImplementedError("External forces are not implemented yet.") - if f_contacts is not None and f_contacts.shape[0] != 0: - raise NotImplementedError("Contact forces are not implemented yet.") # compute q and qdot - q = self.compute_q(q_u, q_v_init=q_v_init) - qdot = self.compute_qdot(q, qdot_u) + q = self.compute_q()(self.q_u, self.q_v_init) + qdot = self.compute_qdot()(q, self.qdot_u) partitioned_mass_matrix = self.partitioned_mass_matrix(q) m_uu = partitioned_mass_matrix[: self.nb_independent_joints, : self.nb_independent_joints] @@ -276,14 +276,14 @@ def partitioned_forward_dynamics( second_term = m_uv + coupling_matrix_vu.T @ m_vv # compute the non-linear effect - non_linear_effect = self.partitioned_non_linear_effect(q, qdot, external_forces, f_contacts) + non_linear_effect = self.partitioned_non_linear_effect(q, qdot) non_linear_effect_u = non_linear_effect[: self.nb_independent_joints] non_linear_effect_v = non_linear_effect[self.nb_independent_joints :] modified_non_linear_effect = non_linear_effect_u + coupling_matrix_vu.T @ non_linear_effect_v # compute the tau - partitioned_tau = self.partitioned_tau(tau) + partitioned_tau = self.partitioned_tau(self.tau) tau_u = partitioned_tau[: self.nb_independent_joints] tau_v = partitioned_tau[self.nb_independent_joints :] @@ -293,7 +293,13 @@ def partitioned_forward_dynamics( modified_generalized_forces - second_term @ self.biais_vector(q, qdot) - modified_non_linear_effect ) - return qddot_u + casadi_fun = Function("partitioned_forward_dynamics", + [self.q_u, self.qdot_u, self.q_v_init, self.tau], + [qddot_u], + ["q_u", "qdot_u", "q_v_init", "tau"], + ["qddot_u"]) + + return casadi_fun def coupling_matrix(self, q: MX) -> MX: """ @@ -357,22 +363,19 @@ def check_state_v_size(self, state_v): if state_v.shape[0] != self.nb_dependent_joints: raise ValueError(f"Length of state v size should be: {self.nb_dependent_joints}. Got: {state_v.shape[0]}") - def compute_q_v(self, q_u: MX | SX | DM, q_v_init: MX | SX | DM = None) -> MX | SX | DM: + def compute_q_v(self) -> Function: """ - Compute the dependent joint positions from the independent joint positions. - This function might be misleading because it can be used for numerical purpose with DM - or for symbolic purpose with MX. The return type is not enforced. + Compute the dependent joint positions (q_v) from the independent joint positions (q_u). """ - decision_variables_sym = MX.sym("decision_variables_sym", self.nb_dependent_joints) - q_u_sym = MX.sym("q_u_sym", q_u.shape[0], q_u.shape[1]) - q = self.state_from_partition(q_u_sym, decision_variables_sym) + q_v_sym = MX.sym("q_v_sym", self.nb_dependent_joints) + q_u_sym = MX.sym("q_u_sym", self.q_u.shape[0], self.q_u.shape[1]) + q = self.state_from_partition(q_u_sym, q_v_sym) mx_residuals = self.holonomic_constraints(q) - q_v_init = MX.zeros(self.nb_dependent_joints) if q_v_init is None else q_v_init - ifcn_input = (q_v_init, q_u) + ifcn_input = (self.q_v_init, self.q_u) residuals = Function( "final_states_residuals", - [decision_variables_sym, q_u_sym], + [q_v_sym, q_u_sym], [mx_residuals], ).expand() @@ -381,25 +384,54 @@ def compute_q_v(self, q_u: MX | SX | DM, q_v_init: MX | SX | DM = None) -> MX | ifcn = rootfinder("ifcn", "newton", residuals, opts) v_opt = ifcn(*ifcn_input) - return v_opt - - def compute_q(self, q_u: MX, q_v_init: MX = None) -> MX: - q_v = self.compute_q_v(q_u, q_v_init) - return self.state_from_partition(q_u, q_v) - - def compute_qdot_v(self, q: MX, qdot_u: MX) -> MX: - coupling_matrix_vu = self.coupling_matrix(q) - return coupling_matrix_vu @ qdot_u - - def _compute_qdot_v(self, q_u: MX, qdot_u: MX) -> MX: - q = self.compute_q(q_u) - return self.compute_qdot_v(q, qdot_u) - - def compute_qdot(self, q: MX, qdot_u: MX) -> MX: - qdot_v = self.compute_qdot_v(q, qdot_u) - return self.state_from_partition(qdot_u, qdot_v) - - def compute_qddot_v(self, q: MX, qdot: MX, qddot_u: MX) -> MX: + casadi_fun = Function("compute_q_v", + [self.q_u, self.q_v_init], + [v_opt], + ["q_u", "q_v_init"], + ["q_v"]) + return casadi_fun + + def compute_q(self) -> Function: + q_v = self.compute_q_v()(self.q_u, self.q_v_init) + biorbd_return = self.state_from_partition(self.q_u, q_v) + casadi_fun = Function("compute_q", + [self.q_u, self.q_v_init], + [biorbd_return], + ["q_u", "q_v_init"], + ["q"]) + return casadi_fun + + def compute_qdot_v(self) -> Function: + coupling_matrix_vu = self.coupling_matrix(self.q) + biorbd_return = coupling_matrix_vu @ self.qdot_u + casadi_fun = Function("compute_qdot_v", + [self.q, self.qdot_u], + [biorbd_return], + ["q", "qdot_u"], + ["qdot_v"]) + return casadi_fun + + def _compute_qdot_v(self) -> Function: + q = self.compute_q()(self.q_u, self.q_v_init) + biorbd_return = self.compute_qdot_v()(q, self.qdot_u) + casadi_fun = Function("compute_qdot_v", + [self.q, self.qdot_u, self.q_v_init], + [biorbd_return], + ["q", "qdot_u"], + ["qdot_v"]) + return casadi_fun + + def compute_qdot(self) -> Function: + qdot_v = self.compute_qdot_v()(self.q, self.qdot_u) + biorbd_return = self.state_from_partition(self.qdot_u, qdot_v) + casadi_fun = Function("compute_qdot", + [self.q, self.qdot_u], + [biorbd_return], + ["q", "qdot_u"], + ["qdot"]) + return casadi_fun + + def compute_qddot_v(self) -> Function: """ Sources ------- @@ -408,10 +440,16 @@ def compute_qddot_v(self, q: MX, qdot: MX, qddot_u: MX) -> MX: https://doi.org/10.5194/ms-4-199-2013, 2013. Equation (17) in the paper. """ - coupling_matrix_vu = self.coupling_matrix(q) - return coupling_matrix_vu @ qddot_u + self.biais_vector(q, qdot) - - def compute_qddot(self, q: MX, qdot: MX, qddot_u: MX) -> MX: + coupling_matrix_vu = self.coupling_matrix(self.q) + biorbd_return = coupling_matrix_vu @ self.qddot_u + self.biais_vector(self.q, self.qdot) + casadi_fun = Function("compute_qddot_v", + [self.q, self.qdot, self.qddot_u], + [biorbd_return], + ["q", "qdot", "qddot_u"], + ["qddot_v"]) + return casadi_fun + + def compute_qddot(self) -> Function: """ Sources ------- @@ -420,12 +458,16 @@ def compute_qddot(self, q: MX, qdot: MX, qddot_u: MX) -> MX: https://doi.org/10.5194/ms-4-199-2013, 2013. Equation (17) in the paper. """ - qddot_v = self.compute_qddot_v(q, qdot, qddot_u) - return self.state_from_partition(qddot_u, qddot_v) - - def compute_the_lagrangian_multipliers( - self, q_u: MX, qdot_u: MX, tau: MX, external_forces: MX = None, f_contacts: MX = None - ) -> MX: + qddot_v = self.compute_qddot_v()(self.q, self.qdot, self.qddot_u) + biorbd_return = self.state_from_partition(self.qddot_u, qddot_v) + casadi_fun = Function("compute_qddot", + [self.q, self.qdot, self.qddot_u], + [biorbd_return], + ["q", "qdot", "qddot_u"], + ["qddot"]) + return casadi_fun + + def compute_the_lagrangian_multipliers(self) -> Function: """ Sources ------- @@ -433,16 +475,20 @@ def compute_the_lagrangian_multipliers( ROBOTRAN: a powerful symbolic gnerator of multibody models, Mech. Sci., 4, 199–219, https://doi.org/10.5194/ms-4-199-2013, 2013. """ - q = self.compute_q(q_u) - qdot = self.compute_qdot(q, qdot_u) - qddot_u = self.partitioned_forward_dynamics(q_u, qdot_u, tau, external_forces, f_contacts) - qddot = self.compute_qddot(q, qdot, qddot_u) - - return self._compute_the_lagrangian_multipliers(q, qdot, qddot, tau, external_forces, f_contacts) - - def _compute_the_lagrangian_multipliers( - self, q: MX, qdot: MX, qddot: MX, tau: MX, external_forces: MX = None, f_contacts: MX = None - ) -> MX: + q = self.compute_q()(self.q_u, self.q_v_init) + qdot = self.compute_qdot()(self.q, self.qdot_u) + qddot_u = self.partitioned_forward_dynamics()(self.q_u, self.qdot_u, self.q_v_init, self.tau) + qddot = self.compute_qddot()(q, qdot, qddot_u) + + biorbd_return = self._compute_the_lagrangian_multipliers()(q, qdot, qddot, self.tau) + casadi_fun = Function("compute_the_lagrangian_multipliers", + [self.q_u, self.qdot_u, self.q_v_init, self.tau], + [biorbd_return], + ["q_u", "qdot_u", "q_v_init", "tau"], + ["lambda"]) + return casadi_fun + + def _compute_the_lagrangian_multipliers(self) -> Function: """ Sources ------- @@ -451,27 +497,31 @@ def _compute_the_lagrangian_multipliers( https://doi.org/10.5194/ms-4-199-2013, 2013. Equation (17) in the paper. """ - if external_forces is not None: - raise NotImplementedError("External forces are not implemented yet.") - if f_contacts is not None: - raise NotImplementedError("Contact forces are not implemented yet.") - partitioned_constraints_jacobian = self.partitioned_constraints_jacobian(q) + + partitioned_constraints_jacobian = self.partitioned_constraints_jacobian(self.q) partitioned_constraints_jacobian_v = partitioned_constraints_jacobian[:, self.nb_independent_joints :] partitioned_constraints_jacobian_v_t_inv = inv(partitioned_constraints_jacobian_v.T) - partitioned_mass_matrix = self.partitioned_mass_matrix(q) + partitioned_mass_matrix = self.partitioned_mass_matrix(self.q) m_vu = partitioned_mass_matrix[self.nb_independent_joints :, : self.nb_independent_joints] m_vv = partitioned_mass_matrix[self.nb_independent_joints :, self.nb_independent_joints :] - qddot_u = qddot[self._independent_joint_index] - qddot_v = qddot[self._dependent_joint_index] + qddot_u = self.qddot[self._independent_joint_index] + qddot_v = self.qddot[self._dependent_joint_index] - non_linear_effect = self.partitioned_non_linear_effect(q, qdot, external_forces, f_contacts) + non_linear_effect = self.partitioned_non_linear_effect(self.q, self.qdot) non_linear_effect_v = non_linear_effect[self.nb_independent_joints :] - partitioned_tau = self.partitioned_tau(tau) + partitioned_tau = self.partitioned_tau(self.tau) partitioned_tau_v = partitioned_tau[self.nb_independent_joints :] - return partitioned_constraints_jacobian_v_t_inv @ ( + biorbd_return = partitioned_constraints_jacobian_v_t_inv @ ( m_vu @ qddot_u + m_vv @ qddot_v + non_linear_effect_v - partitioned_tau_v ) + casadi_fun = Function("_compute_the_lagrangian_multipliers", + [self.q, self.qdot, self.qddot, self.tau], + [biorbd_return], + ["q", "qdot", "qddot", "tau"], + ["lambda"]) + return casadi_fun + diff --git a/bioptim/models/biorbd/multi_biorbd_model.py b/bioptim/models/biorbd/multi_biorbd_model.py index 2d82fc60f..c40fb63cb 100644 --- a/bioptim/models/biorbd/multi_biorbd_model.py +++ b/bioptim/models/biorbd/multi_biorbd_model.py @@ -965,7 +965,7 @@ def _var_mapping(self, key: str, range_for_mapping: int | list | tuple | range, def lagrangian(self): raise NotImplementedError("lagrangian is not implemented yet for MultiBiorbdModel") - def partitioned_forward_dynamics(self, q_u, qdot_u, tau, external_forces=None, f_contacts=None, q_v_init=None): + def partitioned_forward_dynamics(self, q_u, qdot_u, q_v_init, tau): raise NotImplementedError("partitioned_forward_dynamics is not implemented yet for MultiBiorbdModel") @staticmethod diff --git a/bioptim/models/biorbd/variational_biorbd_model.py b/bioptim/models/biorbd/variational_biorbd_model.py index c4917d8e0..84db8a3a6 100644 --- a/bioptim/models/biorbd/variational_biorbd_model.py +++ b/bioptim/models/biorbd/variational_biorbd_model.py @@ -3,7 +3,7 @@ """ import biorbd_casadi as biorbd -from casadi import SX, MX, vertcat, jacobian, transpose +from casadi import SX, MX, DM, vertcat, jacobian, transpose from .holonomic_biorbd_model import HolonomicBiorbdModel from ...misc.enums import ControlType, QuadratureRule @@ -29,12 +29,13 @@ def __init__( self.control_type = control_type self.control_discrete_approximation = control_discrete_approximation + def discrete_lagrangian( self, - q1: MX | SX, - q2: MX | SX, - time_step: MX | SX, - ) -> MX | SX: + q1: MX | SX | DM, + q2: MX | SX | DM, + time_step: MX | SX | DM, + ) -> MX | SX | DM: """ Compute the discrete Lagrangian of a biorbd model. diff --git a/bioptim/models/protocols/biomodel.py b/bioptim/models/protocols/biomodel.py index e341fed30..e188a5006 100644 --- a/bioptim/models/protocols/biomodel.py +++ b/bioptim/models/protocols/biomodel.py @@ -367,7 +367,7 @@ def lagrangian(self) -> Function: The Lagrangian. """ - def partitioned_forward_dynamics(self, q_u, qdot_u, tau, external_forces=None, q_v_init=None) -> Function: + def partitioned_forward_dynamics(self, q_u, qdot_u, q_v_init, tau) -> Function: """ @ipuch: I need help on how to implement this! This is the forward dynamics of the model, but only for the independent joints diff --git a/bioptim/models/protocols/holonomic_biomodel.py b/bioptim/models/protocols/holonomic_biomodel.py index cc94f860b..91717c29c 100644 --- a/bioptim/models/protocols/holonomic_biomodel.py +++ b/bioptim/models/protocols/holonomic_biomodel.py @@ -200,7 +200,7 @@ def partitioned_mass_matrix(self, q: MX) -> MX: The partitioned mass matrix, reordered in function independent and dependent joints """ - def partitioned_non_linear_effect(self, q: MX, qdot: MX, f_ext=None, f_contacts=None) -> MX: + def partitioned_non_linear_effect(self, q: MX, qdot: MX) -> MX: """ This function returns the partitioned non-linear effect, reordered in function independent and dependent joints @@ -327,57 +327,21 @@ def state_from_partition(self, state_u: MX, state_v: MX) -> MX: https://doi.org/10.5194/ms-4-199-2013, 2013. """ - def compute_q_v(self, q_u: MX | DM, q_v_init: MX | DM = None) -> MX | DM: + def compute_q_v(self) -> Function: """ Compute the dependent joint from the independent joint, This is done by solving the system of equations given by the holonomic constraints At the end of this step, we get admissible generalized coordinates w.r.t. the holonomic constraints - - Parameters - ---------- - q_u: MX | DM - The generalized coordinates - q_v_init: MX | DM - The initial guess for the dependent joint coordinates - - Returns - ------- - MX | DM - The dependent joint """ - def compute_q(self, q_u: MX, q_v_init: MX = None) -> MX: + def compute_q(self) -> Function: """ Compute the generalized coordinates from the independent joint coordinates - - Parameters - ---------- - q_u: MX - The independent joint coordinates - q_v_init: MX - The initial guess for the dependent joint coordinates - - Returns - ------- - MX - The generalized coordinates """ - def compute_qdot_v(self, q: MX, qdot_u: MX) -> MX: + def compute_qdot_v(self) -> Function: """ Compute the dependent joint velocities from the independent joint velocities and the positions. - - Parameters - ---------- - q: MX - The generalized coordinates - qdot_u: MX - The independent joint velocities - - Returns - ------- - MX - The dependent joint velocities """ def compute_qdot(self, q: MX, qdot_u: MX) -> MX: @@ -397,25 +361,11 @@ def compute_qdot(self, q: MX, qdot_u: MX) -> MX: The dependent joint velocities """ - def compute_qddot_v(self, q: MX, qdot: MX, qddot_u: MX) -> MX: + def compute_qddot_v(self) -> Function: """ Compute the dependent joint accelerations from the independent joint accelerations and the velocities and positions. - Parameters - ---------- - q: MX - The generalized coordinates - qdot: - The generalized velocities - qddot_u: - The independent joint accelerations - - Returns - ------- - MX - The dependent joint accelerations - Sources ------- Docquier, N., Poncelet, A., and Fisette, P.: @@ -424,24 +374,10 @@ def compute_qddot_v(self, q: MX, qdot: MX, qddot_u: MX) -> MX: Equation (17) in the paper. """ - def compute_qddot(self, q: MX, qdot: MX, qddot_u: MX) -> MX: + def compute_qddot(self) -> Function: """ Compute the accelerations from the independent joint accelerations and the velocities and positions. - Parameters - ---------- - q: MX - The generalized coordinates - qdot: - The generalized velocities - qddot_u: - The independent joint accelerations - - Returns - ------- - MX - The generalized accelerations - Sources ------- Docquier, N., Poncelet, A., and Fisette, P.: @@ -450,30 +386,9 @@ def compute_qddot(self, q: MX, qdot: MX, qddot_u: MX) -> MX: Equation (17) in the paper. """ - def compute_the_lagrangian_multipliers( - self, q: MX, qdot: MX, qddot: MX, tau: MX, external_forces: MX = None, f_contacts: MX = None - ) -> MX: + def compute_the_lagrangian_multipliers(self) -> Function: """ Compute the Lagrangian multiplier, denoted lambda in the paper: - Parameters - ---------- - q: MX - The generalized coordinates - qdot: MX - The generalized velocities - qddot: MX - The generalized accelerations - tau: MX - The generalized torques - external_forces: MX - The external forces - f_contacts: MX - The contact forces - - Returns - ------- - MX - The Lagrangian multipliers Sources ------- diff --git a/tests/shard1/test_biorbd_model_holonomic.py b/tests/shard1/test_biorbd_model_holonomic.py index a6d672faf..831cda2b4 100644 --- a/tests/shard1/test_biorbd_model_holonomic.py +++ b/tests/shard1/test_biorbd_model_holonomic.py @@ -3,7 +3,7 @@ import numpy as np import numpy.testing as npt import pytest -from casadi import DM, MX, Function +from casadi import DM, MX from bioptim import HolonomicBiorbdModel, HolonomicConstraintsFcn, HolonomicConstraintsList, Solver, SolutionMerge from tests.utils import TestUtils @@ -103,7 +103,7 @@ def test_model_holonomic(): ) TestUtils.assert_equal(model.holonomic_constraints_derivative(q, q_dot), [-7.65383105, -0.44473154]) TestUtils.assert_equal(model.holonomic_constraints_double_derivative(q, q_dot, q_ddot), [10.23374996, -11.73729905]) - TestUtils.assert_equal(model.constrained_forward_dynamics(q, q_dot, tau), [-5.18551845, -3.01921376, 25.79451813]) + TestUtils.assert_equal(model.constrained_forward_dynamics()(q, q_dot, tau, []), [-5.18551845, -3.01921376, 25.79451813]) TestUtils.assert_equal( model.partitioned_mass_matrix(q), [ @@ -124,44 +124,29 @@ def test_model_holonomic(): q_u = MX(TestUtils.to_array(q[model._independent_joint_index])) qdot_u = MX(TestUtils.to_array(q_dot[model._independent_joint_index])) q_v = MX(TestUtils.to_array(q[model._dependent_joint_index])) - q_u_sym = MX.sym("q_u_sym", model.nb_independent_joints, 1) - qdot_u_sym = MX.sym("qdot_u_sym", model.nb_independent_joints, 1) - tau_sym = MX.sym("tau_sym", model.nb_tau, 1) - - partitioned_forward_dynamics_func = Function( - "partitioned_forward_dynamics", - [q_u_sym, qdot_u_sym, tau_sym], - [model.partitioned_forward_dynamics(q_u_sym, qdot_u_sym, tau_sym, q_v_init=q_v)], - ) - TestUtils.assert_equal(partitioned_forward_dynamics_func(q_u, qdot_u, tau), -1.101808, expand=False) + + TestUtils.assert_equal(model.partitioned_forward_dynamics()(q_u, qdot_u, q_v, tau), -1.101808) TestUtils.assert_equal(model.coupling_matrix(q), [5.79509793, -0.35166415], expand=False) TestUtils.assert_equal(model.biais_vector(q, q_dot), [27.03137348, 23.97095718], expand=False) TestUtils.assert_equal(model.state_from_partition(q_u, q_v), q) - compute_v_from_u_func = Function("compute_q_v", [q_u_sym], [model.compute_q_v(q_u_sym, q_v_init=q_v)]) - TestUtils.assert_equal(compute_v_from_u_func(q_u), [2 * np.pi / 3, 2 * np.pi / 3], expand=False) - compute_q_func = Function("compute_q", [q_u_sym], [model.compute_q_v(q_u_sym, q_v_init=q_v)]) - TestUtils.assert_equal(compute_q_func(q_u), [2.0943951, 2.0943951], expand=False) - TestUtils.assert_equal(model.compute_qdot_v(q, qdot_u), [23.18039172, -1.4066566], expand=False) - TestUtils.assert_equal(model.compute_qdot(q, qdot_u), [4.0, 23.18039172, -1.4066566], expand=False) + TestUtils.assert_equal(model.compute_q_v()(q_u, q_v), [2 * np.pi / 3, 2 * np.pi / 3]) + TestUtils.assert_equal(model.compute_q()(q_u, q_v), [2.0943951, 2.0943951], expand=False) + TestUtils.assert_equal(model.compute_qdot_v()(q, qdot_u), [23.18039172, -1.4066566], expand=False) + TestUtils.assert_equal(model.compute_qdot()(q, qdot_u), [4.0, 23.18039172, -1.4066566], expand=False) npt.assert_almost_equal( - model.compute_q_v(DM([0.0]), q_v_init=DM([1.0, 1.0])).toarray().squeeze(), + model.compute_q_v()(DM([0.0]), DM([1.0, 1.0])).toarray().squeeze(), np.array([2 * np.pi / 3, 2 * np.pi / 3]), decimal=6, ) TestUtils.assert_equal( - model._compute_the_lagrangian_multipliers(q, q_dot, q_ddot, tau), [20.34808, 27.119224], expand=False - ) - compute_the_lagrangian_multipliers = Function( - "compute_the_lagrangian_multipliers", - [q_u_sym, qdot_u_sym, tau_sym], - [model.compute_the_lagrangian_multipliers(q_u_sym, qdot_u_sym, tau_sym)], + model._compute_the_lagrangian_multipliers()(q, q_dot, q_ddot, tau), [20.34808, 27.119224], expand=False ) TestUtils.assert_equal( - compute_the_lagrangian_multipliers( - MX(np.zeros(model.nb_independent_joints)), MX(np.ones(model.nb_independent_joints) * 0.001), tau + model.compute_the_lagrangian_multipliers()( + MX(np.zeros(model.nb_independent_joints)), MX(np.ones(model.nb_independent_joints) * 0.001), MX(np.zeros(model.nb_dependent_joints)), tau ), [np.nan, np.nan], expand=False, diff --git a/tests/shard1/test_dynamics.py b/tests/shard1/test_dynamics.py index 8629c9cdb..218ebf874 100644 --- a/tests/shard1/test_dynamics.py +++ b/tests/shard1/test_dynamics.py @@ -20,8 +20,8 @@ ParameterContainer, ParameterList, PhaseDynamics, - ExternalForcesList, - ExternalForcesType, + ExternalForces, + ExternalForceType, ReferenceFrame, ) from tests.utils import TestUtils @@ -108,20 +108,18 @@ def test_torque_driven(with_contact, with_external_force, cx, phase_dynamics): 0, ] - external_forces = ExternalForcesList() + external_forces = ExternalForces() external_forces.add( key="Seg0", data=external_forces_array[:3, :], - force_type=ExternalForcesType.TORQUE, + force_type=ExternalForceType.TORQUE, force_reference_frame=ReferenceFrame.GLOBAL, - phase=0, ) external_forces.add( key="Seg0", data=external_forces_array[3:6, :], - force_type=ExternalForcesType.LINEAR_FORCE, + force_type=ExternalForceType.FORCE, force_reference_frame=ReferenceFrame.GLOBAL, - phase=0, ) nlp.model = BiorbdModel( @@ -369,20 +367,18 @@ def test_torque_derivative_driven(with_contact, with_external_force, cx, phase_d 0, 0, ] - external_forces = ExternalForcesList() + external_forces = ExternalForces() external_forces.add( key="Seg0", data=external_forces_array[:3, :], - force_type=ExternalForcesType.TORQUE, + force_type=ExternalForceType.TORQUE, force_reference_frame=ReferenceFrame.GLOBAL, - phase=0, ) external_forces.add( key="Seg0", data=external_forces_array[3:6, :], - force_type=ExternalForcesType.LINEAR_FORCE, + force_type=ExternalForceType.FORCE, force_reference_frame=ReferenceFrame.GLOBAL, - phase=0, ) nlp.model = BiorbdModel( @@ -740,20 +736,18 @@ def test_torque_activation_driven(with_contact, with_external_force, cx, phase_d 0, 0, ] - external_forces = ExternalForcesList() + external_forces = ExternalForces() external_forces.add( key="Seg0", data=external_forces_array[:3, :], - force_type=ExternalForcesType.TORQUE, + force_type=ExternalForceType.TORQUE, force_reference_frame=ReferenceFrame.GLOBAL, - phase=0, ) external_forces.add( key="Seg0", data=external_forces_array[3:6, :], - force_type=ExternalForcesType.LINEAR_FORCE, + force_type=ExternalForceType.FORCE, force_reference_frame=ReferenceFrame.GLOBAL, - phase=0, ) nlp.model = BiorbdModel( @@ -937,20 +931,18 @@ def test_torque_activation_driven_with_residual_torque( 0, 0, ] - external_forces = ExternalForcesList() + external_forces = ExternalForces() external_forces.add( key="Seg0", data=external_forces_array[:3, :], - force_type=ExternalForcesType.TORQUE, + force_type=ExternalForceType.TORQUE, force_reference_frame=ReferenceFrame.GLOBAL, - phase=0, ) external_forces.add( key="Seg0", data=external_forces_array[3:6, :], - force_type=ExternalForcesType.LINEAR_FORCE, + force_type=ExternalForceType.FORCE, force_reference_frame=ReferenceFrame.GLOBAL, - phase=0, ) nlp.model = BiorbdModel( @@ -1196,20 +1188,18 @@ def test_muscle_driven(with_excitations, with_contact, with_residual_torque, wit 0, 0, ] - external_forces = ExternalForcesList() + external_forces = ExternalForces() external_forces.add( key="r_ulna_radius_hand_rotation1", data=external_forces_array[:3, :], - force_type=ExternalForcesType.TORQUE, + force_type=ExternalForceType.TORQUE, force_reference_frame=ReferenceFrame.GLOBAL, - phase=0, ) external_forces.add( key="r_ulna_radius_hand_rotation1", data=external_forces_array[3:6, :], - force_type=ExternalForcesType.LINEAR_FORCE, + force_type=ExternalForceType.FORCE, force_reference_frame=ReferenceFrame.GLOBAL, - phase=0, ) nlp.model = BiorbdModel( diff --git a/tests/shard1/test_enums.py b/tests/shard1/test_enums.py index 901655a80..11b38bc94 100644 --- a/tests/shard1/test_enums.py +++ b/tests/shard1/test_enums.py @@ -13,7 +13,7 @@ SolutionIntegrator, QuadratureRule, SoftContactDynamics, - ExternalForcesType, + ExternalForceType, ReferenceFrame, ) @@ -189,11 +189,12 @@ def test_multi_cyclic_cycle_solutions(): def test_external_forces_type(): - assert ExternalForcesType.LINEAR_FORCE.value == "linear_force" - assert ExternalForcesType.TORQUE.value == "torque" + assert ExternalForceType.FORCE.value == "force" + assert ExternalForceType.TORQUE.value == "torque" + assert ExternalForceType.TORQUE_AND_FORCE.value == "force_and_torque" # verify the number of elements - assert len(ExternalForcesType) == 2 + assert len(ExternalForceType) == 3 def test_reference_frame(): diff --git a/tests/shard3/test_external_forces.py b/tests/shard3/test_external_forces.py index 4a3219bbe..28f023f6b 100644 --- a/tests/shard3/test_external_forces.py +++ b/tests/shard3/test_external_forces.py @@ -7,14 +7,14 @@ from bioptim import ( PhaseDynamics, SolutionMerge, - ExternalForcesType, + ExternalForceType, ReferenceFrame, BiorbdModel, Node, OptimalControlProgram, DynamicsList, DynamicsFcn, - ExternalForcesList, + ExternalForces, ObjectiveList, ObjectiveFcn, ConstraintList, @@ -25,7 +25,7 @@ @pytest.mark.parametrize("phase_dynamics", [PhaseDynamics.SHARED_DURING_THE_PHASE, PhaseDynamics.ONE_PER_NODE]) @pytest.mark.parametrize("use_sx", [True, False]) -@pytest.mark.parametrize("force_type", [ExternalForcesType.LINEAR_FORCE, ExternalForcesType.TORQUE]) +@pytest.mark.parametrize("force_type", [ExternalForceType.FORCE, ExternalForceType.TORQUE]) @pytest.mark.parametrize("force_reference_frame", [ReferenceFrame.GLOBAL, ReferenceFrame.LOCAL]) @pytest.mark.parametrize("use_point_of_applications", [True, False]) @pytest.mark.parametrize("point_of_application_reference_frame", [ReferenceFrame.GLOBAL, ReferenceFrame.LOCAL]) @@ -42,15 +42,15 @@ def test_example_external_forces(phase_dynamics, if use_point_of_applications == False: point_of_application_reference_frame = None - if force_type == ExternalForcesType.LINEAR_FORCE and force_reference_frame == ReferenceFrame.GLOBAL and point_of_application_reference_frame == ReferenceFrame.GLOBAL: + if force_type == ExternalForceType.FORCE and force_reference_frame == ReferenceFrame.GLOBAL and point_of_application_reference_frame == ReferenceFrame.GLOBAL: # This combination is already tested in test_global_getting_started.py return # Errors for some combinations - if force_type == ExternalForcesType.TORQUE and use_point_of_applications: + if force_type == ExternalForceType.TORQUE and use_point_of_applications: with pytest.raises( ValueError, - match="Point of application cannot be used with ExternalForcesType.TORQUE", + match="Point of application cannot be used with ExternalForceType.TORQUE", ): ocp = ocp_module.prepare_ocp( biorbd_model_path=bioptim_folder + "/models/cube_with_forces.bioMod", @@ -79,22 +79,6 @@ def test_example_external_forces(phase_dynamics, ) return - if force_type == ExternalForcesType.TORQUE and force_reference_frame == ReferenceFrame.LOCAL: - with pytest.raises( - ValueError, - match="External torques are defined in global reference frame", - ): - ocp = ocp_module.prepare_ocp( - biorbd_model_path=bioptim_folder + "/models/cube_with_forces.bioMod", - phase_dynamics=phase_dynamics, - force_type=force_type, - force_reference_frame=force_reference_frame, - use_point_of_applications=use_point_of_applications, - point_of_application_reference_frame=point_of_application_reference_frame, - use_sx=use_sx, - ) - return - ocp = ocp_module.prepare_ocp( biorbd_model_path=bioptim_folder + "/models/cube_with_forces.bioMod", phase_dynamics=phase_dynamics, @@ -124,7 +108,7 @@ def test_example_external_forces(phase_dynamics, npt.assert_almost_equal(qdot[:, 0], np.array([0.0, 0.0, 0.0, 0.0]), decimal=5) npt.assert_almost_equal(qdot[:, -1], np.array([0.0, 0.0, 0.0, 0.0]), decimal=5) - if force_type == ExternalForcesType.TORQUE: + if force_type == ExternalForceType.TORQUE: npt.assert_almost_equal(f[0, 0], 19847.887805189126) @@ -139,9 +123,12 @@ def test_example_external_forces(phase_dynamics, npt.assert_almost_equal(q[:, -1], np.array([-4.69169398e-15, 2.00000000e+00, -1.43453709e-17, 0.0]), decimal=5) # how the force is stored - data = ocp.nlp[0].numerical_data_timeseries["forces_in_global"] + if force_reference_frame == ReferenceFrame.LOCAL: + data = ocp.nlp[0].numerical_data_timeseries["forces_in_local"] + else: + data = ocp.nlp[0].numerical_data_timeseries["forces_in_global"] npt.assert_equal(data.shape, (9, 2, 31)) - npt.assert_almost_equal(data[:, 0, 0], np.array([0., 0., -2., 0., 0., 0., 0., 0., 0.])) + npt.assert_almost_equal(data[:, 0, 0], np.array([0., 0., -2., 0., 0., 0., 0., 0., 0.])) else: if force_reference_frame == ReferenceFrame.GLOBAL: @@ -234,6 +221,7 @@ def test_example_external_forces(phase_dynamics, def prepare_ocp( biorbd_model_path: str = "models/cube_with_forces.bioMod", + use_torque_and_force_at_the_same_time: bool = False, ) -> OptimalControlProgram: # Problem parameters @@ -269,43 +257,57 @@ def prepare_ocp( Test_torque[1, :] = -1.8 Test_torque[1, 4] = -18 - external_forces = ExternalForcesList() - external_forces.add( - key="Seg1", - data=Seg1_force, - force_type=ExternalForcesType.LINEAR_FORCE, - force_reference_frame=ReferenceFrame.GLOBAL, - point_of_application=Seg1_point_of_application, - point_of_application_reference_frame=ReferenceFrame.GLOBAL, - phase=0, - ) - external_forces.add( - key="Seg1", - data=Seg1_torque, - force_type=ExternalForcesType.TORQUE, - force_reference_frame=ReferenceFrame.GLOBAL, - point_of_application=None, - point_of_application_reference_frame=None, - phase=0, - ) - external_forces.add( - key="Test", - data=Test_force, - force_type=ExternalForcesType.LINEAR_FORCE, - force_reference_frame=ReferenceFrame.LOCAL, - point_of_application=Test_point_of_application, - point_of_application_reference_frame=ReferenceFrame.LOCAL, - phase=0, - ) - external_forces.add( - key="Test", - data=Test_torque, - force_type=ExternalForcesType.TORQUE, - force_reference_frame=ReferenceFrame.GLOBAL, - point_of_application=None, - point_of_application_reference_frame=None, - phase=0, - ) + external_forces = ExternalForces() + if use_torque_and_force_at_the_same_time: + external_forces.add( + key="Seg1", + data=np.vstack((Seg1_torque, Seg1_force)), + force_type=ExternalForceType.TORQUE_AND_FORCE, + force_reference_frame=ReferenceFrame.GLOBAL, + point_of_application=Seg1_point_of_application, + point_of_application_reference_frame=ReferenceFrame.GLOBAL, + ) + external_forces.add( + key="Test", + data=np.vstack((Test_torque, Test_force)), + force_type=ExternalForceType.TORQUE_AND_FORCE, + force_reference_frame=ReferenceFrame.LOCAL, + point_of_application=Test_point_of_application, + point_of_application_reference_frame=ReferenceFrame.LOCAL, + ) + else: + external_forces.add( + key="Seg1", + data=Seg1_force, + force_type=ExternalForceType.FORCE, + force_reference_frame=ReferenceFrame.GLOBAL, + point_of_application=Seg1_point_of_application, + point_of_application_reference_frame=ReferenceFrame.GLOBAL, + ) + external_forces.add( + key="Seg1", + data=Seg1_torque, + force_type=ExternalForceType.TORQUE, + force_reference_frame=ReferenceFrame.GLOBAL, + point_of_application=None, + point_of_application_reference_frame=None, + ) + external_forces.add( + key="Test", + data=Test_force, + force_type=ExternalForceType.FORCE, + force_reference_frame=ReferenceFrame.LOCAL, + point_of_application=Test_point_of_application, + point_of_application_reference_frame=ReferenceFrame.LOCAL, + ) + external_forces.add( + key="Test", + data=Test_torque, + force_type=ExternalForceType.TORQUE, + force_reference_frame=ReferenceFrame.LOCAL, + point_of_application=None, + point_of_application_reference_frame=None, + ) bio_model = BiorbdModel(biorbd_model_path, external_forces=external_forces) @@ -353,48 +355,135 @@ def test_example_external_forces_all_at_once(): from bioptim.examples.getting_started import example_external_forces as ocp_module bioptim_folder = os.path.dirname(ocp_module.__file__) - ocp = prepare_ocp( + ocp_false = prepare_ocp( biorbd_model_path=bioptim_folder + "/models/cube_with_forces.bioMod", + use_torque_and_force_at_the_same_time=False, ) - sol = ocp.solve() + sol_false = ocp_false.solve() # Check objective function value - f = np.array(sol.cost) - npt.assert_equal(f.shape, (1, 1)) - npt.assert_almost_equal(f[0, 0], 7067.8516045402175) + f_false = np.array(sol_false.cost) + npt.assert_equal(f_false.shape, (1, 1)) + npt.assert_almost_equal(f_false[0, 0], 7075.438561173094) # Check constraints - g = np.array(sol.constraints) - npt.assert_equal(g.shape, (246, 1)) - npt.assert_almost_equal(g, np.zeros((246, 1))) + g_false = np.array(sol_false.constraints) + npt.assert_equal(g_false.shape, (246, 1)) + npt.assert_almost_equal(g_false, np.zeros((246, 1))) # Check some of the results - states = sol.decision_states(to_merge=SolutionMerge.NODES) - controls = sol.decision_controls(to_merge=SolutionMerge.NODES) - q, qdot, tau = states["q"], states["qdot"], controls["tau"] + states_false = sol_false.decision_states(to_merge=SolutionMerge.NODES) + controls_false = sol_false.decision_controls(to_merge=SolutionMerge.NODES) + q_false, qdot_false, tau_false = states_false["q"], states_false["qdot"], controls_false["tau"] # initial and final controls - npt.assert_almost_equal(tau[:, 0], np.array([1.56834240e-09, 6.98419368e+00, -1.49703626e-10, 0.0])) - npt.assert_almost_equal(tau[:, 10], np.array([-7.45768885e-10, 6.24337052e+00, 6.62627954e-10, 0.0])) - npt.assert_almost_equal(tau[:, 20], np.array([-5.73998856e-10, 5.50254736e+00, 5.21623414e-10, 0.0])) - npt.assert_almost_equal(tau[:, -1], np.array([1.79690124e-09, 4.83580652e+00, -2.97742253e-10, 0.0])) + npt.assert_almost_equal(tau_false[:, 0], np.array([0.18595882, 6.98424521, -0.30071214, 0.])) + npt.assert_almost_equal(tau_false[:, 10], np.array([-0.05224518, 6.24340374, -0.17010067, 0.])) + npt.assert_almost_equal(tau_false[:, 20], np.array([-0.03359529, 5.50256227, -0.11790254, 0.])) + npt.assert_almost_equal(tau_false[:, -1], np.array([0.07805599, 4.83580495, -0.14719148, 0.])) # initial and final position - npt.assert_almost_equal(q[:, 0], np.array([-4.07966993e-15, 6.99774100e-16, 3.12712716e-11, 0.0]), decimal=5) - npt.assert_almost_equal(q[:, -1], np.array([-4.35983004e-15, 2.00000000e+00, 2.69788862e-11, 0.0]), decimal=5) + npt.assert_almost_equal(q_false[:, 0], np.array([-4.01261246e-15, 7.00046055e-16, 2.15375856e-03, 0.0]), decimal=5) + npt.assert_almost_equal(q_false[:, -1], np.array([-4.29786206e-15, 2.00000000e+00, 1.66939403e-03, 0.0]), decimal=5) # initial and final velocities - npt.assert_almost_equal(qdot[:, 0], np.array([0.0, 0.0, 0.0, 0.0]), decimal=5) - npt.assert_almost_equal(qdot[:, -1], np.array([0.0, 0.0, 0.0, 0.0]), decimal=5) + npt.assert_almost_equal(qdot_false[:, 0], np.array([0.0, 0.0, 0.0, 0.0]), decimal=5) + npt.assert_almost_equal(qdot_false[:, -1], np.array([0.0, 0.0, 0.0, 0.0]), decimal=5) # how the force is stored - data_Seg1 = ocp.nlp[0].numerical_data_timeseries["forces_in_global"] - npt.assert_equal(data_Seg1.shape, (9, 1, 31)) - npt.assert_almost_equal(data_Seg1[:, 0, 0], np.array([0. , 1.8, 0. , 0. , 0. , -2. , 0. , 0. , 0.])) + data_Seg1_false = ocp_false.nlp[0].numerical_data_timeseries["forces_in_global"] + npt.assert_equal(data_Seg1_false.shape, (9, 1, 31)) + npt.assert_almost_equal(data_Seg1_false[:, 0, 0], np.array([0. , 1.8 , 0. , 0. , 0. , -2. , 0.05 , -0.05 , + 0.007])) - data_Test = ocp.nlp[0].numerical_data_timeseries["forces_in_local"] - npt.assert_equal(data_Test.shape, (9, 1, 31)) - npt.assert_almost_equal(data_Test[:, 0, 0], np.array([0. , -1.8, 0. , 0. , 0. , 5. , 0. , 0. , 0.])) + data_Test_false = ocp_false.nlp[0].numerical_data_timeseries["forces_in_local"] + npt.assert_equal(data_Test_false.shape, (9, 1, 31)) + npt.assert_almost_equal(data_Test_false[:, 0, 0], np.array([0. , -1.8, 0. , 0. , 0. , 5. , -0.009, 0.01 , + -0.01])) # detailed cost values - npt.assert_almost_equal(sol.detailed_cost[0]["cost_value_weighted"], f[0, 0]) + npt.assert_almost_equal(sol_false.detailed_cost[0]["cost_value_weighted"], f_false[0, 0]) + + + ocp_true = prepare_ocp( + biorbd_model_path=bioptim_folder + "/models/cube_with_forces.bioMod", + use_torque_and_force_at_the_same_time=True, + ) + sol_true = ocp_true.solve() + + # Check objective function value + f_true = np.array(sol_true.cost) + npt.assert_equal(f_true.shape, (1, 1)) + npt.assert_almost_equal(f_true[0, 0], f_false[0, 0]) + + # Check constraints + g_true = np.array(sol_true.constraints) + npt.assert_equal(g_true.shape, (246, 1)) + npt.assert_almost_equal(g_true, np.zeros((246, 1))) + + # Check some of the results + states_true = sol_true.decision_states(to_merge=SolutionMerge.NODES) + controls_true = sol_true.decision_controls(to_merge=SolutionMerge.NODES) + q_true, qdot_true, tau_true = states_true["q"], states_true["qdot"], controls_true["tau"] + + # initial and final controls + npt.assert_almost_equal(tau_true[:, 0], tau_false[:, 0]) + npt.assert_almost_equal(tau_true[:, 10], tau_false[:, 10]) + npt.assert_almost_equal(tau_true[:, 20], tau_false[:, 20]) + npt.assert_almost_equal(tau_true[:, -1], tau_false[:, -1]) + + # initial and final position + npt.assert_almost_equal(q_true[:, 0], q_true[:, 0]) + npt.assert_almost_equal(q_true[:, -1], q_true[:, -1]) + + # initial and final velocities + npt.assert_almost_equal(qdot_true[:, 0], qdot_true[:, 0]) + npt.assert_almost_equal(qdot_true[:, -1], qdot_true[:, -1]) + + # how the force is stored + data_Seg1_true = ocp_true.nlp[0].numerical_data_timeseries["forces_in_global"] + npt.assert_equal(data_Seg1_true.shape, (9, 1, 31)) + npt.assert_almost_equal(data_Seg1_true[:, 0, 0], data_Seg1_false[:, 0, 0]) + + data_Test_true = ocp_true.nlp[0].numerical_data_timeseries["forces_in_local"] + npt.assert_equal(data_Test_true.shape, (9, 1, 31)) + npt.assert_almost_equal(data_Test_true[:, 0, 0], data_Test_false[:, 0, 0]) + + # detailed cost values + npt.assert_almost_equal(sol_true.detailed_cost[0]["cost_value_weighted"], f_true[0, 0]) + + +def test_fail(): + + n_shooting = 30 + fake_force = np.zeros((3, n_shooting + 1)) + + external_forces = ExternalForces() + external_forces.add( + key="Seg1", + data=np.vstack((fake_force, fake_force)), + force_type=ExternalForceType.TORQUE_AND_FORCE, + force_reference_frame=ReferenceFrame.GLOBAL, + ) + external_forces.add( + key="Test", + data=np.vstack((fake_force, fake_force)), + force_type=ExternalForceType.TORQUE_AND_FORCE, + force_reference_frame=ReferenceFrame.LOCAL, + ) + + with pytest.raises(ValueError, match="The force is already defined for Seg1"): + external_forces.add( + key="Seg1", + data=fake_force, + force_type=ExternalForceType.FORCE, + force_reference_frame=ReferenceFrame.GLOBAL, + ) + + with pytest.raises(ValueError, match="The torque is already defined for Seg1"): + external_forces.add( + key="Seg1", + data=fake_force, + force_type=ExternalForceType.TORQUE, + force_reference_frame=ReferenceFrame.GLOBAL, + ) \ No newline at end of file diff --git a/tests/shard4/test_penalty.py b/tests/shard4/test_penalty.py index 2068a7f71..71f9f1fef 100644 --- a/tests/shard4/test_penalty.py +++ b/tests/shard4/test_penalty.py @@ -516,7 +516,7 @@ def test_penalty_track_markers_acceleration(penalty_origin, value, phase_dynamic npt.assert_almost_equal(res, expected, decimal=5) - + @pytest.mark.parametrize("phase_dynamics", [PhaseDynamics.SHARED_DURING_THE_PHASE, PhaseDynamics.ONE_PER_NODE]) @pytest.mark.parametrize("penalty_origin", [ObjectiveFcn.Lagrange, ObjectiveFcn.Mayer, ConstraintFcn]) @pytest.mark.parametrize("value", [0.1, -10]) From 978c2d2265a30cfa2d21e8b0b9485a5d346df7aa Mon Sep 17 00:00:00 2001 From: eve-mac Date: Mon, 28 Oct 2024 10:11:07 -0400 Subject: [PATCH 063/108] blacked --- bioptim/dynamics/dynamics_functions.py | 4 +- .../holonomic_constraints/two_pendulums.py | 8 +- bioptim/misc/enums.py | 1 - .../models/biorbd/holonomic_biorbd_model.py | 92 ++++++++----------- .../models/biorbd/variational_biorbd_model.py | 1 - tests/shard1/test_biorbd_model_holonomic.py | 9 +- 6 files changed, 54 insertions(+), 61 deletions(-) diff --git a/bioptim/dynamics/dynamics_functions.py b/bioptim/dynamics/dynamics_functions.py index cc56e41f3..b2443ca6a 100644 --- a/bioptim/dynamics/dynamics_functions.py +++ b/bioptim/dynamics/dynamics_functions.py @@ -1136,7 +1136,9 @@ 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) - q_v_init = DM.zeros(nlp.model.nb_dependent_joints) # @ipuch: I'm not sure of this, where q_v_init is supposed to be if not zeros? + q_v_init = DM.zeros( + nlp.model.nb_dependent_joints + ) # @ipuch: I'm not sure of this, where q_v_init is supposed to be if not zeros? qddot_u = nlp.model.partitioned_forward_dynamics()(q_u, qdot_u, q_v_init, tau, nlp.parameters.cx) return DynamicsEvaluation(dxdt=vertcat(qdot_u, qddot_u), defects=None) diff --git a/bioptim/examples/holonomic_constraints/two_pendulums.py b/bioptim/examples/holonomic_constraints/two_pendulums.py index a2b32d924..421e026ec 100644 --- a/bioptim/examples/holonomic_constraints/two_pendulums.py +++ b/bioptim/examples/holonomic_constraints/two_pendulums.py @@ -71,7 +71,13 @@ def compute_all_states(sol, bio_model: HolonomicBiorbdModel): .squeeze() ) qddot[:, i] = bio_model.compute_qddot()(q[:, i], qdot[:, i], qddot_u_i).toarray().squeeze() - lambdas[:, i] = bio_model.compute_the_lagrangian_multipliers()(states["q_u"][:, i][:, np.newaxis], states["qdot_u"][:, i], q_v_init[:, i], tau[:, i]).toarray().squeeze() + lambdas[:, i] = ( + bio_model.compute_the_lagrangian_multipliers()( + states["q_u"][:, i][:, np.newaxis], states["qdot_u"][:, i], q_v_init[:, i], tau[:, i] + ) + .toarray() + .squeeze() + ) return q, qdot, qddot, lambdas diff --git a/bioptim/misc/enums.py b/bioptim/misc/enums.py index 16a528ff6..0c15ba662 100644 --- a/bioptim/misc/enums.py +++ b/bioptim/misc/enums.py @@ -233,4 +233,3 @@ class ExternalForceType(Enum): FORCE = "force" TORQUE = "torque" TORQUE_AND_FORCE = "torque_and_force" - diff --git a/bioptim/models/biorbd/holonomic_biorbd_model.py b/bioptim/models/biorbd/holonomic_biorbd_model.py index 60c9c1ed5..ac1e59506 100644 --- a/bioptim/models/biorbd/holonomic_biorbd_model.py +++ b/bioptim/models/biorbd/holonomic_biorbd_model.py @@ -42,7 +42,6 @@ def __init__( self.qddot_v = MX.sym("qddot_v_mx", self.nb_dependent_joints, 1) self.q_v_init = MX.sym("q_v_init_mx", self.nb_dependent_joints, 1) - def set_newton_tol(self, newton_tol: float): self._newton_tol = newton_tol @@ -178,9 +177,9 @@ def constrained_forward_dynamics(self) -> Function: biais = -self.holonomic_constraints_jacobian(self.qdot) @ self.qdot if self.stabilization: - biais -= self.alpha * self.holonomic_constraints(self.q) + self.beta * self.holonomic_constraints_derivative( - self.q, self.qdot - ) + biais -= self.alpha * self.holonomic_constraints( + self.q + ) + self.beta * self.holonomic_constraints_derivative(self.q, self.qdot) tau_augmented = vertcat(tau_augmented, biais) @@ -293,11 +292,13 @@ def partitioned_forward_dynamics(self) -> Function: modified_generalized_forces - second_term @ self.biais_vector(q, qdot) - modified_non_linear_effect ) - casadi_fun = Function("partitioned_forward_dynamics", - [self.q_u, self.qdot_u, self.q_v_init, self.tau], - [qddot_u], - ["q_u", "qdot_u", "q_v_init", "tau"], - ["qddot_u"]) + casadi_fun = Function( + "partitioned_forward_dynamics", + [self.q_u, self.qdot_u, self.q_v_init, self.tau], + [qddot_u], + ["q_u", "qdot_u", "q_v_init", "tau"], + ["qddot_u"], + ) return casadi_fun @@ -384,51 +385,33 @@ def compute_q_v(self) -> Function: ifcn = rootfinder("ifcn", "newton", residuals, opts) v_opt = ifcn(*ifcn_input) - casadi_fun = Function("compute_q_v", - [self.q_u, self.q_v_init], - [v_opt], - ["q_u", "q_v_init"], - ["q_v"]) + casadi_fun = Function("compute_q_v", [self.q_u, self.q_v_init], [v_opt], ["q_u", "q_v_init"], ["q_v"]) return casadi_fun def compute_q(self) -> Function: q_v = self.compute_q_v()(self.q_u, self.q_v_init) biorbd_return = self.state_from_partition(self.q_u, q_v) - casadi_fun = Function("compute_q", - [self.q_u, self.q_v_init], - [biorbd_return], - ["q_u", "q_v_init"], - ["q"]) + casadi_fun = Function("compute_q", [self.q_u, self.q_v_init], [biorbd_return], ["q_u", "q_v_init"], ["q"]) return casadi_fun def compute_qdot_v(self) -> Function: coupling_matrix_vu = self.coupling_matrix(self.q) biorbd_return = coupling_matrix_vu @ self.qdot_u - casadi_fun = Function("compute_qdot_v", - [self.q, self.qdot_u], - [biorbd_return], - ["q", "qdot_u"], - ["qdot_v"]) + casadi_fun = Function("compute_qdot_v", [self.q, self.qdot_u], [biorbd_return], ["q", "qdot_u"], ["qdot_v"]) return casadi_fun def _compute_qdot_v(self) -> Function: q = self.compute_q()(self.q_u, self.q_v_init) biorbd_return = self.compute_qdot_v()(q, self.qdot_u) - casadi_fun = Function("compute_qdot_v", - [self.q, self.qdot_u, self.q_v_init], - [biorbd_return], - ["q", "qdot_u"], - ["qdot_v"]) + casadi_fun = Function( + "compute_qdot_v", [self.q, self.qdot_u, self.q_v_init], [biorbd_return], ["q", "qdot_u"], ["qdot_v"] + ) return casadi_fun def compute_qdot(self) -> Function: qdot_v = self.compute_qdot_v()(self.q, self.qdot_u) biorbd_return = self.state_from_partition(self.qdot_u, qdot_v) - casadi_fun = Function("compute_qdot", - [self.q, self.qdot_u], - [biorbd_return], - ["q", "qdot_u"], - ["qdot"]) + casadi_fun = Function("compute_qdot", [self.q, self.qdot_u], [biorbd_return], ["q", "qdot_u"], ["qdot"]) return casadi_fun def compute_qddot_v(self) -> Function: @@ -442,11 +425,9 @@ def compute_qddot_v(self) -> Function: """ coupling_matrix_vu = self.coupling_matrix(self.q) biorbd_return = coupling_matrix_vu @ self.qddot_u + self.biais_vector(self.q, self.qdot) - casadi_fun = Function("compute_qddot_v", - [self.q, self.qdot, self.qddot_u], - [biorbd_return], - ["q", "qdot", "qddot_u"], - ["qddot_v"]) + casadi_fun = Function( + "compute_qddot_v", [self.q, self.qdot, self.qddot_u], [biorbd_return], ["q", "qdot", "qddot_u"], ["qddot_v"] + ) return casadi_fun def compute_qddot(self) -> Function: @@ -460,11 +441,9 @@ def compute_qddot(self) -> Function: """ qddot_v = self.compute_qddot_v()(self.q, self.qdot, self.qddot_u) biorbd_return = self.state_from_partition(self.qddot_u, qddot_v) - casadi_fun = Function("compute_qddot", - [self.q, self.qdot, self.qddot_u], - [biorbd_return], - ["q", "qdot", "qddot_u"], - ["qddot"]) + casadi_fun = Function( + "compute_qddot", [self.q, self.qdot, self.qddot_u], [biorbd_return], ["q", "qdot", "qddot_u"], ["qddot"] + ) return casadi_fun def compute_the_lagrangian_multipliers(self) -> Function: @@ -481,11 +460,13 @@ def compute_the_lagrangian_multipliers(self) -> Function: qddot = self.compute_qddot()(q, qdot, qddot_u) biorbd_return = self._compute_the_lagrangian_multipliers()(q, qdot, qddot, self.tau) - casadi_fun = Function("compute_the_lagrangian_multipliers", - [self.q_u, self.qdot_u, self.q_v_init, self.tau], - [biorbd_return], - ["q_u", "qdot_u", "q_v_init", "tau"], - ["lambda"]) + casadi_fun = Function( + "compute_the_lagrangian_multipliers", + [self.q_u, self.qdot_u, self.q_v_init, self.tau], + [biorbd_return], + ["q_u", "qdot_u", "q_v_init", "tau"], + ["lambda"], + ) return casadi_fun def _compute_the_lagrangian_multipliers(self) -> Function: @@ -518,10 +499,11 @@ def _compute_the_lagrangian_multipliers(self) -> Function: biorbd_return = partitioned_constraints_jacobian_v_t_inv @ ( m_vu @ qddot_u + m_vv @ qddot_v + non_linear_effect_v - partitioned_tau_v ) - casadi_fun = Function("_compute_the_lagrangian_multipliers", - [self.q, self.qdot, self.qddot, self.tau], - [biorbd_return], - ["q", "qdot", "qddot", "tau"], - ["lambda"]) + casadi_fun = Function( + "_compute_the_lagrangian_multipliers", + [self.q, self.qdot, self.qddot, self.tau], + [biorbd_return], + ["q", "qdot", "qddot", "tau"], + ["lambda"], + ) return casadi_fun - diff --git a/bioptim/models/biorbd/variational_biorbd_model.py b/bioptim/models/biorbd/variational_biorbd_model.py index 84db8a3a6..a143e3ca8 100644 --- a/bioptim/models/biorbd/variational_biorbd_model.py +++ b/bioptim/models/biorbd/variational_biorbd_model.py @@ -29,7 +29,6 @@ def __init__( self.control_type = control_type self.control_discrete_approximation = control_discrete_approximation - def discrete_lagrangian( self, q1: MX | SX | DM, diff --git a/tests/shard1/test_biorbd_model_holonomic.py b/tests/shard1/test_biorbd_model_holonomic.py index 831cda2b4..6d063bb09 100644 --- a/tests/shard1/test_biorbd_model_holonomic.py +++ b/tests/shard1/test_biorbd_model_holonomic.py @@ -103,7 +103,9 @@ def test_model_holonomic(): ) TestUtils.assert_equal(model.holonomic_constraints_derivative(q, q_dot), [-7.65383105, -0.44473154]) TestUtils.assert_equal(model.holonomic_constraints_double_derivative(q, q_dot, q_ddot), [10.23374996, -11.73729905]) - TestUtils.assert_equal(model.constrained_forward_dynamics()(q, q_dot, tau, []), [-5.18551845, -3.01921376, 25.79451813]) + TestUtils.assert_equal( + model.constrained_forward_dynamics()(q, q_dot, tau, []), [-5.18551845, -3.01921376, 25.79451813] + ) TestUtils.assert_equal( model.partitioned_mass_matrix(q), [ @@ -146,7 +148,10 @@ def test_model_holonomic(): ) TestUtils.assert_equal( model.compute_the_lagrangian_multipliers()( - MX(np.zeros(model.nb_independent_joints)), MX(np.ones(model.nb_independent_joints) * 0.001), MX(np.zeros(model.nb_dependent_joints)), tau + MX(np.zeros(model.nb_independent_joints)), + MX(np.ones(model.nb_independent_joints) * 0.001), + MX(np.zeros(model.nb_dependent_joints)), + tau, ), [np.nan, np.nan], expand=False, From fb25101d79c957942e2ce87cfd46d711e0103a24 Mon Sep 17 00:00:00 2001 From: eve-mac Date: Tue, 29 Oct 2024 11:45:56 -0400 Subject: [PATCH 064/108] Holonomic ok --- bioptim/models/biorbd/holonomic_biorbd_model.py | 17 +++++++++++------ bioptim/models/biorbd/multi_biorbd_model.py | 1 - tests/shard1/test_biorbd_model_holonomic.py | 6 +++--- 3 files changed, 14 insertions(+), 10 deletions(-) diff --git a/bioptim/models/biorbd/holonomic_biorbd_model.py b/bioptim/models/biorbd/holonomic_biorbd_model.py index ac1e59506..21fc0346e 100644 --- a/bioptim/models/biorbd/holonomic_biorbd_model.py +++ b/bioptim/models/biorbd/holonomic_biorbd_model.py @@ -33,6 +33,10 @@ def __init__( self._dependent_joint_index = [] self._independent_joint_index = [i for i in range(self.nb_q)] + if parameters is not None: + raise NotImplementedError("HolonomicBiorbdModel does not support parameters yet") + + def _holonomic_symbolic_variables(self): # Declaration of MX variables of the right shape for the creation of CasADi Functions self.q_u = MX.sym("q_u_mx", self.nb_independent_joints, 1) self.qdot_u = MX.sym("qdot_u_mx", self.nb_independent_joints, 1) @@ -100,6 +104,7 @@ def set_holonomic_configuration( if dependent_joint_index and independent_joint_index: self.check_dependant_jacobian() + self._holonomic_symbolic_variables() def check_dependant_jacobian(self): partitioned_constraints_jacobian = self.partitioned_constraints_jacobian(self.q) partitioned_constraints_jacobian_v = partitioned_constraints_jacobian[:, self.nb_independent_joints :] @@ -200,7 +205,6 @@ def constrained_forward_dynamics(self) -> Function: def partitioned_mass_matrix(self, q: MX) -> MX: # q_u: independent # q_v: dependent - # @ Ipuch: either we send the parameters as an arg of the function or we return a Function... mass_matrix = self.mass_matrix()(q, []) mass_matrix_uu = mass_matrix[self._independent_joint_index, self._independent_joint_index] mass_matrix_uv = mass_matrix[self._independent_joint_index, self._dependent_joint_index] @@ -213,9 +217,7 @@ def partitioned_mass_matrix(self, q: MX) -> MX: return vertcat(first_line, second_line) def partitioned_non_linear_effect(self, q: MX, qdot: MX) -> MX: - # @Ipuch: Not sure external forces work - external_forces_set = self.model.externalForceSet() - non_linear_effect = self.model.NonLinearEffect(q, qdot, external_forces_set).to_mx() + non_linear_effect = self.non_linear_effects()(q, qdot, []) non_linear_effect_u = non_linear_effect[self._independent_joint_index] non_linear_effect_v = non_linear_effect[self._dependent_joint_index] @@ -389,6 +391,9 @@ def compute_q_v(self) -> Function: return casadi_fun def compute_q(self) -> Function: + """ + If you don't know what to put as a q_v_init, use zeros. + """ q_v = self.compute_q_v()(self.q_u, self.q_v_init) biorbd_return = self.state_from_partition(self.q_u, q_v) casadi_fun = Function("compute_q", [self.q_u, self.q_v_init], [biorbd_return], ["q_u", "q_v_init"], ["q"]) @@ -455,7 +460,7 @@ def compute_the_lagrangian_multipliers(self) -> Function: https://doi.org/10.5194/ms-4-199-2013, 2013. """ q = self.compute_q()(self.q_u, self.q_v_init) - qdot = self.compute_qdot()(self.q, self.qdot_u) + qdot = self.compute_qdot()(q, self.qdot_u) qddot_u = self.partitioned_forward_dynamics()(self.q_u, self.qdot_u, self.q_v_init, self.tau) qddot = self.compute_qddot()(q, qdot, qddot_u) @@ -500,7 +505,7 @@ def _compute_the_lagrangian_multipliers(self) -> Function: m_vu @ qddot_u + m_vv @ qddot_v + non_linear_effect_v - partitioned_tau_v ) casadi_fun = Function( - "_compute_the_lagrangian_multipliers", + "compute_the_lagrangian_multipliers", [self.q, self.qdot, self.qddot, self.tau], [biorbd_return], ["q", "qdot", "qddot", "tau"], diff --git a/bioptim/models/biorbd/multi_biorbd_model.py b/bioptim/models/biorbd/multi_biorbd_model.py index c40fb63cb..711942317 100644 --- a/bioptim/models/biorbd/multi_biorbd_model.py +++ b/bioptim/models/biorbd/multi_biorbd_model.py @@ -81,7 +81,6 @@ def __init__( self.tau = MX.sym("tau_mx", self.nb_tau, 1) self.muscle = MX.sym("muscle_mx", self.nb_muscles, 1) self.activations = MX.sym("activations_mx", self.nb_muscles, 1) - # TODO: parameters should be handled model by model self.parameters = MX.sym("parameters_to_be_implemented", 0, 1) def __getitem__(self, index): diff --git a/tests/shard1/test_biorbd_model_holonomic.py b/tests/shard1/test_biorbd_model_holonomic.py index 6d063bb09..c80484dfe 100644 --- a/tests/shard1/test_biorbd_model_holonomic.py +++ b/tests/shard1/test_biorbd_model_holonomic.py @@ -127,13 +127,13 @@ def test_model_holonomic(): qdot_u = MX(TestUtils.to_array(q_dot[model._independent_joint_index])) q_v = MX(TestUtils.to_array(q[model._dependent_joint_index])) - TestUtils.assert_equal(model.partitioned_forward_dynamics()(q_u, qdot_u, q_v, tau), -1.101808) + TestUtils.assert_equal(model.partitioned_forward_dynamics()(q_u, qdot_u, q_v, tau), -1.101808, expand=False) TestUtils.assert_equal(model.coupling_matrix(q), [5.79509793, -0.35166415], expand=False) TestUtils.assert_equal(model.biais_vector(q, q_dot), [27.03137348, 23.97095718], expand=False) TestUtils.assert_equal(model.state_from_partition(q_u, q_v), q) - TestUtils.assert_equal(model.compute_q_v()(q_u, q_v), [2 * np.pi / 3, 2 * np.pi / 3]) - TestUtils.assert_equal(model.compute_q()(q_u, q_v), [2.0943951, 2.0943951], expand=False) + TestUtils.assert_equal(model.compute_q_v()(q_u, q_v), [2 * np.pi / 3, 2 * np.pi / 3], expand=False) + TestUtils.assert_equal(model.compute_q()(q_u, q_v), [1., 2.0943951, 2.0943951], expand=False) TestUtils.assert_equal(model.compute_qdot_v()(q, qdot_u), [23.18039172, -1.4066566], expand=False) TestUtils.assert_equal(model.compute_qdot()(q, qdot_u), [4.0, 23.18039172, -1.4066566], expand=False) From d71cb9a930fb83f0b6b6c5aba4cecb872148a40a Mon Sep 17 00:00:00 2001 From: eve-mac Date: Tue, 29 Oct 2024 11:47:43 -0400 Subject: [PATCH 065/108] important typo --- bioptim/misc/external_forces.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/bioptim/misc/external_forces.py b/bioptim/misc/external_forces.py index 53ecee7d2..ab00905b5 100644 --- a/bioptim/misc/external_forces.py +++ b/bioptim/misc/external_forces.py @@ -141,9 +141,9 @@ def get_external_forces_segments(external_forces: ExternalForces): elif force_torsor.force_reference_frame == ReferenceFrame.LOCAL and force_torsor.point_of_application_reference_frame == ReferenceFrame.GLOBAL: raise NotImplementedError("External forces in local reference frame cannot have a point of application in the global reference frame yet") - if force_torsor.force_reference_frame == ReferenceFrame.GLOBAL and (force_torsor.point_of_application_reference_frame == ReferenceFrame.GLOBAL or force.point_of_application_reference_frame is None): + if force_torsor.force_reference_frame == ReferenceFrame.GLOBAL and (force_torsor.point_of_application_reference_frame == ReferenceFrame.GLOBAL or force_torsor.point_of_application_reference_frame is None): segments_to_apply_forces_in_global.append(force_torsor.key) - elif force_torsor.force_reference_frame == ReferenceFrame.LOCAL and (force_torsor.point_of_application_reference_frame == ReferenceFrame.LOCAL or force.point_of_application_reference_frame is None): + elif force_torsor.force_reference_frame == ReferenceFrame.LOCAL and (force_torsor.point_of_application_reference_frame == ReferenceFrame.LOCAL or force_torsor.point_of_application_reference_frame is None): segments_to_apply_forces_in_local.append(force_torsor.key) elif force_torsor.force_reference_frame == ReferenceFrame.GLOBAL and force_torsor.point_of_application_reference_frame == ReferenceFrame.LOCAL: segments_to_apply_translational_forces.append(force_torsor.key) From 46ee47f2477e3fc34c18959f93d2c9a679553d83 Mon Sep 17 00:00:00 2001 From: eve-mac Date: Tue, 29 Oct 2024 11:47:54 -0400 Subject: [PATCH 066/108] blacked --- bioptim/models/biorbd/holonomic_biorbd_model.py | 1 + tests/shard1/test_biorbd_model_holonomic.py | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/bioptim/models/biorbd/holonomic_biorbd_model.py b/bioptim/models/biorbd/holonomic_biorbd_model.py index 21fc0346e..5bea60711 100644 --- a/bioptim/models/biorbd/holonomic_biorbd_model.py +++ b/bioptim/models/biorbd/holonomic_biorbd_model.py @@ -105,6 +105,7 @@ def set_holonomic_configuration( self.check_dependant_jacobian() self._holonomic_symbolic_variables() + def check_dependant_jacobian(self): partitioned_constraints_jacobian = self.partitioned_constraints_jacobian(self.q) partitioned_constraints_jacobian_v = partitioned_constraints_jacobian[:, self.nb_independent_joints :] diff --git a/tests/shard1/test_biorbd_model_holonomic.py b/tests/shard1/test_biorbd_model_holonomic.py index c80484dfe..8df79fc43 100644 --- a/tests/shard1/test_biorbd_model_holonomic.py +++ b/tests/shard1/test_biorbd_model_holonomic.py @@ -133,7 +133,7 @@ def test_model_holonomic(): TestUtils.assert_equal(model.state_from_partition(q_u, q_v), q) TestUtils.assert_equal(model.compute_q_v()(q_u, q_v), [2 * np.pi / 3, 2 * np.pi / 3], expand=False) - TestUtils.assert_equal(model.compute_q()(q_u, q_v), [1., 2.0943951, 2.0943951], expand=False) + TestUtils.assert_equal(model.compute_q()(q_u, q_v), [1.0, 2.0943951, 2.0943951], expand=False) TestUtils.assert_equal(model.compute_qdot_v()(q, qdot_u), [23.18039172, -1.4066566], expand=False) TestUtils.assert_equal(model.compute_qdot()(q, qdot_u), [4.0, 23.18039172, -1.4066566], expand=False) From 8bb744b2d0280516737e5c87dd7e6551d072de04 Mon Sep 17 00:00:00 2001 From: eve-mac Date: Tue, 29 Oct 2024 14:04:44 -0400 Subject: [PATCH 067/108] fixed some tests --- bioptim/dynamics/configure_problem.py | 11 ++++++++--- bioptim/dynamics/dynamics_functions.py | 8 +++----- bioptim/limits/penalty_option.py | 1 + bioptim/misc/external_forces.py | 2 +- bioptim/models/biorbd/holonomic_biorbd_model.py | 2 +- tests/shard1/test_enums.py | 2 +- tests/shard3/test_get_time_solution.py | 1 - 7 files changed, 15 insertions(+), 12 deletions(-) diff --git a/bioptim/dynamics/configure_problem.py b/bioptim/dynamics/configure_problem.py index 02e94f6fd..272a2a134 100644 --- a/bioptim/dynamics/configure_problem.py +++ b/bioptim/dynamics/configure_problem.py @@ -221,8 +221,10 @@ def torque_driven( # Configure the contact forces if with_contact: ConfigureProblem.configure_contact_function(ocp, nlp, DynamicsFunctions.forces_from_torque_driven) + # Configure the soft contact forces ConfigureProblem.configure_soft_contact_function(ocp, nlp) + # Algebraic constraints of soft contact forces if needed if soft_contacts_dynamics == SoftContactDynamics.CONSTRAINT: ocp.implicit_constraints.add( @@ -853,9 +855,10 @@ def configure_lagrange_multipliers_function(ocp, nlp, dyn_func: Callable, **extr nlp.numerical_timeseries.cx, ], [ - dyn_func( + dyn_func()( nlp.get_var_from_states_or_controls("q_u", nlp.states.scaled.cx, nlp.controls.scaled.cx), nlp.get_var_from_states_or_controls("qdot_u", nlp.states.scaled.cx, nlp.controls.scaled.cx), + DM.zeros(nlp.model.nb_dependent_joints, 1), DynamicsFunctions.get(nlp.controls["tau"], nlp.controls.scaled.cx), ) ], @@ -917,8 +920,9 @@ def configure_qv(ocp, nlp, dyn_func: Callable, **extra_params): nlp.numerical_timeseries.cx, ], [ - dyn_func( + dyn_func()( nlp.get_var_from_states_or_controls("q_u", nlp.states.cx, nlp.controls.cx), + DM.zeros(nlp.model.nb_dependent_joints, 1), ) ], ["t_span", "x", "u", "p", "a", "d"], @@ -975,9 +979,10 @@ def configure_qdotv(ocp, nlp, dyn_func: Callable, **extra_params): nlp.numerical_timeseries.cx, ], [ - dyn_func( + dyn_func()( nlp.get_var_from_states_or_controls("q_u", nlp.states.scaled.cx, nlp.controls.scaled.cx), nlp.get_var_from_states_or_controls("qdot_u", nlp.states.scaled.cx, nlp.controls.scaled.cx), + DM.zeros(nlp.model.nb_dependent_joints, 1), ) ], ["t_span", "x", "u", "p", "a", "d"], diff --git a/bioptim/dynamics/dynamics_functions.py b/bioptim/dynamics/dynamics_functions.py index b2443ca6a..894c3492d 100644 --- a/bioptim/dynamics/dynamics_functions.py +++ b/bioptim/dynamics/dynamics_functions.py @@ -1,9 +1,7 @@ -import numpy as np -from casadi import horzcat, vertcat, MX, SX +from casadi import horzcat, vertcat, MX, SX, DM from .dynamics_evaluation import DynamicsEvaluation from .fatigue.fatigue_dynamics import FatigueList -from ..misc.enums import DefectType from ..misc.mapping import BiMapping from ..optimization.optimization_variable import OptimizationVariable @@ -1138,7 +1136,7 @@ def holonomic_torque_driven( tau = DynamicsFunctions.get(nlp.controls["tau"], controls) q_v_init = DM.zeros( nlp.model.nb_dependent_joints - ) # @ipuch: I'm not sure of this, where q_v_init is supposed to be if not zeros? - qddot_u = nlp.model.partitioned_forward_dynamics()(q_u, qdot_u, q_v_init, tau, nlp.parameters.cx) + ) + qddot_u = nlp.model.partitioned_forward_dynamics()(q_u, qdot_u, q_v_init, tau) return DynamicsEvaluation(dxdt=vertcat(qdot_u, qddot_u), defects=None) diff --git a/bioptim/limits/penalty_option.py b/bioptim/limits/penalty_option.py index 3054b662e..f35989528 100644 --- a/bioptim/limits/penalty_option.py +++ b/bioptim/limits/penalty_option.py @@ -896,6 +896,7 @@ def add_or_replace_to_penalty_pool(self, ocp, nlp): # The active controller is always the last one, and they all should be the same length anyway for node in range(len(controllers[-1])): + # TODO # TODO WARNING THE NEXT IF STATEMENT IS A BUG DELIBERATELY INTRODUCED TO FIT THE PREVIOUS RESULTS. # IT SHOULD BE REMOVED AS SOON AS THE MERGE IS DONE (AND VALUES OF THE TESTS ADJUSTED) if self.integrate and self.target is not None: diff --git a/bioptim/misc/external_forces.py b/bioptim/misc/external_forces.py index ab00905b5..63b73083f 100644 --- a/bioptim/misc/external_forces.py +++ b/bioptim/misc/external_forces.py @@ -136,7 +136,7 @@ def get_external_forces_segments(external_forces: ExternalForces): for key in external_forces.keys(): force_torsor = external_forces[key] # Check sanity first - if force_torsor.force_reference_frame == ReferenceFrame.GLOBAL and force_torsor.point_of_application_reference_frame == ReferenceFrame.LOCAL and force.torque_data is not None: + if force_torsor.force_reference_frame == ReferenceFrame.GLOBAL and force_torsor.point_of_application_reference_frame == ReferenceFrame.LOCAL and force_torsor.torque_data is not None: raise NotImplementedError("External forces in global reference frame cannot have a point of application in the local reference frame and torques defined at the same time yet") elif force_torsor.force_reference_frame == ReferenceFrame.LOCAL and force_torsor.point_of_application_reference_frame == ReferenceFrame.GLOBAL: raise NotImplementedError("External forces in local reference frame cannot have a point of application in the global reference frame yet") diff --git a/bioptim/models/biorbd/holonomic_biorbd_model.py b/bioptim/models/biorbd/holonomic_biorbd_model.py index 5bea60711..b8cf9f49f 100644 --- a/bioptim/models/biorbd/holonomic_biorbd_model.py +++ b/bioptim/models/biorbd/holonomic_biorbd_model.py @@ -410,7 +410,7 @@ def _compute_qdot_v(self) -> Function: q = self.compute_q()(self.q_u, self.q_v_init) biorbd_return = self.compute_qdot_v()(q, self.qdot_u) casadi_fun = Function( - "compute_qdot_v", [self.q, self.qdot_u, self.q_v_init], [biorbd_return], ["q", "qdot_u"], ["qdot_v"] + "compute_qdot_v", [self.q_u, self.qdot_u, self.q_v_init], [biorbd_return], ["q_u", "qdot_u", "q_v_init"], ["qdot_v"] ) return casadi_fun diff --git a/tests/shard1/test_enums.py b/tests/shard1/test_enums.py index 11b38bc94..f6dc04040 100644 --- a/tests/shard1/test_enums.py +++ b/tests/shard1/test_enums.py @@ -191,7 +191,7 @@ def test_multi_cyclic_cycle_solutions(): def test_external_forces_type(): assert ExternalForceType.FORCE.value == "force" assert ExternalForceType.TORQUE.value == "torque" - assert ExternalForceType.TORQUE_AND_FORCE.value == "force_and_torque" + assert ExternalForceType.TORQUE_AND_FORCE.value == "torque_and_force" # verify the number of elements assert len(ExternalForceType) == 3 diff --git a/tests/shard3/test_get_time_solution.py b/tests/shard3/test_get_time_solution.py index abb46c06c..c0ed317a7 100644 --- a/tests/shard3/test_get_time_solution.py +++ b/tests/shard3/test_get_time_solution.py @@ -1,7 +1,6 @@ import os import pytest -import numpy as np import numpy.testing as npt from bioptim import OdeSolver, Solver, PhaseDynamics, SolutionMerge, TimeAlignment, ControlType, Solution From f145d20b376ff7b2883fd33c6574634e9f33c972 Mon Sep 17 00:00:00 2001 From: eve-mac Date: Tue, 29 Oct 2024 14:18:07 -0400 Subject: [PATCH 068/108] deg fix --- bioptim/dynamics/configure_problem.py | 17 ++++++++++++++++- bioptim/limits/penalty.py | 5 +++-- 2 files changed, 19 insertions(+), 3 deletions(-) diff --git a/bioptim/dynamics/configure_problem.py b/bioptim/dynamics/configure_problem.py index 272a2a134..bb50daf10 100644 --- a/bioptim/dynamics/configure_problem.py +++ b/bioptim/dynamics/configure_problem.py @@ -1211,7 +1211,22 @@ def configure_soft_contact_function(ocp, nlp): A reference to the phase """ component_list = ["Mx", "My", "Mz", "Fx", "Fy", "Fz"] - nlp.soft_contact_forces_func = nlp.model.soft_contact_forces() + + global_soft_contact_force_func = nlp.model.soft_contact_forces()( + nlp.states["q"].cx_start, nlp.states["qdot"].cx_start, nlp.parameters.cx + ) + nlp.soft_contact_forces_func = Function( + "soft_contact_forces_func", + [ + nlp.time_cx, + nlp.states.cx_start, + nlp.controls.cx_start, + nlp.parameters.cx_start, + ], + [global_soft_contact_force_func], + ["t", "x", "u", "p"], + ["soft_contact_forces"], + ).expand() for i_sc in range(nlp.model.nb_soft_contacts): all_soft_contact_names = [] diff --git a/bioptim/limits/penalty.py b/bioptim/limits/penalty.py index 87254a9dd..0cdaafcdd 100644 --- a/bioptim/limits/penalty.py +++ b/bioptim/limits/penalty.py @@ -863,8 +863,9 @@ def minimize_soft_contact_forces( force_idx.append(4 + (6 * i_sc)) force_idx.append(5 + (6 * i_sc)) soft_contact_force = controller.get_nlp.soft_contact_forces_func( - controller.q, - controller.qdot, + controller.time.cx, + controller.states.cx_start, + controller.controls.cx_start, controller.parameters.cx, ) return soft_contact_force[force_idx] From 687c0e2f8be0eb7df60401674122d69e17e266d3 Mon Sep 17 00:00:00 2001 From: eve-mac Date: Tue, 29 Oct 2024 15:01:03 -0400 Subject: [PATCH 069/108] deleted track_vector_orientation --- README.md | 3 - bioptim/dynamics/configure_problem.py | 2 + .../track/track_vector_orientation.py | 129 ------------------ bioptim/limits/constraints.py | 1 - bioptim/limits/objective_functions.py | 6 - bioptim/limits/penalty.py | 80 +---------- tests/shard1/test_global_align.py | 41 ------ tests/shard1/test_prepare_all_examples.py | 13 -- tests/shard4/test_penalty.py | 40 ------ 9 files changed, 3 insertions(+), 312 deletions(-) delete mode 100644 bioptim/examples/track/track_vector_orientation.py diff --git a/README.md b/README.md index 3436c650a..54a16b665 100644 --- a/README.md +++ b/README.md @@ -2371,9 +2371,6 @@ To implement this tracking task, we use the `ConstraintFcn.TRACK_SEGMENT_WITH_CU minimizes the distance between a segment and an RT. The extra parameters `segment_index: int` and `rt_index: int` must be passed to the Objective constructor. -### The [track_vector_orientation.py](./bioptim/examples/track/track_vector_orientation.py) file -*#TODO* - ## Moving estimation horizon (MHE) In this section, we perform MHE on the pendulum example. diff --git a/bioptim/dynamics/configure_problem.py b/bioptim/dynamics/configure_problem.py index bb50daf10..934e4f5be 100644 --- a/bioptim/dynamics/configure_problem.py +++ b/bioptim/dynamics/configure_problem.py @@ -1212,6 +1212,8 @@ def configure_soft_contact_function(ocp, nlp): """ component_list = ["Mx", "My", "Mz", "Fx", "Fy", "Fz"] + # TODO: this intermediary function is necessary for the tests (probably because really sensitive) + # but it should ideally be removed sometime global_soft_contact_force_func = nlp.model.soft_contact_forces()( nlp.states["q"].cx_start, nlp.states["qdot"].cx_start, nlp.parameters.cx ) diff --git a/bioptim/examples/track/track_vector_orientation.py b/bioptim/examples/track/track_vector_orientation.py deleted file mode 100644 index 9e6c215e5..000000000 --- a/bioptim/examples/track/track_vector_orientation.py +++ /dev/null @@ -1,129 +0,0 @@ -""" -This example is a trivial example where a stick must keep its axis aligned with the one -side of a box during the whole duration of the movement. -""" - -import platform - -from bioptim import ( - BiorbdModel, - Node, - OptimalControlProgram, - DynamicsList, - DynamicsFcn, - ObjectiveList, - ObjectiveFcn, - BoundsList, - InitialGuessList, - OdeSolver, - OdeSolverBase, - Solver, - PhaseDynamics, -) - - -def prepare_ocp( - biorbd_model_path: str, - final_time: float, - n_shooting: int, - ode_solver: OdeSolverBase = OdeSolver.RK4(), - phase_dynamics: PhaseDynamics = PhaseDynamics.SHARED_DURING_THE_PHASE, - expand_dynamics: bool = True, -) -> OptimalControlProgram: - """ - Prepare the ocp - - Parameters - ---------- - biorbd_model_path: str - The path to the model - final_time: float - The time of the final node - n_shooting: int - The number of shooting points - ode_solver: - The ode solver to use - phase_dynamics: PhaseDynamics - If the dynamics equation within a phase is unique or changes at each node. - PhaseDynamics.SHARED_DURING_THE_PHASE is much faster, but lacks the capability to have changing dynamics within - a phase. A good example of when PhaseDynamics.ONE_PER_NODE should be used is when different external forces - are applied at each node - expand_dynamics: bool - If the dynamics function should be expanded. Please note, this will solve the problem faster, but will slow down - the declaration of the OCP, so it is a trade-off. Also depending on the solver, it may or may not work - (for instance IRK is not compatible with expanded dynamics) - - Returns - ------- - The OptimalControlProgram ready to be solved - """ - - bio_model = BiorbdModel(biorbd_model_path) - - # Add objective functions - objective_functions = ObjectiveList() - objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=1) - objective_functions.add( - ObjectiveFcn.Mayer.TRACK_VECTOR_ORIENTATIONS_FROM_MARKERS, - node=Node.ALL, - weight=100, - vector_0_marker_0="m0", - vector_0_marker_1="m3", - vector_1_marker_0="origin", - vector_1_marker_1="m6", - ) - - # Dynamics - dynamics = DynamicsList() - dynamics.add(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"][2, [0, -1]] = [-1.57, 1.57] - x_bounds["qdot"] = bio_model.bounds_from_ranges("qdot") - - # Define control path constraint - tau_min, tau_max, tau_init = -100, 100, 2 - u_bounds = BoundsList() - u_bounds["tau"] = [tau_min] * bio_model.nb_tau, [tau_max] * bio_model.nb_tau - - u_init = InitialGuessList() - u_init["tau"] = [tau_init] * bio_model.nb_tau - - # ------------- # - - return OptimalControlProgram( - bio_model, - dynamics, - n_shooting, - final_time, - x_bounds=x_bounds, - u_bounds=u_bounds, - u_init=u_init, - objective_functions=objective_functions, - ode_solver=ode_solver, - ) - - -def main(): - """ - Prepares, solves and animates the program - """ - - ocp = prepare_ocp( - biorbd_model_path="models/cube_and_line.bioMod", - n_shooting=30, - final_time=1, - ) - ocp.add_plot_penalty() - - # --- Solve the program --- # - sol = ocp.solve(Solver.IPOPT(show_online_optim=platform.system() == "Linux")) - - # --- Show results --- # - sol.animate() - - -if __name__ == "__main__": - main() diff --git a/bioptim/limits/constraints.py b/bioptim/limits/constraints.py index 67d750b98..978771a57 100644 --- a/bioptim/limits/constraints.py +++ b/bioptim/limits/constraints.py @@ -1074,7 +1074,6 @@ def get_type() -> Callable TRACK_SEGMENT_VELOCITY = (PenaltyFunctionAbstract.Functions.minimize_segment_velocity,) TRACK_SEGMENT_WITH_CUSTOM_RT = (PenaltyFunctionAbstract.Functions.track_segment_with_custom_rt,) TRACK_STATE = (PenaltyFunctionAbstract.Functions.minimize_states,) - TRACK_VECTOR_ORIENTATIONS_FROM_MARKERS = (PenaltyFunctionAbstract.Functions.track_vector_orientations_from_markers,) @staticmethod def get_type(): diff --git a/bioptim/limits/objective_functions.py b/bioptim/limits/objective_functions.py index 019674207..d4d44a09a 100644 --- a/bioptim/limits/objective_functions.py +++ b/bioptim/limits/objective_functions.py @@ -368,9 +368,6 @@ def get_type() -> Callable TRACK_SEGMENT_WITH_CUSTOM_RT = (PenaltyFunctionAbstract.Functions.track_segment_with_custom_rt,) TRACK_SOFT_CONTACT_FORCES = (PenaltyFunctionAbstract.Functions.minimize_soft_contact_forces,) TRACK_STATE = (PenaltyFunctionAbstract.Functions.minimize_states,) - TRACK_VECTOR_ORIENTATIONS_FROM_MARKERS = ( - PenaltyFunctionAbstract.Functions.track_vector_orientations_from_markers, - ) @staticmethod def get_type() -> Callable: @@ -419,9 +416,6 @@ def get_type() -> Callable TRACK_POWER = (PenaltyFunctionAbstract.Functions.minimize_power,) TRACK_SEGMENT_WITH_CUSTOM_RT = (PenaltyFunctionAbstract.Functions.track_segment_with_custom_rt,) TRACK_STATE = (PenaltyFunctionAbstract.Functions.minimize_states,) - TRACK_VECTOR_ORIENTATIONS_FROM_MARKERS = ( - PenaltyFunctionAbstract.Functions.track_vector_orientations_from_markers, - ) MINIMIZE_CONTACT_FORCES_END_OF_INTERVAL = ( PenaltyFunctionAbstract.Functions.minimize_contact_forces_end_of_interval, ) diff --git a/bioptim/limits/penalty.py b/bioptim/limits/penalty.py index 0cdaafcdd..8f31a4957 100644 --- a/bioptim/limits/penalty.py +++ b/bioptim/limits/penalty.py @@ -1,14 +1,12 @@ import inspect from typing import Any -import biorbd_casadi as biorbd -from casadi import horzcat, vertcat, Function, atan2, dot, cross, sqrt, MX_eye, SX_eye, SX, jacobian, trace +from casadi import horzcat, vertcat, Function, acos, dot, norm_fro, MX_eye, SX_eye, SX, jacobian, trace from math import inf from .penalty_controller import PenaltyController from .penalty_option import PenaltyOption from ..misc.enums import Node, Axis, ControlType, QuadratureRule -from ..models.biorbd.biorbd_model import BiorbdModel from ..models.protocols.stochastic_biomodel import StochasticBioModel @@ -1055,82 +1053,6 @@ def minimize_segment_velocity( return segment_angular_velocity[axes] - @staticmethod - def track_vector_orientations_from_markers( - penalty: PenaltyOption, - controller: PenaltyController, - vector_0_marker_0: int | str, - vector_0_marker_1: int | str, - vector_1_marker_0: int | str, - vector_1_marker_1: int | str, - ): - """ - Aligns two vectors together. - The first vector is defined by vector_0_marker_1 - vector_0_marker_0. - The second vector is defined by vector_1_marker_1 - vector_1_marker_0. - Note that is minimizes the angle between the two vectors, thus it is not possible ti specify an axis. - By default, this function is quadratic, meaning that it minimizes the angle between the two vectors. - WARNING: please be careful as there is a discontinuity when the two vectors are orthogonal. - - Parameters - ---------- - penalty: PenaltyOption - The actual penalty to declare - controller: PenaltyController - The penalty node elements - vector_0_marker_0: int | str - Name or index of the first marker of the first vector - vector_0_marker_1: int | str - Name or index of the second marker of the first vector - vector_1_marker_0: int | str - Name or index of the first marker of the second vector - vector_1_marker_1: int | str - Name or index of the second marker of the second vector - """ - - penalty.quadratic = True if penalty.quadratic is None else penalty.quadratic - - vector_0_marker_0_idx = ( - controller.model.marker_index(vector_0_marker_0) - if isinstance(vector_0_marker_0, str) - else vector_0_marker_0 - ) - vector_0_marker_1_idx = ( - controller.model.marker_index(vector_0_marker_1) - if isinstance(vector_0_marker_1, str) - else vector_0_marker_1 - ) - vector_1_marker_0_idx = ( - controller.model.marker_index(vector_1_marker_0) - if isinstance(vector_1_marker_0, str) - else vector_1_marker_0 - ) - vector_1_marker_1_idx = ( - controller.model.marker_index(vector_1_marker_1) - if isinstance(vector_1_marker_1, str) - else vector_1_marker_1 - ) - - vector_0_marker_0_position = controller.model.marker(index=vector_0_marker_0_idx)( - controller.q, controller.parameters.cx - ) - vector_0_marker_1_position = controller.model.marker(index=vector_0_marker_1_idx)( - controller.q, controller.parameters.cx - ) - vector_1_marker_0_position = controller.model.marker(index=vector_1_marker_0_idx)( - controller.q, controller.parameters.cx - ) - vector_1_marker_1_position = controller.model.marker(index=vector_1_marker_1_idx)( - controller.q, 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 - cross_prod = cross(vector_0, vector_1) - cross_prod_norm = sqrt(cross_prod[0] ** 2 + cross_prod[1] ** 2 + cross_prod[2] ** 2) - out = atan2(cross_prod_norm, dot(vector_0, vector_1)) - - return out @staticmethod def state_continuity(penalty: PenaltyOption, controller: PenaltyController | list): diff --git a/tests/shard1/test_global_align.py b/tests/shard1/test_global_align.py index 7170b97f9..e48600995 100644 --- a/tests/shard1/test_global_align.py +++ b/tests/shard1/test_global_align.py @@ -103,44 +103,3 @@ def test_track_marker_on_segment(ode_solver, phase_dynamics): # simulate TestUtils.simulate(sol) - - -@pytest.mark.parametrize("phase_dynamics", [PhaseDynamics.SHARED_DURING_THE_PHASE, PhaseDynamics.ONE_PER_NODE]) -def test_track_vector_orientation(phase_dynamics): - from bioptim.examples.track import track_vector_orientation as ocp_module - - bioptim_folder = os.path.dirname(ocp_module.__file__) - - ocp = ocp_module.prepare_ocp( - biorbd_model_path=bioptim_folder + "/models/cube_and_line.bioMod", - final_time=1, - n_shooting=10, - phase_dynamics=phase_dynamics, - expand_dynamics=True, - ) - sol = ocp.solve() - - # Check objective function value - f = np.array(sol.cost) - npt.assert_equal(f.shape, (1, 1)) - npt.assert_almost_equal(f[0, 0], 2.614556858357712e-08) - - # Check constraints - g = np.array(sol.constraints) - npt.assert_equal(g.shape, (80, 1)) - npt.assert_almost_equal(g, np.zeros((80, 1))) - - # Check some of the results - states = sol.decision_states(to_merge=SolutionMerge.NODES) - controls = sol.decision_controls(to_merge=SolutionMerge.NODES) - q, qdot, tau = states["q"], states["qdot"], controls["tau"] - - # initial and final position - npt.assert_almost_equal(q[:, 0], np.array([0.80000001, -0.68299837, -1.57, -1.56999089])) - npt.assert_almost_equal(q[:, -1], np.array([0.80000001, -0.68299837, 1.57, 1.56999138])) - # initial and final velocities - npt.assert_almost_equal(qdot[:, 0], np.array((3.37753150e-16, 4.90499995e00, 3.14001172e00, 3.13997056e00))) - npt.assert_almost_equal(qdot[:, -1], np.array((3.37753131e-16, -4.90499995e00, 3.14001059e00, 3.13997170e00))) - # initial and final controls - npt.assert_almost_equal(tau[:, 0], np.array([-1.27269674e-24, 1.97261036e-08, -3.01420965e-05, 3.01164220e-05])) - npt.assert_almost_equal(tau[:, -1], np.array([-2.10041085e-23, 1.97261036e-08, 2.86303889e-05, -2.86047182e-05])) diff --git a/tests/shard1/test_prepare_all_examples.py b/tests/shard1/test_prepare_all_examples.py index 704657941..8193cc7fb 100644 --- a/tests/shard1/test_prepare_all_examples.py +++ b/tests/shard1/test_prepare_all_examples.py @@ -1416,19 +1416,6 @@ def test__track__track_segment_on_rt(): ) -def test__track__track_vector_orientation(): - from bioptim.examples.track import track_vector_orientation as ocp_module - - bioptim_folder = os.path.dirname(ocp_module.__file__) - - ocp_module.prepare_ocp( - biorbd_model_path=bioptim_folder + "/models/cube_and_line.bioMod", - n_shooting=30, - final_time=1, - expand_dynamics=False, - ) - - def test__getting_started__example_variable_scaling(): from bioptim.examples.getting_started import example_variable_scaling as ocp_module diff --git a/tests/shard4/test_penalty.py b/tests/shard4/test_penalty.py index 71f9f1fef..7df38ab8c 100644 --- a/tests/shard4/test_penalty.py +++ b/tests/shard4/test_penalty.py @@ -995,46 +995,6 @@ def test_penalty_minimize_segment_velocity(penalty_origin, value, phase_dynamics npt.assert_almost_equal(res.T, expected) -@pytest.mark.parametrize("phase_dynamics", [PhaseDynamics.SHARED_DURING_THE_PHASE, PhaseDynamics.ONE_PER_NODE]) -@pytest.mark.parametrize("penalty_origin", [ObjectiveFcn.Lagrange, ObjectiveFcn.Mayer, ConstraintFcn]) -@pytest.mark.parametrize("value", [0.1, -10]) -def test_penalty_minimize_vector_orientation(penalty_origin, value, phase_dynamics): - ocp = prepare_test_ocp(phase_dynamics=phase_dynamics) - t = [0] - phases_dt = [0.05] - x = [DM(np.array([0, 0, value, 0, 0, 0, 0, 0]))] - u = [0] - p = [] - a = [] - d = [] - - penalty_type = penalty_origin.TRACK_VECTOR_ORIENTATIONS_FROM_MARKERS - - if isinstance(penalty_type, (ObjectiveFcn.Lagrange, ObjectiveFcn.Mayer)): - penalty = Objective( - penalty_type, - vector_0_marker_0="m0", - vector_0_marker_1="m3", - vector_1_marker_0="origin", - vector_1_marker_1="m6", - ) - else: - penalty = Constraint( - penalty_type, - vector_0_marker_0="m0", - vector_0_marker_1="m3", - vector_1_marker_0="origin", - vector_1_marker_1="m6", - ) - - res = get_penalty_value(ocp, penalty, t, phases_dt, x, u, p, a, d) - - if value == 0.1: - npt.assert_almost_equal(float(res), 0.09999999999999999) - else: - npt.assert_almost_equal(float(res), 2.566370614359173) - - @pytest.mark.parametrize("phase_dynamics", [PhaseDynamics.SHARED_DURING_THE_PHASE, PhaseDynamics.ONE_PER_NODE]) @pytest.mark.parametrize("penalty_origin", [ConstraintFcn]) @pytest.mark.parametrize("value", [0.1, -10]) From 2317196f5712a84efab80f3dceef8cbe72d304d8 Mon Sep 17 00:00:00 2001 From: eve-mac Date: Tue, 29 Oct 2024 15:12:35 -0400 Subject: [PATCH 070/108] removed commented lines --- bioptim/dynamics/dynamics_functions.py | 16 ---------------- 1 file changed, 16 deletions(-) diff --git a/bioptim/dynamics/dynamics_functions.py b/bioptim/dynamics/dynamics_functions.py index 894c3492d..3bef7beba 100644 --- a/bioptim/dynamics/dynamics_functions.py +++ b/bioptim/dynamics/dynamics_functions.py @@ -1017,25 +1017,9 @@ def inverse_dynamics( if external_forces is None: tau = nlp.model.inverse_dynamics(with_contact=with_contact)(q, qdot, qddot, [], nlp.parameters.cx) else: - # @ipuch not sure of this part tau = nlp.model.inverse_dynamics(with_contact=with_contact)( q, qdot, qddot, external_forces, nlp.parameters.cx ) - - # if "tau" in nlp.states: - # tau_shape = nlp.states["tau"].cx.shape[0] - # elif "tau" in nlp.controls: - # tau_shape = nlp.controls["tau"].cx.shape[0] - # else: - # tau_shape = nlp.model.nb_tau - - # tau = nlp.cx(tau_shape, nlp.ns) - # if external_forces is not None: - # for i in range(external_forces.shape[1]): - # 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 From 12a35d8072af17d15fb3f3df46ec9a31ff23b85a Mon Sep 17 00:00:00 2001 From: eve-mac Date: Tue, 29 Oct 2024 16:00:38 -0400 Subject: [PATCH 071/108] comment --- .../contact_forces_inequality_constraint_muscle.py | 7 ------- 1 file changed, 7 deletions(-) diff --git a/bioptim/examples/muscle_driven_with_contact/contact_forces_inequality_constraint_muscle.py b/bioptim/examples/muscle_driven_with_contact/contact_forces_inequality_constraint_muscle.py index fa17b4a09..44f5f46f2 100644 --- a/bioptim/examples/muscle_driven_with_contact/contact_forces_inequality_constraint_muscle.py +++ b/bioptim/examples/muscle_driven_with_contact/contact_forces_inequality_constraint_muscle.py @@ -29,13 +29,6 @@ def prepare_ocp(biorbd_model_path, phase_time, n_shooting, min_bound, max_bound, expand_dynamics=True): # --- Options --- # # BioModel path - - # TODO: Charbie - # force = ForcesList() - # force.add(Px=0, Py= 0, Pz= 0, Mx= 0, My= 0, Mz= 0 - # point_of_application_in_global=True, - # segment_name=) - bio_model = BiorbdModel(biorbd_model_path) tau_min, tau_max, tau_init = -500.0, 500.0, 0.0 activation_min, activation_max, activation_init = 0.0, 1.0, 0.5 From 5a3fcf59c8d2e8192a22008f64a1a03fa6e62729 Mon Sep 17 00:00:00 2001 From: eve-mac Date: Tue, 29 Oct 2024 16:01:28 -0400 Subject: [PATCH 072/108] blacked --- bioptim/dynamics/dynamics_functions.py | 4 +--- bioptim/limits/penalty.py | 1 - bioptim/models/biorbd/holonomic_biorbd_model.py | 6 +++++- 3 files changed, 6 insertions(+), 5 deletions(-) diff --git a/bioptim/dynamics/dynamics_functions.py b/bioptim/dynamics/dynamics_functions.py index 3bef7beba..cb6bda67f 100644 --- a/bioptim/dynamics/dynamics_functions.py +++ b/bioptim/dynamics/dynamics_functions.py @@ -1118,9 +1118,7 @@ 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) - q_v_init = DM.zeros( - nlp.model.nb_dependent_joints - ) + q_v_init = DM.zeros(nlp.model.nb_dependent_joints) qddot_u = nlp.model.partitioned_forward_dynamics()(q_u, qdot_u, q_v_init, tau) return DynamicsEvaluation(dxdt=vertcat(qdot_u, qddot_u), defects=None) diff --git a/bioptim/limits/penalty.py b/bioptim/limits/penalty.py index 8f31a4957..621d49177 100644 --- a/bioptim/limits/penalty.py +++ b/bioptim/limits/penalty.py @@ -1053,7 +1053,6 @@ def minimize_segment_velocity( return segment_angular_velocity[axes] - @staticmethod def state_continuity(penalty: PenaltyOption, controller: PenaltyController | list): if controller.control_type in ( diff --git a/bioptim/models/biorbd/holonomic_biorbd_model.py b/bioptim/models/biorbd/holonomic_biorbd_model.py index b8cf9f49f..9a24425ed 100644 --- a/bioptim/models/biorbd/holonomic_biorbd_model.py +++ b/bioptim/models/biorbd/holonomic_biorbd_model.py @@ -410,7 +410,11 @@ def _compute_qdot_v(self) -> Function: q = self.compute_q()(self.q_u, self.q_v_init) biorbd_return = self.compute_qdot_v()(q, self.qdot_u) casadi_fun = Function( - "compute_qdot_v", [self.q_u, self.qdot_u, self.q_v_init], [biorbd_return], ["q_u", "qdot_u", "q_v_init"], ["qdot_v"] + "compute_qdot_v", + [self.q_u, self.qdot_u, self.q_v_init], + [biorbd_return], + ["q_u", "qdot_u", "q_v_init"], + ["qdot_v"], ) return casadi_fun From e1ba6c6c8fe503c5dd188761591d48304de3064f Mon Sep 17 00:00:00 2001 From: eve-mac Date: Wed, 30 Oct 2024 10:36:10 -0400 Subject: [PATCH 073/108] made changes requested by pariterre --- bioptim/dynamics/dynamics_functions.py | 31 ++++++++++++++++--- bioptim/limits/penalty.py | 1 - bioptim/limits/phase_transition.py | 6 +--- bioptim/models/protocols/biomodel.py | 1 - .../optimization/optimal_control_program.py | 4 +-- ...st_global_stochastic_except_collocation.py | 1 + 6 files changed, 31 insertions(+), 13 deletions(-) diff --git a/bioptim/dynamics/dynamics_functions.py b/bioptim/dynamics/dynamics_functions.py index cb6bda67f..10d50897b 100644 --- a/bioptim/dynamics/dynamics_functions.py +++ b/bioptim/dynamics/dynamics_functions.py @@ -3,6 +3,7 @@ from .dynamics_evaluation import DynamicsEvaluation from .fatigue.fatigue_dynamics import FatigueList from ..misc.mapping import BiMapping +from ..misc.enums import DefectType from ..optimization.optimization_variable import OptimizationVariable @@ -151,7 +152,31 @@ def torque_driven( if fatigue is not None and "tau" in fatigue: dxdt = fatigue["tau"].dynamics(dxdt, nlp, states, controls) - return DynamicsEvaluation(dxdt, None) + defects = None + # TODO: contacts and fatigue to be handled with implicit dynamics + if nlp.ode_solver.defects_type == DefectType.IMPLICIT: + if not with_contact and fatigue is None: + qddot = DynamicsFunctions.get(nlp.states_dot["qddot"], nlp.states_dot.scaled.cx) + tau_id = DynamicsFunctions.inverse_dynamics( + nlp, q, qdot, qddot, with_contact, external_forces + ) + defects = nlp.cx(dq.shape[0] + tau_id.shape[0], tau_id.shape[1]) + + dq_defects = [] + for _ in range(tau_id.shape[1]): + dq_defects.append( + dq + - DynamicsFunctions.compute_qdot( + nlp, + q, + DynamicsFunctions.get(nlp.states_dot.scaled["qdot"], nlp.states_dot.scaled.cx), + ) + ) + defects[: dq.shape[0], :] = horzcat(*dq_defects) + # We modified on purpose the size of the tau to keep the zero in the defects in order to respect the dynamics + defects[dq.shape[0] :, :] = tau - tau_id + + return DynamicsEvaluation(dxdt, defects) @staticmethod def torque_driven_free_floating_base( @@ -227,9 +252,7 @@ def torque_driven_free_floating_base( dxdt[:n_q, :] = horzcat(*[dq for _ in range(ddq.shape[1])]) dxdt[n_q:, :] = ddq - defects = None - - return DynamicsEvaluation(dxdt, defects) + return DynamicsEvaluation(dxdt, None) @staticmethod def stochastic_torque_driven( diff --git a/bioptim/limits/penalty.py b/bioptim/limits/penalty.py index 621d49177..50c41876a 100644 --- a/bioptim/limits/penalty.py +++ b/bioptim/limits/penalty.py @@ -208,7 +208,6 @@ def stochastic_minimize_expected_feedback_efforts(penalty: PenaltyOption, contro jac_e_fb_x = jacobian(e_fb_mx, controller.states_scaled.cx) - # todo: Charbie remode this function jac_e_fb_x_cx = Function( "jac_e_fb_x", [ diff --git a/bioptim/limits/phase_transition.py b/bioptim/limits/phase_transition.py index 185c685e8..87e29e9dc 100644 --- a/bioptim/limits/phase_transition.py +++ b/bioptim/limits/phase_transition.py @@ -5,10 +5,9 @@ from .multinode_constraint import MultinodeConstraint from .multinode_penalty import MultinodePenalty, MultinodePenaltyFunctions -from .objective_functions import ObjectiveFunction from .path_conditions import Bounds from ..limits.penalty import PenaltyFunctionAbstract, PenaltyController -from ..misc.enums import Node, PenaltyType, InterpolationType, ControlType +from ..misc.enums import Node, PenaltyType, InterpolationType from ..misc.fcn_enum import FcnEnum from ..misc.mapping import BiMapping from ..misc.options import UniquePerPhaseOptionList @@ -50,8 +49,6 @@ def __init__( max_bound: float = 0, **extra_parameters: Any, ): - # TODO: @pariterre: where did phase_post go !? - if not isinstance(transition, PhaseTransitionFcn): custom_function = transition transition = PhaseTransitionFcn.CUSTOM @@ -288,7 +285,6 @@ def impact(transition, controllers: list[PenaltyController, PenaltyController]): ) val = vertcat(val, continuity) - # name = f"PHASE_TRANSITION_{pre.phase_idx % ocp.n_phases}_{post.phase_idx % ocp.n_phases}" return val @staticmethod diff --git a/bioptim/models/protocols/biomodel.py b/bioptim/models/protocols/biomodel.py index e188a5006..fe3265bf7 100644 --- a/bioptim/models/protocols/biomodel.py +++ b/bioptim/models/protocols/biomodel.py @@ -369,7 +369,6 @@ def lagrangian(self) -> Function: def partitioned_forward_dynamics(self, q_u, qdot_u, q_v_init, tau) -> Function: """ - @ipuch: I need help on how to implement this! This is the forward dynamics of the model, but only for the independent joints Parameters diff --git a/bioptim/optimization/optimal_control_program.py b/bioptim/optimization/optimal_control_program.py index c857029f0..f86d6ef4e 100644 --- a/bioptim/optimization/optimal_control_program.py +++ b/bioptim/optimization/optimal_control_program.py @@ -1062,7 +1062,7 @@ def _declare_parameters(self, parameters: ParameterList): if not isinstance(parameters, ParameterList): raise RuntimeError("new_parameter must be a Parameter or a ParameterList") - self.parameters = ParameterContainer(use_sx=(True if self.cx == SX else False)) + self.parameters = ParameterContainer(use_sx=(self.cx == SX)) self.parameters.initialize(parameters) def update_bounds( @@ -1630,7 +1630,7 @@ def define_parameters_phase_time( self.phase_time = phase_time if isinstance(phase_time, (tuple, list)) else [phase_time] - self.dt_parameter = ParameterList(use_sx=(True if self.cx == SX else False)) + self.dt_parameter = ParameterList(use_sx=(self.cx == SX)) for i_phase in range(self.n_phases): if i_phase != self.time_phase_mapping.to_second.map_idx[i_phase]: self.dt_parameter.add_a_copied_element(self.time_phase_mapping.to_second.map_idx[i_phase]) diff --git a/tests/shard6/test_global_stochastic_except_collocation.py b/tests/shard6/test_global_stochastic_except_collocation.py index a3ed1c4a6..a4cffa67d 100644 --- a/tests/shard6/test_global_stochastic_except_collocation.py +++ b/tests/shard6/test_global_stochastic_except_collocation.py @@ -11,6 +11,7 @@ # Integrated values should be handled another way # In the meantime, let's skip this test +# Please note that the test is very sensitive, so approximate values are enough # @pytest.mark.parametrize("use_sx", [True, False]) # def test_arm_reaching_muscle_driven(use_sx): # from bioptim.examples.stochastic_optimal_control import arm_reaching_muscle_driven as ocp_module From d052ac83e3eab9a1fafce15c05be81e5661c7a81 Mon Sep 17 00:00:00 2001 From: eve-mac Date: Wed, 30 Oct 2024 10:42:52 -0400 Subject: [PATCH 074/108] fixed the symbolic problem --- bioptim/dynamics/configure_problem.py | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/bioptim/dynamics/configure_problem.py b/bioptim/dynamics/configure_problem.py index 934e4f5be..27332e115 100644 --- a/bioptim/dynamics/configure_problem.py +++ b/bioptim/dynamics/configure_problem.py @@ -1215,15 +1215,17 @@ def configure_soft_contact_function(ocp, nlp): # TODO: this intermediary function is necessary for the tests (probably because really sensitive) # but it should ideally be removed sometime global_soft_contact_force_func = nlp.model.soft_contact_forces()( - nlp.states["q"].cx_start, nlp.states["qdot"].cx_start, nlp.parameters.cx + nlp.states["q"].mapping.to_second.map(nlp.states["q"].cx_start), + nlp.states["qdot"].mapping.to_second.map(nlp.states["qdot"].cx_start), + nlp.parameters.cx, ) nlp.soft_contact_forces_func = Function( "soft_contact_forces_func", [ nlp.time_cx, - nlp.states.cx_start, - nlp.controls.cx_start, - nlp.parameters.cx_start, + nlp.states.scaled.cx_start, + nlp.controls.scaled.cx_start, + nlp.parameters.scaled.cx_start, ], [global_soft_contact_force_func], ["t", "x", "u", "p"], From 2f42904053b5837512ede45902148c950fee3372 Mon Sep 17 00:00:00 2001 From: eve-mac Date: Wed, 30 Oct 2024 10:43:02 -0400 Subject: [PATCH 075/108] blacked --- bioptim/dynamics/dynamics_functions.py | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/bioptim/dynamics/dynamics_functions.py b/bioptim/dynamics/dynamics_functions.py index 10d50897b..6f0330d7d 100644 --- a/bioptim/dynamics/dynamics_functions.py +++ b/bioptim/dynamics/dynamics_functions.py @@ -157,9 +157,7 @@ def torque_driven( if nlp.ode_solver.defects_type == DefectType.IMPLICIT: if not with_contact and fatigue is None: qddot = DynamicsFunctions.get(nlp.states_dot["qddot"], nlp.states_dot.scaled.cx) - tau_id = DynamicsFunctions.inverse_dynamics( - nlp, q, qdot, qddot, with_contact, external_forces - ) + tau_id = DynamicsFunctions.inverse_dynamics(nlp, q, qdot, qddot, with_contact, external_forces) defects = nlp.cx(dq.shape[0] + tau_id.shape[0], tau_id.shape[1]) dq_defects = [] From ac9a691605e85ee99ec75c4adb5fda8db8025d2b Mon Sep 17 00:00:00 2001 From: Ipuch Date: Wed, 30 Oct 2024 17:29:05 -0400 Subject: [PATCH 076/108] first example external_force rolls --- bioptim/__init__.py | 78 +++--- bioptim/dynamics/configure_problem.py | 56 +---- .../example_external_forces.py | 68 +++--- bioptim/interfaces/interface_utils.py | 15 +- bioptim/misc/enums.py | 21 +- bioptim/misc/external_forces.py | 153 ------------ bioptim/models/biorbd/biorbd_model.py | 143 ++++------- bioptim/models/biorbd/external_forces.py | 231 ++++++++++++++++++ bioptim/optimization/non_linear_program.py | 13 +- 9 files changed, 383 insertions(+), 395 deletions(-) delete mode 100644 bioptim/misc/external_forces.py create mode 100644 bioptim/models/biorbd/external_forces.py diff --git a/bioptim/__init__.py b/bioptim/__init__.py index a20e07f2d..24da06bec 100644 --- a/bioptim/__init__.py +++ b/bioptim/__init__.py @@ -164,37 +164,50 @@ """ -from .misc.__version__ import __version__ from .dynamics.configure_problem import ConfigureProblem, DynamicsFcn, DynamicsList, Dynamics -from .dynamics.dynamics_functions import DynamicsFunctions +from .dynamics.configure_problem import ConfigureProblem, DynamicsFcn, DynamicsList, Dynamics from .dynamics.dynamics_evaluation import DynamicsEvaluation +from .dynamics.dynamics_evaluation import DynamicsEvaluation +from .dynamics.dynamics_functions import DynamicsFunctions +from .dynamics.dynamics_functions import DynamicsFunctions +from .dynamics.fatigue.effort_perception import EffortPerception, TauEffortPerception +from .dynamics.fatigue.effort_perception import EffortPerception, TauEffortPerception +from .dynamics.fatigue.fatigue_dynamics import FatigueList from .dynamics.fatigue.fatigue_dynamics import FatigueList -from .dynamics.fatigue.xia_fatigue import XiaFatigue, XiaTauFatigue, XiaFatigueStabilized from .dynamics.fatigue.michaud_fatigue import MichaudFatigue, MichaudTauFatigue -from .dynamics.fatigue.effort_perception import EffortPerception, TauEffortPerception +from .dynamics.fatigue.michaud_fatigue import MichaudFatigue, MichaudTauFatigue +from .dynamics.fatigue.xia_fatigue import XiaFatigue, XiaTauFatigue, XiaFatigueStabilized +from .dynamics.fatigue.xia_fatigue import XiaFatigue, XiaTauFatigue, XiaFatigueStabilized from .dynamics.ode_solver import OdeSolver, OdeSolverBase - +from .dynamics.ode_solver import OdeSolver, OdeSolverBase +from .gui.online_callback_server import PlottingServer +from .gui.online_callback_server import PlottingServer +from .gui.plot import CustomPlot +from .gui.plot import CustomPlot +from .interfaces import Solver from .interfaces import Solver - -from .models.biorbd.biorbd_model import BiorbdModel -from .models.biorbd.multi_biorbd_model import MultiBiorbdModel -from .models.biorbd.holonomic_biorbd_model import HolonomicBiorbdModel -from .models.biorbd.variational_biorbd_model import VariationalBiorbdModel -from .models.biorbd.stochastic_biorbd_model import StochasticBiorbdModel -from .models.holonomic_constraints import HolonomicConstraintsFcn, HolonomicConstraintsList -from .models.protocols.stochastic_biomodel import StochasticBioModel -from .models.protocols.biomodel import BioModel - from .limits.constraints import ConstraintFcn, ConstraintList, Constraint, ParameterConstraintList -from .limits.phase_transition import PhaseTransitionFcn, PhaseTransitionList, PhaseTransition +from .limits.constraints import ConstraintFcn, ConstraintList, Constraint, ParameterConstraintList +from .limits.fatigue_path_conditions import FatigueBounds, FatigueInitialGuess +from .limits.fatigue_path_conditions import FatigueBounds, FatigueInitialGuess from .limits.multinode_constraint import MultinodeConstraintFcn, MultinodeConstraintList, MultinodeConstraint +from .limits.multinode_constraint import MultinodeConstraintFcn, MultinodeConstraintList, MultinodeConstraint +from .limits.multinode_objective import MultinodeObjectiveFcn, MultinodeObjectiveList, MultinodeObjective from .limits.multinode_objective import MultinodeObjectiveFcn, MultinodeObjectiveList, MultinodeObjective from .limits.objective_functions import ObjectiveFcn, ObjectiveList, Objective, ParameterObjectiveList +from .limits.objective_functions import ObjectiveFcn, ObjectiveList, Objective, ParameterObjectiveList +from .limits.path_conditions import BoundsList, InitialGuessList, Bounds, InitialGuess from .limits.path_conditions import BoundsList, InitialGuessList, Bounds, InitialGuess -from .limits.fatigue_path_conditions import FatigueBounds, FatigueInitialGuess +from .limits.penalty_controller import PenaltyController from .limits.penalty_controller import PenaltyController from .limits.penalty_helpers import PenaltyHelpers - +from .limits.penalty_helpers import PenaltyHelpers +from .limits.phase_transition import PhaseTransitionFcn, PhaseTransitionList, PhaseTransition +from .limits.phase_transition import PhaseTransitionFcn, PhaseTransitionList, PhaseTransition +from .misc.__version__ import __version__ +from .misc.__version__ import __version__ +from .misc.casadi_expand import lt, le, gt, ge, if_else, if_else_zero +from .misc.casadi_expand import lt, le, gt, ge, if_else, if_else_zero from .misc.enums import ( Axis, Node, @@ -212,31 +225,32 @@ MultiCyclicCycleSolutions, PhaseDynamics, OnlineOptim, - ReferenceFrame, - ExternalForceType, ) from .misc.mapping import BiMappingList, BiMapping, Mapping, NodeMapping, NodeMappingList, SelectionMapping, Dependency -from .misc.external_forces import ExternalForce, ExternalForces - +from .misc.mapping import BiMappingList, BiMapping, Mapping, NodeMapping, NodeMappingList, SelectionMapping, Dependency +from .models.biorbd.biorbd_model import BiorbdModel +from .models.biorbd.external_forces import ExternalForceSetTimeSeries +from .models.biorbd.holonomic_biorbd_model import HolonomicBiorbdModel +from .models.biorbd.multi_biorbd_model import MultiBiorbdModel +from .models.biorbd.stochastic_biorbd_model import StochasticBiorbdModel +from .models.biorbd.variational_biorbd_model import VariationalBiorbdModel +from .models.holonomic_constraints import HolonomicConstraintsFcn, HolonomicConstraintsList +from .models.protocols.biomodel import BioModel +from .models.protocols.stochastic_biomodel import StochasticBioModel from .optimization.multi_start import MultiStart from .optimization.non_linear_program import NonLinearProgram from .optimization.optimal_control_program import OptimalControlProgram -from .optimization.receding_horizon_optimization import MovingHorizonEstimator, NonlinearModelPredictiveControl +from .optimization.optimization_variable import OptimizationVariableList +from .optimization.parameters import ParameterList, ParameterContainer +from .optimization.problem_type import SocpType from .optimization.receding_horizon_optimization import ( CyclicNonlinearModelPredictiveControl, CyclicMovingHorizonEstimator, MultiCyclicNonlinearModelPredictiveControl, ) -from .optimization.parameters import ParameterList, ParameterContainer +from .optimization.receding_horizon_optimization import MovingHorizonEstimator, NonlinearModelPredictiveControl from .optimization.solution.solution import Solution from .optimization.solution.solution_data import SolutionMerge, TimeAlignment -from .optimization.optimization_variable import OptimizationVariableList +from .optimization.stochastic_optimal_control_program import StochasticOptimalControlProgram from .optimization.variable_scaling import VariableScalingList, VariableScaling from .optimization.variational_optimal_control_program import VariationalOptimalControlProgram - -from .optimization.stochastic_optimal_control_program import StochasticOptimalControlProgram -from .optimization.problem_type import SocpType -from .misc.casadi_expand import lt, le, gt, ge, if_else, if_else_zero - -from .gui.plot import CustomPlot -from .gui.online_callback_server import PlottingServer diff --git a/bioptim/dynamics/configure_problem.py b/bioptim/dynamics/configure_problem.py index 934e4f5be..aee4f6211 100644 --- a/bioptim/dynamics/configure_problem.py +++ b/bioptim/dynamics/configure_problem.py @@ -1,7 +1,8 @@ -import numpy as np -from casadi import vertcat, Function, DM, horzcat from typing import Callable, Any +import numpy as np +from casadi import vertcat, Function, DM + from .configure_new_variable import NewVariableConfiguration from .dynamics_functions import DynamicsFunctions from .fatigue.fatigue_dynamics import FatigueList @@ -14,12 +15,10 @@ ConstraintType, SoftContactDynamics, PhaseDynamics, - ReferenceFrame, ) from ..misc.fcn_enum import FcnEnum from ..misc.mapping import BiMapping, Mapping from ..misc.options import UniquePerPhaseOptionList, OptionGeneric -from ..misc.external_forces import ExternalForces from ..models.protocols.stochastic_biomodel import StochasticBioModel from ..optimization.problem_type import SocpType @@ -1964,7 +1963,7 @@ def __init__( state_continuity_weight: float | None = None, phase_dynamics: PhaseDynamics = PhaseDynamics.SHARED_DURING_THE_PHASE, numerical_data_timeseries: dict[str, np.ndarray] = None, - external_forces: ExternalForces = None, + # external_forces: ExternalForceSetTimeSeries = None, **extra_parameters: Any, ): """ @@ -2012,53 +2011,6 @@ def __init__( self.state_continuity_weight = state_continuity_weight self.phase_dynamics = phase_dynamics self.numerical_data_timeseries = numerical_data_timeseries - self.external_forces_in_numerical_timeseries(external_forces) - - def external_forces_in_numerical_timeseries(self, external_forces): - if external_forces: - self.numerical_data_timeseries = ( - {} if self.numerical_data_timeseries is None else self.numerical_data_timeseries - ) - for key in external_forces.keys(): - force = external_forces[key] - if force.torque_data is not None: - data = force.torque_data.reshape(3, 1, -1) - else: - data = np.zeros((3, 1, force.len)) - if force.force_data is not None: - data = np.concatenate((data, force.force_data.reshape(3, 1, -1)), axis=0) - else: - data = np.concatenate((data, np.zeros((3, 1, force.len))), axis=0) - if force.point_of_application is not None: - data = np.concatenate((data, force.point_of_application.reshape(3, 1, -1)), axis=0) - else: - data = np.concatenate((data, np.zeros((3, 1, force.len))), axis=0) - - if force.force_reference_frame == ReferenceFrame.LOCAL: - if "forces_in_local" in self.numerical_data_timeseries: - self.numerical_data_timeseries["forces_in_local"] = np.concatenate( - (self.numerical_data_timeseries["forces_in_local"], data), axis=1 - ) - else: - self.numerical_data_timeseries["forces_in_local"] = data - elif force.point_of_application_reference_frame == ReferenceFrame.LOCAL: - # exclude the torques for translational forces - if "translational_forces" in self.numerical_data_timeseries: - self.numerical_data_timeseries["translational_forces"] = np.concatenate( - (self.numerical_data_timeseries["translational_forces"], data[3:, :, :]), axis=1 - ) - else: - self.numerical_data_timeseries["translational_forces"] = data[ - 3:, - :, - ] - else: - if "forces_in_global" in self.numerical_data_timeseries: - self.numerical_data_timeseries["forces_in_global"] = np.concatenate( - (self.numerical_data_timeseries["forces_in_global"], data), axis=1 - ) - else: - self.numerical_data_timeseries["forces_in_global"] = data class DynamicsList(UniquePerPhaseOptionList): diff --git a/bioptim/examples/getting_started/example_external_forces.py b/bioptim/examples/getting_started/example_external_forces.py index 50f9dd06f..086717a50 100644 --- a/bioptim/examples/getting_started/example_external_forces.py +++ b/bioptim/examples/getting_started/example_external_forces.py @@ -25,9 +25,7 @@ OdeSolverBase, Solver, PhaseDynamics, - ExternalForces, - ExternalForceType, - ReferenceFrame, + ExternalForceSetTimeSeries, ) @@ -36,10 +34,10 @@ def prepare_ocp( ode_solver: OdeSolverBase = OdeSolver.RK4(), expand_dynamics: bool = True, phase_dynamics: PhaseDynamics = PhaseDynamics.ONE_PER_NODE, - force_type: ExternalForceType = ExternalForceType.FORCE, - force_reference_frame: ReferenceFrame = ReferenceFrame.GLOBAL, + # force_type: ExternalForceType = ExternalForceType.FORCE, + # force_reference_frame: ReferenceFrame = ReferenceFrame.GLOBAL, use_point_of_applications: bool = False, - point_of_application_reference_frame: ReferenceFrame = ReferenceFrame.GLOBAL, + # point_of_application_reference_frame: ReferenceFrame = ReferenceFrame.GLOBAL, n_threads: int = 1, use_sx: bool = False, ) -> OptimalControlProgram: @@ -75,59 +73,71 @@ def prepare_ocp( final_time = 2 # Linear external forces - Seg1_force = np.zeros((3, n_shooting + 1)) + # Seg1_force = np.zeros((3, n_shooting + 1)) + Seg1_force = np.zeros((3, n_shooting)) Seg1_force[2, :] = -2 Seg1_force[2, 4] = -22 - Test_force = np.zeros((3, n_shooting + 1)) + # Test_force = np.zeros((3, n_shooting + 1)) + Test_force = np.zeros((3, n_shooting)) Test_force[2, :] = 5 Test_force[2, 4] = 52 if use_point_of_applications: - Seg1_point_of_application = np.zeros((3, n_shooting + 1)) + # Seg1_point_of_application = np.zeros((3, n_shooting + 1)) + Seg1_point_of_application = np.zeros((3, n_shooting)) Seg1_point_of_application[0, :] = 0.05 Seg1_point_of_application[1, :] = -0.05 Seg1_point_of_application[2, :] = 0.007 - Test_point_of_application = np.zeros((3, n_shooting + 1)) + # Test_point_of_application = np.zeros((3, n_shooting + 1)) + Test_point_of_application = np.zeros((3, n_shooting)) Test_point_of_application[0, :] = -0.009 Test_point_of_application[1, :] = 0.01 Test_point_of_application[2, :] = -0.01 else: Seg1_point_of_application = None Test_point_of_application = None - - external_forces = ExternalForces() - external_forces.add( - key="Seg1", # Name of the segment where the external force is applied - data=Seg1_force, # 3 x (n_shooting_points+1) array - force_type=force_type, # Type of the external force (ExternalForceType.FORCE) - force_reference_frame=force_reference_frame, # Reference frame of the external force (ReferenceFrame.GLOBAL) - point_of_application=Seg1_point_of_application, # Position of the point of application - point_of_application_reference_frame=point_of_application_reference_frame, # Reference frame of the point of application (ReferenceFrame.GLOBAL) - ) - external_forces.add( - key="Test", # Name of the segment where the external force is applied - data=Test_force, # 3 x (n_shooting_points+1) array - force_type=force_type, # Type of the external force (ExternalForceType.FORCE) - force_reference_frame=force_reference_frame, # Reference frame of the external force (ReferenceFrame.GLOBAL) - point_of_application=Test_point_of_application, # Position of the point of application - point_of_application_reference_frame=point_of_application_reference_frame, # Reference frame of the point of application (ReferenceFrame.GLOBAL) + # + # external_forces = ExternalForces() + # external_forces.add( + # key="Seg1", # Name of the segment where the external force is applied + # data=Seg1_force, # 3 x (n_shooting_points+1) array + # force_type=force_type, # Type of the external force (ExternalForceType.FORCE) + # force_reference_frame=force_reference_frame, # Reference frame of the external force (ReferenceFrame.GLOBAL) + # point_of_application=Seg1_point_of_application, # Position of the point of application + # point_of_application_reference_frame=point_of_application_reference_frame, # Reference frame of the point of application (ReferenceFrame.GLOBAL) + # ) + # external_forces.add( + # key="Test", # Name of the segment where the external force is applied + # data=Test_force, # 3 x (n_shooting_points+1) array + # force_type=force_type, # Type of the external force (ExternalForceType.FORCE) + # force_reference_frame=force_reference_frame, # Reference frame of the external force (ReferenceFrame.GLOBAL) + # point_of_application=Test_point_of_application, # Position of the point of application + # point_of_application_reference_frame=point_of_application_reference_frame, # Reference frame of the point of application (ReferenceFrame.GLOBAL) + # ) + + external_force_set = ExternalForceSetTimeSeries( + nb_frames=n_shooting, ) + external_force_set.add_translational_force("Seg1", Seg1_force, point_of_application=Seg1_point_of_application) + external_force_set.add_translational_force("Test", Test_force, point_of_application=Test_point_of_application) - bio_model = BiorbdModel(biorbd_model_path, external_forces=external_forces) + bio_model = BiorbdModel(biorbd_model_path, external_force_set=external_force_set) # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100) # Dynamics + numerical_time_series = {"external_forces": external_force_set.to_numerical_time_series()} + dynamics = DynamicsList() dynamics.add( DynamicsFcn.TORQUE_DRIVEN, expand_dynamics=expand_dynamics, phase_dynamics=phase_dynamics, - external_forces=external_forces, + numerical_data_timeseries=numerical_time_series, ) # Constraints diff --git a/bioptim/interfaces/interface_utils.py b/bioptim/interfaces/interface_utils.py index c37bbcb31..068b71875 100644 --- a/bioptim/interfaces/interface_utils.py +++ b/bioptim/interfaces/interface_utils.py @@ -1,4 +1,3 @@ -from sys import platform from time import perf_counter import numpy as np @@ -7,8 +6,8 @@ from bioptim.optimization.solution.solution import Solution from ..gui.online_callback_multiprocess import OnlineCallbackMultiprocess -from ..gui.online_callback_server import OnlineCallbackServer from ..gui.online_callback_multiprocess_server import OnlineCallbackMultiprocessServer +from ..gui.online_callback_server import OnlineCallbackServer from ..limits.path_conditions import Bounds from ..limits.penalty_helpers import PenaltyHelpers from ..misc.enums import InterpolationType, OnlineOptim @@ -402,15 +401,15 @@ def _get_a(ocp, phase_idx, node_idx, subnodes_idx, scaled): def get_numerical_timeseries(ocp, phase_idx, node_idx, subnodes_idx): - dict = ocp.nlp[phase_idx].numerical_data_timeseries - if dict is None: + timeseries = ocp.nlp[phase_idx].numerical_data_timeseries + if timeseries is None: return ocp.cx() else: values = None - for i_d, d in enumerate(dict): - for i_element in range(dict[d].shape[1]): + for i_d, (key, array) in enumerate(timeseries.items()): + for i_element in range(array.shape[1]): if values is None: - values = dict[d][:, i_element, node_idx] + values = array[:, i_element, node_idx] else: - values = vertcat(values, dict[d][:, i_element, node_idx]) + values = vertcat(values, array[:, i_element, node_idx]) return values diff --git a/bioptim/misc/enums.py b/bioptim/misc/enums.py index 0c15ba662..0d7ed2e43 100644 --- a/bioptim/misc/enums.py +++ b/bioptim/misc/enums.py @@ -1,5 +1,5 @@ -from enum import Enum, IntEnum, auto import platform +from enum import Enum, IntEnum, auto class PhaseDynamics(Enum): @@ -214,22 +214,3 @@ class MultiCyclicCycleSolutions(Enum): NONE = "none" FIRST_CYCLES = "first_cycles" ALL_CYCLES = "all_cycles" - - -class ReferenceFrame(Enum): - """ - Selection of reference frame - """ - - GLOBAL = "global" - LOCAL = "local" - - -class ExternalForceType(Enum): - """ - Selection of external forces type - """ - - FORCE = "force" - TORQUE = "torque" - TORQUE_AND_FORCE = "torque_and_force" diff --git a/bioptim/misc/external_forces.py b/bioptim/misc/external_forces.py deleted file mode 100644 index 63b73083f..000000000 --- a/bioptim/misc/external_forces.py +++ /dev/null @@ -1,153 +0,0 @@ -from typing import Any - -import numpy as np - -from ..misc.options import OptionGeneric, OptionDict -from ..misc.enums import ExternalForceType, ReferenceFrame - -class ExternalForce(OptionGeneric): - - def __init__( - self, - key: str, - force_data: np.ndarray, - torque_data: np.ndarray, - force_reference_frame: ReferenceFrame, - point_of_application_reference_frame: ReferenceFrame, - point_of_application: np.ndarray = None, - **extra_parameters: Any, - ): - super(ExternalForce, self).__init__(**extra_parameters) - if self.list_index != -1 and self.list_index is not None: - raise NotImplementedError("All external forces must be declared, list_index cannot be used for now.") - - if force_data is not None and force_data.shape[0] != 3: - raise ValueError(f"External forces must have 3 rows, got {force_data.shape[0]}") - - if torque_data is not None and torque_data.shape[0] != 3: - raise ValueError(f"External torques must have 3 rows, got {torque_data.shape[0]}") - - self.key = key - self.force_data = force_data - self.torque_data = torque_data - self.force_reference_frame = force_reference_frame - self.point_of_application = point_of_application - self.point_of_application_reference_frame = point_of_application_reference_frame - - @property - def len(self): - """ - Returns the number of nodes in the external forces - """ - if self.force_data is not None: - return self.force_data.shape[1] - elif self.torque_data is not None: - return self.torque_data.shape[1] - else: - raise ValueError("External forces must have either force_data or torque_data defined") - -class ExternalForces(OptionDict): - - def __init__(self, *args, **kwargs): - super(ExternalForces, self).__init__(sub_type=ExternalForce) - - def add( - self, - key: str, - data: np.ndarray, - force_type: ExternalForceType, - force_reference_frame: ReferenceFrame, - point_of_application: np.ndarray = None, - point_of_application_reference_frame: ReferenceFrame = None, - **extra_arguments: Any, - ): - - if key in self and self[key].force_type == force_type: - raise ValueError( - f"There should be only one external force with the same key, force_type (key:{key} and force_type: {force_type} already exists)") - - if force_reference_frame not in [ReferenceFrame.GLOBAL, ReferenceFrame.LOCAL]: - raise ValueError( - f"External force reference frame must be of type ReferenceFrame.GLOBAL or ReferenceFrame.LOCAL, got{force_reference_frame}") - - if point_of_application_reference_frame not in [ReferenceFrame.GLOBAL, ReferenceFrame.LOCAL, None]: - raise ValueError( - f"Point of application reference frame must be of type ReferenceFrame.GLOBAL or ReferenceFrame.LOCAL, got{point_of_application_reference_frame}") - - if point_of_application is not None: - if point_of_application.shape[0] != 3: - raise ValueError(f"Point of application must have 3 rows, got {point_of_application.shape[0]}") - if force_type == ExternalForceType.TORQUE: - raise ValueError("Point of application cannot be used with ExternalForceType.TORQUE") - - if key in self.keys(): - # They must both be in the same reference frame - if force_reference_frame != self[key].force_reference_frame: - raise ValueError(f"External forces must be in the same reference frame, got {force_reference_frame} and {self[key].force_reference_frame}") - - force_data = None - if key in self.keys() and self[key].force_data is not None: - if force_type == ExternalForceType.FORCE: - raise ValueError(f"The force is already defined for {key}") - else: - force_data = self[key].force_data - point_of_application = self[key].point_of_application - point_of_application_reference_frame = self[key].point_of_application_reference_frame - elif force_type == ExternalForceType.FORCE: - force_data = data - - torque_data = None - if key in self.keys() and self[key].torque_data is not None: - if force_type == ExternalForceType.TORQUE: - raise ValueError(f"The torque is already defined for {key}") - else: - torque_data = self[key].torque_data - elif force_type == ExternalForceType.TORQUE: - torque_data = data - - if force_type == ExternalForceType.TORQUE_AND_FORCE: - if force_data is not None: - raise ValueError(f"The force is already defined for {key}") - elif torque_data is not None: - raise ValueError(f"The torque is already defined for {key}") - else: - torque_data = data[:3, :] - force_data = data[3:, :] - - super(ExternalForces, self)._add( - key=key, - force_data=force_data, - torque_data=torque_data, - force_reference_frame=force_reference_frame, - point_of_application=point_of_application, - point_of_application_reference_frame=point_of_application_reference_frame, - **extra_arguments, - ) - - def print(self): - raise NotImplementedError("Printing of ExternalForces is not ready yet") - -def get_external_forces_segments(external_forces: ExternalForces): - - segments_to_apply_forces_in_global = [] - segments_to_apply_forces_in_local = [] - segments_to_apply_translational_forces = [] - if external_forces is not None: - for key in external_forces.keys(): - force_torsor = external_forces[key] - # Check sanity first - if force_torsor.force_reference_frame == ReferenceFrame.GLOBAL and force_torsor.point_of_application_reference_frame == ReferenceFrame.LOCAL and force_torsor.torque_data is not None: - raise NotImplementedError("External forces in global reference frame cannot have a point of application in the local reference frame and torques defined at the same time yet") - elif force_torsor.force_reference_frame == ReferenceFrame.LOCAL and force_torsor.point_of_application_reference_frame == ReferenceFrame.GLOBAL: - raise NotImplementedError("External forces in local reference frame cannot have a point of application in the global reference frame yet") - - if force_torsor.force_reference_frame == ReferenceFrame.GLOBAL and (force_torsor.point_of_application_reference_frame == ReferenceFrame.GLOBAL or force_torsor.point_of_application_reference_frame is None): - segments_to_apply_forces_in_global.append(force_torsor.key) - elif force_torsor.force_reference_frame == ReferenceFrame.LOCAL and (force_torsor.point_of_application_reference_frame == ReferenceFrame.LOCAL or force_torsor.point_of_application_reference_frame is None): - segments_to_apply_forces_in_local.append(force_torsor.key) - elif force_torsor.force_reference_frame == ReferenceFrame.GLOBAL and force_torsor.point_of_application_reference_frame == ReferenceFrame.LOCAL: - segments_to_apply_translational_forces.append(force_torsor.key) - else: - raise ValueError("This should not happen") - - return segments_to_apply_forces_in_global, segments_to_apply_forces_in_local, segments_to_apply_translational_forces \ No newline at end of file diff --git a/bioptim/models/biorbd/biorbd_model.py b/bioptim/models/biorbd/biorbd_model.py index dcdca885f..a8fea3996 100644 --- a/bioptim/models/biorbd/biorbd_model.py +++ b/bioptim/models/biorbd/biorbd_model.py @@ -1,3 +1,5 @@ +from typing import Callable + import biorbd_casadi as biorbd import numpy as np from biorbd_casadi import ( @@ -6,14 +8,13 @@ GeneralizedTorque, GeneralizedAcceleration, ) -from casadi import SX, MX, vertcat, horzcat, norm_fro, Function, reshape -from typing import Callable +from casadi import SX, MX, vertcat, horzcat, norm_fro, Function +from bioptim.models.biorbd.external_forces import ExternalForceSetTimeSeries from ..utils import _var_mapping, bounds_from_ranges from ...limits.path_conditions import Bounds from ...misc.mapping import BiMapping, BiMappingList from ...misc.utils import check_version -from ...misc.external_forces import ExternalForces, get_external_forces_segments from ...optimization.parameters import ParameterList check_version(biorbd, "1.11.1", "1.12.0") @@ -28,9 +29,8 @@ def __init__( self, bio_model: str | biorbd.Model, friction_coefficients: np.ndarray = None, - external_forces: ExternalForces = None, parameters: ParameterList = None, - nb_supplementary_forces_in_global=0, + external_force_set: ExternalForceSetTimeSeries = 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'") @@ -41,25 +41,8 @@ def __init__( parameters[param_key].apply_parameter(self) self._friction_coefficients = friction_coefficients - # TODO extend the interface to make conversions and allow all mixed cases in an unified way - ( - segments_to_apply_forces_in_global, - segments_to_apply_forces_in_local, - segments_to_apply_translational_forces, - ) = get_external_forces_segments(external_forces) - self._segments_to_apply_forces_in_global = segments_to_apply_forces_in_global - self._segments_to_apply_forces_in_local = segments_to_apply_forces_in_local - self._segments_to_apply_translational_forces = segments_to_apply_translational_forces - - # Declare the number of external forces that are not numerical values - if nb_supplementary_forces_in_global != 0 and ( - segments_to_apply_forces_in_global != [] - or segments_to_apply_forces_in_local != [] - or segments_to_apply_translational_forces != [] - ): - raise ValueError( - "You cannot provide nb_supplementary_forces_in_global and segments_to_apply_forces_in_global/segments_to_apply_forces_in_local/segments_to_apply_translational_forces at the same time" - ) + # External forces + self.external_force_set = self._set_external_force_set(external_force_set) # Declaration of MX variables of the right shape for the creation of CasADi Functions self.q = MX.sym("q_mx", self.nb_q, 1) @@ -69,36 +52,18 @@ def __init__( self.tau = MX.sym("tau_mx", self.nb_tau, 1) self.muscle = MX.sym("muscle_mx", self.nb_muscles, 1) self.activations = MX.sym("activations_mx", self.nb_muscles, 1) - - if nb_supplementary_forces_in_global != 0: - self.external_forces = MX.sym( - "supplementary_forces_in_global", - 9 * nb_supplementary_forces_in_global, - 1, - ) - self.external_forces_set = None - else: - self.external_forces = MX.sym( - "external_forces_mx", - 9 * self.nb_forces_in_global + 9 * self.nb_forces_in_local + 6 * self.nb_translational_forces, - 1, - ) - self.external_forces_set = self._dispatch_forces() + self.external_forces = MX.sym("external_forces_mx", external_force_set.nb_external_forces_components, 1) # TODO: remove mx (the MX parameters should be created inside the BiorbdModel) self.parameters = parameters.mx if parameters else MX() - @property - def nb_forces_in_global(self) -> int: - return len(self._segments_to_apply_forces_in_global) - - @property - def nb_forces_in_local(self) -> int: - return len(self._segments_to_apply_forces_in_local) - - @property - def nb_translational_forces(self) -> int: - return len(self._segments_to_apply_translational_forces) + def _set_external_force_set(self, external_force_set: ExternalForceSetTimeSeries): + """ + It checks the external forces and binds them to the model. + """ + external_force_set._check_segment_names(tuple([s.name().to_string() for s in self.model.segments()])) + external_force_set._check_all_string_points_of_application(self.marker_names) + external_force_set._bind() @property def name(self) -> str: @@ -462,50 +427,38 @@ def reorder_qddot_root_joints(qddot_root, qddot_joints) -> MX | SX: return vertcat(qddot_root, qddot_joints) def _dispatch_forces(self): + biorbd_external_forces = self.model.externalForceSet() + + # "type of external force": (function to call, number of force components, number of point of application components) + bioptim_to_biorbd_map = { + "in_global": (external_force_set.add, 6), + "torque_in_global": (external_force_set.add, 3), + "translational_in_global": (external_force_set.addTranslationalForce, 3), + "in_local": (external_force_set.addInSegmentReferenceFrame, 6), + "torque_in_local": (external_force_set.addInSegmentReferenceFrame, 3), + } - if self.nb_forces_in_global == 0 and self.nb_forces_in_local == 0 and self.nb_translational_forces == 0: - return None - else: - external_forces_set = self.model.externalForceSet() - forces_in_global = reshape( - self.external_forces[: 9 * self.nb_forces_in_global], (9, self.nb_forces_in_global) - ) - forces_in_local = reshape( - self.external_forces[ - 9 * self.nb_forces_in_global : 9 * self.nb_forces_in_global + 9 * self.nb_forces_in_local - ], - (9, self.nb_forces_in_local), - ) - translational_forces = reshape( - self.external_forces[9 * self.nb_forces_in_global + 9 * self.nb_forces_in_local :], - (6, self.nb_translational_forces), - ) + symbolic_counter = 0 + for attr in bioptim_to_biorbd_map.keys(): + function = bioptim_to_biorbd_map[attr][0] + for segment, forces in getattr(self, attr).items(): + for force in forces: + array_point_of_application = isinstance(force["point_of_application"], np.ndarray) + + start = symbolic_counter + stop = symbolic_counter + bioptim_to_biorbd_map[attr][1] + force_slicer = slice(start, stop) - if forces_in_global.shape[1] > 0: - # Add the external forces in the global reference frame - for i_element in range(forces_in_global.shape[1]): - name = self._segments_to_apply_forces_in_global[i_element] - values = forces_in_global[:6, i_element] - point_of_application = forces_in_global[6:9, i_element] - external_forces_set.add(name, values, point_of_application) - - if forces_in_local.shape[1] > 0: - # Add the external forces in the local reference frame - for i_element in range(forces_in_local.shape[1]): - name = self._segments_to_apply_forces_in_local[i_element] - values = forces_in_local[:6, i_element] - point_of_application = forces_in_local[6:9, i_element] - external_forces_set.addInSegmentReferenceFrame(name, values, point_of_application) - - elif translational_forces.shape[1] > 0: - # Add the translational forces - for i_elements in range(translational_forces.shape[1]): - name = self._segments_to_apply_translational_forces[i_elements] - values = translational_forces[:3, i_elements] - point_of_application = translational_forces[3:6, i_elements] - external_forces_set.addTranslationalForce(values, name, point_of_application) - - return external_forces_set + if array_point_of_application: + point_of_application = self.external_forces[slice(stop, stop + 3)] + else: + # turn it into a NodeSegment + point_of_application = biorbd.NodeSegment(force["point_of_application"]) + + function(segment, self.external_forces[force_slicer], point_of_application) + symbolic_counter += stop + 3 if array_point_of_application else stop + + return biorbd_external_forces def forward_dynamics(self, with_contact: bool = False) -> Function: @@ -514,7 +467,7 @@ def forward_dynamics(self, with_contact: bool = False) -> Function: tau_biorbd = GeneralizedTorque(self.tau) if with_contact: - if self.external_forces_set is None: + if self.external_force_set is None: biorbd_return = self.model.ForwardDynamicsConstraintsDirect(q_biorbd, qdot_biorbd, tau_biorbd).to_mx() else: biorbd_return = self.model.ForwardDynamicsConstraintsDirect( @@ -528,11 +481,11 @@ def forward_dynamics(self, with_contact: bool = False) -> Function: ["qddot"], ) else: - if self.external_forces_set is None: + if self.external_force_set is None: biorbd_return = self.model.ForwardDynamics(q_biorbd, qdot_biorbd, tau_biorbd).to_mx() else: biorbd_return = self.model.ForwardDynamics( - q_biorbd, qdot_biorbd, tau_biorbd, self.external_forces_set + q_biorbd, qdot_biorbd, tau_biorbd, self.external_forces ).to_mx() casadi_fun = Function( "forward_dynamics", diff --git a/bioptim/models/biorbd/external_forces.py b/bioptim/models/biorbd/external_forces.py new file mode 100644 index 000000000..702be0ff2 --- /dev/null +++ b/bioptim/models/biorbd/external_forces.py @@ -0,0 +1,231 @@ +import numpy as np + + +class ExternalForceSetTimeSeries: + """ + A class to manage external forces applied to a set of segments over a series of frames. + + Attributes + ---------- + _nb_frames : int + The number of frames in the time series. + in_global : dict[str, {}] + Dictionary to store global external forces for each segment. + torque_in_global : dict[str, {}] + Dictionary to store global torques for each segment. + translational_in_global : dict[str, {}] + Dictionary to store global translational forces for each segment. + in_local : dict[str, {}] + Dictionary to store local external forces for each segment. + torque_in_local : dict[str, {}] + Dictionary to store local torques for each segment. + _bind : bool + Flag to indicate if the external forces are binded and cannot be modified. + """ + + def __init__(self, nb_frames: int): + """ + Initialize the ExternalForceSetTimeSeries with the number of frames. + + Parameters + ---------- + nb_frames : int + The number of frames in the time series. + """ + self._nb_frames = nb_frames + + self.in_global: dict[str, {}] = {} + self.torque_in_global: dict[str, {}] = {} + self.translational_in_global: dict[str, {}] = {} + self.in_local: dict[str, {}] = {} + self.torque_in_local: dict[str, {}] = {} + + self._bind_flag = False + + @property + def _can_be_modified(self) -> bool: + return not self._bind_flag + + def _check_if_can_be_modified(self) -> None: + if not self._can_be_modified: + raise RuntimeError("External forces have been binded and cannot be modified anymore.") + + @property + def nb_frames(self) -> int: + return self._nb_frames + + def _check_values_frame_shape(self, values: np.ndarray) -> None: + if values.shape[1] != self._nb_frames: + raise ValueError( + f"External forces must have the same number of columns as the number of shooting points, " + f"got {values.shape[1]} instead of {self._nb_frames}" + ) + + def add(self, segment: str, values: np.ndarray, point_of_application: np.ndarray | str = None): + self._check_if_can_be_modified() + if values.shape[0] != 6: + raise ValueError(f"External forces must have 6 rows, got {values.shape[0]}") + self._check_values_frame_shape(values) + + point_of_application = np.zeros((3, self._nb_frames)) if point_of_application is None else point_of_application + self._check_point_of_application(point_of_application) + + self.in_global = ensure_list(self.in_global, segment) + self.in_global[segment].append({"values": values, "point_of_application": point_of_application}) + + def add_torque(self, segment: str, values: np.ndarray): + self._check_if_can_be_modified() + if values.shape[0] != 3: + raise ValueError(f"External torques must have 3 rows, got {values.shape[0]}") + self._check_values_frame_shape(values) + + self.torque_in_global = ensure_list(self.torque_in_global, segment) + self.torque_in_global[segment].append({"values": values, "point_of_application": None}) + + def add_translational_force(self, segment: str, values: np.ndarray, point_of_application: np.ndarray | str = None): + self._check_if_can_be_modified() + if values.shape[0] != 3: + raise ValueError(f"External forces must have 3 rows, got {values.shape[0]}") + self._check_values_frame_shape(values) + + point_of_application = np.zeros((3, self._nb_frames)) if point_of_application is None else point_of_application + self._check_point_of_application(point_of_application) + self.translational_in_global = ensure_list(self.translational_in_global, segment) + self.translational_in_global[segment].append({"values": values, "point_of_application": point_of_application}) + + def add_in_segment_frame(self, segment: str, values: np.ndarray, point_of_application: np.ndarray | str = None): + self._check_if_can_be_modified() + if values.shape[0] != 6: + raise ValueError(f"External forces must have 6 rows, got {values.shape[0]}") + self._check_values_frame_shape(values) + + point_of_application = np.zeros((3, self._nb_frames)) if point_of_application is None else point_of_application + self._check_point_of_application(point_of_application) + self.in_local = ensure_list(self.in_local, segment) + self.in_local[segment].append({"values": values, "point_of_application": point_of_application}) + + def add_torque_in_segment_frame(self, segment: str, values: np.ndarray): + self._check_if_can_be_modified() + if values.shape[0] != 3: + raise ValueError(f"External torques must have 3 rows, got {values.shape[0]}") + self._check_values_frame_shape(values) + + self.torque_in_local = ensure_list(self.torque_in_local, segment) + self.torque_in_local[segment].append({"values": values, "point_of_application": None}) + + @property + def nb_external_forces(self) -> int: + attributes = ["in_global", "torque_in_global", "translational_in_global", "in_local", "torque_in_local"] + return sum([len(values) for attr in attributes for values in getattr(self, attr).values()]) + + @property + def nb_external_forces_components(self) -> int: + """Return the number of vertical components of the external forces if concatenated in a unique vector""" + attributes_three_components = ["torque_in_global", "translational_in_global", "torque_in_local"] + attributes_six_components = ["in_global", "in_local"] + + components = 0 + for attr in attributes_three_components: + for values in getattr(self, attr).values(): + nb_point_of_application_as_str = sum( + [isinstance(force["point_of_application"], str) for force in values] + ) + components += 6 * len(values) - 3 * nb_point_of_application_as_str + + for attr in attributes_six_components: + for values in getattr(self, attr).values(): + nb_point_of_application_as_str = sum( + [isinstance(force["point_of_application"], str) for force in values] + ) + components += 9 * len(values) - 3 * nb_point_of_application_as_str + + return components + + def _check_point_of_application(self, point_of_application: np.ndarray | str) -> None: + if isinstance(point_of_application, str): + # The point of application is a string, nothing to check yet + return + + point_of_application = np.array(point_of_application) + if point_of_application.shape[0] != 3 and point_of_application.shape[1] != 3: + raise ValueError( + f"Point of application must have" + f" 3 rows and {self._nb_frames} columns, got {point_of_application.shape}" + ) + + return + + def _check_segment_names(self, segment_names: tuple[str, ...]) -> None: + attributes = ["in_global", "torque_in_global", "translational_in_global", "in_local", "torque_in_local"] + wrong_segments = [] + for attr in attributes: + for segment, _ in getattr(self, attr).items(): + if segment not in segment_names: + wrong_segments.append(segment) + + if wrong_segments: + raise ValueError( + f"Segments {wrong_segments} specified in the external forces are not in the model." + f" Available segments are {segment_names}." + ) + + def _check_all_string_points_of_application(self, model_points_of_application) -> None: + attributes = ["in_global", "translational_in_global", "in_local"] + wrong_points_of_application = [] + for attr in attributes: + for segment, forces in getattr(self, attr).items(): + for force in forces: + if ( + isinstance(force["point_of_application"], str) + and force["point_of_application"] not in model_points_of_application + ): + wrong_points_of_application.append(force["point_of_application"]) + + if wrong_points_of_application: + raise ValueError( + f"Points of application {wrong_points_of_application} specified in the external forces are not in the model." + f" Available points of application are {model_points_of_application}." + ) + + def _bind(self): + """prevent further modification of the external forces""" + self._bind_flag = True + + def to_numerical_time_series(self): + """Convert the external forces to a numerical time series""" + fext_numerical_time_series = np.zeros((self.nb_external_forces_components, 1, self.nb_frames + 1)) + + # "type of external force": (function to call, number of force components, number of point of application components) + bioptim_to_vector_map = { + "in_global": 6, + "torque_in_global": 3, + "translational_in_global": 3, + "in_local": 6, + "torque_in_local": 3, + } + + symbolic_counter = 0 + for attr in bioptim_to_vector_map.keys(): + for segment, forces in getattr(self, attr).items(): + for force in forces: + array_point_of_application = isinstance(force["point_of_application"], np.ndarray) + + start = symbolic_counter + stop = symbolic_counter + bioptim_to_vector_map[attr] + force_slicer = slice(start, stop) + fext_numerical_time_series[force_slicer, 0, :-1] = force["values"] + + if array_point_of_application: + poa_slicer = slice(stop, stop + 3) + fext_numerical_time_series[poa_slicer, 0, :-1] = force["point_of_application"] + + symbolic_counter += stop + 3 if array_point_of_application else stop + + return fext_numerical_time_series + + +def ensure_list(data, key) -> dict[str, list]: + """Ensure that the key exists in the data and the value is a list""" + if data.get(key) is None: + data[key] = [] + return data diff --git a/bioptim/optimization/non_linear_program.py b/bioptim/optimization/non_linear_program.py index 399cc8e03..15584e06b 100644 --- a/bioptim/optimization/non_linear_program.py +++ b/bioptim/optimization/non_linear_program.py @@ -1,20 +1,20 @@ from typing import Callable, Any import casadi -from casadi import SX, MX, Function, horzcat, vertcat +from casadi import SX, MX, vertcat -from .optimization_variable import OptimizationVariable, OptimizationVariableContainer +from .optimization_variable import OptimizationVariableContainer +from ..dynamics.dynamics_evaluation import DynamicsEvaluation +from ..dynamics.dynamics_functions import DynamicsFunctions from ..dynamics.ode_solver import OdeSolver from ..limits.path_conditions import InitialGuessList, BoundsList from ..misc.enums import ControlType, PhaseDynamics -from ..misc.options import OptionList from ..misc.mapping import NodeMapping -from ..dynamics.dynamics_evaluation import DynamicsEvaluation -from ..dynamics.dynamics_functions import DynamicsFunctions +from ..misc.options import OptionList from ..models.protocols.biomodel import BioModel from ..models.protocols.holonomic_biomodel import HolonomicBioModel -from ..models.protocols.variational_biomodel import VariationalBioModel from ..models.protocols.stochastic_biomodel import StochasticBioModel +from ..models.protocols.variational_biomodel import VariationalBioModel class NonLinearProgram: @@ -454,6 +454,7 @@ def get_var_from_states_or_controls( def get_external_forces( self, states: MX.sym, controls: MX.sym, algebraic_states: MX.sym, numerical_timeseries: MX.sym ): + # si c'est numeric c'est numerical nana sinon c'est dans les etats et tout ça def retrieve_forces(name: str, external_forces: MX): if name in self.states: From b771b597915859a6c71802c5ac27022cecf22e6a Mon Sep 17 00:00:00 2001 From: Ipuch Date: Wed, 30 Oct 2024 17:57:19 -0400 Subject: [PATCH 077/108] refactor : more tests to pass --- bioptim/models/biorbd/biorbd_model.py | 6 +- tests/shard1/test_dynamics.py | 3 - tests/shard1/test_enums.py | 19 -- tests/shard3/test_external_forces.py | 239 ++++++++++++++------------ 4 files changed, 131 insertions(+), 136 deletions(-) diff --git a/bioptim/models/biorbd/biorbd_model.py b/bioptim/models/biorbd/biorbd_model.py index a8fea3996..66732d3ba 100644 --- a/bioptim/models/biorbd/biorbd_model.py +++ b/bioptim/models/biorbd/biorbd_model.py @@ -42,7 +42,7 @@ def __init__( self._friction_coefficients = friction_coefficients # External forces - self.external_force_set = self._set_external_force_set(external_force_set) + self.external_force_set = self._set_external_force_set(external_force_set) if external_force_set else None # Declaration of MX variables of the right shape for the creation of CasADi Functions self.q = MX.sym("q_mx", self.nb_q, 1) @@ -52,7 +52,9 @@ def __init__( self.tau = MX.sym("tau_mx", self.nb_tau, 1) self.muscle = MX.sym("muscle_mx", self.nb_muscles, 1) self.activations = MX.sym("activations_mx", self.nb_muscles, 1) - self.external_forces = MX.sym("external_forces_mx", external_force_set.nb_external_forces_components, 1) + self.external_forces = MX.sym( + "external_forces_mx", external_force_set.nb_external_forces_components if external_force_set else 0, 1 + ) # TODO: remove mx (the MX parameters should be created inside the BiorbdModel) self.parameters = parameters.mx if parameters else MX() diff --git a/tests/shard1/test_dynamics.py b/tests/shard1/test_dynamics.py index 218ebf874..6d62ebca5 100644 --- a/tests/shard1/test_dynamics.py +++ b/tests/shard1/test_dynamics.py @@ -20,9 +20,6 @@ ParameterContainer, ParameterList, PhaseDynamics, - ExternalForces, - ExternalForceType, - ReferenceFrame, ) from tests.utils import TestUtils diff --git a/tests/shard1/test_enums.py b/tests/shard1/test_enums.py index f6dc04040..5c475234b 100644 --- a/tests/shard1/test_enums.py +++ b/tests/shard1/test_enums.py @@ -13,8 +13,6 @@ SolutionIntegrator, QuadratureRule, SoftContactDynamics, - ExternalForceType, - ReferenceFrame, ) from bioptim.misc.enums import SolverType, PenaltyType, ConstraintType @@ -186,20 +184,3 @@ def test_multi_cyclic_cycle_solutions(): # verify the number of elements assert len(MultiCyclicCycleSolutions) == 3 - - -def test_external_forces_type(): - assert ExternalForceType.FORCE.value == "force" - assert ExternalForceType.TORQUE.value == "torque" - assert ExternalForceType.TORQUE_AND_FORCE.value == "torque_and_force" - - # verify the number of elements - assert len(ExternalForceType) == 3 - - -def test_reference_frame(): - assert ReferenceFrame.GLOBAL.value == "global" - assert ReferenceFrame.LOCAL.value == "local" - - # verify the number of elements - assert len(ReferenceFrame) == 2 diff --git a/tests/shard3/test_external_forces.py b/tests/shard3/test_external_forces.py index 28f023f6b..a3ec836e6 100644 --- a/tests/shard3/test_external_forces.py +++ b/tests/shard3/test_external_forces.py @@ -7,14 +7,11 @@ from bioptim import ( PhaseDynamics, SolutionMerge, - ExternalForceType, - ReferenceFrame, BiorbdModel, Node, OptimalControlProgram, DynamicsList, DynamicsFcn, - ExternalForces, ObjectiveList, ObjectiveFcn, ConstraintList, @@ -25,67 +22,73 @@ @pytest.mark.parametrize("phase_dynamics", [PhaseDynamics.SHARED_DURING_THE_PHASE, PhaseDynamics.ONE_PER_NODE]) @pytest.mark.parametrize("use_sx", [True, False]) -@pytest.mark.parametrize("force_type", [ExternalForceType.FORCE, ExternalForceType.TORQUE]) -@pytest.mark.parametrize("force_reference_frame", [ReferenceFrame.GLOBAL, ReferenceFrame.LOCAL]) -@pytest.mark.parametrize("use_point_of_applications", [True, False]) -@pytest.mark.parametrize("point_of_application_reference_frame", [ReferenceFrame.GLOBAL, ReferenceFrame.LOCAL]) -def test_example_external_forces(phase_dynamics, - use_sx, - force_type, - force_reference_frame, - use_point_of_applications, - point_of_application_reference_frame): +# @pytest.mark.parametrize("force_type", [ExternalForceType.FORCE, ExternalForceType.TORQUE]) +# @pytest.mark.parametrize("force_reference_frame", [ReferenceFrame.GLOBAL, ReferenceFrame.LOCAL]) +# @pytest.mark.parametrize("use_point_of_applications", [True, False]) +# @pytest.mark.parametrize("point_of_application_reference_frame", [ReferenceFrame.GLOBAL, ReferenceFrame.LOCAL]) +def test_example_external_forces( + phase_dynamics, + use_sx, + # force_type, + # force_reference_frame, + # use_point_of_applications, + # point_of_application_reference_frame, +): from bioptim.examples.getting_started import example_external_forces as ocp_module bioptim_folder = os.path.dirname(ocp_module.__file__) - if use_point_of_applications == False: - point_of_application_reference_frame = None - - if force_type == ExternalForceType.FORCE and force_reference_frame == ReferenceFrame.GLOBAL and point_of_application_reference_frame == ReferenceFrame.GLOBAL: - # This combination is already tested in test_global_getting_started.py - return - - # Errors for some combinations - if force_type == ExternalForceType.TORQUE and use_point_of_applications: - with pytest.raises( - ValueError, - match="Point of application cannot be used with ExternalForceType.TORQUE", - ): - ocp = ocp_module.prepare_ocp( - biorbd_model_path=bioptim_folder + "/models/cube_with_forces.bioMod", - phase_dynamics=phase_dynamics, - force_type=force_type, - force_reference_frame=force_reference_frame, - use_point_of_applications=use_point_of_applications, - point_of_application_reference_frame=point_of_application_reference_frame, - use_sx=use_sx, - ) - return - - if force_reference_frame == ReferenceFrame.LOCAL and point_of_application_reference_frame == ReferenceFrame.GLOBAL: - with pytest.raises( - NotImplementedError, - match="External forces in local reference frame cannot have a point of application in the global reference frame yet", - ): - ocp = ocp_module.prepare_ocp( - biorbd_model_path=bioptim_folder + "/models/cube_with_forces.bioMod", - phase_dynamics=phase_dynamics, - force_type=force_type, - force_reference_frame=force_reference_frame, - use_point_of_applications=use_point_of_applications, - point_of_application_reference_frame=point_of_application_reference_frame, - use_sx=use_sx, - ) - return + # if use_point_of_applications == False: + # point_of_application_reference_frame = None + # + # if ( + # force_type == ExternalForceType.FORCE + # and force_reference_frame == ReferenceFrame.GLOBAL + # and point_of_application_reference_frame == ReferenceFrame.GLOBAL + # ): + # # This combination is already tested in test_global_getting_started.py + # return + # + # # Errors for some combinations + # if force_type == ExternalForceType.TORQUE and use_point_of_applications: + # with pytest.raises( + # ValueError, + # match="Point of application cannot be used with ExternalForceType.TORQUE", + # ): + # ocp = ocp_module.prepare_ocp( + # biorbd_model_path=bioptim_folder + "/models/cube_with_forces.bioMod", + # phase_dynamics=phase_dynamics, + # force_type=force_type, + # force_reference_frame=force_reference_frame, + # use_point_of_applications=use_point_of_applications, + # point_of_application_reference_frame=point_of_application_reference_frame, + # use_sx=use_sx, + # ) + # return + # + # if force_reference_frame == ReferenceFrame.LOCAL and point_of_application_reference_frame == ReferenceFrame.GLOBAL: + # with pytest.raises( + # NotImplementedError, + # match="External forces in local reference frame cannot have a point of application in the global reference frame yet", + # ): + # ocp = ocp_module.prepare_ocp( + # biorbd_model_path=bioptim_folder + "/models/cube_with_forces.bioMod", + # phase_dynamics=phase_dynamics, + # force_type=force_type, + # force_reference_frame=force_reference_frame, + # use_point_of_applications=use_point_of_applications, + # point_of_application_reference_frame=point_of_application_reference_frame, + # use_sx=use_sx, + # ) + # return ocp = ocp_module.prepare_ocp( biorbd_model_path=bioptim_folder + "/models/cube_with_forces.bioMod", phase_dynamics=phase_dynamics, - force_type=force_type, - force_reference_frame=force_reference_frame, - use_point_of_applications=use_point_of_applications, - point_of_application_reference_frame=point_of_application_reference_frame, + # force_type=force_type, + # force_reference_frame=force_reference_frame, + # use_point_of_applications=use_point_of_applications, + # point_of_application_reference_frame=point_of_application_reference_frame, use_sx=use_sx, ) sol = ocp.solve() @@ -113,14 +116,14 @@ def test_example_external_forces(phase_dynamics, npt.assert_almost_equal(f[0, 0], 19847.887805189126) # initial and final controls - npt.assert_almost_equal(tau[:, 0], np.array([2.03776708e-09, 1.27132259e+01, 2.32230666e-27, 0.0])) - npt.assert_almost_equal(tau[:, 10], np.array([-8.23139024e-10, 1.07110012e+01, 5.45884198e-26, 0.0])) - npt.assert_almost_equal(tau[:, 20], np.array([-6.72563418e-10, 8.70877651e+00, -2.09699944e-26, 0.0])) - npt.assert_almost_equal(tau[:, -1], np.array([2.03777147e-09, 6.90677426e+00, -1.66232727e-25, 0.0])) + npt.assert_almost_equal(tau[:, 0], np.array([2.03776708e-09, 1.27132259e01, 2.32230666e-27, 0.0])) + npt.assert_almost_equal(tau[:, 10], np.array([-8.23139024e-10, 1.07110012e01, 5.45884198e-26, 0.0])) + npt.assert_almost_equal(tau[:, 20], np.array([-6.72563418e-10, 8.70877651e00, -2.09699944e-26, 0.0])) + npt.assert_almost_equal(tau[:, -1], np.array([2.03777147e-09, 6.90677426e00, -1.66232727e-25, 0.0])) # initial and final position - npt.assert_almost_equal(q[:, 0], np.array([-4.69166539e-15, 6.03679344e-16, -1.43453710e-17, 0.0]), decimal=5) - npt.assert_almost_equal(q[:, -1], np.array([-4.69169398e-15, 2.00000000e+00, -1.43453709e-17, 0.0]), decimal=5) + npt.assert_almost_equal(q[:, 0], np.array([-4.69166539e-15, 6.03679344e-16, -1.43453710e-17, 0.0]), decimal=5) + npt.assert_almost_equal(q[:, -1], np.array([-4.69169398e-15, 2.00000000e00, -1.43453709e-17, 0.0]), decimal=5) # how the force is stored if force_reference_frame == ReferenceFrame.LOCAL: @@ -128,7 +131,7 @@ def test_example_external_forces(phase_dynamics, else: data = ocp.nlp[0].numerical_data_timeseries["forces_in_global"] npt.assert_equal(data.shape, (9, 2, 31)) - npt.assert_almost_equal(data[:, 0, 0], np.array([0., 0., -2., 0., 0., 0., 0., 0., 0.])) + npt.assert_almost_equal(data[:, 0, 0], np.array([0.0, 0.0, -2.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])) else: if force_reference_frame == ReferenceFrame.GLOBAL: @@ -137,39 +140,49 @@ def test_example_external_forces(phase_dynamics, npt.assert_almost_equal(f[0, 0], 7067.851604540217) # initial and final controls - npt.assert_almost_equal(tau[:, 0], np.array([2.03776698e-09, 6.98419368e+00, -8.87085933e-09, 0.0])) - npt.assert_almost_equal(tau[:, 10], np.array([-8.23139073e-10, 6.24337052e+00, -9.08634557e-09, 0.0])) - npt.assert_almost_equal(tau[:, 20], np.array([-6.72563331e-10, 5.50254736e+00, -9.36203643e-09, 0.00])) - npt.assert_almost_equal(tau[:, -1], np.array([2.03777157e-09, 4.83580652e+00, -9.46966399e-09, 0.0])) + npt.assert_almost_equal(tau[:, 0], np.array([2.03776698e-09, 6.98419368e00, -8.87085933e-09, 0.0])) + npt.assert_almost_equal( + tau[:, 10], np.array([-8.23139073e-10, 6.24337052e00, -9.08634557e-09, 0.0]) + ) + npt.assert_almost_equal( + tau[:, 20], np.array([-6.72563331e-10, 5.50254736e00, -9.36203643e-09, 0.00]) + ) + npt.assert_almost_equal(tau[:, -1], np.array([2.03777157e-09, 4.83580652e00, -9.46966399e-09, 0.0])) # initial and final position - npt.assert_almost_equal(q[:, 0], np.array([-4.69166257e-15, 6.99774238e-16, -1.15797965e+00, 0.0]), - decimal=5) - npt.assert_almost_equal(q[:, -1], np.array([-4.69169076e-15, 2.00000000e+00, -1.15744926e+00, 0.0]), - decimal=5) + npt.assert_almost_equal( + q[:, 0], np.array([-4.69166257e-15, 6.99774238e-16, -1.15797965e00, 0.0]), decimal=5 + ) + npt.assert_almost_equal( + q[:, -1], np.array([-4.69169076e-15, 2.00000000e00, -1.15744926e00, 0.0]), decimal=5 + ) # how the force is stored data = ocp.nlp[0].numerical_data_timeseries["translational_forces"] npt.assert_equal(data.shape, (6, 2, 31)) - npt.assert_almost_equal(data[:, 0, 0], np.array([0., 0., -2., 0.05, -0.05, 0.007])) + npt.assert_almost_equal(data[:, 0, 0], np.array([0.0, 0.0, -2.0, 0.05, -0.05, 0.007])) else: npt.assert_almost_equal(f[0, 0], 7067.851604540217) # initial and final controls - npt.assert_almost_equal(tau[:, 0], np.array([1.53525114e-09, 6.98419368e+00, -4.06669623e-10, 0.0])) - npt.assert_almost_equal(tau[:, 10], np.array([-6.45031008e-10, 6.24337052e+00, -4.06669624e-10, 0.0])) - npt.assert_almost_equal(tau[:, 20], np.array([-5.51259131e-10, 5.50254736e+00, -4.06669632e-10, 0.0])) - npt.assert_almost_equal(tau[:, -1], np.array([1.64434894e-09, 4.83580652e+00, -4.06669638e-10, 0.0])) + npt.assert_almost_equal(tau[:, 0], np.array([1.53525114e-09, 6.98419368e00, -4.06669623e-10, 0.0])) + npt.assert_almost_equal(tau[:, 10], np.array([-6.45031008e-10, 6.24337052e00, -4.06669624e-10, 0.0])) + npt.assert_almost_equal(tau[:, 20], np.array([-5.51259131e-10, 5.50254736e00, -4.06669632e-10, 0.0])) + npt.assert_almost_equal(tau[:, -1], np.array([1.64434894e-09, 4.83580652e00, -4.06669638e-10, 0.0])) # initial and final position - npt.assert_almost_equal(q[:, 0], np.array([-3.41506199e-15, 6.99773953e-16, -4.11684795e-12, 0.0]), decimal=5) - npt.assert_almost_equal(q[:, -1], np.array([-3.84536375e-15, 2.00000000e+00, 6.27099006e-11, 0.0]), decimal=5) + npt.assert_almost_equal( + q[:, 0], np.array([-3.41506199e-15, 6.99773953e-16, -4.11684795e-12, 0.0]), decimal=5 + ) + npt.assert_almost_equal( + q[:, -1], np.array([-3.84536375e-15, 2.00000000e00, 6.27099006e-11, 0.0]), decimal=5 + ) # how the force is stored data = ocp.nlp[0].numerical_data_timeseries["forces_in_global"] npt.assert_equal(data.shape, (9, 2, 31)) - npt.assert_almost_equal(data[:, 0, 0], np.array([0., 0., 0., 0., 0., -2., 0., 0., 0.])) + npt.assert_almost_equal(data[:, 0, 0], np.array([0.0, 0.0, 0.0, 0.0, 0.0, -2.0, 0.0, 0.0, 0.0])) else: if use_point_of_applications: @@ -177,48 +190,51 @@ def test_example_external_forces(phase_dynamics, npt.assert_almost_equal(f[0, 0], 7076.043800572127) # initial and final controls - npt.assert_almost_equal(tau[:, 0], np.array([0.01922272, 6.98428645, -0.28099094, 0.])) - npt.assert_almost_equal(tau[:, 10], np.array([4.40289150e-03, 6.24343465e+00, -2.16557082e-01, 0.0])) - npt.assert_almost_equal(tau[:, 20], np.array([-0.01041693, 5.50258285, -0.14650525, 0.])) - npt.assert_almost_equal(tau[:, -1], np.array([-0.02375477, 4.83581623, -0.12679319, 0.])) + npt.assert_almost_equal(tau[:, 0], np.array([0.01922272, 6.98428645, -0.28099094, 0.0])) + npt.assert_almost_equal(tau[:, 10], np.array([4.40289150e-03, 6.24343465e00, -2.16557082e-01, 0.0])) + npt.assert_almost_equal(tau[:, 20], np.array([-0.01041693, 5.50258285, -0.14650525, 0.0])) + npt.assert_almost_equal(tau[:, -1], np.array([-0.02375477, 4.83581623, -0.12679319, 0.0])) # initial and final position - npt.assert_almost_equal(q[:, 0], np.array([-4.65561476e-15, 7.00215056e-16, -1.34683990e-03, 0.0]), - decimal=5) - npt.assert_almost_equal(q[:, -1], np.array([-4.76138727e-15, 2.00000000e+00, 2.00652560e-03, 0.0]), - decimal=5) + npt.assert_almost_equal( + q[:, 0], np.array([-4.65561476e-15, 7.00215056e-16, -1.34683990e-03, 0.0]), decimal=5 + ) + npt.assert_almost_equal( + q[:, -1], np.array([-4.76138727e-15, 2.00000000e00, 2.00652560e-03, 0.0]), decimal=5 + ) # how the force is stored data = ocp.nlp[0].numerical_data_timeseries["forces_in_local"] npt.assert_equal(data.shape, (9, 2, 31)) - npt.assert_almost_equal(data[:, 0, 0], np.array([0., 0., 0., 0., 0., -2., 0.05, -0.05, - 0.007])) + npt.assert_almost_equal( + data[:, 0, 0], np.array([0.0, 0.0, 0.0, 0.0, 0.0, -2.0, 0.05, -0.05, 0.007]) + ) else: npt.assert_almost_equal(f[0, 0], 7067.851604540217) # initial and final controls - npt.assert_almost_equal(tau[:, 0], np.array([1.92280974e-09, 6.98419368e+00, -4.00749051e-10, 0.0])) - npt.assert_almost_equal(tau[:, 10], np.array([-8.87099972e-10, 6.24337052e+00, 2.22483147e-10, 0.0])) - npt.assert_almost_equal(tau[:, 20], np.array([-6.85527926e-10, 5.50254736e+00, 1.70072550e-10, 0.0])) - npt.assert_almost_equal(tau[:, -1], np.array([2.07070380e-09, 4.83580652e+00, -3.91202705e-10, 0.0])) + npt.assert_almost_equal(tau[:, 0], np.array([1.92280974e-09, 6.98419368e00, -4.00749051e-10, 0.0])) + npt.assert_almost_equal(tau[:, 10], np.array([-8.87099972e-10, 6.24337052e00, 2.22483147e-10, 0.0])) + npt.assert_almost_equal(tau[:, 20], np.array([-6.85527926e-10, 5.50254736e00, 1.70072550e-10, 0.0])) + npt.assert_almost_equal(tau[:, -1], np.array([2.07070380e-09, 4.83580652e00, -3.91202705e-10, 0.0])) # initial and final position - npt.assert_almost_equal(q[:, 0], np.array([-4.64041726e-15, 6.99774094e-16, 4.75924448e-11, 0.0]), - decimal=5) - npt.assert_almost_equal(q[:, -1], np.array([-4.74298258e-15, 2.00000000e+00, 3.79354612e-11, 0.0]), - decimal=5) + npt.assert_almost_equal( + q[:, 0], np.array([-4.64041726e-15, 6.99774094e-16, 4.75924448e-11, 0.0]), decimal=5 + ) + npt.assert_almost_equal( + q[:, -1], np.array([-4.74298258e-15, 2.00000000e00, 3.79354612e-11, 0.0]), decimal=5 + ) # how the force is stored data = ocp.nlp[0].numerical_data_timeseries["forces_in_local"] npt.assert_equal(data.shape, (9, 2, 31)) - npt.assert_almost_equal(data[:, 0, 0], np.array([0., 0., 0., 0., 0., -2., 0.0, 0.0, - 0.0])) + npt.assert_almost_equal(data[:, 0, 0], np.array([0.0, 0.0, 0.0, 0.0, 0.0, -2.0, 0.0, 0.0, 0.0])) # detailed cost values npt.assert_almost_equal(sol.detailed_cost[0]["cost_value_weighted"], f[0, 0]) - def prepare_ocp( biorbd_model_path: str = "models/cube_with_forces.bioMod", use_torque_and_force_at_the_same_time: bool = False, @@ -350,9 +366,11 @@ def prepare_ocp( constraints=constraints, ) + def test_example_external_forces_all_at_once(): from bioptim.examples.getting_started import example_external_forces as ocp_module + bioptim_folder = os.path.dirname(ocp_module.__file__) ocp_false = prepare_ocp( @@ -377,14 +395,14 @@ def test_example_external_forces_all_at_once(): q_false, qdot_false, tau_false = states_false["q"], states_false["qdot"], controls_false["tau"] # initial and final controls - npt.assert_almost_equal(tau_false[:, 0], np.array([0.18595882, 6.98424521, -0.30071214, 0.])) - npt.assert_almost_equal(tau_false[:, 10], np.array([-0.05224518, 6.24340374, -0.17010067, 0.])) - npt.assert_almost_equal(tau_false[:, 20], np.array([-0.03359529, 5.50256227, -0.11790254, 0.])) - npt.assert_almost_equal(tau_false[:, -1], np.array([0.07805599, 4.83580495, -0.14719148, 0.])) + npt.assert_almost_equal(tau_false[:, 0], np.array([0.18595882, 6.98424521, -0.30071214, 0.0])) + npt.assert_almost_equal(tau_false[:, 10], np.array([-0.05224518, 6.24340374, -0.17010067, 0.0])) + npt.assert_almost_equal(tau_false[:, 20], np.array([-0.03359529, 5.50256227, -0.11790254, 0.0])) + npt.assert_almost_equal(tau_false[:, -1], np.array([0.07805599, 4.83580495, -0.14719148, 0.0])) # initial and final position - npt.assert_almost_equal(q_false[:, 0], np.array([-4.01261246e-15, 7.00046055e-16, 2.15375856e-03, 0.0]), decimal=5) - npt.assert_almost_equal(q_false[:, -1], np.array([-4.29786206e-15, 2.00000000e+00, 1.66939403e-03, 0.0]), decimal=5) + npt.assert_almost_equal(q_false[:, 0], np.array([-4.01261246e-15, 7.00046055e-16, 2.15375856e-03, 0.0]), decimal=5) + npt.assert_almost_equal(q_false[:, -1], np.array([-4.29786206e-15, 2.00000000e00, 1.66939403e-03, 0.0]), decimal=5) # initial and final velocities npt.assert_almost_equal(qdot_false[:, 0], np.array([0.0, 0.0, 0.0, 0.0]), decimal=5) @@ -393,18 +411,15 @@ def test_example_external_forces_all_at_once(): # how the force is stored data_Seg1_false = ocp_false.nlp[0].numerical_data_timeseries["forces_in_global"] npt.assert_equal(data_Seg1_false.shape, (9, 1, 31)) - npt.assert_almost_equal(data_Seg1_false[:, 0, 0], np.array([0. , 1.8 , 0. , 0. , 0. , -2. , 0.05 , -0.05 , - 0.007])) + npt.assert_almost_equal(data_Seg1_false[:, 0, 0], np.array([0.0, 1.8, 0.0, 0.0, 0.0, -2.0, 0.05, -0.05, 0.007])) data_Test_false = ocp_false.nlp[0].numerical_data_timeseries["forces_in_local"] npt.assert_equal(data_Test_false.shape, (9, 1, 31)) - npt.assert_almost_equal(data_Test_false[:, 0, 0], np.array([0. , -1.8, 0. , 0. , 0. , 5. , -0.009, 0.01 , - -0.01])) + npt.assert_almost_equal(data_Test_false[:, 0, 0], np.array([0.0, -1.8, 0.0, 0.0, 0.0, 5.0, -0.009, 0.01, -0.01])) # detailed cost values npt.assert_almost_equal(sol_false.detailed_cost[0]["cost_value_weighted"], f_false[0, 0]) - ocp_true = prepare_ocp( biorbd_model_path=bioptim_folder + "/models/cube_with_forces.bioMod", use_torque_and_force_at_the_same_time=True, @@ -486,4 +501,4 @@ def test_fail(): data=fake_force, force_type=ExternalForceType.TORQUE, force_reference_frame=ReferenceFrame.GLOBAL, - ) \ No newline at end of file + ) From d65c63a96550bb1353cc5338e11fbce3dc421197 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Wed, 30 Oct 2024 18:43:10 -0400 Subject: [PATCH 078/108] fix: model external force set --- bioptim/models/biorbd/biorbd_model.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/bioptim/models/biorbd/biorbd_model.py b/bioptim/models/biorbd/biorbd_model.py index 66732d3ba..162f285d7 100644 --- a/bioptim/models/biorbd/biorbd_model.py +++ b/bioptim/models/biorbd/biorbd_model.py @@ -473,7 +473,7 @@ def forward_dynamics(self, with_contact: bool = False) -> Function: biorbd_return = self.model.ForwardDynamicsConstraintsDirect(q_biorbd, qdot_biorbd, tau_biorbd).to_mx() else: biorbd_return = self.model.ForwardDynamicsConstraintsDirect( - q_biorbd, qdot_biorbd, tau_biorbd, self.external_forces_set + q_biorbd, qdot_biorbd, tau_biorbd, self.external_forces ).to_mx() casadi_fun = Function( "constrained_forward_dynamics", @@ -503,11 +503,11 @@ def inverse_dynamics(self, with_contact: bool = False) -> Function: q_biorbd = GeneralizedCoordinates(self.q) qdot_biorbd = GeneralizedVelocity(self.qdot) qddot_biorbd = GeneralizedAcceleration(self.qddot) - if self.external_forces_set is None: + if self.external_force_set is None: biorbd_return = self.model.InverseDynamics(q_biorbd, qdot_biorbd, qddot_biorbd).to_mx() else: biorbd_return = self.model.InverseDynamics( - q_biorbd, qdot_biorbd, qddot_biorbd, self.external_forces_set + q_biorbd, qdot_biorbd, qddot_biorbd, self.external_force_set ).to_mx() casadi_fun = Function( "inverse_dynamics", @@ -523,13 +523,13 @@ def contact_forces_from_constrained_forward_dynamics(self) -> Function: q_biorbd = GeneralizedCoordinates(self.q) qdot_biorbd = GeneralizedVelocity(self.qdot) tau_biorbd = GeneralizedTorque(self.tau) - if self.external_forces_set is None: + if self.external_force_set is None: biorbd_return = self.model.ContactForcesFromForwardDynamicsConstraintsDirect( q_biorbd, qdot_biorbd, tau_biorbd ).to_mx() else: biorbd_return = self.model.ContactForcesFromForwardDynamicsConstraintsDirect( - q_biorbd, qdot_biorbd, tau_biorbd, self.external_forces_set + q_biorbd, qdot_biorbd, tau_biorbd, self.external_force_set ).to_mx() casadi_fun = Function( "contact_forces_from_constrained_forward_dynamics", From d1ab3c3560bfd4c1d9b9ea740cd291f16aca04f3 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Wed, 30 Oct 2024 18:43:51 -0400 Subject: [PATCH 079/108] early tests changes not there yet --- tests/shard1/test_dynamics.py | 34 +++++++++++++++++----------------- 1 file changed, 17 insertions(+), 17 deletions(-) diff --git a/tests/shard1/test_dynamics.py b/tests/shard1/test_dynamics.py index 6d62ebca5..00f521167 100644 --- a/tests/shard1/test_dynamics.py +++ b/tests/shard1/test_dynamics.py @@ -20,6 +20,7 @@ ParameterContainer, ParameterList, PhaseDynamics, + ExternalForceSetTimeSeries, ) from tests.utils import TestUtils @@ -39,7 +40,13 @@ def __init__(self, nlp, use_sx): @pytest.mark.parametrize("phase_dynamics", [PhaseDynamics.SHARED_DURING_THE_PHASE, PhaseDynamics.ONE_PER_NODE]) @pytest.mark.parametrize("cx", [MX, SX]) -@pytest.mark.parametrize("with_external_force", [False, True]) +@pytest.mark.parametrize( + "with_external_force", + [ + # False, + True + ], +) @pytest.mark.parametrize("with_contact", [False, True]) def test_torque_driven(with_contact, with_external_force, cx, phase_dynamics): # Prepare the program @@ -47,8 +54,9 @@ def test_torque_driven(with_contact, with_external_force, cx, phase_dynamics): nlp.ns = 5 external_forces = None + numerical_time_series = None if with_external_force: - external_forces_array = np.zeros((9, nlp.ns + 1)) + external_forces_array = np.zeros((9, nlp.ns)) external_forces_array[:, 0] = [ 0.374540118847362, 0.950714306409916, @@ -105,23 +113,15 @@ def test_torque_driven(with_contact, with_external_force, cx, phase_dynamics): 0, ] - external_forces = ExternalForces() - external_forces.add( - key="Seg0", - data=external_forces_array[:3, :], - force_type=ExternalForceType.TORQUE, - force_reference_frame=ReferenceFrame.GLOBAL, - ) - external_forces.add( - key="Seg0", - data=external_forces_array[3:6, :], - force_type=ExternalForceType.FORCE, - force_reference_frame=ReferenceFrame.GLOBAL, - ) + external_forces = ExternalForceSetTimeSeries(nb_frames=nlp.ns) + # external_forces.add_torque("Seg0", external_forces_array[:3, :]) + # external_forces.add_translational_force("Seg0", external_forces_array[3:6, :]) + external_forces.add("Seg0", external_forces_array[:6, :]) + numerical_time_series = {"external_forces": external_forces.to_numerical_time_series()} nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod", - external_forces=external_forces, + external_force_set=external_forces, ) nlp.cx = cx @@ -146,7 +146,7 @@ def test_torque_driven(with_contact, with_external_force, cx, phase_dynamics): with_contact=with_contact, expand_dynamics=True, phase_dynamics=phase_dynamics, - external_forces=external_forces, + numerical_data_timeseries=numerical_time_series, ), False, ) From d699a8494fefb4e869d6972f6c314292521d2076 Mon Sep 17 00:00:00 2001 From: eve-mac Date: Wed, 30 Oct 2024 10:36:10 -0400 Subject: [PATCH 080/108] made changes requested by pariterre --- bioptim/dynamics/dynamics_functions.py | 31 ++++++++++++++++--- bioptim/limits/penalty.py | 1 - bioptim/limits/phase_transition.py | 6 +--- bioptim/models/protocols/biomodel.py | 1 - .../optimization/optimal_control_program.py | 4 +-- ...st_global_stochastic_except_collocation.py | 1 + 6 files changed, 31 insertions(+), 13 deletions(-) diff --git a/bioptim/dynamics/dynamics_functions.py b/bioptim/dynamics/dynamics_functions.py index cb6bda67f..10d50897b 100644 --- a/bioptim/dynamics/dynamics_functions.py +++ b/bioptim/dynamics/dynamics_functions.py @@ -3,6 +3,7 @@ from .dynamics_evaluation import DynamicsEvaluation from .fatigue.fatigue_dynamics import FatigueList from ..misc.mapping import BiMapping +from ..misc.enums import DefectType from ..optimization.optimization_variable import OptimizationVariable @@ -151,7 +152,31 @@ def torque_driven( if fatigue is not None and "tau" in fatigue: dxdt = fatigue["tau"].dynamics(dxdt, nlp, states, controls) - return DynamicsEvaluation(dxdt, None) + defects = None + # TODO: contacts and fatigue to be handled with implicit dynamics + if nlp.ode_solver.defects_type == DefectType.IMPLICIT: + if not with_contact and fatigue is None: + qddot = DynamicsFunctions.get(nlp.states_dot["qddot"], nlp.states_dot.scaled.cx) + tau_id = DynamicsFunctions.inverse_dynamics( + nlp, q, qdot, qddot, with_contact, external_forces + ) + defects = nlp.cx(dq.shape[0] + tau_id.shape[0], tau_id.shape[1]) + + dq_defects = [] + for _ in range(tau_id.shape[1]): + dq_defects.append( + dq + - DynamicsFunctions.compute_qdot( + nlp, + q, + DynamicsFunctions.get(nlp.states_dot.scaled["qdot"], nlp.states_dot.scaled.cx), + ) + ) + defects[: dq.shape[0], :] = horzcat(*dq_defects) + # We modified on purpose the size of the tau to keep the zero in the defects in order to respect the dynamics + defects[dq.shape[0] :, :] = tau - tau_id + + return DynamicsEvaluation(dxdt, defects) @staticmethod def torque_driven_free_floating_base( @@ -227,9 +252,7 @@ def torque_driven_free_floating_base( dxdt[:n_q, :] = horzcat(*[dq for _ in range(ddq.shape[1])]) dxdt[n_q:, :] = ddq - defects = None - - return DynamicsEvaluation(dxdt, defects) + return DynamicsEvaluation(dxdt, None) @staticmethod def stochastic_torque_driven( diff --git a/bioptim/limits/penalty.py b/bioptim/limits/penalty.py index 621d49177..50c41876a 100644 --- a/bioptim/limits/penalty.py +++ b/bioptim/limits/penalty.py @@ -208,7 +208,6 @@ def stochastic_minimize_expected_feedback_efforts(penalty: PenaltyOption, contro jac_e_fb_x = jacobian(e_fb_mx, controller.states_scaled.cx) - # todo: Charbie remode this function jac_e_fb_x_cx = Function( "jac_e_fb_x", [ diff --git a/bioptim/limits/phase_transition.py b/bioptim/limits/phase_transition.py index 185c685e8..87e29e9dc 100644 --- a/bioptim/limits/phase_transition.py +++ b/bioptim/limits/phase_transition.py @@ -5,10 +5,9 @@ from .multinode_constraint import MultinodeConstraint from .multinode_penalty import MultinodePenalty, MultinodePenaltyFunctions -from .objective_functions import ObjectiveFunction from .path_conditions import Bounds from ..limits.penalty import PenaltyFunctionAbstract, PenaltyController -from ..misc.enums import Node, PenaltyType, InterpolationType, ControlType +from ..misc.enums import Node, PenaltyType, InterpolationType from ..misc.fcn_enum import FcnEnum from ..misc.mapping import BiMapping from ..misc.options import UniquePerPhaseOptionList @@ -50,8 +49,6 @@ def __init__( max_bound: float = 0, **extra_parameters: Any, ): - # TODO: @pariterre: where did phase_post go !? - if not isinstance(transition, PhaseTransitionFcn): custom_function = transition transition = PhaseTransitionFcn.CUSTOM @@ -288,7 +285,6 @@ def impact(transition, controllers: list[PenaltyController, PenaltyController]): ) val = vertcat(val, continuity) - # name = f"PHASE_TRANSITION_{pre.phase_idx % ocp.n_phases}_{post.phase_idx % ocp.n_phases}" return val @staticmethod diff --git a/bioptim/models/protocols/biomodel.py b/bioptim/models/protocols/biomodel.py index e188a5006..fe3265bf7 100644 --- a/bioptim/models/protocols/biomodel.py +++ b/bioptim/models/protocols/biomodel.py @@ -369,7 +369,6 @@ def lagrangian(self) -> Function: def partitioned_forward_dynamics(self, q_u, qdot_u, q_v_init, tau) -> Function: """ - @ipuch: I need help on how to implement this! This is the forward dynamics of the model, but only for the independent joints Parameters diff --git a/bioptim/optimization/optimal_control_program.py b/bioptim/optimization/optimal_control_program.py index c857029f0..f86d6ef4e 100644 --- a/bioptim/optimization/optimal_control_program.py +++ b/bioptim/optimization/optimal_control_program.py @@ -1062,7 +1062,7 @@ def _declare_parameters(self, parameters: ParameterList): if not isinstance(parameters, ParameterList): raise RuntimeError("new_parameter must be a Parameter or a ParameterList") - self.parameters = ParameterContainer(use_sx=(True if self.cx == SX else False)) + self.parameters = ParameterContainer(use_sx=(self.cx == SX)) self.parameters.initialize(parameters) def update_bounds( @@ -1630,7 +1630,7 @@ def define_parameters_phase_time( self.phase_time = phase_time if isinstance(phase_time, (tuple, list)) else [phase_time] - self.dt_parameter = ParameterList(use_sx=(True if self.cx == SX else False)) + self.dt_parameter = ParameterList(use_sx=(self.cx == SX)) for i_phase in range(self.n_phases): if i_phase != self.time_phase_mapping.to_second.map_idx[i_phase]: self.dt_parameter.add_a_copied_element(self.time_phase_mapping.to_second.map_idx[i_phase]) diff --git a/tests/shard6/test_global_stochastic_except_collocation.py b/tests/shard6/test_global_stochastic_except_collocation.py index a3ed1c4a6..a4cffa67d 100644 --- a/tests/shard6/test_global_stochastic_except_collocation.py +++ b/tests/shard6/test_global_stochastic_except_collocation.py @@ -11,6 +11,7 @@ # Integrated values should be handled another way # In the meantime, let's skip this test +# Please note that the test is very sensitive, so approximate values are enough # @pytest.mark.parametrize("use_sx", [True, False]) # def test_arm_reaching_muscle_driven(use_sx): # from bioptim.examples.stochastic_optimal_control import arm_reaching_muscle_driven as ocp_module From dc3b4ccaafb40552539cbd15af93efdb30c2dbb2 Mon Sep 17 00:00:00 2001 From: eve-mac Date: Wed, 30 Oct 2024 10:42:52 -0400 Subject: [PATCH 081/108] fixed the symbolic problem --- bioptim/dynamics/configure_problem.py | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/bioptim/dynamics/configure_problem.py b/bioptim/dynamics/configure_problem.py index aee4f6211..8304bed83 100644 --- a/bioptim/dynamics/configure_problem.py +++ b/bioptim/dynamics/configure_problem.py @@ -1214,15 +1214,17 @@ def configure_soft_contact_function(ocp, nlp): # TODO: this intermediary function is necessary for the tests (probably because really sensitive) # but it should ideally be removed sometime global_soft_contact_force_func = nlp.model.soft_contact_forces()( - nlp.states["q"].cx_start, nlp.states["qdot"].cx_start, nlp.parameters.cx + nlp.states["q"].mapping.to_second.map(nlp.states["q"].cx_start), + nlp.states["qdot"].mapping.to_second.map(nlp.states["qdot"].cx_start), + nlp.parameters.cx, ) nlp.soft_contact_forces_func = Function( "soft_contact_forces_func", [ nlp.time_cx, - nlp.states.cx_start, - nlp.controls.cx_start, - nlp.parameters.cx_start, + nlp.states.scaled.cx_start, + nlp.controls.scaled.cx_start, + nlp.parameters.scaled.cx_start, ], [global_soft_contact_force_func], ["t", "x", "u", "p"], From 31c9e17d8621b1227c55d85b7aa26693c59e55ea Mon Sep 17 00:00:00 2001 From: eve-mac Date: Wed, 30 Oct 2024 10:43:02 -0400 Subject: [PATCH 082/108] blacked --- bioptim/dynamics/dynamics_functions.py | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/bioptim/dynamics/dynamics_functions.py b/bioptim/dynamics/dynamics_functions.py index 10d50897b..6f0330d7d 100644 --- a/bioptim/dynamics/dynamics_functions.py +++ b/bioptim/dynamics/dynamics_functions.py @@ -157,9 +157,7 @@ def torque_driven( if nlp.ode_solver.defects_type == DefectType.IMPLICIT: if not with_contact and fatigue is None: qddot = DynamicsFunctions.get(nlp.states_dot["qddot"], nlp.states_dot.scaled.cx) - tau_id = DynamicsFunctions.inverse_dynamics( - nlp, q, qdot, qddot, with_contact, external_forces - ) + tau_id = DynamicsFunctions.inverse_dynamics(nlp, q, qdot, qddot, with_contact, external_forces) defects = nlp.cx(dq.shape[0] + tau_id.shape[0], tau_id.shape[1]) dq_defects = [] From 207d9d4303e12d2f52e99ae0fda756e5112c0353 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Wed, 30 Oct 2024 19:48:33 -0400 Subject: [PATCH 083/108] fix: multiple force --- bioptim/limits/penalty_helpers.py | 3 ++- bioptim/models/biorbd/biorbd_model.py | 32 ++++++++++++++++++--------- 2 files changed, 23 insertions(+), 12 deletions(-) diff --git a/bioptim/limits/penalty_helpers.py b/bioptim/limits/penalty_helpers.py index 5adacf1e0..c5e55cf98 100644 --- a/bioptim/limits/penalty_helpers.py +++ b/bioptim/limits/penalty_helpers.py @@ -138,7 +138,8 @@ def numerical_timeseries(penalty, index, get_numerical_timeseries: Callable): raise NotImplementedError( "Numerical data timeseries is not implemented for multinode penalties yet." ) - # Note to the developers: We do not think this will raise an error at runtime, but the results will be wrong is cx_start or cx_ens are used in multiple occasions with different values. + # Note to the developers: We do not think this will raise an error at runtime, + # but the results will be wrong is cx_start or cx_end are used in multiple occasions with different values. else: d = get_numerical_timeseries(penalty.phase, node, 0) # cx_start diff --git a/bioptim/models/biorbd/biorbd_model.py b/bioptim/models/biorbd/biorbd_model.py index 162f285d7..109a1e298 100644 --- a/bioptim/models/biorbd/biorbd_model.py +++ b/bioptim/models/biorbd/biorbd_model.py @@ -42,7 +42,9 @@ def __init__( self._friction_coefficients = friction_coefficients # External forces - self.external_force_set = self._set_external_force_set(external_force_set) if external_force_set else None + self.external_force_set = ( + self._set_external_force_set(external_force_set) if external_force_set is not None else None + ) # Declaration of MX variables of the right shape for the creation of CasADi Functions self.q = MX.sym("q_mx", self.nb_q, 1) @@ -55,6 +57,7 @@ def __init__( self.external_forces = MX.sym( "external_forces_mx", external_force_set.nb_external_forces_components if external_force_set else 0, 1 ) + self.biorbd_external_forces_set = self._dispatch_forces() if external_force_set else None # TODO: remove mx (the MX parameters should be created inside the BiorbdModel) self.parameters = parameters.mx if parameters else MX() @@ -67,6 +70,8 @@ def _set_external_force_set(self, external_force_set: ExternalForceSetTimeSeries external_force_set._check_all_string_points_of_application(self.marker_names) external_force_set._bind() + return external_force_set + @property def name(self) -> str: # parse the path and split to get the .bioMod name @@ -433,17 +438,22 @@ def _dispatch_forces(self): # "type of external force": (function to call, number of force components, number of point of application components) bioptim_to_biorbd_map = { - "in_global": (external_force_set.add, 6), - "torque_in_global": (external_force_set.add, 3), - "translational_in_global": (external_force_set.addTranslationalForce, 3), - "in_local": (external_force_set.addInSegmentReferenceFrame, 6), - "torque_in_local": (external_force_set.addInSegmentReferenceFrame, 3), + "in_global": (biorbd_external_forces.add, 6), + "torque_in_global": (biorbd_external_forces.add, 3), + "translational_in_global": ( + lambda segment, force, point_of_application: biorbd_external_forces.addTranslationalForce( + force, segment, point_of_application + ), + 3, + ), + "in_local": (biorbd_external_forces.addInSegmentReferenceFrame, 6), + "torque_in_local": (biorbd_external_forces.addInSegmentReferenceFrame, 3), } symbolic_counter = 0 for attr in bioptim_to_biorbd_map.keys(): function = bioptim_to_biorbd_map[attr][0] - for segment, forces in getattr(self, attr).items(): + for segment, forces in getattr(self.external_force_set, attr).items(): for force in forces: array_point_of_application = isinstance(force["point_of_application"], np.ndarray) @@ -473,7 +483,7 @@ def forward_dynamics(self, with_contact: bool = False) -> Function: biorbd_return = self.model.ForwardDynamicsConstraintsDirect(q_biorbd, qdot_biorbd, tau_biorbd).to_mx() else: biorbd_return = self.model.ForwardDynamicsConstraintsDirect( - q_biorbd, qdot_biorbd, tau_biorbd, self.external_forces + q_biorbd, qdot_biorbd, tau_biorbd, externalForces=self.biorbd_external_forces_set ).to_mx() casadi_fun = Function( "constrained_forward_dynamics", @@ -487,7 +497,7 @@ def forward_dynamics(self, with_contact: bool = False) -> Function: biorbd_return = self.model.ForwardDynamics(q_biorbd, qdot_biorbd, tau_biorbd).to_mx() else: biorbd_return = self.model.ForwardDynamics( - q_biorbd, qdot_biorbd, tau_biorbd, self.external_forces + q_biorbd, qdot_biorbd, tau_biorbd, externalForces=self.biorbd_external_forces_set ).to_mx() casadi_fun = Function( "forward_dynamics", @@ -507,7 +517,7 @@ def inverse_dynamics(self, with_contact: bool = False) -> Function: biorbd_return = self.model.InverseDynamics(q_biorbd, qdot_biorbd, qddot_biorbd).to_mx() else: biorbd_return = self.model.InverseDynamics( - q_biorbd, qdot_biorbd, qddot_biorbd, self.external_force_set + q_biorbd, qdot_biorbd, qddot_biorbd, externalForces=self.biorbd_external_forces_set ).to_mx() casadi_fun = Function( "inverse_dynamics", @@ -529,7 +539,7 @@ def contact_forces_from_constrained_forward_dynamics(self) -> Function: ).to_mx() else: biorbd_return = self.model.ContactForcesFromForwardDynamicsConstraintsDirect( - q_biorbd, qdot_biorbd, tau_biorbd, self.external_force_set + q_biorbd, qdot_biorbd, tau_biorbd, externalForces=self.biorbd_external_forces_set ).to_mx() casadi_fun = Function( "contact_forces_from_constrained_forward_dynamics", From 36729baa29f08469816918069b09d68cdc17c58c Mon Sep 17 00:00:00 2001 From: Ipuch Date: Wed, 30 Oct 2024 19:48:39 -0400 Subject: [PATCH 084/108] docs --- bioptim/interfaces/interface_utils.py | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/bioptim/interfaces/interface_utils.py b/bioptim/interfaces/interface_utils.py index 068b71875..83962aa9f 100644 --- a/bioptim/interfaces/interface_utils.py +++ b/bioptim/interfaces/interface_utils.py @@ -138,15 +138,24 @@ def _shake_tree_for_penalties(ocp, penalties_cx, v, v_bounds, expand): """ Remove the dt in the objectives and constraints if they are constant + Note: Shake tree is a metaphor for saying it makes every unnecessary variables disappear / fall + Parameters ---------- - ocp + ocp: OptimalControlProgram + A reference to the current OptimalControlProgram penalties_cx + all the penalties of the ocp (objective, constraints) v + full vector of variables of the ocp v_bounds + all the bounds of the variables, use to detect constant variables if min == max + expand : bool + expand if possible the penalty but can failed but ignored if there is matrix inversion or newton descent for example Returns ------- + The penalties without extra variables """ dt = [] From 651f2d37831d226daef892d8c4d78f79110dfb32 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Wed, 30 Oct 2024 19:50:35 -0400 Subject: [PATCH 085/108] tests: failed but rolled --- bioptim/models/biorbd/biorbd_model.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bioptim/models/biorbd/biorbd_model.py b/bioptim/models/biorbd/biorbd_model.py index 109a1e298..5d64f2db6 100644 --- a/bioptim/models/biorbd/biorbd_model.py +++ b/bioptim/models/biorbd/biorbd_model.py @@ -497,7 +497,7 @@ def forward_dynamics(self, with_contact: bool = False) -> Function: biorbd_return = self.model.ForwardDynamics(q_biorbd, qdot_biorbd, tau_biorbd).to_mx() else: biorbd_return = self.model.ForwardDynamics( - q_biorbd, qdot_biorbd, tau_biorbd, externalForces=self.biorbd_external_forces_set + q_biorbd, qdot_biorbd, tau_biorbd, self.biorbd_external_forces_set ).to_mx() casadi_fun = Function( "forward_dynamics", From bd2ea77b46545e1939d8e87115c9a628e3fd25b6 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Thu, 31 Oct 2024 10:55:13 -0400 Subject: [PATCH 086/108] fix: segmentation faults, but rolling on a complete test --- .../example_external_forces.py | 77 +++-- bioptim/models/biorbd/biorbd_model.py | 19 +- tests/shard3/test_external_forces.py | 303 +++++++++++------- 3 files changed, 241 insertions(+), 158 deletions(-) diff --git a/bioptim/examples/getting_started/example_external_forces.py b/bioptim/examples/getting_started/example_external_forces.py index 086717a50..32b5fc6f1 100644 --- a/bioptim/examples/getting_started/example_external_forces.py +++ b/bioptim/examples/getting_started/example_external_forces.py @@ -34,10 +34,7 @@ def prepare_ocp( ode_solver: OdeSolverBase = OdeSolver.RK4(), expand_dynamics: bool = True, phase_dynamics: PhaseDynamics = PhaseDynamics.ONE_PER_NODE, - # force_type: ExternalForceType = ExternalForceType.FORCE, - # force_reference_frame: ReferenceFrame = ReferenceFrame.GLOBAL, - use_point_of_applications: bool = False, - # point_of_application_reference_frame: ReferenceFrame = ReferenceFrame.GLOBAL, + external_force_method: str = "translational_force", n_threads: int = 1, use_sx: bool = False, ) -> OptimalControlProgram: @@ -83,45 +80,47 @@ def prepare_ocp( Test_force[2, :] = 5 Test_force[2, 4] = 52 - if use_point_of_applications: - # Seg1_point_of_application = np.zeros((3, n_shooting + 1)) - Seg1_point_of_application = np.zeros((3, n_shooting)) - Seg1_point_of_application[0, :] = 0.05 - Seg1_point_of_application[1, :] = -0.05 - Seg1_point_of_application[2, :] = 0.007 - - # Test_point_of_application = np.zeros((3, n_shooting + 1)) - Test_point_of_application = np.zeros((3, n_shooting)) - Test_point_of_application[0, :] = -0.009 - Test_point_of_application[1, :] = 0.01 - Test_point_of_application[2, :] = -0.01 - else: - Seg1_point_of_application = None - Test_point_of_application = None - # - # external_forces = ExternalForces() - # external_forces.add( - # key="Seg1", # Name of the segment where the external force is applied - # data=Seg1_force, # 3 x (n_shooting_points+1) array - # force_type=force_type, # Type of the external force (ExternalForceType.FORCE) - # force_reference_frame=force_reference_frame, # Reference frame of the external force (ReferenceFrame.GLOBAL) - # point_of_application=Seg1_point_of_application, # Position of the point of application - # point_of_application_reference_frame=point_of_application_reference_frame, # Reference frame of the point of application (ReferenceFrame.GLOBAL) - # ) - # external_forces.add( - # key="Test", # Name of the segment where the external force is applied - # data=Test_force, # 3 x (n_shooting_points+1) array - # force_type=force_type, # Type of the external force (ExternalForceType.FORCE) - # force_reference_frame=force_reference_frame, # Reference frame of the external force (ReferenceFrame.GLOBAL) - # point_of_application=Test_point_of_application, # Position of the point of application - # point_of_application_reference_frame=point_of_application_reference_frame, # Reference frame of the point of application (ReferenceFrame.GLOBAL) - # ) + # Seg1_point_of_application + # if use_point_of_applications: + # Seg1_point_of_application = np.zeros((3, n_shooting + 1)) + Seg1_point_of_application = np.zeros((3, n_shooting)) + Seg1_point_of_application[0, :] = 0.05 + Seg1_point_of_application[1, :] = -0.05 + Seg1_point_of_application[2, :] = 0.007 + + # Test_point_of_application = np.zeros((3, n_shooting + 1)) + Test_point_of_application = np.zeros((3, n_shooting)) + Test_point_of_application[0, :] = -0.009 + Test_point_of_application[1, :] = 0.01 + Test_point_of_application[2, :] = -0.01 + # else: + # Seg1_point_of_application = None + # Test_point_of_application = None external_force_set = ExternalForceSetTimeSeries( nb_frames=n_shooting, ) - external_force_set.add_translational_force("Seg1", Seg1_force, point_of_application=Seg1_point_of_application) - external_force_set.add_translational_force("Test", Test_force, point_of_application=Test_point_of_application) + # Displays all the possible combinations of external forces + if external_force_method == "translational_force": + # not mandatory to specify the point of application + external_force_set.add_translational_force("Seg1", Seg1_force, point_of_application=Seg1_point_of_application) + external_force_set.add_translational_force("Test", Test_force, point_of_application=Test_point_of_application) + elif external_force_method == "in_global": + # not mandatory to specify the point of application + external_force_set.add("Seg1", np.concatenate((Seg1_force, Seg1_force), axis=0)) + external_force_set.add("Test", np.concatenate((Test_force, Test_force), axis=0)) + elif external_force_method == "in_global_torque": + # cannot specify the point of application as it is a pure torque + external_force_set.add_torque("Seg1", Seg1_force) + external_force_set.add_torque("Test", Test_force) + elif external_force_method == "in_segment_torque": + # cannot specify the point of application as it is a pure torque + external_force_set.add_torque_in_segment_frame("Seg1", Seg1_force) + external_force_set.add_torque_in_segment_frame("Test", Test_force) + elif external_force_method == "in_segment": + # not mandatory to specify the point of application + external_force_set.add_in_segment_frame("Seg1", np.concatenate((Seg1_force, Seg1_force), axis=0)) + external_force_set.add_in_segment_frame("Test", np.concatenate((Test_force, Test_force), axis=0)) bio_model = BiorbdModel(biorbd_model_path, external_force_set=external_force_set) diff --git a/bioptim/models/biorbd/biorbd_model.py b/bioptim/models/biorbd/biorbd_model.py index 5d64f2db6..eac2e5bdb 100644 --- a/bioptim/models/biorbd/biorbd_model.py +++ b/bioptim/models/biorbd/biorbd_model.py @@ -439,7 +439,12 @@ def _dispatch_forces(self): # "type of external force": (function to call, number of force components, number of point of application components) bioptim_to_biorbd_map = { "in_global": (biorbd_external_forces.add, 6), - "torque_in_global": (biorbd_external_forces.add, 3), + "torque_in_global": ( + lambda segment, torque, point_of_application: biorbd_external_forces.add( + segment, vertcat(torque, MX([0, 0, 0])) + ), + 3, + ), "translational_in_global": ( lambda segment, force, point_of_application: biorbd_external_forces.addTranslationalForce( force, segment, point_of_application @@ -447,7 +452,12 @@ def _dispatch_forces(self): 3, ), "in_local": (biorbd_external_forces.addInSegmentReferenceFrame, 6), - "torque_in_local": (biorbd_external_forces.addInSegmentReferenceFrame, 3), + "torque_in_local": ( + lambda segment, torque, point_of_application: biorbd_external_forces.addInSegmentReferenceFrame( + segment, vertcat(torque, MX([0, 0, 0])), MX([0, 0, 0]) + ), + 3, + ), } symbolic_counter = 0 @@ -456,6 +466,7 @@ def _dispatch_forces(self): for segment, forces in getattr(self.external_force_set, attr).items(): for force in forces: array_point_of_application = isinstance(force["point_of_application"], np.ndarray) + str_point_of_application = isinstance(force["point_of_application"], str) start = symbolic_counter stop = symbolic_counter + bioptim_to_biorbd_map[attr][1] @@ -463,9 +474,11 @@ def _dispatch_forces(self): if array_point_of_application: point_of_application = self.external_forces[slice(stop, stop + 3)] - else: + elif str_point_of_application: # turn it into a NodeSegment point_of_application = biorbd.NodeSegment(force["point_of_application"]) + else: + point_of_application = None function(segment, self.external_forces[force_slicer], point_of_application) symbolic_counter += stop + 3 if array_point_of_application else stop diff --git a/tests/shard3/test_external_forces.py b/tests/shard3/test_external_forces.py index a3ec836e6..035fe0387 100644 --- a/tests/shard3/test_external_forces.py +++ b/tests/shard3/test_external_forces.py @@ -20,6 +20,81 @@ ) +@pytest.mark.parametrize( + "phase_dynamics", + [ + PhaseDynamics.SHARED_DURING_THE_PHASE, + PhaseDynamics.ONE_PER_NODE, + ], +) +@pytest.mark.parametrize( + "use_sx", + [ + True, + False, + ], +) +@pytest.mark.parametrize( + "method", + [ + "translational_force", + "in_global", + "in_global_torque", + "in_segment_torque", + "in_segment", + ], +) +def test_example_external_forces_ipuch( + phase_dynamics, + use_sx, + method, +): + from bioptim.examples.getting_started import example_external_forces as ocp_module + + bioptim_folder = os.path.dirname(ocp_module.__file__) + + ocp = ocp_module.prepare_ocp( + biorbd_model_path=bioptim_folder + "/models/cube_with_forces.bioMod", + phase_dynamics=phase_dynamics, + external_force_method=method, + use_sx=use_sx, + ) + sol = ocp.solve() + + # Check objective function value + f = np.array(sol.cost) + npt.assert_equal(f.shape, (1, 1)) + + # Check constraints + g = np.array(sol.constraints) + npt.assert_equal(g.shape, (246, 1)) + npt.assert_almost_equal(g, np.zeros((246, 1))) + + # Check some of the results + states = sol.decision_states(to_merge=SolutionMerge.NODES) + controls = sol.decision_controls(to_merge=SolutionMerge.NODES) + q, qdot, tau = states["q"], states["qdot"], controls["tau"] + + # initial and final velocities + npt.assert_almost_equal(qdot[:, 0], np.array([0.0, 0.0, 0.0, 0.0]), decimal=5) + npt.assert_almost_equal(qdot[:, -1], np.array([0.0, 0.0, 0.0, 0.0]), decimal=5) + + npt.assert_almost_equal(f[0, 0], 19847.887805189126) + + # initial and final controls + npt.assert_almost_equal(tau[:, 0], np.array([2.03776708e-09, 1.27132259e01, 2.32230666e-27, 0.0])) + npt.assert_almost_equal(tau[:, 10], np.array([-8.23139024e-10, 1.07110012e01, 5.45884198e-26, 0.0])) + npt.assert_almost_equal(tau[:, 20], np.array([-6.72563418e-10, 8.70877651e00, -2.09699944e-26, 0.0])) + npt.assert_almost_equal(tau[:, -1], np.array([2.03777147e-09, 6.90677426e00, -1.66232727e-25, 0.0])) + + # initial and final position + npt.assert_almost_equal(q[:, 0], np.array([-4.69166539e-15, 6.03679344e-16, -1.43453710e-17, 0.0]), decimal=5) + npt.assert_almost_equal(q[:, -1], np.array([-4.69169398e-15, 2.00000000e00, -1.43453709e-17, 0.0]), decimal=5) + + # detailed cost values + npt.assert_almost_equal(sol.detailed_cost[0]["cost_value_weighted"], f[0, 0]) + + @pytest.mark.parametrize("phase_dynamics", [PhaseDynamics.SHARED_DURING_THE_PHASE, PhaseDynamics.ONE_PER_NODE]) @pytest.mark.parametrize("use_sx", [True, False]) # @pytest.mark.parametrize("force_type", [ExternalForceType.FORCE, ExternalForceType.TORQUE]) @@ -111,125 +186,121 @@ def test_example_external_forces( npt.assert_almost_equal(qdot[:, 0], np.array([0.0, 0.0, 0.0, 0.0]), decimal=5) npt.assert_almost_equal(qdot[:, -1], np.array([0.0, 0.0, 0.0, 0.0]), decimal=5) - if force_type == ExternalForceType.TORQUE: - - npt.assert_almost_equal(f[0, 0], 19847.887805189126) - - # initial and final controls - npt.assert_almost_equal(tau[:, 0], np.array([2.03776708e-09, 1.27132259e01, 2.32230666e-27, 0.0])) - npt.assert_almost_equal(tau[:, 10], np.array([-8.23139024e-10, 1.07110012e01, 5.45884198e-26, 0.0])) - npt.assert_almost_equal(tau[:, 20], np.array([-6.72563418e-10, 8.70877651e00, -2.09699944e-26, 0.0])) - npt.assert_almost_equal(tau[:, -1], np.array([2.03777147e-09, 6.90677426e00, -1.66232727e-25, 0.0])) + # if force_type == ExternalForceType.TORQUE: + # + npt.assert_almost_equal(f[0, 0], 19847.887805189126) - # initial and final position - npt.assert_almost_equal(q[:, 0], np.array([-4.69166539e-15, 6.03679344e-16, -1.43453710e-17, 0.0]), decimal=5) - npt.assert_almost_equal(q[:, -1], np.array([-4.69169398e-15, 2.00000000e00, -1.43453709e-17, 0.0]), decimal=5) + # initial and final controls + npt.assert_almost_equal(tau[:, 0], np.array([2.03776708e-09, 1.27132259e01, 2.32230666e-27, 0.0])) + npt.assert_almost_equal(tau[:, 10], np.array([-8.23139024e-10, 1.07110012e01, 5.45884198e-26, 0.0])) + npt.assert_almost_equal(tau[:, 20], np.array([-6.72563418e-10, 8.70877651e00, -2.09699944e-26, 0.0])) + npt.assert_almost_equal(tau[:, -1], np.array([2.03777147e-09, 6.90677426e00, -1.66232727e-25, 0.0])) - # how the force is stored - if force_reference_frame == ReferenceFrame.LOCAL: - data = ocp.nlp[0].numerical_data_timeseries["forces_in_local"] - else: - data = ocp.nlp[0].numerical_data_timeseries["forces_in_global"] - npt.assert_equal(data.shape, (9, 2, 31)) - npt.assert_almost_equal(data[:, 0, 0], np.array([0.0, 0.0, -2.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])) + # initial and final position + npt.assert_almost_equal(q[:, 0], np.array([-4.69166539e-15, 6.03679344e-16, -1.43453710e-17, 0.0]), decimal=5) + npt.assert_almost_equal(q[:, -1], np.array([-4.69169398e-15, 2.00000000e00, -1.43453709e-17, 0.0]), decimal=5) + # + # # how the force is stored + # if force_reference_frame == ReferenceFrame.LOCAL: + # data = ocp.nlp[0].numerical_data_timeseries["forces_in_local"] + # else: + # data = ocp.nlp[0].numerical_data_timeseries["forces_in_global"] + # npt.assert_equal(data.shape, (9, 2, 31)) + # npt.assert_almost_equal(data[:, 0, 0], np.array([0.0, 0.0, -2.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])) + # + # else: + # if force_reference_frame == ReferenceFrame.GLOBAL: + # if use_point_of_applications: + # if point_of_application_reference_frame == ReferenceFrame.LOCAL: + # npt.assert_almost_equal(f[0, 0], 7067.851604540217) + # + # # initial and final controls + # npt.assert_almost_equal(tau[:, 0], np.array([2.03776698e-09, 6.98419368e00, -8.87085933e-09, 0.0])) + # npt.assert_almost_equal( + # tau[:, 10], np.array([-8.23139073e-10, 6.24337052e00, -9.08634557e-09, 0.0]) + # ) + # npt.assert_almost_equal( + # tau[:, 20], np.array([-6.72563331e-10, 5.50254736e00, -9.36203643e-09, 0.00]) + # ) + # npt.assert_almost_equal(tau[:, -1], np.array([2.03777157e-09, 4.83580652e00, -9.46966399e-09, 0.0])) + # + # # initial and final position + # npt.assert_almost_equal( + # q[:, 0], np.array([-4.69166257e-15, 6.99774238e-16, -1.15797965e00, 0.0]), decimal=5 + # ) + # npt.assert_almost_equal( + # q[:, -1], np.array([-4.69169076e-15, 2.00000000e00, -1.15744926e00, 0.0]), decimal=5 + # ) + # + # # how the force is stored + # data = ocp.nlp[0].numerical_data_timeseries["translational_forces"] + # npt.assert_equal(data.shape, (6, 2, 31)) + # npt.assert_almost_equal(data[:, 0, 0], np.array([0.0, 0.0, -2.0, 0.05, -0.05, 0.007])) - else: - if force_reference_frame == ReferenceFrame.GLOBAL: - if use_point_of_applications: - if point_of_application_reference_frame == ReferenceFrame.LOCAL: - npt.assert_almost_equal(f[0, 0], 7067.851604540217) - - # initial and final controls - npt.assert_almost_equal(tau[:, 0], np.array([2.03776698e-09, 6.98419368e00, -8.87085933e-09, 0.0])) - npt.assert_almost_equal( - tau[:, 10], np.array([-8.23139073e-10, 6.24337052e00, -9.08634557e-09, 0.0]) - ) - npt.assert_almost_equal( - tau[:, 20], np.array([-6.72563331e-10, 5.50254736e00, -9.36203643e-09, 0.00]) - ) - npt.assert_almost_equal(tau[:, -1], np.array([2.03777157e-09, 4.83580652e00, -9.46966399e-09, 0.0])) - - # initial and final position - npt.assert_almost_equal( - q[:, 0], np.array([-4.69166257e-15, 6.99774238e-16, -1.15797965e00, 0.0]), decimal=5 - ) - npt.assert_almost_equal( - q[:, -1], np.array([-4.69169076e-15, 2.00000000e00, -1.15744926e00, 0.0]), decimal=5 - ) - - # how the force is stored - data = ocp.nlp[0].numerical_data_timeseries["translational_forces"] - npt.assert_equal(data.shape, (6, 2, 31)) - npt.assert_almost_equal(data[:, 0, 0], np.array([0.0, 0.0, -2.0, 0.05, -0.05, 0.007])) - - else: - npt.assert_almost_equal(f[0, 0], 7067.851604540217) - - # initial and final controls - npt.assert_almost_equal(tau[:, 0], np.array([1.53525114e-09, 6.98419368e00, -4.06669623e-10, 0.0])) - npt.assert_almost_equal(tau[:, 10], np.array([-6.45031008e-10, 6.24337052e00, -4.06669624e-10, 0.0])) - npt.assert_almost_equal(tau[:, 20], np.array([-5.51259131e-10, 5.50254736e00, -4.06669632e-10, 0.0])) - npt.assert_almost_equal(tau[:, -1], np.array([1.64434894e-09, 4.83580652e00, -4.06669638e-10, 0.0])) - - # initial and final position - npt.assert_almost_equal( - q[:, 0], np.array([-3.41506199e-15, 6.99773953e-16, -4.11684795e-12, 0.0]), decimal=5 - ) - npt.assert_almost_equal( - q[:, -1], np.array([-3.84536375e-15, 2.00000000e00, 6.27099006e-11, 0.0]), decimal=5 - ) - - # how the force is stored - data = ocp.nlp[0].numerical_data_timeseries["forces_in_global"] - npt.assert_equal(data.shape, (9, 2, 31)) - npt.assert_almost_equal(data[:, 0, 0], np.array([0.0, 0.0, 0.0, 0.0, 0.0, -2.0, 0.0, 0.0, 0.0])) - - else: - if use_point_of_applications: - if point_of_application_reference_frame == ReferenceFrame.LOCAL: - npt.assert_almost_equal(f[0, 0], 7076.043800572127) - - # initial and final controls - npt.assert_almost_equal(tau[:, 0], np.array([0.01922272, 6.98428645, -0.28099094, 0.0])) - npt.assert_almost_equal(tau[:, 10], np.array([4.40289150e-03, 6.24343465e00, -2.16557082e-01, 0.0])) - npt.assert_almost_equal(tau[:, 20], np.array([-0.01041693, 5.50258285, -0.14650525, 0.0])) - npt.assert_almost_equal(tau[:, -1], np.array([-0.02375477, 4.83581623, -0.12679319, 0.0])) - - # initial and final position - npt.assert_almost_equal( - q[:, 0], np.array([-4.65561476e-15, 7.00215056e-16, -1.34683990e-03, 0.0]), decimal=5 - ) - npt.assert_almost_equal( - q[:, -1], np.array([-4.76138727e-15, 2.00000000e00, 2.00652560e-03, 0.0]), decimal=5 - ) - - # how the force is stored - data = ocp.nlp[0].numerical_data_timeseries["forces_in_local"] - npt.assert_equal(data.shape, (9, 2, 31)) - npt.assert_almost_equal( - data[:, 0, 0], np.array([0.0, 0.0, 0.0, 0.0, 0.0, -2.0, 0.05, -0.05, 0.007]) - ) - else: - npt.assert_almost_equal(f[0, 0], 7067.851604540217) - - # initial and final controls - npt.assert_almost_equal(tau[:, 0], np.array([1.92280974e-09, 6.98419368e00, -4.00749051e-10, 0.0])) - npt.assert_almost_equal(tau[:, 10], np.array([-8.87099972e-10, 6.24337052e00, 2.22483147e-10, 0.0])) - npt.assert_almost_equal(tau[:, 20], np.array([-6.85527926e-10, 5.50254736e00, 1.70072550e-10, 0.0])) - npt.assert_almost_equal(tau[:, -1], np.array([2.07070380e-09, 4.83580652e00, -3.91202705e-10, 0.0])) - - # initial and final position - npt.assert_almost_equal( - q[:, 0], np.array([-4.64041726e-15, 6.99774094e-16, 4.75924448e-11, 0.0]), decimal=5 - ) - npt.assert_almost_equal( - q[:, -1], np.array([-4.74298258e-15, 2.00000000e00, 3.79354612e-11, 0.0]), decimal=5 - ) - - # how the force is stored - data = ocp.nlp[0].numerical_data_timeseries["forces_in_local"] - npt.assert_equal(data.shape, (9, 2, 31)) - npt.assert_almost_equal(data[:, 0, 0], np.array([0.0, 0.0, 0.0, 0.0, 0.0, -2.0, 0.0, 0.0, 0.0])) + # else: + # npt.assert_almost_equal(f[0, 0], 7067.851604540217) + # + # # initial and final controls + # npt.assert_almost_equal(tau[:, 0], np.array([1.53525114e-09, 6.98419368e00, -4.06669623e-10, 0.0])) + # npt.assert_almost_equal(tau[:, 10], np.array([-6.45031008e-10, 6.24337052e00, -4.06669624e-10, 0.0])) + # npt.assert_almost_equal(tau[:, 20], np.array([-5.51259131e-10, 5.50254736e00, -4.06669632e-10, 0.0])) + # npt.assert_almost_equal(tau[:, -1], np.array([1.64434894e-09, 4.83580652e00, -4.06669638e-10, 0.0])) + # + # # initial and final position + # npt.assert_almost_equal(q[:, 0], np.array([-3.41506199e-15, 6.99773953e-16, -4.11684795e-12, 0.0]), decimal=5) + # npt.assert_almost_equal(q[:, -1], np.array([-3.84536375e-15, 2.00000000e00, 6.27099006e-11, 0.0]), decimal=5) + # + # # how the force is stored + # data = ocp.nlp[0].numerical_data_timeseries["forces_in_global"] + # npt.assert_equal(data.shape, (9, 2, 31)) + # npt.assert_almost_equal(data[:, 0, 0], np.array([0.0, 0.0, 0.0, 0.0, 0.0, -2.0, 0.0, 0.0, 0.0])) + + # else: + # if use_point_of_applications: + # if point_of_application_reference_frame == ReferenceFrame.LOCAL: + # npt.assert_almost_equal(f[0, 0], 7076.043800572127) + # + # # initial and final controls + # npt.assert_almost_equal(tau[:, 0], np.array([0.01922272, 6.98428645, -0.28099094, 0.0])) + # npt.assert_almost_equal(tau[:, 10], np.array([4.40289150e-03, 6.24343465e00, -2.16557082e-01, 0.0])) + # npt.assert_almost_equal(tau[:, 20], np.array([-0.01041693, 5.50258285, -0.14650525, 0.0])) + # npt.assert_almost_equal(tau[:, -1], np.array([-0.02375477, 4.83581623, -0.12679319, 0.0])) + # + # # initial and final position + # npt.assert_almost_equal( + # q[:, 0], np.array([-4.65561476e-15, 7.00215056e-16, -1.34683990e-03, 0.0]), decimal=5 + # ) + # npt.assert_almost_equal( + # q[:, -1], np.array([-4.76138727e-15, 2.00000000e00, 2.00652560e-03, 0.0]), decimal=5 + # ) + # + # # how the force is stored + # data = ocp.nlp[0].numerical_data_timeseries["forces_in_local"] + # npt.assert_equal(data.shape, (9, 2, 31)) + # npt.assert_almost_equal( + # data[:, 0, 0], np.array([0.0, 0.0, 0.0, 0.0, 0.0, -2.0, 0.05, -0.05, 0.007]) + # ) + # else: + # npt.assert_almost_equal(f[0, 0], 7067.851604540217) + # + # # initial and final controls + # npt.assert_almost_equal(tau[:, 0], np.array([1.92280974e-09, 6.98419368e00, -4.00749051e-10, 0.0])) + # npt.assert_almost_equal(tau[:, 10], np.array([-8.87099972e-10, 6.24337052e00, 2.22483147e-10, 0.0])) + # npt.assert_almost_equal(tau[:, 20], np.array([-6.85527926e-10, 5.50254736e00, 1.70072550e-10, 0.0])) + # npt.assert_almost_equal(tau[:, -1], np.array([2.07070380e-09, 4.83580652e00, -3.91202705e-10, 0.0])) + # + # # initial and final position + # npt.assert_almost_equal( + # q[:, 0], np.array([-4.64041726e-15, 6.99774094e-16, 4.75924448e-11, 0.0]), decimal=5 + # ) + # npt.assert_almost_equal( + # q[:, -1], np.array([-4.74298258e-15, 2.00000000e00, 3.79354612e-11, 0.0]), decimal=5 + # ) + # + # # how the force is stored + # data = ocp.nlp[0].numerical_data_timeseries["forces_in_local"] + # npt.assert_equal(data.shape, (9, 2, 31)) + # npt.assert_almost_equal(data[:, 0, 0], np.array([0.0, 0.0, 0.0, 0.0, 0.0, -2.0, 0.0, 0.0, 0.0])) # detailed cost values npt.assert_almost_equal(sol.detailed_cost[0]["cost_value_weighted"], f[0, 0]) From 9d320bc6fb5400198750de2b4666989d94fd65fa Mon Sep 17 00:00:00 2001 From: Ipuch Date: Thu, 31 Oct 2024 11:27:51 -0400 Subject: [PATCH 087/108] tests : extra_test for the class itself --- bioptim/models/biorbd/biorbd_model.py | 6 +- tests/shard3/test_external_force_class.py | 193 ++++++++++++++++++++++ 2 files changed, 196 insertions(+), 3 deletions(-) create mode 100644 tests/shard3/test_external_force_class.py diff --git a/bioptim/models/biorbd/biorbd_model.py b/bioptim/models/biorbd/biorbd_model.py index eac2e5bdb..eb742b0a4 100644 --- a/bioptim/models/biorbd/biorbd_model.py +++ b/bioptim/models/biorbd/biorbd_model.py @@ -463,8 +463,8 @@ def _dispatch_forces(self): symbolic_counter = 0 for attr in bioptim_to_biorbd_map.keys(): function = bioptim_to_biorbd_map[attr][0] - for segment, forces in getattr(self.external_force_set, attr).items(): - for force in forces: + for segment, forces_on_segment in getattr(self.external_force_set, attr).items(): + for force in forces_on_segment: array_point_of_application = isinstance(force["point_of_application"], np.ndarray) str_point_of_application = isinstance(force["point_of_application"], str) @@ -475,7 +475,7 @@ def _dispatch_forces(self): if array_point_of_application: point_of_application = self.external_forces[slice(stop, stop + 3)] elif str_point_of_application: - # turn it into a NodeSegment + # turn it into a NodeSegment, might not work yet. point_of_application = biorbd.NodeSegment(force["point_of_application"]) else: point_of_application = None diff --git a/tests/shard3/test_external_force_class.py b/tests/shard3/test_external_force_class.py new file mode 100644 index 000000000..6792fe05b --- /dev/null +++ b/tests/shard3/test_external_force_class.py @@ -0,0 +1,193 @@ +import os +import re + +import numpy as np +import pytest + +from bioptim import ExternalForceSetTimeSeries, BiorbdModel + + +# Fixture for creating a standard ExternalForceSetTimeSeries instance +@pytest.fixture +def external_forces(): + return ExternalForceSetTimeSeries(nb_frames=10) + + +# Fixture for creating a numpy array of forces +@pytest.fixture +def force_array(): + return np.random.rand(6, 10) + + +# Fixture for creating a numpy array of torques +@pytest.fixture +def torque_array(): + return np.random.rand(3, 10) + + +def test_initialization(external_forces): + """Test the basic initialization of ExternalForceSetTimeSeries.""" + assert external_forces.nb_frames == 10 + assert external_forces._can_be_modified is True + assert external_forces.nb_external_forces == 0 + assert external_forces.nb_external_forces_components == 0 + + +def test_add_global_force(external_forces, force_array): + """Test adding global forces to a segment.""" + segment_name = "segment1" + external_forces.add(segment_name, force_array) + + assert len(external_forces.in_global[segment_name]) == 1 + assert np.array_equal(external_forces.in_global[segment_name][0]["values"], force_array) + + +def test_add_global_force_invalid_shape(external_forces): + """Test adding global forces with incorrect shape raises an error.""" + segment_name = "segment1" + invalid_array = np.random.rand(5, 10) # Wrong number of rows + + with pytest.raises(ValueError, match="External forces must have 6 rows"): + external_forces.add(segment_name, invalid_array) + + +def test_add_global_force_wrong_frame_count(external_forces): + """Test adding global forces with incorrect frame count raises an error.""" + segment_name = "segment1" + wrong_frame_array = np.random.rand(6, 5) # Wrong number of frames + + with pytest.raises(ValueError, match="External forces must have the same number of columns"): + external_forces.add(segment_name, wrong_frame_array) + + +def test_add_torque(external_forces, torque_array): + """Test adding global torques to a segment.""" + segment_name = "segment1" + external_forces.add_torque(segment_name, torque_array) + + assert len(external_forces.torque_in_global[segment_name]) == 1 + assert np.array_equal(external_forces.torque_in_global[segment_name][0]["values"], torque_array) + + +def test_add_torque_invalid_shape(external_forces): + """Test adding torques with incorrect shape raises an error.""" + segment_name = "segment1" + invalid_array = np.random.rand(4, 10) # Wrong number of rows + + with pytest.raises(ValueError, match="External torques must have 3 rows"): + external_forces.add_torque(segment_name, invalid_array) + + +def test_point_of_application(external_forces, force_array): + """Test adding forces with custom point of application.""" + segment_name = "segment1" + point_of_application = np.random.rand(3, 10) + external_forces.add(segment_name, force_array, point_of_application) + + added_force = external_forces.in_global[segment_name][0] + assert np.array_equal(added_force["point_of_application"], point_of_application) + + +def test_bind_prevents_modification(external_forces, force_array): + """Test that binding prevents further modifications.""" + segment_name = "segment1" + external_forces.add(segment_name, force_array) + external_forces._bind() + + with pytest.raises(RuntimeError, match="External forces have been binded"): + external_forces.add(segment_name, force_array) + + +def test_external_forces_components_calculation(external_forces): + """Test the calculation of external forces components.""" + segment1, segment2 = "segment1", "segment2" + + # Add various types of forces + external_forces.add(segment1, np.random.rand(6, 10)) + external_forces.add_torque(segment2, np.random.rand(3, 10)) + external_forces.add_translational_force(segment1, np.random.rand(3, 10)) + + # The actual calculation depends on implementation details + assert external_forces.nb_external_forces_components == 9 + 6 + 6 + assert external_forces.nb_external_forces == 3 + + +def test_to_numerical_time_series(external_forces, force_array): + """Test conversion to numerical time series.""" + segment_name = "segment1" + external_forces.add(segment_name, force_array) + + numerical_series = external_forces.to_numerical_time_series() + + assert numerical_series.shape == (9, 1, 11) # Depends on implementation + assert np.array_equal(numerical_series[0:6, 0, :-1], force_array) + + +def test_multiple_force_types(external_forces): + """Test adding multiple types of forces to the same segment.""" + segment_name = "segment1" + + external_forces.add(segment_name, np.random.rand(6, 10)) + external_forces.add_torque(segment_name, np.random.rand(3, 10)) + external_forces.add_translational_force(segment_name, np.random.rand(3, 10)) + external_forces.add_in_segment_frame(segment_name, np.random.rand(6, 10)) + external_forces.add_torque_in_segment_frame(segment_name, np.random.rand(3, 10)) + + assert external_forces.nb_external_forces == 5 + + +def test_fail_within_biomod(external_forces): + """Test inserting the external forces in a model.""" + from bioptim.examples.getting_started import example_external_forces as ocp_module + + bioptim_folder = os.path.dirname(ocp_module.__file__) + + invalid_segment_name = "segment1" + force_array = np.random.rand(6, 10) + torque_array = np.random.rand(3, 10) + point_of_application = np.random.rand(3, 10) + + external_forces.add(invalid_segment_name, force_array, point_of_application) + external_forces.add_torque(invalid_segment_name, torque_array) + + # Define a model with valid segment names + valid_segment_names = ("Seg1", "ground", "Test") + + # Check that the ValueError is raised with the correct message + with pytest.raises( + ValueError, + match=re.escape( + f"Segments ['{invalid_segment_name}', '{invalid_segment_name}'] " + f"specified in the external forces are not in the model." + f" Available segments are {valid_segment_names}." + ), + ): + BiorbdModel( + f"{bioptim_folder}/models/cube_with_forces.bioMod", + external_force_set=external_forces, + ) + + +def test_success_within_biomod(external_forces): + """Test inserting the external forces in a model.""" + from bioptim.examples.getting_started import example_external_forces as ocp_module + + bioptim_folder = os.path.dirname(ocp_module.__file__) + + force_array = np.random.rand(6, 10) + torque_array = np.random.rand(3, 10) + point_of_application = np.random.rand(3, 10) + + external_forces.add("Seg1", force_array, point_of_application) + external_forces.add_torque("Test", torque_array) + + model = BiorbdModel( + f"{bioptim_folder}/models/cube_with_forces.bioMod", + external_force_set=external_forces, + ) + + assert model.external_forces.shape == (15, 1) + assert model.biorbd_external_forces_set is not None + + with pytest.raises(RuntimeError, match="External forces have been binded and cannot be modified anymore."): + external_forces.add("Seg1", force_array, point_of_application) From 3efdc4803013ec6136c62d8d39456b9926919097 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Thu, 31 Oct 2024 12:08:25 -0400 Subject: [PATCH 088/108] retrieve the force mx otherwise, sothiing is applied. --- bioptim/optimization/non_linear_program.py | 67 ++++++++++++---------- 1 file changed, 38 insertions(+), 29 deletions(-) diff --git a/bioptim/optimization/non_linear_program.py b/bioptim/optimization/non_linear_program.py index 15584e06b..897e5236d 100644 --- a/bioptim/optimization/non_linear_program.py +++ b/bioptim/optimization/non_linear_program.py @@ -138,7 +138,6 @@ def __init__(self, phase_dynamics: PhaseDynamics, use_sx: bool): self.extra_dynamics_func: list = [] self.implicit_dynamics_func = None self.dynamics_type = None - self.numerical_timeseries = None self.g = [] self.g_internal = [] self.g_implicit = [] @@ -182,6 +181,7 @@ def __init__(self, phase_dynamics: PhaseDynamics, use_sx: bool): self.states_dot = OptimizationVariableContainer(self.phase_dynamics) self.controls = OptimizationVariableContainer(self.phase_dynamics) self.numerical_data_timeseries = OptimizationVariableContainer(self.phase_dynamics) + self.numerical_timeseries = None # parameters is currently a clone of ocp.parameters, but should hold phase parameters from ..optimization.parameters import ParameterContainer @@ -454,35 +454,44 @@ def get_var_from_states_or_controls( def get_external_forces( self, states: MX.sym, controls: MX.sym, algebraic_states: MX.sym, numerical_timeseries: MX.sym ): - # si c'est numeric c'est numerical nana sinon c'est dans les etats et tout ça - - def retrieve_forces(name: str, external_forces: MX): - if name in self.states: - external_forces = vertcat(external_forces, DynamicsFunctions.get(self.states[name], states)) - if name in self.controls: - external_forces = vertcat(external_forces, DynamicsFunctions.get(self.controls[name], controls)) - if name in self.algebraic_states: - external_forces = vertcat( - external_forces, DynamicsFunctions.get(self.algebraic_states[name], algebraic_states) - ) - if self.numerical_timeseries is not None: - component_numerical_timeseries = 0 - for key in self.numerical_timeseries.keys(): - if name in key: - component_numerical_timeseries += 1 - if component_numerical_timeseries > 0: - for i_component in range(component_numerical_timeseries): - external_forces = vertcat( - external_forces, - DynamicsFunctions.get( - self.numerical_timeseries[f"{name}_{i_component}"], numerical_timeseries - ), - ) - return external_forces external_forces = self.cx(0, 1) - external_forces = retrieve_forces("forces_in_global", external_forces) - external_forces = retrieve_forces("forces_in_local", external_forces) - external_forces = retrieve_forces("translational_forces", external_forces) + external_forces = self.retrieve_forces( + "external_forces", external_forces, states, controls, algebraic_states, numerical_timeseries + ) + + return external_forces + def retrieve_forces( + self, + name: str, + external_forces: MX, + states: MX.sym, + controls: MX.sym, + algebraic_states: MX.sym, + numerical_timeseries: MX.sym, + ): + """ + This function retrieves the external forces whether they are in + states, controls, algebraic_states or numerical_timeseries + """ + if name in self.states: + external_forces = vertcat(external_forces, DynamicsFunctions.get(self.states[name], states)) + if name in self.controls: + external_forces = vertcat(external_forces, DynamicsFunctions.get(self.controls[name], controls)) + if name in self.algebraic_states: + external_forces = vertcat( + external_forces, DynamicsFunctions.get(self.algebraic_states[name], algebraic_states) + ) + if self.numerical_timeseries is not None: + component_numerical_timeseries = 0 + for key in self.numerical_timeseries.keys(): + if name in key: + component_numerical_timeseries += 1 + if component_numerical_timeseries > 0: + for i_component in range(component_numerical_timeseries): + external_forces = vertcat( + external_forces, + DynamicsFunctions.get(self.numerical_timeseries[f"{name}_{i_component}"], numerical_timeseries), + ) return external_forces From cd2ca845340e6c0017bd46c7b7d7bc05047530b2 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Thu, 31 Oct 2024 13:56:37 -0400 Subject: [PATCH 089/108] test: delete test but not values hell yeah ! --- tests/shard3/test_external_forces.py | 251 ++++----------------------- 1 file changed, 32 insertions(+), 219 deletions(-) diff --git a/tests/shard3/test_external_forces.py b/tests/shard3/test_external_forces.py index 035fe0387..ef939fe98 100644 --- a/tests/shard3/test_external_forces.py +++ b/tests/shard3/test_external_forces.py @@ -44,7 +44,7 @@ "in_segment", ], ) -def test_example_external_forces_ipuch( +def test_example_external_forces( phase_dynamics, use_sx, method, @@ -61,11 +61,10 @@ def test_example_external_forces_ipuch( ) sol = ocp.solve() - # Check objective function value f = np.array(sol.cost) npt.assert_equal(f.shape, (1, 1)) + npt.assert_almost_equal(sol.detailed_cost[0]["cost_value_weighted"], f[0, 0]) - # Check constraints g = np.array(sol.constraints) npt.assert_equal(g.shape, (246, 1)) npt.assert_almost_equal(g, np.zeros((246, 1))) @@ -75,235 +74,49 @@ def test_example_external_forces_ipuch( controls = sol.decision_controls(to_merge=SolutionMerge.NODES) q, qdot, tau = states["q"], states["qdot"], controls["tau"] - # initial and final velocities npt.assert_almost_equal(qdot[:, 0], np.array([0.0, 0.0, 0.0, 0.0]), decimal=5) npt.assert_almost_equal(qdot[:, -1], np.array([0.0, 0.0, 0.0, 0.0]), decimal=5) - npt.assert_almost_equal(f[0, 0], 19847.887805189126) + if method in ["in_global_torque", "in_segment_torque"]: + # initial and final velocities + npt.assert_almost_equal(qdot[:, 0], np.array([0.0, 0.0, 0.0, 0.0]), decimal=5) + npt.assert_almost_equal(qdot[:, -1], np.array([0.0, 0.0, 0.0, 0.0]), decimal=5) - # initial and final controls - npt.assert_almost_equal(tau[:, 0], np.array([2.03776708e-09, 1.27132259e01, 2.32230666e-27, 0.0])) - npt.assert_almost_equal(tau[:, 10], np.array([-8.23139024e-10, 1.07110012e01, 5.45884198e-26, 0.0])) - npt.assert_almost_equal(tau[:, 20], np.array([-6.72563418e-10, 8.70877651e00, -2.09699944e-26, 0.0])) - npt.assert_almost_equal(tau[:, -1], np.array([2.03777147e-09, 6.90677426e00, -1.66232727e-25, 0.0])) + npt.assert_almost_equal(f[0, 0], 19847.887805189126) - # initial and final position - npt.assert_almost_equal(q[:, 0], np.array([-4.69166539e-15, 6.03679344e-16, -1.43453710e-17, 0.0]), decimal=5) - npt.assert_almost_equal(q[:, -1], np.array([-4.69169398e-15, 2.00000000e00, -1.43453709e-17, 0.0]), decimal=5) + # initial and final controls + npt.assert_almost_equal(tau[:, 0], np.array([2.03776708e-09, 1.27132259e01, 2.32230666e-27, 0.0])) + npt.assert_almost_equal(tau[:, 10], np.array([-8.23139024e-10, 1.07110012e01, 5.45884198e-26, 0.0])) + npt.assert_almost_equal(tau[:, 20], np.array([-6.72563418e-10, 8.70877651e00, -2.09699944e-26, 0.0])) + npt.assert_almost_equal(tau[:, -1], np.array([2.03777147e-09, 6.90677426e00, -1.66232727e-25, 0.0])) - # detailed cost values - npt.assert_almost_equal(sol.detailed_cost[0]["cost_value_weighted"], f[0, 0]) + # initial and final position + npt.assert_almost_equal(q[:, 0], np.array([-4.69166539e-15, 6.03679344e-16, -1.43453710e-17, 0.0]), decimal=5) + npt.assert_almost_equal(q[:, -1], np.array([-4.69169398e-15, 2.00000000e00, -1.43453709e-17, 0.0]), decimal=5) + if method in ["translational_force", "in_global", "in_segment"]: -@pytest.mark.parametrize("phase_dynamics", [PhaseDynamics.SHARED_DURING_THE_PHASE, PhaseDynamics.ONE_PER_NODE]) -@pytest.mark.parametrize("use_sx", [True, False]) -# @pytest.mark.parametrize("force_type", [ExternalForceType.FORCE, ExternalForceType.TORQUE]) -# @pytest.mark.parametrize("force_reference_frame", [ReferenceFrame.GLOBAL, ReferenceFrame.LOCAL]) -# @pytest.mark.parametrize("use_point_of_applications", [True, False]) -# @pytest.mark.parametrize("point_of_application_reference_frame", [ReferenceFrame.GLOBAL, ReferenceFrame.LOCAL]) -def test_example_external_forces( - phase_dynamics, - use_sx, - # force_type, - # force_reference_frame, - # use_point_of_applications, - # point_of_application_reference_frame, -): - from bioptim.examples.getting_started import example_external_forces as ocp_module + npt.assert_almost_equal(f[0, 0], 7067.851604540217) - bioptim_folder = os.path.dirname(ocp_module.__file__) + # initial and final controls + npt.assert_almost_equal(tau[:, 0], np.array([2.03776698e-09, 6.98419368e00, -8.87085933e-09, 0.0])) + npt.assert_almost_equal(tau[:, 10], np.array([-8.23139073e-10, 6.24337052e00, -9.08634557e-09, 0.0])) + npt.assert_almost_equal(tau[:, 20], np.array([-6.72563331e-10, 5.50254736e00, -9.36203643e-09, 0.00])) + npt.assert_almost_equal(tau[:, -1], np.array([2.03777157e-09, 4.83580652e00, -9.46966399e-09, 0.0])) - # if use_point_of_applications == False: - # point_of_application_reference_frame = None - # - # if ( - # force_type == ExternalForceType.FORCE - # and force_reference_frame == ReferenceFrame.GLOBAL - # and point_of_application_reference_frame == ReferenceFrame.GLOBAL - # ): - # # This combination is already tested in test_global_getting_started.py - # return - # - # # Errors for some combinations - # if force_type == ExternalForceType.TORQUE and use_point_of_applications: - # with pytest.raises( - # ValueError, - # match="Point of application cannot be used with ExternalForceType.TORQUE", - # ): - # ocp = ocp_module.prepare_ocp( - # biorbd_model_path=bioptim_folder + "/models/cube_with_forces.bioMod", - # phase_dynamics=phase_dynamics, - # force_type=force_type, - # force_reference_frame=force_reference_frame, - # use_point_of_applications=use_point_of_applications, - # point_of_application_reference_frame=point_of_application_reference_frame, - # use_sx=use_sx, - # ) - # return - # - # if force_reference_frame == ReferenceFrame.LOCAL and point_of_application_reference_frame == ReferenceFrame.GLOBAL: - # with pytest.raises( - # NotImplementedError, - # match="External forces in local reference frame cannot have a point of application in the global reference frame yet", - # ): - # ocp = ocp_module.prepare_ocp( - # biorbd_model_path=bioptim_folder + "/models/cube_with_forces.bioMod", - # phase_dynamics=phase_dynamics, - # force_type=force_type, - # force_reference_frame=force_reference_frame, - # use_point_of_applications=use_point_of_applications, - # point_of_application_reference_frame=point_of_application_reference_frame, - # use_sx=use_sx, - # ) - # return + # initial and final position - ocp = ocp_module.prepare_ocp( - biorbd_model_path=bioptim_folder + "/models/cube_with_forces.bioMod", - phase_dynamics=phase_dynamics, - # force_type=force_type, - # force_reference_frame=force_reference_frame, - # use_point_of_applications=use_point_of_applications, - # point_of_application_reference_frame=point_of_application_reference_frame, - use_sx=use_sx, - ) - sol = ocp.solve() + if method == "translational_force": + npt.assert_almost_equal(q[:, 0], np.array([-4.69166257e-15, 6.99774238e-16, -1.15797965e00, 0.0]), decimal=5) + npt.assert_almost_equal(q[:, -1], np.array([-4.69169076e-15, 2.00000000e00, -1.15744926e00, 0.0]), decimal=5) - # Check objective function value - f = np.array(sol.cost) - npt.assert_equal(f.shape, (1, 1)) - - # Check constraints - g = np.array(sol.constraints) - npt.assert_equal(g.shape, (246, 1)) - npt.assert_almost_equal(g, np.zeros((246, 1))) - - # Check some of the results - states = sol.decision_states(to_merge=SolutionMerge.NODES) - controls = sol.decision_controls(to_merge=SolutionMerge.NODES) - q, qdot, tau = states["q"], states["qdot"], controls["tau"] + if method == "in_global": + npt.assert_almost_equal(q[:, 0], np.array([-3.41506199e-15, 6.99773953e-16, -4.11684795e-12, 0.0]), decimal=5) + npt.assert_almost_equal(q[:, -1], np.array([-3.84536375e-15, 2.00000000e00, 6.27099006e-11, 0.0]), decimal=5) - # initial and final velocities - npt.assert_almost_equal(qdot[:, 0], np.array([0.0, 0.0, 0.0, 0.0]), decimal=5) - npt.assert_almost_equal(qdot[:, -1], np.array([0.0, 0.0, 0.0, 0.0]), decimal=5) - - # if force_type == ExternalForceType.TORQUE: - # - npt.assert_almost_equal(f[0, 0], 19847.887805189126) - - # initial and final controls - npt.assert_almost_equal(tau[:, 0], np.array([2.03776708e-09, 1.27132259e01, 2.32230666e-27, 0.0])) - npt.assert_almost_equal(tau[:, 10], np.array([-8.23139024e-10, 1.07110012e01, 5.45884198e-26, 0.0])) - npt.assert_almost_equal(tau[:, 20], np.array([-6.72563418e-10, 8.70877651e00, -2.09699944e-26, 0.0])) - npt.assert_almost_equal(tau[:, -1], np.array([2.03777147e-09, 6.90677426e00, -1.66232727e-25, 0.0])) - - # initial and final position - npt.assert_almost_equal(q[:, 0], np.array([-4.69166539e-15, 6.03679344e-16, -1.43453710e-17, 0.0]), decimal=5) - npt.assert_almost_equal(q[:, -1], np.array([-4.69169398e-15, 2.00000000e00, -1.43453709e-17, 0.0]), decimal=5) - # - # # how the force is stored - # if force_reference_frame == ReferenceFrame.LOCAL: - # data = ocp.nlp[0].numerical_data_timeseries["forces_in_local"] - # else: - # data = ocp.nlp[0].numerical_data_timeseries["forces_in_global"] - # npt.assert_equal(data.shape, (9, 2, 31)) - # npt.assert_almost_equal(data[:, 0, 0], np.array([0.0, 0.0, -2.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])) - # - # else: - # if force_reference_frame == ReferenceFrame.GLOBAL: - # if use_point_of_applications: - # if point_of_application_reference_frame == ReferenceFrame.LOCAL: - # npt.assert_almost_equal(f[0, 0], 7067.851604540217) - # - # # initial and final controls - # npt.assert_almost_equal(tau[:, 0], np.array([2.03776698e-09, 6.98419368e00, -8.87085933e-09, 0.0])) - # npt.assert_almost_equal( - # tau[:, 10], np.array([-8.23139073e-10, 6.24337052e00, -9.08634557e-09, 0.0]) - # ) - # npt.assert_almost_equal( - # tau[:, 20], np.array([-6.72563331e-10, 5.50254736e00, -9.36203643e-09, 0.00]) - # ) - # npt.assert_almost_equal(tau[:, -1], np.array([2.03777157e-09, 4.83580652e00, -9.46966399e-09, 0.0])) - # - # # initial and final position - # npt.assert_almost_equal( - # q[:, 0], np.array([-4.69166257e-15, 6.99774238e-16, -1.15797965e00, 0.0]), decimal=5 - # ) - # npt.assert_almost_equal( - # q[:, -1], np.array([-4.69169076e-15, 2.00000000e00, -1.15744926e00, 0.0]), decimal=5 - # ) - # - # # how the force is stored - # data = ocp.nlp[0].numerical_data_timeseries["translational_forces"] - # npt.assert_equal(data.shape, (6, 2, 31)) - # npt.assert_almost_equal(data[:, 0, 0], np.array([0.0, 0.0, -2.0, 0.05, -0.05, 0.007])) - - # else: - # npt.assert_almost_equal(f[0, 0], 7067.851604540217) - # - # # initial and final controls - # npt.assert_almost_equal(tau[:, 0], np.array([1.53525114e-09, 6.98419368e00, -4.06669623e-10, 0.0])) - # npt.assert_almost_equal(tau[:, 10], np.array([-6.45031008e-10, 6.24337052e00, -4.06669624e-10, 0.0])) - # npt.assert_almost_equal(tau[:, 20], np.array([-5.51259131e-10, 5.50254736e00, -4.06669632e-10, 0.0])) - # npt.assert_almost_equal(tau[:, -1], np.array([1.64434894e-09, 4.83580652e00, -4.06669638e-10, 0.0])) - # - # # initial and final position - # npt.assert_almost_equal(q[:, 0], np.array([-3.41506199e-15, 6.99773953e-16, -4.11684795e-12, 0.0]), decimal=5) - # npt.assert_almost_equal(q[:, -1], np.array([-3.84536375e-15, 2.00000000e00, 6.27099006e-11, 0.0]), decimal=5) - # - # # how the force is stored - # data = ocp.nlp[0].numerical_data_timeseries["forces_in_global"] - # npt.assert_equal(data.shape, (9, 2, 31)) - # npt.assert_almost_equal(data[:, 0, 0], np.array([0.0, 0.0, 0.0, 0.0, 0.0, -2.0, 0.0, 0.0, 0.0])) - - # else: - # if use_point_of_applications: - # if point_of_application_reference_frame == ReferenceFrame.LOCAL: - # npt.assert_almost_equal(f[0, 0], 7076.043800572127) - # - # # initial and final controls - # npt.assert_almost_equal(tau[:, 0], np.array([0.01922272, 6.98428645, -0.28099094, 0.0])) - # npt.assert_almost_equal(tau[:, 10], np.array([4.40289150e-03, 6.24343465e00, -2.16557082e-01, 0.0])) - # npt.assert_almost_equal(tau[:, 20], np.array([-0.01041693, 5.50258285, -0.14650525, 0.0])) - # npt.assert_almost_equal(tau[:, -1], np.array([-0.02375477, 4.83581623, -0.12679319, 0.0])) - # - # # initial and final position - # npt.assert_almost_equal( - # q[:, 0], np.array([-4.65561476e-15, 7.00215056e-16, -1.34683990e-03, 0.0]), decimal=5 - # ) - # npt.assert_almost_equal( - # q[:, -1], np.array([-4.76138727e-15, 2.00000000e00, 2.00652560e-03, 0.0]), decimal=5 - # ) - # - # # how the force is stored - # data = ocp.nlp[0].numerical_data_timeseries["forces_in_local"] - # npt.assert_equal(data.shape, (9, 2, 31)) - # npt.assert_almost_equal( - # data[:, 0, 0], np.array([0.0, 0.0, 0.0, 0.0, 0.0, -2.0, 0.05, -0.05, 0.007]) - # ) - # else: - # npt.assert_almost_equal(f[0, 0], 7067.851604540217) - # - # # initial and final controls - # npt.assert_almost_equal(tau[:, 0], np.array([1.92280974e-09, 6.98419368e00, -4.00749051e-10, 0.0])) - # npt.assert_almost_equal(tau[:, 10], np.array([-8.87099972e-10, 6.24337052e00, 2.22483147e-10, 0.0])) - # npt.assert_almost_equal(tau[:, 20], np.array([-6.85527926e-10, 5.50254736e00, 1.70072550e-10, 0.0])) - # npt.assert_almost_equal(tau[:, -1], np.array([2.07070380e-09, 4.83580652e00, -3.91202705e-10, 0.0])) - # - # # initial and final position - # npt.assert_almost_equal( - # q[:, 0], np.array([-4.64041726e-15, 6.99774094e-16, 4.75924448e-11, 0.0]), decimal=5 - # ) - # npt.assert_almost_equal( - # q[:, -1], np.array([-4.74298258e-15, 2.00000000e00, 3.79354612e-11, 0.0]), decimal=5 - # ) - # - # # how the force is stored - # data = ocp.nlp[0].numerical_data_timeseries["forces_in_local"] - # npt.assert_equal(data.shape, (9, 2, 31)) - # npt.assert_almost_equal(data[:, 0, 0], np.array([0.0, 0.0, 0.0, 0.0, 0.0, -2.0, 0.0, 0.0, 0.0])) - - # detailed cost values - npt.assert_almost_equal(sol.detailed_cost[0]["cost_value_weighted"], f[0, 0]) + if method == "in_segment": + npt.assert_almost_equal(q[:, 0], np.array([-4.64041726e-15, 6.99774094e-16, 4.75924448e-11, 0.0]), decimal=5) + npt.assert_almost_equal(q[:, -1], np.array([-4.74298258e-15, 2.00000000e00, 3.79354612e-11, 0.0]), decimal=5) def prepare_ocp( From cdf22ebea2d0e01eee876b024d41ecdfa073f6d6 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Thu, 31 Oct 2024 16:41:30 -0400 Subject: [PATCH 090/108] tests : no working yet --- tests/shard3/test_external_forces.py | 297 +++++++++++---------------- 1 file changed, 116 insertions(+), 181 deletions(-) diff --git a/tests/shard3/test_external_forces.py b/tests/shard3/test_external_forces.py index ef939fe98..9c2988310 100644 --- a/tests/shard3/test_external_forces.py +++ b/tests/shard3/test_external_forces.py @@ -17,6 +17,7 @@ ConstraintList, ConstraintFcn, BoundsList, + ExternalForceSetTimeSeries, ) @@ -121,95 +122,60 @@ def test_example_external_forces( def prepare_ocp( biorbd_model_path: str = "models/cube_with_forces.bioMod", - use_torque_and_force_at_the_same_time: bool = False, + together: bool = False, ) -> OptimalControlProgram: # Problem parameters - n_shooting = 30 + n_shooting = 60 final_time = 2 # Linear external forces - Seg1_force = np.zeros((3, n_shooting + 1)) + Seg1_force = np.zeros((3, n_shooting)) Seg1_force[2, :] = -2 - Seg1_force[2, 4] = -22 - Test_force = np.zeros((3, n_shooting + 1)) + Test_force = np.zeros((3, n_shooting)) + Seg1_force[2, :] = -2 Test_force[2, :] = 5 Test_force[2, 4] = 52 # Point of application - Seg1_point_of_application = np.zeros((3, n_shooting + 1)) + Seg1_point_of_application = np.zeros((3, n_shooting)) Seg1_point_of_application[0, :] = 0.05 Seg1_point_of_application[1, :] = -0.05 Seg1_point_of_application[2, :] = 0.007 - Test_point_of_application = np.zeros((3, n_shooting + 1)) + Test_point_of_application = np.zeros((3, n_shooting)) Test_point_of_application[0, :] = -0.009 Test_point_of_application[1, :] = 0.01 Test_point_of_application[2, :] = -0.01 # Torques external forces - Seg1_torque = np.zeros((3, n_shooting + 1)) + Seg1_torque = np.zeros((3, n_shooting)) Seg1_torque[1, :] = 1.8 Seg1_torque[1, 4] = 18 - Test_torque = np.zeros((3, n_shooting + 1)) + Test_torque = np.zeros((3, n_shooting)) Test_torque[1, :] = -1.8 Test_torque[1, 4] = -18 - external_forces = ExternalForces() - if use_torque_and_force_at_the_same_time: - external_forces.add( - key="Seg1", - data=np.vstack((Seg1_torque, Seg1_force)), - force_type=ExternalForceType.TORQUE_AND_FORCE, - force_reference_frame=ReferenceFrame.GLOBAL, - point_of_application=Seg1_point_of_application, - point_of_application_reference_frame=ReferenceFrame.GLOBAL, - ) + if together: + external_forces = ExternalForceSetTimeSeries(nb_frames=n_shooting) + # external_forces.add("Seg1", np.vstack((Seg1_torque, Seg1_force)), Seg1_point_of_application) external_forces.add( - key="Test", - data=np.vstack((Test_torque, Test_force)), - force_type=ExternalForceType.TORQUE_AND_FORCE, - force_reference_frame=ReferenceFrame.LOCAL, - point_of_application=Test_point_of_application, - point_of_application_reference_frame=ReferenceFrame.LOCAL, + "Test", np.vstack((np.zeros((3, n_shooting)), Seg1_force)), point_of_application=Seg1_point_of_application ) + # external_forces.add("Test", np.vstack((Test_torque, Test_force)), Test_point_of_application) + print("yo", external_forces.to_numerical_time_series()[:, 0, 4]) else: - external_forces.add( - key="Seg1", - data=Seg1_force, - force_type=ExternalForceType.FORCE, - force_reference_frame=ReferenceFrame.GLOBAL, - point_of_application=Seg1_point_of_application, - point_of_application_reference_frame=ReferenceFrame.GLOBAL, - ) - external_forces.add( - key="Seg1", - data=Seg1_torque, - force_type=ExternalForceType.TORQUE, - force_reference_frame=ReferenceFrame.GLOBAL, - point_of_application=None, - point_of_application_reference_frame=None, - ) - external_forces.add( - key="Test", - data=Test_force, - force_type=ExternalForceType.FORCE, - force_reference_frame=ReferenceFrame.LOCAL, - point_of_application=Test_point_of_application, - point_of_application_reference_frame=ReferenceFrame.LOCAL, - ) - external_forces.add( - key="Test", - data=Test_torque, - force_type=ExternalForceType.TORQUE, - force_reference_frame=ReferenceFrame.LOCAL, - point_of_application=None, - point_of_application_reference_frame=None, - ) + external_forces = ExternalForceSetTimeSeries(nb_frames=n_shooting) + # external_forces.add_torque("Seg1", Seg1_torque) + external_forces.add_translational_force("Test", Seg1_force, point_of_application=Seg1_point_of_application) + # external_forces.add_torque("Test", Test_torque) + # external_forces.add_translational_force("Test", Test_force, point_of_application=Test_point_of_application) + print("yo", external_forces.to_numerical_time_series()[:, 0, 4]) - bio_model = BiorbdModel(biorbd_model_path, external_forces=external_forces) + bio_model = BiorbdModel(biorbd_model_path, external_force_set=external_forces) + numerical_data_timeseries = {"external_forces": external_forces.to_numerical_time_series()} # Add objective functions objective_functions = ObjectiveList() @@ -219,7 +185,7 @@ def prepare_ocp( dynamics = DynamicsList() dynamics.add( DynamicsFcn.TORQUE_DRIVEN, - external_forces=external_forces, + numerical_data_timeseries=numerical_data_timeseries, ) # Constraints @@ -251,7 +217,14 @@ def prepare_ocp( ) -def test_example_external_forces_all_at_once(): +@pytest.mark.parametrize( + "together", + [ + True, + False, + ], +) +def test_example_external_forces_all_at_once(together: bool): from bioptim.examples.getting_started import example_external_forces as ocp_module @@ -259,130 +232,92 @@ def test_example_external_forces_all_at_once(): ocp_false = prepare_ocp( biorbd_model_path=bioptim_folder + "/models/cube_with_forces.bioMod", - use_torque_and_force_at_the_same_time=False, - ) - sol_false = ocp_false.solve() - - # Check objective function value - f_false = np.array(sol_false.cost) - npt.assert_equal(f_false.shape, (1, 1)) - npt.assert_almost_equal(f_false[0, 0], 7075.438561173094) - - # Check constraints - g_false = np.array(sol_false.constraints) - npt.assert_equal(g_false.shape, (246, 1)) - npt.assert_almost_equal(g_false, np.zeros((246, 1))) - - # Check some of the results - states_false = sol_false.decision_states(to_merge=SolutionMerge.NODES) - controls_false = sol_false.decision_controls(to_merge=SolutionMerge.NODES) - q_false, qdot_false, tau_false = states_false["q"], states_false["qdot"], controls_false["tau"] - - # initial and final controls - npt.assert_almost_equal(tau_false[:, 0], np.array([0.18595882, 6.98424521, -0.30071214, 0.0])) - npt.assert_almost_equal(tau_false[:, 10], np.array([-0.05224518, 6.24340374, -0.17010067, 0.0])) - npt.assert_almost_equal(tau_false[:, 20], np.array([-0.03359529, 5.50256227, -0.11790254, 0.0])) - npt.assert_almost_equal(tau_false[:, -1], np.array([0.07805599, 4.83580495, -0.14719148, 0.0])) - - # initial and final position - npt.assert_almost_equal(q_false[:, 0], np.array([-4.01261246e-15, 7.00046055e-16, 2.15375856e-03, 0.0]), decimal=5) - npt.assert_almost_equal(q_false[:, -1], np.array([-4.29786206e-15, 2.00000000e00, 1.66939403e-03, 0.0]), decimal=5) - - # initial and final velocities - npt.assert_almost_equal(qdot_false[:, 0], np.array([0.0, 0.0, 0.0, 0.0]), decimal=5) - npt.assert_almost_equal(qdot_false[:, -1], np.array([0.0, 0.0, 0.0, 0.0]), decimal=5) - - # how the force is stored - data_Seg1_false = ocp_false.nlp[0].numerical_data_timeseries["forces_in_global"] - npt.assert_equal(data_Seg1_false.shape, (9, 1, 31)) - npt.assert_almost_equal(data_Seg1_false[:, 0, 0], np.array([0.0, 1.8, 0.0, 0.0, 0.0, -2.0, 0.05, -0.05, 0.007])) - - data_Test_false = ocp_false.nlp[0].numerical_data_timeseries["forces_in_local"] - npt.assert_equal(data_Test_false.shape, (9, 1, 31)) - npt.assert_almost_equal(data_Test_false[:, 0, 0], np.array([0.0, -1.8, 0.0, 0.0, 0.0, 5.0, -0.009, 0.01, -0.01])) - - # detailed cost values - npt.assert_almost_equal(sol_false.detailed_cost[0]["cost_value_weighted"], f_false[0, 0]) - - ocp_true = prepare_ocp( - biorbd_model_path=bioptim_folder + "/models/cube_with_forces.bioMod", - use_torque_and_force_at_the_same_time=True, + together=together, ) - sol_true = ocp_true.solve() - - # Check objective function value - f_true = np.array(sol_true.cost) - npt.assert_equal(f_true.shape, (1, 1)) - npt.assert_almost_equal(f_true[0, 0], f_false[0, 0]) - - # Check constraints - g_true = np.array(sol_true.constraints) - npt.assert_equal(g_true.shape, (246, 1)) - npt.assert_almost_equal(g_true, np.zeros((246, 1))) - - # Check some of the results - states_true = sol_true.decision_states(to_merge=SolutionMerge.NODES) - controls_true = sol_true.decision_controls(to_merge=SolutionMerge.NODES) - q_true, qdot_true, tau_true = states_true["q"], states_true["qdot"], controls_true["tau"] - - # initial and final controls - npt.assert_almost_equal(tau_true[:, 0], tau_false[:, 0]) - npt.assert_almost_equal(tau_true[:, 10], tau_false[:, 10]) - npt.assert_almost_equal(tau_true[:, 20], tau_false[:, 20]) - npt.assert_almost_equal(tau_true[:, -1], tau_false[:, -1]) - - # initial and final position - npt.assert_almost_equal(q_true[:, 0], q_true[:, 0]) - npt.assert_almost_equal(q_true[:, -1], q_true[:, -1]) - - # initial and final velocities - npt.assert_almost_equal(qdot_true[:, 0], qdot_true[:, 0]) - npt.assert_almost_equal(qdot_true[:, -1], qdot_true[:, -1]) - - # how the force is stored - data_Seg1_true = ocp_true.nlp[0].numerical_data_timeseries["forces_in_global"] - npt.assert_equal(data_Seg1_true.shape, (9, 1, 31)) - npt.assert_almost_equal(data_Seg1_true[:, 0, 0], data_Seg1_false[:, 0, 0]) - data_Test_true = ocp_true.nlp[0].numerical_data_timeseries["forces_in_local"] - npt.assert_equal(data_Test_true.shape, (9, 1, 31)) - npt.assert_almost_equal(data_Test_true[:, 0, 0], data_Test_false[:, 0, 0]) - - # detailed cost values - npt.assert_almost_equal(sol_true.detailed_cost[0]["cost_value_weighted"], f_true[0, 0]) - - -def test_fail(): - - n_shooting = 30 - fake_force = np.zeros((3, n_shooting + 1)) - - external_forces = ExternalForces() - external_forces.add( - key="Seg1", - data=np.vstack((fake_force, fake_force)), - force_type=ExternalForceType.TORQUE_AND_FORCE, - force_reference_frame=ReferenceFrame.GLOBAL, - ) - external_forces.add( - key="Test", - data=np.vstack((fake_force, fake_force)), - force_type=ExternalForceType.TORQUE_AND_FORCE, - force_reference_frame=ReferenceFrame.LOCAL, - ) + if together: + z = ocp_false.nlp[0].dynamics_func( + np.ones(2), np.ones(8), np.ones(4), [], [], np.hstack((np.zeros(3), np.ones(6))) + ) - with pytest.raises(ValueError, match="The force is already defined for Seg1"): - external_forces.add( - key="Seg1", - data=fake_force, - force_type=ExternalForceType.FORCE, - force_reference_frame=ReferenceFrame.GLOBAL, + npt.assert_almost_equal( + np.array(z[4:]).squeeze(), + np.array([2.00e00, -7.81e00, 1.00e00, 1.00e08]), ) + else: + z = ocp_false.nlp[0].dynamics_func(np.ones(2), np.ones(8), np.ones(4), [], [], np.ones(6)) - with pytest.raises(ValueError, match="The torque is already defined for Seg1"): - external_forces.add( - key="Seg1", - data=fake_force, - force_type=ExternalForceType.TORQUE, - force_reference_frame=ReferenceFrame.GLOBAL, + npt.assert_almost_equal( + np.array(z[4:]).squeeze(), + np.array([2.0000000e00, -7.8100000e00, -6.8294197e-01, 1.0000000e08]), ) + + # z = ocp_false.nlp[0].dynamics_func(np.ones(2), np.ones(8), np.ones(4), [], [], np.zeros(9)) + # + # npt.assert_almost_equal( + # np.array(z[4:]).squeeze(), + # np.array([9.99999990e-01, -8.81000001e00, 1.00000000e00, 1.00000000e08]), + # ) + # + # z = ocp_false.nlp[0].dynamics_func(np.ones(2), np.ones(8), np.ones(4), [], [], np.ones(9)) + # + # npt.assert_almost_equal( + # np.array(z[4:]).squeeze(), + # np.array([2.00e00, -7.81e00, 2.00e00, 1.00e08]), + # ) + # + # z = ocp_false.nlp[0].model.forward_dynamics(with_contact=False)(np.ones(4), np.ones(4), np.ones(4), np.ones(9), []) + # npt.assert_almost_equal( + # np.array(z).squeeze(), + # np.array([2.00e00, -7.81e00, 2.00e00, 1.00e08]), + # ) + + # z = ocp_false.nlp[0].dynamics_func(np.ones(2), np.ones(8), np.ones(4), [], [], np.ones(18) * 0.1) + # + # npt.assert_almost_equal( + # np.array(z[4:]).squeeze(), + # np.array([1.20e00, -8.61e00, 1.20e00, 1.10e08]), + # ) + + # sol_false = ocp_false.solve() + # + # # Check objective function value + # f_false = np.array(sol_false.cost) + # npt.assert_equal(f_false.shape, (1, 1)) + # npt.assert_almost_equal(f_false[0, 0], 7075.438561173094) + # + # # Check constraints + # g_false = np.array(sol_false.constraints) + # npt.assert_equal(g_false.shape, (246, 1)) + # npt.assert_almost_equal(g_false, np.zeros((246, 1))) + # + # # Check some of the results + # states_false = sol_false.decision_states(to_merge=SolutionMerge.NODES) + # controls_false = sol_false.decision_controls(to_merge=SolutionMerge.NODES) + # q_false, qdot_false, tau_false = states_false["q"], states_false["qdot"], controls_false["tau"] + # + # # initial and final controls + # npt.assert_almost_equal(tau_false[:, 0], np.array([0.18595882, 6.98424521, -0.30071214, 0.0])) + # npt.assert_almost_equal(tau_false[:, 10], np.array([-0.05224518, 6.24340374, -0.17010067, 0.0])) + # npt.assert_almost_equal(tau_false[:, 20], np.array([-0.03359529, 5.50256227, -0.11790254, 0.0])) + # npt.assert_almost_equal(tau_false[:, -1], np.array([0.07805599, 4.83580495, -0.14719148, 0.0])) + # + # # initial and final position + # npt.assert_almost_equal(q_false[:, 0], np.array([-4.01261246e-15, 7.00046055e-16, 2.15375856e-03, 0.0]), decimal=5) + # npt.assert_almost_equal(q_false[:, -1], np.array([-4.29786206e-15, 2.00000000e00, 1.66939403e-03, 0.0]), decimal=5) + # + # # initial and final velocities + # npt.assert_almost_equal(qdot_false[:, 0], np.array([0.0, 0.0, 0.0, 0.0]), decimal=5) + # npt.assert_almost_equal(qdot_false[:, -1], np.array([0.0, 0.0, 0.0, 0.0]), decimal=5) + # + # # how the force is stored + # data_Seg1_false = ocp_false.nlp[0].numerical_data_timeseries["forces_in_global"] + # npt.assert_equal(data_Seg1_false.shape, (9, 1, 31)) + # npt.assert_almost_equal(data_Seg1_false[:, 0, 0], np.array([0.0, 1.8, 0.0, 0.0, 0.0, -2.0, 0.05, -0.05, 0.007])) + # + # data_Test_false = ocp_false.nlp[0].numerical_data_timeseries["forces_in_local"] + # npt.assert_equal(data_Test_false.shape, (9, 1, 31)) + # npt.assert_almost_equal(data_Test_false[:, 0, 0], np.array([0.0, -1.8, 0.0, 0.0, 0.0, 5.0, -0.009, 0.01, -0.01])) + # + # # detailed cost values + # npt.assert_almost_equal(sol_false.detailed_cost[0]["cost_value_weighted"], f_false[0, 0]) From 9d51c8829bfa6402b9da6e2be1038bc996cd161e Mon Sep 17 00:00:00 2001 From: Ipuch Date: Thu, 31 Oct 2024 16:41:43 -0400 Subject: [PATCH 091/108] fix: for sure --- bioptim/models/biorbd/external_forces.py | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/bioptim/models/biorbd/external_forces.py b/bioptim/models/biorbd/external_forces.py index 702be0ff2..14cbd4f7a 100644 --- a/bioptim/models/biorbd/external_forces.py +++ b/bioptim/models/biorbd/external_forces.py @@ -121,16 +121,17 @@ def nb_external_forces(self) -> int: @property def nb_external_forces_components(self) -> int: """Return the number of vertical components of the external forces if concatenated in a unique vector""" - attributes_three_components = ["torque_in_global", "translational_in_global", "torque_in_local"] + attributes_no_point_of_application = ["torque_in_global", "torque_in_local"] attributes_six_components = ["in_global", "in_local"] components = 0 - for attr in attributes_three_components: + for attr in attributes_no_point_of_application: for values in getattr(self, attr).values(): - nb_point_of_application_as_str = sum( - [isinstance(force["point_of_application"], str) for force in values] - ) - components += 6 * len(values) - 3 * nb_point_of_application_as_str + components += 3 * len(values) + + for values in self.translational_in_global.values(): + nb_point_of_application_as_str = sum([isinstance(force["point_of_application"], str) for force in values]) + components += 6 * len(values) - 3 * nb_point_of_application_as_str for attr in attributes_six_components: for values in getattr(self, attr).values(): @@ -219,7 +220,7 @@ def to_numerical_time_series(self): poa_slicer = slice(stop, stop + 3) fext_numerical_time_series[poa_slicer, 0, :-1] = force["point_of_application"] - symbolic_counter += stop + 3 if array_point_of_application else stop + symbolic_counter = stop + 3 if array_point_of_application else stop return fext_numerical_time_series From 7676f72a6a8ab4b77e7884e1b58ff8337e18784d Mon Sep 17 00:00:00 2001 From: Ipuch Date: Thu, 31 Oct 2024 16:41:57 -0400 Subject: [PATCH 092/108] fix: --- bioptim/models/biorbd/biorbd_model.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/bioptim/models/biorbd/biorbd_model.py b/bioptim/models/biorbd/biorbd_model.py index eb742b0a4..2fa834e8f 100644 --- a/bioptim/models/biorbd/biorbd_model.py +++ b/bioptim/models/biorbd/biorbd_model.py @@ -434,6 +434,7 @@ def reorder_qddot_root_joints(qddot_root, qddot_joints) -> MX | SX: return vertcat(qddot_root, qddot_joints) def _dispatch_forces(self): + """Dispatch the symbolic MX into the biorbd external forces object""" biorbd_external_forces = self.model.externalForceSet() # "type of external force": (function to call, number of force components, number of point of application components) @@ -441,7 +442,7 @@ def _dispatch_forces(self): "in_global": (biorbd_external_forces.add, 6), "torque_in_global": ( lambda segment, torque, point_of_application: biorbd_external_forces.add( - segment, vertcat(torque, MX([0, 0, 0])) + segment, vertcat(torque, MX([0, 0, 0])), MX([0, 0, 0]) ), 3, ), From 6780f0c5c003e95fc1537a1d4d114f5d6733d5b2 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Thu, 31 Oct 2024 16:42:16 -0400 Subject: [PATCH 093/108] clean: polish exemple --- .../getting_started/example_external_forces.py | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/bioptim/examples/getting_started/example_external_forces.py b/bioptim/examples/getting_started/example_external_forces.py index 32b5fc6f1..1481e950c 100644 --- a/bioptim/examples/getting_started/example_external_forces.py +++ b/bioptim/examples/getting_started/example_external_forces.py @@ -69,33 +69,23 @@ def prepare_ocp( n_shooting = 30 final_time = 2 - # Linear external forces - # Seg1_force = np.zeros((3, n_shooting + 1)) Seg1_force = np.zeros((3, n_shooting)) Seg1_force[2, :] = -2 Seg1_force[2, 4] = -22 - # Test_force = np.zeros((3, n_shooting + 1)) Test_force = np.zeros((3, n_shooting)) Test_force[2, :] = 5 Test_force[2, 4] = 52 - # Seg1_point_of_application - # if use_point_of_applications: - # Seg1_point_of_application = np.zeros((3, n_shooting + 1)) Seg1_point_of_application = np.zeros((3, n_shooting)) Seg1_point_of_application[0, :] = 0.05 Seg1_point_of_application[1, :] = -0.05 Seg1_point_of_application[2, :] = 0.007 - # Test_point_of_application = np.zeros((3, n_shooting + 1)) Test_point_of_application = np.zeros((3, n_shooting)) Test_point_of_application[0, :] = -0.009 Test_point_of_application[1, :] = 0.01 Test_point_of_application[2, :] = -0.01 - # else: - # Seg1_point_of_application = None - # Test_point_of_application = None external_force_set = ExternalForceSetTimeSeries( nb_frames=n_shooting, From 7e4a91ae1a1a81e903e7f6884062364117b46286 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Thu, 31 Oct 2024 17:51:26 -0400 Subject: [PATCH 094/108] fix: see you tomorrow --- bioptim/models/biorbd/biorbd_model.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bioptim/models/biorbd/biorbd_model.py b/bioptim/models/biorbd/biorbd_model.py index 2fa834e8f..1a09224be 100644 --- a/bioptim/models/biorbd/biorbd_model.py +++ b/bioptim/models/biorbd/biorbd_model.py @@ -482,7 +482,7 @@ def _dispatch_forces(self): point_of_application = None function(segment, self.external_forces[force_slicer], point_of_application) - symbolic_counter += stop + 3 if array_point_of_application else stop + symbolic_counter = stop + 3 if array_point_of_application else stop return biorbd_external_forces From 41f17b53174ced3c72337d662ddfe2d82224bf64 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Fri, 1 Nov 2024 09:39:58 -0400 Subject: [PATCH 095/108] test together are separate does the same. --- .../example_external_forces.py | 8 +- bioptim/models/biorbd/external_forces.py | 24 ++- tests/shard3/test_external_forces.py | 176 +++++++++--------- 3 files changed, 106 insertions(+), 102 deletions(-) diff --git a/bioptim/examples/getting_started/example_external_forces.py b/bioptim/examples/getting_started/example_external_forces.py index 1481e950c..dd80b6135 100644 --- a/bioptim/examples/getting_started/example_external_forces.py +++ b/bioptim/examples/getting_started/example_external_forces.py @@ -93,8 +93,12 @@ def prepare_ocp( # Displays all the possible combinations of external forces if external_force_method == "translational_force": # not mandatory to specify the point of application - external_force_set.add_translational_force("Seg1", Seg1_force, point_of_application=Seg1_point_of_application) - external_force_set.add_translational_force("Test", Test_force, point_of_application=Test_point_of_application) + external_force_set.add_translational_force( + "Seg1", Seg1_force, point_of_application_in_local=Seg1_point_of_application + ) + external_force_set.add_translational_force( + "Test", Test_force, point_of_application_in_local=Test_point_of_application + ) elif external_force_method == "in_global": # not mandatory to specify the point of application external_force_set.add("Seg1", np.concatenate((Seg1_force, Seg1_force), axis=0)) diff --git a/bioptim/models/biorbd/external_forces.py b/bioptim/models/biorbd/external_forces.py index 14cbd4f7a..19f895e36 100644 --- a/bioptim/models/biorbd/external_forces.py +++ b/bioptim/models/biorbd/external_forces.py @@ -82,27 +82,37 @@ def add_torque(self, segment: str, values: np.ndarray): self.torque_in_global = ensure_list(self.torque_in_global, segment) self.torque_in_global[segment].append({"values": values, "point_of_application": None}) - def add_translational_force(self, segment: str, values: np.ndarray, point_of_application: np.ndarray | str = None): + def add_translational_force( + self, segment: str, values: np.ndarray, point_of_application_in_local: np.ndarray | str = None + ): self._check_if_can_be_modified() if values.shape[0] != 3: raise ValueError(f"External forces must have 3 rows, got {values.shape[0]}") self._check_values_frame_shape(values) - point_of_application = np.zeros((3, self._nb_frames)) if point_of_application is None else point_of_application - self._check_point_of_application(point_of_application) + point_of_application_in_local = ( + np.zeros((3, self._nb_frames)) if point_of_application_in_local is None else point_of_application_in_local + ) + self._check_point_of_application(point_of_application_in_local) self.translational_in_global = ensure_list(self.translational_in_global, segment) - self.translational_in_global[segment].append({"values": values, "point_of_application": point_of_application}) + self.translational_in_global[segment].append( + {"values": values, "point_of_application": point_of_application_in_local} + ) - def add_in_segment_frame(self, segment: str, values: np.ndarray, point_of_application: np.ndarray | str = None): + def add_in_segment_frame( + self, segment: str, values: np.ndarray, point_of_application_in_local: np.ndarray | str = None + ): self._check_if_can_be_modified() if values.shape[0] != 6: raise ValueError(f"External forces must have 6 rows, got {values.shape[0]}") self._check_values_frame_shape(values) - point_of_application = np.zeros((3, self._nb_frames)) if point_of_application is None else point_of_application + point_of_application = ( + np.zeros((3, self._nb_frames)) if point_of_application_in_local is None else point_of_application_in_local + ) self._check_point_of_application(point_of_application) self.in_local = ensure_list(self.in_local, segment) - self.in_local[segment].append({"values": values, "point_of_application": point_of_application}) + self.in_local[segment].append({"values": values, "point_of_application": point_of_application_in_local}) def add_torque_in_segment_frame(self, segment: str, values: np.ndarray): self._check_if_can_be_modified() diff --git a/tests/shard3/test_external_forces.py b/tests/shard3/test_external_forces.py index 9c2988310..20f73fb3c 100644 --- a/tests/shard3/test_external_forces.py +++ b/tests/shard3/test_external_forces.py @@ -126,7 +126,7 @@ def prepare_ocp( ) -> OptimalControlProgram: # Problem parameters - n_shooting = 60 + n_shooting = 30 final_time = 2 # Linear external forces @@ -160,19 +160,23 @@ def prepare_ocp( if together: external_forces = ExternalForceSetTimeSeries(nb_frames=n_shooting) - # external_forces.add("Seg1", np.vstack((Seg1_torque, Seg1_force)), Seg1_point_of_application) - external_forces.add( - "Test", np.vstack((np.zeros((3, n_shooting)), Seg1_force)), point_of_application=Seg1_point_of_application - ) - # external_forces.add("Test", np.vstack((Test_torque, Test_force)), Test_point_of_application) - print("yo", external_forces.to_numerical_time_series()[:, 0, 4]) + external_forces.add("Seg1", np.vstack((Seg1_torque, Seg1_force)), Seg1_point_of_application) + external_forces.add("Test", np.vstack((Test_torque, Test_force)), Test_point_of_application) + else: external_forces = ExternalForceSetTimeSeries(nb_frames=n_shooting) - # external_forces.add_torque("Seg1", Seg1_torque) - external_forces.add_translational_force("Test", Seg1_force, point_of_application=Seg1_point_of_application) - # external_forces.add_torque("Test", Test_torque) - # external_forces.add_translational_force("Test", Test_force, point_of_application=Test_point_of_application) - print("yo", external_forces.to_numerical_time_series()[:, 0, 4]) + external_forces.add( + "Seg1", np.vstack((Seg1_torque, np.zeros((3, n_shooting)))), point_of_application=Seg1_point_of_application + ) + external_forces.add( + "Seg1", np.vstack((np.zeros((3, n_shooting)), Seg1_force)), point_of_application=Seg1_point_of_application + ) + external_forces.add( + "Test", np.vstack((Test_torque, np.zeros((3, n_shooting)))), point_of_application=Test_point_of_application + ) + external_forces.add( + "Test", np.vstack((np.zeros((3, n_shooting)), Test_force)), point_of_application=Test_point_of_application + ) bio_model = BiorbdModel(biorbd_model_path, external_force_set=external_forces) numerical_data_timeseries = {"external_forces": external_forces.to_numerical_time_series()} @@ -230,94 +234,80 @@ def test_example_external_forces_all_at_once(together: bool): bioptim_folder = os.path.dirname(ocp_module.__file__) - ocp_false = prepare_ocp( + ocp = prepare_ocp( biorbd_model_path=bioptim_folder + "/models/cube_with_forces.bioMod", together=together, ) - if together: - z = ocp_false.nlp[0].dynamics_func( - np.ones(2), np.ones(8), np.ones(4), [], [], np.hstack((np.zeros(3), np.ones(6))) - ) + sol = ocp.solve() + # # Check objective function value + f = np.array(sol.cost) + npt.assert_equal(f.shape, (1, 1)) + npt.assert_almost_equal(f[0, 0], 5507.938264053537) + npt.assert_almost_equal(f[0, 0], sol.detailed_cost[0]["cost_value_weighted"]) + # Check constraints + g = np.array(sol.constraints) + npt.assert_equal(g.shape, (246, 1)) + npt.assert_almost_equal(g, np.zeros((246, 1))) + + # Check some of the results + states = sol.decision_states(to_merge=SolutionMerge.NODES) + controls = sol.decision_controls(to_merge=SolutionMerge.NODES) + q, qdot, tau = states["q"], states["qdot"], controls["tau"] + + # initial and final controls + npt.assert_almost_equal(tau[:, 0], np.array([-0.17782387, 4.9626883, -0.11993298, 0.0])) + npt.assert_almost_equal(tau[:, 10], np.array([0.0590585, 5.15623667, -0.11993298, 0.0])) + npt.assert_almost_equal(tau[:, 20], np.array([0.03581958, 5.34978503, -0.11993298, 0.0])) + npt.assert_almost_equal(tau[:, -1], np.array([-0.12181904, 5.52397856, -0.11993298, 0.0])) + + # initial and final position + npt.assert_almost_equal( + q[:, 0], np.array([-3.05006547e-15, 7.80152284e-16, -5.64943197e-03, 0.00000000e00]), decimal=5 + ) + npt.assert_almost_equal( + q[:, -1], np.array([-3.85629113e-15, 2.00000000e00, -5.02503164e-04, 0.00000000e00]), decimal=5 + ) + + # initial and final velocities + npt.assert_almost_equal(qdot[:, 0], np.array([0.0, 0.0, 0.0, 0.0]), decimal=5) + npt.assert_almost_equal(qdot[:, -1], np.array([0.0, 0.0, 0.0, 0.0]), decimal=5) + + # how the force is stored + data_Seg1 = ocp.nlp[0].numerical_data_timeseries["external_forces"] + if together: + npt.assert_equal(data_Seg1.shape, (18, 1, 31)) + no_zeros_data_Seg1 = data_Seg1[~np.all(np.all(data_Seg1 == 0, axis=1), axis=1)] + npt.assert_equal(no_zeros_data_Seg1.shape, (10, 1, 31)) npt.assert_almost_equal( - np.array(z[4:]).squeeze(), - np.array([2.00e00, -7.81e00, 1.00e00, 1.00e08]), + no_zeros_data_Seg1[:, 0, 4], + np.array([1.8e01, -2.0e00, 5.0e-02, -5.0e-02, 7.0e-03, -1.8e01, 5.2e01, -9.0e-03, 1.0e-02, -1.0e-02]), ) else: - z = ocp_false.nlp[0].dynamics_func(np.ones(2), np.ones(8), np.ones(4), [], [], np.ones(6)) - + npt.assert_equal(data_Seg1.shape, (36, 1, 31)) + no_zeros_data_Seg1 = data_Seg1[~np.all(np.all(data_Seg1 == 0, axis=1), axis=1)] + npt.assert_equal(no_zeros_data_Seg1.shape, (16, 1, 31)) npt.assert_almost_equal( - np.array(z[4:]).squeeze(), - np.array([2.0000000e00, -7.8100000e00, -6.8294197e-01, 1.0000000e08]), + no_zeros_data_Seg1[:, 0, 4], + np.array( + [ + 1.8e01, + 5.0e-02, + -5.0e-02, + 7.0e-03, + -2.0e00, + 5.0e-02, + -5.0e-02, + 7.0e-03, + -1.8e01, + -9.0e-03, + 1.0e-02, + -1.0e-02, + 5.2e01, + -9.0e-03, + 1.0e-02, + -1.0e-02, + ] + ), ) - - # z = ocp_false.nlp[0].dynamics_func(np.ones(2), np.ones(8), np.ones(4), [], [], np.zeros(9)) - # - # npt.assert_almost_equal( - # np.array(z[4:]).squeeze(), - # np.array([9.99999990e-01, -8.81000001e00, 1.00000000e00, 1.00000000e08]), - # ) - # - # z = ocp_false.nlp[0].dynamics_func(np.ones(2), np.ones(8), np.ones(4), [], [], np.ones(9)) - # - # npt.assert_almost_equal( - # np.array(z[4:]).squeeze(), - # np.array([2.00e00, -7.81e00, 2.00e00, 1.00e08]), - # ) - # - # z = ocp_false.nlp[0].model.forward_dynamics(with_contact=False)(np.ones(4), np.ones(4), np.ones(4), np.ones(9), []) - # npt.assert_almost_equal( - # np.array(z).squeeze(), - # np.array([2.00e00, -7.81e00, 2.00e00, 1.00e08]), - # ) - - # z = ocp_false.nlp[0].dynamics_func(np.ones(2), np.ones(8), np.ones(4), [], [], np.ones(18) * 0.1) - # - # npt.assert_almost_equal( - # np.array(z[4:]).squeeze(), - # np.array([1.20e00, -8.61e00, 1.20e00, 1.10e08]), - # ) - - # sol_false = ocp_false.solve() - # - # # Check objective function value - # f_false = np.array(sol_false.cost) - # npt.assert_equal(f_false.shape, (1, 1)) - # npt.assert_almost_equal(f_false[0, 0], 7075.438561173094) - # - # # Check constraints - # g_false = np.array(sol_false.constraints) - # npt.assert_equal(g_false.shape, (246, 1)) - # npt.assert_almost_equal(g_false, np.zeros((246, 1))) - # - # # Check some of the results - # states_false = sol_false.decision_states(to_merge=SolutionMerge.NODES) - # controls_false = sol_false.decision_controls(to_merge=SolutionMerge.NODES) - # q_false, qdot_false, tau_false = states_false["q"], states_false["qdot"], controls_false["tau"] - # - # # initial and final controls - # npt.assert_almost_equal(tau_false[:, 0], np.array([0.18595882, 6.98424521, -0.30071214, 0.0])) - # npt.assert_almost_equal(tau_false[:, 10], np.array([-0.05224518, 6.24340374, -0.17010067, 0.0])) - # npt.assert_almost_equal(tau_false[:, 20], np.array([-0.03359529, 5.50256227, -0.11790254, 0.0])) - # npt.assert_almost_equal(tau_false[:, -1], np.array([0.07805599, 4.83580495, -0.14719148, 0.0])) - # - # # initial and final position - # npt.assert_almost_equal(q_false[:, 0], np.array([-4.01261246e-15, 7.00046055e-16, 2.15375856e-03, 0.0]), decimal=5) - # npt.assert_almost_equal(q_false[:, -1], np.array([-4.29786206e-15, 2.00000000e00, 1.66939403e-03, 0.0]), decimal=5) - # - # # initial and final velocities - # npt.assert_almost_equal(qdot_false[:, 0], np.array([0.0, 0.0, 0.0, 0.0]), decimal=5) - # npt.assert_almost_equal(qdot_false[:, -1], np.array([0.0, 0.0, 0.0, 0.0]), decimal=5) - # - # # how the force is stored - # data_Seg1_false = ocp_false.nlp[0].numerical_data_timeseries["forces_in_global"] - # npt.assert_equal(data_Seg1_false.shape, (9, 1, 31)) - # npt.assert_almost_equal(data_Seg1_false[:, 0, 0], np.array([0.0, 1.8, 0.0, 0.0, 0.0, -2.0, 0.05, -0.05, 0.007])) - # - # data_Test_false = ocp_false.nlp[0].numerical_data_timeseries["forces_in_local"] - # npt.assert_equal(data_Test_false.shape, (9, 1, 31)) - # npt.assert_almost_equal(data_Test_false[:, 0, 0], np.array([0.0, -1.8, 0.0, 0.0, 0.0, 5.0, -0.009, 0.01, -0.01])) - # - # # detailed cost values - # npt.assert_almost_equal(sol_false.detailed_cost[0]["cost_value_weighted"], f_false[0, 0]) From 2fc1928897256a39ea5319be2475882baa20442c Mon Sep 17 00:00:00 2001 From: Ipuch Date: Fri, 1 Nov 2024 09:51:36 -0400 Subject: [PATCH 096/108] docs and fix: point application in local --- bioptim/models/biorbd/biorbd_model.py | 3 +++ bioptim/models/biorbd/external_forces.py | 16 ++++++++++++++-- 2 files changed, 17 insertions(+), 2 deletions(-) diff --git a/bioptim/models/biorbd/biorbd_model.py b/bioptim/models/biorbd/biorbd_model.py index 1a09224be..0840491e1 100644 --- a/bioptim/models/biorbd/biorbd_model.py +++ b/bioptim/models/biorbd/biorbd_model.py @@ -524,6 +524,9 @@ def forward_dynamics(self, with_contact: bool = False) -> Function: def inverse_dynamics(self, with_contact: bool = False) -> Function: + if with_contact: + raise NotImplementedError("Inverse dynamics with contact is not implemented yet") + q_biorbd = GeneralizedCoordinates(self.q) qdot_biorbd = GeneralizedVelocity(self.qdot) qddot_biorbd = GeneralizedAcceleration(self.qddot) diff --git a/bioptim/models/biorbd/external_forces.py b/bioptim/models/biorbd/external_forces.py index 19f895e36..a8572f217 100644 --- a/bioptim/models/biorbd/external_forces.py +++ b/bioptim/models/biorbd/external_forces.py @@ -102,15 +102,27 @@ def add_translational_force( def add_in_segment_frame( self, segment: str, values: np.ndarray, point_of_application_in_local: np.ndarray | str = None ): + """ + Add external forces in the segment frame. + + Parameters + ---------- + segment: str + The name of the segment. + values: np.ndarray + The external forces (torques, forces) in the segment frame. + point_of_application_in_local + The point of application of the external forces in the segment frame. + """ self._check_if_can_be_modified() if values.shape[0] != 6: raise ValueError(f"External forces must have 6 rows, got {values.shape[0]}") self._check_values_frame_shape(values) - point_of_application = ( + point_of_application_in_local = ( np.zeros((3, self._nb_frames)) if point_of_application_in_local is None else point_of_application_in_local ) - self._check_point_of_application(point_of_application) + self._check_point_of_application(point_of_application_in_local) self.in_local = ensure_list(self.in_local, segment) self.in_local[segment].append({"values": values, "point_of_application": point_of_application_in_local}) From d2edb7f093f4530d76ad017b5e5b1aa119076c11 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Fri, 1 Nov 2024 10:31:23 -0400 Subject: [PATCH 097/108] tests: make them all pass ! --- .../example_external_forces.py | 23 +- bioptim/models/biorbd/biorbd_model.py | 6 +- tests/shard1/test_dynamics.py | 460 ++++-------------- tests/shard3/test_external_force_class.py | 4 +- tests/shard3/test_global_getting_started.py | 9 +- 5 files changed, 128 insertions(+), 374 deletions(-) diff --git a/bioptim/examples/getting_started/example_external_forces.py b/bioptim/examples/getting_started/example_external_forces.py index dd80b6135..89ba3078a 100644 --- a/bioptim/examples/getting_started/example_external_forces.py +++ b/bioptim/examples/getting_started/example_external_forces.py @@ -34,7 +34,8 @@ def prepare_ocp( ode_solver: OdeSolverBase = OdeSolver.RK4(), expand_dynamics: bool = True, phase_dynamics: PhaseDynamics = PhaseDynamics.ONE_PER_NODE, - external_force_method: str = "translational_force", + external_force_method: str = "in_global", + use_point_of_applications: bool = False, n_threads: int = 1, use_sx: bool = False, ) -> OptimalControlProgram: @@ -94,15 +95,27 @@ def prepare_ocp( if external_force_method == "translational_force": # not mandatory to specify the point of application external_force_set.add_translational_force( - "Seg1", Seg1_force, point_of_application_in_local=Seg1_point_of_application + "Seg1", + Seg1_force, + point_of_application_in_local=Seg1_point_of_application if use_point_of_applications else None, ) external_force_set.add_translational_force( - "Test", Test_force, point_of_application_in_local=Test_point_of_application + "Test", + Test_force, + point_of_application_in_local=Test_point_of_application if use_point_of_applications else None, ) elif external_force_method == "in_global": # not mandatory to specify the point of application - external_force_set.add("Seg1", np.concatenate((Seg1_force, Seg1_force), axis=0)) - external_force_set.add("Test", np.concatenate((Test_force, Test_force), axis=0)) + external_force_set.add( + "Seg1", + np.concatenate((Seg1_force, Seg1_force), axis=0), + point_of_application=Seg1_point_of_application if use_point_of_applications else None, + ) + external_force_set.add( + "Test", + np.concatenate((Test_force, Test_force), axis=0), + point_of_application=Test_point_of_application if use_point_of_applications else None, + ) elif external_force_method == "in_global_torque": # cannot specify the point of application as it is a pure torque external_force_set.add_torque("Seg1", Seg1_force) diff --git a/bioptim/models/biorbd/biorbd_model.py b/bioptim/models/biorbd/biorbd_model.py index 0840491e1..6db010189 100644 --- a/bioptim/models/biorbd/biorbd_model.py +++ b/bioptim/models/biorbd/biorbd_model.py @@ -497,7 +497,7 @@ def forward_dynamics(self, with_contact: bool = False) -> Function: biorbd_return = self.model.ForwardDynamicsConstraintsDirect(q_biorbd, qdot_biorbd, tau_biorbd).to_mx() else: biorbd_return = self.model.ForwardDynamicsConstraintsDirect( - q_biorbd, qdot_biorbd, tau_biorbd, externalForces=self.biorbd_external_forces_set + q_biorbd, qdot_biorbd, tau_biorbd, self.biorbd_external_forces_set ).to_mx() casadi_fun = Function( "constrained_forward_dynamics", @@ -534,7 +534,7 @@ def inverse_dynamics(self, with_contact: bool = False) -> Function: biorbd_return = self.model.InverseDynamics(q_biorbd, qdot_biorbd, qddot_biorbd).to_mx() else: biorbd_return = self.model.InverseDynamics( - q_biorbd, qdot_biorbd, qddot_biorbd, externalForces=self.biorbd_external_forces_set + q_biorbd, qdot_biorbd, qddot_biorbd, self.biorbd_external_forces_set ).to_mx() casadi_fun = Function( "inverse_dynamics", @@ -556,7 +556,7 @@ def contact_forces_from_constrained_forward_dynamics(self) -> Function: ).to_mx() else: biorbd_return = self.model.ContactForcesFromForwardDynamicsConstraintsDirect( - q_biorbd, qdot_biorbd, tau_biorbd, externalForces=self.biorbd_external_forces_set + q_biorbd, qdot_biorbd, tau_biorbd, self.biorbd_external_forces_set ).to_mx() casadi_fun = Function( "contact_forces_from_constrained_forward_dynamics", diff --git a/tests/shard1/test_dynamics.py b/tests/shard1/test_dynamics.py index 00f521167..f3fd78187 100644 --- a/tests/shard1/test_dynamics.py +++ b/tests/shard1/test_dynamics.py @@ -38,85 +38,85 @@ def __init__(self, nlp, use_sx): self.n_threads = 1 +N_SHOOTING = 5 +EXTERNAL_FORCE_ARRAY = np.zeros((9, N_SHOOTING)) +EXTERNAL_FORCE_ARRAY[:, 0] = [ + 0.374540118847362, + 0.950714306409916, + 0.731993941811405, + 0.598658484197037, + 0.156018640442437, + 0.155994520336203, + 0, + 0, + 0, +] +EXTERNAL_FORCE_ARRAY[:, 1] = [ + 0.058083612168199, + 0.866176145774935, + 0.601115011743209, + 0.708072577796045, + 0.020584494295802, + 0.969909852161994, + 0, + 0, + 0, +] +EXTERNAL_FORCE_ARRAY[:, 2] = [ + 0.832442640800422, + 0.212339110678276, + 0.181824967207101, + 0.183404509853434, + 0.304242242959538, + 0.524756431632238, + 0, + 0, + 0, +] +EXTERNAL_FORCE_ARRAY[:, 3] = [ + 0.431945018642116, + 0.291229140198042, + 0.611852894722379, + 0.139493860652042, + 0.292144648535218, + 0.366361843293692, + 0, + 0, + 0, +] +EXTERNAL_FORCE_ARRAY[:, 4] = [ + 0.456069984217036, + 0.785175961393014, + 0.19967378215836, + 0.514234438413612, + 0.592414568862042, + 0.046450412719998, + 0, + 0, + 0, +] + + @pytest.mark.parametrize("phase_dynamics", [PhaseDynamics.SHARED_DURING_THE_PHASE, PhaseDynamics.ONE_PER_NODE]) @pytest.mark.parametrize("cx", [MX, SX]) @pytest.mark.parametrize( "with_external_force", - [ - # False, - True - ], + [False, True], ) @pytest.mark.parametrize("with_contact", [False, True]) def test_torque_driven(with_contact, with_external_force, cx, phase_dynamics): # Prepare the program nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx == SX)) - nlp.ns = 5 + nlp.ns = N_SHOOTING external_forces = None numerical_time_series = None if with_external_force: - external_forces_array = np.zeros((9, nlp.ns)) - external_forces_array[:, 0] = [ - 0.374540118847362, - 0.950714306409916, - 0.731993941811405, - 0.598658484197037, - 0.156018640442437, - 0.155994520336203, - 0, - 0, - 0, - ] - external_forces_array[:, 1] = [ - 0.058083612168199, - 0.866176145774935, - 0.601115011743209, - 0.708072577796045, - 0.020584494295802, - 0.969909852161994, - 0, - 0, - 0, - ] - external_forces_array[:, 2] = [ - 0.832442640800422, - 0.212339110678276, - 0.181824967207101, - 0.183404509853434, - 0.304242242959538, - 0.524756431632238, - 0, - 0, - 0, - ] - external_forces_array[:, 3] = [ - 0.431945018642116, - 0.291229140198042, - 0.611852894722379, - 0.139493860652042, - 0.292144648535218, - 0.366361843293692, - 0, - 0, - 0, - ] - external_forces_array[:, 4] = [ - 0.456069984217036, - 0.785175961393014, - 0.19967378215836, - 0.514234438413612, - 0.592414568862042, - 0.046450412719998, - 0, - 0, - 0, - ] external_forces = ExternalForceSetTimeSeries(nb_frames=nlp.ns) # external_forces.add_torque("Seg0", external_forces_array[:3, :]) # external_forces.add_translational_force("Seg0", external_forces_array[3:6, :]) - external_forces.add("Seg0", external_forces_array[:6, :]) + external_forces.add("Seg0", EXTERNAL_FORCE_ARRAY[:6, :], point_of_application=EXTERNAL_FORCE_ARRAY[6:, :]) numerical_time_series = {"external_forces": external_forces.to_numerical_time_series()} nlp.model = BiorbdModel( @@ -228,7 +228,7 @@ def test_torque_driven_soft_contacts_dynamics(with_contact, cx, implicit_contact nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod" ) - nlp.ns = 5 + nlp.ns = N_SHOOTING nlp.cx = cx nlp.time_cx = cx.sym("time", 1, 1) nlp.dt = cx.sym("dt", 1, 1) @@ -304,83 +304,18 @@ def test_torque_driven_soft_contacts_dynamics(with_contact, cx, implicit_contact def test_torque_derivative_driven(with_contact, with_external_force, cx, phase_dynamics): # Prepare the program nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx == SX)) - nlp.ns = 5 + nlp.ns = N_SHOOTING external_forces = None + numerical_timeseries = None if with_external_force: - external_forces_array = np.zeros((9, nlp.ns + 1)) - external_forces_array[:, 0] = [ - 0.3745401188473625, - 0.9507143064099162, - 0.7319939418114051, - 0.5986584841970366, - 0.15601864044243652, - 0.15599452033620265, - 0, - 0, - 0, - ] - external_forces_array[:, 1] = [ - 0.05808361216819946, - 0.8661761457749352, - 0.6011150117432088, - 0.7080725777960455, - 0.020584494295802447, - 0.9699098521619943, - 0, - 0, - 0, - ] - external_forces_array[:, 2] = [ - 0.8324426408004217, - 0.21233911067827616, - 0.18182496720710062, - 0.18340450985343382, - 0.3042422429595377, - 0.5247564316322378, - 0, - 0, - 0, - ] - external_forces_array[:, 3] = [ - 0.43194501864211576, - 0.2912291401980419, - 0.6118528947223795, - 0.13949386065204183, - 0.29214464853521815, - 0.3663618432936917, - 0, - 0, - 0, - ] - external_forces_array[:, 4] = [ - 0.45606998421703593, - 0.7851759613930136, - 0.19967378215835974, - 0.5142344384136116, - 0.5924145688620425, - 0.046450412719997725, - 0, - 0, - 0, - ] - external_forces = ExternalForces() - external_forces.add( - key="Seg0", - data=external_forces_array[:3, :], - force_type=ExternalForceType.TORQUE, - force_reference_frame=ReferenceFrame.GLOBAL, - ) - external_forces.add( - key="Seg0", - data=external_forces_array[3:6, :], - force_type=ExternalForceType.FORCE, - force_reference_frame=ReferenceFrame.GLOBAL, - ) + external_forces = ExternalForceSetTimeSeries(nb_frames=nlp.ns) + external_forces.add("Seg0", EXTERNAL_FORCE_ARRAY[:6, :], point_of_application=EXTERNAL_FORCE_ARRAY[6:, :]) + numerical_timeseries = {"external_forces": external_forces.to_numerical_time_series()} nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod", - external_forces=external_forces, + external_force_set=external_forces, ) nlp.cx = cx nlp.time_cx = cx.sym("time", 1, 1) @@ -404,7 +339,7 @@ def test_torque_derivative_driven(with_contact, with_external_force, cx, phase_d with_contact=with_contact, expand_dynamics=True, phase_dynamics=phase_dynamics, - external_forces=external_forces, + numerical_data_timeseries=numerical_timeseries, ), False, ) @@ -430,7 +365,7 @@ def test_torque_derivative_driven(with_contact, with_external_force, cx, phase_d controls = np.random.rand(nlp.controls.shape, nlp.ns) params = np.random.rand(nlp.parameters.shape, nlp.ns) algebraic_states = np.random.rand(nlp.algebraic_states.shape, nlp.ns) - numerical_timeseries = external_forces_array[:, 0] if with_external_force else [] + numerical_timeseries = EXTERNAL_FORCE_ARRAY[:, 0] if with_external_force else [] time = np.random.rand(2) x_out = np.array( nlp.dynamics_func( @@ -530,7 +465,7 @@ def test_torque_derivative_driven_soft_contacts_dynamics(with_contact, cx, impli nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod" ) - nlp.ns = 5 + nlp.ns = N_SHOOTING nlp.cx = cx nlp.time_cx = cx.sym("time", 1, 1) nlp.dt = cx.sym("dt", 1, 1) @@ -635,7 +570,7 @@ def test_soft_contacts_dynamics_errors(dynamics, phase_dynamics): nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod" ) - nlp.ns = 5 + nlp.ns = N_SHOOTING nlp.cx = MX nlp.u_bounds = np.zeros((nlp.model.nb_q * 4, 1)) @@ -673,83 +608,18 @@ def test_soft_contacts_dynamics_errors(dynamics, phase_dynamics): def test_torque_activation_driven(with_contact, with_external_force, cx, phase_dynamics): # Prepare the program nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx == SX)) - nlp.ns = 5 + nlp.ns = N_SHOOTING external_forces = None + numerical_timeseries = None if with_external_force: - external_forces_array = np.zeros((9, nlp.ns + 1)) - external_forces_array[:, 0] = [ - 0.3745401188473625, - 0.9507143064099162, - 0.7319939418114051, - 0.5986584841970366, - 0.15601864044243652, - 0.15599452033620265, - 0, - 0, - 0, - ] - external_forces_array[:, 1] = [ - 0.05808361216819946, - 0.8661761457749352, - 0.6011150117432088, - 0.7080725777960455, - 0.020584494295802447, - 0.9699098521619943, - 0, - 0, - 0, - ] - external_forces_array[:, 2] = [ - 0.8324426408004217, - 0.21233911067827616, - 0.18182496720710062, - 0.18340450985343382, - 0.3042422429595377, - 0.5247564316322378, - 0, - 0, - 0, - ] - external_forces_array[:, 3] = [ - 0.43194501864211576, - 0.2912291401980419, - 0.6118528947223795, - 0.13949386065204183, - 0.29214464853521815, - 0.3663618432936917, - 0, - 0, - 0, - ] - external_forces_array[:, 4] = [ - 0.45606998421703593, - 0.7851759613930136, - 0.19967378215835974, - 0.5142344384136116, - 0.5924145688620425, - 0.046450412719997725, - 0, - 0, - 0, - ] - external_forces = ExternalForces() - external_forces.add( - key="Seg0", - data=external_forces_array[:3, :], - force_type=ExternalForceType.TORQUE, - force_reference_frame=ReferenceFrame.GLOBAL, - ) - external_forces.add( - key="Seg0", - data=external_forces_array[3:6, :], - force_type=ExternalForceType.FORCE, - force_reference_frame=ReferenceFrame.GLOBAL, - ) + external_forces = ExternalForceSetTimeSeries(nb_frames=nlp.ns) + external_forces.add("Seg0", EXTERNAL_FORCE_ARRAY[:6, :], point_of_application=EXTERNAL_FORCE_ARRAY[6:, :]) + numerical_timeseries = {"external_forces": external_forces.to_numerical_time_series()} nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod", - external_forces=external_forces, + external_force_set=external_forces, ) nlp.cx = cx nlp.time_cx = cx.sym("time", 1, 1) @@ -773,7 +643,7 @@ def test_torque_activation_driven(with_contact, with_external_force, cx, phase_d with_contact=with_contact, expand_dynamics=True, phase_dynamics=phase_dynamics, - external_forces=external_forces, + numerical_data_timeseries=numerical_timeseries, ), False, ) @@ -798,7 +668,7 @@ def test_torque_activation_driven(with_contact, with_external_force, cx, phase_d controls = np.random.rand(nlp.controls.shape, nlp.ns) params = np.random.rand(nlp.parameters.shape, nlp.ns) algebraic_states = np.random.rand(nlp.algebraic_states.shape, nlp.ns) - numerical_timeseries = external_forces_array[:, 0] if with_external_force else [] + numerical_timeseries = EXTERNAL_FORCE_ARRAY[:, 0] if with_external_force else [] time = np.random.rand(2) x_out = np.array( nlp.dynamics_func( @@ -868,83 +738,18 @@ def test_torque_activation_driven_with_residual_torque( ): # Prepare the program nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx == SX)) - nlp.ns = 5 + nlp.ns = N_SHOOTING external_forces = None + numerical_timeseries = None if with_external_force: - external_forces_array = np.zeros((9, nlp.ns + 1)) - external_forces_array[:, 0] = [ - 0.3745401188473625, - 0.9507143064099162, - 0.7319939418114051, - 0.5986584841970366, - 0.15601864044243652, - 0.15599452033620265, - 0, - 0, - 0, - ] - external_forces_array[:, 1] = [ - 0.05808361216819946, - 0.8661761457749352, - 0.6011150117432088, - 0.7080725777960455, - 0.020584494295802447, - 0.9699098521619943, - 0, - 0, - 0, - ] - external_forces_array[:, 2] = [ - 0.8324426408004217, - 0.21233911067827616, - 0.18182496720710062, - 0.18340450985343382, - 0.3042422429595377, - 0.5247564316322378, - 0, - 0, - 0, - ] - external_forces_array[:, 3] = [ - 0.43194501864211576, - 0.2912291401980419, - 0.6118528947223795, - 0.13949386065204183, - 0.29214464853521815, - 0.3663618432936917, - 0, - 0, - 0, - ] - external_forces_array[:, 4] = [ - 0.45606998421703593, - 0.7851759613930136, - 0.19967378215835974, - 0.5142344384136116, - 0.5924145688620425, - 0.046450412719997725, - 0, - 0, - 0, - ] - external_forces = ExternalForces() - external_forces.add( - key="Seg0", - data=external_forces_array[:3, :], - force_type=ExternalForceType.TORQUE, - force_reference_frame=ReferenceFrame.GLOBAL, - ) - external_forces.add( - key="Seg0", - data=external_forces_array[3:6, :], - force_type=ExternalForceType.FORCE, - force_reference_frame=ReferenceFrame.GLOBAL, - ) + external_forces = ExternalForceSetTimeSeries(nb_frames=nlp.ns) + external_forces.add("Seg0", EXTERNAL_FORCE_ARRAY[:6, :], point_of_application=EXTERNAL_FORCE_ARRAY[6:, :]) + numerical_timeseries = {"external_forces": external_forces.to_numerical_time_series()} nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/torque_driven_ocp/models/2segments_2dof_2contacts.bioMod", - external_forces=external_forces, + external_force_set=external_forces, ) nlp.cx = cx nlp.time_cx = cx.sym("time", 1, 1) @@ -967,7 +772,7 @@ def test_torque_activation_driven_with_residual_torque( with_residual_torque=with_residual_torque, expand_dynamics=True, phase_dynamics=phase_dynamics, - external_forces=external_forces, + numerical_data_timeseries=numerical_timeseries, ), False, ) @@ -992,7 +797,7 @@ def test_torque_activation_driven_with_residual_torque( controls = np.random.rand(nlp.controls.shape, nlp.ns) params = np.random.rand(nlp.parameters.shape, nlp.ns) algebraic_states = np.random.rand(nlp.algebraic_states.shape, nlp.ns) - numerical_timeseries = external_forces_array[:, 0] if with_external_force else [] + numerical_timeseries = EXTERNAL_FORCE_ARRAY[:, 0] if with_external_force else [] time = np.random.rand(2) x_out = np.array( nlp.dynamics_func( @@ -1065,7 +870,7 @@ def test_torque_driven_free_floating_base(cx, phase_dynamics): nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod" ) - nlp.ns = 5 + nlp.ns = N_SHOOTING nlp.cx = cx nlp.time_cx = cx.sym("time", 1, 1) nlp.dt = cx.sym("dt", 1, 1) @@ -1125,83 +930,22 @@ def test_torque_driven_free_floating_base(cx, phase_dynamics): def test_muscle_driven(with_excitations, with_contact, with_residual_torque, with_external_force, cx, phase_dynamics): # Prepare the program nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx == SX)) - nlp.ns = 5 + nlp.ns = N_SHOOTING external_forces = None + numerical_timeseries = None if with_external_force: - external_forces_array = np.zeros((9, nlp.ns + 1)) - external_forces_array[:, 0] = [ - 0.3745401188473625, - 0.9507143064099162, - 0.7319939418114051, - 0.5986584841970366, - 0.15601864044243652, - 0.15599452033620265, - 0, - 0, - 0, - ] - external_forces_array[:, 1] = [ - 0.05808361216819946, - 0.8661761457749352, - 0.6011150117432088, - 0.7080725777960455, - 0.020584494295802447, - 0.9699098521619943, - 0, - 0, - 0, - ] - external_forces_array[:, 2] = [ - 0.8324426408004217, - 0.21233911067827616, - 0.18182496720710062, - 0.18340450985343382, - 0.3042422429595377, - 0.5247564316322378, - 0, - 0, - 0, - ] - external_forces_array[:, 3] = [ - 0.43194501864211576, - 0.2912291401980419, - 0.6118528947223795, - 0.13949386065204183, - 0.29214464853521815, - 0.3663618432936917, - 0, - 0, - 0, - ] - external_forces_array[:, 4] = [ - 0.45606998421703593, - 0.7851759613930136, - 0.19967378215835974, - 0.5142344384136116, - 0.5924145688620425, - 0.046450412719997725, - 0, - 0, - 0, - ] - external_forces = ExternalForces() - external_forces.add( - key="r_ulna_radius_hand_rotation1", - data=external_forces_array[:3, :], - force_type=ExternalForceType.TORQUE, - force_reference_frame=ReferenceFrame.GLOBAL, - ) + external_forces = ExternalForceSetTimeSeries(nb_frames=nlp.ns) external_forces.add( - key="r_ulna_radius_hand_rotation1", - data=external_forces_array[3:6, :], - force_type=ExternalForceType.FORCE, - force_reference_frame=ReferenceFrame.GLOBAL, + "r_ulna_radius_hand_rotation1", + EXTERNAL_FORCE_ARRAY[:6, :], + point_of_application=EXTERNAL_FORCE_ARRAY[6:, :], ) + numerical_timeseries = {"external_forces": external_forces.to_numerical_time_series()} nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/muscle_driven_ocp/models/arm26_with_contact.bioMod", - external_forces=external_forces, + external_force_set=external_forces, ) nlp.cx = cx @@ -1229,7 +973,7 @@ def test_muscle_driven(with_excitations, with_contact, with_residual_torque, wit with_contact=with_contact, expand_dynamics=True, phase_dynamics=phase_dynamics, - external_forces=external_forces, + numerical_data_timeseries=numerical_timeseries, ), False, ) @@ -1255,7 +999,7 @@ def test_muscle_driven(with_excitations, with_contact, with_residual_torque, wit controls = np.random.rand(nlp.controls.shape, nlp.ns) params = np.random.rand(nlp.parameters.shape, nlp.ns) algebraic_states = np.random.rand(nlp.algebraic_states.shape, nlp.ns) - numerical_timeseries = external_forces_array[:, 0] if with_external_force else [] + numerical_timeseries = EXTERNAL_FORCE_ARRAY[:, 0] if with_external_force else [] time = np.random.rand(2) x_out = np.array( nlp.dynamics_func( @@ -1384,7 +1128,7 @@ def test_joints_acceleration_driven(cx, phase_dynamics): nlp = NonLinearProgram(phase_dynamics=phase_dynamics, use_sx=(cx == SX)) nlp.model = BiorbdModel(TestUtils.bioptim_folder() + "/examples/getting_started/models/double_pendulum.bioMod") - nlp.ns = 5 + nlp.ns = N_SHOOTING nlp.cx = cx nlp.time_cx = cx.sym("time", 1, 1) nlp.dt = cx.sym("dt", 1, 1) @@ -1466,7 +1210,7 @@ def configure(ocp, nlp, with_contact=None, numerical_data_timeseries=None): nlp.model = BiorbdModel( TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod" ) - nlp.ns = 5 + nlp.ns = N_SHOOTING nlp.cx = MX nlp.time_cx = nlp.cx.sym("time", 1, 1) nlp.dt = nlp.cx.sym("dt", 1, 1) diff --git a/tests/shard3/test_external_force_class.py b/tests/shard3/test_external_force_class.py index 6792fe05b..3bd8ae382 100644 --- a/tests/shard3/test_external_force_class.py +++ b/tests/shard3/test_external_force_class.py @@ -108,7 +108,7 @@ def test_external_forces_components_calculation(external_forces): external_forces.add_translational_force(segment1, np.random.rand(3, 10)) # The actual calculation depends on implementation details - assert external_forces.nb_external_forces_components == 9 + 6 + 6 + assert external_forces.nb_external_forces_components == 9 + 3 + 6 assert external_forces.nb_external_forces == 3 @@ -186,7 +186,7 @@ def test_success_within_biomod(external_forces): external_force_set=external_forces, ) - assert model.external_forces.shape == (15, 1) + assert model.external_forces.shape == (9 + 3, 1) assert model.biorbd_external_forces_set is not None with pytest.raises(RuntimeError, match="External forces have been binded and cannot be modified anymore."): diff --git a/tests/shard3/test_global_getting_started.py b/tests/shard3/test_global_getting_started.py index f310d09d5..54c8911b9 100644 --- a/tests/shard3/test_global_getting_started.py +++ b/tests/shard3/test_global_getting_started.py @@ -4,14 +4,15 @@ import os import pickle +import platform import re import shutil -import platform -import pytest import numpy as np import numpy.testing as npt +import pytest from casadi import sum1, sum2 + from bioptim import ( InterpolationType, OdeSolver, @@ -21,9 +22,7 @@ ControlType, PhaseDynamics, SolutionMerge, - __version__, ) - from tests.utils import TestUtils @@ -561,8 +560,6 @@ def test_phase_transitions(ode_solver, phase_dynamics): @pytest.mark.parametrize("phase_dynamics", [PhaseDynamics.SHARED_DURING_THE_PHASE, PhaseDynamics.ONE_PER_NODE]) @pytest.mark.parametrize("ode_solver", [OdeSolver.RK4, OdeSolver.RK8, OdeSolver.COLLOCATION]) # OdeSolver.IRK def test_parameter_optimization(ode_solver, phase_dynamics): - from bioptim.examples.getting_started import custom_parameters as ocp_module - return # TODO: Fix parameter scaling :( # For reducing time phase_dynamics == PhaseDynamics.ONE_PER_NODE is skipped for redundant tests if phase_dynamics == PhaseDynamics.ONE_PER_NODE and ode_solver in (OdeSolver.RK8, OdeSolver.COLLOCATION): From 0313d4a0feed4b38aa4cd3ef8376922c9d699273 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Fri, 1 Nov 2024 10:34:43 -0400 Subject: [PATCH 098/108] clean and fix --- tests/shard1/test_dynamics.py | 4 +--- tests/shard3/test_external_forces.py | 1 + 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/tests/shard1/test_dynamics.py b/tests/shard1/test_dynamics.py index f3fd78187..1910cca7a 100644 --- a/tests/shard1/test_dynamics.py +++ b/tests/shard1/test_dynamics.py @@ -114,8 +114,6 @@ def test_torque_driven(with_contact, with_external_force, cx, phase_dynamics): if with_external_force: external_forces = ExternalForceSetTimeSeries(nb_frames=nlp.ns) - # external_forces.add_torque("Seg0", external_forces_array[:3, :]) - # external_forces.add_translational_force("Seg0", external_forces_array[3:6, :]) external_forces.add("Seg0", EXTERNAL_FORCE_ARRAY[:6, :], point_of_application=EXTERNAL_FORCE_ARRAY[6:, :]) numerical_time_series = {"external_forces": external_forces.to_numerical_time_series()} @@ -171,7 +169,7 @@ def test_torque_driven(with_contact, with_external_force, cx, phase_dynamics): controls = np.random.rand(nlp.controls.shape, nlp.ns) params = np.random.rand(nlp.parameters.shape, nlp.ns) algebraic_states = np.random.rand(nlp.algebraic_states.shape, nlp.ns) - numerical_timeseries = external_forces_array[:, 0] if with_external_force else [] + numerical_timeseries = EXTERNAL_FORCE_ARRAY[:, 0] if with_external_force else [] time = np.random.rand(2) x_out = np.array( nlp.dynamics_func( diff --git a/tests/shard3/test_external_forces.py b/tests/shard3/test_external_forces.py index 20f73fb3c..fea7fa1e8 100644 --- a/tests/shard3/test_external_forces.py +++ b/tests/shard3/test_external_forces.py @@ -59,6 +59,7 @@ def test_example_external_forces( phase_dynamics=phase_dynamics, external_force_method=method, use_sx=use_sx, + use_point_of_applications= method == "translational_force", # Only to preserve the tested values ) sol = ocp.solve() From 1d75f7148f4c47af2f8694c3901944c86767c95b Mon Sep 17 00:00:00 2001 From: Ipuch Date: Fri, 1 Nov 2024 11:00:44 -0400 Subject: [PATCH 099/108] feat: marker assignement works fine --- .../examples/getting_started/example_external_forces.py | 6 ++++++ bioptim/models/biorbd/biorbd_model.py | 2 +- tests/shard3/test_external_forces.py | 8 +++++++- 3 files changed, 14 insertions(+), 2 deletions(-) diff --git a/bioptim/examples/getting_started/example_external_forces.py b/bioptim/examples/getting_started/example_external_forces.py index 89ba3078a..f7b8c10e5 100644 --- a/bioptim/examples/getting_started/example_external_forces.py +++ b/bioptim/examples/getting_started/example_external_forces.py @@ -104,6 +104,12 @@ def prepare_ocp( Test_force, point_of_application_in_local=Test_point_of_application if use_point_of_applications else None, ) + elif external_force_method == "translational_force_on_a_marker": + external_force_set.add_translational_force( + "Test", + Test_force, + point_of_application_in_local="m0", # The point of application is the marker + ) elif external_force_method == "in_global": # not mandatory to specify the point of application external_force_set.add( diff --git a/bioptim/models/biorbd/biorbd_model.py b/bioptim/models/biorbd/biorbd_model.py index 6db010189..8026bd7a5 100644 --- a/bioptim/models/biorbd/biorbd_model.py +++ b/bioptim/models/biorbd/biorbd_model.py @@ -477,7 +477,7 @@ def _dispatch_forces(self): point_of_application = self.external_forces[slice(stop, stop + 3)] elif str_point_of_application: # turn it into a NodeSegment, might not work yet. - point_of_application = biorbd.NodeSegment(force["point_of_application"]) + point_of_application = self.model.marker(self.marker_index(force["point_of_application"])) else: point_of_application = None diff --git a/tests/shard3/test_external_forces.py b/tests/shard3/test_external_forces.py index fea7fa1e8..f9bcdad57 100644 --- a/tests/shard3/test_external_forces.py +++ b/tests/shard3/test_external_forces.py @@ -43,6 +43,7 @@ "in_global_torque", "in_segment_torque", "in_segment", + "translational_force_on_a_marker", ], ) def test_example_external_forces( @@ -59,7 +60,7 @@ def test_example_external_forces( phase_dynamics=phase_dynamics, external_force_method=method, use_sx=use_sx, - use_point_of_applications= method == "translational_force", # Only to preserve the tested values + use_point_of_applications=method == "translational_force", # Only to preserve the tested values ) sol = ocp.solve() @@ -120,6 +121,11 @@ def test_example_external_forces( npt.assert_almost_equal(q[:, 0], np.array([-4.64041726e-15, 6.99774094e-16, 4.75924448e-11, 0.0]), decimal=5) npt.assert_almost_equal(q[:, -1], np.array([-4.74298258e-15, 2.00000000e00, 3.79354612e-11, 0.0]), decimal=5) + if method == "translational_force_on_a_marker": + npt.assert_almost_equal(q[:, 0], np.array([-4.69167e-15, 7.80151e-16, -1.30653e-17, 0.00000e00]), decimal=5) + npt.assert_almost_equal(q[:, -1], np.array([-4.69169e-15, 2.00000e00, -1.30653e-17, 0.00000e00]), decimal=5) + npt.assert_almost_equal(qdot[:, 15], np.array([-3.39214e-19, 1.42151e00, -3.35346e-26, 0.00000e00]), decimal=5) + def prepare_ocp( biorbd_model_path: str = "models/cube_with_forces.bioMod", From 57858072436bbf50a1a2ee18cccde02d8e373efa Mon Sep 17 00:00:00 2001 From: Ipuch Date: Fri, 1 Nov 2024 11:27:10 -0400 Subject: [PATCH 100/108] refactor: force_dispatch --- bioptim/models/biorbd/biorbd_model.py | 132 +++++++++++++++-------- bioptim/models/biorbd/external_forces.py | 22 ++++ 2 files changed, 109 insertions(+), 45 deletions(-) diff --git a/bioptim/models/biorbd/biorbd_model.py b/bioptim/models/biorbd/biorbd_model.py index 8026bd7a5..17125ad56 100644 --- a/bioptim/models/biorbd/biorbd_model.py +++ b/bioptim/models/biorbd/biorbd_model.py @@ -10,7 +10,14 @@ ) from casadi import SX, MX, vertcat, horzcat, norm_fro, Function -from bioptim.models.biorbd.external_forces import ExternalForceSetTimeSeries +from bioptim.models.biorbd.external_forces import ( + ExternalForceSetTimeSeries, + _add_global_force, + _add_torque_global, + _add_translational_global, + _add_local_force, + _add_torque_local, +) from ..utils import _var_mapping, bounds_from_ranges from ...limits.path_conditions import Bounds from ...misc.mapping import BiMapping, BiMappingList @@ -433,59 +440,94 @@ def forward_dynamics_free_floating_base(self) -> Function: def reorder_qddot_root_joints(qddot_root, qddot_joints) -> MX | SX: return vertcat(qddot_root, qddot_joints) - def _dispatch_forces(self): + def _dispatch_forces(self) -> biorbd.ExternalForceSet: """Dispatch the symbolic MX into the biorbd external forces object""" biorbd_external_forces = self.model.externalForceSet() - # "type of external force": (function to call, number of force components, number of point of application components) - bioptim_to_biorbd_map = { - "in_global": (biorbd_external_forces.add, 6), - "torque_in_global": ( - lambda segment, torque, point_of_application: biorbd_external_forces.add( - segment, vertcat(torque, MX([0, 0, 0])), MX([0, 0, 0]) - ), - 3, - ), - "translational_in_global": ( - lambda segment, force, point_of_application: biorbd_external_forces.addTranslationalForce( - force, segment, point_of_application - ), - 3, - ), - "in_local": (biorbd_external_forces.addInSegmentReferenceFrame, 6), - "torque_in_local": ( - lambda segment, torque, point_of_application: biorbd_external_forces.addInSegmentReferenceFrame( - segment, vertcat(torque, MX([0, 0, 0])), MX([0, 0, 0]) - ), - 3, - ), + # "type of external force": (function to call, number of force components) + force_mapping = { + "in_global": (_add_global_force, 6), + "torque_in_global": (_add_torque_global, 3), + "translational_in_global": (_add_translational_global, 3), + "in_local": (_add_local_force, 6), + "torque_in_local": (_add_torque_local, 3), } symbolic_counter = 0 - for attr in bioptim_to_biorbd_map.keys(): - function = bioptim_to_biorbd_map[attr][0] - for segment, forces_on_segment in getattr(self.external_force_set, attr).items(): - for force in forces_on_segment: - array_point_of_application = isinstance(force["point_of_application"], np.ndarray) - str_point_of_application = isinstance(force["point_of_application"], str) - - start = symbolic_counter - stop = symbolic_counter + bioptim_to_biorbd_map[attr][1] - force_slicer = slice(start, stop) - - if array_point_of_application: - point_of_application = self.external_forces[slice(stop, stop + 3)] - elif str_point_of_application: - # turn it into a NodeSegment, might not work yet. - point_of_application = self.model.marker(self.marker_index(force["point_of_application"])) - else: - point_of_application = None - - function(segment, self.external_forces[force_slicer], point_of_application) - symbolic_counter = stop + 3 if array_point_of_application else stop + for force_type, val in force_mapping.items(): + add_force_func, num_force_components = val + symbolic_counter = self._dispatch_forces_of_type( + force_type, add_force_func, num_force_components, symbolic_counter, biorbd_external_forces + ) return biorbd_external_forces + def _dispatch_forces_of_type( + self, + force_type: str, + add_force_func: "Callable", + num_force_components: int, + symbolic_counter: int, + biorbd_external_forces: "biorbd.ExternalForces", + ) -> int: + """ + Helper method to dispatch forces of a specific external forces. + + Parameters + ---------- + force_type: str + The type of external force to dispatch among in_global, torque_in_global, translational_in_global, in_local, torque_in_local. + add_force_func: Callable + The function to call to add the force to the biorbd external forces object. + num_force_components: int + The number of force components for the given type + symbolic_counter: int + The current symbolic counter to slice the whole external_forces mx. + biorbd_external_forces: biorbd.ExternalForces + The biorbd external forces object to add the forces to. + + Returns + ------- + int + The updated symbolic counter. + """ + for segment, forces_on_segment in getattr(self.external_force_set, force_type).items(): + for force in forces_on_segment: + force_slicer = slice(symbolic_counter, symbolic_counter + num_force_components) + + point_of_application_mx = self._get_point_of_application(force, force_slicer.stop) + + add_force_func( + biorbd_external_forces, segment, self.external_forces[force_slicer], point_of_application_mx + ) + symbolic_counter = force_slicer.stop + ( + 3 if isinstance(force["point_of_application"], np.ndarray) else 0 + ) + + return symbolic_counter + + def _get_point_of_application(self, force, stop_index) -> biorbd.NodeSegment | np.ndarray | None: + """ + Determine the point of application mx slice based on its type. Only sliced if an array is stored + + Parameters + ---------- + force : dict + The force dictionary with details on the point of application. + stop_index : int + Index position in MX where the point of application components start. + + Returns + ------- + biorbd.NodeSegment | np.ndarray | None + Returns a slice of MX, a marker node, or None if no point of application is defined. + """ + if isinstance(force["point_of_application"], np.ndarray): + return self.external_forces[slice(stop_index, stop_index + 3)] + elif isinstance(force["point_of_application"], str): + return self.model.marker(self.marker_index(force["point_of_application"])) + return None + def forward_dynamics(self, with_contact: bool = False) -> Function: q_biorbd = GeneralizedCoordinates(self.q) diff --git a/bioptim/models/biorbd/external_forces.py b/bioptim/models/biorbd/external_forces.py index a8572f217..766081002 100644 --- a/bioptim/models/biorbd/external_forces.py +++ b/bioptim/models/biorbd/external_forces.py @@ -1,4 +1,5 @@ import numpy as np +from casadi import MX, vertcat class ExternalForceSetTimeSeries: @@ -252,3 +253,24 @@ def ensure_list(data, key) -> dict[str, list]: if data.get(key) is None: data[key] = [] return data + + +# Specific functions for adding each force type to improve readability +def _add_global_force(biorbd_external_forces, segment, force, point_of_application): + biorbd_external_forces.add(segment, force, point_of_application) + + +def _add_torque_global(biorbd_external_forces, segment, torque, _): + biorbd_external_forces.add(segment, vertcat(torque, MX([0, 0, 0])), MX([0, 0, 0])) + + +def _add_translational_global(biorbd_external_forces, segment, force, point_of_application): + biorbd_external_forces.addTranslationalForce(force, segment, point_of_application) + + +def _add_local_force(biorbd_external_forces, segment, force, point_of_application): + biorbd_external_forces.addInSegmentReferenceFrame(segment, force, point_of_application) + + +def _add_torque_local(biorbd_external_forces, segment, torque, _): + biorbd_external_forces.addInSegmentReferenceFrame(segment, vertcat(torque, MX([0, 0, 0])), MX([0, 0, 0])) From 79cfcb8cb329088ddd07ff7057fc2b6cff82b7bc Mon Sep 17 00:00:00 2001 From: Ipuch Date: Fri, 1 Nov 2024 11:48:53 -0400 Subject: [PATCH 101/108] refactor: spring mass clean --- bioptim/examples/torque_driven_ocp/spring_load.py | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/bioptim/examples/torque_driven_ocp/spring_load.py b/bioptim/examples/torque_driven_ocp/spring_load.py index e51cbfbc1..ad392c091 100644 --- a/bioptim/examples/torque_driven_ocp/spring_load.py +++ b/bioptim/examples/torque_driven_ocp/spring_load.py @@ -147,11 +147,10 @@ def custom_dynamic( qdot = DynamicsFunctions.get(nlp.states["qdot"], states) tau = DynamicsFunctions.get(nlp.controls["tau"], controls) - force_vector = MX.zeros(9) stiffness = 100 - force_vector[5] = -sign(q[0]) * stiffness * q[0] ** 2 # traction-compression spring + tau += -sign(q[0]) * stiffness * q[0] # damping - qddot = nlp.model.forward_dynamics(with_contact=False)(q, qdot, tau, force_vector, []) + qddot = nlp.model.forward_dynamics(with_contact=False)(q, qdot, tau, [], []) return DynamicsEvaluation(dxdt=vertcat(qdot, qddot), defects=None) @@ -183,7 +182,7 @@ def prepare_ocp( ): # BioModel path - m = BiorbdModel(biorbd_model_path, nb_supplementary_forces_in_global=1) + m = BiorbdModel(biorbd_model_path) m.set_gravity(np.array((0, 0, 0))) weight = 1 From 49969a3095674e7c4ad20fddc4ec854c7490056f Mon Sep 17 00:00:00 2001 From: Ipuch Date: Fri, 1 Nov 2024 11:57:31 -0400 Subject: [PATCH 102/108] refactor: slight displacing variable in a method blacked --- bioptim/models/biorbd/biorbd_model.py | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/bioptim/models/biorbd/biorbd_model.py b/bioptim/models/biorbd/biorbd_model.py index 17125ad56..201652325 100644 --- a/bioptim/models/biorbd/biorbd_model.py +++ b/bioptim/models/biorbd/biorbd_model.py @@ -48,12 +48,17 @@ def __init__( parameters[param_key].apply_parameter(self) self._friction_coefficients = friction_coefficients - # External forces self.external_force_set = ( self._set_external_force_set(external_force_set) if external_force_set is not None else None ) + self._symbolic_variables() + self.biorbd_external_forces_set = self._dispatch_forces() if external_force_set else None + + # TODO: remove mx (the MX parameters should be created inside the BiorbdModel) + self.parameters = parameters.mx if parameters else MX() - # Declaration of MX variables of the right shape for the creation of CasADi Functions + def _symbolic_variables(self): + """Declaration of MX variables of the right shape for the creation of CasADi Functions""" self.q = MX.sym("q_mx", self.nb_q, 1) self.qdot = MX.sym("qdot_mx", self.nb_qdot, 1) self.qddot = MX.sym("qddot_mx", self.nb_qddot, 1) @@ -62,12 +67,10 @@ def __init__( self.muscle = MX.sym("muscle_mx", self.nb_muscles, 1) self.activations = MX.sym("activations_mx", self.nb_muscles, 1) self.external_forces = MX.sym( - "external_forces_mx", external_force_set.nb_external_forces_components if external_force_set else 0, 1 + "external_forces_mx", + self.external_force_set.nb_external_forces_components if self.external_force_set else 0, + 1, ) - self.biorbd_external_forces_set = self._dispatch_forces() if external_force_set else None - - # TODO: remove mx (the MX parameters should be created inside the BiorbdModel) - self.parameters = parameters.mx if parameters else MX() def _set_external_force_set(self, external_force_set: ExternalForceSetTimeSeries): """ From 7812c0ae20f02dff1992df4a0eca5ac065568209 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Fri, 1 Nov 2024 12:08:20 -0400 Subject: [PATCH 103/108] Auto stash before checking out "HEAD" docs: variational model --- .../models/biorbd/variational_biorbd_model.py | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/bioptim/models/biorbd/variational_biorbd_model.py b/bioptim/models/biorbd/variational_biorbd_model.py index a143e3ca8..39c8ff596 100644 --- a/bioptim/models/biorbd/variational_biorbd_model.py +++ b/bioptim/models/biorbd/variational_biorbd_model.py @@ -24,6 +24,22 @@ def __init__( control_discrete_approximation: QuadratureRule = QuadratureRule.MIDPOINT, parameters: ParameterList = None, ): + """ + Initialize the VariationalBiorbdModel. + + Parameters + ---------- + bio_model : str | biorbd.Model + The path to the biorbd model file or a biorbd.Model instance. + discrete_approximation : QuadratureRule, optional + The quadrature rule for the discrete approximation (default is QuadratureRule.TRAPEZOIDAL). + control_type : ControlType, optional + The type of control to be used (default is ControlType.CONSTANT). + control_discrete_approximation : QuadratureRule, optional + The quadrature rule for the control discrete approximation (default is QuadratureRule.MIDPOINT). + parameters : ParameterList, optional + A list of parameters for the model (default is None). + """ super().__init__(bio_model, parameters=parameters) self.discrete_approximation = discrete_approximation self.control_type = control_type From bb1dfe7345db62488d9108cbd595cd954f934926 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Fri, 1 Nov 2024 13:07:21 -0400 Subject: [PATCH 104/108] refactor: example --- .../example_external_forces.py | 126 +++++++++--------- 1 file changed, 65 insertions(+), 61 deletions(-) diff --git a/bioptim/examples/getting_started/example_external_forces.py b/bioptim/examples/getting_started/example_external_forces.py index f7b8c10e5..3d5d664d8 100644 --- a/bioptim/examples/getting_started/example_external_forces.py +++ b/bioptim/examples/getting_started/example_external_forces.py @@ -70,6 +70,62 @@ def prepare_ocp( n_shooting = 30 final_time = 2 + # External forces + external_force_set = setup_external_forces(n_shooting, external_force_method, use_point_of_applications) + + bio_model = BiorbdModel(biorbd_model_path, external_force_set=external_force_set) + + # Add objective functions + objective_functions = ObjectiveList() + objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100) + + # Dynamics + numerical_time_series = {"external_forces": external_force_set.to_numerical_time_series()} + + dynamics = DynamicsList() + dynamics.add( + DynamicsFcn.TORQUE_DRIVEN, + expand_dynamics=expand_dynamics, + phase_dynamics=phase_dynamics, + numerical_data_timeseries=numerical_time_series, + ) + + # Constraints + constraints = ConstraintList() + constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.START, first_marker="m0", second_marker="m1") + constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m2") + + # Path constraint + x_bounds = BoundsList() + x_bounds["q"] = bio_model.bounds_from_ranges("q") + x_bounds["q"][3, [0, -1]] = 0 + x_bounds["qdot"] = bio_model.bounds_from_ranges("qdot") + x_bounds["qdot"][:3, [0, -1]] = 0 + + # Define control path constraint + u_bounds = BoundsList() + tau_min, tau_max = -100, 100 + u_bounds["tau"] = [tau_min] * bio_model.nb_tau, [tau_max] * bio_model.nb_tau + + return OptimalControlProgram( + bio_model, + dynamics, + n_shooting, + final_time, + x_bounds=x_bounds, + u_bounds=u_bounds, + objective_functions=objective_functions, + constraints=constraints, + ode_solver=ode_solver, + n_threads=n_threads, + use_sx=use_sx, + ) + + +def setup_external_forces( + n_shooting: int, external_force_method: str, use_point_of_applications: bool +) -> ExternalForceSetTimeSeries: + """Configure external forces based on the specified method.""" Seg1_force = np.zeros((3, n_shooting)) Seg1_force[2, :] = -2 Seg1_force[2, 4] = -22 @@ -88,12 +144,10 @@ def prepare_ocp( Test_point_of_application[1, :] = 0.01 Test_point_of_application[2, :] = -0.01 - external_force_set = ExternalForceSetTimeSeries( - nb_frames=n_shooting, - ) - # Displays all the possible combinations of external forces + external_force_set = ExternalForceSetTimeSeries(nb_frames=n_shooting) + + # Add appropriate forces based on method if external_force_method == "translational_force": - # not mandatory to specify the point of application external_force_set.add_translational_force( "Seg1", Seg1_force, @@ -104,14 +158,11 @@ def prepare_ocp( Test_force, point_of_application_in_local=Test_point_of_application if use_point_of_applications else None, ) + elif external_force_method == "translational_force_on_a_marker": - external_force_set.add_translational_force( - "Test", - Test_force, - point_of_application_in_local="m0", # The point of application is the marker - ) + external_force_set.add_translational_force("Test", Test_force, point_of_application_in_local="m0") + elif external_force_method == "in_global": - # not mandatory to specify the point of application external_force_set.add( "Seg1", np.concatenate((Seg1_force, Seg1_force), axis=0), @@ -123,65 +174,18 @@ def prepare_ocp( point_of_application=Test_point_of_application if use_point_of_applications else None, ) elif external_force_method == "in_global_torque": - # cannot specify the point of application as it is a pure torque external_force_set.add_torque("Seg1", Seg1_force) external_force_set.add_torque("Test", Test_force) + elif external_force_method == "in_segment_torque": - # cannot specify the point of application as it is a pure torque external_force_set.add_torque_in_segment_frame("Seg1", Seg1_force) external_force_set.add_torque_in_segment_frame("Test", Test_force) + elif external_force_method == "in_segment": - # not mandatory to specify the point of application external_force_set.add_in_segment_frame("Seg1", np.concatenate((Seg1_force, Seg1_force), axis=0)) external_force_set.add_in_segment_frame("Test", np.concatenate((Test_force, Test_force), axis=0)) - bio_model = BiorbdModel(biorbd_model_path, external_force_set=external_force_set) - - # Add objective functions - objective_functions = ObjectiveList() - objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100) - - # Dynamics - numerical_time_series = {"external_forces": external_force_set.to_numerical_time_series()} - - dynamics = DynamicsList() - dynamics.add( - DynamicsFcn.TORQUE_DRIVEN, - expand_dynamics=expand_dynamics, - phase_dynamics=phase_dynamics, - numerical_data_timeseries=numerical_time_series, - ) - - # Constraints - constraints = ConstraintList() - constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.START, first_marker="m0", second_marker="m1") - constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m2") - - # Path constraint - x_bounds = BoundsList() - x_bounds["q"] = bio_model.bounds_from_ranges("q") - x_bounds["q"][3, [0, -1]] = 0 - x_bounds["qdot"] = bio_model.bounds_from_ranges("qdot") - x_bounds["qdot"][:3, [0, -1]] = 0 - - # Define control path constraint - u_bounds = BoundsList() - tau_min, tau_max = -100, 100 - u_bounds["tau"] = [tau_min] * bio_model.nb_tau, [tau_max] * bio_model.nb_tau - - return OptimalControlProgram( - bio_model, - dynamics, - n_shooting, - final_time, - x_bounds=x_bounds, - u_bounds=u_bounds, - objective_functions=objective_functions, - constraints=constraints, - ode_solver=ode_solver, - n_threads=n_threads, - use_sx=use_sx, - ) + return external_force_set def main(): From f429573c3aaa2a9bba0bcbf7a7d63b74f54336b7 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Tue, 5 Nov 2024 09:55:54 -0500 Subject: [PATCH 105/108] docs: viewer tracked markers --- bioptim/models/biorbd/viewer_utils.py | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/bioptim/models/biorbd/viewer_utils.py b/bioptim/models/biorbd/viewer_utils.py index 067ae73e8..7ac1f4d0d 100644 --- a/bioptim/models/biorbd/viewer_utils.py +++ b/bioptim/models/biorbd/viewer_utils.py @@ -8,7 +8,14 @@ def _prepare_tracked_markers_for_animation( nlps: list["NonLinearProgram", ...], n_shooting: int = None ) -> list[np.ndarray, ...]: - """Prepare the markers which are tracked to the animation""" + """ + Prepare the markers which are tracked to the animation + + Returns + ------- + list[np.ndarray, ...] + The markers to be tracked for each phase + """ all_tracked_markers = [] From 959c700e1da86b60cb71723cd2aa61c1706ddaf6 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Fri, 8 Nov 2024 10:32:16 -0500 Subject: [PATCH 106/108] clean: dead comment --- bioptim/dynamics/configure_problem.py | 1 - 1 file changed, 1 deletion(-) diff --git a/bioptim/dynamics/configure_problem.py b/bioptim/dynamics/configure_problem.py index 8304bed83..10231287e 100644 --- a/bioptim/dynamics/configure_problem.py +++ b/bioptim/dynamics/configure_problem.py @@ -1965,7 +1965,6 @@ def __init__( state_continuity_weight: float | None = None, phase_dynamics: PhaseDynamics = PhaseDynamics.SHARED_DURING_THE_PHASE, numerical_data_timeseries: dict[str, np.ndarray] = None, - # external_forces: ExternalForceSetTimeSeries = None, **extra_parameters: Any, ): """ From 0064c2aa36cece7ee17bc49038efebb504505937 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Fri, 8 Nov 2024 10:34:31 -0500 Subject: [PATCH 107/108] refactor: defects = None root dynamics --- bioptim/dynamics/dynamics_functions.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/bioptim/dynamics/dynamics_functions.py b/bioptim/dynamics/dynamics_functions.py index 6f0330d7d..fb1848421 100644 --- a/bioptim/dynamics/dynamics_functions.py +++ b/bioptim/dynamics/dynamics_functions.py @@ -2,8 +2,8 @@ from .dynamics_evaluation import DynamicsEvaluation from .fatigue.fatigue_dynamics import FatigueList -from ..misc.mapping import BiMapping from ..misc.enums import DefectType +from ..misc.mapping import BiMapping from ..optimization.optimization_variable import OptimizationVariable @@ -250,7 +250,7 @@ def torque_driven_free_floating_base( dxdt[:n_q, :] = horzcat(*[dq for _ in range(ddq.shape[1])]) dxdt[n_q:, :] = ddq - return DynamicsEvaluation(dxdt, None) + return DynamicsEvaluation(dxdt, defects=None) @staticmethod def stochastic_torque_driven( From 1cdaf8e2b2954c6507c5052e356ff648b27bf060 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Fri, 8 Nov 2024 10:39:03 -0500 Subject: [PATCH 108/108] refactor: -= instead of +=- --- bioptim/examples/torque_driven_ocp/spring_load.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bioptim/examples/torque_driven_ocp/spring_load.py b/bioptim/examples/torque_driven_ocp/spring_load.py index ad392c091..6abaed9b1 100644 --- a/bioptim/examples/torque_driven_ocp/spring_load.py +++ b/bioptim/examples/torque_driven_ocp/spring_load.py @@ -148,7 +148,7 @@ def custom_dynamic( tau = DynamicsFunctions.get(nlp.controls["tau"], controls) stiffness = 100 - tau += -sign(q[0]) * stiffness * q[0] # damping + tau -= sign(q[0]) * stiffness * q[0] # damping qddot = nlp.model.forward_dynamics(with_contact=False)(q, qdot, tau, [], [])