diff --git a/bionc/__init__.py b/bionc/__init__.py index 36b91a4d..fbdcd449 100644 --- a/bionc/__init__.py +++ b/bionc/__init__.py @@ -1,15 +1,5 @@ -from .model_creation import ( - AxisTemplate, - AxisFunctionTemplate, - MarkerTemplate, - SegmentTemplate, - NaturalSegmentTemplate, - InertiaParametersTemplate, - BiomechanicalModelTemplate, - C3dData, - Data, - GenericDynamicModel, -) +# from bionc import bionc_casadi +# from bionc import bionc_numpy from .bionc_numpy import ( Axis, NaturalMarker, @@ -19,23 +9,29 @@ BiomechanicalModel, JointType, ExternalForceSet, - ExternalForce, + ExternalForceInGlobalLocalPoint, NaturalInertialParameters, compute_transformation_matrix, + InverseKinematics, ) - -from .protocols import natural_coordinates -from bionc import bionc_casadi -from bionc import bionc_numpy - +from .bionc_numpy.homogenous_transform import HomogeneousTransform +from .bionc_numpy.natural_accelerations import SegmentNaturalAccelerations, NaturalAccelerations from .bionc_numpy.natural_coordinates import SegmentNaturalCoordinates, NaturalCoordinates from .bionc_numpy.natural_velocities import SegmentNaturalVelocities, NaturalVelocities -from .bionc_numpy.natural_accelerations import SegmentNaturalAccelerations, NaturalAccelerations -from .bionc_numpy.homogenous_transform import HomogeneousTransform - +from .model_creation import ( + AxisTemplate, + AxisFunctionTemplate, + MarkerTemplate, + SegmentTemplate, + NaturalSegmentTemplate, + InertiaParametersTemplate, + BiomechanicalModelTemplate, + C3dData, + Data, + GenericDynamicModel, +) +from .protocols import natural_coordinates from .utils.enums import NaturalAxis, CartesianAxis, EulerSequence, TransformationMatrixType -from .utils.transformation_matrix import TransformationMatrixUtil from .utils.ode_solver import RK4, forward_integration - +from .utils.transformation_matrix import TransformationMatrixUtil from .vizualization import Viz -from .bionc_numpy import InverseKinematics diff --git a/bionc/bionc_casadi/__init__.py b/bionc/bionc_casadi/__init__.py index ecb17c29..cf58aa17 100644 --- a/bionc/bionc_casadi/__init__.py +++ b/bionc/bionc_casadi/__init__.py @@ -1,19 +1,22 @@ -from .natural_coordinates import SegmentNaturalCoordinates, NaturalCoordinates -from .natural_velocities import SegmentNaturalVelocities, NaturalVelocities -from .natural_accelerations import SegmentNaturalAccelerations, NaturalAccelerations -from .homogenous_transform import HomogeneousTransform -from .natural_segment import NaturalSegment - # The actual model to inherit from from .biomechanical_model import BiomechanicalModel +from .cartesian_vector import vector_projection_in_non_orthogonal_basis +from .external_force import ExternalForceSet +from .external_force_global import ExternalForceInGlobal +from .external_force_global_local_point import ExternalForceInGlobalLocalPoint +from .external_force_global_on_proximal import ExternalForceInGlobalOnProximal +from .external_force_in_local import ExternalForceInLocal +from .homogenous_transform import HomogeneousTransform +from .inertia_parameters import InertiaParameters +from .joints import Joint +from .joints_with_ground import GroundJoint +from .natural_accelerations import SegmentNaturalAccelerations, NaturalAccelerations # Some classes to define the BiomechanicalModel from .natural_axis import Axis +from .natural_coordinates import SegmentNaturalCoordinates, NaturalCoordinates from .natural_marker import NaturalMarker, Marker, SegmentNaturalVector from .natural_segment import NaturalSegment -from .inertia_parameters import InertiaParameters -from .joints import Joint -from .joints_with_ground import GroundJoint +from .natural_segment import NaturalSegment from .natural_vector import NaturalVector -from .external_force import ExternalForceSet, ExternalForce -from .cartesian_vector import vector_projection_in_non_orthogonal_basis +from .natural_velocities import SegmentNaturalVelocities, NaturalVelocities diff --git a/bionc/bionc_casadi/biomechanical_model.py b/bionc/bionc_casadi/biomechanical_model.py index 81bdf4f1..e9d377dd 100644 --- a/bionc/bionc_casadi/biomechanical_model.py +++ b/bionc/bionc_casadi/biomechanical_model.py @@ -6,7 +6,8 @@ from .biomechanical_model_markers import BiomechanicalModelMarkers from .biomechanical_model_segments import BiomechanicalModelSegments from .cartesian_vector import vector_projection_in_non_orthogonal_basis -from .external_force import ExternalForceSet, ExternalForce +from .external_force import ExternalForceSet +from .external_force_global_on_proximal import ExternalForceInGlobalOnProximal from .natural_accelerations import NaturalAccelerations from .natural_coordinates import NaturalCoordinates from .natural_velocities import NaturalVelocities @@ -505,7 +506,7 @@ def _inverse_dynamics_recursive_step( Qi = Q.vector(segment_index) Qddoti = Qddot.vector(segment_index) - external_forces_i = external_forces.to_segment_natural_external_forces(segment_index=segment_index, Q=Q) + external_forces_i = external_forces.to_segment_natural_external_forces(segment_idx=segment_index, Q=Q) subtree_intersegmental_generalized_forces = MX.zeros((12, 1)) for child_index in self.children(segment_index): @@ -521,15 +522,13 @@ def _inverse_dynamics_recursive_step( lambdas=lambdas, ) # sum the generalized forces of each subsegment and transport them to the parent proximal point - intersegmental_generalized_forces = ExternalForce.from_components( - application_point_in_local=[0, 0, 0], force=forces[:, child_index], torque=torques[:, child_index] - ) - subtree_intersegmental_generalized_forces += intersegmental_generalized_forces.transport_to( - to_segment_index=segment_index, - new_application_point_in_local=[0, 0, 0], # proximal point - from_segment_index=child_index, - Q=Q, + intersegmental_generalized_forces = ExternalForceInGlobalOnProximal.from_components( + force=forces[:, child_index], torque=torques[:, child_index] ) + subtree_intersegmental_generalized_forces += intersegmental_generalized_forces.transport_to_another_segment( + Qfrom=Q.vector(child_index), Qto=Q.vector(segment_index) + ).natural_forces() + segment_i = self.segment_from_index(segment_index) force_i, torque_i, lambda_i = segment_i.inverse_dynamics( diff --git a/bionc/bionc_casadi/external_force.py b/bionc/bionc_casadi/external_force.py index d67b465a..31004e79 100644 --- a/bionc/bionc_casadi/external_force.py +++ b/bionc/bionc_casadi/external_force.py @@ -1,140 +1,16 @@ -from casadi import MX, inv, cross, vertcat -import numpy as np +from typing import Union -from .natural_vector import NaturalVector -from .natural_coordinates import SegmentNaturalCoordinates, NaturalCoordinates +from casadi import MX +from .external_force_global import ExternalForceInGlobal +from .external_force_global_local_point import ExternalForceInGlobalLocalPoint +from .external_force_global_on_proximal import ExternalForceInGlobalOnProximal +from .external_force_in_local import ExternalForceInLocal +from .natural_coordinates import 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, vertcat(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] - - def to_natural_force(self, Qi: SegmentNaturalCoordinates) -> MX: - """ - Apply external forces to the segment - - Parameters - ---------- - Qi: SegmentNaturalCoordinates - Segment natural coordinates - - Returns - ------- - MX - The external forces adequately transformed for the equation of motion in natural coordinates [12 x 1] - """ - - pseudo_interpolation_matrix = Qi.compute_pseudo_interpolation_matrix() - point_interpolation_matrix = NaturalVector(self.application_point_in_local).interpolate() - - fext = point_interpolation_matrix.T @ self.force - fext += pseudo_interpolation_matrix.T @ self.torque - - return fext - - def transport_to( - self, - to_segment_index: int, - new_application_point_in_local: np.ndarray, - Q: NaturalCoordinates, - from_segment_index: int, - ) -> MX: - """ - Transport the external force to another segment and another application point - - Parameters - ---------- - to_segment_index: int - The index of the new segment - new_application_point_in_local: np.ndarray - The application point of the force in the natural coordinate system of the new segment - Q: NaturalCoordinates - The natural coordinates of the system - from_segment_index: int - The index of the current segment the force is applied on - - Returns - ------- - np.ndarray - The external forces adequately transformed for the equation of motion in natural coordinates [12 x 1] - """ - - Qi_old = Q.vector(from_segment_index) - Qi_new = Q.vector(to_segment_index) - - old_point_interpolation_matrix = NaturalVector(self.application_point_in_local).interpolate() - new_point_interpolation_matrix = NaturalVector(new_application_point_in_local).interpolate() - - old_application_point_in_global = old_point_interpolation_matrix @ Qi_old - new_application_point_in_global = new_point_interpolation_matrix @ Qi_new - - new_pseudo_interpolation_matrix = Qi_new.compute_pseudo_interpolation_matrix() - - # Bour's formula to transport the moment from the application point to the new application point - lever_arm = new_application_point_in_global - old_application_point_in_global - additional_torque = new_pseudo_interpolation_matrix.T @ cross(lever_arm, self.force) - fext = self.to_natural_force(Qi_new) - fext += additional_torque - - return fext +ExternalForce = Union[ + ExternalForceInGlobal, ExternalForceInGlobalOnProximal, ExternalForceInGlobalLocalPoint, ExternalForceInLocal +] class ExternalForceSet: @@ -162,13 +38,6 @@ class ExternalForceSet: nb_segments This function returns the number of segments. - Examples - -------- - >>> from bionc import ExternalForceSet, ExternalForce - >>> import numpy as np - >>> f_ext = ExternalForceSet.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): @@ -195,7 +64,7 @@ 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): + def add_in_global(self, segment_index: int, external_force: MX, point_in_global: MX = None): """ Add an external force to the segment @@ -204,50 +73,73 @@ def add_external_force(self, segment_index: int, external_force: ExternalForce): segment_index: int The index of the segment external_force: - The external force to add + The external force to add [6 x 1], (torque, force) + point_in_global: MX + The point in global coordinates [3 x 1] """ - self.external_forces[segment_index].append(external_force) + self.external_forces[segment_index].append( + ExternalForceInGlobal( + application_point_in_global=point_in_global if point_in_global is not None else MX.zeros(3, 1), + external_forces=external_force, + ) + ) - def to_natural_external_forces(self, Q: NaturalCoordinates) -> MX: + def add_in_global_local_point(self, segment_index: int, external_force: MX, point_in_local: MX = None): """ - Converts and sums all the segment natural external forces to the full vector of natural external forces + Add an external force to the segment Parameters ---------- - Q : NaturalCoordinates - The natural coordinates of the model + segment_index: int + The index of the segment + external_force: + The external force to add [6 x 1], (torque, force) + point_in_local: MX + The point in global coordinates [3 x 1] """ - - 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" + self.external_forces[segment_index].append( + ExternalForceInGlobalLocalPoint( + application_point_in_local=point_in_local if point_in_local is not None else MX.zeros(3, 1), + external_forces=external_force, ) + ) - 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 + def add_in_local( + self, + segment_index: int, + external_force: MX, + point_in_local: MX = None, + transformation_matrix=None, + ): + """ + Add an external force to the segment - return natural_external_forces + Parameters + ---------- + segment_index: int + The index of the segment + external_force: + The external force in local cartesian frame to add [6 x 1], (torque, force) + point_in_local: MX + The point in global coordinates [3 x 1] + """ + self.external_forces[segment_index].append( + ExternalForceInLocal( + application_point_in_local=point_in_local if point_in_local is not None else MX.zeros(3, 1), + external_forces=external_force, + transformation_matrix=transformation_matrix, + ) + ) - def to_segment_natural_external_forces(self, Q: NaturalCoordinates, segment_index: int) -> MX: + 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 - for one segment + Converts and sums all natural external forces + to the full vector of natural external forces [nb_segment * 12 x 1] Parameters ---------- Q : NaturalCoordinates The natural coordinates of the model - segment_index: int - The index of the segment - - Returns - ------- - segment_natural_external_forces: MX """ if len(self.external_forces) != Q.nb_qi(): @@ -255,12 +147,24 @@ def to_segment_natural_external_forces(self, Q: NaturalCoordinates, segment_inde "The number of segment in the model and the number of segment in the external forces must be the same" ) - if segment_index >= len(self.external_forces): - raise ValueError("The segment index is out of range") + natural_external_forces = MX.zeros(12 * Q.nb_qi(), 1) + for segment_index in range(self.nb_segments): + + segment_natural_external_forces = self.to_segment_natural_external_forces(segment_index, Q) + slice_index = slice(segment_index * 12, (segment_index + 1) * 12) + natural_external_forces[slice_index, 0:1] = segment_natural_external_forces + + return natural_external_forces - segment_natural_external_forces = np.zeros((12, 1)) - for external_force in self.external_forces[segment_index]: - segment_natural_external_forces += external_force.to_natural_force(Q.vector(segment_index)) + def to_segment_natural_external_forces(self, segment_idx: int, Q: NaturalCoordinates) -> MX: + """ + Converts and sums all the segment natural external forces + to the full vector of natural external forces [12 x 1] + """ + segment_natural_external_forces = MX.zeros(12, 1) + segment_external_forces = self.external_forces[segment_idx] + for external_force in segment_external_forces: + segment_natural_external_forces += external_force.to_generalized_natural_forces(Q.vector(segment_idx)) return segment_natural_external_forces diff --git a/bionc/bionc_casadi/external_force_global.py b/bionc/bionc_casadi/external_force_global.py new file mode 100644 index 00000000..c687468a --- /dev/null +++ b/bionc/bionc_casadi/external_force_global.py @@ -0,0 +1,100 @@ +from casadi import MX, vertcat, cross + +from .external_force_global_on_proximal import ExternalForceInGlobalOnProximal +from .natural_coordinates import SegmentNaturalCoordinates + + +class ExternalForceInGlobal: + """ + This class represents an external force applied to a segment. + + Attributes + ---------- + application_point_in_local : MX + The application point of the force in the natural coordinate system of the segment + external_forces : MX + 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_global: MX, external_forces: MX): + self.application_point_in_global = MX(application_point_in_global) + self.external_forces = MX(external_forces) + + @classmethod + def from_components(cls, application_point_in_global: MX, force: MX, torque: MX): + """ + This function creates an external force from its components. + + Parameters + ---------- + application_point_in_global : MX + 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_global, vertcat(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] + + def transport_on_proximal( + self, + Qi: SegmentNaturalCoordinates, + ): + """ + Transport the external force to another segment and another application point in cartesian coordinates + + Parameters + ---------- + Qi: SegmentNaturalCoordinates + The natural coordinates of the system + + Returns + ------- + ExternalForceInGlobalOnProximal + The external force on the proximal point of the segment + """ + + old_application_point_in_global = self.application_point_in_global + new_application_point_in_global = Qi.rp + + # Bour's formula to transport the moment from the application point to the new application point + lever_arm = new_application_point_in_global - old_application_point_in_global + additional_torque = cross(lever_arm, self.force) + + # Sum the additional torque to the existing torque + new_external_forces = self.external_forces.__copy__() + new_external_forces[0:3] += additional_torque + + return ExternalForceInGlobalOnProximal(external_forces=new_external_forces) + + def to_generalized_natural_forces(self, Qi: SegmentNaturalCoordinates): + """This function returns the external force in the generalized natural forces [12x1] format.""" + return self.transport_on_proximal(Qi).to_generalized_natural_forces(Qi) diff --git a/bionc/bionc_casadi/external_force_global_local_point.py b/bionc/bionc_casadi/external_force_global_local_point.py new file mode 100644 index 00000000..d68a38dd --- /dev/null +++ b/bionc/bionc_casadi/external_force_global_local_point.py @@ -0,0 +1,105 @@ +from casadi import MX, vertcat, cross + +from .external_force_global_on_proximal import ExternalForceInGlobalOnProximal +from .natural_coordinates import SegmentNaturalCoordinates +from .natural_vector import NaturalVector + + +class ExternalForceInGlobalLocalPoint: + """ + This class represents an external force applied to a segment. + + Attributes + ---------- + application_point_in_local : MX + The application point of the force in the natural coordinate system of the segment + external_forces : MX + 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: MX, external_forces: 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: MX, force: MX, torque: MX): + """ + This function creates an external force from its components. + + Parameters + ---------- + application_point_in_local : MX + 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, vertcat(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] + + def transport_on_proximal( + self, + Qi: SegmentNaturalCoordinates, + ): + """ + Transport the external force to another segment and another application point in cartesian coordinates + + Parameters + ---------- + Qi: SegmentNaturalCoordinates + The natural coordinates of the system + + Returns + ------- + ExternalForceInGlobalOnProximal + The external force on the proximal point of the segment + """ + qi_array = Qi + + old_point_interpolation_matrix = NaturalVector(self.application_point_in_local).interpolate() + new_point_interpolation_matrix = NaturalVector.proximal().interpolate() + + old_application_point_in_global = old_point_interpolation_matrix @ qi_array + new_application_point_in_global = new_point_interpolation_matrix @ qi_array + + # Bour's formula to transport the moment from the application point to the new application point + lever_arm = new_application_point_in_global - old_application_point_in_global + + additional_torque = cross(lever_arm, self.force) + + new_external_forces = self.external_forces.__copy__() + new_external_forces[0:3] += additional_torque + + return ExternalForceInGlobalOnProximal(external_forces=new_external_forces) + + def to_generalized_natural_forces(self, Qi: SegmentNaturalCoordinates) -> MX: + """This function returns the external force in the generalized natural forces [12x1] format.""" + return self.transport_on_proximal(Qi).to_generalized_natural_forces(Qi) diff --git a/bionc/bionc_casadi/external_force_global_on_proximal.py b/bionc/bionc_casadi/external_force_global_on_proximal.py new file mode 100644 index 00000000..34c78ec4 --- /dev/null +++ b/bionc/bionc_casadi/external_force_global_on_proximal.py @@ -0,0 +1,143 @@ +from casadi import MX, vertcat, cross, transpose + +from .natural_coordinates import SegmentNaturalCoordinates +from .natural_vector import NaturalVector + + +class ExternalForceInGlobalOnProximal: + """ + This class represents an external force applied to a segment. This is the standard representation + of an external force this is why there is no application point as it is assumed to be at the proximal point (rp). + + Attributes + ---------- + external_forces : MX + 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. + natural_forces(Qi) + Format external forces to the segment + natural_moments(Qi) + Format external moments to the segment + to_generalized_natural_force(Qi) + Format external moments and forces to the generalized external force in the natural coordinate format. + """ + + def __init__(self, external_forces: MX): + self.external_forces = MX(external_forces) + + @classmethod + def from_components(cls, force: MX, torque: MX): + """ + This function creates an external force from its components. + + Parameters + ---------- + force + The force vector in the global coordinate system + torque + The torque vector in the global coordinate system + + Returns + ------- + ExternalForce + """ + + return cls(vertcat(torque, force)) + + @property + def force(self) -> MX: + """The cartesian force vector in the global coordinate system""" + return self.external_forces[3:6] + + @property + def torque(self) -> MX: + """The cartesian torque vector in the global coordinate system""" + return self.external_forces[0:3] + + def natural_forces(self) -> MX: + """ + Apply external forces to the segment + + Parameters + ---------- + Qi: SegmentNaturalCoordinates + Segment natural coordinates + + Returns + ------- + MX + The external forces adequately transformed for the equation of motion in natural coordinates [12 x 1] + """ + point_interpolation_matrix = NaturalVector.proximal().interpolate() + + return transpose(point_interpolation_matrix) @ self.force + + def natural_moments(self, Qi: SegmentNaturalCoordinates) -> MX: + """ + Apply external moments to the segment + + Parameters + ---------- + Qi: SegmentNaturalCoordinates + Segment natural coordinates + + Returns + ------- + MX + The external forces adequately transformed for the equation of motion in natural coordinates [12 x 1] + """ + pseudo_interpolation_matrix = Qi.compute_pseudo_interpolation_matrix() + + return transpose(pseudo_interpolation_matrix) @ self.torque + + def to_generalized_natural_forces(self, Qi: SegmentNaturalCoordinates) -> MX: + """ + Format external moments and forces to the generalized external force in the natural coordinate format. + + Parameters + ---------- + Qi: SegmentNaturalCoordinates + Segment natural coordinates + + Returns + ------- + MX + The external forces adequately transformed for the equation of motion in natural coordinates [12 x 1] + """ + return self.natural_forces() + self.natural_moments(Qi) + + def transport_to_another_segment(self, Qfrom: SegmentNaturalCoordinates, Qto: SegmentNaturalCoordinates) -> MX: + """ + Transport the external force to another segment and another application point in cartesian coordinates + + Parameters + ---------- + Qfrom: SegmentNaturalCoordinates + The natural coordinates of the segment from which the force is transported + Qto: SegmentNaturalCoordinates + The natural coordinates of the segment to which the force is transported + + Returns + ------- + ExternalForceInGlobalOnProximal + The external force on the proximal point of the segment + """ + qi_array = Qfrom + qj_array = Qto + + proximal_interpolation_matrix = NaturalVector.proximal().interpolate() + + old_application_point_in_global = proximal_interpolation_matrix @ qi_array + new_application_point_in_global = proximal_interpolation_matrix @ qj_array + + lever_arm = new_application_point_in_global - old_application_point_in_global + + return ExternalForceInGlobalOnProximal.from_components(self.force, self.torque + cross(lever_arm, self.force)) diff --git a/bionc/bionc_casadi/external_force_in_local.py b/bionc/bionc_casadi/external_force_in_local.py new file mode 100644 index 00000000..c9d28e7a --- /dev/null +++ b/bionc/bionc_casadi/external_force_in_local.py @@ -0,0 +1,134 @@ +from casadi import MX, vertcat, cross, inv, transpose + +from .external_force_global_on_proximal import ExternalForceInGlobalOnProximal +from .natural_coordinates import SegmentNaturalCoordinates +from .natural_vector import NaturalVector +from .utils import to_numeric_MX + + +class ExternalForceInLocal: + """ + 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), in local frame too + transformation_matrix : np.ndarray + The transformation matrix of the segment + + 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: MX, + external_forces: MX, + transformation_matrix: MX, + ): + self.application_point_in_local = MX(application_point_in_local) + self.external_forces = MX(external_forces) + self.transformation_matrix = MX(transformation_matrix) + + transformation_matrix_inverse = inv(transpose(self.transformation_matrix)) + self.transformation_matrix_inv = to_numeric_MX(transformation_matrix_inverse) + + @classmethod + def from_components( + cls, + application_point_in_local: MX, + force: MX, + torque: MX, + transformation_matrix: MX, + ): + """ + This function creates an external force from its components. + + Parameters + ---------- + application_point_in_local : MX + 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 + transformation_matrix : MX + The transformation matrix of the segment + + Returns + ------- + ExternalForce + """ + + return cls(application_point_in_local, vertcat(torque, force), transformation_matrix) + + @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] + + def forces_in_global(self, Qi: SegmentNaturalCoordinates): + rotation_matrix = Qi.to_uvw_matrix() @ self.transformation_matrix_inv + + force_in_global = rotation_matrix @ self.force + torque_in_global = rotation_matrix @ self.torque + + return vertcat(torque_in_global, force_in_global) + + def transport_on_proximal( + self, + Qi: SegmentNaturalCoordinates, + ): + """ + Transport the external force to another segment and another application point in cartesian coordinates + + Parameters + ---------- + Qi: SegmentNaturalCoordinates + The natural coordinates of the system + + Returns + ------- + ExternalForceInGlobalOnProximal + The external force on the proximal point of the segment + """ + qi_array = Qi + + old_point_interpolation_matrix = NaturalVector(self.application_point_in_local).interpolate() + new_point_interpolation_matrix = NaturalVector.proximal().interpolate() + + old_application_point_in_global = old_point_interpolation_matrix @ qi_array + new_application_point_in_global = new_point_interpolation_matrix @ qi_array + + # Bour's formula to transport the moment from the application point to the new application point + lever_arm = new_application_point_in_global - old_application_point_in_global + + new_external_forces = self.forces_in_global(Qi) + additional_torque = cross(lever_arm, new_external_forces[3:6]) + + # Sum the additional torque to the existing torque + new_external_forces[0:3] += additional_torque + + return ExternalForceInGlobalOnProximal(external_forces=new_external_forces) + + def to_generalized_natural_forces(self, Qi: SegmentNaturalCoordinates) -> MX: + """This function returns the external force in the generalized natural forces [12x1] format.""" + return self.transport_on_proximal(Qi).to_generalized_natural_forces(Qi) diff --git a/bionc/bionc_casadi/generalized_force.py b/bionc/bionc_casadi/generalized_force.py index fbfe0902..5f01eef3 100644 --- a/bionc/bionc_casadi/generalized_force.py +++ b/bionc/bionc_casadi/generalized_force.py @@ -1,13 +1,15 @@ from casadi import MX -from .external_force import ExternalForce +from .external_force_global_local_point import ExternalForceInGlobalLocalPoint from .natural_coordinates import SegmentNaturalCoordinates, NaturalCoordinates from ..utils.enums import CartesianAxis, EulerSequence from .rotations import euler_axes_from_rotation_matrices from ..protocols.joint import JointBase as Joint -class JointGeneralizedForces(ExternalForce): +class JointGeneralizedForces(ExternalForceInGlobalLocalPoint): + # todo: this class need to be rethinked, the inheritance is not optimal. + # but preserved for now as refactoring externalforces """ Made to handle joint generalized forces, it inherits from ExternalForce @@ -35,4 +37,4 @@ def __init__( ): super().__init__(external_forces=external_forces, application_point_in_local=application_point_in_local) - # TODO ! + # TODO : Not impletemented at all, but only for numpy, need a revision first. diff --git a/bionc/bionc_casadi/natural_coordinates.py b/bionc/bionc_casadi/natural_coordinates.py index 2e75aab3..2038e668 100644 --- a/bionc/bionc_casadi/natural_coordinates.py +++ b/bionc/bionc_casadi/natural_coordinates.py @@ -1,7 +1,7 @@ from typing import Union import numpy as np -from casadi import MX, vertcat, inv, cross +from casadi import MX, vertcat, inv, cross, horzcat from .cartesian_vector import vector_projection_in_non_orthogonal_basis from .natural_vector import NaturalVector @@ -93,6 +93,10 @@ def to_components(self): def to_uvw(self): return self.u, self.v, self.w + def to_uvw_matrix(self) -> MX: + """Return the matrix of the natural basis""" + return horzcat(self.u, self.v, self.w) + def to_natural_vector(self, vector: MX | np.ndarray) -> NaturalVector: """ This function converts a vector expressed in the global coordinate system diff --git a/bionc/bionc_casadi/natural_segment.py b/bionc/bionc_casadi/natural_segment.py index 45e237b8..732c832c 100644 --- a/bionc/bionc_casadi/natural_segment.py +++ b/bionc/bionc_casadi/natural_segment.py @@ -297,7 +297,7 @@ def segment_coordinates_system( return HomogeneousTransform.from_rt( # rotation=self.compute_transformation_matrix(transformation_matrix_type) @ horzcat(Q.u, Q.v, Q.w), # NOTE: I would like to make numerical inversion disappear and the transpose too, plz implement analytical inversion. - rotation=horzcat(Q.u, Q.v, Q.w) @ transformation_matrix_inverse, + rotation=Q.to_uvw_matrix() @ transformation_matrix_inverse, translation=Q.rp, ) diff --git a/bionc/bionc_numpy/__init__.py b/bionc/bionc_numpy/__init__.py index 016d636e..812df05f 100644 --- a/bionc/bionc_numpy/__init__.py +++ b/bionc/bionc_numpy/__init__.py @@ -1,23 +1,26 @@ -from .natural_coordinates import SegmentNaturalCoordinates, NaturalCoordinates -from .natural_velocities import SegmentNaturalVelocities, NaturalVelocities -from .natural_accelerations import SegmentNaturalAccelerations, NaturalAccelerations -from .homogenous_transform import HomogeneousTransform -from .natural_segment import NaturalSegment - # The actual model to inherit from from .biomechanical_model import BiomechanicalModel +from .cartesian_vector import vector_projection_in_non_orthogonal_basis +from .enums import JointType +from .external_force import ExternalForceSet +from .external_force_global import ExternalForceInGlobal +from .external_force_global_local_point import ExternalForceInGlobalLocalPoint +from .external_force_global_on_proximal import ExternalForceInGlobalOnProximal +from .external_force_in_local import ExternalForceInLocal +from .homogenous_transform import HomogeneousTransform +from .inertia_parameters import InertiaParameters +from .inverse_kinematics import InverseKinematics +from .joints import Joint +from .joints_with_ground import GroundJoint +from .natural_accelerations import SegmentNaturalAccelerations, NaturalAccelerations # Some classes to define the BiomechanicalModel from .natural_axis import Axis +from .natural_coordinates import SegmentNaturalCoordinates, NaturalCoordinates +from .natural_inertial_parameters import NaturalInertialParameters from .natural_marker import NaturalMarker, Marker, SegmentNaturalVector from .natural_segment import NaturalSegment -from .inertia_parameters import InertiaParameters -from .enums import JointType -from .joints import Joint -from .joints_with_ground import GroundJoint +from .natural_segment import NaturalSegment from .natural_vector import NaturalVector -from .inverse_kinematics import InverseKinematics -from .external_force import ExternalForceSet, ExternalForce -from .cartesian_vector import vector_projection_in_non_orthogonal_basis -from .natural_inertial_parameters import NaturalInertialParameters +from .natural_velocities import SegmentNaturalVelocities, NaturalVelocities from .transformation_matrix import compute_transformation_matrix diff --git a/bionc/bionc_numpy/biomechanical_model.py b/bionc/bionc_numpy/biomechanical_model.py index 046c8b1b..96dd8ede 100644 --- a/bionc/bionc_numpy/biomechanical_model.py +++ b/bionc/bionc_numpy/biomechanical_model.py @@ -7,7 +7,8 @@ from .biomechanical_model_markers import BiomechanicalModelMarkers from .biomechanical_model_segments import BiomechanicalModelSegments from .cartesian_vector import vector_projection_in_non_orthogonal_basis -from .external_force import ExternalForceSet, ExternalForce +from .external_force import ExternalForceSet +from .external_force_global_on_proximal import ExternalForceInGlobalOnProximal from .generalized_force import JointGeneralizedForcesList from .natural_accelerations import NaturalAccelerations from .natural_coordinates import NaturalCoordinates @@ -553,8 +554,9 @@ def _inverse_dynamics_recursive_step( Qi = Q.vector(segment_index) Qddoti = Qddot.vector(segment_index) - external_forces_i = external_forces.to_segment_natural_external_forces(segment_index=segment_index, Q=Q) + external_forces_i = external_forces.to_segment_natural_external_forces(segment_idx=segment_index, Q=Q) + # Get subtree intersegmental generalized forces subtree_intersegmental_generalized_forces = np.zeros((12, 1)) for child_index in self.children(segment_index): if not visited_segments[child_index]: @@ -569,21 +571,20 @@ def _inverse_dynamics_recursive_step( lambdas=lambdas, ) # sum the generalized forces of each subsegment and transport them to the parent proximal point - intersegmental_generalized_forces = ExternalForce.from_components( - application_point_in_local=[0, 0, 0], force=forces[:, child_index], torque=torques[:, child_index] + intersegmental_generalized_forces = ExternalForceInGlobalOnProximal.from_components( + force=forces[:, child_index], torque=torques[:, child_index] ) - subtree_intersegmental_generalized_forces += intersegmental_generalized_forces.transport_to( - to_segment_index=segment_index, - new_application_point_in_local=[0, 0, 0], # proximal point - from_segment_index=child_index, - Q=Q, - )[:, np.newaxis] + subtree_intersegmental_generalized_forces += intersegmental_generalized_forces.transport_to_another_segment( + Qfrom=Q.vector(child_index), Qto=Q.vector(segment_index) + ).natural_forces()[:, np.newaxis] + segment_i = self.segment_from_index(segment_index) + # Now we can compute the inverse dynamics of the segment force_i, torque_i, lambda_i = segment_i.inverse_dynamics( - Qi=Qi, - Qddoti=Qddoti, - subtree_intersegmental_generalized_forces=subtree_intersegmental_generalized_forces, + Qi, + Qddoti, + subtree_intersegmental_generalized_forces, segment_external_forces=external_forces_i, ) # re-assigned the computed values to the output arrays diff --git a/bionc/bionc_numpy/external_force.py b/bionc/bionc_numpy/external_force.py index 88aeae86..8367c098 100644 --- a/bionc/bionc_numpy/external_force.py +++ b/bionc/bionc_numpy/external_force.py @@ -1,138 +1,16 @@ -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] - - 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 [12 x 1] - """ - - pseudo_interpolation_matrix = Qi.compute_pseudo_interpolation_matrix() - 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 - - return np.array(fext) - - def transport_to( - self, - to_segment_index: int, - new_application_point_in_local: np.ndarray, - Q: NaturalCoordinates, - from_segment_index: int, - ): - """ - Transport the external force to another segment and another application point - - Parameters - ---------- - to_segment_index: int - The index of the new segment - new_application_point_in_local: np.ndarray - The application point of the force in the natural coordinate system of the new segment - Q: NaturalCoordinates - The natural coordinates of the system - from_segment_index: int - The index of the current segment the force is applied on - - Returns - ------- - np.ndarray - The external forces adequately transformed for the equation of motion in natural coordinates [12 x 1] - """ - - Qi_old = Q.vector(from_segment_index) - Qi_new = Q.vector(to_segment_index) - - old_point_interpolation_matrix = NaturalVector(self.application_point_in_local).interpolate() - new_point_interpolation_matrix = NaturalVector(new_application_point_in_local).interpolate() +from typing import Union - old_application_point_in_global = np.array(old_point_interpolation_matrix @ Qi_old).squeeze() - new_application_point_in_global = np.array(new_point_interpolation_matrix @ Qi_new).squeeze() - - new_pseudo_interpolation_matrix = Qi_new.compute_pseudo_interpolation_matrix() +import numpy as np - # Bour's formula to transport the moment from the application point to the new application point - lever_arm = new_application_point_in_global - old_application_point_in_global - additional_torque = new_pseudo_interpolation_matrix.T @ np.cross(lever_arm, self.force) - fext = self.to_natural_force(Qi_new) - fext += additional_torque +from .external_force_global import ExternalForceInGlobal +from .external_force_global_local_point import ExternalForceInGlobalLocalPoint +from .external_force_global_on_proximal import ExternalForceInGlobalOnProximal +from .external_force_in_local import ExternalForceInLocal +from .natural_coordinates import NaturalCoordinates - return fext +ExternalForce = Union[ + ExternalForceInGlobal, ExternalForceInGlobalOnProximal, ExternalForceInGlobalLocalPoint, ExternalForceInLocal +] class ExternalForceSet: @@ -160,13 +38,13 @@ class ExternalForceSet: nb_segments This function returns the number of segments. - Examples + Examples to revised -------- - >>> from bionc import ExternalForceSet, ExternalForce - >>> import numpy as np - >>> f_ext = ExternalForceSet.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) + from bionc import ExternalForceSet, ExternalForce + import numpy as np + f_ext = ExternalForceSet.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): @@ -193,7 +71,7 @@ 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): + def add_in_global(self, segment_index: int, external_force: np.ndarray, point_in_global: np.ndarray = None): """ Add an external force to the segment @@ -202,52 +80,75 @@ def add_external_force(self, segment_index: int, external_force: ExternalForce): segment_index: int The index of the segment external_force: - The external force to add + The external force to add [6 x 1], (torque, force) + point_in_global: np.ndarray + The point in global coordinates [3 x 1] """ - self.external_forces[segment_index].append(external_force) + self.external_forces[segment_index].append( + ExternalForceInGlobal( + application_point_in_global=point_in_global if point_in_global is not None else np.zeros((3, 1)), + external_forces=external_force, + ) + ) - def to_natural_external_forces(self, Q: NaturalCoordinates) -> np.ndarray: + def add_in_global_local_point( + self, segment_index: int, external_force: np.ndarray, point_in_local: np.ndarray = None + ): """ - Converts and sums all the segment natural external forces to the full vector of natural external forces + Add an external force to the segment Parameters ---------- - Q : NaturalCoordinates - The natural coordinates of the model + segment_index: int + The index of the segment + external_force: + The external force to add [6 x 1], (torque, force) + point_in_local: np.ndarray + The point in global coordinates [3 x 1] """ - - 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" + self.external_forces[segment_index].append( + ExternalForceInGlobalLocalPoint( + application_point_in_local=point_in_local if point_in_local is not None else np.zeros((3, 1)), + external_forces=external_force, ) + ) - 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 + def add_in_local( + self, + segment_index: int, + external_force: np.ndarray, + point_in_local: np.ndarray = None, + transformation_matrix=None, + ): + """ + Add an external force to the segment - return natural_external_forces + Parameters + ---------- + segment_index: int + The index of the segment + external_force: + The external force in local cartesian frame to add [6 x 1], (torque, force) + point_in_local: np.ndarray + The point in global coordinates [3 x 1] + """ + self.external_forces[segment_index].append( + ExternalForceInLocal( + application_point_in_local=point_in_local if point_in_local is not None else np.zeros((3, 1)), + external_forces=external_force, + transformation_matrix=transformation_matrix, + ) + ) - def to_segment_natural_external_forces(self, Q: NaturalCoordinates, segment_index: int) -> np.ndarray: + 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 - for one segment + Converts and sums all natural external forces + to the full vector of natural external forces [nb_segment * 12 x 1] Parameters ---------- Q : NaturalCoordinates The natural coordinates of the model - segment_index: int - The index of the segment - - Returns - ------- - segment_natural_external_forces: np.ndarray """ if len(self.external_forces) != Q.nb_qi(): @@ -255,12 +156,26 @@ def to_segment_natural_external_forces(self, Q: NaturalCoordinates, segment_inde "The number of segment in the model and the number of segment in the external forces must be the same" ) - if segment_index >= len(self.external_forces): - raise ValueError("The segment index is out of range") + natural_external_forces = np.zeros((12 * Q.nb_qi(), 1)) + for segment_index in range(self.nb_segments): + segment_natural_external_forces = self.to_segment_natural_external_forces(segment_index, Q) + slice_index = slice(segment_index * 12, (segment_index + 1) * 12) + natural_external_forces[slice_index, 0:1] = segment_natural_external_forces + + return natural_external_forces + + def to_segment_natural_external_forces(self, segment_idx: int, Q: NaturalCoordinates) -> np.ndarray: + """ + Converts and sums all the segment natural external forces + to the full vector of natural external forces [12 x 1] + """ segment_natural_external_forces = np.zeros((12, 1)) - for external_force in self.external_forces[segment_index]: - segment_natural_external_forces += external_force.to_natural_force(Q.vector(segment_index))[:, np.newaxis] + segment_external_forces = self.external_forces[segment_idx] + for external_force in segment_external_forces: + segment_natural_external_forces += external_force.to_generalized_natural_forces(Q.vector(segment_idx))[ + :, np.newaxis + ] return segment_natural_external_forces diff --git a/bionc/bionc_numpy/external_force_global.py b/bionc/bionc_numpy/external_force_global.py new file mode 100644 index 00000000..dd4c1a8f --- /dev/null +++ b/bionc/bionc_numpy/external_force_global.py @@ -0,0 +1,100 @@ +import numpy as np + +from .external_force_global_on_proximal import ExternalForceInGlobalOnProximal +from .natural_coordinates import SegmentNaturalCoordinates + + +class ExternalForceInGlobal: + """ + 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_global: np.ndarray, external_forces: np.ndarray): + self.application_point_in_global = application_point_in_global + self.external_forces = external_forces + + @classmethod + def from_components(cls, application_point_in_global: np.ndarray, force: np.ndarray, torque: np.ndarray): + """ + This function creates an external force from its components. + + Parameters + ---------- + application_point_in_global : 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_global, 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] + + def transport_on_proximal( + self, + Qi: SegmentNaturalCoordinates, + ): + """ + Transport the external force to another segment and another application point in cartesian coordinates + + Parameters + ---------- + Qi: SegmentNaturalCoordinates + The natural coordinates of the system + + Returns + ------- + ExternalForceInGlobalOnProximal + The external force on the proximal point of the segment + """ + + old_application_point_in_global = self.application_point_in_global + new_application_point_in_global = Qi.rp + + # Bour's formula to transport the moment from the application point to the new application point + lever_arm = new_application_point_in_global - old_application_point_in_global + additional_torque = np.cross(lever_arm, self.force) + + # Sum the additional torque to the existing torque + new_external_forces = self.external_forces.copy() + new_external_forces[0:3] += additional_torque + + return ExternalForceInGlobalOnProximal(external_forces=new_external_forces) + + def to_generalized_natural_forces(self, Qi: SegmentNaturalCoordinates): + """This function returns the external force in the generalized natural forces [12x1] format.""" + return self.transport_on_proximal(Qi).to_generalized_natural_forces(Qi) diff --git a/bionc/bionc_numpy/external_force_global_local_point.py b/bionc/bionc_numpy/external_force_global_local_point.py new file mode 100644 index 00000000..7dcb149e --- /dev/null +++ b/bionc/bionc_numpy/external_force_global_local_point.py @@ -0,0 +1,104 @@ +import numpy as np + +from .external_force_global_on_proximal import ExternalForceInGlobalOnProximal +from .natural_coordinates import SegmentNaturalCoordinates +from .natural_vector import NaturalVector + + +class ExternalForceInGlobalLocalPoint: + """ + 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] + + def transport_on_proximal( + self, + Qi: SegmentNaturalCoordinates, + ): + """ + Transport the external force to another segment and another application point in cartesian coordinates + + Parameters + ---------- + Qi: SegmentNaturalCoordinates + The natural coordinates of the system + + Returns + ------- + ExternalForceInGlobalOnProximal + The external force on the proximal point of the segment + """ + qi_array = np.array(Qi).squeeze() + + old_point_interpolation_matrix = NaturalVector(self.application_point_in_local).interpolate() + new_point_interpolation_matrix = NaturalVector.proximal().interpolate() + + old_application_point_in_global = np.array(old_point_interpolation_matrix @ qi_array).squeeze() + new_application_point_in_global = np.array(new_point_interpolation_matrix @ qi_array).squeeze() + + # Bour's formula to transport the moment from the application point to the new application point + lever_arm = new_application_point_in_global - old_application_point_in_global + additional_torque = np.cross(lever_arm, self.force) + + new_external_forces = self.external_forces.copy() + new_external_forces[0:3] += additional_torque + + return ExternalForceInGlobalOnProximal(external_forces=new_external_forces) + + def to_generalized_natural_forces(self, Qi: SegmentNaturalCoordinates) -> np.ndarray: + """This function returns the external force in the generalized natural forces [12x1] format.""" + return self.transport_on_proximal(Qi).to_generalized_natural_forces(Qi) diff --git a/bionc/bionc_numpy/external_force_global_on_proximal.py b/bionc/bionc_numpy/external_force_global_on_proximal.py new file mode 100644 index 00000000..0701d2d5 --- /dev/null +++ b/bionc/bionc_numpy/external_force_global_on_proximal.py @@ -0,0 +1,147 @@ +import numpy as np + +from .natural_coordinates import SegmentNaturalCoordinates +from .natural_vector import NaturalVector + + +class ExternalForceInGlobalOnProximal: + """ + This class represents an external force applied to a segment. This is the standard representation + of an external force this is why there is no application point as it is assumed to be at the proximal point (rp). + + Attributes + ---------- + 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. + natural_forces(Qi) + Format external forces to the segment + natural_moments(Qi) + Format external moments to the segment + to_generalized_natural_force(Qi) + Format external moments and forces to the generalized external force in the natural coordinate format. + """ + + def __init__(self, external_forces: np.ndarray): + self.external_forces = external_forces + + @classmethod + def from_components(cls, force: np.ndarray, torque: np.ndarray): + """ + This function creates an external force from its components. + + Parameters + ---------- + force + The force vector in the global coordinate system + torque + The torque vector in the global coordinate system + + Returns + ------- + ExternalForce + """ + + return cls(np.concatenate((torque, force))) + + @property + def force(self) -> np.ndarray: + """The cartesian force vector in the global coordinate system""" + return self.external_forces[3:6] + + @property + def torque(self) -> np.ndarray: + """The cartesian torque vector in the global coordinate system""" + return self.external_forces[0:3] + + def natural_forces(self) -> 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 [12 x 1] + """ + point_interpolation_matrix = NaturalVector.proximal().interpolate() + + return np.array(point_interpolation_matrix.T @ self.force) + + def natural_moments(self, Qi: SegmentNaturalCoordinates) -> np.ndarray: + """ + Apply external moments 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 [12 x 1] + """ + pseudo_interpolation_matrix = Qi.compute_pseudo_interpolation_matrix() + + return pseudo_interpolation_matrix.T @ self.torque + + def to_generalized_natural_forces(self, Qi: SegmentNaturalCoordinates) -> np.ndarray: + """ + Format external moments and forces to the generalized external force in the natural coordinate format. + + Parameters + ---------- + Qi: SegmentNaturalCoordinates + Segment natural coordinates + + Returns + ------- + np.ndarray + The external forces adequately transformed for the equation of motion in natural coordinates [12 x 1] + """ + return self.natural_forces() + self.natural_moments(Qi) + + def transport_to_another_segment( + self, Qfrom: SegmentNaturalCoordinates, Qto: SegmentNaturalCoordinates + ) -> np.ndarray: + """ + Transport the external force to another segment and another application point in cartesian coordinates + + Parameters + ---------- + Qfrom: SegmentNaturalCoordinates + The natural coordinates of the segment from which the force is transported + Qto: SegmentNaturalCoordinates + The natural coordinates of the segment to which the force is transported + + Returns + ------- + ExternalForceInGlobalOnProximal + The external force on the proximal point of the segment + """ + qi_array = np.array(Qfrom).squeeze() + qj_array = np.array(Qto).squeeze() + + proximal_interpolation_matrix = NaturalVector.proximal().interpolate() + + old_application_point_in_global = np.array(proximal_interpolation_matrix @ qi_array).squeeze() + new_application_point_in_global = np.array(proximal_interpolation_matrix @ qj_array).squeeze() + + lever_arm = new_application_point_in_global - old_application_point_in_global + + return ExternalForceInGlobalOnProximal.from_components( + self.force, self.torque + np.cross(lever_arm, self.force) + ) diff --git a/bionc/bionc_numpy/external_force_in_local.py b/bionc/bionc_numpy/external_force_in_local.py new file mode 100644 index 00000000..6724acbe --- /dev/null +++ b/bionc/bionc_numpy/external_force_in_local.py @@ -0,0 +1,131 @@ +import numpy as np + +from .external_force_global_on_proximal import ExternalForceInGlobalOnProximal +from .natural_coordinates import SegmentNaturalCoordinates +from .natural_vector import NaturalVector + + +class ExternalForceInLocal: + """ + 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), in local frame too + transformation_matrix : np.ndarray + The transformation matrix of the segment + + 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, + transformation_matrix: np.ndarray, + ): + self.application_point_in_local = application_point_in_local + self.external_forces = external_forces + self.transformation_matrix = transformation_matrix + self.transformation_matrix_inv = np.linalg.inv(self.transformation_matrix.T) + + @classmethod + def from_components( + cls, + application_point_in_local: np.ndarray, + force: np.ndarray, + torque: np.ndarray, + transformation_matrix: 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 + transformation_matrix : np.ndarray + The transformation matrix of the segment + + Returns + ------- + ExternalForce + """ + + return cls(application_point_in_local, np.concatenate((torque, force)), transformation_matrix) + + @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] + + def forces_in_global(self, Qi: SegmentNaturalCoordinates): + rotation_matrix = Qi.to_uvw_matrix() @ self.transformation_matrix_inv + + force_in_global = rotation_matrix @ self.force + torque_in_global = rotation_matrix @ self.torque + + return np.concatenate((torque_in_global, force_in_global)) + + def transport_on_proximal( + self, + Qi: SegmentNaturalCoordinates, + ): + """ + Transport the external force to another segment and another application point in cartesian coordinates + + Parameters + ---------- + Qi: SegmentNaturalCoordinates + The natural coordinates of the system + + Returns + ------- + ExternalForceInGlobalOnProximal + The external force on the proximal point of the segment + """ + qi_array = np.array(Qi).squeeze() + + old_point_interpolation_matrix = NaturalVector(self.application_point_in_local).interpolate() + new_point_interpolation_matrix = NaturalVector.proximal().interpolate() + + old_application_point_in_global = np.array(old_point_interpolation_matrix @ qi_array).squeeze() + new_application_point_in_global = np.array(new_point_interpolation_matrix @ qi_array).squeeze() + + # Bour's formula to transport the moment from the application point to the new application point + lever_arm = new_application_point_in_global - old_application_point_in_global + + new_external_forces = self.forces_in_global(Qi) + additional_torque = np.cross(lever_arm, new_external_forces[3:6]) + + # Sum the additional torque to the existing torque + new_external_forces[0:3] += additional_torque + + return ExternalForceInGlobalOnProximal(external_forces=new_external_forces) + + def to_generalized_natural_forces(self, Qi: SegmentNaturalCoordinates) -> np.ndarray: + """This function returns the external force in the generalized natural forces [12x1] format.""" + return self.transport_on_proximal(Qi).to_generalized_natural_forces(Qi) diff --git a/bionc/bionc_numpy/generalized_force.py b/bionc/bionc_numpy/generalized_force.py index aa94c3c9..2f8eafdc 100644 --- a/bionc/bionc_numpy/generalized_force.py +++ b/bionc/bionc_numpy/generalized_force.py @@ -1,13 +1,16 @@ import numpy as np -from .external_force import ExternalForce +from .external_force_global_local_point import ExternalForceInGlobalLocalPoint +from .external_force_global_on_proximal import ExternalForceInGlobalOnProximal from .natural_coordinates import SegmentNaturalCoordinates, NaturalCoordinates from .rotations import euler_axes_from_rotation_matrices from ..protocols.joint import JointBase as Joint from ..utils.enums import CartesianAxis, EulerSequence -class JointGeneralizedForces(ExternalForce): +class JointGeneralizedForces(ExternalForceInGlobalLocalPoint): + # todo: this class need to be rethinked, the inheritance is not optimal. + # but preserved for now as refactoring externalforces """ Made to handle joint generalized forces, it inherits from ExternalForce @@ -202,7 +205,7 @@ def empty_from_nb_joint(cls, nb_joint: int): """ return cls(joint_generalized_forces=[[] for _ in range(nb_joint)]) - def joint_generalized_force(self, joint_index: int) -> list[ExternalForce]: + def joint_generalized_force(self, joint_index: int) -> list[ExternalForceInGlobalOnProximal]: """Returns the external forces of the segment""" return self.joint_generalized_forces[joint_index] diff --git a/bionc/bionc_numpy/natural_coordinates.py b/bionc/bionc_numpy/natural_coordinates.py index 9ebb6f8e..ee354fc7 100644 --- a/bionc/bionc_numpy/natural_coordinates.py +++ b/bionc/bionc_numpy/natural_coordinates.py @@ -77,6 +77,10 @@ def to_components(self): def to_uvw(self): return self.u, self.v, self.w + def to_uvw_matrix(self) -> np.ndarray: + """Return the matrix of the natural basis""" + return np.concatenate((self.u[:, np.newaxis], self.v[:, np.newaxis], self.w[:, np.newaxis]), axis=1) + def to_natural_vector(self, vector: np.ndarray) -> NaturalVector | np.ndarray: """ This function converts a vector expressed in the global coordinate system @@ -126,12 +130,13 @@ def axis(self, axis: Union[str, NaturalAxis]) -> np.ndarray: def compute_pseudo_interpolation_matrix(self) -> np.ndarray: """ - Return the force moment transformation matrix + Return the force moment transformation matrix that allows to transform cartesian moments into generalized natural forces, + also denoted Nstar^T. Returns ------- np.ndarray - The force moment transformation matrix + The force moment transformation matrix, also Nstar^T [12 x 3] """ # default we apply force at the proximal point diff --git a/bionc/bionc_numpy/natural_segment.py b/bionc/bionc_numpy/natural_segment.py index 2692bb57..477f8bed 100644 --- a/bionc/bionc_numpy/natural_segment.py +++ b/bionc/bionc_numpy/natural_segment.py @@ -382,7 +382,7 @@ def segment_coordinates_system( return HomogeneousTransform.from_rt( # rotation=self.compute_transformation_matrix(transformation_matrix_type) # @ np.concatenate((Q.u[:, np.newaxis], Q.v[:, np.newaxis], Q.w[:, np.newaxis]), axis=1), - rotation=np.concatenate((Q.u[:, np.newaxis], Q.v[:, np.newaxis], Q.w[:, np.newaxis]), axis=1) @ + rotation=Q.to_uvw_matrix() @ # NOTE: I would like to make numerical inversion disappear and the transpose too x) np.linalg.inv(self.compute_transformation_matrix(transformation_matrix_type).T), translation=Q.rp[:, np.newaxis], @@ -789,6 +789,14 @@ def inverse_dynamics( subtree_intersegmental_generalized_forces: np.ndarray, segment_external_forces: np.ndarray, ) -> tuple[np.ndarray, np.ndarray, np.ndarray]: + """ + + Returns + ------- + tuple[np.ndarray, np.ndarray, np.ndarray] + The generalized forces [3 x 1], torques [3 x 1], and lagrange multipliers [6 x 1] + """ + proximal_interpolation_matrix = NaturalVector.proximal().interpolate() pseudo_interpolation_matrix = Qi.compute_pseudo_interpolation_matrix() rigid_body_constraints_jacobian = self.rigid_body_constraint_jacobian(Qi=Qi) diff --git a/bionc/bionc_numpy/transformation_matrix.py b/bionc/bionc_numpy/transformation_matrix.py index 2b9a2a4c..38f62b5e 100644 --- a/bionc/bionc_numpy/transformation_matrix.py +++ b/bionc/bionc_numpy/transformation_matrix.py @@ -1,47 +1,7 @@ import numpy as np from numpy import cos, sin -from ..utils.enums import NaturalAxis, TransformationMatrixType - - -def compute_transformation_matrix( - matrix_type: TransformationMatrixType, length: float, alpha: float, beta: float, gamma: float -): - """ - Create a transformation matrix from a TransformationMatrixType - - Parameters - ---------- - matrix_type: TransformationMatrixType - The type of transformation matrix to create, such as TransformationMatrixType.Buv, TransformationMatrixType.Bvw, etc. - length: float - The length of the segment - alpha: float - The alpha angle - beta: float - The beta angle - gamma: float - The gamma angle - - Returns - ------- - numpy.ndarray - The transformation matrix - """ - if matrix_type == TransformationMatrixType.Buv: - return _transformation_matrix_Buv(length, alpha, beta, gamma) - elif matrix_type == TransformationMatrixType.Bvu: - return _transformation_matrix_Bvu(length, alpha, beta, gamma) - elif matrix_type == TransformationMatrixType.Bwu: - return _transformation_matrix_Bwu(length, alpha, beta, gamma) - elif matrix_type == TransformationMatrixType.Buw: - return _transformation_matrix_Buw(length, alpha, beta, gamma) - elif matrix_type == TransformationMatrixType.Bvw: - return _transformation_matrix_Bvw(length, alpha, beta, gamma) - elif matrix_type == TransformationMatrixType.Bwv: - return _transformation_matrix_Bwv(length, alpha, beta, gamma) - else: - raise ValueError(f"Unknown TransformationMatrixType: {matrix_type}") +from ..utils.enums import TransformationMatrixType def _transformation_matrix_Buv(length: float, alpha: float, beta: float, gamma: float) -> np.ndarray: @@ -166,3 +126,44 @@ def _transformation_matrix_Bvw(length: float, alpha: float, beta: float, gamma: def _transformation_matrix_Bwv(length: float, alpha: float, beta: float, gamma: float) -> np.ndarray: raise NotImplementedError("The transformation matrix Bwv is not implemented yet.") + + +TRANSFORMATION_MAP = { + TransformationMatrixType.Buv: _transformation_matrix_Buv, + TransformationMatrixType.Bvu: _transformation_matrix_Bvu, + TransformationMatrixType.Bwu: _transformation_matrix_Bwu, + TransformationMatrixType.Buw: _transformation_matrix_Buw, + TransformationMatrixType.Bvw: _transformation_matrix_Bvw, + TransformationMatrixType.Bwv: _transformation_matrix_Bwv, +} + + +def compute_transformation_matrix( + matrix_type: TransformationMatrixType, length: float, alpha: float, beta: float, gamma: float +): + """ + Create a transformation matrix from a TransformationMatrixType + + Parameters + ---------- + matrix_type: TransformationMatrixType + The type of transformation matrix to create, such as TransformationMatrixType.Buv, TransformationMatrixType.Bvw, etc. + length: float + The length of the segment + alpha: float + The alpha angle + beta: float + The beta angle + gamma: float + The gamma angle + + Returns + ------- + numpy.ndarray + The transformation matrix + """ + + if matrix_type not in TRANSFORMATION_MAP: + raise ValueError(f"Unknown TransformationMatrixType: {matrix_type}") + + return TRANSFORMATION_MAP[matrix_type](length, alpha, beta, gamma) diff --git a/bionc/model_creation/biomechanical_model_template.py b/bionc/model_creation/biomechanical_model_template.py index 8c3b333f..99d8580f 100644 --- a/bionc/model_creation/biomechanical_model_template.py +++ b/bionc/model_creation/biomechanical_model_template.py @@ -1,8 +1,7 @@ from .protocols import Data from .segment_template import SegmentTemplate -from ..bionc_numpy.biomechanical_model import BiomechanicalModel -from ..bionc_numpy.enums import JointType + from ..utils.enums import NaturalAxis, EulerSequence, TransformationMatrixType @@ -25,7 +24,7 @@ def __setitem__(self, name: str, segment: SegmentTemplate): def add_joint( self, name: str, - joint_type: JointType, + joint_type: "JointType", parent: str, child: str, parent_axis: NaturalAxis | tuple[NaturalAxis] | list[NaturalAxis] = None, @@ -97,7 +96,7 @@ def add_joint( child_basis=child_basis, ) - def update(self, data: Data) -> BiomechanicalModel: + def update(self, data: Data) -> "BiomechanicalModel": """ Collapse the model to an actual personalized biomechanical model based on the generic model and the data file (usually a static trial) @@ -107,6 +106,8 @@ def update(self, data: Data) -> BiomechanicalModel: data The data to collapse the model from """ + from ..bionc_numpy.biomechanical_model import BiomechanicalModel + model = BiomechanicalModel() for name in self.segments: s = self.segments[name] diff --git a/bionc/model_creation/inertia_parameters_template.py b/bionc/model_creation/inertia_parameters_template.py index e3283b4f..b71138ff 100644 --- a/bionc/model_creation/inertia_parameters_template.py +++ b/bionc/model_creation/inertia_parameters_template.py @@ -1,10 +1,6 @@ from typing import Callable import numpy as np - -from ..bionc_numpy.inertia_parameters import InertiaParameters -from bionc.bionc_numpy.biomechanical_model import BiomechanicalModel -from ..bionc_numpy.natural_segment import NaturalSegment from .protocols import Data @@ -34,8 +30,11 @@ def __init__( self.inertia = inertia def to_real( - self, data: Data, kinematic_chain: BiomechanicalModel, parent_scs: NaturalSegment = None - ) -> InertiaParameters: + self, data: Data, kinematic_chain: "BiomechanicalModel", parent_scs: "NaturalSegment" = None + ) -> "InertiaParameters": + + from ..bionc_numpy.inertia_parameters import InertiaParameters + return InertiaParameters.from_data( data, self.relative_mass, diff --git a/bionc/model_creation/marker_template.py b/bionc/model_creation/marker_template.py index 1b978764..100192bf 100644 --- a/bionc/model_creation/marker_template.py +++ b/bionc/model_creation/marker_template.py @@ -1,15 +1,8 @@ -from typing import Callable, Union +from typing import Callable import numpy as np -from bionc.bionc_numpy.biomechanical_model import BiomechanicalModel -from ..bionc_numpy.natural_marker import NaturalMarker, Marker - -# from .biomechanical_model_template import BiomechanicalModelTemplate from .protocols import Data -from ..bionc_numpy.natural_segment import NaturalSegment - -# from ..utils.natural_coordinates import SegmentNaturalCoordinates from ..protocols.natural_coordinates import SegmentNaturalCoordinates @@ -49,8 +42,10 @@ def __init__( self.marker_type = marker_type def to_marker( - self, data: Data, kinematic_chain: BiomechanicalModel, natural_segment: NaturalSegment = None - ) -> Union[NaturalMarker, Marker]: + self, data: Data, kinematic_chain: "BiomechanicalModel", natural_segment: "NaturalSegment" = None + ) -> "Marker": + from ..bionc_numpy.natural_marker import Marker + return Marker.from_data( data=data, name=self.name, @@ -61,8 +56,10 @@ def to_marker( ) def to_natural_marker( - self, data: Data, kinematic_chain: BiomechanicalModel, Q_xp: SegmentNaturalCoordinates = None - ) -> Union[NaturalMarker, Marker]: + self, data: Data, kinematic_chain: "BiomechanicalModel", Q_xp: SegmentNaturalCoordinates = None + ) -> "NaturalMarker": + from ..bionc_numpy.natural_marker import NaturalMarker + return NaturalMarker.from_data( data, self.name, diff --git a/bionc/model_creation/natural_axis_template.py b/bionc/model_creation/natural_axis_template.py index 864ce795..497dbfe1 100644 --- a/bionc/model_creation/natural_axis_template.py +++ b/bionc/model_creation/natural_axis_template.py @@ -2,11 +2,8 @@ import numpy as np -from ..bionc_numpy.natural_axis import Axis -from bionc.bionc_numpy.biomechanical_model import BiomechanicalModel from .marker_template import MarkerTemplate from .protocols import Data -from ..bionc_numpy.natural_segment import NaturalSegment class AxisTemplate: @@ -24,7 +21,7 @@ def __init__(self, start: Callable | str, end: Callable | str): self.start = MarkerTemplate(function=start, marker_type="Marker") self.end = MarkerTemplate(function=end, marker_type="Marker") - def to_axis(self, data: Data, kinematic_chain: BiomechanicalModel, parent_scs: NaturalSegment = None) -> Axis: + def to_axis(self, data: Data, kinematic_chain: "BiomechanicalModel", parent_scs: "NaturalSegment" = None) -> "Axis": """ Compute the axis from actual data @@ -38,6 +35,7 @@ def to_axis(self, data: Data, kinematic_chain: BiomechanicalModel, parent_scs: N parent_scs The transformation from global to local """ + from ..bionc_numpy.natural_axis import Axis start = self.start.to_marker(data, kinematic_chain, parent_scs) end = self.end.to_marker(data, kinematic_chain, parent_scs) @@ -85,7 +83,7 @@ def __init__(self, function: Callable): """ self.axis_function = function - def to_axis(self, data: Data, kinematic_chain: BiomechanicalModel, parent_scs: NaturalSegment = None) -> Axis: + def to_axis(self, data: Data, kinematic_chain: "BiomechanicalModel", parent_scs: "NaturalSegment" = None) -> "Axis": """ Compute the axis from actual data @@ -99,4 +97,6 @@ def to_axis(self, data: Data, kinematic_chain: BiomechanicalModel, parent_scs: N parent_scs The transformation from global to local """ + from ..bionc_numpy.natural_axis import Axis + return Axis.from_data(data, self.axis_function, kinematic_chain) diff --git a/bionc/model_creation/natural_segment_template.py b/bionc/model_creation/natural_segment_template.py index e3a372ee..86d2b5e9 100644 --- a/bionc/model_creation/natural_segment_template.py +++ b/bionc/model_creation/natural_segment_template.py @@ -1,12 +1,9 @@ from typing import Callable -from .natural_axis_template import AxisTemplate -from bionc.bionc_numpy.biomechanical_model import BiomechanicalModel - -from ..protocols.natural_coordinates import SegmentNaturalCoordinates from .marker_template import MarkerTemplate +from .natural_axis_template import AxisTemplate from .protocols import Data -from ..bionc_numpy.natural_segment import NaturalSegment +from ..protocols.natural_coordinates import SegmentNaturalCoordinates class NaturalSegmentTemplate: @@ -41,7 +38,7 @@ def __init__( self.distal_point = MarkerTemplate(function=distal_point, marker_type="Marker") self.w_axis = w_axis - def experimental_Q(self, data: Data, kinematic_chain: BiomechanicalModel) -> SegmentNaturalCoordinates: + def experimental_Q(self, data: Data, kinematic_chain: "BiomechanicalModel") -> SegmentNaturalCoordinates: """ Collapse the generic SegmentCoordinateSystem to an actual SegmentCoordinateSystemReal with value based on the model and the data @@ -69,7 +66,7 @@ def experimental_Q(self, data: Data, kinematic_chain: BiomechanicalModel) -> Seg ) return self.Q - def update(self) -> NaturalSegment: + def update(self) -> "NaturalSegment": """ Collapse the generic SegmentCoordinateSystem to an actual SegmentCoordinateSystemReal with value based on the model and the data @@ -84,5 +81,6 @@ def update(self) -> NaturalSegment: NaturalSegment The collapsed SegmentCoordinateSystemReal """ + from ..bionc_numpy.natural_segment import NaturalSegment return NaturalSegment.from_experimental_Q(self.Q) diff --git a/bionc/protocols/natural_markers.py b/bionc/protocols/natural_markers.py index 07116f5f..ae5d8378 100644 --- a/bionc/protocols/natural_markers.py +++ b/bionc/protocols/natural_markers.py @@ -4,8 +4,8 @@ import numpy as np from .biomechanical_model import GenericBiomechanicalModel -from ..model_creation.protocols import Data from .natural_coordinates import SegmentNaturalCoordinates +from ..model_creation.protocols import Data class AbstractNaturalMarker(ABC): @@ -21,9 +21,6 @@ class AbstractNaturalMarker(ABC): """ - def __init__(self): - self.name = None - @abstractmethod def from_data( cls, diff --git a/bionc/utils/c3d_ik_exporter.py b/bionc/utils/c3d_ik_exporter.py index 8013b7f7..a51a23ae 100644 --- a/bionc/utils/c3d_ik_exporter.py +++ b/bionc/utils/c3d_ik_exporter.py @@ -4,8 +4,8 @@ import numpy as np from .c3d_export_utils import add_point_from_dictionary -from ..bionc_numpy import NaturalCoordinates from ..protocols.biomechanical_model import GenericBiomechanicalModel +from ..protocols.natural_coordinates import NaturalCoordinates class C3DInverseKinematicsExporter: @@ -38,9 +38,10 @@ def add_technical_markers(self, Q: NaturalCoordinates) -> None: c3d_file : ezc3d.c3d The c3d file with natural coordinate points added """ + from ..bionc_numpy.natural_coordinates import NaturalCoordinates as NaturalCoordinatesNumpy - if Q is not isinstance(Q, NaturalCoordinates): - Q = NaturalCoordinates(Q) + if Q is not isinstance(Q, NaturalCoordinatesNumpy): + Q = NaturalCoordinatesNumpy(Q) model_markers = self.model.markers(Q) @@ -65,9 +66,11 @@ def add_natural_coordinate(self, Q: NaturalCoordinates) -> None: c3d_file : ezc3d.c3d The c3d file with natural coordinate points added """ + from ..bionc_numpy.natural_coordinates import NaturalCoordinates as NaturalCoordinatesNumpy + + if Q is not isinstance(Q, NaturalCoordinatesNumpy): + Q = NaturalCoordinatesNumpy(Q) - if Q is not isinstance(Q, NaturalCoordinates): - Q = NaturalCoordinates(Q) # Calulation of a reasonable factor for the u and w list_factor = [] for s in range(Q.nb_qi()): diff --git a/examples/forward_dynamics/double_pendulum_with_force.py b/examples/forward_dynamics/double_pendulum_with_force.py index 046b51c5..a5fdfbc3 100644 --- a/examples/forward_dynamics/double_pendulum_with_force.py +++ b/examples/forward_dynamics/double_pendulum_with_force.py @@ -1,5 +1,6 @@ import numpy as np +from bionc import NaturalAxis, CartesianAxis, RK4, TransformationMatrixType from bionc.bionc_numpy import ( BiomechanicalModel, NaturalSegment, @@ -9,9 +10,7 @@ SegmentNaturalVelocities, NaturalVelocities, ExternalForceSet, - ExternalForce, ) -from bionc import NaturalAxis, CartesianAxis, RK4, TransformationMatrixType def build_n_link_pendulum(nb_segments: int = 1) -> BiomechanicalModel: @@ -203,36 +202,18 @@ def main(mode: str = "force_equilibrium"): fext = ExternalForceSet.empty_from_nb_segment(nb_segment=nb_segments) # then add a force if mode == "force_equilibrium": - 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]), - ) - force2 = 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]), - ) + wrench = np.concatenate((np.array([0, 0, 0]), np.array([0, 0, 1 * 9.81]))) + fext.add_in_global_local_point(segment_index=0, external_force=wrench, point_in_local=np.array([0, 0.5, 0])) + wrench2 = np.concatenate((np.array([0, 0, 0]), np.array([0, 0, 1 * 9.81]))) + fext.add_in_global_local_point(segment_index=1, external_force=wrench2, point_in_local=np.array([0, 0.5, 0])) + elif mode == "no_equilibrium": - force1 = ExternalForce.from_components( - force=np.array([0, 0, 1 * 9.81]), - torque=np.array([0, 0, 0]), - application_point_in_local=np.array([0, -0.25, 0]), - ) - force2 = ExternalForce.from_components( - force=np.array([0, 0, 1 * 9.81]), - torque=np.array([0, 0, 0]), - application_point_in_local=np.array([0, -0.25, 0]), - ) + wrench1 = np.concatenate((np.array([0, 0, 0]), np.array([0, 0, 1 * 9.81]))) + fext.add_in_global_local_point(segment_index=0, external_force=wrench1, point_in_local=np.array([0, 0.25, 0])) + fext.add_in_global_local_point(segment_index=1, external_force=wrench1, point_in_local=np.array([0, 0.25, 0])) else: raise ValueError("mode must be 'force_equilibrium', 'moment_equilibrium' or 'no_equilibrium'") - # # then add the force to the list on segment 0 - fext.add_external_force(external_force=force1, segment_index=0) - fext.add_external_force(external_force=force2, segment_index=1) - model, time_steps, all_states, dynamics = apply_force_and_drop_pendulum( t_final=10, external_forces=fext, nb_segments=nb_segments ) @@ -241,8 +222,8 @@ def main(mode: str = "force_equilibrium"): if __name__ == "__main__": - model, all_states = main(mode="force_equilibrium") - # model, all_states = main(mode="no_equilibrium") + # model, all_states = main(mode="force_equilibrium") + model, all_states = main(mode="no_equilibrium") # animate the motion from bionc import Viz diff --git a/examples/forward_dynamics/pendulum_with_force.nmod b/examples/forward_dynamics/pendulum_with_force.nmod index 98d34b0c..1fd3fbf3 100644 Binary files a/examples/forward_dynamics/pendulum_with_force.nmod 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 index a32b0787..29a77093 100644 --- a/examples/forward_dynamics/pendulum_with_force.py +++ b/examples/forward_dynamics/pendulum_with_force.py @@ -1,5 +1,6 @@ import numpy as np +from bionc import NaturalAxis, CartesianAxis, RK4, TransformationMatrixType from bionc.bionc_numpy import ( BiomechanicalModel, NaturalSegment, @@ -9,9 +10,7 @@ SegmentNaturalVelocities, NaturalVelocities, ExternalForceSet, - ExternalForce, ) -from bionc import NaturalAxis, CartesianAxis, RK4, TransformationMatrixType def build_pendulum(): @@ -174,40 +173,39 @@ def main(mode: str = "moment_equilibrium"): fext = ExternalForceSet.empty_from_nb_segment(1) # then add a force if mode == "moment_equilibrium": - force1 = ExternalForce.from_components( - # 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]), + force = np.array([0, 0, 0]) + torque = np.array([-1 * 9.81 * 0.50, 0, 0]) + fext.add_in_global_local_point( + external_force=np.concatenate([torque, force]), segment_index=0, point_in_local=np.array([0, 0, 0]) ) elif mode == "force_equilibrium": - 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]), + force = np.array([0, 0, 1 * 9.81]) + torque = np.array([0, 0, 0]) + fext.add_in_global_local_point( + external_force=np.concatenate([torque, force]), + segment_index=0, + point_in_local=np.array([0, 0.5, 0]), ) + elif mode == "no_equilibrium": - force1 = ExternalForce.from_components( - # this will not prevent the pendulum to fall it will keep drag the pendulum down - force=np.array([0, 0, 1.0 * 9.81]), - torque=np.array([0, 0, 0]), - application_point_in_local=np.array([0, -0.25, 0]), + # this will not prevent the pendulum to fall it will keep drag the pendulum down + fext.add_in_global_local_point( + segment_index=0, + external_force=np.concatenate([np.array([0, 0, 1.0 * 9.81]), [0, 0, 0]]), + point_in_local=np.array([0, 0.25, 0]), ) + else: raise ValueError("mode should be one of 'moment_equilibrium', 'force_equilibrium', 'no_equilibrium'") - # 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) return model, all_states if __name__ == "__main__": - model, all_states = main(mode="moment_equilibrium") - # model, all_states = main(mode="force_equilibrium") + # model, all_states = main(mode="moment_equilibrium") + model, all_states = main(mode="force_equilibrium") # model, all_states = main(mode="no_equilibrium") # animate the motion diff --git a/examples/inverse_dynamics/three_link_pendulum.py b/examples/inverse_dynamics/three_link_pendulum.py index b736227e..b356eb1a 100644 --- a/examples/inverse_dynamics/three_link_pendulum.py +++ b/examples/inverse_dynamics/three_link_pendulum.py @@ -1,3 +1,6 @@ +import numpy as np + +from bionc import NaturalAxis, CartesianAxis, TransformationMatrixType from bionc.bionc_numpy import ( NaturalCoordinates, NaturalAccelerations, @@ -8,9 +11,6 @@ JointType, ) -from bionc import NaturalAxis, CartesianAxis, TransformationMatrixType -import numpy as np - def build_n_link_pendulum(nb_segments: int = 1) -> BiomechanicalModel: """Build a n-link pendulum model""" @@ -63,7 +63,7 @@ def build_n_link_pendulum(nb_segments: int = 1) -> BiomechanicalModel: return model -def main(mode: str = "horizontal"): +def main(mode: str = "horizontal", with_fext: bool = False): nb_segments = 3 model = build_n_link_pendulum(nb_segments=nb_segments) @@ -96,7 +96,15 @@ def main(mode: str = "horizontal"): ] Qddot = NaturalAccelerations.from_qddoti(tuple(tuple_of_Qddot)) - torques, forces, lambdas = model.inverse_dynamics(Q=Q, Qddot=Qddot) + if with_fext: + fext = model.external_force_set() + fext.add_in_global_local_point(1, np.array([0.1, 0.2, 0.3, 0.01, 0.02, 0.03]), np.array([0.01, 0.02, 0.03])) + fext.add_in_global(2, np.array([0.1, 0.2, 0.3, 0.01, 0.02, 0.03]) * 0.1, np.array([0.01, 0.02, 0.03]) * 0.1) + + else: + fext = None + + torques, forces, lambdas = model.inverse_dynamics(Q=Q, Qddot=Qddot, external_forces=fext) print(torques) print(forces) diff --git a/pendulum.nmod b/pendulum.nmod new file mode 100644 index 00000000..15d737cc Binary files /dev/null and b/pendulum.nmod differ diff --git a/pendulum_with_force.nmod b/pendulum_with_force.nmod new file mode 100644 index 00000000..1fd3fbf3 Binary files /dev/null and b/pendulum_with_force.nmod differ diff --git a/tests/test_external_force.py b/tests/test_external_force.py index f977d004..88cc145e 100644 --- a/tests/test_external_force.py +++ b/tests/test_external_force.py @@ -1,6 +1,8 @@ +import numpy as np import pytest +from casadi import vertcat + from .utils import TestUtils -import numpy as np @pytest.mark.parametrize( @@ -12,14 +14,13 @@ @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, 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 ( ExternalForceSet, - ExternalForce, SegmentNaturalCoordinates, NaturalCoordinates, SegmentNaturalVelocities, @@ -30,12 +31,10 @@ def test_external_force(bionc_type, external_force_tuple): module = TestUtils.load_module(bionc + "/examples/forward_dynamics/pendulum_with_force.py") fext = ExternalForceSet.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], + external_force = np.concatenate([external_force_tuple[1], external_force_tuple[0]]) + fext.add_in_global_local_point( + external_force=external_force, segment_index=0, 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) @@ -59,11 +58,11 @@ def test_external_force(bionc_type, external_force_tuple): "casadi", ], ) -def test_external_force(bionc_type): +def test_external_force_local_point(bionc_type): if bionc_type == "numpy": from bionc.bionc_numpy import ( ExternalForceSet, - ExternalForce, + ExternalForceInGlobalLocalPoint, SegmentNaturalCoordinates, NaturalCoordinates, SegmentNaturalVelocities, @@ -72,32 +71,48 @@ def test_external_force(bionc_type): else: from bionc.bionc_casadi import ( ExternalForceSet, - ExternalForce, + ExternalForceInGlobalLocalPoint, SegmentNaturalCoordinates, NaturalCoordinates, SegmentNaturalVelocities, NaturalVelocities, ) - force1 = ExternalForce.from_components( + force1 = ExternalForceInGlobalLocalPoint.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( + force2 = ExternalForceInGlobalLocalPoint.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])) + TestUtils.assert_equal(force1.torque, np.array([0.04, 0.05, 0.06]), squeeze=True, expand=True) + TestUtils.assert_equal(force1.force, np.array([0.01, 0.02, 0.03]), squeeze=True) + TestUtils.assert_equal(force2.torque, np.array([0.14, 0.15, 0.16]), squeeze=True) + TestUtils.assert_equal(force2.force, np.array([0.11, 0.12, 0.13]), squeeze=True) fext = ExternalForceSet.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) + fext.add_in_global_local_point( + external_force=( + np.concatenate([force1.torque, force1.force]) + if bionc_type == "numpy" + else vertcat(force1.torque, force1.force) + ), + segment_index=0, + point_in_local=np.array([0.07, 0.08, 0.09]), + ) + fext.add_in_global_local_point( + external_force=( + np.concatenate([force2.torque, force2.force]) + if bionc_type == "numpy" + else vertcat(force2.torque, force2.force) + ), + segment_index=2, + point_in_local=np.array([0.17, 0.18, 0.19]), + ) # check that the pendulum is not moving Q0 = SegmentNaturalCoordinates.from_components( @@ -111,8 +126,6 @@ def test_external_force(bionc_type): Q = NaturalCoordinates.from_qi((Q0, Q1, Q2)) pseudo_interpolation_matrix = Q2.compute_pseudo_interpolation_matrix() - natural_force = force2.to_natural_force(Q2) - natural_forces = fext.to_natural_external_forces(Q) TestUtils.assert_equal( pseudo_interpolation_matrix, @@ -165,93 +178,491 @@ def test_external_force(bionc_type): expand=False, ) + natural_force = force2.transport_on_proximal(Q2).to_generalized_natural_forces(Q2) + natural_force_2_expected = np.array( + [ + 0.0, + 0.17762857, + 0.0, + 0.14266857, + 0.15266857, + 0.32601143, + -0.03266857, + -0.03266857, + -0.19601143, + 0.13083429, + 0.02180571, + 0.02180571, + ] + ) + TestUtils.assert_equal(natural_force, natural_force_2_expected, expand=False, squeeze=True) + + natural_forces = fext.to_natural_external_forces(Q) + complete_natural_force_expected = np.concatenate( + ( + np.array( + [ + [0.0], + [0.0594], + [0.0], + [0.01], + [0.02], + [0.0694], + [0.0], + [0.0], + [-0.0394], + [0.0512], + [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.0], + [0.0], + ] + ), + natural_force_2_expected[:, np.newaxis], + ) + ) TestUtils.assert_equal( - natural_force, + natural_forces, + complete_natural_force_expected, + expand=False, + squeeze=False, + ) + + transported_force = force2.transport_on_proximal(Q2) + TestUtils.assert_equal(transported_force.force, np.array([0.11, 0.12, 0.13]), squeeze=True) + TestUtils.assert_equal(transported_force.torque, np.array([0.13868, 0.15264, 0.15868]), squeeze=True) + + new_natural_force = ( + force2.transport_on_proximal(Q2) + .transport_to_another_segment( + Qfrom=Q2, + Qto=Q1, + ) + .to_generalized_natural_forces(Q1) + ) + + TestUtils.assert_equal( + new_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, + 0.0, + 0.1689, + 0.0, + 0.12522333, + 0.13522333, + 0.29745667, + -0.01522333, + -0.01522333, + -0.16745667, + 0.14175333, + 0.01288667, + 0.01288667, ] ), expand=False, + squeeze=True, + ) + + fext.add_in_global_local_point( + segment_index=2, + external_force=( + np.concatenate([force2.torque, force2.force]) + if bionc_type == "numpy" + else vertcat(force2.torque, force2.force) + ), + point_in_local=np.array([0.17, 0.18, 0.19]), + ) + segment_force_2 = fext.to_segment_natural_external_forces(Q=Q, segment_idx=2) + TestUtils.assert_equal( + np.squeeze(segment_force_2) if bionc_type == "numpy" else segment_force_2, + np.squeeze(natural_force_2_expected * 2) if bionc_type == "numpy" else natural_force_2_expected * 2.0, + expand=False, + squeeze=True, ) + +@pytest.mark.parametrize( + "bionc_type", + [ + "numpy", + "casadi", + ], +) +def test_external_force_in_global(bionc_type): + if bionc_type == "numpy": + from bionc.bionc_numpy import ( + ExternalForceSet, + ExternalForceInGlobal, + SegmentNaturalCoordinates, + NaturalCoordinates, + SegmentNaturalVelocities, + NaturalVelocities, + ) + else: + from bionc.bionc_casadi import ( + ExternalForceSet, + ExternalForceInGlobal, + SegmentNaturalCoordinates, + NaturalCoordinates, + SegmentNaturalVelocities, + NaturalVelocities, + ) + + force1 = ExternalForceInGlobal.from_components( + force=np.array([0.01, 0.02, 0.03]), + torque=np.array([0.04, 0.05, 0.06]), + application_point_in_global=np.array([0.07, 0.08, 0.09]), + ) + force2 = ExternalForceInGlobal.from_components( + force=np.array([0.11, 0.12, 0.13]), + torque=np.array([0.14, 0.15, 0.16]), + application_point_in_global=np.array([0.17, 0.18, 0.19]), + ) + + TestUtils.assert_equal(force1.torque, np.array([0.04, 0.05, 0.06]), squeeze=True) + TestUtils.assert_equal(force1.force, np.array([0.01, 0.02, 0.03]), squeeze=True) + TestUtils.assert_equal(force2.torque, np.array([0.14, 0.15, 0.16]), squeeze=True) + TestUtils.assert_equal(force2.force, np.array([0.11, 0.12, 0.13]), squeeze=True) + + fext = ExternalForceSet.empty_from_nb_segment(3) + fext.add_in_global( + external_force=( + np.concatenate([force1.torque, force1.force]) + if bionc_type == "numpy" + else vertcat(force1.torque, force1.force) + ), + segment_index=0, + point_in_global=np.array([0.07, 0.08, 0.09]), + ) + fext.add_in_global( + external_force=( + np.concatenate([force2.torque, force2.force]) + if bionc_type == "numpy" + else vertcat(force2.torque, force2.force) + ), + segment_index=2, + point_in_global=np.array([0.17, 0.18, 0.19]), + ) + + # 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)) + + natural_force = force2.transport_on_proximal(Q2).to_generalized_natural_forces(Q2) + natural_force_2_expected = np.array( + [ + 0.0, + 0.17957143, + 0.0, + 0.14305714, + 0.15305714, + 0.32834286, + -0.03305714, + -0.03305714, + -0.19834286, + 0.12617143, + 0.02102857, + 0.02102857, + ] + ) + TestUtils.assert_equal(natural_force, natural_force_2_expected, expand=False, squeeze=True) + + natural_forces = fext.to_natural_external_forces(Q) + complete_natural_force_expected = np.concatenate( + ( + np.array( + [ + [0.0], + [0.0594], + [0.0], + [0.01], + [0.02], + [0.0694], + [0.0], + [0.0], + [-0.0394], + [0.0512], + [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.0], + [0.0], + ] + ), + natural_force_2_expected[:, np.newaxis], + ) + ) TestUtils.assert_equal( natural_forces, + complete_natural_force_expected, + expand=False, + squeeze=False, + ) + + new_natural_force = ( + force2.transport_on_proximal(Q2) + .transport_to_another_segment( + Qfrom=Q2, + Qto=Q1, + ) + .to_generalized_natural_forces(Q1) + ) + + TestUtils.assert_equal( + new_natural_force, 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], + 0.0, + 0.17116667, + 0.0, + 0.12545, + 0.13545, + 0.29995, + -0.01545, + -0.01545, + -0.16995, + 0.13676667, + 0.01243333, + 0.01243333, ] ), expand=False, + squeeze=True, + ) + + fext.add_in_global( + segment_index=2, + external_force=( + np.concatenate([force2.torque, force2.force]) + if bionc_type == "numpy" + else vertcat(force2.torque, force2.force) + ), + point_in_global=np.array([0.17, 0.18, 0.19]), + ) + segment_force_2 = fext.to_segment_natural_external_forces(Q=Q, segment_idx=2) + TestUtils.assert_equal( + np.squeeze(segment_force_2) if bionc_type == "numpy" else segment_force_2, + np.squeeze(natural_force_2_expected * 2) if bionc_type == "numpy" else natural_force_2_expected * 2.0, + expand=False, + squeeze=True, + ) + + +@pytest.mark.parametrize( + "bionc_type", + [ + "numpy", + "casadi", + ], +) +def test_external_force_in_local(bionc_type): + if bionc_type == "numpy": + from bionc.bionc_numpy import ( + ExternalForceSet, + ExternalForceInLocal, + SegmentNaturalCoordinates, + NaturalCoordinates, + SegmentNaturalVelocities, + NaturalVelocities, + ) + else: + from bionc.bionc_casadi import ( + ExternalForceSet, + ExternalForceInLocal, + SegmentNaturalCoordinates, + NaturalCoordinates, + SegmentNaturalVelocities, + NaturalVelocities, + ) + dummy_transformation_matrix = np.eye(3) + np.ones((3, 3)) * 0.001 + dummy_transformation_matrix[2, 1] = 0.2 + force1 = ExternalForceInLocal.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]), + transformation_matrix=dummy_transformation_matrix, + ) + force2 = ExternalForceInLocal.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]), + transformation_matrix=dummy_transformation_matrix, + ) + + TestUtils.assert_equal(force1.torque, np.array([0.04, 0.05, 0.06]), squeeze=True) + TestUtils.assert_equal(force1.force, np.array([0.01, 0.02, 0.03]), squeeze=True) + TestUtils.assert_equal(force2.torque, np.array([0.14, 0.15, 0.16]), squeeze=True) + TestUtils.assert_equal(force2.force, np.array([0.11, 0.12, 0.13]), squeeze=True) + + fext = ExternalForceSet.empty_from_nb_segment(3) + fext.add_in_local( + external_force=( + np.concatenate([force1.torque, force1.force]) + if bionc_type == "numpy" + else vertcat(force1.torque, force1.force) + ), + segment_index=0, + point_in_local=np.array([0.07, 0.08, 0.09]), + transformation_matrix=dummy_transformation_matrix, + ) + fext.add_in_local( + external_force=( + np.concatenate([force2.torque, force2.force]) + if bionc_type == "numpy" + else vertcat(force2.torque, force2.force) + ), + segment_index=2, + point_in_local=np.array([0.17, 0.18, 0.19]), + transformation_matrix=dummy_transformation_matrix, + ) + + # 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)) + + natural_force = force2.transport_on_proximal(Q2).to_generalized_natural_forces(Q2) + natural_force_2_expected = np.array( + [ + 0.0, + 0.24572225, + 0.0, + 0.20196297, + 0.18615927, + 0.44411017, + -0.04442944, + -0.04442944, + -0.26657665, + 0.1537273, + 0.02562122, + 0.02562122, + ] + ) + TestUtils.assert_equal( + natural_force, + natural_force_2_expected, + expand=False, + squeeze=True, + ) + + natural_forces = fext.to_natural_external_forces(Q) + complete_natural_force_expected = np.concatenate( + ( + np.array( + [ + [0.0], + [0.05967894], + [0.0], + [0.00994612], + [0.01398684], + [0.06867157], + [0.0], + [0.0], + [-0.03872545], + [0.0391508], + [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.0], + [0.0], + ] + ), + natural_force_2_expected[:, np.newaxis], + ) + ) + TestUtils.assert_equal( + natural_forces, + complete_natural_force_expected, + expand=False, squeeze=False, ) - new_natural_force = force2.transport_to( - to_segment_index=0, - new_application_point_in_local=np.array([0.005, 0.01, 0.02]), - Q=Q, - from_segment_index=1, + new_natural_force = ( + force2.transport_on_proximal(Q2) + .transport_to_another_segment( + Qfrom=Q2, + Qto=Q1, + ) + .to_generalized_natural_forces(Q1) ) TestUtils.assert_equal( new_natural_force, np.array( - [0.0187, 0.17794, 0.0221, 0.1298, 0.1416, 0.29034, -0.0198, -0.0216, -0.16034, 0.17637, 0.0228, 0.0247] + [ + 0.0, + 0.2383283, + 0.0, + 0.17818587, + 0.16238218, + 0.40470934, + -0.02065235, + -0.02065235, + -0.22717582, + 0.16623615, + 0.01511238, + 0.01511238, + ] ), expand=False, + squeeze=True, ) - fext.add_external_force(external_force=force2, segment_index=2) - segment_force_2 = fext.to_segment_natural_external_forces(Q=Q, segment_index=2) + fext.add_in_local( + segment_index=2, + external_force=( + np.concatenate([force2.torque, force2.force]) + if bionc_type == "numpy" + else vertcat(force2.torque, force2.force) + ), + point_in_local=np.array([0.17, 0.18, 0.19]), + transformation_matrix=dummy_transformation_matrix, + ) + segment_force_2 = fext.to_segment_natural_external_forces(Q=Q, segment_idx=2) TestUtils.assert_equal( np.squeeze(segment_force_2) if bionc_type == "numpy" else segment_force_2, - np.squeeze(natural_force * 2) if bionc_type == "numpy" else natural_force * 2.0, + np.squeeze(natural_force_2_expected * 2) if bionc_type == "numpy" else natural_force_2_expected * 2.0, expand=False, squeeze=True, ) diff --git a/tests/test_inverse_dynamics.py b/tests/test_inverse_dynamics.py index 5c9bf9d3..31f0ff8f 100644 --- a/tests/test_inverse_dynamics.py +++ b/tests/test_inverse_dynamics.py @@ -1,3 +1,7 @@ +import numpy as np +import pytest +from casadi import MX + from bionc import ( BiomechanicalModel, NaturalSegment, @@ -7,11 +11,6 @@ EulerSequence, TransformationMatrixType, ) -from casadi import MX - -import numpy as np -import pytest - from .utils import TestUtils @@ -471,3 +470,55 @@ def test_id_example(): ), expand=False, ) + + +def test_id_example_with_fext(): + bionc = TestUtils.bionc_folder() + module_id = TestUtils.load_module(bionc + "/examples/inverse_dynamics/three_link_pendulum.py") + + b = module_id.main("vertical", with_fext=True) + assert isinstance(b, tuple) + assert len(b) == 3 + + torques = b[0] + forces = b[1] + lambdas = b[2] + + TestUtils.assert_equal( + torques, + np.array( + [ + [1.1000e-02, -1.0000e-02, -1.0000e-03], + [2.2000e-02, -2.0000e-02, -2.0000e-03], + [-2.9397e01, 9.7800e00, 1.9617e01], + ], + ), + expand=False, + ) + + TestUtils.assert_equal( + forces, + np.array( + [ + [0.0, -0.1013, -0.012], + [0.0, -0.2001, -0.019], + [0.0, -0.2995, -0.03], + ] + ), + expand=False, + ) + + TestUtils.assert_equal( + lambdas, + np.array( + [ + [0.00000000e00, 0.00000000e00, 0.00000000e00], + [0.00000000e00, 0.00000000e00, 0.00000000e00], + [0.00000000e00, 0.00000000e00, 0.00000000e00], + [0.00000000e00, 2.45250000e00, 4.90500000e00], + [0.00000000e00, -3.00344627e-16, -6.00689255e-16], + [0.00000000e00, 0.00000000e00, 0.00000000e00], + ] + ), + expand=False, + ) diff --git a/tests/test_natural_segment.py b/tests/test_natural_segment.py index 19c74dbd..fe9e3973 100644 --- a/tests/test_natural_segment.py +++ b/tests/test_natural_segment.py @@ -255,6 +255,30 @@ def test_natural_segment(bionc_type): TestUtils.assert_equal(my_segment.kinetic_energy(Qdoti), np.array(0.970531), expand=False) TestUtils.assert_equal(my_segment.potential_energy(Qi), np.array(0.229), expand=False) + my_segment2 = NaturalSegment.with_cartesian_inertial_parameters( + name="box", + alpha=np.pi / 2 + 0.1, + beta=np.pi / 2 - 0.05, + gamma=np.pi / 2 + 0.01, + length=1, + mass=1, + center_of_mass=np.array([0, 0.01, 0]), + inertia=np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]), + inertial_transformation_matrix=TransformationMatrixType.Buv, + ) + mat = my_segment2.segment_coordinates_system(Qi) + TestUtils.assert_equal( + mat, + np.array( + [ + [0.11, -0.09890496, 0.39714038, 0.21], + [0.12, -0.09880496, 0.40670988, 0.22], + [0.13, -0.09870496, 0.41627937, 0.23], + [0.0, 0.0, 0.0, 1.0], + ] + ), + ) + @pytest.mark.parametrize( "bionc_type", diff --git a/tests/test_ultimate_examples.py b/tests/test_ultimate_examples.py index 485c9f83..db5c2c4f 100644 --- a/tests/test_ultimate_examples.py +++ b/tests/test_ultimate_examples.py @@ -109,15 +109,18 @@ def test_single_pendulum_dofs(mode): raise ValueError("Invalid mode") -def test_forward_dynamics_with_force(): +@pytest.mark.parametrize( + "mode", + ["moment_equilibrium", "force_equilibrium", "no_equilibrium"], +) +def test_forward_dynamics_with_force(mode): bionc = TestUtils.bionc_folder() module_fd = TestUtils.load_module(bionc + "/examples/forward_dynamics/pendulum_with_force.py") - model, all_states = module_fd.main(mode="moment_equilibrium") + model, all_states = module_fd.main(mode=mode) - np.testing.assert_almost_equal( - all_states[:, -1], - np.array( + mode_to_states_expected = { + "moment_equilibrium": np.array( [ 1.0, 0.0, @@ -145,13 +148,7 @@ def test_forward_dynamics_with_force(): 0.0, ] ), - ) - - model, all_states = module_fd.main(mode="force_equilibrium") - - np.testing.assert_almost_equal( - all_states[:, -1], - np.array( + "force_equilibrium": np.array( [ 1.0, 0.0, @@ -179,40 +176,39 @@ def test_forward_dynamics_with_force(): 0.0, ] ), - ) - - model, all_states = module_fd.main(mode="no_equilibrium") - - np.testing.assert_almost_equal( - all_states[:, -1], - np.array( + "no_equilibrium": np.array( [ 1.00000000e00, - -1.25752847e-15, - -2.58263690e-16, - 4.26089219e-16, - 1.62086166e-16, - 1.14800688e-15, - 6.25950500e-16, - 9.99998625e-01, - -1.29093221e-03, - -1.02667903e-17, - -1.29099764e-03, - -9.99999167e-01, - -5.60086390e-31, - -4.23925536e-16, - -1.04410614e-16, - 1.25862608e-16, - 1.74366566e-17, - 2.62623414e-16, - 2.31835366e-16, - 7.25566569e-05, - 5.62890649e-02, - -1.23324768e-18, - 5.62889248e-02, - -7.25601194e-05, + -1.35070921e-14, + -5.74195276e-15, + 4.31085832e-15, + 6.18406905e-17, + 2.11272817e-15, + 6.29819871e-15, + -9.42305961e-01, + -3.34739950e-01, + -7.85747160e-18, + -3.34742001e-01, + 9.42309818e-01, + -4.31132552e-30, + 7.14524221e-15, + -1.75584183e-14, + 7.70496015e-16, + -5.53946285e-18, + 3.70702716e-16, + 2.21909111e-15, + 4.28930797e-01, + -1.20745434e00, + 1.24168228e-17, + -1.20746022e00, + -4.28933759e-01, ] ), + } + + np.testing.assert_almost_equal( + all_states[:, -1], + mode_to_states_expected[mode], ) diff --git a/tests/utils.py b/tests/utils.py index 58b7e984..a21aecbd 100644 --- a/tests/utils.py +++ b/tests/utils.py @@ -1,8 +1,9 @@ -from typing import Any, Union -from pathlib import Path import importlib.util -from casadi import MX, Function +from pathlib import Path +from typing import Any, Union + import numpy as np +from casadi import MX, Function class TestUtils: