diff --git a/.github/workflows/codecov.yml b/.github/workflows/codecov.yml new file mode 100644 index 00000000..62262f7a --- /dev/null +++ b/.github/workflows/codecov.yml @@ -0,0 +1,26 @@ +# Check if the tests covers all the code +name: Codecov(erage). + +on: [pull_request] + +jobs: + build: + runs-on: ubuntu-latest + name: Test python API + steps: + - uses: actions/checkout@master + - name: Install requirements + run: pip install -r requirements.txt + - name: Run tests and collect coverage + run: pytest --cov . + - name: Upload coverage reports to Codecov +# run: | +# # Replace `linux` below with the appropriate OS +# # Options are `alpine`, `linux`, `macos`, `windows` +# curl -Os https://uploader.codecov.io/latest/linux/codecov +# chmod +x codecov +# ./codecov -t ${CODECOV_TOKEN} + - uses: codecov/codecov-action@v3 + name: Upload coverage to Codecov + with: + token: ${{ secrets.CODECOV_TOKEN }} \ No newline at end of file diff --git a/.github/workflows/run_tests.yml b/.github/workflows/run_tests.yml index 70e8319d..38351cb6 100644 --- a/.github/workflows/run_tests.yml +++ b/.github/workflows/run_tests.yml @@ -21,7 +21,7 @@ jobs: run: shell: bash -l {0} steps: - - uses: actions/checkout@v2 + - uses: actions/checkout@master - name: Setup environment uses: conda-incubator/setup-miniconda@v2 @@ -39,7 +39,7 @@ jobs: mamba list - name: Install extra dependencies - run: mamba install pytest-cov pytest pytest-cov codecov -cconda-forge + run: mamba install pytest-cov pytest codecov -cconda-forge - name: Run the actual tests on LINUX run: | @@ -48,13 +48,23 @@ jobs: if: matrix.label == 'linux-64' - name: Run the actual tests - run: pytest -v --color=yes --cov-report term-missing --cov=bionc tests + run: pytest -v --color=yes --cov-report=xml --cov=bionc tests if: matrix.label == 'osx-64' - name: Upload coverage to Codecov - run: codecov + run: codecov --token ${{ secrets.CODECOV_TOKEN }} if: matrix.label == 'linux-64' +# - name: Upload coverage to Codecov +# uses: codecov/codecov-action@v3 +# with: +# token: ${{ secrets.CODECOV_TOKEN }} +# fail_ci_if_error: true +# directory: /home/runner/work/bioNC +# files: ./coverage.xml +# if: matrix.label == 'linux-64' + + - name: Test installed version of bioviz on LINUX run: | python setup.py install diff --git a/bionc/__init__.py b/bionc/__init__.py index 9db019fe..3413c288 100644 --- a/bionc/__init__.py +++ b/bionc/__init__.py @@ -18,6 +18,8 @@ InertiaParameters, BiomechanicalModel, JointType, + ExternalForceList, + ExternalForce, ) from .protocols import natural_coordinates diff --git a/bionc/bionc_casadi/__init__.py b/bionc/bionc_casadi/__init__.py index 42f3ca16..ce98bc7b 100644 --- a/bionc/bionc_casadi/__init__.py +++ b/bionc/bionc_casadi/__init__.py @@ -14,3 +14,4 @@ from .inertia_parameters import InertiaParameters from .joint import Joint, GroundJoint from .natural_vector import NaturalVector +from .external_force import ExternalForceList, ExternalForce diff --git a/bionc/bionc_casadi/external_force.py b/bionc/bionc_casadi/external_force.py new file mode 100644 index 00000000..1ae61690 --- /dev/null +++ b/bionc/bionc_casadi/external_force.py @@ -0,0 +1,229 @@ +from casadi import MX, inv, cross +import numpy as np + +from .natural_vector import NaturalVector +from .natural_coordinates import SegmentNaturalCoordinates, NaturalCoordinates + + +class ExternalForce: + """ + This class represents an external force applied to a segment. + + Attributes + ---------- + application_point_in_local : np.ndarray + The application point of the force in the natural coordinate system of the segment + external_forces : np.ndarray + The external force vector in the global coordinate system (torque, force) + + Methods + ------- + from_components(application_point_in_local, force, torque) + This function creates an external force from its components. + force + This function returns the force vector of the external force. + torque + This function returns the torque vector of the external force. + compute_pseudo_interpolation_matrix() + This function computes the pseudo interpolation matrix of the external force. + to_natural_force + This function returns the external force in the natural coordinate format. + """ + + def __init__(self, application_point_in_local: np.ndarray | MX, external_forces: np.ndarray | MX): + self.application_point_in_local = MX(application_point_in_local) + self.external_forces = MX(external_forces) + + @classmethod + def from_components(cls, application_point_in_local: np.ndarray | MX, force: np.ndarray | MX, torque: np.ndarray | MX): + """ + This function creates an external force from its components. + + Parameters + ---------- + application_point_in_local : np.ndarray + The application point of the force in the natural coordinate system of the segment + force + The force vector in the global coordinate system + torque + The torque vector in the global coordinate system + + Returns + ------- + ExternalForce + """ + + return cls(application_point_in_local, np.concatenate((torque, force))) + + @property + def force(self) -> MX: + """The force vector in the global coordinate system""" + return self.external_forces[3:6] + + @property + def torque(self) -> MX: + """The torque vector in the global coordinate system""" + return self.external_forces[0:3] + + @staticmethod + def compute_pseudo_interpolation_matrix(Qi: SegmentNaturalCoordinates) -> MX: + """ + Return the force moment transformation matrix + + Parameters + ---------- + Qi : SegmentNaturalCoordinates + The natural coordinates of the segment + + Returns + ------- + np.ndarray + The force moment transformation matrix + """ + # default we apply force at the proximal point + + left_interpolation_matrix = MX.zeros((12, 3)) + + left_interpolation_matrix[9:12, 0] = Qi.u + left_interpolation_matrix[0:3, 1] = Qi.v + left_interpolation_matrix[3:6, 2] = -Qi.w + left_interpolation_matrix[6:9, 2] = Qi.w + + # Matrix of lever arms for forces equivalent to moment at proximal endpoint, denoted Bstar + lever_arm_force_matrix = MX.zeros((3, 3)) + + lever_arm_force_matrix[:, 0] = cross(Qi.w, Qi.u) + lever_arm_force_matrix[:, 1] = cross(Qi.u, Qi.v) + lever_arm_force_matrix[:, 2] = cross(-Qi.v, Qi.w) + + return (left_interpolation_matrix @ inv(lever_arm_force_matrix)).T # NOTE: inv may induce symbolic error. + + def to_natural_force(self, Qi: SegmentNaturalCoordinates) -> MX: + """ + Apply external forces to the segment + + Parameters + ---------- + Qi: SegmentNaturalCoordinates + Segment natural coordinates + + Returns + ------- + np.ndarray + The external forces adequately transformed for the equation of motion in natural coordinates + """ + + pseudo_interpolation_matrix = self.compute_pseudo_interpolation_matrix(Qi) + point_interpolation_matrix = NaturalVector(self.application_point_in_local).interpolate() + application_point_in_global = point_interpolation_matrix @ Qi + + fext = point_interpolation_matrix.T @ self.force + fext += pseudo_interpolation_matrix.T @ self.torque + + # Bour's formula to transport the moment from the application point to the proximal point + # fext += pseudo_interpolation_matrix.T @ np.cross(application_point_in_global - Qi.rp, self.force) + + return fext + + +class ExternalForceList: + """ + This class is made to handle all the external forces of each segment, if none are provided, it will be an empty list. + All segment forces are expressed in natural coordinates to be added to the equation of motion as: + + Q @ Qddot + K^T @ lambda = Weight + f_ext + + Attributes + ---------- + external_forces : list + List of ExternalForces for each segment + + Methods + ------- + add_external_force(segment_index, external_force) + This function adds an external force to the list of external forces. + empty_from_nb_segment(nb_segment) + This function creates an empty ExternalForceList from the number of segments. + to_natural_external_forces(Q) + This function returns the external forces in the natural coordinate format. + segment_external_forces(segment_index) + This function returns the external forces of a segment. + nb_segments + This function returns the number of segments. + + Examples + -------- + >>> from bionc import ExternalForceList, ExternalForce + >>> import numpy as np + >>> f_ext = ExternalForceList.empty_from_nb_segment(2) + >>> segment_force = ExternalForce(force=np.array([0,1,1.1]), torque=np.zeros(3), application_point_in_local=np.array([0,0.5,0])) + >>> f_ext.add_external_force(segment_index=0, external_force=segment_force) + """ + + def __init__(self, external_forces: list[list[ExternalForce, ...]] = None): + if external_forces is None: + raise ValueError( + "f_ext must be a list of ExternalForces, or use the classmethod" + "NaturalExternalForceList.empty_from_nb_segment(nb_segment)" + ) + self.external_forces = external_forces + + @property + def nb_segments(self) -> int: + """Returns the number of segments""" + return len(self.external_forces) + + @classmethod + def empty_from_nb_segment(cls, nb_segment: int): + """ + Create an empty NaturalExternalForceList from the model size + """ + return cls(external_forces=[[] for _ in range(nb_segment)]) + + def segment_external_forces(self, segment_index: int) -> list[ExternalForce]: + """Returns the external forces of the segment""" + return self.external_forces[segment_index] + + def add_external_force(self, segment_index: int, external_force: ExternalForce): + """ + Add an external force to the segment + + Parameters + ---------- + segment_index: int + The index of the segment + external_force: + The external force to add + """ + self.external_forces[segment_index].append(external_force) + + def to_natural_external_forces(self, Q: NaturalCoordinates) -> MX: + """ + Converts and sums all the segment natural external forces to the full vector of natural external forces + + Parameters + ---------- + Q : NaturalCoordinates + The natural coordinates of the model + """ + + if len(self.external_forces) != Q.nb_qi(): + raise ValueError( + "The number of segment in the model and the number of segment in the external forces must be the same" + ) + + natural_external_forces = MX.zeros((12 * Q.nb_qi(), 1)) + for segment_index, segment_external_forces in enumerate(self.external_forces): + segment_natural_external_forces = MX.zeros((12, 1)) + slice_index = slice(segment_index * 12, (segment_index + 1) * 12) + for external_force in segment_external_forces: + segment_natural_external_forces += external_force.to_natural_force(Q.vector(segment_index)) + natural_external_forces[slice_index, 0:1] = segment_natural_external_forces + + return natural_external_forces + + def __iter__(self): + return iter(self.external_forces) + + def __len__(self): + return len(self.external_forces) diff --git a/bionc/bionc_casadi/joint.py b/bionc/bionc_casadi/joint.py index 0952735f..243835e5 100644 --- a/bionc/bionc_casadi/joint.py +++ b/bionc/bionc_casadi/joint.py @@ -1,4 +1,4 @@ -from casadi import MX, dot, cos, transpose, sumsqr +from casadi import MX, dot, cos, transpose, sumsqr, vertcat import numpy as np from .natural_segment import NaturalSegment @@ -568,7 +568,7 @@ def constraint(self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNatura return constraint def parent_constraint_jacobian( - self, Q_child: SegmentNaturalCoordinates, Q_parent: SegmentNaturalCoordinates + self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNaturalCoordinates ) -> MX: parent_point_location = self.parent_point.position_in_global(Q_parent) child_point_location = self.child_point.position_in_global(Q_child) @@ -588,7 +588,7 @@ def child_constraint_jacobian( return K_k_child def parent_constraint_jacobian_derivative( - self, Qdot_child: SegmentNaturalVelocities, Qdot_parent: SegmentNaturalVelocities + self, Qdot_parent: SegmentNaturalVelocities, Qdot_child: SegmentNaturalVelocities ) -> MX: parent_point_location = self.parent_point.position_in_global(Qdot_parent) child_point_location = self.child_point.position_in_global(Qdot_child) @@ -871,14 +871,16 @@ def __init__( self, name: str, child: NaturalSegment, - Q_child_ref: SegmentNaturalCoordinates, + rp_child_ref: SegmentNaturalCoordinates = None, + rd_child_ref: SegmentNaturalCoordinates = None, index: int = None, ): super(GroundJoint.Weld, self).__init__(name, None, child, index) # check size and type of parent axis - self.Q_child_ref = Q_child_ref - self.nb_constraints = 12 + self.rp_child_ref = rp_child_ref + self.rd_child_ref = rd_child_ref + self.nb_constraints = 6 def constraint(self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNaturalCoordinates) -> MX: """ @@ -891,7 +893,7 @@ def constraint(self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNatura Kinematic constraints of the joint [12, 1] """ - return self.Q_child_ref - Q_child + return vertcat(self.rp_child_ref - Q_child.rp, self.rd_child_ref - Q_child.rd) def parent_constraint_jacobian( self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNaturalCoordinates @@ -901,7 +903,7 @@ def parent_constraint_jacobian( def child_constraint_jacobian( self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNaturalCoordinates ) -> MX: - K_k_child = -MX.eye(self.nb_constraints) + K_k_child = -MX.eye(12)[3:9, :] return K_k_child diff --git a/bionc/bionc_casadi/natural_segment.py b/bionc/bionc_casadi/natural_segment.py index 8b00f11c..b657c678 100644 --- a/bionc/bionc_casadi/natural_segment.py +++ b/bionc/bionc_casadi/natural_segment.py @@ -2,7 +2,7 @@ import numpy as np from casadi import MX -from casadi import cos, sin, transpose, vertcat, sqrt, inv, dot, sum1, sum2 +from casadi import cos, sin, transpose, vertcat, sqrt, inv, dot, sum1, cross, norm_2 from ..bionc_casadi.natural_coordinates import SegmentNaturalCoordinates from ..bionc_casadi.natural_velocities import SegmentNaturalVelocities diff --git a/bionc/bionc_numpy/__init__.py b/bionc/bionc_numpy/__init__.py index 77afdcc4..9b8c1847 100644 --- a/bionc/bionc_numpy/__init__.py +++ b/bionc/bionc_numpy/__init__.py @@ -16,3 +16,4 @@ from .joint import Joint, GroundJoint from .natural_vector import NaturalVector from .inverse_kinematics import InverseKinematics +from .external_force import ExternalForceList, ExternalForce diff --git a/bionc/bionc_numpy/biomechanical_model.py b/bionc/bionc_numpy/biomechanical_model.py index c8f1b019..d748421b 100644 --- a/bionc/bionc_numpy/biomechanical_model.py +++ b/bionc/bionc_numpy/biomechanical_model.py @@ -6,6 +6,7 @@ from .natural_accelerations import NaturalAccelerations from ..protocols.biomechanical_model import GenericBiomechanicalModel from .inverse_kinematics import InverseKinematics +from .external_force import ExternalForceList class BiomechanicalModel(GenericBiomechanicalModel): @@ -632,7 +633,7 @@ def forward_dynamics( self, Q: NaturalCoordinates, Qdot: NaturalCoordinates, - # external_forces: ExternalForces + external_forces: ExternalForceList = None, stabilization: dict = None, ) -> np.ndarray: """ @@ -644,6 +645,8 @@ def forward_dynamics( The natural coordinates of the segment [12 * nb_segments, 1] Qdot : NaturalCoordinates The natural coordinates time derivative of the segment [12 * nb_segments, 1] + external_forces : ExternalForceList + The list of external forces applied on the system stabilization: dict Dictionary containing the Baumgarte's stabilization parameters: * alpha: float @@ -660,6 +663,11 @@ def forward_dynamics( K = self.holonomic_constraints_jacobian(Q) Kdot = self.holonomic_constraints_jacobian_derivative(Qdot) + external_forces = ( + ExternalForceList.empty_from_nb_segment(self.nb_segments) if external_forces is None else external_forces + ) + fext = external_forces.to_natural_external_forces(Q) + # KKT system # [G, K.T] [Qddot] = [forces] # [K, 0 ] [lambda] = [biais] @@ -667,7 +675,7 @@ def forward_dynamics( lower_KKT_matrix = np.concatenate((K, np.zeros((K.shape[0], K.shape[0]))), axis=1) KKT_matrix = np.concatenate((upper_KKT_matrix, lower_KKT_matrix), axis=0) - forces = self.weight() + forces = self.weight() + fext biais = -Kdot @ Qdot if stabilization is not None: diff --git a/bionc/bionc_numpy/external_force.py b/bionc/bionc_numpy/external_force.py new file mode 100644 index 00000000..f8ad4c77 --- /dev/null +++ b/bionc/bionc_numpy/external_force.py @@ -0,0 +1,230 @@ +import numpy as np + +from .natural_vector import NaturalVector +from .natural_coordinates import SegmentNaturalCoordinates, NaturalCoordinates + + +class ExternalForce: + """ + This class represents an external force applied to a segment. + + Attributes + ---------- + application_point_in_local : np.ndarray + The application point of the force in the natural coordinate system of the segment + external_forces : np.ndarray + The external force vector in the global coordinate system (torque, force) + + Methods + ------- + from_components(application_point_in_local, force, torque) + This function creates an external force from its components. + force + This function returns the force vector of the external force. + torque + This function returns the torque vector of the external force. + compute_pseudo_interpolation_matrix() + This function computes the pseudo interpolation matrix of the external force. + to_natural_force + This function returns the external force in the natural coordinate format. + """ + + def __init__(self, application_point_in_local: np.ndarray, external_forces: np.ndarray): + self.application_point_in_local = application_point_in_local + self.external_forces = external_forces + + @classmethod + def from_components(cls, application_point_in_local: np.ndarray, force: np.ndarray, torque: np.ndarray): + """ + This function creates an external force from its components. + + Parameters + ---------- + application_point_in_local : np.ndarray + The application point of the force in the natural coordinate system of the segment + force + The force vector in the global coordinate system + torque + The torque vector in the global coordinate system + + Returns + ------- + ExternalForce + """ + + return cls(application_point_in_local, np.concatenate((torque, force))) + + @property + def force(self) -> np.ndarray: + """The force vector in the global coordinate system""" + return self.external_forces[3:6] + + @property + def torque(self) -> np.ndarray: + """The torque vector in the global coordinate system""" + return self.external_forces[0:3] + + @staticmethod + def compute_pseudo_interpolation_matrix(Qi: SegmentNaturalCoordinates) -> np.ndarray: + """ + Return the force moment transformation matrix + + Parameters + ---------- + Qi : SegmentNaturalCoordinates + The natural coordinates of the segment + + Returns + ------- + np.ndarray + The force moment transformation matrix + """ + # default we apply force at the proximal point + + left_interpolation_matrix = np.zeros((12, 3)) + + left_interpolation_matrix[9:12, 0] = Qi.u + left_interpolation_matrix[0:3, 1] = Qi.v + left_interpolation_matrix[3:6, 2] = -Qi.w + left_interpolation_matrix[6:9, 2] = Qi.w + + # Matrix of lever arms for forces equivalent to moment at proximal endpoint, denoted Bstar + lever_arm_force_matrix = np.zeros((3, 3)) + + lever_arm_force_matrix[:, 0] = np.cross(Qi.w, Qi.u) + lever_arm_force_matrix[:, 1] = np.cross(Qi.u, Qi.v) + lever_arm_force_matrix[:, 2] = np.cross(-Qi.v, Qi.w) + + return (left_interpolation_matrix @ np.linalg.inv(lever_arm_force_matrix)).T + + def to_natural_force(self, Qi: SegmentNaturalCoordinates) -> np.ndarray: + """ + Apply external forces to the segment + + Parameters + ---------- + Qi: SegmentNaturalCoordinates + Segment natural coordinates + + Returns + ------- + np.ndarray + The external forces adequately transformed for the equation of motion in natural coordinates + """ + + pseudo_interpolation_matrix = self.compute_pseudo_interpolation_matrix(Qi) + point_interpolation_matrix = NaturalVector(self.application_point_in_local).interpolate() + application_point_in_global = np.array(point_interpolation_matrix @ Qi).squeeze() + + fext = point_interpolation_matrix.T @ self.force + fext += pseudo_interpolation_matrix.T @ self.torque + + # Bour's formula to transport the moment from the application point to the proximal point + # fext += pseudo_interpolation_matrix.T @ np.cross(application_point_in_global - Qi.rp, self.force) + + return np.array(fext) + + +class ExternalForceList: + """ + This class is made to handle all the external forces of each segment, if none are provided, it will be an empty list. + All segment forces are expressed in natural coordinates to be added to the equation of motion as: + + Q @ Qddot + K^T @ lambda = Weight + f_ext + + Attributes + ---------- + external_forces : list + List of ExternalForces for each segment + + Methods + ------- + add_external_force(segment_index, external_force) + This function adds an external force to the list of external forces. + empty_from_nb_segment(nb_segment) + This function creates an empty ExternalForceList from the number of segments. + to_natural_external_forces(Q) + This function returns the external forces in the natural coordinate format. + segment_external_forces(segment_index) + This function returns the external forces of a segment. + nb_segments + This function returns the number of segments. + + Examples + -------- + >>> from bionc import ExternalForceList, ExternalForce + >>> import numpy as np + >>> f_ext = ExternalForceList.empty_from_nb_segment(2) + >>> segment_force = ExternalForce(force=np.array([0,1,1.1]), torque=np.zeros(3), application_point_in_local=np.array([0,0.5,0])) + >>> f_ext.add_external_force(segment_index=0, external_force=segment_force) + """ + + def __init__(self, external_forces: list[list[ExternalForce, ...]] = None): + if external_forces is None: + raise ValueError( + "f_ext must be a list of ExternalForces, or use the classmethod" + "NaturalExternalForceList.empty_from_nb_segment(nb_segment)" + ) + self.external_forces = external_forces + + @property + def nb_segments(self) -> int: + """Returns the number of segments""" + return len(self.external_forces) + + @classmethod + def empty_from_nb_segment(cls, nb_segment: int): + """ + Create an empty NaturalExternalForceList from the model size + """ + return cls(external_forces=[[] for _ in range(nb_segment)]) + + def segment_external_forces(self, segment_index: int) -> list[ExternalForce]: + """Returns the external forces of the segment""" + return self.external_forces[segment_index] + + def add_external_force(self, segment_index: int, external_force: ExternalForce): + """ + Add an external force to the segment + + Parameters + ---------- + segment_index: int + The index of the segment + external_force: + The external force to add + """ + self.external_forces[segment_index].append(external_force) + + def to_natural_external_forces(self, Q: NaturalCoordinates) -> np.ndarray: + """ + Converts and sums all the segment natural external forces to the full vector of natural external forces + + Parameters + ---------- + Q : NaturalCoordinates + The natural coordinates of the model + """ + + if len(self.external_forces) != Q.nb_qi(): + raise ValueError( + "The number of segment in the model and the number of segment in the external forces must be the same" + ) + + natural_external_forces = np.zeros((12 * Q.nb_qi(), 1)) + for segment_index, segment_external_forces in enumerate(self.external_forces): + segment_natural_external_forces = np.zeros((12, 1)) + slice_index = slice(segment_index * 12, (segment_index + 1) * 12) + for external_force in segment_external_forces: + segment_natural_external_forces += external_force.to_natural_force(Q.vector(segment_index))[ + :, np.newaxis + ] + natural_external_forces[slice_index, 0:1] = segment_natural_external_forces + + return natural_external_forces + + def __iter__(self): + return iter(self.external_forces) + + def __len__(self): + return len(self.external_forces) diff --git a/bionc/bionc_numpy/inverse_kinematics.py b/bionc/bionc_numpy/inverse_kinematics.py index 1ccc9135..ceaa19ca 100644 --- a/bionc/bionc_numpy/inverse_kinematics.py +++ b/bionc/bionc_numpy/inverse_kinematics.py @@ -1,4 +1,4 @@ -from casadi import vertcat, horzcat, MX, Function, nlpsol +from casadi import vertcat, horzcat, MX, Function, nlpsol, SX, Function import numpy as np from pyomeca import Markers @@ -8,6 +8,26 @@ ) from ..protocols.biomechanical_model import GenericBiomechanicalModel as BiomechanicalModel +from ..bionc_numpy.natural_coordinates import NaturalCoordinates as NaturalCoordinatesNumpy + + +def _mx_to_sx(mx: MX, symbolics: list[MX]) -> SX: + """ + Converts a MX to a SX + + Parameters + ---------- + mx : MX + The MX to convert + symbolics : list[MX] + The symbolics to use + + Returns + ------- + The converted SX + """ + f = Function("f", symbolics, [mx]).expand() + return f(*symbolics) def _solve_nlp(method: str, nlp: dict, Q_init: np.ndarray, lbg: np.ndarray, ubg: np.ndarray, options: dict): @@ -47,6 +67,29 @@ def _solve_nlp(method: str, nlp: dict, Q_init: np.ndarray, lbg: np.ndarray, ubg: return r +def sarrus(matrix: MX): + """ + Computes the determinant of a 3x3 matrix using the Sarrus rule + + Parameters + ---------- + matrix : MX + The matrix to compute the determinant of + + Returns + ------- + The determinant of the matrix + """ + return ( + matrix[0, 0] * matrix[1, 1] * matrix[2, 2] + + matrix[0, 1] * matrix[1, 2] * matrix[2, 0] + + matrix[0, 2] * matrix[1, 0] * matrix[2, 1] + - matrix[0, 0] * matrix[1, 2] * matrix[2, 1] + - matrix[0, 1] * matrix[1, 0] * matrix[2, 2] + - matrix[0, 2] * matrix[1, 1] * matrix[2, 0] + ) + + class InverseKinematics: """ Inverse kinematics solver also known as Multibody Kinematics Optimization (MKO) @@ -90,6 +133,8 @@ class InverseKinematics: builds the constraints to satisfy animate() Animates the solution of the inverse kinematics + _active_direct_frame_constraints() + builds the constraints to ensure that the determinant of the matrix [u, v, w] is positive """ def __init__( @@ -98,8 +143,30 @@ def __init__( experimental_markers: np.ndarray | str, Q_init: np.ndarray | NaturalCoordinates = None, solve_frame_per_frame: bool = True, + active_direct_frame_constraints: bool = False, + use_sx: bool = True, ): + """ + Parameters + ---------- + model : BiomechanicalModel + The model considered (bionc.numpy) + experimental_markers : np.ndarray | str + The experimental markers (3xNxM numpy array), or a path to a c3d file + Q_init : np.ndarray | NaturalCoordinates + The initial guess for the inverse kinematics computed from the experimental markers + solve_frame_per_frame : bool + If True, the inverse kinematics is solved frame per frame, otherwise it is solved for the whole motion + active_direct_frame_constraints : bool + If True, the direct frame constraints are active, otherwise they are not. + It ensures that rigid body constraints lead to positive determinants or the matrix [u, v, w]. + use_sx : bool + If True, the symbolic variables are SX, otherwise they are MX (SX are faster but take more memory) + """ + self._frame_per_frame = solve_frame_per_frame + self._active_direct_frame_constraints = active_direct_frame_constraints + self.use_sx = use_sx if not isinstance(model, BiomechanicalModel): raise ValueError("model must be a BiomechanicalModel") @@ -125,6 +192,7 @@ def __init__( self.Q_init = Q_init self.Qopt = None + self.segment_determinants = None self.nb_frames = self.experimental_markers.shape[2] self.nb_markers = self.experimental_markers.shape[1] @@ -192,31 +260,44 @@ def solve( lbg = np.zeros(self.model.nb_holonomic_constraints) ubg = np.zeros(self.model.nb_holonomic_constraints) constraints = self._constraints(self._Q_sym) + if self._active_direct_frame_constraints: + constraints = vertcat(constraints, self._direct_frame_constraints(self._Q_sym)) + lbg = np.concatenate((lbg, np.zeros(self.model.nb_segments))) + # upper bounds infinity + ubg = np.concatenate((ubg, np.full(self.model.nb_segments, np.inf))) nlp = dict( x=self._vert_Q_sym, - g=constraints, + g=_mx_to_sx(constraints, [self._vert_Q_sym]) if self.use_sx else constraints, ) for f in range(self.nb_frames): - nlp["f"] = self._objective_function(self._Q_sym, self.experimental_markers[:, :, f]) + objective = self._objective_function(self._Q_sym, self.experimental_markers[:, :, f]) + nlp["f"] = _mx_to_sx(objective, [self._vert_Q_sym]) if self.use_sx else objective Q_init = self.Q_init[:, f : f + 1] r = _solve_nlp(method, nlp, Q_init, lbg, ubg, options) Qopt[:, f : f + 1] = r["x"].toarray() else: constraints = self._constraints(self._Q_sym) + if self._active_direct_frame_constraints: + constraints = vertcat(constraints, self._direct_frame_constraints(self._Q_sym)) objective = self._objective(self._Q_sym, self.experimental_markers) nlp = dict( x=self._vert_Q_sym, - f=objective, - g=constraints, + f=_mx_to_sx(objective, [self._vert_Q_sym]) if self.use_sx else objective, + g=_mx_to_sx(constraints, [self._vert_Q_sym]) if self.use_sx else constraints, ) Q_init = self.Q_init.reshape((12 * self.model.nb_segments * self.nb_frames, 1)) lbg = np.zeros(self.model.nb_holonomic_constraints * self.nb_frames) ubg = np.zeros(self.model.nb_holonomic_constraints * self.nb_frames) + if self._active_direct_frame_constraints: + lbg = np.concatenate((lbg, np.zeros(self.model.nb_segments * self.nb_frames))) + ubg = np.concatenate((ubg, np.full(self.model.nb_segments * self.nb_frames, np.inf))) r = _solve_nlp(method, nlp, Q_init, lbg, ubg, options) Qopt = r["x"].reshape((12 * self.model.nb_segments, self.nb_frames)).toarray() self.Qopt = Qopt.reshape((12 * self.model.nb_segments, self.nb_frames)) + self.check_segment_determinants() + return Qopt def _declare_sym_Q(self) -> tuple[MX, MX]: @@ -256,4 +337,27 @@ def _constraints(self, Q) -> MX: phik.append(self._model_mx.joint_constraints(Q_f)) return vertcat(*phir, *phik) + def _direct_frame_constraints(self, Q): + """Computes the direct frame constraints and handle single frame or multi frames""" + nb_frames = 1 if self._frame_per_frame else self.nb_frames + direct_frame_constraints = [] + for f in range(nb_frames): + Q_f = NaturalCoordinates(Q[:, f]) + for ii in range(self.model.nb_segments): + u, v, w = Q_f.vector(ii).to_uvw() + direct_frame_constraints.append(sarrus(horzcat(u, v, w))) + return vertcat(*direct_frame_constraints) + + def check_segment_determinants(self): + """Checks the determinant of each segment frame""" + self.segment_determinants = np.zeros((self.model.nb_segments, self.nb_frames)) + for i in range(0, self.Qopt.shape[1]): + Qi = NaturalCoordinatesNumpy(self.Qopt)[:, i : i + 1] + for s in range(0, self.model.nb_segments): + u, v, w = Qi.vector(s).to_uvw() + matrix = np.concatenate((u[:, np.newaxis], v[:, np.newaxis], w[:, np.newaxis]), axis=1) + self.segment_determinants[s, i] = np.linalg.det(matrix) + if self.segment_determinants[s, i] < 0: + print(f"Warning: frame {i} segment {s} has a negative determinant") + # todo: def sol() -> dict that returns the details of the inverse kinematics such as all the metrics, etc... diff --git a/bionc/bionc_numpy/joint.py b/bionc/bionc_numpy/joint.py index 4262b8fa..33fc3fb2 100644 --- a/bionc/bionc_numpy/joint.py +++ b/bionc/bionc_numpy/joint.py @@ -643,7 +643,7 @@ def constraint(self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNatura return constraint def parent_constraint_jacobian( - self, Q_child: SegmentNaturalCoordinates, Q_parent: SegmentNaturalCoordinates + self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNaturalCoordinates ) -> np.ndarray: parent_point_location = self.parent_point.position_in_global(Q_parent) child_point_location = self.child_point.position_in_global(Q_child) @@ -663,7 +663,7 @@ def child_constraint_jacobian( return np.array(K_k_child) def parent_constraint_jacobian_derivative( - self, Qdot_child: SegmentNaturalVelocities, Qdot_parent: SegmentNaturalVelocities + self, Qdot_parent: SegmentNaturalVelocities, Qdot_child: SegmentNaturalVelocities ) -> np.ndarray: parent_point_location = self.parent_point.position_in_global(Qdot_parent) child_point_location = self.child_point.position_in_global(Qdot_child) @@ -870,7 +870,7 @@ def constraint_jacobian_derivative( Returns ------- - MX + np.ndarray joint constraints jacobian of the child segment [5, 12] """ @@ -1010,14 +1010,16 @@ def __init__( self, name: str, child: NaturalSegment, - Q_child_ref: SegmentNaturalCoordinates | np.ndarray = None, + rp_child_ref: SegmentNaturalCoordinates | np.ndarray = None, + rd_child_ref: SegmentNaturalCoordinates | np.ndarray = None, index: int = None, ): super(GroundJoint.Weld, self).__init__(name, None, child, index) + self.rp_child_ref = rp_child_ref + self.rd_child_ref = rd_child_ref # check size and type of parent axis - self.Q_child_ref = SegmentNaturalCoordinates(Q_child_ref) - self.nb_constraints = 12 + self.nb_constraints = 6 def constraint(self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNaturalCoordinates) -> np.ndarray: """ @@ -1030,7 +1032,7 @@ def constraint(self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNatura Kinematic constraints of the joint [12, 1] """ - return self.Q_child_ref.vector - Q_child.vector + return np.concatenate((self.rp_child_ref - Q_child.rp, self.rd_child_ref - Q_child.rd), axis=0) def parent_constraint_jacobian( self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNaturalCoordinates @@ -1040,7 +1042,7 @@ def parent_constraint_jacobian( def child_constraint_jacobian( self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNaturalCoordinates ) -> np.ndarray: - K_k_child = -np.eye(self.nb_constraints) + K_k_child = -np.eye(12)[3:9, :] return K_k_child @@ -1101,5 +1103,6 @@ def to_mx(self): name=self.name, child=self.child.to_mx(), index=self.index, - Q_child_ref=self.Q_child_ref, + rp_child_ref=self.rp_child_ref, + rd_child_ref=self.rd_child_ref, ) diff --git a/bionc/protocols/biomechanical_model.py b/bionc/protocols/biomechanical_model.py index 5fbdaa4f..c0bedddf 100644 --- a/bionc/protocols/biomechanical_model.py +++ b/bionc/protocols/biomechanical_model.py @@ -314,6 +314,13 @@ def nb_markers_technical(self) -> int: nb_markers += self.segments[key].nb_markers_technical return nb_markers + @property + def segment_names(self) -> list[str]: + """ + This function returns the names of the segments in the model + """ + return list(self.segments.keys()) + @property def marker_names(self) -> list[str]: """ @@ -468,6 +475,17 @@ def segment_from_index(self, index: int): return segment raise ValueError(f"The segment index does not exist, the model has only {self.nb_segments} segments") + @property + def normalized_coordinates(self) -> tuple[tuple[int, ...]]: + idx = [] + for i in range(self.nb_segments): + # create list from i* 12 to (i+1) * 12 + segment_idx = [i for i in range(i * 12, (i + 1) * 12)] + idx.append(segment_idx[0:3]) + idx.append(segment_idx[9:12]) + + return idx + @property def mass_matrix(self): """ diff --git a/examples/forward_dynamics/pendulum_with_force.nmod b/examples/forward_dynamics/pendulum_with_force.nmod new file mode 100644 index 00000000..f91c1971 Binary files /dev/null and b/examples/forward_dynamics/pendulum_with_force.nmod differ diff --git a/examples/forward_dynamics/pendulum_with_force.py b/examples/forward_dynamics/pendulum_with_force.py new file mode 100644 index 00000000..ad457f1e --- /dev/null +++ b/examples/forward_dynamics/pendulum_with_force.py @@ -0,0 +1,240 @@ +import numpy as np + +from bionc.bionc_numpy import ( + BiomechanicalModel, + NaturalSegment, + JointType, + SegmentNaturalCoordinates, + NaturalCoordinates, + SegmentNaturalVelocities, + NaturalVelocities, + ExternalForceList, + ExternalForce, +) +from bionc import NaturalAxis, CartesianAxis + + +def build_pendulum(): + # Let's create a model + model = BiomechanicalModel() + # fill the biomechanical model with the segment + model["pendulum"] = NaturalSegment( + name="pendulum", + alpha=np.pi / 2, # setting alpha, beta, gamma to pi/2 creates a orthogonal coordinate system + beta=np.pi / 2, + gamma=np.pi / 2, + length=1, + mass=1, + center_of_mass=np.array([0, -0.5, 0]), # in segment coordinates system + inertia=np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]), # in segment coordinates system + ) + # add a revolute joint (still experimental) + # if you want to add a revolute joint, + # you need to ensure that x is always orthogonal to u and v + model._add_joint( + dict( + name="hinge", + joint_type=JointType.GROUND_REVOLUTE, + parent="GROUND", + child="pendulum", + parent_axis=[CartesianAxis.X, CartesianAxis.X], + child_axis=[NaturalAxis.V, NaturalAxis.W], # meaning we pivot around the cartesian x-axis + theta=[np.pi / 2, np.pi / 2], + ) + ) + + model.save("pendulum_with_force.nmod") + + return model + + +def apply_force_and_drop_pendulum(t_final: float = 10, external_forces: ExternalForceList = None): + """ + This function is used to test the external force + + Parameters + ---------- + t_final: float + The final time of the simulation + external_forces: ExternalForceList + The external forces applied to the model + + Returns + ------- + tuple[BiomechanicalModel, np.ndarray, np.ndarray, Callable]: + model : BiomechanicalModel + The model to be simulated + time_steps : np.ndarray + The time steps of the simulation + all_states : np.ndarray + The states of the system at each time step X = [Q, Qdot] + dynamics : Callable + The dynamics of the system, f(t, X) = [Xdot, lambdas] + + """ + model = build_pendulum() + + Qi = SegmentNaturalCoordinates.from_components(u=[1, 0, 0], rp=[0, 0, 0], rd=[0, -1, 0], w=[0, 0, 1]) + Q = NaturalCoordinates(Qi) + Qdoti = SegmentNaturalVelocities.from_components(udot=[0, 0, 0], rpdot=[0, 0, 0], rddot=[0, 0, 0], wdot=[0, 0, 0]) + Qdot = NaturalVelocities(Qdoti) + + time_steps, all_states, dynamics = drop_the_pendulum( + model=model, + Q_init=Q, + Qdot_init=Qdot, + external_forces=external_forces, + t_final=t_final, + steps_per_second=60, + ) + + return model, time_steps, all_states, dynamics + + +def drop_the_pendulum( + model: BiomechanicalModel, + Q_init: NaturalCoordinates, + Qdot_init: NaturalVelocities, + external_forces: ExternalForceList, + t_final: float = 2, + steps_per_second: int = 50, +): + """ + This function simulates the dynamics of a natural segment falling from 0m during 2s + + Parameters + ---------- + model : BiomechanicalModel + The model to be simulated + Q_init : SegmentNaturalCoordinates + The initial natural coordinates of the segment + Qdot_init : SegmentNaturalVelocities + The initial natural velocities of the segment + external_forces : ExternalForceList + The external forces applied to the model + t_final : float, optional + The final time of the simulation, by default 2 + steps_per_second : int, optional + The number of steps per second, by default 50 + + Returns + ------- + tuple: + time_steps : np.ndarray + The time steps of the simulation + all_states : np.ndarray + The states of the system at each time step X = [Q, Qdot] + dynamics : Callable + The dynamics of the system, f(t, X) = [Xdot, lambdas] + """ + + print("Evaluate Rigid Body Constraints:") + print(model.rigid_body_constraints(Q_init)) + print("Evaluate Rigid Body Constraints Jacobian Derivative:") + print(model.rigid_body_constraint_jacobian_derivative(Qdot_init)) + + if (model.rigid_body_constraints(Q_init) > 1e-6).any(): + print(model.rigid_body_constraints(Q_init)) + raise ValueError( + "The segment natural coordinates don't satisfy the rigid body constraint, at initial conditions." + ) + + t_final = t_final # [s] + steps_per_second = steps_per_second + time_steps = np.linspace(0, t_final, steps_per_second * t_final + 1) + + # initial conditions, x0 = [Qi, Qidot] + states_0 = np.concatenate((Q_init.to_array(), Qdot_init.to_array()), axis=0) + + fext = external_forces + + # Create the forward dynamics function Callable (f(t, x) -> xdot) + def dynamics(t, states): + idx_coordinates = slice(0, model.nb_Q) + idx_velocities = slice(model.nb_Q, model.nb_Q + model.nb_Qdot) + + qddot, lambdas = model.forward_dynamics( + NaturalCoordinates(states[idx_coordinates]), + NaturalVelocities(states[idx_velocities]), + external_forces=fext, + ) + return np.concatenate((states[idx_velocities], qddot.to_array()), axis=0), lambdas + + # Solve the Initial Value Problem (IVP) for each time step + normalize_idx = model.normalized_coordinates + all_states = RK4(t=time_steps, f=lambda t, states: dynamics(t, states)[0], y0=states_0, normalize_idx=normalize_idx) + + return time_steps, all_states, dynamics + + +def RK4( + t: np.ndarray, + f, + y0: np.ndarray, + normalize_idx: tuple[tuple[int, ...]] = None, + args=(), +) -> np.ndarray: + """ + Runge-Kutta 4th order method + + Parameters + ---------- + t : array_like + time steps + f : Callable + function to be integrated in the form f(t, y, *args) + y0 : np.ndarray + initial conditions of states + normalize_idx : tuple(tuple) + indices of states to be normalized together + + Returns + ------- + y : array_like + states for each time step + + """ + n = len(t) + y = np.zeros((len(y0), n)) + y[:, 0] = y0 + for i in range(n - 1): + h = t[i + 1] - t[i] + yi = np.squeeze(y[:, i]) + k1 = f(t[i], yi, *args) + k2 = f(t[i] + h / 2.0, yi + k1 * h / 2.0, *args) + k3 = f(t[i] + h / 2.0, yi + k2 * h / 2.0, *args) + k4 = f(t[i] + h, yi + k3 * h, *args) + y[:, i + 1] = yi + (h / 6.0) * (k1 + 2 * k2 + 2 * k3 + k4) + + # verify after each time step the normalization of the states + if normalize_idx is not None: + for idx in normalize_idx: + y[idx, i + 1] = y[idx, i + 1] / np.linalg.norm(y[idx, i + 1]) + return y + + +if __name__ == "__main__": + # add an external force applied on the segment 0 + # first build the object + fext = ExternalForceList.empty_from_nb_segment(1) + # then add a force + force1 = ExternalForce.from_components( + # this force will prevent the pendulum to fall + force=np.array([0, 0, 1 * 9.81]), + torque=np.array([0, 0, 0]), + application_point_in_local=np.array([0, -0.5, 0]), + # this moment will prevent the pendulum to fall + # force=np.array([0, 0, 0]), + # torque=np.array([-1 * 9.81 * 0.50, 0, 0]), + # application_point_in_local=np.array([0, 0, 0]), + ) + # then add the force to the list on segment 0 + fext.add_external_force(external_force=force1, segment_index=0) + + model, time_steps, all_states, dynamics = apply_force_and_drop_pendulum(t_final=10, external_forces=fext) + + # animate the motion + from bionc import Viz + + viz = Viz(model) + viz.animate(all_states[:12, :], None) diff --git a/examples/inverse_kinematics/inverse_kinematics_study.py b/examples/inverse_kinematics/inverse_kinematics_study.py new file mode 100644 index 00000000..78256dff --- /dev/null +++ b/examples/inverse_kinematics/inverse_kinematics_study.py @@ -0,0 +1,72 @@ +""" +This example shows how to use the InverseKinematics class to solve an inverse kinematics problem. +""" +from bionc import InverseKinematics, Viz +import numpy as np +from pyomeca import Markers +from tests.utils import TestUtils +import time + + +def main(): + # build the model from the lower limb example + bionc = TestUtils.bionc_folder() + module = TestUtils.load_module(bionc + "/examples/model_creation/main.py") + + # Generate c3d file + filename = module.generate_c3d_file() + # Generate model + model = module.model_creation_from_measured_data(filename) + + # getting noisy markers for 200 frames + markers = Markers.from_c3d(filename).to_numpy()[:3, :, :] # 2 frames + markers = np.repeat(markers, 100, axis=2) # 2 x 100 frames + np.random.seed(42) + markers = markers + np.random.normal(0, 0.01, markers.shape) # add noise + + # you can import the class from bionc + ik_solver = InverseKinematics( + model, markers, solve_frame_per_frame=True, active_direct_frame_constraints=True, use_sx=True + ) + + tic1 = time.time() + Qopt_ipopt = ik_solver.solve(method="sqpmethod") # tend to find lower cost functions but may flip axis. + toc1 = time.time() + + print(f"time to solve 200 frames with ipopt: {toc1 - tic1}") + + return ik_solver, Qopt_ipopt, model, markers + + +if __name__ == "__main__": + ik_solver, Qopt, model, markers = main() + + # display the results of the optimization + import matplotlib.pyplot as plt + + plt.figure() + from bionc import NaturalCoordinates + + det = np.zeros((model.nb_segments, Qopt.shape[1])) + for i in range(0, Qopt.shape[1]): + Qi = NaturalCoordinates(Qopt)[:, i : i + 1] + for s in range(0, model.nb_segments): + u, v, w = Qi.vector(s).to_uvw() + matrix = np.concatenate((u[:, np.newaxis], v[:, np.newaxis], w[:, np.newaxis]), axis=1) + det[s, i] = np.linalg.det(matrix) + if det[s, i] < 0: + print(f"frame {i} segment {s} has a negative determinant") + + plt.plot(det.T, label=model.segment_names, marker="o", ms=1.5) + # set ylim -5 5 + plt.ylim(-5, 5) + plt.legend() + plt.show() + + viz = Viz( + model, + show_center_of_mass=False, # no center of mass in this example + show_xp_markers=True, + show_model_markers=True, + ) + viz.animate(Qopt[:, 197:198], markers_xp=markers[:, :, 197:198]) diff --git a/examples/knee_parallel_mechanism/knee_feikes.py b/examples/knee_parallel_mechanism/knee_feikes.py new file mode 100644 index 00000000..37ab7151 --- /dev/null +++ b/examples/knee_parallel_mechanism/knee_feikes.py @@ -0,0 +1,252 @@ +import numpy as np + +from bionc import JointType + +from bionc.bionc_numpy import ( + BiomechanicalModel, + NaturalSegment, + NaturalMarker, +) + + +def create_knee_model() -> BiomechanicalModel: + """ + This function creates a biomechanical model of a knee with a parallel mechanism. + + Data from + ---------- + Generic model of the right knee + J.D. Feikes et al. J Biomech 36 (2003) 125-129 + In tibia SCS = femur SCS at full extension + Origin at midpoint between medial and lateral centers + X = -X ISB, Y = Z ISB, Z = Y ISB + + """ + model = BiomechanicalModel() + # fill the biomechanical model with the segment + model["THIGH"] = NaturalSegment( + name="THIGH", + length=0.4, + mass=12, + center_of_mass=np.array([0, -0.1, 0]), # in segment coordinates system + inertia=np.array([[0.1, 0, 0], [0, 0.1, 0], [0, 0, 0.1]]), # in segment coordinates system + ) + + # added markers + model["THIGH"].add_natural_marker( + NaturalMarker( + name="hip_center", + parent_name="THIGH", + position=np.array([0, 0, 0]), + is_technical=True, + is_anatomical=False, + ) + ) + model["THIGH"].add_natural_marker( + NaturalMarker( + name="condyle_center", + parent_name="THIGH", + position=np.array([0, -1, 0]), + is_technical=True, + is_anatomical=False, + ) + ) + + model["THIGH"].add_natural_marker_from_segment_coordinates( + name="medial_centre_femur", + location=np.array([-5.0, 0.6, -22.3]) / 1000, + is_distal_location=True, + is_technical=True, + is_anatomical=True, + ) + model["THIGH"].add_natural_marker_from_segment_coordinates( + name="lateral_centre_femur", + location=np.array([5.0, -0.6, 22.3]) / 1000, + is_distal_location=True, + is_technical=True, + is_anatomical=True, + ) + model["THIGH"].add_natural_marker_from_segment_coordinates( + name="ACL_femur", + location=np.array([-27.8, 12.7, 5.0]) / 1000, + is_distal_location=True, + is_technical=False, + is_anatomical=True, + ) + model["THIGH"].add_natural_marker_from_segment_coordinates( + name="PCL_femur", + location=np.array([-20.6, -4.3, -15.7]) / 1000, + is_distal_location=True, + is_technical=False, + is_anatomical=True, + ) + model["THIGH"].add_natural_marker_from_segment_coordinates( + name="MCL_femur", + location=np.array([-9.7, 10.2, -42.3]) / 1000, + is_distal_location=True, + is_technical=False, + is_anatomical=True, + ) + + model["SHANK"] = NaturalSegment( + name="SHANK", + length=0.4, + mass=8, + center_of_mass=np.array([0, -0.1, 0]), # in segment coordinates system + inertia=np.array([[0.1, 0, 0], [0, 0.1, 0], [0, 0, 0.1]]), # in segment coordinates system + ) + + model["SHANK"].add_natural_marker( + NaturalMarker( + name="condyle_center", + parent_name="SHANK", + position=np.array([0, 0, 0]), + is_technical=True, + is_anatomical=False, + ) + ) + model["SHANK"].add_natural_marker( + NaturalMarker( + name="ankle_center", + parent_name="SHANK", + position=np.array([0, -1, 0]), + is_technical=True, + is_anatomical=False, + ) + ) + + model["SHANK"].add_natural_marker( + NaturalMarker( + name="ankle_center", + parent_name="SHANK", + position=np.array([0, -1, 0]), + is_technical=True, + is_anatomical=False, + ) + ) + model["SHANK"].add_natural_marker_from_segment_coordinates( + name="medial_contact_shank", + location=np.array([-0.005, -0.0202, -0.0223]), + is_technical=False, + is_anatomical=True, + ) + model["SHANK"].add_natural_marker_from_segment_coordinates( + name="lateral_contact_shank", + location=np.array([0.005, -0.0202, 0.0223]), + is_technical=False, + is_anatomical=True, + ) + model["SHANK"].add_natural_marker_from_segment_coordinates( + name="ACL_shank", + location=np.array([-0.0, -0.0152, 0.0]), + is_technical=False, + is_anatomical=True, + ) + model["SHANK"].add_natural_marker_from_segment_coordinates( + name="PCL_shank", + location=np.array([-0.0303, -0.0237, -0.0024]), + is_technical=False, + is_anatomical=True, + ) + model["SHANK"].add_natural_marker_from_segment_coordinates( + name="MCL_shank", + location=np.array([0.0012, -0.0672, -0.0036]), + is_technical=False, + is_anatomical=True, + ) + model["SHANK"].add_natural_marker_from_segment_coordinates( + name="LCL_shank", + location=np.array([0.0242639, -0.0479992, 0.0371213]), + is_technical=False, + is_anatomical=True, + ) + model["SHANK"].add_natural_vector_from_segment_coordinates( + name="medial_normal_shank", + direction=np.array([0, 1, 0]), + normalize=True, + ) + model["SHANK"].add_natural_vector_from_segment_coordinates( + name="lateral_normal_shank", + direction=np.array([0, 1, 0]), + normalize=True, + ) + + model._add_joint( + dict( + name="HIP", + joint_type=JointType.GROUND_SPHERICAL, + parent="GROUND", + child="THIGH", + ground_application_point=np.array([0, 0, 0]), + ) + ) + + # model._add_joint( + # dict( + # name="SPHERICAL_KNEE", + # joint_type=JointType.SPHERICAL, + # parent="THIGH", + # child="SHANK", + # ) + # ) + + # Parallel Mechanism joint definition + model._add_joint( + dict( + name="medial_knee", + joint_type=JointType.SPHERE_ON_PLANE, + parent="THIGH", + child="SHANK", + sphere_radius=np.linalg.norm(np.array([-5.0, -20.2, -22.3]) / 1000 - np.array([-5.0, 0.6, -22.3]) / 1000), + sphere_center="medial_centre_femur", + plane_normal="medial_normal_shank", + plane_point="medial_contact_shank", + ) + ) + model._add_joint( + dict( + name="lateral_knee", + joint_type=JointType.SPHERE_ON_PLANE, + parent="THIGH", + child="SHANK", + sphere_radius=np.linalg.norm(np.array([5.0, -20.2, 22.3]) / 1000 - np.array([5.0, -0.6, 22.3]) / 1000), + sphere_center="lateral_centre_femur", + plane_normal="lateral_normal_shank", + plane_point="lateral_contact_shank", + ) + ) + model._add_joint( + dict( + name="ACL", + joint_type=JointType.CONSTANT_LENGTH, + parent="THIGH", + child="SHANK", + length=np.linalg.norm(np.array([-0.0, -15.2, 0.0]) / 1000 - np.array([-27.8, 12.7, 5.0]) / 1000), + parent_point="ACL_femur", + child_point="ACL_shank", + ) + ) + model._add_joint( + dict( + name="PCL", + joint_type=JointType.CONSTANT_LENGTH, + parent="THIGH", + child="SHANK", + length=np.linalg.norm(np.array([-20.6, -4.3, -15.7]) / 1000 - np.array([-30.3, -23.7, -2.4]) / 1000), + parent_point="PCL_femur", + child_point="PCL_shank", + ) + ) + model._add_joint( + dict( + name="MCL", + joint_type=JointType.CONSTANT_LENGTH, + parent="THIGH", + child="SHANK", + length=np.linalg.norm(np.array([-9.7, 10.2, -42.3]) / 1000 - np.array([1.2, -67.2, -3.6]) / 1000), + parent_point="MCL_femur", + child_point="MCL_shank", + ) + ) + + return model diff --git a/examples/knee_parallel_mechanism/knee.py b/examples/knee_parallel_mechanism/knee_sancisi.py similarity index 77% rename from examples/knee_parallel_mechanism/knee.py rename to examples/knee_parallel_mechanism/knee_sancisi.py index f796c32f..fd5550ae 100644 --- a/examples/knee_parallel_mechanism/knee.py +++ b/examples/knee_parallel_mechanism/knee_sancisi.py @@ -2,10 +2,6 @@ from bionc import JointType -# from bionc.bionc_casadi import ( -# BiomechanicalModel, -# NaturalSegment, -# ) from bionc.bionc_numpy import ( BiomechanicalModel, NaturalSegment, @@ -197,75 +193,75 @@ def create_knee_model() -> BiomechanicalModel: ) ) - model._add_joint( - dict( - name="SPHERICAL_KNEE", - joint_type=JointType.SPHERICAL, - parent="THIGH", - child="SHANK", - ) - ) - - # Parallel Mechanism joint definition - # model._add_joint( - # dict( - # name="medial_knee", - # joint_type=JointType.SPHERE_ON_PLANE, - # parent="THIGH", - # child="SHANK", - # sphere_radius=0.03232, - # sphere_center="medial_centre_femur", - # plane_normal="medial_normal_shank", - # plane_point="medial_contact_shank", - # ) - # ) - # model._add_joint( - # dict( - # name="lateral_knee", - # joint_type=JointType.SPHERE_ON_PLANE, - # parent="THIGH", - # child="SHANK", - # sphere_radius=0.02834, - # sphere_center="lateral_centre_femur", - # plane_normal="lateral_normal_shank", - # plane_point="lateral_contact_shank", - # ) - # ) - # model._add_joint( - # dict( - # name="ACL", - # joint_type=JointType.CONSTANT_LENGTH, - # parent="THIGH", - # child="SHANK", - # # length=0.04053, # from article - # length=0.04356820117764876, # from data - # parent_point="ACL_femur", - # child_point="ACL_shank", - # ) - # ) - # model._add_joint( - # dict( - # name="PCL", - # joint_type=JointType.CONSTANT_LENGTH, - # parent="THIGH", - # child="SHANK", - # # length=0.04326, # from article - # length=0.042833461279363376, # from data - # parent_point="PCL_femur", - # child_point="PCL_shank", - # ) - # ) # model._add_joint( # dict( - # name="MCL", - # joint_type=JointType.CONSTANT_LENGTH, + # name="SPHERICAL_KNEE", + # joint_type=JointType.SPHERICAL, # parent="THIGH", # child="SHANK", - # # length=0.12970, # from article - # length=0.10828123262317323, - # parent_point="MCL_femur", - # child_point="MCL_shank", # ) # ) + # Parallel Mechanism joint definition + model._add_joint( + dict( + name="medial_knee", + joint_type=JointType.SPHERE_ON_PLANE, + parent="THIGH", + child="SHANK", + sphere_radius=0.03232, + sphere_center="medial_centre_femur", + plane_normal="medial_normal_shank", + plane_point="medial_contact_shank", + ) + ) + model._add_joint( + dict( + name="lateral_knee", + joint_type=JointType.SPHERE_ON_PLANE, + parent="THIGH", + child="SHANK", + sphere_radius=0.02834, + sphere_center="lateral_centre_femur", + plane_normal="lateral_normal_shank", + plane_point="lateral_contact_shank", + ) + ) + model._add_joint( + dict( + name="ACL", + joint_type=JointType.CONSTANT_LENGTH, + parent="THIGH", + child="SHANK", + # length=0.04053, # from article + length=0.04356820117764876, # from data + parent_point="ACL_femur", + child_point="ACL_shank", + ) + ) + model._add_joint( + dict( + name="PCL", + joint_type=JointType.CONSTANT_LENGTH, + parent="THIGH", + child="SHANK", + # length=0.04326, # from article + length=0.042833461279363376, # from data + parent_point="PCL_femur", + child_point="PCL_shank", + ) + ) + model._add_joint( + dict( + name="MCL", + joint_type=JointType.CONSTANT_LENGTH, + parent="THIGH", + child="SHANK", + # length=0.12970, # from article + length=0.10828123262317323, + parent_point="MCL_femur", + child_point="MCL_shank", + ) + ) + return model diff --git a/examples/knee_parallel_mechanism/simulation_feikes.py b/examples/knee_parallel_mechanism/simulation_feikes.py new file mode 100644 index 00000000..d3946534 --- /dev/null +++ b/examples/knee_parallel_mechanism/simulation_feikes.py @@ -0,0 +1,61 @@ +import numpy as np +from bionc import NaturalCoordinates, SegmentNaturalCoordinates, Viz, SegmentNaturalVelocities, NaturalVelocities + +from knee_feikes import create_knee_model +from utils import forward_integration, post_computations + +model = create_knee_model() + +Q0 = SegmentNaturalCoordinates(np.array([1, 0, 0, 0, 0.0, 0, 0, 0.4, 0, 0, 0, 1])) +Q1 = SegmentNaturalCoordinates(np.array([1, 0, 0, 0, 0.4, 0, 0, 0.8, 0, 0, 0, 1])) + +Q = NaturalCoordinates.from_qi((Q0, Q1)) +print(model.rigid_body_constraints(NaturalCoordinates(Q))) +print(model.joint_constraints(NaturalCoordinates(Q))) + +viz = Viz(model, size_model_marker=0.004, show_frames=True, show_ground_frame=True, size_xp_marker=0.005) +viz.animate(Q) + +# simulation in forward dynamics +tuple_of_Qdot = [ + SegmentNaturalVelocities.from_components(udot=[0, 0, 0], rpdot=[0, 0, 0], rddot=[0, 0, 0], wdot=[0, 0, 0]) + for i in range(0, model.nb_segments) +] +Qdot = NaturalVelocities.from_qdoti(tuple(tuple_of_Qdot)) + +# actual simulation +t_final = 0.5 # seconds +time_steps, all_states, dynamics = forward_integration( + model=model, + Q_init=Q, + Qdot_init=Qdot, + t_final=t_final, + steps_per_second=1000, +) + +defects, defects_dot, joint_defects, all_lambdas = post_computations( + model=model, + time_steps=time_steps, + all_states=all_states, + dynamics=dynamics, +) + +# plot results +import matplotlib.pyplot as plt + +plt.figure() +for i in range(0, model.nb_rigid_body_constraints): + plt.plot(time_steps, defects[i, :], marker="o", label=f"defects {i}") +plt.title("Rigid body constraints") +plt.legend() + +plt.figure() +for i in range(0, model.nb_joint_constraints): + plt.plot(time_steps, joint_defects[i, :], marker="o", label=f"joint_defects {i}") +plt.title("Joint constraints") +plt.legend() +plt.show() + +# animation +viz = Viz(model) +viz.animate(NaturalCoordinates(all_states[: (12 * model.nb_segments), :]), None) diff --git a/examples/knee_parallel_mechanism/simulation.py b/examples/knee_parallel_mechanism/simulation_sancisi.py similarity index 94% rename from examples/knee_parallel_mechanism/simulation.py rename to examples/knee_parallel_mechanism/simulation_sancisi.py index 8af72fe4..29837745 100644 --- a/examples/knee_parallel_mechanism/simulation.py +++ b/examples/knee_parallel_mechanism/simulation_sancisi.py @@ -2,7 +2,7 @@ from bionc import NaturalCoordinates, SegmentNaturalCoordinates, Viz, SegmentNaturalVelocities, NaturalVelocities from bionc.bionc_casadi.utils import to_numeric_MX -from knee import create_knee_model +from knee_sancisi import create_knee_model from utils import forward_integration, post_computations model = create_knee_model() @@ -18,8 +18,8 @@ print(model.rigid_body_constraints(NaturalCoordinates(Q))) print(model.joint_constraints(NaturalCoordinates(Q))) -# viz = Viz(model, size_model_marker=0.004, show_frames=True, show_ground_frame=False, size_xp_marker=0.005) -# viz.animate(Q) +viz = Viz(model, size_model_marker=0.004, show_frames=True, show_ground_frame=False, size_xp_marker=0.005) +viz.animate(Q) marker_names = model.marker_names @@ -64,7 +64,7 @@ Qdot = NaturalVelocities.from_qdoti(tuple(tuple_of_Qdot)) # actual simulation -t_final = 1 # seconds +t_final = 2 # seconds time_steps, all_states, dynamics = forward_integration( model=model, Q_init=NaturalCoordinates(Q_opt), diff --git a/examples/knee_parallel_mechanism/utils.py b/examples/knee_parallel_mechanism/utils.py index 498e7c62..586d142c 100644 --- a/examples/knee_parallel_mechanism/utils.py +++ b/examples/knee_parallel_mechanism/utils.py @@ -71,12 +71,19 @@ def dynamics(t, states): return np.concatenate((states[idx_velocities], qddot.to_array()), axis=0), lambdas # Solve the Initial Value Problem (IVP) for each time step - all_states = RK4(t=time_steps, f=lambda t, states: dynamics(t, states)[0], y0=states_0) + normalize_idx = model.normalized_coordinates + all_states = RK4(t=time_steps, f=lambda t, states: dynamics(t, states)[0], y0=states_0, normalize_idx=normalize_idx) return time_steps, all_states, dynamics -def RK4(t: np.ndarray, f, y0: np.ndarray, args=()) -> np.ndarray: +def RK4( + t: np.ndarray, + f, + y0: np.ndarray, + normalize_idx: tuple[tuple[int, ...]] = None, + args=(), +) -> np.ndarray: """ Runge-Kutta 4th order method @@ -88,6 +95,8 @@ def RK4(t: np.ndarray, f, y0: np.ndarray, args=()) -> np.ndarray: function to be integrated in the form f(t, y, *args) y0 : np.ndarray initial conditions of states + normalize_idx : tuple(tuple) + indices of states to be normalized together Returns ------- @@ -99,7 +108,6 @@ def RK4(t: np.ndarray, f, y0: np.ndarray, args=()) -> np.ndarray: y = np.zeros((len(y0), n)) y[:, 0] = y0 for i in range(n - 1): - print(f"frame {i}") h = t[i + 1] - t[i] yi = np.squeeze(y[:, i]) k1 = f(t[i], yi, *args) @@ -107,6 +115,10 @@ def RK4(t: np.ndarray, f, y0: np.ndarray, args=()) -> np.ndarray: k3 = f(t[i] + h / 2.0, yi + k2 * h / 2.0, *args) k4 = f(t[i] + h, yi + k3 * h, *args) y[:, i + 1] = yi + (h / 6.0) * (k1 + 2 * k2 + 2 * k3 + k4) + + if normalize_idx is not None: + for idx in normalize_idx: + y[idx, i + 1] = y[idx, i + 1] / np.linalg.norm(y[idx, i + 1]) return y diff --git a/examples/models/lower_limb.nc b/examples/models/lower_limb.nc index b1612220..8bb6546c 100644 Binary files a/examples/models/lower_limb.nc and b/examples/models/lower_limb.nc differ diff --git a/examples/play_with_joints/constant_length.py b/examples/play_with_joints/constant_length.py new file mode 100644 index 00000000..cc4e27a8 --- /dev/null +++ b/examples/play_with_joints/constant_length.py @@ -0,0 +1,187 @@ +from bionc import ( + BiomechanicalModel, + SegmentNaturalCoordinates, + SegmentNaturalVelocities, + NaturalCoordinates, + NaturalVelocities, + NaturalSegment, + JointType, + CartesianAxis, + NaturalAxis, + NaturalMarker, + Viz, +) + +import numpy as np + +from utils import forward_integration, post_computations + + +def build_two_link_segment(): + # Let's create a model + model = BiomechanicalModel() + # number of segments + # fill the biomechanical model with the segment + for i in range(2): + name = f"segment_{i}" + model[name] = NaturalSegment( + name=name, + alpha=np.pi / 2, # setting alpha, beta, gamma to pi/2 creates an orthogonal coordinate system + beta=np.pi / 2, + gamma=np.pi / 2, + length=0.8, + mass=1, + center_of_mass=np.array([0, -0.5, 0]), # in segment coordinates system + inertia=np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]), # in segment coordinates system + ) + + model["segment_0"].add_natural_marker( + NaturalMarker( + name="point_A", + parent_name="segment_0", + position=np.array([0, -1, 0]), + is_technical=True, + is_anatomical=False, + ) + ) + + model["segment_1"].add_natural_marker( + NaturalMarker( + name="point_B", + parent_name="segment_1", + position=np.array([0, 0, 0]), + is_technical=True, + is_anatomical=False, + ) + ) + + # add a revolute joint (still experimental) + # if you want to add a revolute joint, + # you need to ensure that x is always orthogonal to u and v + model._add_joint( + dict( + name="hinge_0", + joint_type=JointType.GROUND_REVOLUTE, + parent="GROUND", + child="segment_0", + parent_axis=[CartesianAxis.X, CartesianAxis.X], + child_axis=[NaturalAxis.V, NaturalAxis.W], # meaning we pivot around the cartesian x-axis + theta=[np.pi / 2, np.pi / 2], + ) + ) + model._add_joint( + dict( + name=f"constant_length", + joint_type=JointType.CONSTANT_LENGTH, + parent=f"segment_0", + child=f"segment_1", + length=0.2, + parent_point="point_A", + child_point="point_B", + ) + ) + return model + + +def main(initial_pose: str = "hanged"): + model = build_two_link_segment() + + if initial_pose == "hanged": + Q0 = SegmentNaturalCoordinates([1, 0, 0, 0, 0, 0, 0, 0, -0.8, 0, -1, 0]) + Q1 = SegmentNaturalCoordinates([1, 0, 0, 0, 0, -1, 0, 0, -1.8, 0, -1, 0]) + elif initial_pose == "ready_to_swing": + Q0 = SegmentNaturalCoordinates([1, 0, 0, 0, 0, 0, 0, 0, -0.8, 0, -1, 0]) + Q1 = SegmentNaturalCoordinates( + [ + 1, + 0, + 0, + 0.2 * np.cos(np.pi / 4), + 0, + -(0.8 + 0.2 * np.sin(np.pi / 4)), + 0.2 * np.cos(np.pi / 4), + 0, + -(0.8 + 0.2 * np.sin(np.pi / 4)) - 0.8, + 0, + -1, + 0, + ] + ) + Q = NaturalCoordinates.from_qi((Q0, Q1)) + + # # numpy version + # Q_test = NaturalCoordinates(np.arange(24)) + # jacobian_numpy = model.joint_constraints_jacobian(Q_test) + # + # model_mx = model.to_mx() + # sym = NaturalCoordinatesMX.sym(2) + # j_constraints_sym = model_mx.joint_constraints(sym) + # # jacobian + # j_jacobian_sym = jacobian(j_constraints_sym, sym) + # j_jacobian_func = Function("j_jacobian_func", [sym], [j_jacobian_sym]) + # + # jacobian_mx = j_jacobian_func(np.arange(24)).toarray() + + print("--------------------") + print("INITIAL CONSTRAINTS") + print(model.rigid_body_constraints(Q)) + print(model.joint_constraints(Q)) + print("--------------------") + + # uncomment to see the initial position + # viz = Viz(model) + # viz.animate(Q) + + tuple_of_Qdot = [ + SegmentNaturalVelocities.from_components(udot=[0, 0, 0], rpdot=[0, 0, 0], rddot=[0, 0, 0], wdot=[0, 0, 0]) + for i in range(0, model.nb_segments) + ] + Qdot = NaturalVelocities.from_qdoti(tuple(tuple_of_Qdot)) + + # actual simulation + t_final = 5 # seconds + time_steps, all_states, dynamics = forward_integration( + model=model, + Q_init=Q, + Qdot_init=Qdot, + t_final=t_final, + steps_per_second=100, + ) + + defects, defects_dot, joint_defects, all_lambdas = post_computations( + model=model, + time_steps=time_steps, + all_states=all_states, + dynamics=dynamics, + ) + + # plot results + import matplotlib.pyplot as plt + + plt.figure() + for i in range(0, model.nb_rigid_body_constraints): + plt.plot(time_steps, defects[i, :], marker="o", label=f"defects {i}") + plt.title("Rigid body constraints") + plt.legend() + + plt.figure() + for i in range(0, model.nb_joint_constraints): + plt.plot(time_steps, joint_defects[i, :], marker="o", label=f"joint_defects {i}") + plt.title("Joint constraints") + plt.legend() + + plt.figure() + for i in range(0, model.nb_holonomic_constraints): + plt.plot(time_steps, all_lambdas[i, :], marker="o", label=f"lambda {i}") + plt.title("Lagrange multipliers") + plt.legend() + plt.show() + + # animation + viz = Viz(model) + viz.animate(NaturalCoordinates(all_states[: (12 * model.nb_segments), :]), None) + + +if __name__ == "__main__": + # main("hanged") + main("ready_to_swing") diff --git a/examples/play_with_joints/two_constant_length.py b/examples/play_with_joints/two_constant_length.py new file mode 100644 index 00000000..05a4c023 --- /dev/null +++ b/examples/play_with_joints/two_constant_length.py @@ -0,0 +1,207 @@ +from bionc import ( + BiomechanicalModel, + SegmentNaturalCoordinates, + SegmentNaturalVelocities, + NaturalCoordinates, + NaturalVelocities, + NaturalSegment, + JointType, + CartesianAxis, + NaturalAxis, + NaturalMarker, + Viz, +) + +import numpy as np + +from utils import forward_integration, post_computations + + +def build_two_link_segment(): + # Let's create a model + model = BiomechanicalModel() + # number of segments + # fill the biomechanical model with the segment + for i in range(2): + name = f"segment_{i}" + model[name] = NaturalSegment( + name=name, + alpha=np.pi / 2, # setting alpha, beta, gamma to pi/2 creates an orthogonal coordinate system + beta=np.pi / 2, + gamma=np.pi / 2, + length=0.8, + mass=1, + center_of_mass=np.array([0, -0.5, 0]), # in segment coordinates system + inertia=np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]), # in segment coordinates system + ) + + model["segment_0"].add_natural_marker( + NaturalMarker( + name="point_A", + parent_name="segment_0", + position=np.array([0, -1, 0]), + is_technical=True, + is_anatomical=False, + ) + ) + + model["segment_0"].add_natural_marker( + NaturalMarker( + name="point_AA", + parent_name="segment_0", + position=np.array([0, -1, 0.05]), + is_technical=True, + is_anatomical=False, + ) + ) + + model["segment_1"].add_natural_marker( + NaturalMarker( + name="point_B", + parent_name="segment_1", + position=np.array([0, 0, 0]), + is_technical=True, + is_anatomical=False, + ) + ) + + model["segment_1"].add_natural_marker( + NaturalMarker( + name="point_BB", + parent_name="segment_1", + position=np.array([0, 0, 0.05]), + is_technical=True, + is_anatomical=False, + ) + ) + + # add a revolute joint (still experimental) + # if you want to add a revolute joint, + # you need to ensure that x is always orthogonal to u and v + model._add_joint( + dict( + name="hinge_0", + joint_type=JointType.GROUND_REVOLUTE, + parent="GROUND", + child="segment_0", + parent_axis=[CartesianAxis.X, CartesianAxis.X], + child_axis=[NaturalAxis.V, NaturalAxis.W], # meaning we pivot around the cartesian x-axis + theta=[np.pi / 2, np.pi / 2], + ) + ) + model._add_joint( + dict( + name=f"constant_length", + joint_type=JointType.CONSTANT_LENGTH, + parent=f"segment_0", + child=f"segment_1", + length=0.2, + parent_point="point_A", + child_point="point_B", + ) + ) + + model._add_joint( + dict( + name=f"constant_length_2", + joint_type=JointType.CONSTANT_LENGTH, + parent=f"segment_0", + child=f"segment_1", + length=0.2, + parent_point="point_AA", + child_point="point_BB", + ) + ) + + return model + + +def main(initial_pose: str = "hanged"): + model = build_two_link_segment() + + if initial_pose == "hanged": + Q0 = SegmentNaturalCoordinates([1, 0, 0, 0, 0, 0, 0, 0, -0.8, 0, -1, 0]) + Q1 = SegmentNaturalCoordinates([1, 0, 0, 0, 0, -1, 0, 0, -1.8, 0, -1, 0]) + elif initial_pose == "ready_to_swing": + Q0 = SegmentNaturalCoordinates([1, 0, 0, 0, 0, 0, 0, 0, -0.8, 0, -1, 0]) + Q1 = SegmentNaturalCoordinates( + [ + 1, + 0, + 0, + 0.2 * np.cos(np.pi / 4), + 0, + -(0.8 + 0.2 * np.sin(np.pi / 4)), + 0.2 * np.cos(np.pi / 4), + 0, + -(0.8 + 0.2 * np.sin(np.pi / 4)) - 0.8, + 0, + -1, + 0, + ] + ) + Q = NaturalCoordinates.from_qi((Q0, Q1)) + + print("--------------------") + print("INITIAL CONSTRAINTS") + print(model.rigid_body_constraints(Q)) + print(model.joint_constraints(Q)) + print("--------------------") + + # uncomment to see the initial position + # viz = Viz(model) + # viz.animate(Q) + + tuple_of_Qdot = [ + SegmentNaturalVelocities.from_components(udot=[0, 0, 0], rpdot=[0, 0, 0], rddot=[0, 0, 0], wdot=[0, 0, 0]) + for i in range(0, model.nb_segments) + ] + Qdot = NaturalVelocities.from_qdoti(tuple(tuple_of_Qdot)) + + # actual simulation + t_final = 5 # seconds + time_steps, all_states, dynamics = forward_integration( + model=model, + Q_init=Q, + Qdot_init=Qdot, + t_final=t_final, + steps_per_second=100, + ) + + defects, defects_dot, joint_defects, all_lambdas = post_computations( + model=model, + time_steps=time_steps, + all_states=all_states, + dynamics=dynamics, + ) + + # plot results + import matplotlib.pyplot as plt + + plt.figure() + for i in range(0, model.nb_rigid_body_constraints): + plt.plot(time_steps, defects[i, :], marker="o", label=f"defects {i}") + plt.title("Rigid body constraints") + plt.legend() + + plt.figure() + for i in range(0, model.nb_joint_constraints): + plt.plot(time_steps, joint_defects[i, :], marker="o", label=f"joint_defects {i}") + plt.title("Joint constraints") + plt.legend() + + plt.figure() + for i in range(0, model.nb_holonomic_constraints): + plt.plot(time_steps, all_lambdas[i, :], marker="o", label=f"lambda {i}") + plt.title("Lagrange multipliers") + plt.legend() + plt.show() + + # animation + viz = Viz(model) + viz.animate(NaturalCoordinates(all_states[: (12 * model.nb_segments), :]), None) + + +if __name__ == "__main__": + main("hanged") + # main("ready_to_swing") diff --git a/examples/play_with_joints/utils.py b/examples/play_with_joints/utils.py new file mode 100644 index 00000000..586d142c --- /dev/null +++ b/examples/play_with_joints/utils.py @@ -0,0 +1,181 @@ +from bionc import ( + BiomechanicalModel, + NaturalCoordinates, + NaturalVelocities, +) +import numpy as np + + +def forward_integration( + model: BiomechanicalModel, + Q_init: NaturalCoordinates, + Qdot_init: NaturalVelocities, + t_final: float = 2, + steps_per_second: int = 50, +): + """ + This function simulates the dynamics of a natural segment falling from 0m during 2s + + Parameters + ---------- + model : BiomechanicalModel + The model to be simulated + Q_init : SegmentNaturalCoordinates + The initial natural coordinates of the segment + Qdot_init : SegmentNaturalVelocities + The initial natural velocities of the segment + t_final : float, optional + The final time of the simulation, by default 2 + steps_per_second : int, optional + The number of steps per second, by default 50 + + Returns + ------- + tuple: + time_steps : np.ndarray + The time steps of the simulation + all_states : np.ndarray + The states of the system at each time step X = [Q, Qdot] + dynamics : Callable + The dynamics of the system, f(t, X) = [Xdot, lambdas] + """ + + print("Evaluate Rigid Body Constraints:") + print(model.rigid_body_constraints(Q_init)) + print("Evaluate Rigid Body Constraints Jacobian Derivative:") + print(model.rigid_body_constraint_jacobian_derivative(Qdot_init)) + + if (model.rigid_body_constraints(Q_init) > 1e-4).any(): + print(model.rigid_body_constraints(Q_init)) + raise ValueError( + "The segment natural coordinates don't satisfy the rigid body constraint, at initial conditions." + ) + + t_final = t_final # [s] + steps_per_second = steps_per_second + time_steps = np.linspace(0, t_final, int(steps_per_second * t_final + 1)) + + # initial conditions, x0 = [Qi, Qidot] + states_0 = np.concatenate((Q_init.to_array(), Qdot_init.to_array()), axis=0) + + # Create the forward dynamics function Callable (f(t, x) -> xdot) + def dynamics(t, states): + idx_coordinates = slice(0, model.nb_Q) + idx_velocities = slice(model.nb_Q, model.nb_Q + model.nb_Qdot) + + qddot, lambdas = model.forward_dynamics( + NaturalCoordinates(states[idx_coordinates]), + NaturalVelocities(states[idx_velocities]), + # stabilization=dict(alpha=0.5, beta=0.5), + ) + return np.concatenate((states[idx_velocities], qddot.to_array()), axis=0), lambdas + + # Solve the Initial Value Problem (IVP) for each time step + normalize_idx = model.normalized_coordinates + all_states = RK4(t=time_steps, f=lambda t, states: dynamics(t, states)[0], y0=states_0, normalize_idx=normalize_idx) + + return time_steps, all_states, dynamics + + +def RK4( + t: np.ndarray, + f, + y0: np.ndarray, + normalize_idx: tuple[tuple[int, ...]] = None, + args=(), +) -> np.ndarray: + """ + Runge-Kutta 4th order method + + Parameters + ---------- + t : array_like + time steps + f : Callable + function to be integrated in the form f(t, y, *args) + y0 : np.ndarray + initial conditions of states + normalize_idx : tuple(tuple) + indices of states to be normalized together + + Returns + ------- + y : array_like + states for each time step + + """ + n = len(t) + y = np.zeros((len(y0), n)) + y[:, 0] = y0 + for i in range(n - 1): + h = t[i + 1] - t[i] + yi = np.squeeze(y[:, i]) + k1 = f(t[i], yi, *args) + k2 = f(t[i] + h / 2.0, yi + k1 * h / 2.0, *args) + k3 = f(t[i] + h / 2.0, yi + k2 * h / 2.0, *args) + k4 = f(t[i] + h, yi + k3 * h, *args) + y[:, i + 1] = yi + (h / 6.0) * (k1 + 2 * k2 + 2 * k3 + k4) + + if normalize_idx is not None: + for idx in normalize_idx: + y[idx, i + 1] = y[idx, i + 1] / np.linalg.norm(y[idx, i + 1]) + return y + + +def post_computations(model: BiomechanicalModel, time_steps: np.ndarray, all_states: np.ndarray, dynamics): + """ + This function computes: + - the rigid body constraint error + - the rigid body constraint jacobian derivative error + - the joint constraint error + - the lagrange multipliers of the rigid body constraint + + Parameters + ---------- + model : NaturalSegment + The segment to be simulated + time_steps : np.ndarray + The time steps of the simulation + all_states : np.ndarray + The states of the system at each time step X = [Q, Qdot] + dynamics : Callable + The dynamics of the system, f(t, X) = [Xdot, lambdas] + + Returns + ------- + tuple: + rigid_body_constraint_error : np.ndarray + The rigid body constraint error at each time step + rigid_body_constraint_jacobian_derivative_error : np.ndarray + The rigid body constraint jacobian derivative error at each time step + joint_constraints: np.ndarray + The joint constraints at each time step + lambdas : np.ndarray + The lagrange multipliers of the rigid body constraint at each time step + """ + idx_coordinates = slice(0, model.nb_Q) + idx_velocities = slice(model.nb_Q, model.nb_Q + model.nb_Qdot) + + # compute the quantities of interest after the integration + all_lambdas = np.zeros((model.nb_holonomic_constraints, len(time_steps))) + defects = np.zeros((model.nb_rigid_body_constraints, len(time_steps))) + defects_dot = np.zeros((model.nb_rigid_body_constraints, len(time_steps))) + joint_defects = np.zeros((model.nb_joint_constraints, len(time_steps))) + joint_defects_dot = np.zeros((model.nb_joint_constraints, len(time_steps))) + + for i in range(len(time_steps)): + defects[:, i] = model.rigid_body_constraints(NaturalCoordinates(all_states[idx_coordinates, i])) + defects_dot[:, i] = model.rigid_body_constraints_derivative( + NaturalCoordinates(all_states[idx_coordinates, i]), NaturalVelocities(all_states[idx_velocities, i]) + ) + + joint_defects[:, i] = model.joint_constraints(NaturalCoordinates(all_states[idx_coordinates, i])) + # todo : to be implemented + # joint_defects_dot = model.joint_constraints_derivative( + # NaturalCoordinates(all_states[idx_coordinates, i]), + # NaturalVelocities(all_states[idx_velocities, i])) + # ) + + all_lambdas[:, i : i + 1] = dynamics(time_steps[i], all_states[:, i])[1] + + return defects, defects_dot, joint_defects, all_lambdas diff --git a/pendulum_with_force.nmod b/pendulum_with_force.nmod new file mode 100644 index 00000000..f91c1971 Binary files /dev/null and b/pendulum_with_force.nmod differ diff --git a/tests/test_biomech_model.py b/tests/test_biomech_model.py index 16a68920..14861ea6 100644 --- a/tests/test_biomech_model.py +++ b/tests/test_biomech_model.py @@ -6960,6 +6960,14 @@ def test_biomech_model(bionc_type): "RTAR", ] assert natural_model.marker_names_technical == marker_names_technical + segment_names = [ + "PELVIS", + "THIGH", + "SHANK", + "FOOT", + ] + + assert natural_model.segment_names == segment_names filename = "natural_model.nc" if bionc_type == "numpy": diff --git a/tests/test_external_force.py b/tests/test_external_force.py new file mode 100644 index 00000000..aa007aa3 --- /dev/null +++ b/tests/test_external_force.py @@ -0,0 +1,233 @@ +import pytest +from .utils import TestUtils +import numpy as np + + +@pytest.mark.parametrize( + "bionc_type", + [ + "numpy", + ], +) +@pytest.mark.parametrize( + "external_force_tuple", + [ + (np.array([0, 0, 1 * 9.81]), np.array([0, 0, 0]), np.array([0, -0.5, 0])), + (np.array([0, 0, 0]), np.array([-1 * 9.81 * 0.50, 0, 0]), np.array([0, 0, 0])), + ], +) +def test_external_force(bionc_type, external_force_tuple): + from bionc.bionc_numpy import ( + ExternalForceList, + ExternalForce, + SegmentNaturalCoordinates, + NaturalCoordinates, + SegmentNaturalVelocities, + NaturalVelocities, + ) + + bionc = TestUtils.bionc_folder() + module = TestUtils.load_module(bionc + "/examples/forward_dynamics/pendulum_with_force.py") + + fext = ExternalForceList.empty_from_nb_segment(1) + force1 = ExternalForce.from_components( + force=external_force_tuple[0], + torque=external_force_tuple[1], + application_point_in_local=external_force_tuple[2], + ) + fext.add_external_force(external_force=force1, segment_index=0) + + _, _, all_states, _ = module.apply_force_and_drop_pendulum(t_final=1, external_forces=fext) + + # check that the pendulum is not moving + Qi = SegmentNaturalCoordinates.from_components(u=[1, 0, 0], rp=[0, 0, 0], rd=[0, -1, 0], w=[0, 0, 1]) + Q = NaturalCoordinates(Qi) + Qdoti = SegmentNaturalVelocities.from_components(udot=[0, 0, 0], rpdot=[0, 0, 0], rddot=[0, 0, 0], wdot=[0, 0, 0]) + Qdot = NaturalVelocities(Qdoti) + + np.testing.assert_allclose(all_states[:12, 0], Q.to_array(), atol=1e-6) + np.testing.assert_allclose(all_states[:12, -1], Q.to_array(), atol=1e-6) + + np.testing.assert_allclose(all_states[12:, 0], Qdot.to_array(), atol=1e-6) + np.testing.assert_allclose(all_states[12:, -1], Qdot.to_array(), atol=1e-6) + + +@pytest.mark.parametrize( + "bionc_type", + [ + "numpy", + "casadi", + ], +) +def test_external_force(bionc_type): + if bionc_type == "numpy": + from bionc.bionc_numpy import ( + ExternalForceList, + ExternalForce, + SegmentNaturalCoordinates, + NaturalCoordinates, + SegmentNaturalVelocities, + NaturalVelocities, + ) + else: + from bionc.bionc_casadi import ( + ExternalForceList, + ExternalForce, + SegmentNaturalCoordinates, + NaturalCoordinates, + SegmentNaturalVelocities, + NaturalVelocities, + ) + + force1 = ExternalForce.from_components( + force=np.array([0.01, 0.02, 0.03]), + torque=np.array([0.04, 0.05, 0.06]), + application_point_in_local=np.array([0.07, 0.08, 0.09]), + ) + force2 = ExternalForce.from_components( + force=np.array([0.11, 0.12, 0.13]), + torque=np.array([0.14, 0.15, 0.16]), + application_point_in_local=np.array([0.17, 0.18, 0.19]), + ) + + TestUtils.assert_equal(force1.torque, np.array([0.04, 0.05, 0.06])) + TestUtils.assert_equal(force1.force, np.array([0.01, 0.02, 0.03])) + TestUtils.assert_equal(force2.torque, np.array([0.14, 0.15, 0.16])) + TestUtils.assert_equal(force2.force, np.array([0.11, 0.12, 0.13])) + + fext = ExternalForceList.empty_from_nb_segment(3) + fext.add_external_force(external_force=force1, segment_index=0) + fext.add_external_force(external_force=force2, segment_index=2) + + # check that the pendulum is not moving + Q0 = SegmentNaturalCoordinates.from_components( + u=[1, 0, 0], + rp=[0, 0, 0], + rd=[0, -1, 0], + w=[0, 0, 1], + ) + Q1 = SegmentNaturalCoordinates(Q0 + 0.1) + Q2 = SegmentNaturalCoordinates(Q1 + 0.1) + Q = NaturalCoordinates.from_qi((Q0, Q1, Q2)) + + pseudo_interpolation_matrix = force2.compute_pseudo_interpolation_matrix(Q2) + natural_force = force2.to_natural_force(Q2) + natural_forces = fext.to_natural_external_forces(Q) + + TestUtils.assert_equal( + pseudo_interpolation_matrix, + np.array( + [ + [ + 0.0, + 0.14285714, + 0.0, + 0.17142857, + 0.17142857, + 1.02857143, + -0.17142857, + -0.17142857, + -1.02857143, + 0.0, + 0.0, + 0.0, + ], + [ + 0.0, + 0.14285714, + 0.0, + 0.02857143, + 0.02857143, + 0.17142857, + -0.02857143, + -0.02857143, + -0.17142857, + 0.85714286, + 0.14285714, + 0.14285714, + ], + [ + 0.0, + 0.85714286, + 0.0, + 0.02857143, + 0.02857143, + 0.17142857, + -0.02857143, + -0.02857143, + -0.17142857, + 0.0, + 0.0, + 0.0, + ], + ] + ), + expand=False, + ) + + TestUtils.assert_equal( + natural_force, + np.array( + [ + 0.0187, + 0.19897143, + 0.0221, + 0.16265714, + 0.17445714, + 0.35054286, + -0.05265714, + -0.05445714, + -0.22054286, + 0.14947143, + 0.04422857, + 0.04612857, + ] + ), + expand=False, + ) + + TestUtils.assert_equal( + natural_forces, + np.array( + [ + [0.0007], + [0.0614], + [0.0021], + [0.0108], + [0.0216], + [0.0724], + [-0.0008], + [-0.0016], + [-0.0424], + [0.0509], + [0.0018], + [0.0027], + [0.0], + [0.0], + [0.0], + [0.0], + [0.0], + [0.0], + [0.0], + [0.0], + [0.0], + [0.0], + [0.0], + [0.0], + [0.0187], + [0.19897143], + [0.0221], + [0.16265714], + [0.17445714], + [0.35054286], + [-0.05265714], + [-0.05445714], + [-0.22054286], + [0.14947143], + [0.04422857], + [0.04612857], + ] + ), + expand=False, + squeeze=False, + ) diff --git a/tests/test_joints.py b/tests/test_joints.py index b53f44d3..482e0a25 100644 --- a/tests/test_joints.py +++ b/tests/test_joints.py @@ -147,12 +147,8 @@ def test_joints(bionc_type, joint_type: JointType): joint = GroundJoint.Weld( name="Weld", child=bbox, - Q_child_ref=SegmentNaturalCoordinates.from_components( - u=[0.1, 0.2, 0.3], - rp=[0.21, 0.04, 0.053], - rd=[0.22, 0.041, 0.052], - w=[0.23, 0.042, 0.051], - ), + rp_child_ref=np.array([0.1, 0.2, 0.3]), + rd_child_ref=np.array([0.2, 0.04, 0.05]), index=0, ) else: @@ -350,18 +346,18 @@ def test_joints(bionc_type, joint_type: JointType): parent_jacobian_res = np.array( [ [ - 0.163589, - 0.056279, - 0.231968, - 1.963063, - 0.675351, - 2.783621, - -0.327177, - -0.112558, - -0.463937, - 0.490766, - 0.168838, - 0.695905, + -0.045226, + 0.014514, + 0.170561, + -0.542708, + 0.174171, + 2.046735, + 0.090451, + -0.029029, + -0.341123, + -0.135677, + 0.043543, + 0.511684, ] ] ) @@ -545,22 +541,38 @@ def test_joints(bionc_type, joint_type: JointType): elif joint_type == JointType.GROUND_WELD: TestUtils.assert_equal( joint.constraint(Q1, Q2), - np.array([-1.3, -1.9, -2.9, -1.29, -1.06, -3.147, -1.38, -2.159, -4.148, -1.47, -1.958, -5.249]), + np.array([-1.4, -0.9, -2.9, -1.4, -2.16, -4.15]), decimal=6, ) child_jacobian = joint.constraint_jacobian(Q1, Q2) TestUtils.assert_equal( child_jacobian, - -np.eye(12), + -np.eye(12)[3:9, :], decimal=6, ) child_jacobian_dot = joint.constraint_jacobian_derivative(Q1, Q2) TestUtils.assert_equal( child_jacobian_dot, - np.zeros((12, 12)), + np.zeros((6, 12)), decimal=6, ) assert joint.parent_constraint_jacobian(Q1, Q2) is None assert joint.parent_constraint_jacobian_derivative(Q1, Q2) is None + + +# def test_numpy_jacobian_from_casadi_derivatives() +# todo: +# # numpy version +# Q_test = NaturalCoordinates(np.arange(24)) +# jacobian_numpy = model.joint_constraints_jacobian(Q_test) +# +# model_mx = model.to_mx() +# sym = NaturalCoordinatesMX.sym(2) +# j_constraints_sym = model_mx.joint_constraints(sym) +# # jacobian +# j_jacobian_sym = jacobian(j_constraints_sym, sym) +# j_jacobian_func = Function("j_jacobian_func", [sym], [j_jacobian_sym]) +# +# jacobian_mx = j_jacobian_func(np.arange(24)).toarray()