diff --git a/bioptim/dynamics/configure_problem.py b/bioptim/dynamics/configure_problem.py index 8ae7a12db..549999bdf 100644 --- a/bioptim/dynamics/configure_problem.py +++ b/bioptim/dynamics/configure_problem.py @@ -302,11 +302,9 @@ def torque_driven( def torque_driven_free_floating_base( ocp, nlp, - with_contact: bool = False, with_passive_torque: bool = False, with_ligament: bool = False, with_friction: bool = False, - external_forces: list = None, ): """ Configure the dynamics for a torque driven program with a free floating base. @@ -320,22 +318,14 @@ def torque_driven_free_floating_base( A reference to the ocp nlp: NonLinearProgram A reference to the phase - with_contact: bool - If the dynamic with contact should be used with_passive_torque: bool If the dynamic with passive torque should be used with_ligament: bool If the dynamic with ligament should be used with_friction: bool If the dynamic with joint friction should be used (friction = coefficients * qdot) - external_forces: list[Any] - A list of external forces """ - _check_contacts_in_biorbd_model(with_contact, nlp.model.nb_contacts, nlp.phase_idx) - _check_external_forces_format(external_forces, nlp.ns, nlp.phase_idx) - _check_external_forces_and_phase_dynamics(external_forces, nlp.phase_dynamics, nlp.phase_idx) - nb_q = nlp.model.nb_q nb_qdot = nlp.model.nb_qdot nb_root = nlp.model.nb_root @@ -424,17 +414,9 @@ def torque_driven_free_floating_base( ocp, nlp, DynamicsFunctions.torque_driven_free_floating_base, - with_contact=with_contact, with_passive_torque=with_passive_torque, with_ligament=with_ligament, with_friction=with_friction, - 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 ) @staticmethod @@ -1092,9 +1074,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.mx_reduced[nlp.states["q"].index], - nlp.states.mx_reduced[nlp.states["qdot"].index], + nlp.states["q"].mapping.to_second.map(q), nlp.states["qdot"].mapping.to_second.map(qdot) ) nlp.soft_contact_forces_func = Function( "soft_contact_forces_func", diff --git a/bioptim/dynamics/dynamics_functions.py b/bioptim/dynamics/dynamics_functions.py index 2373a6fe3..09537a747 100644 --- a/bioptim/dynamics/dynamics_functions.py +++ b/bioptim/dynamics/dynamics_functions.py @@ -196,11 +196,9 @@ def torque_driven_free_floating_base( parameters: MX.sym, algebraic_states: MX.sym, nlp, - with_contact: bool, with_passive_torque: bool, with_ligament: bool, with_friction: bool, - external_forces: list = None, ) -> DynamicsEvaluation: """ Forward dynamics driven by joint torques without actuation of the free floating base, optional external forces can be declared. @@ -219,16 +217,12 @@ def torque_driven_free_floating_base( The algebraic states of the system nlp: NonLinearProgram The definition of the system - with_contact: bool - If the dynamic with contact should be used with_passive_torque: bool If the dynamic with passive torque should be used with_ligament: bool If the dynamic with ligament should be used with_friction: bool If the dynamic with friction should be used - external_forces: list[Any] - The external forces Returns ---------- @@ -256,7 +250,7 @@ def torque_driven_free_floating_base( tau_full = vertcat(MX.zeros(nlp.model.nb_root), tau_joints) - ddq = DynamicsFunctions.forward_dynamics(nlp, q_full, qdot_full, tau_full, with_contact, external_forces) + ddq = DynamicsFunctions.forward_dynamics(nlp, q_full, qdot_full, tau_full, with_contact=False) dxdt = MX(n_q + n_qdot, ddq.shape[1]) dxdt[:n_q, :] = horzcat(*[dq for _ in range(ddq.shape[1])]) dxdt[n_q:, :] = ddq diff --git a/bioptim/models/biorbd/biorbd_model.py b/bioptim/models/biorbd/biorbd_model.py index 123bdc304..a87c9b992 100644 --- a/bioptim/models/biorbd/biorbd_model.py +++ b/bioptim/models/biorbd/biorbd_model.py @@ -116,41 +116,90 @@ def homogeneous_matrices_in_child(self, segment_id) -> MX: 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 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 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, 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 @@ -161,6 +210,8 @@ def segment_angular_velocity(self, q, qdot, idx) -> MX: """ 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() @@ -215,11 +266,18 @@ 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) - return self.model.torque(tau_activations, q_biorbd, qdot_biorbd).to_mx() + 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) @@ -276,6 +334,9 @@ def extract_elements(e) -> tuple[str, Any] | tuple[Any, str, Any]: 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) external_forces_set = self._dispatch_forces(external_forces, translational_forces) q_biorbd = GeneralizedCoordinates(q) @@ -285,6 +346,9 @@ def forward_dynamics(self, q, qdot, tau, external_forces=None, translational_for 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) + self.check_q_size(q) + self.check_qdot_size(qdot) + self.check_tau_size(tau) q_biorbd = GeneralizedCoordinates(q) qdot_biorbd = GeneralizedVelocity(qdot) @@ -295,6 +359,9 @@ def constrained_forward_dynamics(self, q, qdot, tau, external_forces=None, trans def inverse_dynamics(self, q, qdot, qddot, external_forces=None, translational_forces=None) -> MX: 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) @@ -305,6 +372,9 @@ def contact_forces_from_constrained_forward_dynamics( self, q, qdot, tau, external_forces=None, translational_forces=None ) -> MX: 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) @@ -314,25 +384,35 @@ def contact_forces_from_constrained_forward_dynamics( ).to_mx() 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 muscle_activation_dot(self, muscle_excitations) -> MX: + self.check_muscle_size(muscle_excitations) 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() 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_joint_torque(self, activations, q, qdot) -> MX: + self.check_q_size(q) + self.check_qdot_size(qdot) + self.check_muscle_size(activations) + muscles_states = self.model.stateSet() for k in range(self.model.nbMuscles()): muscles_states[k].setActivation(activations[k]) @@ -341,6 +421,7 @@ def muscle_joint_torque(self, activations, q, qdot) -> MX: return self.model.muscularJointTorque(muscles_states, q_biorbd, qdot_biorbd).to_mx() def markers(self, q) -> list[MX]: + self.check_q_size(q) return [m.to_mx() for m in self.model.markers(GeneralizedCoordinates(q))] @property @@ -351,6 +432,7 @@ 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) if reference_segment_index is not None: global_homogeneous_matrix = self.model.globalJCS(GeneralizedCoordinates(q), reference_segment_index) @@ -390,6 +472,8 @@ 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) if reference_index is None: return [ m.to_mx() @@ -414,6 +498,9 @@ def marker_velocities(self, q, qdot, reference_index=None) -> list[MX]: return out 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) if reference_index is None: return [ m.to_mx() @@ -433,7 +520,9 @@ def marker_accelerations(self, q, qdot, qddot, reference_index=None) -> list[MX] inverse=True, ) for m in self.model.markersAcceleration( - GeneralizedCoordinates(q), GeneralizedVelocity(qdot), GeneralizedAcceleration(qddot) + GeneralizedCoordinates(q), + GeneralizedVelocity(qdot), + GeneralizedAcceleration(qddot), ): if m.applyRT(homogeneous_matrix_transposed) is None: out.append(m.to_mx()) @@ -442,12 +531,17 @@ def marker_accelerations(self, q, qdot, qddot, reference_index=None) -> list[MX] def tau_max(self, q, qdot) -> tuple[MX, MX]: self.model.closeActuator() + self.check_q_size(q) + self.check_qdot_size(qdot) q_biorbd = GeneralizedCoordinates(q) qdot_biorbd = GeneralizedVelocity(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) @@ -456,6 +550,7 @@ def rigid_contact_acceleration(self, q, qdot, qddot, contact_index, contact_axis ] def markers_jacobian(self, q) -> list[MX]: + self.check_q_size(q) return [m.to_mx() for m in self.model.markersJacobian(GeneralizedCoordinates(q))] @property @@ -463,6 +558,8 @@ 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) @@ -496,7 +593,12 @@ def normalize_state_quaternions(self, x: MX | SX) -> MX | SX: # 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]]) + quaternion = vertcat( + x[quat_idx[j][3]], + x[quat_idx[j][0]], + x[quat_idx[j][1]], + x[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] @@ -515,6 +617,9 @@ def get_quaternion_idx(self) -> list[list[int]]: return quat_idx def contact_forces(self, q, qdot, tau, external_forces: list = None) -> MX: + self.check_q_size(q) + self.check_qdot_size(qdot) + self.check_tau_size(tau) if external_forces is not None and len(external_forces) != 0: all_forces = MX() for i, f_ext in enumerate(external_forces): @@ -525,11 +630,15 @@ def contact_forces(self, q, qdot, tau, external_forces: list = None) -> MX: 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() @@ -562,16 +671,21 @@ def ranges_from_model(self, variable: str): if segment.parent().to_string().lower() != "root": qddot_ranges += [qddot_range for qddot_range in segment.QDDotRanges()] - if variable == "q" or variable == "q_roots" or variable == "q_joints": + if variable in ["q", "q_roots", "q_joints"]: return q_ranges - elif variable == "qdot" or variable == "qdot_roots" or variable == "qdot_joints": + elif variable in ["qdot", "qdot_roots", "qdot_joints"]: return qdot_ranges elif variable == "qddot" or variable == "qddot_joints": return qddot_ranges else: raise RuntimeError("Wrong variable name") - def _var_mapping(self, key: str, range_for_mapping: int | list | tuple | range, mapping: BiMapping = None) -> dict: + def _var_mapping( + self, + key: str, + range_for_mapping: int | list | tuple | range, + mapping: BiMapping = None, + ) -> dict: return _var_mapping(key, range_for_mapping, mapping) def bounds_from_ranges(self, variables: str | list[str, ...], mapping: BiMapping | BiMappingList = None) -> Bounds: @@ -591,7 +705,7 @@ def animate( solution: "SolutionData", show_now: bool = True, tracked_markers: list[np.ndarray, ...] = None, - **kwargs: Any + **kwargs: Any, ) -> None | list: try: import bioviz @@ -613,16 +727,17 @@ def animate( all_bioviz = [] for idx_phase, data in enumerate(states): - if not isinstance(ocp.nlp[idx_phase].model, BiorbdModel): + if not isinstance(solution.ocp.nlp[idx_phase].model, BiorbdModel): raise NotImplementedError("Animation is only implemented for biorbd models") # This calls each of the function that modify the internal dynamic model based on the parameters - nlp = ocp.nlp[idx_phase] + nlp = solution.ocp.nlp[idx_phase] # noinspection PyTypeChecker biorbd_model: BiorbdModel = nlp.model all_bioviz.append(bioviz.Viz(biorbd_model.path, **kwargs)) + all_bioviz[-1].load_movement(solution.ocp.nlp[idx_phase].variable_mappings["q"].to_second.map(data["q"])) if "q_roots" in solution and "q_joints" in solution: # TODO: Fix the mapping for this case diff --git a/tests/shard1/test_biorbd_model_size.py b/tests/shard1/test_biorbd_model_size.py new file mode 100644 index 000000000..39e3e167a --- /dev/null +++ b/tests/shard1/test_biorbd_model_size.py @@ -0,0 +1,628 @@ +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 + + +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(q_valid, qdot_valid, idx) + # 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) + # 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) + + +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.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.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.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.constrained_forward_dynamics(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_dynamics.py b/tests/shard1/test_dynamics.py index 9f98694d3..41d43a9ce 100644 --- a/tests/shard1/test_dynamics.py +++ b/tests/shard1/test_dynamics.py @@ -1364,9 +1364,7 @@ def test_torque_activation_driven_with_residual_torque( @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_contact", [False, True]) -def test_torque_driven(with_contact, with_external_force, cx, phase_dynamics): +def test_torque_driven_free_floating_base(cx, phase_dynamics): # Prepare the program nlp = NonLinearProgram(phase_dynamics=phase_dynamics) nlp.model = BiorbdModel( @@ -1379,106 +1377,18 @@ def test_torque_driven(with_contact, with_external_force, cx, phase_dynamics): 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.u_bounds = np.zeros((nlp.model.nb_tau - nlp.model.nb_root, 1)) nlp.x_scaling = VariableScalingList() nlp.xdot_scaling = VariableScalingList() nlp.u_scaling = VariableScalingList() nlp.a_scaling = VariableScalingList() - external_forces = ( - [ - [ - [ - "Seg0", - np.array( - [ - 0.374540118847362, - 0.950714306409916, - 0.731993941811405, - 0.598658484197037, - 0.156018640442437, - 0.155994520336203, - ] - ), - ] - ], - [ - [ - "Seg0", - np.array( - [ - 0.058083612168199, - 0.866176145774935, - 0.601115011743209, - 0.708072577796045, - 0.020584494295802, - 0.969909852161994, - ] - ), - ] - ], - [ - [ - "Seg0", - np.array( - [ - 0.832442640800422, - 0.212339110678276, - 0.181824967207101, - 0.183404509853434, - 0.304242242959538, - 0.524756431632238, - ] - ), - ] - ], - [ - [ - "Seg0", - np.array( - [ - 0.431945018642116, - 0.291229140198042, - 0.611852894722379, - 0.139493860652042, - 0.292144648535218, - 0.366361843293692, - ] - ), - ] - ], - [ - [ - "Seg0", - np.array( - [ - 0.456069984217036, - 0.785175961393014, - 0.19967378215836, - 0.514234438413612, - 0.592414568862042, - 0.046450412719998, - ] - ), - ] - ], - ] - if with_external_force - else None - ) - ocp = OptimalControlProgram(nlp) nlp.control_type = ControlType.CONSTANT NonLinearProgram.add( ocp, "dynamics_type", - Dynamics( - DynamicsFcn.TORQUE_DRIVEN_FREE_FLOATING_BASE, - with_contact=with_contact, - expand_dynamics=True, - phase_dynamics=phase_dynamics, - external_forces=external_forces, - ), + Dynamics(DynamicsFcn.TORQUE_DRIVEN_FREE_FLOATING_BASE, expand_dynamics=True, phase_dynamics=phase_dynamics), False, ) phase_index = [i for i in range(ocp.n_phases)] @@ -1491,19 +1401,9 @@ def test_torque_driven(with_contact, with_external_force, cx, phase_dynamics): NonLinearProgram.add(ocp, "use_controls_from_phase_idx", use_controls_from_phase_idx, False) np.random.seed(42) - if with_external_force: - np.random.rand(nlp.ns, 6) # just not to change the values of the next random values # Prepare the dynamics - if phase_dynamics == PhaseDynamics.SHARED_DURING_THE_PHASE and with_external_force: - with pytest.raises( - RuntimeError, - match="Phase 0 has external_forces but the phase_dynamics is PhaseDynamics.SHARED_DURING_THE_PHASE.Please set phase_dynamics=PhaseDynamics.ONE_PER_NODE", - ): - ConfigureProblem.initialize(ocp, nlp) - return - else: - ConfigureProblem.initialize(ocp, nlp) + ConfigureProblem.initialize(ocp, nlp) # Test the results states = np.random.rand(nlp.states.shape, nlp.ns) @@ -1513,33 +1413,10 @@ def test_torque_driven(with_contact, with_external_force, cx, phase_dynamics): time = np.random.rand(2, 1) x_out = np.array(nlp.dynamics_func[0](time, states, controls, params, algebraic_states)) - if with_contact: - contact_out = np.array(nlp.contact_forces_func(time, states, controls, params, algebraic_states)) - if with_external_force: - np.testing.assert_almost_equal( - x_out[:, 0], - [0.96958463, 0.92187424, 0.38867729, 0.54269608, -1.71642952, -0.28661718, 3.47711038, -2.61110605], - ) - np.testing.assert_almost_equal(contact_out[:, 0], [-12.59366904, 128.27098855, 2.35829492]) - - else: - np.testing.assert_almost_equal( - x_out[:, 0], - [0.61185289, 0.78517596, 0.60754485, 0.80839735, -0.26487805, -0.19004763, 0.53746739, -0.12272266], - ) - np.testing.assert_almost_equal(contact_out[:, 0], [-2.30360748, 127.09143179, 5.05814294]) - - else: - if with_external_force: - np.testing.assert_almost_equal( - x_out[:, 0], - [0.96958463, 0.92187424, 0.38867729, 0.54269608, -0.21234549, -10.23941443, 2.07066831, 31.68033189], - ) - else: - np.testing.assert_almost_equal( - x_out[:, 0], - [0.61185289, 0.78517596, 0.60754485, 0.80839735, 0.04791036, -9.96778948, -0.01986505, 4.39786051], - ) + np.testing.assert_almost_equal( + x_out[:, 0], + [0.61185289, 0.78517596, 0.60754485, 0.80839735, 0.04791036, -9.96778948, -0.01986505, 4.39786051], + ) @pytest.mark.parametrize("phase_dynamics", [PhaseDynamics.SHARED_DURING_THE_PHASE, PhaseDynamics.ONE_PER_NODE]) diff --git a/tests/shard1/test_prepare_all_examples.py b/tests/shard1/test_prepare_all_examples.py index 2a98eda10..16e28c0e8 100644 --- a/tests/shard1/test_prepare_all_examples.py +++ b/tests/shard1/test_prepare_all_examples.py @@ -449,9 +449,7 @@ def test__symmetrical_torque_driven_ocp__symmetry_by_constraint(): def test__symmetrical_torque_driven_ocp__symmetry_by_mapping(): - from bioptim.examples.symmetrical_torque_driven_ocp import ( - symmetry_by_mapping as ocp_module, - ) + from bioptim.examples.symmetrical_torque_driven_ocp import symmetry_by_mapping as ocp_module bioptim_folder = os.path.dirname(ocp_module.__file__)