diff --git a/.github/workflows/run_casadi_tests.yml b/.github/workflows/run_casadi_tests.yml index 310187ed6..2b3776fd9 100644 --- a/.github/workflows/run_casadi_tests.yml +++ b/.github/workflows/run_casadi_tests.yml @@ -130,7 +130,8 @@ jobs: - name: Run python binder tests run: | BIORBD_FOLDER=`pwd` - cd $BIORBD_FOLDER/$BUILD_FOLDER/test/binding/Python3 + cd $BIORBD_FOLDER/$BUILD_FOLDER/test/binding/python3 + export CI_MAIN_EXAMPLES_FOLDER=$BIORBD_FOLDER/$EXAMPLES_FOLDER pytest . cd $BIORBD_FOLDER diff --git a/.github/workflows/run_eigen_tests.yml b/.github/workflows/run_eigen_tests.yml index 475a195bc..0911d6f09 100644 --- a/.github/workflows/run_eigen_tests.yml +++ b/.github/workflows/run_eigen_tests.yml @@ -161,7 +161,8 @@ jobs: - name: Run python binder tests run: | BIORBD_FOLDER=`pwd` - cd $BIORBD_FOLDER/$BUILD_FOLDER/test/binding/Python3 + cd $BIORBD_FOLDER/$BUILD_FOLDER/test/binding/python3 + export CI_MAIN_EXAMPLES_FOLDER=$BIORBD_FOLDER/$EXAMPLES_FOLDER pytest . cd $BIORBD_FOLDER diff --git a/binding/python3/model_creation/__init__.py b/binding/python3/model_creation/__init__.py index a484794f0..d6fad3f07 100644 --- a/binding/python3/model_creation/__init__.py +++ b/binding/python3/model_creation/__init__.py @@ -5,9 +5,15 @@ from .axis import Axis from .inertia_parameters import InertiaParameters from .marker import Marker +from .contact import Contact +from .muscle import Muscle +from .muscle_group import MuscleGroup +from .via_point import ViaPoint from .mesh import Mesh +from .mesh_file import MeshFile from .protocols import Data, GenericDynamicModel from .rotations import Rotations +from .range_of_motion import RangeOfMotion, Ranges from .segment import Segment from .segment_coordinate_system import SegmentCoordinateSystem from .translations import Translations @@ -16,7 +22,11 @@ from .biomechanical_model_real import BiomechanicalModelReal from .axis_real import AxisReal from .marker_real import MarkerReal +from .contact_real import ContactReal +from .muscle_real import MuscleReal, MuscleType, MuscleStateType +from .via_point_real import ViaPointReal from .mesh_real import MeshReal +from .mesh_file_real import MeshFileReal from .segment_real import SegmentReal from .segment_coordinate_system_real import SegmentCoordinateSystemReal from .inertia_parameters_real import InertiaParametersReal diff --git a/binding/python3/model_creation/biomechanical_model.py b/binding/python3/model_creation/biomechanical_model.py index 526071946..540b27938 100644 --- a/binding/python3/model_creation/biomechanical_model.py +++ b/binding/python3/model_creation/biomechanical_model.py @@ -1,6 +1,7 @@ from .protocols import Data from .segment_real import SegmentReal -from .segment import Segment +from .muscle_group import MuscleGroup +from .muscle_real import MuscleReal from .segment_coordinate_system_real import SegmentCoordinateSystemReal from .biomechanical_model_real import BiomechanicalModelReal @@ -8,22 +9,14 @@ class BiomechanicalModel: def __init__(self, bio_sym_path: str = None): self.segments = {} + self.muscle_groups = {} + self.muscles = {} + self.via_points = {} if bio_sym_path is None: return raise NotImplementedError("bioMod files are not readable yet") - def __getitem__(self, name: str): - return self.segments[name] - - def __setitem__(self, name: str, segment: Segment): - if segment.name is not None and segment.name != name: - raise ValueError( - "The segment name should be the same as the 'key'. Alternatively, segment.name can be left undefined" - ) - segment.name = name # Make sure the name of the segment fits the internal one - self.segments[name] = segment - def to_real(self, data: Data) -> BiomechanicalModelReal: """ Collapse the model to an actual personalized biomechanical model based on the generic model and the data @@ -54,19 +47,63 @@ def to_real(self, data: Data) -> BiomechanicalModelReal: if s.mesh is not None: mesh = s.mesh.to_mesh(data, model, scs) - model[s.name] = SegmentReal( + mesh_file = None + if s.mesh_file is not None: + mesh_file = s.mesh_file.to_mesh_file(data) + + model.segments[s.name] = SegmentReal( name=s.name, parent_name=s.parent_name, segment_coordinate_system=scs, translations=s.translations, rotations=s.rotations, + q_ranges=s.q_ranges, + qdot_ranges=s.qdot_ranges, inertia_parameters=inertia_parameters, mesh=mesh, + mesh_file=mesh_file, ) for marker in s.markers: model.segments[name].add_marker(marker.to_marker(data, model, scs)) + for contact in s.contacts: + model.segments[name].add_contact(contact.to_contact(data)) + + for name in self.muscle_groups: + mg = self.muscle_groups[name] + + model.muscle_groups[mg.name] = MuscleGroup( + name=mg.name, + origin_parent_name=mg.origin_parent_name, + insertion_parent_name=mg.insertion_parent_name, + ) + + for name in self.muscles: + m = self.muscles[name] + + if m.muscle_group not in model.muscle_groups: + raise RuntimeError( + f"Please create the muscle group {m.muscle_group} before putting the muscle {m.name} in it." + ) + + model.muscles[m.name] = m.to_muscle(model, data) + + for name in self.via_points: + vp = self.via_points[name] + + if vp.muscle_name not in model.muscles: + raise RuntimeError( + f"Please create the muscle {vp.muscle_name} before putting the via point {vp.name} in it." + ) + + if vp.muscle_group not in model.muscle_groups: + raise RuntimeError( + f"Please create the muscle group {vp.muscle_group} before putting the via point {vp.name} in it." + ) + + model.via_points[vp.name] = vp.to_via_point(data) + return model def write(self, save_path: str, data: Data): diff --git a/binding/python3/model_creation/biomechanical_model_real.py b/binding/python3/model_creation/biomechanical_model_real.py index a2148fa9f..9718b8b2e 100644 --- a/binding/python3/model_creation/biomechanical_model_real.py +++ b/binding/python3/model_creation/biomechanical_model_real.py @@ -1,23 +1,49 @@ class BiomechanicalModelReal: def __init__(self): from .segment_real import SegmentReal # Imported here to prevent from circular imports + from .muscle_group import MuscleGroup + from .muscle_real import MuscleReal + from .via_point_real import ViaPointReal self.segments: dict[str:SegmentReal, ...] = {} # From Pythom 3.7 the insertion order in a dict is preserved. This is important because when writing a new # .bioMod file, the order of the segment matters - - def __getitem__(self, name: str): - return self.segments[name] - - def __setitem__(self, name: str, segment: "SegmentReal"): - segment.name = name # Make sure the name of the segment fits the internal one - self.segments[name] = segment + self.muscle_groups: dict[str:MuscleGroup, ...] = {} + self.muscles: dict[str:MuscleReal, ...] = {} + self.via_points: dict[str:ViaPointReal, ...] = {} def __str__(self): out_string = "version 4\n\n" + + out_string += "// --------------------------------------------------------------\n" + out_string += "// SEGMENTS\n" + out_string += "// --------------------------------------------------------------\n\n" for name in self.segments: out_string += str(self.segments[name]) out_string += "\n\n\n" # Give some space between segments + + out_string += "// --------------------------------------------------------------\n" + out_string += "// MUSCLE GROUPS\n" + out_string += "// --------------------------------------------------------------\n\n" + for name in self.muscle_groups: + out_string += str(self.muscle_groups[name]) + out_string += "\n" + out_string += "\n\n\n" # Give some space after muscle groups + + out_string += "// --------------------------------------------------------------\n" + out_string += "// MUSCLES\n" + out_string += "// --------------------------------------------------------------\n\n" + for name in self.muscles: + out_string += str(self.muscles[name]) + out_string += "\n\n\n" # Give some space between muscles + + out_string += "// --------------------------------------------------------------\n" + out_string += "// MUSCLES VIA POINTS\n" + out_string += "// --------------------------------------------------------------\n\n" + for name in self.via_points: + out_string += str(self.via_points[name]) + out_string += "\n\n\n" # Give some space between via points + return out_string def write(self, file_path: str): diff --git a/binding/python3/model_creation/contact.py b/binding/python3/model_creation/contact.py new file mode 100644 index 000000000..fc95e67d0 --- /dev/null +++ b/binding/python3/model_creation/contact.py @@ -0,0 +1,41 @@ +from typing import Callable + +from .protocols import Data +from .translations import Translations +from .contact_real import ContactReal + + +class Contact: + def __init__( + self, + name: str, + function: Callable | str = None, + parent_name: str = None, + axis: Translations = None, + ): + """ + Parameters + ---------- + name + The name of the new contact + function + The function (f(m) -> np.ndarray, where m is a dict of markers) that defines the contact with. + parent_name + The name of the parent the contact is attached to + axis + The axis of the contact + """ + self.name = name + function = function if function is not None else self.name + self.function = (lambda m, bio: m[function]) if isinstance(function, str) else function + self.parent_name = parent_name + self.axis = axis + + def to_contact(self, data: Data) -> ContactReal: + return ContactReal.from_data( + data, + self.name, + self.function, + self.parent_name, + self.axis, + ) diff --git a/binding/python3/model_creation/contact_real.py b/binding/python3/model_creation/contact_real.py new file mode 100644 index 000000000..d60bece62 --- /dev/null +++ b/binding/python3/model_creation/contact_real.py @@ -0,0 +1,80 @@ +from typing import Callable + +import numpy as np + +from .protocols import Data +from .translations import Translations + + +class ContactReal: + def __init__( + self, + name: str, + parent_name: str, + position: tuple[int | float, int | float, int | float] | np.ndarray = None, + axis: Translations = None, + ): + """ + Parameters + ---------- + name + The name of the new contact + parent_name + The name of the parent the contact is attached to + position + The 3d position of the contact + axis + The axis of the contact + """ + self.name = name + self.parent_name = parent_name + if position is None: + position = np.array((0, 0, 0, 1)) + self.position = position if isinstance(position, np.ndarray) else np.array(position) + self.axis = axis + + @staticmethod + def from_data( + data: Data, + name: str, + function: Callable, + parent_name: str, + axis: Translations = None, + ): + """ + This is a constructor for the Contact class. It evaluates the function that defines the contact to get an + actual position + + Parameters + ---------- + data + The data to pick the data from + name + The name of the new contact + function + The function (f(m) -> np.ndarray, where m is a dict of markers (XYZ1 x time)) that defines the contacts in the local joint coordinates. + parent_name + The name of the parent the contact is attached to + axis + The axis of the contact + """ + + # Get the position of the contact points and do some sanity checks + p: np.ndarray = function(data.values) + if not isinstance(p, np.ndarray): + raise RuntimeError(f"The function {function} must return a np.ndarray of dimension 3xT (XYZ x time)") + if p.shape == (3, 1): + p = p.reshape((3,)) + elif p.shape != (3,): + raise RuntimeError(f"The function {function} must return a vector of dimension 3 (XYZ)") + + return ContactReal(name, parent_name, p, axis) + + def __str__(self): + # Define the print function, so it automatically formats things in the file properly + out_string = f"contact\t{self.name}\n" + out_string += f"\tparent\t{self.parent_name}\n" + out_string += f"\tposition\t{np.round(self.position[0], 4)}\t{np.round(self.position[1], 4)}\t{np.round(self.position[2], 4)}\n" + out_string += f"\taxis\t{self.axis.value}\n" + out_string += "endcontact\n" + return out_string diff --git a/binding/python3/model_creation/inertia_parameters_real.py b/binding/python3/model_creation/inertia_parameters_real.py index cb864c6a2..4fd31f36e 100644 --- a/binding/python3/model_creation/inertia_parameters_real.py +++ b/binding/python3/model_creation/inertia_parameters_real.py @@ -81,7 +81,7 @@ def __str__(self): # Define the print function, so it automatically formats things in the file properly com = np.nanmean(self.center_of_mass, axis=1)[:3] - out_string = f"\tmass {self.mass}\n" - out_string += f"\tCenterOfMass {com[0]:0.5f} {com[1]:0.5f} {com[2]:0.5f}\n" - out_string += f"\tinertia_xxyyzz {self.inertia[0]} {self.inertia[1]} {self.inertia[2]}\n" + out_string = f"\tmass\t{self.mass}\n" + out_string += f"\tCenterOfMass\t{com[0]:0.5f}\t{com[1]:0.5f}\t{com[2]:0.5f}\n" + out_string += f"\tinertia_xxyyzz\t{self.inertia[0]}\t{self.inertia[1]}\t{self.inertia[2]}\n" return out_string diff --git a/binding/python3/model_creation/marker_real.py b/binding/python3/model_creation/marker_real.py index e3f3b4f1d..ba8a7c81e 100644 --- a/binding/python3/model_creation/marker_real.py +++ b/binding/python3/model_creation/marker_real.py @@ -101,13 +101,13 @@ def mean_position(self) -> np.ndarray: def __str__(self): # Define the print function, so it automatically formats things in the file properly - out_string = f"marker {self.name}\n" - out_string += f"\tparent {self.parent_name}\n" + out_string = f"marker\t{self.name}\n" + out_string += f"\tparent\t{self.parent_name}\n" p = self.mean_position - out_string += f"\tposition {p[0]:0.4f} {p[1]:0.4f} {p[2]:0.4f}\n" - out_string += f"\ttechnical {1 if self.is_technical else 0}\n" - out_string += f"\tanatomical {1 if self.is_anatomical else 0}\n" + out_string += f"\tposition\t{p[0]:0.4f}\t{p[1]:0.4f}\t{p[2]:0.4f}\n" + out_string += f"\ttechnical\t{1 if self.is_technical else 0}\n" + out_string += f"\tanatomical\t{1 if self.is_anatomical else 0}\n" out_string += "endmarker\n" return out_string diff --git a/binding/python3/model_creation/mesh_file.py b/binding/python3/model_creation/mesh_file.py new file mode 100644 index 000000000..c8d4250ca --- /dev/null +++ b/binding/python3/model_creation/mesh_file.py @@ -0,0 +1,53 @@ +from typing import Callable + +import numpy as np + +from .biomechanical_model_real import BiomechanicalModelReal +from .mesh_file_real import MeshFileReal +from .protocols import Data +from .segment_coordinate_system_real import SegmentCoordinateSystemReal + + +class MeshFile: + def __init__( + self, + mesh_file_name: str, + mesh_color: np.ndarray[float] | list[float] | tuple[float] = None, + scaling_function: Callable = None, + rotation_function: Callable = None, + translation_function: Callable = None, + ): + """ + This is a pre-constructor for the MeshFileReal class. It allows to create a generic model by marker names + + Parameters + ---------- + mesh_file_name + The name of the mesh file + mesh_color + The color the mesh should be displayed in (RGB) + scaling_function + The function that defines the scaling of the mesh + rotation_function + The function that defines the rotation of the mesh + translation_function + The function that defines the translation of the mesh + """ + self.mesh_file_name = mesh_file_name + self.mesh_color = mesh_color + self.scaling_function = scaling_function + self.rotation_function = rotation_function + self.translation_function = translation_function + + def to_mesh_file( + self, + data: Data, + ) -> MeshFileReal: + return MeshFileReal.from_data( + data, + self.mesh_file_name, + self.mesh_color, + self.scaling_function, + self.rotation_function, + self.translation_function, + ) diff --git a/binding/python3/model_creation/mesh_file_real.py b/binding/python3/model_creation/mesh_file_real.py new file mode 100644 index 000000000..54fec8269 --- /dev/null +++ b/binding/python3/model_creation/mesh_file_real.py @@ -0,0 +1,139 @@ +from typing import Callable + +import numpy as np + +from .biomechanical_model_real import BiomechanicalModelReal +from .protocols import Data + + +class MeshFileReal: + def __init__( + self, + mesh_file_name: str, + mesh_color: np.ndarray[float] | list[float] | tuple[float], + mesh_scale: np.ndarray[float] | list[float] | tuple[float], + mesh_rotation: np.ndarray[float] | list[float] | tuple[float], + mesh_translation: np.ndarray[float] | list[float] | tuple[float], + ): + """ + Parameters + ---------- + mesh_file_name + The name of the mesh file + mesh_color + The color the mesh should be displayed in (RGB) + mesh_scale + The scaling that must be applied to the mesh (XYZ) + mesh_rotation + The rotation that must be applied to the mesh (Euler angles: XYZ) + mesh_translation + The translation that must be applied to the mesh (XYZ) + """ + + self.mesh_file_name = mesh_file_name + self.mesh_color = mesh_color + self.mesh_scale = mesh_scale + self.mesh_rotation = mesh_rotation + self.mesh_translation = mesh_translation + + @staticmethod + def from_data( + data: Data, + mesh_file_name: str, + mesh_color: np.ndarray[float] | list[float] | tuple[float] = None, + scaling_function: Callable = None, + rotation_function: Callable = None, + translation_function: Callable = None, + ): + """ + This is a constructor for the MeshFileReal class. It evaluates the functions that defines the mesh file to get + actual characteristics of the mesh + + Parameters + ---------- + data + The data to pick the data from + mesh_file_name + The name of the mesh file + mesh_color + The color the mesh should be displayed in (RGB) + scaling_function + The function (f(m) -> np.ndarray, where m is a dict of markers (XYZ1 x time)) that defines the scaling + rotation_function + The function (f(m) -> np.ndarray, where m is a dict of markers (XYZ1 x time)) that defines the rotation + translation_function + The function (f(m) -> np.ndarray, where m is a dict of markers (XYZ1 x time)) that defines the translation + """ + + if not isinstance(mesh_file_name, str): + raise RuntimeError("The mesh_file_name must be a string") + + if mesh_color is not None: + mesh_color = np.array(mesh_color) + if mesh_color.shape == (3, 1): + mesh_color = mesh_color.reshape((3,)) + elif mesh_color.shape != (3,): + raise RuntimeError("The mesh_color must be a vector of dimension 3 (RGB)") + + if scaling_function is None: + mesh_scale = np.array([1, 1, 1]) + else: + mesh_scale: np.ndarray = scaling_function(data.values) + if not isinstance(mesh_scale, np.ndarray): + raise RuntimeError(f"The scaling_function {scaling_function} must return a vector of dimension 3 (XYZ)") + if mesh_scale.shape == (3, 1): + mesh_scale = mesh_scale.reshape((3,)) + elif mesh_scale.shape != (3,): + raise RuntimeError(f"The scaling_function {scaling_function} must return a vector of dimension 3 (XYZ)") + + if rotation_function is None: + mesh_rotation = np.array([0, 0, 0]) + else: + mesh_rotation: np.ndarray = rotation_function(data.values) + if not isinstance(mesh_rotation, np.ndarray): + raise RuntimeError( + f"The rotation_function {rotation_function} must return a vector of dimension 3 (XYZ)" + ) + if mesh_rotation.shape == (3, 1): + mesh_rotation = mesh_rotation.reshape((3,)) + elif mesh_rotation.shape != (3,): + raise RuntimeError( + f"The rotation_function {rotation_function} must return a vector of dimension 3 (XYZ)" + ) + + if translation_function is None: + mesh_translation = np.array([0, 0, 0]) + else: + mesh_translation: np.ndarray = translation_function(data.values) + if not isinstance(mesh_translation, np.ndarray): + raise RuntimeError( + f"The translation_function {translation_function} must return a vector of dimension 3 (XYZ)" + ) + if mesh_translation.shape == (3, 1): + mesh_translation = mesh_translation.reshape((3,)) + elif mesh_translation.shape != (3,): + raise RuntimeError( + f"The translation_function {translation_function} must return a vector of dimension 3 (XYZ)" + ) + + return MeshFileReal( + mesh_file_name=mesh_file_name, + mesh_color=mesh_color, + mesh_scale=mesh_scale, + mesh_rotation=mesh_rotation, + mesh_translation=mesh_translation, + ) + + def __str__(self): + # Define the print function, so it automatically formats things in the file properly + out_string = "" + out_string += f"\tmeshfile\t{self.mesh_file_name}\n" + if self.mesh_color is not None: + out_string += f"\tmeshcolor\t{self.mesh_color[0]}\t{self.mesh_color[1]}\t{self.mesh_color[2]}\n" + if self.mesh_scale is not None: + out_string += f"\tmeshscale\t{self.mesh_scale[0]}\t{self.mesh_scale[1]}\t{self.mesh_scale[2]}\n" + if self.mesh_rotation is not None and self.mesh_translation is not None: + out_string += f"\tmeshrt\t{self.mesh_rotation[0]}\t{self.mesh_rotation[1]}\t{self.mesh_rotation[2]}\txyz\t{self.mesh_translation[0]}\t{self.mesh_translation[1]}\t{self.mesh_translation[2]}\n" + elif self.mesh_rotation is not None or self.mesh_translation is not None: + raise RuntimeError("The mesh_rotation and mesh_translation must be both defined or both undefined") + return out_string diff --git a/binding/python3/model_creation/mesh_real.py b/binding/python3/model_creation/mesh_real.py index 312239b97..1aef44b1f 100644 --- a/binding/python3/model_creation/mesh_real.py +++ b/binding/python3/model_creation/mesh_real.py @@ -72,5 +72,5 @@ def __str__(self): # Do a sanity check position = np.array(position) p = position if len(position.shape) == 1 else np.nanmean(position, axis=1) - out_string += f"\tmesh {p[0]:0.4f} {p[1]:0.4f} {p[2]:0.4f}\n" + out_string += f"\tmesh\t{p[0]:0.4f}\t{p[1]:0.4f}\t{p[2]:0.4f}\n" return out_string diff --git a/binding/python3/model_creation/muscle.py b/binding/python3/model_creation/muscle.py new file mode 100644 index 000000000..55211413b --- /dev/null +++ b/binding/python3/model_creation/muscle.py @@ -0,0 +1,58 @@ +from typing import Callable +from enum import Enum + +from .protocols import Data +from .muscle_real import MuscleReal, MuscleType, MuscleStateType + + +class Muscle: + def __init__( + self, + name: str, + muscle_type: MuscleType, + state_type: MuscleStateType, + muscle_group: str, + origin_position_function: Callable, + insertion_position_function: Callable, + optimal_length_function: Callable | float, + maximal_force_function: Callable | float, + tendon_slack_length_function: Callable | float, + pennation_angle_function: Callable | float, + maximal_excitation: float = 1, + ): + """ + Parameters + ---------- + name + The name of the muscle + muscle_type + The type of the muscle + """ + self.name = name + self.muscle_type = muscle_type + self.state_type = state_type + self.muscle_group = muscle_group + self.origin_position_function = origin_position_function + self.insertion_position_function = insertion_position_function + self.optimal_length_function = optimal_length_function + self.maximal_force_function = maximal_force_function + self.tendon_slack_length_function = tendon_slack_length_function + self.pennation_angle_function = pennation_angle_function + self.maximal_excitation = maximal_excitation + + def to_muscle(self, model, data: Data) -> MuscleReal: + return MuscleReal.from_data( + data, + model, + self.name, + self.muscle_type, + self.state_type, + self.muscle_group, + self.origin_position_function, + self.insertion_position_function, + self.optimal_length_function, + self.maximal_force_function, + self.tendon_slack_length_function, + self.pennation_angle_function, + self.maximal_excitation, + ) diff --git a/binding/python3/model_creation/muscle_group.py b/binding/python3/model_creation/muscle_group.py new file mode 100644 index 000000000..bd9cb23a3 --- /dev/null +++ b/binding/python3/model_creation/muscle_group.py @@ -0,0 +1,41 @@ +from typing import Callable + +from .protocols import Data + + +class MuscleGroup: + def __init__( + self, + name: str, + origin_parent_name: str, + insertion_parent_name: str, + ): + """ + Parameters + ---------- + name + The name of the new muscle group + origin_parent_name + The name of the parent segment for this muscle group + insertion_parent_name + The name of the insertion segment for this muscle group + """ + # Sanity checks + if not isinstance(name, str): + raise ValueError("The name of the muscle group must be a string.") + if not isinstance(origin_parent_name, str): + raise ValueError("The name of the origin parent segment must be a string.") + if not isinstance(insertion_parent_name, str): + raise ValueError("The name of the insertion parent segment must be a string.") + + self.name = name + self.origin_parent_name = origin_parent_name + self.insertion_parent_name = insertion_parent_name + + def __str__(self): + # Define the print function, so it automatically formats things in the file properly + out_string = f"musclegroup\t{self.name}\n" + out_string += f"\tOriginParent\t{self.origin_parent_name}\n" + out_string += f"\tInsertionParent\t{self.insertion_parent_name}\n" + out_string += "endmusclegroup\n" + return out_string diff --git a/binding/python3/model_creation/muscle_real.py b/binding/python3/model_creation/muscle_real.py new file mode 100644 index 000000000..278a93ab8 --- /dev/null +++ b/binding/python3/model_creation/muscle_real.py @@ -0,0 +1,193 @@ +from typing import Callable + +import numpy as np +from enum import Enum + +from .protocols import Data +from .via_point_real import ViaPointReal + + +class MuscleType(Enum): + HILLTHELEN = "hillthelen" + # TODO: @pariterre to be completed + + +class MuscleStateType(Enum): + DEGROOTE = "DeGroote" + # TODO: @pariterre to be completed + + +class MuscleReal: + def __init__( + self, + name: str, + muscle_type: MuscleType, + state_type: MuscleStateType, + muscle_group: str, + origin_position: tuple[int | float, int | float, int | float] | np.ndarray, + insertion_position: tuple[int | float, int | float, int | float] | np.ndarray, + optimal_length: float, + maximal_force: float, + tendon_slack_length: float, + pennation_angle: float, + maximal_excitation: float, + via_points: list[ViaPointReal] = None, + ): + """ + Parameters + ---------- + name + The name of the new contact + muscle_type + The type of the muscle + state_type + The state type of the muscle + muscle_group + The muscle group the muscle belongs to + origin_position + The origin position of the muscle in the local reference frame of the origin segment @pariterre: please confirm + insertion_position + The insertion position of the muscle the local reference frame of the insertion segment @pariterre: please confirm + optimal_length + The optimal length of the muscle + maximal_force + The maximal force of the muscle can reach + tendon_slack_length + The length of the tendon at rest + pennation_angle + The pennation angle of the muscle + maximal_excitation + The maximal excitation of the muscle (usually 1.0, since it is normalized) + via_points + The via points of the muscle + """ + self.name = name + self.muscle_type = muscle_type + self.state_type = state_type + self.muscle_group = muscle_group + self.origin_position = origin_position if isinstance(origin_position, np.ndarray) else np.array(origin_position) + self.insertion_position = ( + insertion_position if isinstance(insertion_position, np.ndarray) else np.array(insertion_position) + ) + self.optimal_length = optimal_length + self.maximal_force = maximal_force + self.tendon_slack_length = tendon_slack_length + self.pennation_angle = pennation_angle + self.maximal_excitation = maximal_excitation + self.via_points = via_points + + @staticmethod + def from_data( + data: Data, + model, + name: str, + muscle_type: MuscleType, + state_type: MuscleStateType, + muscle_group: str, + origin_position_function: Callable | np.ndarray[float], + insertion_position_function: Callable | np.ndarray[float], + optimal_length_function: Callable | float, + maximal_force_function: Callable | float, + tendon_slack_length_function: Callable | float, + pennation_angle_function: Callable | float, + maximal_excitation: float, + ): + """ + This is a constructor for the Muscle class. It evaluates the function that defines the muscle to get an + actual position + + Parameters + ---------- + data + The data to pick the data from + name + The name of the muscle + muscle_type + The type of the muscle + state_type + The state type of the muscle + muscle_group + The muscle group the muscle belongs to + origin_position_function + The function (f(m) -> np.ndarray, where m is a dict of markers) that defines the origin position of the muscle + insertion_position_function + The function (f(m) -> np.ndarray, where m is a dict of markers) that defines the insertion position of the muscle + optimal_length_function + The function (f(model, m) -> float, where m is a dict of markers) that defines the optimal length of the muscle + maximal_force_function + The function (f(m) -> float, where m is a dict of markers) that defines the maximal force of the muscle + tendon_slack_length_function + The function (f(model, m) -> float, where m is a dict of markers) that defines the tendon slack length of the muscle + pennation_angle_function + The function (f(model, m) -> float, where m is a dict of markers) that defines the pennation angle of the muscle + maximal_excitation + The maximal excitation of the muscle (usually 1.0, since it is normalized) + """ + origin_position: np.ndarray = origin_position_function(data.values) + if not isinstance(origin_position, np.ndarray): + raise RuntimeError( + f"The origin_position_function {origin_position_function} must return a vector of dimension 3 (XYZ)" + ) + if origin_position.shape == (3, 1): + origin_position = origin_position.reshape((3,)) + elif origin_position.shape != (3,): + raise RuntimeError( + f"The origin_position_function {origin_position_function} must return a vector of dimension 3 (XYZ)" + ) + + insertion_position: np.ndarray = insertion_position_function(data.values) + if not isinstance(insertion_position, np.ndarray): + raise RuntimeError( + f"The insertion_position_function {insertion_position_function} must return a vector of dimension 3 (XYZ)" + ) + if insertion_position.shape == (3, 1): + insertion_position = insertion_position.reshape((3,)) + elif insertion_position.shape != (3,): + raise RuntimeError( + f"The insertion_position_function {insertion_position_function} must return a vector of dimension 3 (XYZ)" + ) + + optimal_length: float = optimal_length_function(model, data.values) + if not isinstance(optimal_length, float): + raise RuntimeError(f"The optimal_length_function {optimal_length_function} must return a float") + + maximal_force: float = maximal_force_function(data.values) + if not isinstance(maximal_force, float): + raise RuntimeError(f"The maximal_force_function {maximal_force_function} must return a float") + + tendon_slack_length: float = tendon_slack_length_function(model, data.values) + if not isinstance(tendon_slack_length, float): + raise RuntimeError(f"The tendon_slack_length_function {tendon_slack_length_function} must return a float") + + pennation_angle: float = pennation_angle_function(model, data.values) + if not isinstance(pennation_angle, float): + raise RuntimeError(f"The pennation_angle_function {pennation_angle_function} must return a float") + + return MuscleReal( + name, + muscle_type, + state_type, + muscle_group, + origin_position, + insertion_position, + optimal_length, + maximal_force, + tendon_slack_length, + pennation_angle, + maximal_excitation, + ) + + def __str__(self): + # Define the print function, so it automatically formats things in the file properly + out_string = f"muscle\t{self.name}\n" + out_string += f"\ttype\t{self.muscle_type.value}\n" + out_string += f"\tstatetype\t{self.state_type.value}\n" + out_string += f"\tmusclegroup\t{self.muscle_group}\n" + out_string += f"\toriginposition\t{np.round(self.origin_position[0], 4)}\t{np.round(self.origin_position[1], 4)}\t{np.round(self.origin_position[2], 4)}\n" + out_string += f"\tinsertionposition\t{np.round(self.insertion_position[0], 4)}\t{np.round(self.insertion_position[1], 4)}\t{np.round(self.insertion_position[2], 4)}\n" + out_string += f"\toptimallength\t{self.optimal_length:0.4f}\n" + out_string += f"\tmaximalforce\t{self.maximal_force:0.4f}\n" + out_string += f"\ttendonslacklength\t{self.tendon_slack_length:0.4f}\n" + out_string += f"\tpennationangle\t{self.pennation_angle:0.4f}\n" + out_string += "endmuscle\n" + return out_string diff --git a/binding/python3/model_creation/range_of_motion.py b/binding/python3/model_creation/range_of_motion.py new file mode 100644 index 000000000..23e8328d6 --- /dev/null +++ b/binding/python3/model_creation/range_of_motion.py @@ -0,0 +1,29 @@ +from enum import Enum + + +class Ranges(Enum): + Q = "Q" + Qdot = "Qdot" + + +class RangeOfMotion: + def __init__(self, range_type: Ranges, min_bound: list[float], max_bound: list[float]): + + self.range_type = range_type + self.min_bound = min_bound + self.max_bound = max_bound + + def __str__(self): + # Define the print function, so it automatically formats things in the file properly + if self.range_type == Ranges.Q: + out_string = f"\trangesQ \n" + elif self.range_type == Ranges.Qdot: + out_string = f"\trangesQdot \n" + else: + raise RuntimeError("RangeOfMotion's range_type must be Range.Q or Ranges.Qdot") + + for i_dof in range(len(self.min_bound)): + out_string += f"\t\t{self.min_bound[i_dof]:0.5f}\t{self.max_bound[i_dof]:0.5f}\n" + out_string += "\n" + + return out_string diff --git a/binding/python3/model_creation/segment.py b/binding/python3/model_creation/segment.py index 7b97e92ff..1e09ba21e 100644 --- a/binding/python3/model_creation/segment.py +++ b/binding/python3/model_creation/segment.py @@ -1,7 +1,10 @@ from .inertia_parameters import InertiaParameters from .marker import Marker +from .contact import Contact from .mesh import Mesh +from .mesh_file import MeshFile from .rotations import Rotations +from .range_of_motion import RangeOfMotion, Ranges from .segment_coordinate_system import SegmentCoordinateSystem from .translations import Translations @@ -9,13 +12,16 @@ class Segment: def __init__( self, - name: str = None, + name, parent_name: str = "", translations: Translations = Translations.NONE, rotations: Rotations = Rotations.NONE, + q_ranges: RangeOfMotion = None, + qdot_ranges: RangeOfMotion = None, segment_coordinate_system: SegmentCoordinateSystem = None, inertia_parameters: InertiaParameters = None, mesh: Mesh = None, + mesh_file: MeshFile = None, ): """ Create a new generic segment. @@ -30,20 +36,32 @@ def __init__( The sequence of translation rotations The sequence of rotation + q_ranges + The range of motion of the segment + qdot_ranges + The range of motion of the segment + segment_coordinate_system + The coordinate system of the segment inertia_parameters The inertia parameters of the segment mesh The mesh points of the segment + mesh_file + The mesh file of the segment """ self.name = name self.parent_name = parent_name self.translations = translations self.rotations = rotations + self.q_ranges = q_ranges + self.qdot_ranges = qdot_ranges self.markers = [] + self.contacts = [] self.segment_coordinate_system = segment_coordinate_system self.inertia_parameters = inertia_parameters self.mesh = mesh + self.mesh_file = mesh_file def add_marker(self, marker: Marker): """ @@ -61,3 +79,35 @@ def add_marker(self, marker: Marker): marker.parent_name = self.name self.markers.append(marker) + + def add_contact(self, contact: Contact): + """ + Add a new contact to the segment + + Parameters + ---------- + contact + The contact to add + """ + if contact.parent_name is None: + raise ValueError(f"Contacts must have parents. Contact {contact.name} does not have a parent.") + elif contact.parent_name != self.name: + raise ValueError("The contact name should be the same as the 'key'.") + contact.parent_name = self.name + self.contacts.append(contact) + + def add_range(self, range_type: Ranges, min_bound, max_bound): + """ + Add a new rangeQ to the segment + + Parameters + ---------- + marker + The marker to add + """ + if range_type == Ranges.Q: + self.q_ranges = RangeOfMotion(range_type=range_type, min_bound=min_bound, max_bound=max_bound) + elif range_type == Ranges.Qdot: + self.qdot_ranges = RangeOfMotion(range_type=range_type, min_bound=min_bound, max_bound=max_bound) + else: + raise RuntimeError(f"add_range's range_type must be Ranges.Q or Ranges.Qdot (you have {range_type})") diff --git a/binding/python3/model_creation/segment_real.py b/binding/python3/model_creation/segment_real.py index def007758..946b30a6d 100644 --- a/binding/python3/model_creation/segment_real.py +++ b/binding/python3/model_creation/segment_real.py @@ -1,7 +1,10 @@ from .inertia_parameters_real import InertiaParametersReal from .marker_real import MarkerReal +from .contact import Contact from .mesh_real import MeshReal +from .mesh_file_real import MeshFileReal from .rotations import Rotations +from .range_of_motion import RangeOfMotion from .segment_coordinate_system_real import SegmentCoordinateSystemReal from .translations import Translations @@ -9,41 +12,59 @@ class SegmentReal: def __init__( self, - name: str = None, + name: str, parent_name: str = "", segment_coordinate_system: SegmentCoordinateSystemReal = None, translations: Translations = Translations.NONE, rotations: Rotations = Rotations.NONE, + q_ranges: RangeOfMotion = None, + qdot_ranges: RangeOfMotion = None, inertia_parameters: InertiaParametersReal = None, mesh: MeshReal = None, + mesh_file: MeshFileReal = None, ): self.name = name self.parent_name = parent_name self.translations = translations self.rotations = rotations + self.q_ranges = q_ranges + self.qdot_ranges = qdot_ranges self.markers = [] + self.contacts = [] self.segment_coordinate_system = segment_coordinate_system self.inertia_parameters = inertia_parameters self.mesh = mesh + self.mesh_file = mesh_file def add_marker(self, marker: MarkerReal): self.markers.append(marker) + def add_contact(self, contact: Contact): + if contact.parent_name is None: + raise RuntimeError(f"Contacts must have parents. Contact {contact.name} does not have a parent.") + self.contacts.append(contact) + def __str__(self): # Define the print function, so it automatically formats things in the file properly - out_string = f"segment {self.name}\n" + out_string = f"segment\t{self.name}\n" if self.parent_name: - out_string += f"\tparent {self.parent_name}\n" + out_string += f"\tparent\t{self.parent_name}\n" if self.segment_coordinate_system: - out_string += f"\tRT {self.segment_coordinate_system}\n" + out_string += f"\tRT\t{self.segment_coordinate_system}\n" if self.translations != Translations.NONE: - out_string += f"\ttranslations {self.translations.value}\n" + out_string += f"\ttranslations\t{self.translations.value}\n" if self.rotations != Rotations.NONE: - out_string += f"\trotations {self.rotations.value}\n" + out_string += f"\trotations\t{self.rotations.value}\n" + if self.q_ranges is not None: + out_string += str(self.q_ranges) + if self.qdot_ranges is not None: + out_string += str(self.qdot_ranges) if self.inertia_parameters: out_string += str(self.inertia_parameters) if self.mesh: out_string += str(self.mesh) + if self.mesh_file: + out_string += str(self.mesh_file) out_string += "endsegment\n" # Also print the markers attached to the segment @@ -51,4 +72,11 @@ def __str__(self): for marker in self.markers: marker.parent_name = marker.parent_name if marker.parent_name is not None else self.name out_string += str(marker) + + # Also print the contacts attached to the segment + if self.contacts: + for contact in self.contacts: + contact.parent_name = contact.parent_name + out_string += str(contact) + return out_string diff --git a/binding/python3/model_creation/via_point.py b/binding/python3/model_creation/via_point.py new file mode 100644 index 000000000..914a264ab --- /dev/null +++ b/binding/python3/model_creation/via_point.py @@ -0,0 +1,47 @@ +from typing import Callable + +from .protocols import Data +from .via_point_real import ViaPointReal + + +class ViaPoint: + def __init__( + self, + name: str, + position_function: Callable | str = None, + parent_name: str = None, + muscle_name: str = None, + muscle_group: str = None, + ): + """ + Parameters + ---------- + name + The name of the new via point + position_function + The function (f(m) -> np.ndarray, where m is a dict of markers) that defines the via point with. + parent_name + The name of the parent the via point is attached to + muscle_name + The name of the muscle that passes through this via point + muscle_group + The muscle group the muscle belongs to + """ + self.name = name + position_function = position_function if position_function is not None else self.name + self.position_function = ( + (lambda m, bio: m[position_function]) if isinstance(position_function, str) else position_function + ) + self.parent_name = parent_name + self.muscle_name = muscle_name + self.muscle_group = muscle_group + + def to_via_point(self, data: Data) -> ViaPointReal: + return ViaPointReal.from_data( + data, + self.name, + self.parent_name, + self.muscle_name, + self.muscle_group, + self.position_function, + ) diff --git a/binding/python3/model_creation/via_point_real.py b/binding/python3/model_creation/via_point_real.py new file mode 100644 index 000000000..c7cbcec2c --- /dev/null +++ b/binding/python3/model_creation/via_point_real.py @@ -0,0 +1,89 @@ +from typing import Callable + +import numpy as np + +from .protocols import Data + + +class ViaPointReal: + def __init__( + self, + name: str, + parent_name: str, + muscle_name: str, + muscle_group: str, + position: tuple[int | float, int | float, int | float] | np.ndarray = None, + ): + """ + Parameters + ---------- + name + The name of the new via point + parent_name + The name of the parent the via point is attached to + muscle_name + The name of the muscle that passes through this via point + muscle_group + The muscle group the muscle belongs to + position + The 3d position of the via point in the local reference frame + """ + self.name = name + self.parent_name = parent_name + self.muscle_name = muscle_name + self.muscle_group = muscle_group + if position is None: + position = np.array((0, 0, 0, 1)) + self.position = position if isinstance(position, np.ndarray) else np.array(position) + + @staticmethod + def from_data( + data: Data, + name: str, + parent_name: str, + muscle_name: str, + muscle_group: str, + position_function: Callable, + ): + """ + This is a constructor for the Contact class. It evaluates the function that defines the contact to get an + actual position + + Parameters + ---------- + data + The data to pick the data from + name + The name of the new via point + parent_name + The name of the parent the via point is attached to + muscle_name + The name of the muscle that passes through this via point + muscle_group + The muscle group the muscle belongs to + position_function + The function (f(m) -> np.ndarray, where m is a dict of markers (XYZ1 x time)) that defines the via point in the local joint coordinates. + """ + + # Get the position of the contact points and do some sanity checks + position: np.ndarray = position_function(data.values) + if not isinstance(position, np.ndarray): + raise RuntimeError( + f"The function {position_function} must return a np.ndarray of dimension 3xT (XYZ x time)" + ) + if position.shape == (3, 1): + position = position.reshape((3,)) + elif position.shape != (3,): + raise RuntimeError(f"The function {position_function} must return a vector of dimension 3 (XYZ)") + + return ViaPointReal(name, parent_name, muscle_name, muscle_group, position) + + def __str__(self): + # Define the print function, so it automatically formats things in the file properly + out_string = f"viapoint\t{self.name}\n" + out_string += f"\tparent\t{self.parent_name}\n" + out_string += f"\tmuscle\t{self.muscle_name}\n" + out_string += f"\tmusclegroup\t{self.muscle_group}\n" + out_string += f"\tposition\t{np.round(self.position[0], 4)}\t{np.round(self.position[1], 4)}\t{np.round(self.position[2], 4)}\n" + out_string += "endviapoint\n" + return out_string diff --git a/examples/python3/forwardDynamics.py b/examples/python3/forwardDynamics.py index b09e67432..75b3054eb 100644 --- a/examples/python3/forwardDynamics.py +++ b/examples/python3/forwardDynamics.py @@ -1,3 +1,5 @@ +from pathlib import Path + import numpy as np import biorbd @@ -11,20 +13,25 @@ # Please note that this example will work only with the Eigen backend # +def main(): + # Load a predefined model + current_file_dir = Path(__file__).parent + model = biorbd.Model(f"{current_file_dir}/../pyomecaman.bioMod") + nq = model.nbQ() + nqdot = model.nbQdot() + ntau = model.nbGeneralizedTorque() -# Load a predefined model -model = biorbd.Model("../pyomecaman.bioMod") -nq = model.nbQ() -nqdot = model.nbQdot() -ntau = model.nbGeneralizedTorque() + # Choose a position/velocity/torque to compute dynamics from + Q = np.zeros((nq,)) + Qdot = np.zeros((nqdot,)) + Tau = np.zeros((ntau,)) -# Choose a position/velocity/torque to compute dynamics from -Q = np.zeros((nq,)) -Qdot = np.zeros((nqdot,)) -Tau = np.zeros((ntau,)) + # Proceed with the forward dynamics + Qddot = model.ForwardDynamics(Q, Qdot, Tau) -# Proceed with the forward dynamics -Qddot = model.ForwardDynamics(Q, Qdot, Tau) + # Print them to the console + print(Qddot.to_array()) -# Print them to the console -print(Qddot.to_array()) + +if __name__ == "__main__": + main() diff --git a/examples/python3/forwardDynamicsFromMuscles.py b/examples/python3/forwardDynamicsFromMuscles.py index 21b746496..968922724 100644 --- a/examples/python3/forwardDynamicsFromMuscles.py +++ b/examples/python3/forwardDynamicsFromMuscles.py @@ -1,3 +1,5 @@ +from pathlib import Path + import numpy as np import biorbd @@ -16,66 +18,72 @@ # ACTIVATION-DRIVEN DYNAMICS -# Load a predefined model -model = biorbd.Model("../arm26.bioMod") -nq = model.nbQ() -nqdot = model.nbQdot() -nmus = model.nbMuscles() +def main(): + # Load a predefined model + current_file_dir = Path(__file__).parent + model = biorbd.Model(f"{current_file_dir}/../arm26.bioMod") + nq = model.nbQ() + nqdot = model.nbQdot() + nmus = model.nbMuscles() + + # Choose a state (position/velocity) to compute dynamics from + q = np.zeros((nq,)) + qdot = np.zeros((nqdot,)) + + # Set an arbitrary control to all muscles (half of their maximal activation) + muscles = model.stateSet() + for muscle in muscles: + muscle.setActivation(0.5) # Set muscles activations -# Choose a state (position/velocity) to compute dynamics from -q = np.zeros((nq,)) -qdot = np.zeros((nqdot,)) + # Now one can compute the muscle forces + muscle_forces = model.muscleForces(muscles, q, qdot) -# Set an arbitrary control to all muscles (half of their maximal activation) -muscles = model.stateSet() -for muscle in muscles: - muscle.setActivation(0.5) # Set muscles activations + # Proceed with the computation of joint torque from the muscles + tau = model.muscularJointTorque(muscles, q, qdot) -# Now one can compute the muscle forces -muscle_forces = model.muscleForces(muscles, q, qdot) + # Compute the generalized accelerations using the tau from muscles. + # Please note that in forward dynamics setting, it is usually advised to add + # additional residual torques. You would add them here to tau. + qddot = model.ForwardDynamics(q, qdot, tau) -# Proceed with the computation of joint torque from the muscles -tau = model.muscularJointTorque(muscles, q, qdot) + # Print them to the console + print(qddot.to_array()) -# Compute the generalized accelerations using the tau from muscles. -# Please note that in forward dynamics setting, it is usually advised to add -# additional residual torques. You would add them here to tau. -qddot = model.ForwardDynamics(q, qdot, tau) + # As an extra, let's print the individual muscle forces + print(muscle_forces.to_array()) -# Print them to the console -print(qddot.to_array()) + # qddot needs to be integrated twice to compute the new state (q, qdot). + # Choosing a new control (muscle activation), this small exercise should be repeated to move forward in time. -# As an extra, let's print the individual muscle forces -print(muscle_forces.to_array()) + # EXCITATION-DRIVEN DYNAMICS -# qddot needs to be integrated twice to compute the new state (q, qdot). -# Choosing a new control (muscle activation), this small exercise should be repeated to move forward in time. + # Choose a state (position/velocity/activation) to compute dynamics from + q = np.zeros((nq,)) + qdot = np.zeros((nqdot,)) + act = np.zeros((nmus,)) -# EXCITATION-DRIVEN DYNAMICS + # Set all muscles to their current activation and to an arbitrary excitation of 0.5 + muscles = model.stateSet() + for k, muscle in enumerate(muscles): + muscle.setActivation(act[k]) # Set muscles activations + muscle.setExcitation(0.5) # Set muscles activations -# Choose a state (position/velocity/activation) to compute dynamics from -q = np.zeros((nq,)) -qdot = np.zeros((nqdot,)) -act = np.zeros((nmus,)) + # Compute the derivatives of muscle activations, which should be used in + # a forward integration to compute the time evolution of the muscle activation + actdot = model.activationDot(muscles) -# Set all muscles to their current activation and to an arbitrary excitation of 0.5 -muscles = model.stateSet() -for k, muscle in enumerate(muscles): - muscle.setActivation(act[k]) # Set muscles activations - muscle.setExcitation(0.5) # Set muscles activations + # Print them to the console + print(actdot.to_array()) -# Compute the derivatives of muscle activations, which should be used in -# a forward integration to compute the time evolution of the muscle activation -actdot = model.activationDot(muscles) + # Proceed with the computation of joint torque from the muscles + tau = model.muscularJointTorque(muscles, q, qdot) -# Print them to the console -print(actdot.to_array()) + # Compute the generalized accelerations using the tau from muscles. + qddot = model.ForwardDynamics(q, qdot, tau) -# Proceed with the computation of joint torque from the muscles -tau = model.muscularJointTorque(muscles, q, qdot) + # qddot needs to be integrated twice and actdot integrated once to compute the new state (q, qdot, act). + # Choosing a new control (muscle excitations), this small exercise should be repeated to move forward in time. -# Compute the generalized accelerations using the tau from muscles. -qddot = model.ForwardDynamics(q, qdot, tau) -# qddot needs to be integrated twice and actdot integrated once to compute the new state (q, qdot, act). -# Choosing a new control (muscle excitations), this small exercise should be repeated to move forward in time. +if __name__ == "__main__": + main() diff --git a/examples/python3/forwardKinematics.py b/examples/python3/forwardKinematics.py index 4d6e22409..ca3b8d16f 100644 --- a/examples/python3/forwardKinematics.py +++ b/examples/python3/forwardKinematics.py @@ -1,3 +1,5 @@ +from pathlib import Path + import numpy as np import biorbd @@ -20,34 +22,41 @@ # -# Load a predefined model -model = biorbd.Model("../pyomecaman.bioMod") -nq = model.nbQ() -n_mark = model.nbMarkers() -n_frames = 20 - -# Generate clapping gesture data -qinit = np.array([0, 0, -0.3, 0.35, 1.15, -0.35, 1.15, 0, 0, 0, 0, 0, 0]) -qmid = np.array([0, 0, -0.3, 0.5, 1.15, -0.5, 1.15, 0, 0, 0, 0, 0, 0]) -qfinal = np.array([0, 0, -0.3, 0.35, 1.15, -0.35, 1.15, 0, 0, 0, 0, 0, 0]) -q = np.concatenate((np.linspace(qinit, qmid, n_frames).T, np.linspace(qmid, qfinal, n_frames).T), axis=1) - -# Proceed with the forward kinematics -markers = np.ndarray((3, model.nbMarkers(), 2 * n_frames)) -for i, q in enumerate(q.T): - markers[:, :, i] = np.array([mark.to_array() for mark in model.markers(q)]).T - -# Print the first frame in the console -print(markers[:, :, 0]) - -# Animate the markers -if biorbd_viz_found: - vtkWindow = VtkWindow(background_color=(0.5, 0.5, 0.5)) - vtkModel = VtkModel(vtkWindow, markers_color=(0, 0, 1), markers_size=0.01) - i = 0 - markers = pyomeca.Markers(markers) - while vtkWindow.is_active: - # Update the graph - vtkModel.update_markers(markers[:, :, [i]]) - vtkWindow.update_frame() - i = (i + 1) % markers.shape[2] + +def main(show: bool = True): + # Load a predefined model + current_file_dir = Path(__file__).parent + model = biorbd.Model(f"{current_file_dir}/../pyomecaman.bioMod") + nq = model.nbQ() + n_mark = model.nbMarkers() + n_frames = 20 + + # Generate clapping gesture data + qinit = np.array([0, 0, -0.3, 0.35, 1.15, -0.35, 1.15, 0, 0, 0, 0, 0, 0]) + qmid = np.array([0, 0, -0.3, 0.5, 1.15, -0.5, 1.15, 0, 0, 0, 0, 0, 0]) + qfinal = np.array([0, 0, -0.3, 0.35, 1.15, -0.35, 1.15, 0, 0, 0, 0, 0, 0]) + q = np.concatenate((np.linspace(qinit, qmid, n_frames).T, np.linspace(qmid, qfinal, n_frames).T), axis=1) + + # Proceed with the forward kinematics + markers = np.ndarray((3, model.nbMarkers(), 2 * n_frames)) + for i, q in enumerate(q.T): + markers[:, :, i] = np.array([mark.to_array() for mark in model.markers(q)]).T + + # Print the first frame in the console + print(markers[:, :, 0]) + + # Animate the markers + if show and biorbd_viz_found: + vtkWindow = VtkWindow(background_color=(0.5, 0.5, 0.5)) + vtkModel = VtkModel(vtkWindow, markers_color=(0, 0, 1), markers_size=0.01) + i = 0 + markers = pyomeca.Markers(markers) + while vtkWindow.is_active: + # Update the graph + vtkModel.update_markers(markers[:, :, [i]]) + vtkWindow.update_frame() + i = (i + 1) % markers.shape[2] + + +if __name__ == "__main__": + main() diff --git a/examples/python3/inverseDynamics.py b/examples/python3/inverseDynamics.py index 34a6345c0..eec04ee04 100644 --- a/examples/python3/inverseDynamics.py +++ b/examples/python3/inverseDynamics.py @@ -1,3 +1,5 @@ +from pathlib import Path + import numpy as np import biorbd @@ -12,19 +14,25 @@ # -# Load a predefined model -model = biorbd.Model("../pyomecaman.bioMod") -nq = model.nbQ() -nqdot = model.nbQdot() -nqddot = model.nbQddot() +def main(): + # Load a predefined model + current_file_dir = Path(__file__).parent + model = biorbd.Model(f"{current_file_dir}/../pyomecaman.bioMod") + nq = model.nbQ() + nqdot = model.nbQdot() + nqddot = model.nbQddot() + + # Choose a position/velocity/acceleration to compute dynamics from + Q = np.zeros((nq,)) + Qdot = np.zeros((nqdot,)) + Qddot = np.zeros((nqddot,)) + + # Proceed with the inverse dynamics + Tau = model.InverseDynamics(Q, Qdot, Qddot) -# Choose a position/velocity/acceleration to compute dynamics from -Q = np.zeros((nq,)) -Qdot = np.zeros((nqdot,)) -Qddot = np.zeros((nqddot,)) + # Print them to the console + print(Tau.to_array()) -# Proceed with the inverse dynamics -Tau = model.InverseDynamics(Q, Qdot, Qddot) -# Print them to the console -print(Tau.to_array()) +if __name__ == "__main__": + main() diff --git a/examples/python3/inverseKinematicsKalman.py b/examples/python3/inverseKinematicsKalman.py index 77d122389..289fba4ff 100644 --- a/examples/python3/inverseKinematicsKalman.py +++ b/examples/python3/inverseKinematicsKalman.py @@ -1,3 +1,5 @@ +from pathlib import Path + import numpy as np import biorbd @@ -21,51 +23,56 @@ # Please also note that kalman will be VERY slow if compiled in debug # +def main(show: bool = True): + # Load a predefined model + current_file_dir = Path(__file__).parent + model = biorbd.Model(f"{current_file_dir}/../pyomecaman.bioMod") + nq = model.nbQ() + nb_mus = model.nbMuscles() + n_frames = 20 + + # Generate clapping gesture data + qinit = np.array([0, 0, -0.3, 0.35, 1.15, -0.35, 1.15, 0, 0, 0, 0, 0, 0]) + qmid = np.array([0, 0, -0.3, 0.5, 1.15, -0.5, 1.15, 0, 0, 0, 0, 0, 0]) + qfinal = np.array([0, 0, -0.3, 0.35, 1.15, -0.35, 1.15, 0, 0, 0, 0, 0, 0]) + target_q = np.concatenate((np.linspace(qinit, qmid, n_frames).T, np.linspace(qmid, qfinal, n_frames).T), axis=1) + markers = np.ndarray((3, model.nbMarkers(), 2 * n_frames)) + for i, q in enumerate(target_q.T): + markers[:, :, i] = np.array([mark.to_array() for mark in model.markers(q)]).T -# Load a predefined model -model = biorbd.Model("../pyomecaman.bioMod") -nq = model.nbQ() -nb_mus = model.nbMuscles() -n_frames = 20 + # If ones was using c3d data opened using ezc3d + # import ezc3d + # c3d = ezc3d.c3d(data_filename) + # markers = c3d['data']['points'][:3, :, :] # XYZ1 x markers x time_frame -# Generate clapping gesture data -qinit = np.array([0, 0, -0.3, 0.35, 1.15, -0.35, 1.15, 0, 0, 0, 0, 0, 0]) -qmid = np.array([0, 0, -0.3, 0.5, 1.15, -0.5, 1.15, 0, 0, 0, 0, 0, 0]) -qfinal = np.array([0, 0, -0.3, 0.35, 1.15, -0.35, 1.15, 0, 0, 0, 0, 0, 0]) -target_q = np.concatenate((np.linspace(qinit, qmid, n_frames).T, np.linspace(qmid, qfinal, n_frames).T), axis=1) -markers = np.ndarray((3, model.nbMarkers(), 2 * n_frames)) -for i, q in enumerate(target_q.T): - markers[:, :, i] = np.array([mark.to_array() for mark in model.markers(q)]).T + # Dispatch markers in biorbd structure so EKF can use it + markersOverFrames = [] + for i in range(markers.shape[2]): + markersOverFrames.append([biorbd.NodeSegment(m) for m in markers[:, :, i].T]) -# If ones was using c3d data opened using ezc3d -# import ezc3d -# c3d = ezc3d.c3d(data_filename) -# markers = c3d['data']['points'][:3, :, :] # XYZ1 x markers x time_frame + # Create a Kalman filter structure + freq = 100 # Hz + params = biorbd.KalmanParam(freq) + kalman = biorbd.KalmanReconsMarkers(model, params) -# Dispatch markers in biorbd structure so EKF can use it -markersOverFrames = [] -for i in range(markers.shape[2]): - markersOverFrames.append([biorbd.NodeSegment(m) for m in markers[:, :, i].T]) + # Perform the kalman filter for each frame (the first frame is much longer than the next) + Q = biorbd.GeneralizedCoordinates(model) + Qdot = biorbd.GeneralizedVelocity(model) + Qddot = biorbd.GeneralizedAcceleration(model) + q_recons = np.ndarray((model.nbQ(), len(markersOverFrames))) + for i, targetMarkers in enumerate(markersOverFrames): + kalman.reconstructFrame(model, targetMarkers, Q, Qdot, Qddot) + q_recons[:, i] = Q.to_array() -# Create a Kalman filter structure -freq = 100 # Hz -params = biorbd.KalmanParam(freq) -kalman = biorbd.KalmanReconsMarkers(model, params) + # Print the kinematics to the console + print(f"Frame {i}\nExpected Q = {target_q[:, i]}\nCurrent Q = {q_recons[:, i]}\n") -# Perform the kalman filter for each frame (the first frame is much longer than the next) -Q = biorbd.GeneralizedCoordinates(model) -Qdot = biorbd.GeneralizedVelocity(model) -Qddot = biorbd.GeneralizedAcceleration(model) -q_recons = np.ndarray((model.nbQ(), len(markersOverFrames))) -for i, targetMarkers in enumerate(markersOverFrames): - kalman.reconstructFrame(model, targetMarkers, Q, Qdot, Qddot) - q_recons[:, i] = Q.to_array() + # Animate the results if biorbd viz is installed + if show and biorbd_viz_found: + b = bioviz.Viz(loaded_model=model) + b.load_movement(q_recons) + b.exec() - # Print the kinematics to the console - print(f"Frame {i}\nExpected Q = {target_q[:, i]}\nCurrent Q = {q_recons[:, i]}\n") -# Animate the results if biorbd viz is installed -if biorbd_viz_found: - b = bioviz.Viz(loaded_model=model) - b.load_movement(q_recons) - b.exec() +if __name__ == "__main__": + main() diff --git a/examples/python3/meshFiles/pendulum.STL b/examples/python3/meshFiles/pendulum.STL new file mode 100644 index 000000000..fb6f4bc66 Binary files /dev/null and b/examples/python3/meshFiles/pendulum.STL differ diff --git a/examples/python3/modelCreation.py b/examples/python3/modelCreation.py index 19a8671ae..5130091c6 100644 --- a/examples/python3/modelCreation.py +++ b/examples/python3/modelCreation.py @@ -1,4 +1,5 @@ import os +from pathlib import Path import numpy as np import biorbd @@ -11,22 +12,32 @@ MarkerReal, Mesh, MeshReal, + MeshFile, Segment, SegmentReal, SegmentCoordinateSystemReal, SegmentCoordinateSystem, + Contact, + MuscleGroup, + Muscle, + MuscleType, + MuscleStateType, Translations, Rotations, + RangeOfMotion, + Ranges, + ViaPoint, ) import ezc3d -from .de_leva import DeLevaTable +from de_leva import DeLevaTable # # This examples shows how to # 1. Create a model from scratch using specified dimensions (model_creation_from_static) -# 1. Create a model from scratch using a template with marker names (model_creation_from_data) +# 2. Create a complex model from scratch (complex_model_from_scratch) +# 3. Create a model from scratch using a template with marker names (model_creation_from_data) # # Please note that this example will work only with the Eigen backend # @@ -45,28 +56,31 @@ def model_creation_from_static_trial(remove_temporary: bool = True): bio_model = BiomechanicalModelReal() # The trunk segment - bio_model["TRUNK"] = SegmentReal( + bio_model.segments["TRUNK"] = SegmentReal( + name="TRUNK", translations=Translations.YZ, rotations=Rotations.X, mesh=MeshReal(((0, 0, 0), (0, 0, 0.53))), ) - bio_model["TRUNK"].add_marker(MarkerReal(name="PELVIS", parent_name="TRUNK")) + bio_model.segments["TRUNK"].add_marker(MarkerReal(name="PELVIS", parent_name="TRUNK")) # The head segment - bio_model["HEAD"] = SegmentReal( + bio_model.segments["HEAD"] = SegmentReal( + name="HEAD", parent_name="TRUNK", segment_coordinate_system=SegmentCoordinateSystemReal.from_euler_and_translation( (0, 0, 0), "xyz", (0, 0, 0.53) ), mesh=MeshReal(((0, 0, 0), (0, 0, 0.24))), ) - bio_model["HEAD"].add_marker(MarkerReal(name="BOTTOM_HEAD", parent_name="HEAD", position=(0, 0, 0))) - bio_model["HEAD"].add_marker(MarkerReal(name="TOP_HEAD", parent_name="HEAD", position=(0, 0, 0.24))) - bio_model["HEAD"].add_marker(MarkerReal(name="HEAD_Z", parent_name="HEAD", position=(0, 0, 0.24))) - bio_model["HEAD"].add_marker(MarkerReal(name="HEAD_XZ", parent_name="HEAD", position=(0.24, 0, 0.24))) + bio_model.segments["HEAD"].add_marker(MarkerReal(name="BOTTOM_HEAD", parent_name="HEAD", position=(0, 0, 0))) + bio_model.segments["HEAD"].add_marker(MarkerReal(name="TOP_HEAD", parent_name="HEAD", position=(0, 0, 0.24))) + bio_model.segments["HEAD"].add_marker(MarkerReal(name="HEAD_Z", parent_name="HEAD", position=(0, 0, 0.24))) + bio_model.segments["HEAD"].add_marker(MarkerReal(name="HEAD_XZ", parent_name="HEAD", position=(0.24, 0, 0.24))) # The arm segment - bio_model["UPPER_ARM"] = SegmentReal( + bio_model.segments["UPPER_ARM"] = SegmentReal( + name="UPPER_ARM", parent_name="TRUNK", segment_coordinate_system=SegmentCoordinateSystemReal.from_euler_and_translation( (0, 0, 0), "xyz", (0, 0, 0.53) @@ -74,11 +88,11 @@ def model_creation_from_static_trial(remove_temporary: bool = True): rotations=Rotations.X, mesh=MeshReal(((0, 0, 0), (0, 0, -0.28))), ) - bio_model["UPPER_ARM"].add_marker(MarkerReal(name="SHOULDER", parent_name="UPPER_ARM", position=(0, 0, 0))) - bio_model["UPPER_ARM"].add_marker(MarkerReal(name="SHOULDER_X", parent_name="UPPER_ARM", position=(1, 0, 0))) - bio_model["UPPER_ARM"].add_marker(MarkerReal(name="SHOULDER_XY", parent_name="UPPER_ARM", position=(1, 1, 0))) + bio_model.segments["UPPER_ARM"].add_marker(MarkerReal(name="SHOULDER", parent_name="UPPER_ARM", position=(0, 0, 0))) + bio_model.segments["UPPER_ARM"].add_marker(MarkerReal(name="SHOULDER_X", parent_name="UPPER_ARM", position=(1, 0, 0))) + bio_model.segments["UPPER_ARM"].add_marker(MarkerReal(name="SHOULDER_XY", parent_name="UPPER_ARM", position=(1, 1, 0))) - bio_model["LOWER_ARM"] = SegmentReal( + bio_model.segments["LOWER_ARM"] = SegmentReal( name="LOWER_ARM", parent_name="UPPER_ARM", segment_coordinate_system=SegmentCoordinateSystemReal.from_euler_and_translation( @@ -86,11 +100,11 @@ def model_creation_from_static_trial(remove_temporary: bool = True): ), mesh=MeshReal(((0, 0, 0), (0, 0, -0.27))), ) - bio_model["LOWER_ARM"].add_marker(MarkerReal(name="ELBOW", parent_name="LOWER_ARM", position=(0, 0, 0))) - bio_model["LOWER_ARM"].add_marker(MarkerReal(name="ELBOW_Y", parent_name="LOWER_ARM", position=(0, 1, 0))) - bio_model["LOWER_ARM"].add_marker(MarkerReal(name="ELBOW_XY", parent_name="LOWER_ARM", position=(1, 1, 0))) + bio_model.segments["LOWER_ARM"].add_marker(MarkerReal(name="ELBOW", parent_name="LOWER_ARM", position=(0, 0, 0))) + bio_model.segments["LOWER_ARM"].add_marker(MarkerReal(name="ELBOW_Y", parent_name="LOWER_ARM", position=(0, 1, 0))) + bio_model.segments["LOWER_ARM"].add_marker(MarkerReal(name="ELBOW_XY", parent_name="LOWER_ARM", position=(1, 1, 0))) - bio_model["HAND"] = SegmentReal( + bio_model.segments["HAND"] = SegmentReal( name="HAND", parent_name="LOWER_ARM", segment_coordinate_system=SegmentCoordinateSystemReal.from_euler_and_translation( @@ -98,24 +112,24 @@ def model_creation_from_static_trial(remove_temporary: bool = True): ), mesh=MeshReal(((0, 0, 0), (0, 0, -0.19))), ) - bio_model["HAND"].add_marker(MarkerReal(name="WRIST", parent_name="HAND", position=(0, 0, 0))) - bio_model["HAND"].add_marker(MarkerReal(name="FINGER", parent_name="HAND", position=(0, 0, -0.19))) - bio_model["HAND"].add_marker(MarkerReal(name="HAND_Y", parent_name="HAND", position=(0, 1, 0))) - bio_model["HAND"].add_marker(MarkerReal(name="HAND_YZ", parent_name="HAND", position=(0, 1, 1))) + bio_model.segments["HAND"].add_marker(MarkerReal(name="WRIST", parent_name="HAND", position=(0, 0, 0))) + bio_model.segments["HAND"].add_marker(MarkerReal(name="FINGER", parent_name="HAND", position=(0, 0, -0.19))) + bio_model.segments["HAND"].add_marker(MarkerReal(name="HAND_Y", parent_name="HAND", position=(0, 1, 0))) + bio_model.segments["HAND"].add_marker(MarkerReal(name="HAND_YZ", parent_name="HAND", position=(0, 1, 1))) # The thigh segment - bio_model["THIGH"] = SegmentReal( + bio_model.segments["THIGH"] = SegmentReal( name="THIGH", parent_name="TRUNK", rotations=Rotations.X, mesh=MeshReal(((0, 0, 0), (0, 0, -0.42))), ) - bio_model["THIGH"].add_marker(MarkerReal(name="THIGH_ORIGIN", parent_name="THIGH", position=(0, 0, 0))) - bio_model["THIGH"].add_marker(MarkerReal(name="THIGH_X", parent_name="THIGH", position=(1, 0, 0))) - bio_model["THIGH"].add_marker(MarkerReal(name="THIGH_Y", parent_name="THIGH", position=(0, 1, 0))) + bio_model.segments["THIGH"].add_marker(MarkerReal(name="THIGH_ORIGIN", parent_name="THIGH", position=(0, 0, 0))) + bio_model.segments["THIGH"].add_marker(MarkerReal(name="THIGH_X", parent_name="THIGH", position=(1, 0, 0))) + bio_model.segments["THIGH"].add_marker(MarkerReal(name="THIGH_Y", parent_name="THIGH", position=(0, 1, 0))) # The shank segment - bio_model["SHANK"] = SegmentReal( + bio_model.segments["SHANK"] = SegmentReal( name="SHANK", parent_name="THIGH", segment_coordinate_system=SegmentCoordinateSystemReal.from_euler_and_translation( @@ -124,12 +138,12 @@ def model_creation_from_static_trial(remove_temporary: bool = True): rotations=Rotations.X, mesh=MeshReal(((0, 0, 0), (0, 0, -0.43))), ) - bio_model["SHANK"].add_marker(MarkerReal(name="KNEE", parent_name="SHANK", position=(0, 0, 0))) - bio_model["SHANK"].add_marker(MarkerReal(name="KNEE_Z", parent_name="SHANK", position=(0, 0, 1))) - bio_model["SHANK"].add_marker(MarkerReal(name="KNEE_XZ", parent_name="SHANK", position=(1, 0, 1))) + bio_model.segments["SHANK"].add_marker(MarkerReal(name="KNEE", parent_name="SHANK", position=(0, 0, 0))) + bio_model.segments["SHANK"].add_marker(MarkerReal(name="KNEE_Z", parent_name="SHANK", position=(0, 0, 1))) + bio_model.segments["SHANK"].add_marker(MarkerReal(name="KNEE_XZ", parent_name="SHANK", position=(1, 0, 1))) # The foot segment - bio_model["FOOT"] = SegmentReal( + bio_model.segments["FOOT"] = SegmentReal( name="FOOT", parent_name="SHANK", segment_coordinate_system=SegmentCoordinateSystemReal.from_euler_and_translation( @@ -138,10 +152,10 @@ def model_creation_from_static_trial(remove_temporary: bool = True): rotations=Rotations.X, mesh=MeshReal(((0, 0, 0), (0, 0, 0.25))), ) - bio_model["FOOT"].add_marker(MarkerReal(name="ANKLE", parent_name="FOOT", position=(0, 0, 0))) - bio_model["FOOT"].add_marker(MarkerReal(name="TOE", parent_name="FOOT", position=(0, 0, 0.25))) - bio_model["FOOT"].add_marker(MarkerReal(name="ANKLE_Z", parent_name="FOOT", position=(0, 0, 1))) - bio_model["FOOT"].add_marker(MarkerReal(name="ANKLE_YZ", parent_name="FOOT", position=(0, 1, 1))) + bio_model.segments["FOOT"].add_marker(MarkerReal(name="ANKLE", parent_name="FOOT", position=(0, 0, 0))) + bio_model.segments["FOOT"].add_marker(MarkerReal(name="TOE", parent_name="FOOT", position=(0, 0, 0.25))) + bio_model.segments["FOOT"].add_marker(MarkerReal(name="ANKLE_Z", parent_name="FOOT", position=(0, 0, 1))) + bio_model.segments["FOOT"].add_marker(MarkerReal(name="ANKLE_YZ", parent_name="FOOT", position=(0, 1, 1))) # Put the model together, print it and print it to a bioMod file bio_model.write(kinematic_model_file_path) @@ -156,6 +170,80 @@ def model_creation_from_static_trial(remove_temporary: bool = True): os.remove(kinematic_model_file_path) + +def complex_model_from_scratch(mesh_path, remove_temporary: bool = True): + """ + We define a new model by feeding in the actual dimension and position of the model. + Please note that this model is not a human, it is only used to show the functionalities of the model creation module. + """ + + kinematic_model_file_path = "temporary_complex.bioMod" + + # Create a model holder + bio_model = BiomechanicalModel() + + # The ground segment + bio_model.segments["GROUND"] = Segment(name="GROUND") + + # The pendulum segment + bio_model.segments["PENDULUM"] = Segment( + name="PENDULUM", + translations=Translations.XYZ, + rotations=Rotations.X, + q_ranges=RangeOfMotion(range_type=Ranges.Q, min_bound=[-1, -1, -1, -np.pi], max_bound=[1, 1, 1, np.pi]), + qdot_ranges=RangeOfMotion(range_type=Ranges.Qdot, min_bound=[-10, -10, -10, -np.pi*10], max_bound=[10, 10, 10, np.pi*10]), + mesh_file=MeshFile(mesh_file_name=mesh_path, + mesh_color=np.array([0, 0, 1]), + scaling_function=lambda m: np.array([1, 1, 10]), + rotation_function=lambda m: np.array([np.pi/2, 0, 0]), + translation_function=lambda m: np.array([0.1, 0, 0])), + ) + # The pendulum segment contact point + bio_model.segments["PENDULUM"].add_contact(Contact(name="PENDULUM_CONTACT", + function=lambda m: np.array([0, 0, 0]), + parent_name="PENDULUM", + axis=Translations.XYZ)) + + # The pendulum muscle group + bio_model.muscle_groups["PENDULUM_MUSCLE_GROUP"] = MuscleGroup(name="PENDULUM_MUSCLE_GROUP", + origin_parent_name="GROUND", + insertion_parent_name="PENDULUM") + + # The pendulum muscle + bio_model.muscles["PENDULUM_MUSCLE"] = Muscle("PENDULUM_MUSCLE", + muscle_type=MuscleType.HILLTHELEN, + state_type=MuscleStateType.DEGROOTE, + muscle_group="PENDULUM_MUSCLE_GROUP", + origin_position_function=lambda m: np.array([0, 0, 0]), + insertion_position_function=lambda m: np.array([0, 0, 1]), + optimal_length_function=lambda model, m: 0.1, + maximal_force_function=lambda m: 100.0, + tendon_slack_length_function=lambda model, m: 0.05, + pennation_angle_function=lambda model, m: 0.05, + maximal_excitation=1) + bio_model.via_points["PENDULUM_MUSCLE"] = ViaPoint("PENDULUM_MUSCLE", + position_function=lambda m: np.array([0, 0, 0.5]), + parent_name="PENDULUM", + muscle_name="PENDULUM_MUSCLE", + muscle_group="PENDULUM_MUSCLE_GROUP", + ) + + + # Put the model together, print it and print it to a bioMod file + bio_model.write(kinematic_model_file_path, {}) + + model = biorbd.Model(kinematic_model_file_path) + assert model.nbQ() == 4 + assert model.nbSegment() == 2 + assert model.nbMarkers() == 0 + assert model.nbMuscles() == 1 + assert model.nbMuscleGroups() == 1 + assert model.nbContacts() == 3 + + if remove_temporary: + os.remove(kinematic_model_file_path) + + def model_creation_from_measured_data(remove_temporary: bool = True): """ We are using the previous model to define a new model based on the position of the markers. This is solely so we @@ -192,14 +280,16 @@ def write_markers_to_c3d(save_path: str, model: biorbd.Model): model = BiomechanicalModel() de_leva = DeLevaTable(total_mass=100, sex="female") - model["TRUNK"] = Segment( + model.segments["TRUNK"] = Segment( + name="TRUNK", translations=Translations.YZ, rotations=Rotations.X, inertia_parameters=de_leva["TRUNK"], ) - model["TRUNK"].add_marker(Marker("PELVIS")) + model.segments["TRUNK"].add_marker(Marker("PELVIS")) - model["HEAD"] = Segment( + model.segments["HEAD"] = Segment( + name="HEAD", parent_name="TRUNK", segment_coordinate_system=SegmentCoordinateSystem( "BOTTOM_HEAD", @@ -210,13 +300,13 @@ def write_markers_to_c3d(save_path: str, model: biorbd.Model): mesh=Mesh(("BOTTOM_HEAD", "TOP_HEAD", "HEAD_Z", "HEAD_XZ", "BOTTOM_HEAD")), inertia_parameters=de_leva["HEAD"], ) - model["HEAD"].add_marker(Marker("BOTTOM_HEAD")) - model["HEAD"].add_marker(Marker("TOP_HEAD")) - model["HEAD"].add_marker(Marker("HEAD_Z")) - model["HEAD"].add_marker(Marker("HEAD_XZ")) + model.segments["HEAD"].add_marker(Marker("BOTTOM_HEAD")) + model.segments["HEAD"].add_marker(Marker("TOP_HEAD")) + model.segments["HEAD"].add_marker(Marker("HEAD_Z")) + model.segments["HEAD"].add_marker(Marker("HEAD_XZ")) - model["UPPER_ARM"] = Segment( - "UPPER_ARM", + model.segments["UPPER_ARM"] = Segment( + name="UPPER_ARM", parent_name="TRUNK", rotations=Rotations.X, segment_coordinate_system=SegmentCoordinateSystem( @@ -227,11 +317,12 @@ def write_markers_to_c3d(save_path: str, model: biorbd.Model): ), inertia_parameters=de_leva["UPPER_ARM"], ) - model["UPPER_ARM"].add_marker(Marker("SHOULDER")) - model["UPPER_ARM"].add_marker(Marker("SHOULDER_X")) - model["UPPER_ARM"].add_marker(Marker("SHOULDER_XY")) + model.segments["UPPER_ARM"].add_marker(Marker("SHOULDER")) + model.segments["UPPER_ARM"].add_marker(Marker("SHOULDER_X")) + model.segments["UPPER_ARM"].add_marker(Marker("SHOULDER_XY")) - model["LOWER_ARM"] = Segment( + model.segments["LOWER_ARM"] = Segment( + name="LOWER_ARM", parent_name="UPPER_ARM", segment_coordinate_system=SegmentCoordinateSystem( origin="ELBOW", @@ -241,11 +332,12 @@ def write_markers_to_c3d(save_path: str, model: biorbd.Model): ), inertia_parameters=de_leva["LOWER_ARM"], ) - model["LOWER_ARM"].add_marker(Marker("ELBOW")) - model["LOWER_ARM"].add_marker(Marker("ELBOW_Y")) - model["LOWER_ARM"].add_marker(Marker("ELBOW_XY")) + model.segments["LOWER_ARM"].add_marker(Marker("ELBOW")) + model.segments["LOWER_ARM"].add_marker(Marker("ELBOW_Y")) + model.segments["LOWER_ARM"].add_marker(Marker("ELBOW_XY")) - model["HAND"] = Segment( + model.segments["HAND"] = Segment( + name="HAND", parent_name="LOWER_ARM", segment_coordinate_system=SegmentCoordinateSystem( origin="WRIST", @@ -255,12 +347,13 @@ def write_markers_to_c3d(save_path: str, model: biorbd.Model): ), inertia_parameters=de_leva["HAND"], ) - model["HAND"].add_marker(Marker("WRIST")) - model["HAND"].add_marker(Marker("FINGER")) - model["HAND"].add_marker(Marker("HAND_Y")) - model["HAND"].add_marker(Marker("HAND_YZ")) + model.segments["HAND"].add_marker(Marker("WRIST")) + model.segments["HAND"].add_marker(Marker("FINGER")) + model.segments["HAND"].add_marker(Marker("HAND_Y")) + model.segments["HAND"].add_marker(Marker("HAND_YZ")) - model["THIGH"] = Segment( + model.segments["THIGH"] = Segment( + name="THIGH", parent_name="TRUNK", rotations=Rotations.X, segment_coordinate_system=SegmentCoordinateSystem( @@ -271,11 +364,12 @@ def write_markers_to_c3d(save_path: str, model: biorbd.Model): ), inertia_parameters=de_leva["THIGH"], ) - model["THIGH"].add_marker(Marker("THIGH_ORIGIN")) - model["THIGH"].add_marker(Marker("THIGH_X")) - model["THIGH"].add_marker(Marker("THIGH_Y")) + model.segments["THIGH"].add_marker(Marker("THIGH_ORIGIN")) + model.segments["THIGH"].add_marker(Marker("THIGH_X")) + model.segments["THIGH"].add_marker(Marker("THIGH_Y")) - model["SHANK"] = Segment( + model.segments["SHANK"] = Segment( + name="SHANK", parent_name="THIGH", rotations=Rotations.X, segment_coordinate_system=SegmentCoordinateSystem( @@ -286,11 +380,12 @@ def write_markers_to_c3d(save_path: str, model: biorbd.Model): ), inertia_parameters=de_leva["SHANK"], ) - model["SHANK"].add_marker(Marker("KNEE")) - model["SHANK"].add_marker(Marker("KNEE_Z")) - model["SHANK"].add_marker(Marker("KNEE_XZ")) + model.segments["SHANK"].add_marker(Marker("KNEE")) + model.segments["SHANK"].add_marker(Marker("KNEE_Z")) + model.segments["SHANK"].add_marker(Marker("KNEE_XZ")) - model["FOOT"] = Segment( + model.segments["FOOT"] = Segment( + name="FOOT", parent_name="SHANK", rotations=Rotations.X, segment_coordinate_system=SegmentCoordinateSystem( @@ -301,10 +396,10 @@ def write_markers_to_c3d(save_path: str, model: biorbd.Model): ), inertia_parameters=de_leva["FOOT"], ) - model["FOOT"].add_marker(Marker("ANKLE")) - model["FOOT"].add_marker(Marker("TOE")) - model["FOOT"].add_marker(Marker("ANKLE_Z")) - model["FOOT"].add_marker(Marker("ANKLE_YZ")) + model.segments["FOOT"].add_marker(Marker("ANKLE")) + model.segments["FOOT"].add_marker(Marker("TOE")) + model.segments["FOOT"].add_marker(Marker("ANKLE_Z")) + model.segments["FOOT"].add_marker(Marker("ANKLE_YZ")) # Put the model together, print it and print it to a bioMod file model.write(kinematic_model_file_path, C3dData(c3d_file_path)) @@ -324,6 +419,10 @@ def main(): # Create the model from user defined dimensions model_creation_from_static_trial() + # Cre a complex model from scratch + current_path_file = Path(__file__).parent + complex_model_from_scratch(mesh_path=f"{current_path_file}/meshFiles/pendulum.STL") + # Create the model from a data file and markers as template model_creation_from_measured_data() diff --git a/examples/python3/staticOptimization.py b/examples/python3/staticOptimization.py index 5ac793185..38550e2e5 100644 --- a/examples/python3/staticOptimization.py +++ b/examples/python3/staticOptimization.py @@ -1,3 +1,5 @@ +from pathlib import Path + import numpy as np import biorbd @@ -13,32 +15,42 @@ # -# Load a predefined model -model = biorbd.Model("../arm26.bioMod") -nq = model.nbQ() -nqdot = model.nbQdot() -nqddot = model.nbQddot() -ntau = model.nbGeneralizedTorque() -n_frames = 3 - -# Choose a position/velocity/torque to compute muscle activations from. -# If only one frame the Vector are not mandatory and the Static Optimization function can be called -# directly with numpy arrays -Q = biorbd.VecBiorbdGeneralizedCoordinates() -Qdot = biorbd.VecBiorbdGeneralizedVelocity() -Qddot = biorbd.VecBiorbdGeneralizedAcceleration() -Tau = biorbd.VecBiorbdGeneralizedTorque() -for i in range(n_frames): - Q.append(np.zeros((nq,))) - Qdot.append(np.zeros((nqdot,))) - Qddot.append(np.zeros((nqddot,))) - Tau.append(model.InverseDynamics(Q[i], Qdot[i], Qddot[i])) - -# Proceed with the static optimization -optim = biorbd.StaticOptimization(model, Q, Qdot, Tau) -optim.run() -muscleActivationsPerFrame = optim.finalSolution() - -# Print them to the console -for activations in muscleActivationsPerFrame: - print(activations.to_array()) +def main(): + # Load a predefined model + current_file_dir = Path(__file__).parent + model = biorbd.Model(f"{current_file_dir}/../arm26.bioMod") + nq = model.nbQ() + nqdot = model.nbQdot() + nqddot = model.nbQddot() + ntau = model.nbGeneralizedTorque() + n_frames = 3 + + # Choose a position/velocity/torque to compute muscle activations from. + # If only one frame the Vector are not mandatory and the Static Optimization function can be called + # directly with numpy arrays + Q = biorbd.VecBiorbdGeneralizedCoordinates() + Qdot = biorbd.VecBiorbdGeneralizedVelocity() + Qddot = biorbd.VecBiorbdGeneralizedAcceleration() + Tau = biorbd.VecBiorbdGeneralizedTorque() + for i in range(n_frames): + Q.append(np.zeros((nq,))) + Qdot.append(np.zeros((nqdot,))) + Qddot.append(np.zeros((nqddot,))) + Tau.append(model.InverseDynamics(Q[i], Qdot[i], Qddot[i])) + + # If biorbd was compiled with STATIC_OPTIM=OFF, biorbd.StaticOptimization won't exist + if not hasattr(biorbd, "StaticOptimization"): + raise RuntimeError("In order to use StaticOptimization, biorbd must be compiled with STATIC_OPTIM=ON") + + # Proceed with the static optimization + optim = biorbd.StaticOptimization(model, Q, Qdot, Tau) + optim.run() + muscleActivationsPerFrame = optim.finalSolution() + + # Print them to the console + for activations in muscleActivationsPerFrame: + print(activations.to_array()) + + +if __name__ == "__main__": + main() diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 65a5aecc1..381a685c9 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -118,13 +118,13 @@ if (BINDER_PYTHON3) endif() file(GLOB BIORBD_PYTHON3_TEST_FILES - "${CMAKE_SOURCE_DIR}/test/binding/Python3/*.py") + "${CMAKE_SOURCE_DIR}/test/binding/python3/*.py") file(COPY ${BIORBD_PYTHON3_TEST_FILES} - DESTINATION "${CMAKE_CURRENT_BINARY_DIR}/binding/Python3") + DESTINATION "${CMAKE_CURRENT_BINARY_DIR}/binding/python3") file(GLOB BIORBD_PYTHON3_TEST_FILES_EXTRA - "${CMAKE_SOURCE_DIR}/test/binding/Python3/de_leva/*.py") + "${CMAKE_SOURCE_DIR}/test/binding/python3/de_leva/*.py") file(COPY ${BIORBD_PYTHON3_TEST_FILES_EXTRA} - DESTINATION "${CMAKE_CURRENT_BINARY_DIR}/binding/Python3/de_leva") + DESTINATION "${CMAKE_CURRENT_BINARY_DIR}/binding/python3/de_leva") endif() # This is so you can do 'make test' to see all your tests run, instead of diff --git a/test/binding/Python3/test_model_creation.py b/test/binding/Python3/test_model_creation.py deleted file mode 100644 index 7ecb086c5..000000000 --- a/test/binding/Python3/test_model_creation.py +++ /dev/null @@ -1,331 +0,0 @@ -import os -import pytest - -import numpy as np - - -brbd_to_test = [] -try: - import biorbd - - brbd_to_test.append(biorbd) -except ModuleNotFoundError: - pass - - -import ezc3d - - -@pytest.mark.parametrize("brbd", brbd_to_test) -def test_model_creation_from_static(brbd, remove_temporary: bool = True): - """ - Produces a model from real data - """ - from biorbd.model_creation import ( - Axis, - BiomechanicalModel, - BiomechanicalModelReal, - C3dData, - Marker, - MarkerReal, - Mesh, - MeshReal, - Segment, - SegmentReal, - SegmentCoordinateSystemReal, - SegmentCoordinateSystem, - Translations, - Rotations, - ) - - kinematic_model_file_path = "temporary.bioMod" - - # Create a model holder - bio_model = BiomechanicalModelReal() - - # The trunk segment - bio_model["TRUNK"] = SegmentReal( - translations=Translations.YZ, - rotations=Rotations.X, - mesh=MeshReal(((0, 0, 0), (0, 0, 0.53))), - ) - bio_model["TRUNK"].add_marker(MarkerReal(name="PELVIS", parent_name="TRUNK")) - - # The head segment - bio_model["HEAD"] = SegmentReal( - parent_name="TRUNK", - segment_coordinate_system=SegmentCoordinateSystemReal.from_euler_and_translation( - (0, 0, 0), "xyz", (0, 0, 0.53) - ), - mesh=MeshReal(((0, 0, 0), (0, 0, 0.24))), - ) - bio_model["HEAD"].add_marker(MarkerReal(name="BOTTOM_HEAD", parent_name="HEAD", position=(0, 0, 0))) - bio_model["HEAD"].add_marker(MarkerReal(name="TOP_HEAD", parent_name="HEAD", position=(0, 0, 0.24))) - bio_model["HEAD"].add_marker(MarkerReal(name="HEAD_Z", parent_name="HEAD", position=(0, 0, 0.24))) - bio_model["HEAD"].add_marker(MarkerReal(name="HEAD_XZ", parent_name="HEAD", position=(0.24, 0, 0.24))) - - # The arm segment - bio_model["UPPER_ARM"] = SegmentReal( - parent_name="TRUNK", - segment_coordinate_system=SegmentCoordinateSystemReal.from_euler_and_translation( - (0, 0, 0), "xyz", (0, 0, 0.53) - ), - rotations=Rotations.X, - mesh=MeshReal(((0, 0, 0), (0, 0, -0.28))), - ) - bio_model["UPPER_ARM"].add_marker(MarkerReal(name="SHOULDER", parent_name="UPPER_ARM", position=(0, 0, 0))) - bio_model["UPPER_ARM"].add_marker(MarkerReal(name="SHOULDER_X", parent_name="UPPER_ARM", position=(1, 0, 0))) - bio_model["UPPER_ARM"].add_marker(MarkerReal(name="SHOULDER_XY", parent_name="UPPER_ARM", position=(1, 1, 0))) - - bio_model["LOWER_ARM"] = SegmentReal( - name="LOWER_ARM", - parent_name="UPPER_ARM", - segment_coordinate_system=SegmentCoordinateSystemReal.from_euler_and_translation( - (0, 0, 0), "xyz", (0, 0, -0.28) - ), - mesh=MeshReal(((0, 0, 0), (0, 0, -0.27))), - ) - bio_model["LOWER_ARM"].add_marker(MarkerReal(name="ELBOW", parent_name="LOWER_ARM", position=(0, 0, 0))) - bio_model["LOWER_ARM"].add_marker(MarkerReal(name="ELBOW_Y", parent_name="LOWER_ARM", position=(0, 1, 0))) - bio_model["LOWER_ARM"].add_marker(MarkerReal(name="ELBOW_XY", parent_name="LOWER_ARM", position=(1, 1, 0))) - - bio_model["HAND"] = SegmentReal( - name="HAND", - parent_name="LOWER_ARM", - segment_coordinate_system=SegmentCoordinateSystemReal.from_euler_and_translation( - (0, 0, 0), "xyz", (0, 0, -0.27) - ), - mesh=MeshReal(((0, 0, 0), (0, 0, -0.19))), - ) - bio_model["HAND"].add_marker(MarkerReal(name="WRIST", parent_name="HAND", position=(0, 0, 0))) - bio_model["HAND"].add_marker(MarkerReal(name="FINGER", parent_name="HAND", position=(0, 0, -0.19))) - bio_model["HAND"].add_marker(MarkerReal(name="HAND_Y", parent_name="HAND", position=(0, 1, 0))) - bio_model["HAND"].add_marker(MarkerReal(name="HAND_YZ", parent_name="HAND", position=(0, 1, 1))) - - # The thigh segment - bio_model["THIGH"] = SegmentReal( - name="THIGH", - parent_name="TRUNK", - rotations=Rotations.X, - mesh=MeshReal(((0, 0, 0), (0, 0, -0.42))), - ) - bio_model["THIGH"].add_marker(MarkerReal(name="THIGH_ORIGIN", parent_name="THIGH", position=(0, 0, 0))) - bio_model["THIGH"].add_marker(MarkerReal(name="THIGH_X", parent_name="THIGH", position=(1, 0, 0))) - bio_model["THIGH"].add_marker(MarkerReal(name="THIGH_Y", parent_name="THIGH", position=(0, 1, 0))) - - # The shank segment - bio_model["SHANK"] = SegmentReal( - name="SHANK", - parent_name="THIGH", - segment_coordinate_system=SegmentCoordinateSystemReal.from_euler_and_translation( - (0, 0, 0), "xyz", (0, 0, -0.42) - ), - rotations=Rotations.X, - mesh=MeshReal(((0, 0, 0), (0, 0, -0.43))), - ) - bio_model["SHANK"].add_marker(MarkerReal(name="KNEE", parent_name="SHANK", position=(0, 0, 0))) - bio_model["SHANK"].add_marker(MarkerReal(name="KNEE_Z", parent_name="SHANK", position=(0, 0, 1))) - bio_model["SHANK"].add_marker(MarkerReal(name="KNEE_XZ", parent_name="SHANK", position=(1, 0, 1))) - - # The foot segment - bio_model["FOOT"] = SegmentReal( - name="FOOT", - parent_name="SHANK", - segment_coordinate_system=SegmentCoordinateSystemReal.from_euler_and_translation( - (-np.pi / 2, 0, 0), "xyz", (0, 0, -0.43) - ), - rotations=Rotations.X, - mesh=MeshReal(((0, 0, 0), (0, 0, 0.25))), - ) - bio_model["FOOT"].add_marker(MarkerReal(name="ANKLE", parent_name="FOOT", position=(0, 0, 0))) - bio_model["FOOT"].add_marker(MarkerReal(name="TOE", parent_name="FOOT", position=(0, 0, 0.25))) - bio_model["FOOT"].add_marker(MarkerReal(name="ANKLE_Z", parent_name="FOOT", position=(0, 0, 1))) - bio_model["FOOT"].add_marker(MarkerReal(name="ANKLE_YZ", parent_name="FOOT", position=(0, 1, 1))) - - # Put the model together, print it and print it to a bioMod file - bio_model.write(kinematic_model_file_path) - - model = biorbd.Model(kinematic_model_file_path) - assert model.nbQ() == 7 - assert model.nbSegment() == 8 - assert model.nbMarkers() == 25 - np.testing.assert_almost_equal(model.markers(np.zeros((model.nbQ(),)))[-3].to_array(), [0, 0.25, -0.85], decimal=4) - - if remove_temporary: - os.remove(kinematic_model_file_path) - - -def write_markers_to_c3d(save_path: str, model): - q = np.zeros(model.nbQ()) - marker_names = tuple(name.to_string() for name in model.markerNames()) - marker_positions = np.array(tuple(m.to_array() for m in model.markers(q))).T[:, :, np.newaxis] - c3d = ezc3d.c3d() - - # Fill it with random data - c3d["parameters"]["POINT"]["RATE"]["value"] = [100] - c3d["parameters"]["POINT"]["LABELS"]["value"] = marker_names - c3d["data"]["points"] = marker_positions - - # Write the data - c3d.write(save_path) - - -@pytest.mark.parametrize("brbd", brbd_to_test) -def test_model_creation_from_data(brbd, remove_temporary: bool = True): - from biorbd.model_creation import ( - Axis, - BiomechanicalModel, - BiomechanicalModelReal, - C3dData, - Marker, - MarkerReal, - Mesh, - MeshReal, - Segment, - SegmentReal, - SegmentCoordinateSystemReal, - SegmentCoordinateSystem, - Translations, - Rotations, - ) - from de_leva import DeLevaTable - - kinematic_model_file_path = "temporary.bioMod" - c3d_file_path = "temporary.c3d" - test_model_creation_from_static(brbd, remove_temporary=False) - - # Prepare a fake model and a fake static from the previous test - model = biorbd.Model(kinematic_model_file_path) - write_markers_to_c3d(c3d_file_path, model) - os.remove(kinematic_model_file_path) - - # Fill the kinematic chain model - model = BiomechanicalModel() - de_leva = DeLevaTable(total_mass=100, sex="female") - - model["TRUNK"] = Segment( - translations=Translations.YZ, - rotations=Rotations.X, - inertia_parameters=de_leva["TRUNK"], - ) - model["TRUNK"].add_marker(Marker("PELVIS")) - - model["HEAD"] = Segment( - parent_name="TRUNK", - segment_coordinate_system=SegmentCoordinateSystem( - "BOTTOM_HEAD", - first_axis=Axis(name=Axis.Name.Z, start="BOTTOM_HEAD", end="HEAD_Z"), - second_axis=Axis(name=Axis.Name.X, start="BOTTOM_HEAD", end="HEAD_XZ"), - axis_to_keep=Axis.Name.Z, - ), - mesh=Mesh(("BOTTOM_HEAD", "TOP_HEAD", "HEAD_Z", "HEAD_XZ", "BOTTOM_HEAD")), - inertia_parameters=de_leva["HEAD"], - ) - model["HEAD"].add_marker(Marker("BOTTOM_HEAD")) - model["HEAD"].add_marker(Marker("TOP_HEAD")) - model["HEAD"].add_marker(Marker("HEAD_Z")) - model["HEAD"].add_marker(Marker("HEAD_XZ")) - - model["UPPER_ARM"] = Segment( - "UPPER_ARM", - parent_name="TRUNK", - rotations=Rotations.X, - segment_coordinate_system=SegmentCoordinateSystem( - origin="SHOULDER", - first_axis=Axis(name=Axis.Name.X, start="SHOULDER", end="SHOULDER_X"), - second_axis=Axis(name=Axis.Name.Y, start="SHOULDER", end="SHOULDER_XY"), - axis_to_keep=Axis.Name.X, - ), - inertia_parameters=de_leva["UPPER_ARM"], - ) - model["UPPER_ARM"].add_marker(Marker("SHOULDER")) - model["UPPER_ARM"].add_marker(Marker("SHOULDER_X")) - model["UPPER_ARM"].add_marker(Marker("SHOULDER_XY")) - - model["LOWER_ARM"] = Segment( - parent_name="UPPER_ARM", - segment_coordinate_system=SegmentCoordinateSystem( - origin="ELBOW", - first_axis=Axis(name=Axis.Name.Y, start="ELBOW", end="ELBOW_Y"), - second_axis=Axis(name=Axis.Name.X, start="ELBOW", end="ELBOW_XY"), - axis_to_keep=Axis.Name.Y, - ), - inertia_parameters=de_leva["LOWER_ARM"], - ) - model["LOWER_ARM"].add_marker(Marker("ELBOW")) - model["LOWER_ARM"].add_marker(Marker("ELBOW_Y")) - model["LOWER_ARM"].add_marker(Marker("ELBOW_XY")) - - model["HAND"] = Segment( - parent_name="LOWER_ARM", - segment_coordinate_system=SegmentCoordinateSystem( - origin="WRIST", - first_axis=Axis(name=Axis.Name.Y, start="WRIST", end="HAND_Y"), - second_axis=Axis(name=Axis.Name.Z, start="WRIST", end="HAND_YZ"), - axis_to_keep=Axis.Name.Y, - ), - inertia_parameters=de_leva["HAND"], - ) - model["HAND"].add_marker(Marker("WRIST")) - model["HAND"].add_marker(Marker("FINGER")) - model["HAND"].add_marker(Marker("HAND_Y")) - model["HAND"].add_marker(Marker("HAND_YZ")) - - model["THIGH"] = Segment( - parent_name="TRUNK", - rotations=Rotations.X, - segment_coordinate_system=SegmentCoordinateSystem( - origin="THIGH_ORIGIN", - first_axis=Axis(name=Axis.Name.X, start="THIGH_ORIGIN", end="THIGH_X"), - second_axis=Axis(name=Axis.Name.Y, start="THIGH_ORIGIN", end="THIGH_Y"), - axis_to_keep=Axis.Name.X, - ), - inertia_parameters=de_leva["THIGH"], - ) - model["THIGH"].add_marker(Marker("THIGH_ORIGIN")) - model["THIGH"].add_marker(Marker("THIGH_X")) - model["THIGH"].add_marker(Marker("THIGH_Y")) - - model["SHANK"] = Segment( - parent_name="THIGH", - rotations=Rotations.X, - segment_coordinate_system=SegmentCoordinateSystem( - origin="KNEE", - first_axis=Axis(name=Axis.Name.Z, start="KNEE", end="KNEE_Z"), - second_axis=Axis(name=Axis.Name.X, start="KNEE", end="KNEE_XZ"), - axis_to_keep=Axis.Name.Z, - ), - inertia_parameters=de_leva["SHANK"], - ) - model["SHANK"].add_marker(Marker("KNEE")) - model["SHANK"].add_marker(Marker("KNEE_Z")) - model["SHANK"].add_marker(Marker("KNEE_XZ")) - - model["FOOT"] = Segment( - parent_name="SHANK", - rotations=Rotations.X, - segment_coordinate_system=SegmentCoordinateSystem( - origin="ANKLE", - first_axis=Axis(name=Axis.Name.Z, start="ANKLE", end="ANKLE_Z"), - second_axis=Axis(name=Axis.Name.Y, start="ANKLE", end="ANKLE_YZ"), - axis_to_keep=Axis.Name.Z, - ), - inertia_parameters=de_leva["FOOT"], - ) - model["FOOT"].add_marker(Marker("ANKLE")) - model["FOOT"].add_marker(Marker("TOE")) - model["FOOT"].add_marker(Marker("ANKLE_Z")) - model["FOOT"].add_marker(Marker("ANKLE_YZ")) - - # Put the model together, print it and print it to a bioMod file - model.write(kinematic_model_file_path, C3dData(c3d_file_path)) - - model = biorbd.Model(kinematic_model_file_path) - assert model.nbQ() == 7 - assert model.nbSegment() == 8 - assert model.nbMarkers() == 25 - np.testing.assert_almost_equal(model.markers(np.zeros((model.nbQ(),)))[-3].to_array(), [0, 0.25, -0.85], decimal=4) - - if remove_temporary: - os.remove(kinematic_model_file_path) - os.remove(c3d_file_path) diff --git a/test/binding/Python3/de_leva/__init__.py b/test/binding/python3/de_leva/__init__.py similarity index 69% rename from test/binding/Python3/de_leva/__init__.py rename to test/binding/python3/de_leva/__init__.py index 35a1a1a8c..0cabfb2cd 100644 --- a/test/binding/Python3/de_leva/__init__.py +++ b/test/binding/python3/de_leva/__init__.py @@ -1,4 +1,5 @@ -from biorbd.model_creation import InertiaParameters +import types + import numpy as np @@ -7,7 +8,7 @@ def point_on_vector(coef: float, start: np.ndarray, end: np.ndarray) -> np.ndarr class DeLevaTable: - def __init__(self, total_mass: float, sex: str): + def __init__(self, total_mass: float, sex: str, bmc: types.ModuleType = None): """ Implementation of the DeLeva table Parameters @@ -16,85 +17,106 @@ def __init__(self, total_mass: float, sex: str): The mass of the subject sex The sex ('male' or 'female') of the subject + bmc + The biorbd (or biorbd_casadi) model_creation module """ + + if bmc is None: + # This will be called by the example instead of its own de_leva module because of the relative path problem + # So we load it for it anyway + has_loaded = False + try: + import biorbd.model_creation as bmc + has_loaded = True + except ModuleNotFoundError: + pass + + try: + import biorbd_casadi.model_creation as bmc + has_loaded = True + except ModuleNotFoundError: + pass + + if not has_loaded: + raise ModuleNotFoundError("Could not find biorbd or biorbd_casadi") self.sex = sex self.de_leva_table = { "male": { - "HEAD": InertiaParameters( + "HEAD": bmc.InertiaParameters( mass=lambda m, bio: 0.0694 * total_mass, center_of_mass=lambda m, bio: point_on_vector(0.5002, start=m["TOP_HEAD"], end=m["SHOULDER"]), - inertia=lambda m, bio: InertiaParameters.radii_of_gyration_to_inertia( + inertia=lambda m, bio: bmc.InertiaParameters.radii_of_gyration_to_inertia( mass=0.0694 * total_mass, coef=(0.303, 0.315, 0.261), start=m["TOP_HEAD"], end=m["SHOULDER"], ), ), - "TRUNK": InertiaParameters( + "TRUNK": bmc.InertiaParameters( mass=lambda m, bio: 0.4346 * total_mass, center_of_mass=lambda m, bio: point_on_vector(0.5138, start=m["SHOULDER"], end=m["PELVIS"]), - inertia=lambda m, bio: InertiaParameters.radii_of_gyration_to_inertia( + inertia=lambda m, bio: bmc.InertiaParameters.radii_of_gyration_to_inertia( mass=0.4346 * total_mass, coef=(0.328, 0.306, 0.169), start=m["SHOULDER"], end=m["PELVIS"], ), ), - "UPPER_ARM": InertiaParameters( + "UPPER_ARM": bmc.InertiaParameters( mass=lambda m, bio: 2 * 0.0271 * total_mass, center_of_mass=lambda m, bio: point_on_vector(0.5772, start=m["SHOULDER"], end=m["ELBOW"]), - inertia=lambda m, bio: InertiaParameters.radii_of_gyration_to_inertia( + inertia=lambda m, bio: bmc.InertiaParameters.radii_of_gyration_to_inertia( mass=2 * 0.0271 * total_mass, coef=(0.285, 0.269, 0.158), start=m["SHOULDER"], end=m["ELBOW"], ), ), - "LOWER_ARM": InertiaParameters( + "LOWER_ARM": bmc.InertiaParameters( mass=lambda m, bio: 2 * 0.0162 * total_mass, center_of_mass=lambda m, bio: point_on_vector(0.4574, start=m["ELBOW"], end=m["WRIST"]), - inertia=lambda m, bio: InertiaParameters.radii_of_gyration_to_inertia( + inertia=lambda m, bio: bmc.InertiaParameters.radii_of_gyration_to_inertia( mass=2 * 0.0162 * total_mass, coef=(0.276, 0.265, 0.121), start=m["ELBOW"], end=m["WRIST"], ), ), - "HAND": InertiaParameters( + "HAND": bmc.InertiaParameters( mass=lambda m, bio: 2 * 0.0061 * total_mass, center_of_mass=lambda m, bio: point_on_vector(0.7900, start=m["WRIST"], end=m["FINGER"]), - inertia=lambda m, bio: InertiaParameters.radii_of_gyration_to_inertia( + inertia=lambda m, bio: bmc.InertiaParameters.radii_of_gyration_to_inertia( mass=2 * 0.0061 * total_mass, coef=(0.628, 0.513, 0.401), start=m["WRIST"], end=m["FINGER"], ), ), - "THIGH": InertiaParameters( + "THIGH": bmc.InertiaParameters( mass=lambda m, bio: 2 * 0.1416 * total_mass, center_of_mass=lambda m, bio: point_on_vector(0.4095, start=m["PELVIS"], end=m["KNEE"]), - inertia=lambda m, bio: InertiaParameters.radii_of_gyration_to_inertia( + inertia=lambda m, bio: bmc.InertiaParameters.radii_of_gyration_to_inertia( mass=2 * 0.1416 * total_mass, coef=(0.329, 0.329, 0.149), start=m["PELVIS"], end=m["KNEE"], ), ), - "SHANK": InertiaParameters( + "SHANK": bmc.InertiaParameters( mass=lambda m, bio: 2 * 0.0433 * total_mass, center_of_mass=lambda m, bio: point_on_vector(0.4459, start=m["KNEE"], end=m["ANKLE"]), - inertia=lambda m, bio: InertiaParameters.radii_of_gyration_to_inertia( + inertia=lambda m, bio: bmc.InertiaParameters.radii_of_gyration_to_inertia( mass=2 * 0.0433 * total_mass, coef=(0.255, 0.249, 0.103), start=m["KNEE"], end=m["ANKLE"], ), ), - "FOOT": InertiaParameters( + "FOOT": bmc.InertiaParameters( mass=lambda m, bio: 2 * 0.0137 * total_mass, center_of_mass=lambda m, bio: point_on_vector(0.4415, start=m["ANKLE"], end=m["TOE"]), - inertia=lambda m, bio: InertiaParameters.radii_of_gyration_to_inertia( + inertia=lambda m, bio: bmc.InertiaParameters.radii_of_gyration_to_inertia( mass=2 * 0.0137 * total_mass, coef=(0.257, 0.245, 0.124), start=m["ANKLE"], @@ -103,80 +125,80 @@ def __init__(self, total_mass: float, sex: str): ), }, "female": { - "HEAD": InertiaParameters( + "HEAD": bmc.InertiaParameters( mass=lambda m, bio: 0.0669 * total_mass, center_of_mass=lambda m, bio: point_on_vector(0.4841, start=m["TOP_HEAD"], end=m["SHOULDER"]), - inertia=lambda m, bio: InertiaParameters.radii_of_gyration_to_inertia( + inertia=lambda m, bio: bmc.InertiaParameters.radii_of_gyration_to_inertia( mass=0.0669 * total_mass, coef=(0.271, 0.295, 0.261), start=m["TOP_HEAD"], end=m["SHOULDER"], ), ), - "TRUNK": InertiaParameters( + "TRUNK": bmc.InertiaParameters( mass=lambda m, bio: 0.4257 * total_mass, center_of_mass=lambda m, bio: point_on_vector(0.4964, start=m["SHOULDER"], end=m["PELVIS"]), - inertia=lambda m, bio: InertiaParameters.radii_of_gyration_to_inertia( + inertia=lambda m, bio: bmc.InertiaParameters.radii_of_gyration_to_inertia( mass=0.4257 * total_mass, coef=(0.307, 0.292, 0.147), start=m["SHOULDER"], end=m["PELVIS"], ), ), - "UPPER_ARM": InertiaParameters( + "UPPER_ARM": bmc.InertiaParameters( mass=lambda m, bio: 2 * 0.0255 * total_mass, center_of_mass=lambda m, bio: point_on_vector(0.5754, start=m["SHOULDER"], end=m["ELBOW"]), - inertia=lambda m, bio: InertiaParameters.radii_of_gyration_to_inertia( + inertia=lambda m, bio: bmc.InertiaParameters.radii_of_gyration_to_inertia( mass=2 * 0.0255 * total_mass, coef=(0.278, 0.260, 0.148), start=m["SHOULDER"], end=m["ELBOW"], ), ), - "LOWER_ARM": InertiaParameters( + "LOWER_ARM": bmc.InertiaParameters( mass=lambda m, bio: 2 * 0.0138 * total_mass, center_of_mass=lambda m, bio: point_on_vector(0.4559, start=m["ELBOW"], end=m["WRIST"]), - inertia=lambda m, bio: InertiaParameters.radii_of_gyration_to_inertia( + inertia=lambda m, bio: bmc.InertiaParameters.radii_of_gyration_to_inertia( mass=2 * 0.0138 * total_mass, coef=(0.261, 0.257, 0.094), start=m["ELBOW"], end=m["WRIST"], ), ), - "HAND": InertiaParameters( + "HAND": bmc.InertiaParameters( mass=lambda m, bio: 2 * 0.0056 * total_mass, center_of_mass=lambda m, bio: point_on_vector(0.7474, start=m["WRIST"], end=m["FINGER"]), - inertia=lambda m, bio: InertiaParameters.radii_of_gyration_to_inertia( + inertia=lambda m, bio: bmc.InertiaParameters.radii_of_gyration_to_inertia( mass=2 * 0.0056 * total_mass, coef=(0.531, 0.454, 0.335), start=m["WRIST"], end=m["FINGER"], ), ), - "THIGH": InertiaParameters( + "THIGH": bmc.InertiaParameters( mass=lambda m, bio: 2 * 0.1478 * total_mass, center_of_mass=lambda m, bio: point_on_vector(0.3612, start=m["PELVIS"], end=m["KNEE"]), - inertia=lambda m, bio: InertiaParameters.radii_of_gyration_to_inertia( + inertia=lambda m, bio: bmc.InertiaParameters.radii_of_gyration_to_inertia( mass=2 * 0.1478 * total_mass, coef=(0.369, 0.364, 0.162), start=m["PELVIS"], end=m["KNEE"], ), ), - "SHANK": InertiaParameters( + "SHANK": bmc.InertiaParameters( mass=lambda m, bio: 2 * 0.0481 * total_mass, center_of_mass=lambda m, bio: point_on_vector(0.4416, start=m["KNEE"], end=m["ANKLE"]), - inertia=lambda m, bio: InertiaParameters.radii_of_gyration_to_inertia( + inertia=lambda m, bio: bmc.InertiaParameters.radii_of_gyration_to_inertia( mass=2 * 0.0481 * total_mass, coef=(0.271, 0.267, 0.093), start=m["KNEE"], end=m["ANKLE"], ), ), - "FOOT": InertiaParameters( + "FOOT": bmc.InertiaParameters( mass=lambda m, bio: 2 * 0.0129 * total_mass, center_of_mass=lambda m, bio: point_on_vector(0.4014, start=m["ANKLE"], end=m["TOE"]), - inertia=lambda m, bio: InertiaParameters.radii_of_gyration_to_inertia( + inertia=lambda m, bio: bmc.InertiaParameters.radii_of_gyration_to_inertia( mass=2 * 0.0129 * total_mass, coef=(0.299, 0.279, 0.124), start=m["ANKLE"], diff --git a/test/binding/Python3/test_binder_python_rigidbody.py b/test/binding/python3/test_binder_python_rigidbody.py similarity index 100% rename from test/binding/Python3/test_binder_python_rigidbody.py rename to test/binding/python3/test_binder_python_rigidbody.py diff --git a/test/binding/Python3/test_conversion.py b/test/binding/python3/test_conversion.py similarity index 100% rename from test/binding/Python3/test_conversion.py rename to test/binding/python3/test_conversion.py diff --git a/test/binding/python3/test_examples.py b/test/binding/python3/test_examples.py new file mode 100644 index 000000000..8687bc5aa --- /dev/null +++ b/test/binding/python3/test_examples.py @@ -0,0 +1,95 @@ +import pytest + +available_biorbd = [] + +try: + import biorbd + available_biorbd.append("biorbd") +except ImportError: + pass + +try: + import biorbd_casadi + available_biorbd.append("biorbd_casadi") +except ImportError: + pass + + +def load_example_file(file_name: str): + # Import the file as a module from the path + import os + from pathlib import Path + from importlib.util import spec_from_file_location, module_from_spec + import sys + + # Get the path to the examples + if os.environ.get("CI_MAIN_EXAMPLES_FOLDER"): + examples_path = Path(os.environ["CI_MAIN_EXAMPLES_FOLDER"]) / "python3" + else: + examples_path = Path(__file__).parent.parent.parent.parent / "examples" / "python3" + + # Import the example + sys.path.append(str(examples_path)) + spec = spec_from_file_location("example", examples_path / file_name) + example = module_from_spec(spec) + spec.loader.exec_module(example) + + sys.path.pop() + + return example + + +def test_example_forward_dynamics(): + if "biorbd" not in available_biorbd: + pytest.skip("This example is written for biorbd, but not for the biorbd_casadi") + + forward_dynamics = load_example_file("forwardDynamics.py") + forward_dynamics.main() + + +def test_example_forward_dynamics_from_muscles(): + if "biorbd" not in available_biorbd: + pytest.skip("This example is written for biorbd, but not for the biorbd_casadi") + + forward_dynamics = load_example_file("forwardDynamicsFromMuscles.py") + forward_dynamics.main() + + +def test_example_forward_kinematics(): + if "biorbd" not in available_biorbd: + pytest.skip("This example is written for biorbd, but not for the biorbd_casadi") + + forward_kinematics = load_example_file("forwardKinematics.py") + forward_kinematics.main(show=False) + + +def test_example_inverse_dynamics(): + if "biorbd" not in available_biorbd: + pytest.skip("This example is written for biorbd, but not for the biorbd_casadi") + + inverse_dynamics = load_example_file("inverseDynamics.py") + inverse_dynamics.main() + + +def test_example_inverse_kinematics_kalman(): + if "biorbd" not in available_biorbd: + pytest.skip("This example is written for biorbd, but not for the biorbd_casadi") + + inverse_dynamics_kalman = load_example_file("inverseKinematicsKalman.py") + inverse_dynamics_kalman.main(show=False) + + +def test_example_model_creation(): + if "biorbd" not in available_biorbd: + pytest.skip("This example is written for biorbd, but not for the biorbd_casadi") + + model_creation = load_example_file("modelCreation.py") + model_creation.main() + + +def test_example_static_optimization(): + if "biorbd" not in available_biorbd: + pytest.skip("This example is written for biorbd, but not for the biorbd_casadi") + + static_optimization = load_example_file("staticOptimization.py") + static_optimization.main() diff --git a/test/binding/Python3/test_external_forces.py b/test/binding/python3/test_external_forces.py similarity index 100% rename from test/binding/Python3/test_external_forces.py rename to test/binding/python3/test_external_forces.py diff --git a/test/binding/Python3/test_inverse_kinematics.py b/test/binding/python3/test_inverse_kinematics.py similarity index 100% rename from test/binding/Python3/test_inverse_kinematics.py rename to test/binding/python3/test_inverse_kinematics.py diff --git a/test/binding/python3/test_model_creation.py b/test/binding/python3/test_model_creation.py new file mode 100644 index 000000000..72a1404d8 --- /dev/null +++ b/test/binding/python3/test_model_creation.py @@ -0,0 +1,421 @@ +import os +import pytest + +import numpy as np + + +brbd_to_test = [] +try: + import biorbd + from biorbd import model_creation + + brbd_to_test.append([biorbd, model_creation]) +except ModuleNotFoundError: + pass +try: + import biorbd_casadi + from biorbd_casadi import model_creation + + brbd_to_test.append([biorbd_casadi, model_creation]) +except ModuleNotFoundError: + pass + +if not brbd_to_test: + raise ModuleNotFoundError("No biorbd or biorbd_casadi found") + +import ezc3d + + +@pytest.mark.parametrize("brbd", brbd_to_test) +def test_model_creation_from_static(brbd, remove_temporary: bool = True): + """ + Produces a model from real data + """ + brbd, bmc = brbd + + kinematic_model_file_path = "temporary.bioMod" + + # Create a model holder + bio_model = bmc.BiomechanicalModelReal() + + # The trunk segment + bio_model.segments["TRUNK"] = bmc.SegmentReal( + name="TRUNK", + translations=bmc.Translations.YZ, + rotations=bmc.Rotations.X, + mesh=bmc.MeshReal(((0, 0, 0), (0, 0, 0.53))), + ) + bio_model.segments["TRUNK"].add_marker(bmc.MarkerReal(name="PELVIS", parent_name="TRUNK")) + + # The head segment + bio_model.segments["HEAD"] = bmc.SegmentReal( + name="HEAD", + parent_name="TRUNK", + segment_coordinate_system=bmc.SegmentCoordinateSystemReal.from_euler_and_translation( + (0, 0, 0), "xyz", (0, 0, 0.53) + ), + mesh=bmc.MeshReal(((0, 0, 0), (0, 0, 0.24))), + ) + bio_model.segments["HEAD"].add_marker(bmc.MarkerReal(name="BOTTOM_HEAD", parent_name="HEAD", position=(0, 0, 0))) + bio_model.segments["HEAD"].add_marker(bmc.MarkerReal(name="TOP_HEAD", parent_name="HEAD", position=(0, 0, 0.24))) + bio_model.segments["HEAD"].add_marker(bmc.MarkerReal(name="HEAD_Z", parent_name="HEAD", position=(0, 0, 0.24))) + bio_model.segments["HEAD"].add_marker(bmc.MarkerReal(name="HEAD_XZ", parent_name="HEAD", position=(0.24, 0, 0.24))) + + # The arm segment + bio_model.segments["UPPER_ARM"] = bmc.SegmentReal( + name="UPPER_ARM", + parent_name="TRUNK", + segment_coordinate_system=bmc.SegmentCoordinateSystemReal.from_euler_and_translation( + (0, 0, 0), "xyz", (0, 0, 0.53) + ), + rotations=bmc.Rotations.X, + mesh=bmc.MeshReal(((0, 0, 0), (0, 0, -0.28))), + ) + bio_model.segments["UPPER_ARM"].add_marker(bmc.MarkerReal(name="SHOULDER", parent_name="UPPER_ARM", position=(0, 0, 0))) + bio_model.segments["UPPER_ARM"].add_marker(bmc.MarkerReal(name="SHOULDER_X", parent_name="UPPER_ARM", position=(1, 0, 0))) + bio_model.segments["UPPER_ARM"].add_marker(bmc.MarkerReal(name="SHOULDER_XY", parent_name="UPPER_ARM", position=(1, 1, 0))) + + bio_model.segments["LOWER_ARM"] = bmc.SegmentReal( + name="LOWER_ARM", + parent_name="UPPER_ARM", + segment_coordinate_system=bmc.SegmentCoordinateSystemReal.from_euler_and_translation( + (0, 0, 0), "xyz", (0, 0, -0.28) + ), + mesh=bmc.MeshReal(((0, 0, 0), (0, 0, -0.27))), + ) + bio_model.segments["LOWER_ARM"].add_marker(bmc.MarkerReal(name="ELBOW", parent_name="LOWER_ARM", position=(0, 0, 0))) + bio_model.segments["LOWER_ARM"].add_marker(bmc.MarkerReal(name="ELBOW_Y", parent_name="LOWER_ARM", position=(0, 1, 0))) + bio_model.segments["LOWER_ARM"].add_marker(bmc.MarkerReal(name="ELBOW_XY", parent_name="LOWER_ARM", position=(1, 1, 0))) + + bio_model.segments["HAND"] = bmc.SegmentReal( + name="HAND", + parent_name="LOWER_ARM", + segment_coordinate_system=bmc.SegmentCoordinateSystemReal.from_euler_and_translation( + (0, 0, 0), "xyz", (0, 0, -0.27) + ), + mesh=bmc.MeshReal(((0, 0, 0), (0, 0, -0.19))), + ) + bio_model.segments["HAND"].add_marker(bmc.MarkerReal(name="WRIST", parent_name="HAND", position=(0, 0, 0))) + bio_model.segments["HAND"].add_marker(bmc.MarkerReal(name="FINGER", parent_name="HAND", position=(0, 0, -0.19))) + bio_model.segments["HAND"].add_marker(bmc.MarkerReal(name="HAND_Y", parent_name="HAND", position=(0, 1, 0))) + bio_model.segments["HAND"].add_marker(bmc.MarkerReal(name="HAND_YZ", parent_name="HAND", position=(0, 1, 1))) + + # The thigh segment + bio_model.segments["THIGH"] = bmc.SegmentReal( + name="THIGH", + parent_name="TRUNK", + rotations=bmc.Rotations.X, + mesh=bmc.MeshReal(((0, 0, 0), (0, 0, -0.42))), + ) + bio_model.segments["THIGH"].add_marker(bmc.MarkerReal(name="THIGH_ORIGIN", parent_name="THIGH", position=(0, 0, 0))) + bio_model.segments["THIGH"].add_marker(bmc.MarkerReal(name="THIGH_X", parent_name="THIGH", position=(1, 0, 0))) + bio_model.segments["THIGH"].add_marker(bmc.MarkerReal(name="THIGH_Y", parent_name="THIGH", position=(0, 1, 0))) + + # The shank segment + bio_model.segments["SHANK"] = bmc.SegmentReal( + name="SHANK", + parent_name="THIGH", + segment_coordinate_system=bmc.SegmentCoordinateSystemReal.from_euler_and_translation( + (0, 0, 0), "xyz", (0, 0, -0.42) + ), + rotations=bmc.Rotations.X, + mesh=bmc.MeshReal(((0, 0, 0), (0, 0, -0.43))), + ) + bio_model.segments["SHANK"].add_marker(bmc.MarkerReal(name="KNEE", parent_name="SHANK", position=(0, 0, 0))) + bio_model.segments["SHANK"].add_marker(bmc.MarkerReal(name="KNEE_Z", parent_name="SHANK", position=(0, 0, 1))) + bio_model.segments["SHANK"].add_marker(bmc.MarkerReal(name="KNEE_XZ", parent_name="SHANK", position=(1, 0, 1))) + + # The foot segment + bio_model.segments["FOOT"] = bmc.SegmentReal( + name="FOOT", + parent_name="SHANK", + segment_coordinate_system=bmc.SegmentCoordinateSystemReal.from_euler_and_translation( + (-np.pi / 2, 0, 0), "xyz", (0, 0, -0.43) + ), + rotations=bmc.Rotations.X, + mesh=bmc.MeshReal(((0, 0, 0), (0, 0, 0.25))), + ) + bio_model.segments["FOOT"].add_marker(bmc.MarkerReal(name="ANKLE", parent_name="FOOT", position=(0, 0, 0))) + bio_model.segments["FOOT"].add_marker(bmc.MarkerReal(name="TOE", parent_name="FOOT", position=(0, 0, 0.25))) + bio_model.segments["FOOT"].add_marker(bmc.MarkerReal(name="ANKLE_Z", parent_name="FOOT", position=(0, 0, 1))) + bio_model.segments["FOOT"].add_marker(bmc.MarkerReal(name="ANKLE_YZ", parent_name="FOOT", position=(0, 1, 1))) + + # Put the model together, print it and print it to a bioMod file + bio_model.write(kinematic_model_file_path) + + model = brbd.Model(kinematic_model_file_path) + assert model.nbQ() == 7 + assert model.nbSegment() == 8 + assert model.nbMarkers() == 25 + if brbd.currentLinearAlgebraBackend() == 1: + # If CasADi backend is used + from casadi import MX + + q_sym = MX.sym("q", model.nbQ(), 1) + value = np.array(brbd.to_casadi_func("markers", model.markers, q_sym)(np.zeros((model.nbQ(),))))[:, -3] + else: + value = model.markers(np.zeros((model.nbQ(),)))[-3].to_array() + np.testing.assert_almost_equal(value, [0, 0.25, -0.85], decimal=4) + + if remove_temporary: + os.remove(kinematic_model_file_path) + + +def write_markers_to_c3d(save_path: str, brbd, model): + q = np.zeros(model.nbQ()) + marker_names = tuple(name.to_string() for name in model.markerNames()) + if brbd.currentLinearAlgebraBackend() == 1: + # If CasADi backend is used + from casadi import MX + + q_sym = MX.sym("q", model.nbQ(), 1) + marker_positions = np.array(brbd.to_casadi_func("markers", model.markers, q_sym)(q))[:, :, np.newaxis] + else: + marker_positions = np.array(tuple(m.to_array() for m in model.markers(q))).T[:, :, np.newaxis] + + c3d = ezc3d.c3d() + + # Fill it with random data + c3d["parameters"]["POINT"]["RATE"]["value"] = [100] + c3d["parameters"]["POINT"]["LABELS"]["value"] = marker_names + c3d["data"]["points"] = marker_positions + + # Write the data + c3d.write(save_path) + + +@pytest.mark.parametrize("brbd", brbd_to_test) +def test_model_creation_from_data(brbd, remove_temporary: bool = True): + from de_leva import DeLevaTable + + kinematic_model_file_path = "temporary.bioMod" + c3d_file_path = "temporary.c3d" + test_model_creation_from_static(brbd, remove_temporary=False) + + brbd, bmc = brbd # This must be done after the call to test_model_creation_from_static + + # Prepare a fake model and a fake static from the previous test + model = brbd.Model(kinematic_model_file_path) + write_markers_to_c3d(c3d_file_path, brbd, model) + os.remove(kinematic_model_file_path) + + # Fill the kinematic chain model + model = bmc.BiomechanicalModel() + de_leva = DeLevaTable(total_mass=100, sex="female", bmc=bmc) + + model.segments["TRUNK"] = bmc.Segment( + name="TRUNK", + translations=bmc.Translations.YZ, + rotations=bmc.Rotations.X, + inertia_parameters=de_leva["TRUNK"], + ) + model.segments["TRUNK"].add_marker(bmc.Marker("PELVIS")) + + model.segments["HEAD"] = bmc.Segment( + name="HEAD", + parent_name="TRUNK", + segment_coordinate_system=bmc.SegmentCoordinateSystem( + "BOTTOM_HEAD", + first_axis=bmc.Axis(name=bmc.Axis.Name.Z, start="BOTTOM_HEAD", end="HEAD_Z"), + second_axis=bmc.Axis(name=bmc.Axis.Name.X, start="BOTTOM_HEAD", end="HEAD_XZ"), + axis_to_keep=bmc.Axis.Name.Z, + ), + mesh=bmc.Mesh(("BOTTOM_HEAD", "TOP_HEAD", "HEAD_Z", "HEAD_XZ", "BOTTOM_HEAD")), + inertia_parameters=de_leva["HEAD"], + ) + model.segments["HEAD"].add_marker(bmc.Marker("BOTTOM_HEAD")) + model.segments["HEAD"].add_marker(bmc.Marker("TOP_HEAD")) + model.segments["HEAD"].add_marker(bmc.Marker("HEAD_Z")) + model.segments["HEAD"].add_marker(bmc.Marker("HEAD_XZ")) + + model.segments["UPPER_ARM"] = bmc.Segment( + name="UPPER_ARM", + parent_name="TRUNK", + rotations=bmc.Rotations.X, + segment_coordinate_system=bmc.SegmentCoordinateSystem( + origin="SHOULDER", + first_axis=bmc.Axis(name=bmc.Axis.Name.X, start="SHOULDER", end="SHOULDER_X"), + second_axis=bmc.Axis(name=bmc.Axis.Name.Y, start="SHOULDER", end="SHOULDER_XY"), + axis_to_keep=bmc.Axis.Name.X, + ), + inertia_parameters=de_leva["UPPER_ARM"], + ) + model.segments["UPPER_ARM"].add_marker(bmc.Marker("SHOULDER")) + model.segments["UPPER_ARM"].add_marker(bmc.Marker("SHOULDER_X")) + model.segments["UPPER_ARM"].add_marker(bmc.Marker("SHOULDER_XY")) + + model.segments["LOWER_ARM"] = bmc.Segment( + name="LOWER_ARM", + parent_name="UPPER_ARM", + segment_coordinate_system=bmc.SegmentCoordinateSystem( + origin="ELBOW", + first_axis=bmc.Axis(name=bmc.Axis.Name.Y, start="ELBOW", end="ELBOW_Y"), + second_axis=bmc.Axis(name=bmc.Axis.Name.X, start="ELBOW", end="ELBOW_XY"), + axis_to_keep=bmc.Axis.Name.Y, + ), + inertia_parameters=de_leva["LOWER_ARM"], + ) + model.segments["LOWER_ARM"].add_marker(bmc.Marker("ELBOW")) + model.segments["LOWER_ARM"].add_marker(bmc.Marker("ELBOW_Y")) + model.segments["LOWER_ARM"].add_marker(bmc.Marker("ELBOW_XY")) + + model.segments["HAND"] = bmc.Segment( + name="HAND", + parent_name="LOWER_ARM", + segment_coordinate_system=bmc.SegmentCoordinateSystem( + origin="WRIST", + first_axis=bmc.Axis(name=bmc.Axis.Name.Y, start="WRIST", end="HAND_Y"), + second_axis=bmc.Axis(name=bmc.Axis.Name.Z, start="WRIST", end="HAND_YZ"), + axis_to_keep=bmc.Axis.Name.Y, + ), + inertia_parameters=de_leva["HAND"], + ) + model.segments["HAND"].add_marker(bmc.Marker("WRIST")) + model.segments["HAND"].add_marker(bmc.Marker("FINGER")) + model.segments["HAND"].add_marker(bmc.Marker("HAND_Y")) + model.segments["HAND"].add_marker(bmc.Marker("HAND_YZ")) + + model.segments["THIGH"] = bmc.Segment( + name="THIGH", + parent_name="TRUNK", + rotations=bmc.Rotations.X, + segment_coordinate_system=bmc.SegmentCoordinateSystem( + origin="THIGH_ORIGIN", + first_axis=bmc.Axis(name=bmc.Axis.Name.X, start="THIGH_ORIGIN", end="THIGH_X"), + second_axis=bmc.Axis(name=bmc.Axis.Name.Y, start="THIGH_ORIGIN", end="THIGH_Y"), + axis_to_keep=bmc.Axis.Name.X, + ), + inertia_parameters=de_leva["THIGH"], + ) + model.segments["THIGH"].add_marker(bmc.Marker("THIGH_ORIGIN")) + model.segments["THIGH"].add_marker(bmc.Marker("THIGH_X")) + model.segments["THIGH"].add_marker(bmc.Marker("THIGH_Y")) + + model.segments["SHANK"] = bmc.Segment( + name="SHANK", + parent_name="THIGH", + rotations=bmc.Rotations.X, + segment_coordinate_system=bmc.SegmentCoordinateSystem( + origin="KNEE", + first_axis=bmc.Axis(name=bmc.Axis.Name.Z, start="KNEE", end="KNEE_Z"), + second_axis=bmc.Axis(name=bmc.Axis.Name.X, start="KNEE", end="KNEE_XZ"), + axis_to_keep=bmc.Axis.Name.Z, + ), + inertia_parameters=de_leva["SHANK"], + ) + model.segments["SHANK"].add_marker(bmc.Marker("KNEE")) + model.segments["SHANK"].add_marker(bmc.Marker("KNEE_Z")) + model.segments["SHANK"].add_marker(bmc.Marker("KNEE_XZ")) + + model.segments["FOOT"] = bmc.Segment( + name="FOOT", + parent_name="SHANK", + rotations=bmc.Rotations.X, + segment_coordinate_system=bmc.SegmentCoordinateSystem( + origin="ANKLE", + first_axis=bmc.Axis(name=bmc.Axis.Name.Z, start="ANKLE", end="ANKLE_Z"), + second_axis=bmc.Axis(name=bmc.Axis.Name.Y, start="ANKLE", end="ANKLE_YZ"), + axis_to_keep=bmc.Axis.Name.Z, + ), + inertia_parameters=de_leva["FOOT"], + ) + model.segments["FOOT"].add_marker(bmc.Marker("ANKLE")) + model.segments["FOOT"].add_marker(bmc.Marker("TOE")) + model.segments["FOOT"].add_marker(bmc.Marker("ANKLE_Z")) + model.segments["FOOT"].add_marker(bmc.Marker("ANKLE_YZ")) + + # Put the model together, print it and print it to a bioMod file + model.write(kinematic_model_file_path, bmc.C3dData(c3d_file_path)) + + model = brbd.Model(kinematic_model_file_path) + assert model.nbQ() == 7 + assert model.nbSegment() == 8 + assert model.nbMarkers() == 25 + if brbd.currentLinearAlgebraBackend() == 1: + # If CasADi backend is used + from casadi import MX + + q_sym = MX.sym("q", model.nbQ(), 1) + value = np.array(brbd.to_casadi_func("markers", model.markers, q_sym)(np.zeros((model.nbQ(),))))[:, -3] + else: + value = model.markers(np.zeros((model.nbQ(),)))[-3].to_array() + np.testing.assert_almost_equal(value, [0, 0.25, -0.85], decimal=4) + + if remove_temporary: + os.remove(kinematic_model_file_path) + os.remove(c3d_file_path) + + + +@pytest.mark.parametrize("brbd", brbd_to_test) +def test_complex_model(brbd, remove_temporary: bool = True): + brbd, bmc = brbd + + current_path_folder = os.path.dirname(os.path.realpath(__file__)) + mesh_path = f"{current_path_folder}/../../models/meshFiles/stl/pendulum.STL" + + kinematic_model_file_path = "temporary_complex.bioMod" + + # Create a model holder + bio_model = bmc.BiomechanicalModel() + + # The ground segment + bio_model.segments["GROUND"] = bmc.Segment(name="GROUND") + + # The pendulum segment + bio_model.segments["PENDULUM"] = bmc.Segment( + name="PENDULUM", + translations=bmc.Translations.XYZ, + rotations=bmc.Rotations.X, + q_ranges=bmc.RangeOfMotion(range_type=bmc.Ranges.Q, min_bound=[-1, -1, -1, -np.pi], max_bound=[1, 1, 1, np.pi]), + qdot_ranges=bmc.RangeOfMotion(range_type=bmc.Ranges.Qdot, min_bound=[-10, -10, -10, -np.pi*10], max_bound=[10, 10, 10, np.pi*10]), + mesh_file=bmc.MeshFile(mesh_file_name=mesh_path, + mesh_color=np.array([0, 0, 1]), + scaling_function=lambda m: np.array([1, 1, 10]), + rotation_function=lambda m: np.array([np.pi/2, 0, 0]), + translation_function=lambda m: np.array([0.1, 0, 0])), + ) + # The pendulum segment contact point + bio_model.segments["PENDULUM"].add_contact(bmc.Contact(name="PENDULUM_CONTACT", + function=lambda m: np.array([0, 0, 0]), + parent_name="PENDULUM", + axis=bmc.Translations.XYZ)) + + # The pendulum muscle group + bio_model.muscle_groups["PENDULUM_MUSCLE_GROUP"] = bmc.MuscleGroup(name="PENDULUM_MUSCLE_GROUP", + origin_parent_name="GROUND", + insertion_parent_name="PENDULUM") + + # The pendulum muscle + bio_model.muscles["PENDULUM_MUSCLE"] = bmc.Muscle("PENDULUM_MUSCLE", + muscle_type=bmc.MuscleType.HILLTHELEN, + state_type=bmc.MuscleStateType.DEGROOTE, + muscle_group="PENDULUM_MUSCLE_GROUP", + origin_position_function=lambda m: np.array([0, 0, 0]), + insertion_position_function=lambda m: np.array([0, 0, 1]), + optimal_length_function=lambda model, m: 0.1, + maximal_force_function=lambda m: 100.0, + tendon_slack_length_function=lambda model, m: 0.05, + pennation_angle_function=lambda model, m: 0.05, + maximal_excitation=1) + bio_model.via_points["PENDULUM_MUSCLE"] = bmc.ViaPoint("PENDULUM_MUSCLE", + position_function=lambda m: np.array([0, 0, 0.5]), + parent_name="PENDULUM", + muscle_name="PENDULUM_MUSCLE", + muscle_group="PENDULUM_MUSCLE_GROUP", + ) + + + # Put the model together, print it and print it to a bioMod file + bio_model.write(kinematic_model_file_path, {}) + + model = brbd.Model(kinematic_model_file_path) + assert model.nbQ() == 4 + assert model.nbSegment() == 2 + assert model.nbMarkers() == 0 + assert model.nbMuscles() == 1 + assert model.nbMuscleGroups() == 1 + assert model.nbContacts() == 3 + + if remove_temporary: + os.remove(kinematic_model_file_path) diff --git a/test/binding/Python3/test_utils.py b/test/binding/python3/test_utils.py similarity index 100% rename from test/binding/Python3/test_utils.py rename to test/binding/python3/test_utils.py