From 48b0c75b4dd6f3370fa1f4a9f22ac1d29635c99b Mon Sep 17 00:00:00 2001 From: Ipuch Date: Fri, 29 Nov 2024 13:43:32 -0500 Subject: [PATCH 01/23] feat: better external forces --- bionc/bionc_numpy/external_force.py | 377 ++++++++++-------- bionc/bionc_numpy/external_force_global.py | 97 +++++ .../external_force_global_local_point.py | 101 +++++ .../external_force_global_on_proximal.py | 96 +++++ bionc/bionc_numpy/external_force_in_local.py | 133 ++++++ bionc/bionc_numpy/natural_coordinates.py | 5 +- bionc/bionc_numpy/transformation_matrix.py | 28 +- .../inverse_dynamics/external_force_set.py | 23 ++ 8 files changed, 670 insertions(+), 190 deletions(-) create mode 100644 bionc/bionc_numpy/external_force_global.py create mode 100644 bionc/bionc_numpy/external_force_global_local_point.py create mode 100644 bionc/bionc_numpy/external_force_global_on_proximal.py create mode 100644 bionc/bionc_numpy/external_force_in_local.py create mode 100644 examples/inverse_dynamics/external_force_set.py diff --git a/bionc/bionc_numpy/external_force.py b/bionc/bionc_numpy/external_force.py index 88aeae86..be218bae 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] +from typing import Union - @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() - - 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 ExternalForceGlobalLocalPoint +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, ExternalForceGlobalLocalPoint, 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,59 @@ 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) + if point_in_global is None: + self.external_forces[segment_index].append( + ExternalForceInGlobal(application_point_in_global=point_in_global, external_forces=external_force) + ) + else: + self.external_forces[segment_index].append(ExternalForceInGlobalOnProximal(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] """ + self.external_forces[segment_index].append( + ExternalForceGlobalLocalPoint(application_point_in_local=point_in_local, external_forces=external_force) + ) - if len(self.external_forces) != Q.nb_qi(): - raise ValueError( - "The number of segment in the model and the number of segment in the external forces must be the same" - ) - - natural_external_forces = np.zeros((12 * Q.nb_qi(), 1)) - for segment_index, segment_external_forces in enumerate(self.external_forces): - segment_natural_external_forces = np.zeros((12, 1)) - slice_index = slice(segment_index * 12, (segment_index + 1) * 12) - for external_force in segment_external_forces: - segment_natural_external_forces += external_force.to_natural_force(Q.vector(segment_index))[ - :, np.newaxis - ] - natural_external_forces[slice_index, 0:1] = segment_natural_external_forces + def add_in_local(self, segment_index: int, external_force: np.ndarray, point_in_local: np.ndarray = 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(point_in_local, external_force)) - 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 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,17 +140,161 @@ 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, segment_external_forces in enumerate(self.external_forces): + segment_forces_on_proximal = [] + for external_force in segment_external_forces: + segment_forces_on_proximal += external_force.transport_on_proximal(Q.vector(segment_index)) + + segment_natural_external_forces = np.zeros((12, 1)) + for external_force in segment_forces_on_proximal: + segment_natural_external_forces += external_force.to_generalized_natural_force(Q.vector(segment_index))[ + :, np.newaxis + ] - 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] + slice_index = slice(segment_index * 12, (segment_index + 1) * 12) + natural_external_forces[slice_index, 0:1] = segment_natural_external_forces - return segment_natural_external_forces + return natural_external_forces def __iter__(self): return iter(self.external_forces) def __len__(self): return len(self.external_forces) + + +# class ExternalForceSet: +# """ +# This class is made to handle all the external forces of each segment, if none are provided, it will be an empty list. +# All segment forces are expressed in natural coordinates to be added to the equation of motion as: +# +# Q @ Qddot + K^T @ lambda = Weight + f_ext +# +# Attributes +# ---------- +# external_forces : list +# List of ExternalForces for each segment +# +# Methods +# ------- +# add_external_force(segment_index, external_force) +# This function adds an external force to the list of external forces. +# empty_from_nb_segment(nb_segment) +# This function creates an empty ExternalForceSet from the number of segments. +# to_natural_external_forces(Q) +# This function returns the external forces in the natural coordinate format. +# segment_external_forces(segment_index) +# This function returns the external forces of a segment. +# nb_segments +# This function returns the number of segments. +# +# Examples +# -------- +# >>> from bionc import 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): +# if external_forces is None: +# raise ValueError( +# "f_ext must be a list of ExternalForces, or use the classmethod" +# "NaturalExternalForceSet.empty_from_nb_segment(nb_segment)" +# ) +# self.external_forces = external_forces +# +# @property +# def nb_segments(self) -> int: +# """Returns the number of segments""" +# return len(self.external_forces) +# +# @classmethod +# def empty_from_nb_segment(cls, nb_segment: int): +# """ +# Create an empty NaturalExternalForceSet from the model size +# """ +# return cls(external_forces=[[] for _ in range(nb_segment)]) +# +# def segment_external_forces(self, segment_index: int) -> list[ExternalForce]: +# """Returns the external forces of the segment""" +# return self.external_forces[segment_index] +# +# def add_external_force(self, segment_index: int, external_force: ExternalForce): +# """ +# Add an external force to the segment +# +# Parameters +# ---------- +# segment_index: int +# The index of the segment +# external_force: +# The external force to add +# """ +# self.external_forces[segment_index].append(external_force) +# +# def to_natural_external_forces(self, Q: NaturalCoordinates) -> np.ndarray: +# """ +# Converts and sums all the segment natural external forces to the full vector of natural external forces +# +# Parameters +# ---------- +# Q : NaturalCoordinates +# The natural coordinates of the model +# """ +# +# if len(self.external_forces) != Q.nb_qi(): +# raise ValueError( +# "The number of segment in the model and the number of segment in the external forces must be the same" +# ) +# +# natural_external_forces = np.zeros((12 * Q.nb_qi(), 1)) +# for segment_index, segment_external_forces in enumerate(self.external_forces): +# segment_natural_external_forces = np.zeros((12, 1)) +# slice_index = slice(segment_index * 12, (segment_index + 1) * 12) +# for external_force in segment_external_forces: +# segment_natural_external_forces += external_force.to_natural_force(Q.vector(segment_index))[ +# :, np.newaxis +# ] +# natural_external_forces[slice_index, 0:1] = segment_natural_external_forces +# +# return natural_external_forces +# +# def to_segment_natural_external_forces(self, Q: NaturalCoordinates, segment_index: int) -> np.ndarray: +# """ +# Converts and sums all the segment natural external forces to the full vector of natural external forces +# for one segment +# +# 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(): +# raise ValueError( +# "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") +# +# 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] +# +# return segment_natural_external_forces +# +# def __iter__(self): +# return iter(self.external_forces) +# +# def __len__(self): +# return len(self.external_forces) diff --git a/bionc/bionc_numpy/external_force_global.py b/bionc/bionc_numpy/external_force_global.py new file mode 100644 index 00000000..3f5db247 --- /dev/null +++ b/bionc/bionc_numpy/external_force_global.py @@ -0,0 +1,97 @@ +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) + 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..71f925d5 --- /dev/null +++ b/bionc/bionc_numpy/external_force_global_local_point.py @@ -0,0 +1,101 @@ +import numpy as np + +from .external_force_global_on_proximal import ExternalForceInGlobalOnProximal +from .natural_coordinates import SegmentNaturalCoordinates +from .natural_vector import NaturalVector + + +class ExternalForceGlobalLocalPoint: + """ + 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) + + # Some + new_external_forces = self.external_forces.copy() + new_external_forces[0:3] += additional_torque + + return ExternalForceInGlobalOnProximal(external_forces=new_external_forces) 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..c9958c7d --- /dev/null +++ b/bionc/bionc_numpy/external_force_global_on_proximal.py @@ -0,0 +1,96 @@ +import numpy as np + +from .natural_vector import NaturalVector +from .natural_coordinates import SegmentNaturalCoordinates, NaturalCoordinates + + +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 + + @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 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_force(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) 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..b1aee940 --- /dev/null +++ b/bionc/bionc_numpy/external_force_in_local.py @@ -0,0 +1,133 @@ +import numpy as np + +from .external_force_global_on_proximal import ExternalForceInGlobalOnProximal +from .natural_coordinates import SegmentNaturalCoordinates +from .natural_vector import NaturalVector +from ..utils.enums import TransformationMatrixType + + +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_type : TransformationMatrixType + 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_type: TransformationMatrixType = None, + ): + self.application_point_in_local = application_point_in_local + self.external_forces = external_forces + self.transformation_matrix_type = ( + transformation_matrix_type if transformation_matrix_type is not None else TransformationMatrixType.Buv + ) + + @classmethod + def from_components( + cls, + application_point_in_local: np.ndarray, + force: np.ndarray, + torque: np.ndarray, + transformation_matrix: TransformationMatrixType = None, + ): + """ + 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 : TransformationMatrixType + 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, segment): + rotation_matrix = np.concatenate( + (Qi.u[:, np.newaxis], Qi.v[:, np.newaxis], Qi.w[:, np.newaxis]), axis=1 + ) @ np.linalg.inv(segment.compute_transformation_matrix(self.transformation_matrix_type).T) + + 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, + segment, + ): + """ + Transport the external force to another segment and another application point in cartesian coordinates + + Parameters + ---------- + Qi: SegmentNaturalCoordinates + The natural coordinates of the system + segment + + 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, segment) + additional_torque = np.cross(lever_arm, new_external_forces[3:6]) + + # Sum the additional torque to the existing torque + new_external_forces += additional_torque + + return ExternalForceInGlobalOnProximal(external_forces=new_external_forces) diff --git a/bionc/bionc_numpy/natural_coordinates.py b/bionc/bionc_numpy/natural_coordinates.py index 9ebb6f8e..13aed653 100644 --- a/bionc/bionc_numpy/natural_coordinates.py +++ b/bionc/bionc_numpy/natural_coordinates.py @@ -126,12 +126,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/transformation_matrix.py b/bionc/bionc_numpy/transformation_matrix.py index 2b9a2a4c..2070d94c 100644 --- a/bionc/bionc_numpy/transformation_matrix.py +++ b/bionc/bionc_numpy/transformation_matrix.py @@ -1,7 +1,16 @@ import numpy as np from numpy import cos, sin -from ..utils.enums import NaturalAxis, TransformationMatrixType +from ..utils.enums import TransformationMatrixType + +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( @@ -28,21 +37,12 @@ def compute_transformation_matrix( 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: + + if matrix_type not in TRANSFORMATION_MAP: raise ValueError(f"Unknown TransformationMatrixType: {matrix_type}") + return TRANSFORMATION_MAP[matrix_type](length, alpha, beta, gamma) + def _transformation_matrix_Buv(length: float, alpha: float, beta: float, gamma: float) -> np.ndarray: """ diff --git a/examples/inverse_dynamics/external_force_set.py b/examples/inverse_dynamics/external_force_set.py new file mode 100644 index 00000000..460e7512 --- /dev/null +++ b/examples/inverse_dynamics/external_force_set.py @@ -0,0 +1,23 @@ +from .three_link_pendulum import build_n_link_pendulum + +model = build_n_link_pendulum(nb_segments=3) + +# Options 1 +# external_force_set = ExternalForceSet.empty_from_nb_segment(model.nb_segments) + +# Options 2 +external_force_set = model.external_force_set() + +# add forces and moment +# point_of_application = [0, 0, 0] +# moment_only = [0, 0, 0] +# moment_and_force = [1, 0, 0, 0, 0, 0] + +external_force_set.add_in_global("segment", moment_and_force, point_of_application) +external_force_set.add_in_global_local_point("segment", moment_and_force, point_of_application) +external_force_set.add_in_local("segment", moment_and_force, point_of_application) + + + + + From 5d1cd5d34c15423b5f5e35760872b680fb156b47 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Fri, 29 Nov 2024 15:48:21 -0500 Subject: [PATCH 02/23] refactor: transformation matrix --- bionc/bionc_numpy/transformation_matrix.py | 81 +++++++++++----------- 1 file changed, 41 insertions(+), 40 deletions(-) diff --git a/bionc/bionc_numpy/transformation_matrix.py b/bionc/bionc_numpy/transformation_matrix.py index 2070d94c..38f62b5e 100644 --- a/bionc/bionc_numpy/transformation_matrix.py +++ b/bionc/bionc_numpy/transformation_matrix.py @@ -3,46 +3,6 @@ from ..utils.enums import TransformationMatrixType -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) - 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) From 3f5b1c5d743d8ad9bc94614d982669e822342a13 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Fri, 29 Nov 2024 15:48:43 -0500 Subject: [PATCH 03/23] slowly refactoring --- bionc/bionc_numpy/external_force.py | 31 +++++++++++++++----- bionc/bionc_numpy/external_force_in_local.py | 26 +++++++--------- bionc/bionc_numpy/natural_coordinates.py | 4 +++ 3 files changed, 37 insertions(+), 24 deletions(-) diff --git a/bionc/bionc_numpy/external_force.py b/bionc/bionc_numpy/external_force.py index be218bae..6ab2ebda 100644 --- a/bionc/bionc_numpy/external_force.py +++ b/bionc/bionc_numpy/external_force.py @@ -84,12 +84,12 @@ def add_in_global(self, segment_index: int, external_force: np.ndarray, point_in point_in_global: np.ndarray The point in global coordinates [3 x 1] """ - if point_in_global is None: - self.external_forces[segment_index].append( - ExternalForceInGlobal(application_point_in_global=point_in_global, external_forces=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, ) - else: - self.external_forces[segment_index].append(ExternalForceInGlobalOnProximal(external_force)) + ) def add_in_global_local_point( self, segment_index: int, external_force: np.ndarray, point_in_local: np.ndarray = None @@ -107,10 +107,19 @@ def add_in_global_local_point( The point in global coordinates [3 x 1] """ self.external_forces[segment_index].append( - ExternalForceGlobalLocalPoint(application_point_in_local=point_in_local, external_forces=external_force) + ExternalForceGlobalLocalPoint( + application_point_in_local=point_in_local if point_in_local is not None else np.zeros((3, 1)), + external_forces=external_force, + ) ) - def add_in_local(self, segment_index: int, external_force: np.ndarray, point_in_local: np.ndarray = None): + 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 @@ -123,7 +132,13 @@ def add_in_local(self, segment_index: int, external_force: np.ndarray, point_in_ point_in_local: np.ndarray The point in global coordinates [3 x 1] """ - self.external_forces[segment_index].append(ExternalForceInLocal(point_in_local, external_force)) + 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_natural_external_forces(self, Q: NaturalCoordinates) -> np.ndarray: """ diff --git a/bionc/bionc_numpy/external_force_in_local.py b/bionc/bionc_numpy/external_force_in_local.py index b1aee940..afe47360 100644 --- a/bionc/bionc_numpy/external_force_in_local.py +++ b/bionc/bionc_numpy/external_force_in_local.py @@ -3,7 +3,6 @@ from .external_force_global_on_proximal import ExternalForceInGlobalOnProximal from .natural_coordinates import SegmentNaturalCoordinates from .natural_vector import NaturalVector -from ..utils.enums import TransformationMatrixType class ExternalForceInLocal: @@ -16,7 +15,7 @@ class ExternalForceInLocal: 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_type : TransformationMatrixType + transformation_matrix : np.ndarray The transformation matrix of the segment Methods @@ -37,13 +36,12 @@ def __init__( self, application_point_in_local: np.ndarray, external_forces: np.ndarray, - transformation_matrix_type: TransformationMatrixType = None, + transformation_matrix: np.ndarray, ): self.application_point_in_local = application_point_in_local self.external_forces = external_forces - self.transformation_matrix_type = ( - transformation_matrix_type if transformation_matrix_type is not None else TransformationMatrixType.Buv - ) + self.transformation_matrix = transformation_matrix + self.transformation_matrix_inv = np.linalg.inv(self.transformation_matrix) @classmethod def from_components( @@ -51,7 +49,7 @@ def from_components( application_point_in_local: np.ndarray, force: np.ndarray, torque: np.ndarray, - transformation_matrix: TransformationMatrixType = None, + transformation_matrix: np.ndarray, ): """ This function creates an external force from its components. @@ -64,7 +62,7 @@ def from_components( The force vector in the global coordinate system torque The torque vector in the global coordinate system - transformation_matrix : TransformationMatrixType + transformation_matrix : np.ndarray The transformation matrix of the segment Returns @@ -84,10 +82,8 @@ 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, segment): - rotation_matrix = np.concatenate( - (Qi.u[:, np.newaxis], Qi.v[:, np.newaxis], Qi.w[:, np.newaxis]), axis=1 - ) @ np.linalg.inv(segment.compute_transformation_matrix(self.transformation_matrix_type).T) + 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 @@ -97,7 +93,6 @@ def forces_in_global(self, Qi: SegmentNaturalCoordinates, segment): def transport_on_proximal( self, Qi: SegmentNaturalCoordinates, - segment, ): """ Transport the external force to another segment and another application point in cartesian coordinates @@ -106,7 +101,6 @@ def transport_on_proximal( ---------- Qi: SegmentNaturalCoordinates The natural coordinates of the system - segment Returns ------- @@ -124,10 +118,10 @@ def transport_on_proximal( # 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, segment) + 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 += additional_torque + new_external_forces[0:3] += additional_torque return ExternalForceInGlobalOnProximal(external_forces=new_external_forces) diff --git a/bionc/bionc_numpy/natural_coordinates.py b/bionc/bionc_numpy/natural_coordinates.py index 13aed653..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 From 051bd7e256358ee0d550a50841d2f16da420de7b Mon Sep 17 00:00:00 2001 From: Ipuch Date: Fri, 29 Nov 2024 17:13:37 -0500 Subject: [PATCH 04/23] insane refactor to get rid of circular import --- bionc/__init__.py | 44 +++++++++---------- bionc/bionc_numpy/__init__.py | 31 +++++++------ bionc/bionc_numpy/external_force.py | 6 +-- .../external_force_global_local_point.py | 2 +- .../biomechanical_model_template.py | 9 ++-- .../inertia_parameters_template.py | 11 +++-- bionc/model_creation/marker_template.py | 21 ++++----- bionc/model_creation/natural_axis_template.py | 10 ++--- .../natural_segment_template.py | 12 +++-- bionc/protocols/natural_markers.py | 2 +- bionc/utils/c3d_ik_exporter.py | 13 +++--- 11 files changed, 79 insertions(+), 82 deletions(-) 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_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/external_force.py b/bionc/bionc_numpy/external_force.py index 6ab2ebda..a1b4864a 100644 --- a/bionc/bionc_numpy/external_force.py +++ b/bionc/bionc_numpy/external_force.py @@ -3,13 +3,13 @@ import numpy as np from .external_force_global import ExternalForceInGlobal -from .external_force_global_local_point import ExternalForceGlobalLocalPoint +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 ExternalForce = Union[ - ExternalForceInGlobal, ExternalForceInGlobalOnProximal, ExternalForceGlobalLocalPoint, ExternalForceInLocal + ExternalForceInGlobal, ExternalForceInGlobalOnProximal, ExternalForceInGlobalLocalPoint, ExternalForceInLocal ] @@ -107,7 +107,7 @@ def add_in_global_local_point( The point in global coordinates [3 x 1] """ self.external_forces[segment_index].append( - ExternalForceGlobalLocalPoint( + ExternalForceInGlobalLocalPoint( application_point_in_local=point_in_local if point_in_local is not None else np.zeros((3, 1)), external_forces=external_force, ) diff --git a/bionc/bionc_numpy/external_force_global_local_point.py b/bionc/bionc_numpy/external_force_global_local_point.py index 71f925d5..1f5aa7ab 100644 --- a/bionc/bionc_numpy/external_force_global_local_point.py +++ b/bionc/bionc_numpy/external_force_global_local_point.py @@ -5,7 +5,7 @@ from .natural_vector import NaturalVector -class ExternalForceGlobalLocalPoint: +class ExternalForceInGlobalLocalPoint: """ This class represents an external force applied to a segment. 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..fa606e3b 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): 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()): From 7a36101a8dcbf19d6969444bef553a513c3c763b Mon Sep 17 00:00:00 2001 From: Ipuch Date: Fri, 29 Nov 2024 18:45:45 -0500 Subject: [PATCH 05/23] tests: restored numpy --- bionc/bionc_numpy/external_force.py | 8 +- .../external_force_global_on_proximal.py | 6 +- tests/test_external_force.py | 189 +++++++++--------- 3 files changed, 102 insertions(+), 101 deletions(-) diff --git a/bionc/bionc_numpy/external_force.py b/bionc/bionc_numpy/external_force.py index a1b4864a..54d00448 100644 --- a/bionc/bionc_numpy/external_force.py +++ b/bionc/bionc_numpy/external_force.py @@ -159,13 +159,13 @@ def to_natural_external_forces(self, Q: NaturalCoordinates) -> np.ndarray: for segment_index, segment_external_forces in enumerate(self.external_forces): segment_forces_on_proximal = [] for external_force in segment_external_forces: - segment_forces_on_proximal += external_force.transport_on_proximal(Q.vector(segment_index)) + segment_forces_on_proximal += [external_force.transport_on_proximal(Q.vector(segment_index))] segment_natural_external_forces = np.zeros((12, 1)) for external_force in segment_forces_on_proximal: - segment_natural_external_forces += external_force.to_generalized_natural_force(Q.vector(segment_index))[ - :, np.newaxis - ] + segment_natural_external_forces += external_force.to_generalized_natural_forces( + Q.vector(segment_index) + )[:, np.newaxis] slice_index = slice(segment_index * 12, (segment_index + 1) * 12) natural_external_forces[slice_index, 0:1] = segment_natural_external_forces diff --git a/bionc/bionc_numpy/external_force_global_on_proximal.py b/bionc/bionc_numpy/external_force_global_on_proximal.py index c9958c7d..8ea6474d 100644 --- a/bionc/bionc_numpy/external_force_global_on_proximal.py +++ b/bionc/bionc_numpy/external_force_global_on_proximal.py @@ -1,7 +1,7 @@ import numpy as np +from .natural_coordinates import SegmentNaturalCoordinates from .natural_vector import NaturalVector -from .natural_coordinates import SegmentNaturalCoordinates, NaturalCoordinates class ExternalForceInGlobalOnProximal: @@ -59,7 +59,7 @@ def natural_forces(self) -> np.ndarray: """ point_interpolation_matrix = NaturalVector.proximal().interpolate() - return point_interpolation_matrix.T @ self.force + return np.array(point_interpolation_matrix.T @ self.force) def natural_moments(self, Qi: SegmentNaturalCoordinates) -> np.ndarray: """ @@ -79,7 +79,7 @@ def natural_moments(self, Qi: SegmentNaturalCoordinates) -> np.ndarray: return pseudo_interpolation_matrix.T @ self.torque - def to_generalized_natural_force(self, Qi: SegmentNaturalCoordinates) -> np.ndarray: + 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. diff --git a/tests/test_external_force.py b/tests/test_external_force.py index f977d004..f609da05 100644 --- a/tests/test_external_force.py +++ b/tests/test_external_force.py @@ -1,6 +1,7 @@ +import numpy as np import pytest + from .utils import TestUtils -import numpy as np @pytest.mark.parametrize( @@ -19,7 +20,7 @@ def test_external_force(bionc_type, external_force_tuple): from bionc.bionc_numpy import ( ExternalForceSet, - ExternalForce, + # ExternalForceGlobalLocalPoint, 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[0], external_force_tuple[1]]) + 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) @@ -63,7 +62,7 @@ def test_external_force(bionc_type): if bionc_type == "numpy": from bionc.bionc_numpy import ( ExternalForceSet, - ExternalForce, + ExternalForceInGlobalLocalPoint, SegmentNaturalCoordinates, NaturalCoordinates, SegmentNaturalVelocities, @@ -72,19 +71,18 @@ def test_external_force(bionc_type): else: from bionc.bionc_casadi import ( ExternalForceSet, - ExternalForce, 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]), @@ -96,8 +94,16 @@ def test_external_force(bionc_type): TestUtils.assert_equal(force2.force, np.array([0.11, 0.12, 0.13])) 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]), + 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]), + 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 +117,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 +169,90 @@ 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.177629, + 0.0, + 0.142669, + 0.152669, + 0.326011, + -0.032669, + -0.032669, + -0.196011, + 0.130834, + 0.021806, + 0.021806, + ] + ) TestUtils.assert_equal( natural_force, - np.array( - [ - 0.0187, - 0.19897143, - 0.0221, - 0.16265714, - 0.17445714, - 0.35054286, - -0.05265714, - -0.05445714, - -0.22054286, - 0.14947143, - 0.04422857, - 0.04612857, - ] - ), + natural_force_2_expected, expand=False, ) + 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, - 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], - ] - ), + 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, - ) - - 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] - ), - expand=False, - ) - - fext.add_external_force(external_force=force2, segment_index=2) - segment_force_2 = fext.to_segment_natural_external_forces(Q=Q, segment_index=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, - expand=False, - squeeze=True, - ) + # 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, + # ) + # + # 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] + # ), + # expand=False, + # ) + # + # fext.add_external_force(external_force=force2, segment_index=2) + # segment_force_2 = fext.to_segment_natural_external_forces(Q=Q, segment_index=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, + # expand=False, + # squeeze=True, + # ) From 237eb823498bd81c6513b19b96e9c1a67aef5625 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Fri, 29 Nov 2024 23:19:14 -0500 Subject: [PATCH 06/23] refactor: restoring foward dynamics + force + only one works --- .../forward_dynamics/pendulum_with_force.py | 42 +++++++++---------- 1 file changed, 20 insertions(+), 22 deletions(-) diff --git a/examples/forward_dynamics/pendulum_with_force.py b/examples/forward_dynamics/pendulum_with_force.py index a32b0787..f43cd4e6 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 From 062c519920f9cf8d2fdc0b7cb852919ceddca8f3 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Fri, 29 Nov 2024 23:32:48 -0500 Subject: [PATCH 07/23] example + test: one pendulum with force fixed --- bionc/protocols/natural_markers.py | 3 - .../forward_dynamics/pendulum_with_force.py | 4 +- tests/test_ultimate_examples.py | 80 +++++++++---------- 3 files changed, 40 insertions(+), 47 deletions(-) diff --git a/bionc/protocols/natural_markers.py b/bionc/protocols/natural_markers.py index fa606e3b..ae5d8378 100644 --- a/bionc/protocols/natural_markers.py +++ b/bionc/protocols/natural_markers.py @@ -21,9 +21,6 @@ class AbstractNaturalMarker(ABC): """ - def __init__(self): - self.name = None - @abstractmethod def from_data( cls, diff --git a/examples/forward_dynamics/pendulum_with_force.py b/examples/forward_dynamics/pendulum_with_force.py index f43cd4e6..29a77093 100644 --- a/examples/forward_dynamics/pendulum_with_force.py +++ b/examples/forward_dynamics/pendulum_with_force.py @@ -184,7 +184,7 @@ def main(mode: str = "moment_equilibrium"): fext.add_in_global_local_point( external_force=np.concatenate([torque, force]), segment_index=0, - point_in_local=np.array([0, -0.5, 0]), + point_in_local=np.array([0, 0.5, 0]), ) elif mode == "no_equilibrium": @@ -192,7 +192,7 @@ def main(mode: str = "moment_equilibrium"): 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]), + point_in_local=np.array([0, 0.25, 0]), ) else: 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], ) From 53f273e023468650688d60ce4631250495e3fae4 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Fri, 29 Nov 2024 23:42:07 -0500 Subject: [PATCH 08/23] example: double pendulum with force fixed hell yeah --- .../double_pendulum_with_force.py | 41 +++++-------------- 1 file changed, 11 insertions(+), 30 deletions(-) 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 From 56cb7d52b7ae2d8c075ea3efbed831bf1ebcdf73 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Fri, 29 Nov 2024 23:47:33 -0500 Subject: [PATCH 09/23] refactor: to simplify a loop of external force to generalized forces --- bionc/bionc_numpy/external_force.py | 6 +----- bionc/bionc_numpy/external_force_global.py | 3 +++ bionc/bionc_numpy/external_force_global_local_point.py | 4 ++++ bionc/bionc_numpy/external_force_in_local.py | 4 ++++ 4 files changed, 12 insertions(+), 5 deletions(-) diff --git a/bionc/bionc_numpy/external_force.py b/bionc/bionc_numpy/external_force.py index 54d00448..8b20c81d 100644 --- a/bionc/bionc_numpy/external_force.py +++ b/bionc/bionc_numpy/external_force.py @@ -157,12 +157,8 @@ def to_natural_external_forces(self, Q: NaturalCoordinates) -> np.ndarray: natural_external_forces = np.zeros((12 * Q.nb_qi(), 1)) for segment_index, segment_external_forces in enumerate(self.external_forces): - segment_forces_on_proximal = [] - for external_force in segment_external_forces: - segment_forces_on_proximal += [external_force.transport_on_proximal(Q.vector(segment_index))] - segment_natural_external_forces = np.zeros((12, 1)) - for external_force in segment_forces_on_proximal: + for external_force in segment_external_forces: segment_natural_external_forces += external_force.to_generalized_natural_forces( Q.vector(segment_index) )[:, np.newaxis] diff --git a/bionc/bionc_numpy/external_force_global.py b/bionc/bionc_numpy/external_force_global.py index 3f5db247..dd4c1a8f 100644 --- a/bionc/bionc_numpy/external_force_global.py +++ b/bionc/bionc_numpy/external_force_global.py @@ -95,3 +95,6 @@ def transport_on_proximal( 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 index 1f5aa7ab..07746de9 100644 --- a/bionc/bionc_numpy/external_force_global_local_point.py +++ b/bionc/bionc_numpy/external_force_global_local_point.py @@ -99,3 +99,7 @@ def transport_on_proximal( 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_in_local.py b/bionc/bionc_numpy/external_force_in_local.py index afe47360..925e9f13 100644 --- a/bionc/bionc_numpy/external_force_in_local.py +++ b/bionc/bionc_numpy/external_force_in_local.py @@ -125,3 +125,7 @@ def transport_on_proximal( 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) From afd93facbe5dfaf45c4846eb8d74ed23754d78ff Mon Sep 17 00:00:00 2001 From: Ipuch Date: Sat, 30 Nov 2024 08:31:36 -0500 Subject: [PATCH 10/23] hell yeah this works ! --- bionc/bionc_numpy/biomechanical_model.py | 28 +++++----- bionc/bionc_numpy/external_force.py | 25 ++++++--- .../external_force_global_on_proximal.py | 51 +++++++++++++++++++ 3 files changed, 83 insertions(+), 21 deletions(-) diff --git a/bionc/bionc_numpy/biomechanical_model.py b/bionc/bionc_numpy/biomechanical_model.py index 046c8b1b..c445d0a7 100644 --- a/bionc/bionc_numpy/biomechanical_model.py +++ b/bionc/bionc_numpy/biomechanical_model.py @@ -7,8 +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 .generalized_force import JointGeneralizedForcesList +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 @@ -553,8 +553,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 +570,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 8b20c81d..9d1b20d9 100644 --- a/bionc/bionc_numpy/external_force.py +++ b/bionc/bionc_numpy/external_force.py @@ -142,7 +142,8 @@ def add_in_local( 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 + Converts and sums all natural external forces + to the full vector of natural external forces [nb_segment * 12 x 1] Parameters ---------- @@ -156,18 +157,28 @@ def to_natural_external_forces(self, Q: NaturalCoordinates) -> np.ndarray: ) 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)) - for external_force in segment_external_forces: - segment_natural_external_forces += external_force.to_generalized_natural_forces( - Q.vector(segment_index) - )[:, np.newaxis] + 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)) + 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 + def __iter__(self): return iter(self.external_forces) diff --git a/bionc/bionc_numpy/external_force_global_on_proximal.py b/bionc/bionc_numpy/external_force_global_on_proximal.py index 8ea6474d..0701d2d5 100644 --- a/bionc/bionc_numpy/external_force_global_on_proximal.py +++ b/bionc/bionc_numpy/external_force_global_on_proximal.py @@ -33,6 +33,25 @@ class ExternalForceInGlobalOnProximal: 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""" @@ -94,3 +113,35 @@ def to_generalized_natural_forces(self, Qi: SegmentNaturalCoordinates) -> np.nda 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) + ) From 4068ece05f1657b098cbe0ec9074dfb75d961ca8 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Sat, 30 Nov 2024 08:46:48 -0500 Subject: [PATCH 11/23] test: one more for fext --- bionc/bionc_numpy/natural_segment.py | 8 +++++ tests/test_external_force.py | 52 ++++++++++++++++------------ 2 files changed, 37 insertions(+), 23 deletions(-) diff --git a/bionc/bionc_numpy/natural_segment.py b/bionc/bionc_numpy/natural_segment.py index 2692bb57..962ec3fc 100644 --- a/bionc/bionc_numpy/natural_segment.py +++ b/bionc/bionc_numpy/natural_segment.py @@ -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/tests/test_external_force.py b/tests/test_external_force.py index f609da05..ec485254 100644 --- a/tests/test_external_force.py +++ b/tests/test_external_force.py @@ -233,26 +233,32 @@ def test_external_force(bionc_type): 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, - # ) - # - # 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] - # ), - # expand=False, - # ) - # - # fext.add_external_force(external_force=force2, segment_index=2) - # segment_force_2 = fext.to_segment_natural_external_forces(Q=Q, segment_index=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, - # expand=False, - # 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.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, + ) From 89800c3d0d54e2adf16e5fa640d5296b1fa4ee75 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Sat, 30 Nov 2024 09:08:37 -0500 Subject: [PATCH 12/23] tests: and once more ! --- tests/test_external_force.py | 33 +++++++++++++++++++++++---------- 1 file changed, 23 insertions(+), 10 deletions(-) diff --git a/tests/test_external_force.py b/tests/test_external_force.py index ec485254..927913c1 100644 --- a/tests/test_external_force.py +++ b/tests/test_external_force.py @@ -173,17 +173,17 @@ def test_external_force(bionc_type): natural_force_2_expected = np.array( [ 0.0, - 0.177629, + 0.17762857, 0.0, - 0.142669, - 0.152669, - 0.326011, - -0.032669, - -0.032669, - -0.196011, - 0.130834, - 0.021806, - 0.021806, + 0.14266857, + 0.15266857, + 0.32601143, + -0.03266857, + -0.03266857, + -0.19601143, + 0.13083429, + 0.02180571, + 0.02180571, ] ) TestUtils.assert_equal( @@ -262,3 +262,16 @@ def test_external_force(bionc_type): ), expand=False, ) + + fext.add_in_global_local_point( + segment_index=2, + external_force=np.concatenate([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, + ) From 67e0127f125a635fa5b72bf53a3df80e554ca7c4 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Sat, 30 Nov 2024 09:30:16 -0500 Subject: [PATCH 13/23] more tests --- tests/test_external_force.py | 340 ++++++++++++++++++++++++++++++++++- 1 file changed, 339 insertions(+), 1 deletion(-) diff --git a/tests/test_external_force.py b/tests/test_external_force.py index 927913c1..b88b14ee 100644 --- a/tests/test_external_force.py +++ b/tests/test_external_force.py @@ -58,7 +58,7 @@ 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, @@ -275,3 +275,341 @@ def test_external_force(bionc_type): 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, + 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])) + TestUtils.assert_equal(force1.force, np.array([0.01, 0.02, 0.03])) + TestUtils.assert_equal(force2.torque, np.array([0.14, 0.15, 0.16])) + TestUtils.assert_equal(force2.force, np.array([0.11, 0.12, 0.13])) + + fext = ExternalForceSet.empty_from_nb_segment(3) + fext.add_in_global( + external_force=np.concatenate([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]), + 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, + ) + + 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.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, + ) + + fext.add_in_global( + segment_index=2, + external_force=np.concatenate([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, + SegmentNaturalCoordinates, + NaturalCoordinates, + SegmentNaturalVelocities, + NaturalVelocities, + ) + dummy_transformation_matrix = np.eye(3) + np.ones((3, 3)) * 0.001 + 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])) + TestUtils.assert_equal(force1.force, np.array([0.01, 0.02, 0.03])) + TestUtils.assert_equal(force2.torque, np.array([0.14, 0.15, 0.16])) + TestUtils.assert_equal(force2.force, np.array([0.11, 0.12, 0.13])) + + fext = ExternalForceSet.empty_from_nb_segment(3) + fext.add_in_local( + external_force=np.concatenate([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]), + 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.24582142, + 0.0, + 0.20380465, + 0.21380465, + 0.45534036, + -0.04630714, + -0.04630714, + -0.27784285, + 0.18091023, + 0.0301517, + 0.0301517, + ] + ) + TestUtils.assert_equal( + natural_force, + natural_force_2_expected, + expand=False, + ) + + natural_forces = fext.to_natural_external_forces(Q) + complete_natural_force_expected = np.concatenate( + ( + np.array( + [ + [0.0], + [0.05924985], + [0.0], + [0.00994018], + [0.01994018], + [0.06919003], + [0.0], + [0.0], + [-0.03924985], + [0.05105165], + [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. , 0.23361535, 0. , 0.17919238, 0.18919238, + 0.41614106, -0.02169487, -0.02169487, -0.23864356, 0.19530677, + 0.01775516, 0.01775516]), + expand=False, + ) + + fext.add_in_local( + segment_index=2, + external_force=np.concatenate([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_expected * 2) if bionc_type == "numpy" else natural_force_2_expected * 2.0, + expand=False, + squeeze=True, + ) From ceb9331851e27b7b1f12ee76502931bd3884a373 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Sun, 1 Dec 2024 20:25:48 -0500 Subject: [PATCH 14/23] example: extra id with external forces --- .../inverse_dynamics/three_link_pendulum.py | 18 ++++-- tests/test_external_force.py | 19 +++++- tests/test_inverse_dynamics.py | 61 +++++++++++++++++-- 3 files changed, 85 insertions(+), 13 deletions(-) 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/tests/test_external_force.py b/tests/test_external_force.py index b88b14ee..8625e2b1 100644 --- a/tests/test_external_force.py +++ b/tests/test_external_force.py @@ -594,9 +594,22 @@ def test_external_force_in_local(bionc_type): TestUtils.assert_equal( new_natural_force, - np.array([ 0. , 0.23361535, 0. , 0.17919238, 0.18919238, - 0.41614106, -0.02169487, -0.02169487, -0.23864356, 0.19530677, - 0.01775516, 0.01775516]), + np.array( + [ + 0.0, + 0.23361535, + 0.0, + 0.17919238, + 0.18919238, + 0.41614106, + -0.02169487, + -0.02169487, + -0.23864356, + 0.19530677, + 0.01775516, + 0.01775516, + ] + ), expand=False, ) 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, + ) From a1b59ec148002de8426bef0481b0c966223f3427 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Sun, 1 Dec 2024 20:31:01 -0500 Subject: [PATCH 15/23] tests: fixed --- tests/test_external_force.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tests/test_external_force.py b/tests/test_external_force.py index 8625e2b1..d7bdf567 100644 --- a/tests/test_external_force.py +++ b/tests/test_external_force.py @@ -13,7 +13,7 @@ @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])), ], ) @@ -31,7 +31,7 @@ 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) - external_force = np.concatenate([external_force_tuple[0], external_force_tuple[1]]) + 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] ) From a1363c47b0e7b87ce4b2c4f58f6bf862e3302597 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Sun, 1 Dec 2024 21:12:19 -0500 Subject: [PATCH 16/23] fix and extra tests: missing transpose in transofrmation matrix --- bionc/bionc_casadi/external_force_global.py | 100 ++++++++++++ .../external_force_global_local_point.py | 105 +++++++++++++ .../external_force_global_on_proximal.py | 147 ++++++++++++++++++ bionc/bionc_casadi/external_force_in_local.py | 131 ++++++++++++++++ bionc/bionc_numpy/external_force_in_local.py | 2 +- bionc/bionc_numpy/natural_segment.py | 2 +- tests/test_external_force.py | 54 +++---- tests/test_natural_segment.py | 24 +++ 8 files changed, 536 insertions(+), 29 deletions(-) create mode 100644 bionc/bionc_casadi/external_force_global.py create mode 100644 bionc/bionc_casadi/external_force_global_local_point.py create mode 100644 bionc/bionc_casadi/external_force_global_on_proximal.py create mode 100644 bionc/bionc_casadi/external_force_in_local.py diff --git a/bionc/bionc_casadi/external_force_global.py b/bionc/bionc_casadi/external_force_global.py new file mode 100644 index 00000000..dd4c1a8f --- /dev/null +++ b/bionc/bionc_casadi/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_casadi/external_force_global_local_point.py b/bionc/bionc_casadi/external_force_global_local_point.py new file mode 100644 index 00000000..07746de9 --- /dev/null +++ b/bionc/bionc_casadi/external_force_global_local_point.py @@ -0,0 +1,105 @@ +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) + + # Some + 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_casadi/external_force_global_on_proximal.py b/bionc/bionc_casadi/external_force_global_on_proximal.py new file mode 100644 index 00000000..0701d2d5 --- /dev/null +++ b/bionc/bionc_casadi/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_casadi/external_force_in_local.py b/bionc/bionc_casadi/external_force_in_local.py new file mode 100644 index 00000000..925e9f13 --- /dev/null +++ b/bionc/bionc_casadi/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) + + @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/external_force_in_local.py b/bionc/bionc_numpy/external_force_in_local.py index 925e9f13..6724acbe 100644 --- a/bionc/bionc_numpy/external_force_in_local.py +++ b/bionc/bionc_numpy/external_force_in_local.py @@ -41,7 +41,7 @@ def __init__( 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) + self.transformation_matrix_inv = np.linalg.inv(self.transformation_matrix.T) @classmethod def from_components( diff --git a/bionc/bionc_numpy/natural_segment.py b/bionc/bionc_numpy/natural_segment.py index 962ec3fc..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], diff --git a/tests/test_external_force.py b/tests/test_external_force.py index d7bdf567..0b3b2853 100644 --- a/tests/test_external_force.py +++ b/tests/test_external_force.py @@ -20,7 +20,6 @@ def test_external_force(bionc_type, external_force_tuple): from bionc.bionc_numpy import ( ExternalForceSet, - # ExternalForceGlobalLocalPoint, SegmentNaturalCoordinates, NaturalCoordinates, SegmentNaturalVelocities, @@ -476,6 +475,7 @@ def test_external_force_in_local(bionc_type): 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]), @@ -523,17 +523,17 @@ def test_external_force_in_local(bionc_type): natural_force_2_expected = np.array( [ 0.0, - 0.24582142, + 0.24572225, 0.0, - 0.20380465, - 0.21380465, - 0.45534036, - -0.04630714, - -0.04630714, - -0.27784285, - 0.18091023, - 0.0301517, - 0.0301517, + 0.20196297, + 0.18615927, + 0.44411017, + -0.04442944, + -0.04442944, + -0.26657665, + 0.1537273, + 0.02562122, + 0.02562122, ] ) TestUtils.assert_equal( @@ -548,15 +548,15 @@ def test_external_force_in_local(bionc_type): np.array( [ [0.0], - [0.05924985], + [0.05967894], [0.0], - [0.00994018], - [0.01994018], - [0.06919003], + [0.00994612], + [0.01398684], + [0.06867157], [0.0], [0.0], - [-0.03924985], - [0.05105165], + [-0.03872545], + [0.0391508], [0.0], [0.0], [0.0], @@ -597,17 +597,17 @@ def test_external_force_in_local(bionc_type): np.array( [ 0.0, - 0.23361535, + 0.2383283, 0.0, - 0.17919238, - 0.18919238, - 0.41614106, - -0.02169487, - -0.02169487, - -0.23864356, - 0.19530677, - 0.01775516, - 0.01775516, + 0.17818587, + 0.16238218, + 0.40470934, + -0.02065235, + -0.02065235, + -0.22717582, + 0.16623615, + 0.01511238, + 0.01511238, ] ), 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", From 86316f765e0456194fcce041d3178a52dc125d4a Mon Sep 17 00:00:00 2001 From: Ipuch Date: Sun, 1 Dec 2024 21:37:12 -0500 Subject: [PATCH 17/23] feat: casadi quest --- bionc/bionc_casadi/external_force.py | 252 ++++++------------ bionc/bionc_casadi/external_force_global.py | 24 +- .../external_force_global_local_point.py | 34 +-- .../external_force_global_on_proximal.py | 46 ++-- bionc/bionc_casadi/external_force_in_local.py | 49 ++-- bionc/bionc_casadi/natural_coordinates.py | 6 +- bionc/bionc_casadi/natural_segment.py | 2 +- bionc/bionc_numpy/external_force.py | 136 ---------- tests/test_external_force.py | 37 ++- tests/utils.py | 9 +- 10 files changed, 190 insertions(+), 405 deletions(-) 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 index dd4c1a8f..42377ea3 100644 --- a/bionc/bionc_casadi/external_force_global.py +++ b/bionc/bionc_casadi/external_force_global.py @@ -1,4 +1,4 @@ -import numpy as np +from casadi import MX, vertcat, cross from .external_force_global_on_proximal import ExternalForceInGlobalOnProximal from .natural_coordinates import SegmentNaturalCoordinates @@ -10,9 +10,9 @@ class ExternalForceInGlobal: Attributes ---------- - application_point_in_local : np.ndarray + application_point_in_local : MX The application point of the force in the natural coordinate system of the segment - external_forces : np.ndarray + external_forces : MX The external force vector in the global coordinate system (torque, force) Methods @@ -29,18 +29,18 @@ class ExternalForceInGlobal: 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 + 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: np.ndarray, force: np.ndarray, torque: np.ndarray): + 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 : np.ndarray + 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 @@ -52,15 +52,15 @@ def from_components(cls, application_point_in_global: np.ndarray, force: np.ndar ExternalForce """ - return cls(application_point_in_global, np.concatenate((torque, force))) + return cls(application_point_in_global, vertcat(torque, force)) @property - def force(self) -> np.ndarray: + def force(self) -> MX: """The force vector in the global coordinate system""" return self.external_forces[3:6] @property - def torque(self) -> np.ndarray: + def torque(self) -> MX: """The torque vector in the global coordinate system""" return self.external_forces[0:3] @@ -87,7 +87,7 @@ def transport_on_proximal( # 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) + additional_torque = cross(lever_arm, self.force) # Sum the additional torque to the existing torque new_external_forces = self.external_forces.copy() diff --git a/bionc/bionc_casadi/external_force_global_local_point.py b/bionc/bionc_casadi/external_force_global_local_point.py index 07746de9..f340bd77 100644 --- a/bionc/bionc_casadi/external_force_global_local_point.py +++ b/bionc/bionc_casadi/external_force_global_local_point.py @@ -1,4 +1,4 @@ -import numpy as np +from casadi import MX, vertcat, cross from .external_force_global_on_proximal import ExternalForceInGlobalOnProximal from .natural_coordinates import SegmentNaturalCoordinates @@ -11,9 +11,9 @@ class ExternalForceInGlobalLocalPoint: Attributes ---------- - application_point_in_local : np.ndarray + application_point_in_local : MX The application point of the force in the natural coordinate system of the segment - external_forces : np.ndarray + external_forces : MX The external force vector in the global coordinate system (torque, force) Methods @@ -30,18 +30,18 @@ class ExternalForceInGlobalLocalPoint: 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 + 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: np.ndarray, force: np.ndarray, torque: np.ndarray): + 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 : np.ndarray + 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 @@ -53,15 +53,15 @@ def from_components(cls, application_point_in_local: np.ndarray, force: np.ndarr ExternalForce """ - return cls(application_point_in_local, np.concatenate((torque, force))) + return cls(application_point_in_local, vertcat(torque, force)) @property - def force(self) -> np.ndarray: + def force(self) -> MX: """The force vector in the global coordinate system""" return self.external_forces[3:6] @property - def torque(self) -> np.ndarray: + def torque(self) -> MX: """The torque vector in the global coordinate system""" return self.external_forces[0:3] @@ -82,24 +82,24 @@ def transport_on_proximal( ExternalForceInGlobalOnProximal The external force on the proximal point of the segment """ - qi_array = np.array(Qi).squeeze() + 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 = np.array(old_point_interpolation_matrix @ qi_array).squeeze() - new_application_point_in_global = np.array(new_point_interpolation_matrix @ qi_array).squeeze() + 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 = np.cross(lever_arm, self.force) + additional_torque = cross(lever_arm, self.force) # Some - new_external_forces = self.external_forces.copy() + new_external_forces = self.external_forces new_external_forces[0:3] += additional_torque return ExternalForceInGlobalOnProximal(external_forces=new_external_forces) - def to_generalized_natural_forces(self, Qi: SegmentNaturalCoordinates) -> np.ndarray: + 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 index 0701d2d5..abfbb623 100644 --- a/bionc/bionc_casadi/external_force_global_on_proximal.py +++ b/bionc/bionc_casadi/external_force_global_on_proximal.py @@ -1,4 +1,4 @@ -import numpy as np +from casadi import MX, vertcat, cross from .natural_coordinates import SegmentNaturalCoordinates from .natural_vector import NaturalVector @@ -11,7 +11,7 @@ class ExternalForceInGlobalOnProximal: Attributes ---------- - external_forces : np.ndarray + external_forces : MX The external force vector in the global coordinate system (torque, force) Methods @@ -30,11 +30,11 @@ class ExternalForceInGlobalOnProximal: 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 + def __init__(self, external_forces: MX): + self.external_forces = MX(external_forces) @classmethod - def from_components(cls, force: np.ndarray, torque: np.ndarray): + def from_components(cls, force: MX, torque: MX): """ This function creates an external force from its components. @@ -50,19 +50,19 @@ def from_components(cls, force: np.ndarray, torque: np.ndarray): ExternalForce """ - return cls(np.concatenate((torque, force))) + return cls(vertcat(torque, force)) @property - def force(self) -> np.ndarray: + def force(self) -> MX: """The cartesian force vector in the global coordinate system""" return self.external_forces[3:6] @property - def torque(self) -> np.ndarray: + def torque(self) -> MX: """The cartesian torque vector in the global coordinate system""" return self.external_forces[0:3] - def natural_forces(self) -> np.ndarray: + def natural_forces(self) -> MX: """ Apply external forces to the segment @@ -73,14 +73,14 @@ def natural_forces(self) -> np.ndarray: Returns ------- - np.ndarray + MX 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) + return point_interpolation_matrix.T @ self.force - def natural_moments(self, Qi: SegmentNaturalCoordinates) -> np.ndarray: + def natural_moments(self, Qi: SegmentNaturalCoordinates) -> MX: """ Apply external moments to the segment @@ -91,14 +91,14 @@ def natural_moments(self, Qi: SegmentNaturalCoordinates) -> np.ndarray: Returns ------- - np.ndarray + 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 pseudo_interpolation_matrix.T @ self.torque - def to_generalized_natural_forces(self, Qi: SegmentNaturalCoordinates) -> np.ndarray: + def to_generalized_natural_forces(self, Qi: SegmentNaturalCoordinates) -> MX: """ Format external moments and forces to the generalized external force in the natural coordinate format. @@ -109,14 +109,12 @@ def to_generalized_natural_forces(self, Qi: SegmentNaturalCoordinates) -> np.nda Returns ------- - np.ndarray + 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 - ) -> np.ndarray: + 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 @@ -132,16 +130,14 @@ def transport_to_another_segment( ExternalForceInGlobalOnProximal The external force on the proximal point of the segment """ - qi_array = np.array(Qfrom).squeeze() - qj_array = np.array(Qto).squeeze() + qi_array = Qfrom + qj_array = Qto 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() + 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 + np.cross(lever_arm, self.force) - ) + 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 index 925e9f13..c9d28e7a 100644 --- a/bionc/bionc_casadi/external_force_in_local.py +++ b/bionc/bionc_casadi/external_force_in_local.py @@ -1,8 +1,9 @@ -import numpy as np +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: @@ -34,35 +35,37 @@ class ExternalForceInLocal: def __init__( self, - application_point_in_local: np.ndarray, - external_forces: np.ndarray, - transformation_matrix: np.ndarray, + application_point_in_local: MX, + external_forces: MX, + transformation_matrix: MX, ): - 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) + 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: np.ndarray, - force: np.ndarray, - torque: np.ndarray, - transformation_matrix: np.ndarray, + 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 : np.ndarray + 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 : np.ndarray + transformation_matrix : MX The transformation matrix of the segment Returns @@ -70,15 +73,15 @@ def from_components( ExternalForce """ - return cls(application_point_in_local, np.concatenate((torque, force)), transformation_matrix) + return cls(application_point_in_local, vertcat(torque, force), transformation_matrix) @property - def force(self) -> np.ndarray: + def force(self) -> MX: """The force vector in the global coordinate system""" return self.external_forces[3:6] @property - def torque(self) -> np.ndarray: + def torque(self) -> MX: """The torque vector in the global coordinate system""" return self.external_forces[0:3] @@ -88,7 +91,7 @@ def forces_in_global(self, Qi: SegmentNaturalCoordinates): force_in_global = rotation_matrix @ self.force torque_in_global = rotation_matrix @ self.torque - return np.concatenate((torque_in_global, force_in_global)) + return vertcat(torque_in_global, force_in_global) def transport_on_proximal( self, @@ -107,25 +110,25 @@ def transport_on_proximal( ExternalForceInGlobalOnProximal The external force on the proximal point of the segment """ - qi_array = np.array(Qi).squeeze() + 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 = np.array(old_point_interpolation_matrix @ qi_array).squeeze() - new_application_point_in_global = np.array(new_point_interpolation_matrix @ qi_array).squeeze() + 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 = np.cross(lever_arm, new_external_forces[3:6]) + 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) -> np.ndarray: + 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/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/external_force.py b/bionc/bionc_numpy/external_force.py index 9d1b20d9..8367c098 100644 --- a/bionc/bionc_numpy/external_force.py +++ b/bionc/bionc_numpy/external_force.py @@ -184,139 +184,3 @@ def __iter__(self): def __len__(self): return len(self.external_forces) - - -# class ExternalForceSet: -# """ -# This class is made to handle all the external forces of each segment, if none are provided, it will be an empty list. -# All segment forces are expressed in natural coordinates to be added to the equation of motion as: -# -# Q @ Qddot + K^T @ lambda = Weight + f_ext -# -# Attributes -# ---------- -# external_forces : list -# List of ExternalForces for each segment -# -# Methods -# ------- -# add_external_force(segment_index, external_force) -# This function adds an external force to the list of external forces. -# empty_from_nb_segment(nb_segment) -# This function creates an empty ExternalForceSet from the number of segments. -# to_natural_external_forces(Q) -# This function returns the external forces in the natural coordinate format. -# segment_external_forces(segment_index) -# This function returns the external forces of a segment. -# nb_segments -# This function returns the number of segments. -# -# Examples -# -------- -# >>> from bionc import 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): -# if external_forces is None: -# raise ValueError( -# "f_ext must be a list of ExternalForces, or use the classmethod" -# "NaturalExternalForceSet.empty_from_nb_segment(nb_segment)" -# ) -# self.external_forces = external_forces -# -# @property -# def nb_segments(self) -> int: -# """Returns the number of segments""" -# return len(self.external_forces) -# -# @classmethod -# def empty_from_nb_segment(cls, nb_segment: int): -# """ -# Create an empty NaturalExternalForceSet from the model size -# """ -# return cls(external_forces=[[] for _ in range(nb_segment)]) -# -# def segment_external_forces(self, segment_index: int) -> list[ExternalForce]: -# """Returns the external forces of the segment""" -# return self.external_forces[segment_index] -# -# def add_external_force(self, segment_index: int, external_force: ExternalForce): -# """ -# Add an external force to the segment -# -# Parameters -# ---------- -# segment_index: int -# The index of the segment -# external_force: -# The external force to add -# """ -# self.external_forces[segment_index].append(external_force) -# -# def to_natural_external_forces(self, Q: NaturalCoordinates) -> np.ndarray: -# """ -# Converts and sums all the segment natural external forces to the full vector of natural external forces -# -# Parameters -# ---------- -# Q : NaturalCoordinates -# The natural coordinates of the model -# """ -# -# if len(self.external_forces) != Q.nb_qi(): -# raise ValueError( -# "The number of segment in the model and the number of segment in the external forces must be the same" -# ) -# -# natural_external_forces = np.zeros((12 * Q.nb_qi(), 1)) -# for segment_index, segment_external_forces in enumerate(self.external_forces): -# segment_natural_external_forces = np.zeros((12, 1)) -# slice_index = slice(segment_index * 12, (segment_index + 1) * 12) -# for external_force in segment_external_forces: -# segment_natural_external_forces += external_force.to_natural_force(Q.vector(segment_index))[ -# :, np.newaxis -# ] -# natural_external_forces[slice_index, 0:1] = segment_natural_external_forces -# -# return natural_external_forces -# -# def to_segment_natural_external_forces(self, Q: NaturalCoordinates, segment_index: int) -> np.ndarray: -# """ -# Converts and sums all the segment natural external forces to the full vector of natural external forces -# for one segment -# -# 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(): -# raise ValueError( -# "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") -# -# 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] -# -# return segment_natural_external_forces -# -# def __iter__(self): -# return iter(self.external_forces) -# -# def __len__(self): -# return len(self.external_forces) diff --git a/tests/test_external_force.py b/tests/test_external_force.py index 0b3b2853..f0cea25a 100644 --- a/tests/test_external_force.py +++ b/tests/test_external_force.py @@ -1,5 +1,6 @@ import numpy as np import pytest +from casadi import vertcat from .utils import TestUtils @@ -70,6 +71,7 @@ def test_external_force_local_point(bionc_type): else: from bionc.bionc_casadi import ( ExternalForceSet, + ExternalForceInGlobalLocalPoint, SegmentNaturalCoordinates, NaturalCoordinates, SegmentNaturalVelocities, @@ -87,19 +89,27 @@ def test_external_force_local_point(bionc_type): 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_in_global_local_point( - external_force=np.concatenate([force1.torque, force1.force]), + 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]), + 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]), ) @@ -185,11 +195,7 @@ def test_external_force_local_point(bionc_type): 0.02180571, ] ) - TestUtils.assert_equal( - natural_force, - natural_force_2_expected, - expand=False, - ) + 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( @@ -260,11 +266,16 @@ def test_external_force_local_point(bionc_type): ] ), expand=False, + squeeze=True, ) fext.add_in_global_local_point( segment_index=2, - external_force=np.concatenate([force2.torque, force2.force]), + 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) @@ -296,6 +307,7 @@ def test_external_force_in_global(bionc_type): else: from bionc.bionc_casadi import ( ExternalForceSet, + ExternalForceInGlobal, SegmentNaturalCoordinates, NaturalCoordinates, SegmentNaturalVelocities, @@ -469,6 +481,7 @@ def test_external_force_in_local(bionc_type): else: from bionc.bionc_casadi import ( ExternalForceSet, + ExternalForceInLocal, SegmentNaturalCoordinates, NaturalCoordinates, SegmentNaturalVelocities, diff --git a/tests/utils.py b/tests/utils.py index 58b7e984..fe687e7d 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: @@ -78,4 +79,4 @@ def assert_equal( if isinstance(value, MX): TestUtils.mx_assert_equal(value, expected, decimal=decimal, squeeze=squeeze, expand=expand) else: - np.testing.assert_almost_equal(value, expected, decimal=decimal) + np.testing.assert_almost_equal(value.squeeze() if squeeze else value, expected, decimal=decimal) From 064886d0f19b69132ada7c8126c2785639a943e7 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Mon, 2 Dec 2024 12:26:28 -0500 Subject: [PATCH 18/23] fix: copy trick and tests --- bionc/bionc_casadi/external_force_global.py | 2 +- .../external_force_global_local_point.py | 4 +- .../external_force_global_on_proximal.py | 6 +- .../external_force_global_local_point.py | 1 - tests/test_external_force.py | 65 +++++++++++++------ 5 files changed, 52 insertions(+), 26 deletions(-) diff --git a/bionc/bionc_casadi/external_force_global.py b/bionc/bionc_casadi/external_force_global.py index 42377ea3..c687468a 100644 --- a/bionc/bionc_casadi/external_force_global.py +++ b/bionc/bionc_casadi/external_force_global.py @@ -90,7 +90,7 @@ def transport_on_proximal( 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 = self.external_forces.__copy__() new_external_forces[0:3] += additional_torque return ExternalForceInGlobalOnProximal(external_forces=new_external_forces) diff --git a/bionc/bionc_casadi/external_force_global_local_point.py b/bionc/bionc_casadi/external_force_global_local_point.py index f340bd77..d68a38dd 100644 --- a/bionc/bionc_casadi/external_force_global_local_point.py +++ b/bionc/bionc_casadi/external_force_global_local_point.py @@ -92,10 +92,10 @@ def transport_on_proximal( # 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) - # Some - new_external_forces = self.external_forces + new_external_forces = self.external_forces.__copy__() new_external_forces[0:3] += additional_torque return ExternalForceInGlobalOnProximal(external_forces=new_external_forces) diff --git a/bionc/bionc_casadi/external_force_global_on_proximal.py b/bionc/bionc_casadi/external_force_global_on_proximal.py index abfbb623..34c78ec4 100644 --- a/bionc/bionc_casadi/external_force_global_on_proximal.py +++ b/bionc/bionc_casadi/external_force_global_on_proximal.py @@ -1,4 +1,4 @@ -from casadi import MX, vertcat, cross +from casadi import MX, vertcat, cross, transpose from .natural_coordinates import SegmentNaturalCoordinates from .natural_vector import NaturalVector @@ -78,7 +78,7 @@ def natural_forces(self) -> MX: """ point_interpolation_matrix = NaturalVector.proximal().interpolate() - return point_interpolation_matrix.T @ self.force + return transpose(point_interpolation_matrix) @ self.force def natural_moments(self, Qi: SegmentNaturalCoordinates) -> MX: """ @@ -96,7 +96,7 @@ def natural_moments(self, Qi: SegmentNaturalCoordinates) -> MX: """ pseudo_interpolation_matrix = Qi.compute_pseudo_interpolation_matrix() - return pseudo_interpolation_matrix.T @ self.torque + return transpose(pseudo_interpolation_matrix) @ self.torque def to_generalized_natural_forces(self, Qi: SegmentNaturalCoordinates) -> MX: """ diff --git a/bionc/bionc_numpy/external_force_global_local_point.py b/bionc/bionc_numpy/external_force_global_local_point.py index 07746de9..7dcb149e 100644 --- a/bionc/bionc_numpy/external_force_global_local_point.py +++ b/bionc/bionc_numpy/external_force_global_local_point.py @@ -94,7 +94,6 @@ def transport_on_proximal( lever_arm = new_application_point_in_global - old_application_point_in_global additional_torque = np.cross(lever_arm, self.force) - # Some new_external_forces = self.external_forces.copy() new_external_forces[0:3] += additional_torque diff --git a/tests/test_external_force.py b/tests/test_external_force.py index f0cea25a..88cc145e 100644 --- a/tests/test_external_force.py +++ b/tests/test_external_force.py @@ -238,6 +238,10 @@ def test_external_force_local_point(bionc_type): 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( @@ -325,19 +329,27 @@ def test_external_force_in_global(bionc_type): 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])) - 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) + 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]), + 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]), + 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]), ) @@ -370,11 +382,7 @@ def test_external_force_in_global(bionc_type): 0.02102857, ] ) - TestUtils.assert_equal( - natural_force, - natural_force_2_expected, - expand=False, - ) + 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( @@ -445,11 +453,16 @@ def test_external_force_in_global(bionc_type): ] ), expand=False, + squeeze=True, ) fext.add_in_global( segment_index=2, - external_force=np.concatenate([force2.torque, force2.force]), + 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) @@ -502,20 +515,28 @@ def test_external_force_in_local(bionc_type): transformation_matrix=dummy_transformation_matrix, ) - 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) + 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]), + 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]), + 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, @@ -553,6 +574,7 @@ def test_external_force_in_local(bionc_type): natural_force, natural_force_2_expected, expand=False, + squeeze=True, ) natural_forces = fext.to_natural_external_forces(Q) @@ -624,11 +646,16 @@ def test_external_force_in_local(bionc_type): ] ), expand=False, + squeeze=True, ) fext.add_in_local( segment_index=2, - external_force=np.concatenate([force2.torque, force2.force]), + 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, ) From 184956671b5b603fc66c05ddf8976516017385a0 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Mon, 2 Dec 2024 12:26:58 -0500 Subject: [PATCH 19/23] init --- bionc/bionc_casadi/__init__.py | 25 ++++++++++++++----------- 1 file changed, 14 insertions(+), 11 deletions(-) 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 From 88e7959cf375b1470749d6805de5ae278a5c3ec1 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Mon, 2 Dec 2024 12:29:08 -0500 Subject: [PATCH 20/23] restored utils --- tests/utils.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/utils.py b/tests/utils.py index fe687e7d..a21aecbd 100644 --- a/tests/utils.py +++ b/tests/utils.py @@ -79,4 +79,4 @@ def assert_equal( if isinstance(value, MX): TestUtils.mx_assert_equal(value, expected, decimal=decimal, squeeze=squeeze, expand=expand) else: - np.testing.assert_almost_equal(value.squeeze() if squeeze else value, expected, decimal=decimal) + np.testing.assert_almost_equal(value, expected, decimal=decimal) From e21762d121b3069d240677310d08083293671af0 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Mon, 2 Dec 2024 12:31:28 -0500 Subject: [PATCH 21/23] feat: casadi inverse dynamics --- bionc/bionc_casadi/biomechanical_model.py | 19 +++++++++---------- 1 file changed, 9 insertions(+), 10 deletions(-) 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( From da11caa75a94ce3946b3d0b6173890b5d385c5c5 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Mon, 2 Dec 2024 13:34:31 -0500 Subject: [PATCH 22/23] temporary fix: on joint generalized Forces --- bionc/bionc_casadi/generalized_force.py | 8 +++++--- bionc/bionc_numpy/biomechanical_model.py | 1 + bionc/bionc_numpy/generalized_force.py | 9 ++++++--- .../forward_dynamics/pendulum_with_force.nmod | Bin 4066 -> 4868 bytes pendulum.nmod | Bin 0 -> 4868 bytes pendulum_with_force.nmod | Bin 0 -> 4868 bytes 6 files changed, 12 insertions(+), 6 deletions(-) create mode 100644 pendulum.nmod create mode 100644 pendulum_with_force.nmod 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_numpy/biomechanical_model.py b/bionc/bionc_numpy/biomechanical_model.py index c445d0a7..96dd8ede 100644 --- a/bionc/bionc_numpy/biomechanical_model.py +++ b/bionc/bionc_numpy/biomechanical_model.py @@ -9,6 +9,7 @@ from .cartesian_vector import vector_projection_in_non_orthogonal_basis 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 from .natural_velocities import NaturalVelocities 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/examples/forward_dynamics/pendulum_with_force.nmod b/examples/forward_dynamics/pendulum_with_force.nmod index 98d34b0c06ca9f1337d32e192fe0c8d230685957..1fd3fbf3b089cf6c2c2ca3c4d231b90424a06241 100644 GIT binary patch literal 4868 zcmeHK&2Aev5SAj#c5OLLg0``X^e2Ja%0R43i=qd&M(qYpBUmUBBmF@QVzo<~o2+)# z{c-F90hEL57~rJ{i#$Pl=`$35g*-wY!PlUNc9tAc(yrD{;A?bPQsm6{4QGclv%RrD zUR)~4AOEQB2EN^pPvR$??p`|YP@7r4Yg-=a1P=8CzqB&Ax-J)=h=oVup{Vo8h;DbN zA4h^Kq|@+7U*X+6@EwhY1g!c{@X0Rqoy1Ezf?t)^4srNaoP=O4;tGDgrB)D27+b5v zw>ng?8XqUF@6cV*Eb$5AQGYvTqFIJz%j+^rtS!H|`tQa!i?h5++B6KXA2PfotTK9tLy2XUX{YbhB#wV_{EcDy9439}$S4=g*6zY+#6+!`n$^zAPvA7V7l%K<@@U6!Z$_Sh-`;J;ydDyPr^aN`|?FC6u&5JS^BbX54SGsh@qPyXH!IN zz*dzWYl6?{Gqu7Jn`H)h(E{NkGQsRU}=l&U&j`IzeG_V_s53~0GTi&t&tdX8Wh zc-6Fc7rJ3^51OGg{@PaLMuKhfnHz~m!#nhsg!;B@fhxc5?x}ZKQ*c_C@XE@AL~Tt= z{K1_a+&S;Jp?^zTHA{N0S>=~y@Ovuq?H~vp7dk|$_*bnErmp3Gy$cpW3s(L?QZShP zXl6gj*@58GXp@ynZ;Cp5%AQg_gRAN-%k{L+;@9%dwKX4xkKk__6I3vO5!qY6z|Xk@ zjUihc*%qo?t1n)EgbS$JLX}hg%%cAdIptJc%BPgi&B-_UG~N^Ay1U9@|M2qnaz=Sg zDP*dPOL;YB=|=>-H1lC$CRAQI=8VecqwbnXD|7kFmQ5E` z_6FSMDx_cv_pPo%0`5W_*a76XQSeIiy)_C|!nN}h8!42&e+&p%$aa!88c~%ri{W|GYx{*KIXA z2>oIFUF0{jj&WnYmWOe)U-R*x^$gI^Bo_yIwj?8XtA1!VvsqvgE_~WpukFOLlZf$3pEQ_scoR= z)tO{tGGh)AvZB6FTIBKGy{zzV)v->^?I^IadUdMv7jsJ^P z)t6*@CM@`PDW+xWQmWtQQv2!Uyr$u=2=`$PWZr&5+QN$u1)KSHmwSQRfkEK-GW*hF zuJGX<$P8s}mw+re%yZlIXt(1rMLXcGMqZ$voomJ+*IMD#y;gJ?5-7IZWPq z?$N$9T37@e_r|Krv8qS+BwC4LBF(tl6Y})>DcV7^Jo<)6+2%|sa`QA!Ju2RxqYwY_ N?+(r0OQ!;-{ug4!7I**v diff --git a/pendulum.nmod b/pendulum.nmod new file mode 100644 index 0000000000000000000000000000000000000000..15d737cc4398cbf99a903f8782dccd86cf698b6d GIT binary patch literal 4868 zcmeHK&2Jk;6u0Ya5)!Ah70?8gLaP9d)cDd0acHqfs1j<$0vaknM5FzfWVT)Jn*B%< zq)O%x(nxbb9aI5=3o25@i5o3KLE?|#kKob+mx=?toiE$#EKURm4%x%*?3e$f{#H}LH$zau|swl>po6Whe}UEB0fGjOoS=!v;*=@J*-VKbkwTdYDy zBfQqcejG6>G!4>+IvbwtSA11zlLm-!i_y^*_MOB_nv9;|XnSn>$Typqkun`duJ7OtRxi*I=|0@)!pV~or&G&ccWg+3$PkL)1C z)n?+wu9%}`6kf9+VGA&WnOP?}wZfAZ`=)7P1>Gam`Z~yqs z?nC)h?`r-*W?sF2%`x4t0iFhjROTN6ZgJ>tNF1+p+b-A3-1c01s8CSqMUr&xfCUWx zQ0qv(#y?cylIQ@p=NE;q9@R!ZeCqs5-+gvbJ+%+{pfleqS8tDW2S_YBt@=<7 zAjJD1JuaM@#I6@rF*HeV^yG5S36^*pS(a_HG?%Qit+ertAvZ#6VUYL^+o7Xy(C`NT zP&4@-N>Y~2+NXP|%aYWg$dI$bA`5hjP7m1jp(uVF@~`;`kt=%yPJng?8XqUF@6cV*Eb$5AQGYvTqFIJz%j+^rtS!H|`tQa!i?h5++B6KXA2PfotTK9tLy2XUX{YbhB#wV_{EcDy9439}$S4=g*6zY+#6+!`n$^zAPvA7V7l%K<@@U6!Z$_Sh-`;J;ydDyPr^aN`|?FC6u&5JS^BbX54SGsh@qPyXH!IN zz*dzWYl6?{Gqu7Jn`H)h(E{NkGQsRU}=l&U&j`IzeG_V_s53~0GTi&t&tdX8Wh zc-6Fc7rJ3^51OGg{@PaLMuKhfnHz~m!#nhsg!;B@fhxc5?x}ZKQ*c_C@XE@AL~Tt= z{K1_a+&S;Jp?^zTHA{N0S>=~y@Ovuq?H~vp7dk|$_*bnErmp3Gy$cpW3s(L?QZShP zXl6gj*@58GXp@ynZ;Cp5%AQg_gRAN-%k{L+;@9%dwKX4xkKk__6I3vO5!qY6z|Xk@ zjUihc*%qo?t1n)EgbS$JLX}hg%%cAdIptJc%BPgi&B-_UG~N^Ay1U9@|M2qnaz=Sg zDP*dPOL;YB=|=>-H1lC$CRAQI=8VecqwbnXD|7kFmQ5E` z_6FSMDx_cv_pPo%0`5W_*a76XQSeIiy)_C|!nN}h8!42&e+&p%$aa!88c~%ri{W|GYx{*KIXA z2>oIFUF0{jj&WnYmWOe)U-R*x^$gI^Bo_yIw Date: Mon, 2 Dec 2024 13:36:23 -0500 Subject: [PATCH 23/23] clean: delete prototype --- .../inverse_dynamics/external_force_set.py | 23 ------------------- 1 file changed, 23 deletions(-) delete mode 100644 examples/inverse_dynamics/external_force_set.py diff --git a/examples/inverse_dynamics/external_force_set.py b/examples/inverse_dynamics/external_force_set.py deleted file mode 100644 index 460e7512..00000000 --- a/examples/inverse_dynamics/external_force_set.py +++ /dev/null @@ -1,23 +0,0 @@ -from .three_link_pendulum import build_n_link_pendulum - -model = build_n_link_pendulum(nb_segments=3) - -# Options 1 -# external_force_set = ExternalForceSet.empty_from_nb_segment(model.nb_segments) - -# Options 2 -external_force_set = model.external_force_set() - -# add forces and moment -# point_of_application = [0, 0, 0] -# moment_only = [0, 0, 0] -# moment_and_force = [1, 0, 0, 0, 0, 0] - -external_force_set.add_in_global("segment", moment_and_force, point_of_application) -external_force_set.add_in_global_local_point("segment", moment_and_force, point_of_application) -external_force_set.add_in_local("segment", moment_and_force, point_of_application) - - - - -