From 8f3a2f53875bdde333ee59f8646eb12b44bb80c3 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Wed, 9 Nov 2022 15:55:25 -0500 Subject: [PATCH 01/30] minor changes --- bionc/math_interface/math_interface.py | 51 +++++++++++++++++++++ bionc/model_computations/natural_segment.py | 3 +- 2 files changed, 52 insertions(+), 2 deletions(-) create mode 100644 bionc/math_interface/math_interface.py diff --git a/bionc/math_interface/math_interface.py b/bionc/math_interface/math_interface.py new file mode 100644 index 00000000..b690bd2f --- /dev/null +++ b/bionc/math_interface/math_interface.py @@ -0,0 +1,51 @@ +from casadi import MX, tril, tril2symm +from casadi.casadi import MX as MX_type +import casadi as cas +from numpy import ndarray +import numpy as np + + +def zeros(*args, instance_type: MX | ndarray) -> ndarray | MX: + if instance_type == MX_type: + return MX.zeros(*args) + elif instance_type == ndarray: + return np.zeros(args) + + +def eye(n, instance_type: MX | ndarray) -> ndarray | MX: + if instance_type == MX_type: + return MX.eye(n) + elif instance_type == ndarray: + return np.eye(n) + + +def array(data, instance_type: MX | ndarray) -> ndarray | MX: + if instance_type == MX_type: + return MX(data) + elif instance_type == ndarray: + return np.array(data) + + +def symmetrize_upp(A, instance_type: MX | ndarray) -> ndarray | MX: + """ + This function symmetrizes a matrix by copying the upper triangular part + to the lower triangular part conserving the diagonal. + """ + if instance_type == ndarray: + return np.tril(A) + np.tril(A, -1).T + elif instance_type == MX_type: + return tril2symm(tril(A)) + + +def vertcat(*args, instance_type: MX | ndarray) -> ndarray | MX: + if instance_type == ndarray: + return np.vstack(args) + elif instance_type == MX_type: + return cas.vertcat(*args) + + +def horzcat(*args, instance_type: MX | ndarray) -> ndarray | MX: + if instance_type == ndarray: + return np.hstack(args) + elif instance_type == MX_type: + return cas.horzcat(*args) diff --git a/bionc/model_computations/natural_segment.py b/bionc/model_computations/natural_segment.py index 4f73ee13..fe5db76a 100644 --- a/bionc/model_computations/natural_segment.py +++ b/bionc/model_computations/natural_segment.py @@ -9,7 +9,6 @@ from ..utils.natural_velocities import SegmentNaturalVelocities from ..utils.natural_accelerations import SegmentNaturalAccelerations from ..utils.homogenous_transform import HomogeneousTransform -from ..model_computations.natural_axis import Axis from ..model_computations.natural_marker import SegmentMarker, Marker @@ -604,7 +603,7 @@ def differential_algebraic_equation( Gi = self.mass_matrix Kr = self.rigid_body_constraint_jacobian(Qi) Krdot = self.rigid_body_constraint_jacobian_derivative(Qdoti) - biais = np.matmul(Krdot, Qdoti.vector) + biais = Krdot @ Qdoti.vector A = zeros((18, 18)) A[0:12, 0:12] = Gi From a001bf98393815cf8a0f5b7dc0cc5af6bd3b3335 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Wed, 9 Nov 2022 16:54:56 -0500 Subject: [PATCH 02/30] this looks not bad as a first avenue --- bionc/__init__.py | 30 +- bionc/math_interface/__init__.py | 0 bionc/math_interface/internal.py | 56 ++++ bionc/math_interface/math_interface.py | 12 +- bionc/math_interface/using_casadi/__init__.py | 1 + .../using_casadi/natural_coordinates.py | 184 ++++++++++++ bionc/math_interface/using_numpy/__init__.py | 1 + .../using_numpy/natural_coordinates.py | 190 +++++++++++++ bionc/model_computations/natural_marker.py | 3 +- bionc/model_computations/natural_segment.py | 3 +- bionc/model_creation/marker_template.py | 3 +- .../natural_segment_template.py | 3 +- bionc/utils/__init__.py | 4 +- bionc/utils/natural_coordinates.py | 262 +++++++++--------- examples/natural_coordinates.py | 42 +++ 15 files changed, 651 insertions(+), 143 deletions(-) create mode 100644 bionc/math_interface/__init__.py create mode 100644 bionc/math_interface/internal.py create mode 100644 bionc/math_interface/using_casadi/__init__.py create mode 100644 bionc/math_interface/using_casadi/natural_coordinates.py create mode 100644 bionc/math_interface/using_numpy/__init__.py create mode 100644 bionc/math_interface/using_numpy/natural_coordinates.py create mode 100644 examples/natural_coordinates.py diff --git a/bionc/__init__.py b/bionc/__init__.py index 11a6b2a7..14abadda 100644 --- a/bionc/__init__.py +++ b/bionc/__init__.py @@ -1,4 +1,7 @@ -from .utils.natural_coordinates import SegmentNaturalCoordinates, NaturalCoordinates +from .utils.natural_coordinates import ( + # SegmentNaturalCoordinates, + NaturalCoordinates, +) from .utils.natural_velocities import SegmentNaturalVelocities, NaturalVelocities from .utils.natural_accelerations import SegmentNaturalAccelerations, NaturalAccelerations from .utils.homogenous_transform import HomogeneousTransform @@ -25,3 +28,28 @@ InertiaParameters, BiomechanicalModel, ) + +from .math_interface.math_interface import ( + zeros, + eye, + array, + symmetrize_upp, + vertcat, + horzcat, +) + + +from .math_interface import internal +from .math_interface import using_casadi as bionc_casadi +from .math_interface import using_numpy as bionc_numpy + +# I don't know if it's useful to import the following yet +from .math_interface.internal import SegmentNaturalCoordinates + +from casadi.casadi import MX as MX_type +from numpy import ndarray +# global variable to store the type of the math interface +casadi_type = MX_type +numpy_type = ndarray + + diff --git a/bionc/math_interface/__init__.py b/bionc/math_interface/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/bionc/math_interface/internal.py b/bionc/math_interface/internal.py new file mode 100644 index 00000000..f6e5c2bf --- /dev/null +++ b/bionc/math_interface/internal.py @@ -0,0 +1,56 @@ +from typing import Protocol + + +class SegmentNaturalCoordinates(Protocol): + """ + This class is made to handle Generalized Coordinates of a Segment + """ + + def __new__(cls, input_array): + """ Create a new instance of the class. """ + + @classmethod + def from_components(cls, u, rp, rd, w): + """ Constructor of the class from the components of the natural coordinates """ + + def to_array(self): + """ This function returns the array of the natural coordinates """ + + def u(self): + """ This property returns the u vector of the natural coordinates """ + + def rp(self): + """ This property returns the rp vector of the natural coordinates """ + + def rd(self): + """ This property returns the rd vector of the natural coordinates """ + + def w(self): + """ This property returns the w vector of the natural coordinates """ + + def v(self): + """ This property returns the v vector of the natural coordinates """ + + def vector(self): + """ This property returns the vector of the natural coordinates """ + + def to_components(self): + """ This function returns the components of the natural coordinates """ + + def to_uvw(self): + """ This function returns the uvw vector of the natural coordinates """ + + def to_non_orthogonal_basis(self, vector): + """ + This function converts a vector expressed in the global coordinate system + to a vector expressed in a non-orthogonal coordinate system associated to the segment coordinates. + """ + + def to_interpolation_matrix(self, vector): + """ + This function converts a vector expressed in the global coordinate system + to a vector expressed in a non-orthogonal coordinate system associated to the segment coordinates. + """ + + + diff --git a/bionc/math_interface/math_interface.py b/bionc/math_interface/math_interface.py index b690bd2f..ae7c80b6 100644 --- a/bionc/math_interface/math_interface.py +++ b/bionc/math_interface/math_interface.py @@ -5,28 +5,28 @@ import numpy as np -def zeros(*args, instance_type: MX | ndarray) -> ndarray | MX: +def zeros(*args, instance_type: type) -> ndarray | MX: if instance_type == MX_type: return MX.zeros(*args) elif instance_type == ndarray: return np.zeros(args) -def eye(n, instance_type: MX | ndarray) -> ndarray | MX: +def eye(n, instance_type: type) -> ndarray | MX: if instance_type == MX_type: return MX.eye(n) elif instance_type == ndarray: return np.eye(n) -def array(data, instance_type: MX | ndarray) -> ndarray | MX: +def array(data, instance_type: type) -> ndarray | MX: if instance_type == MX_type: return MX(data) elif instance_type == ndarray: return np.array(data) -def symmetrize_upp(A, instance_type: MX | ndarray) -> ndarray | MX: +def symmetrize_upp(A, instance_type: type) -> ndarray | MX: """ This function symmetrizes a matrix by copying the upper triangular part to the lower triangular part conserving the diagonal. @@ -37,14 +37,14 @@ def symmetrize_upp(A, instance_type: MX | ndarray) -> ndarray | MX: return tril2symm(tril(A)) -def vertcat(*args, instance_type: MX | ndarray) -> ndarray | MX: +def vertcat(*args, instance_type: type) -> ndarray | MX: if instance_type == ndarray: return np.vstack(args) elif instance_type == MX_type: return cas.vertcat(*args) -def horzcat(*args, instance_type: MX | ndarray) -> ndarray | MX: +def horzcat(*args, instance_type: type) -> ndarray | MX: if instance_type == ndarray: return np.hstack(args) elif instance_type == MX_type: diff --git a/bionc/math_interface/using_casadi/__init__.py b/bionc/math_interface/using_casadi/__init__.py new file mode 100644 index 00000000..b40eae23 --- /dev/null +++ b/bionc/math_interface/using_casadi/__init__.py @@ -0,0 +1 @@ +from .natural_coordinates import SegmentNaturalCoordinates diff --git a/bionc/math_interface/using_casadi/natural_coordinates.py b/bionc/math_interface/using_casadi/natural_coordinates.py new file mode 100644 index 00000000..75d5546a --- /dev/null +++ b/bionc/math_interface/using_casadi/natural_coordinates.py @@ -0,0 +1,184 @@ +from casadi import MX, vertcat +from typing import Union +# from bionc.utils.vnop_array import vnop_array +# from bionc.utils.interpolation_matrix import interpolate_natural_vector + + +class SegmentNaturalCoordinates(MX): + """ + This class is made to handle Generalized Coordinates of a Segment + """ + + def __new__(cls, input_array: MX): + """ + Create a new instance of the class. + """ + + obj = MX.__new__(cls) + + return obj + + @classmethod + def from_components( + cls, + u: Union[MX, list] = None, + rp: Union[MX, list] = None, + rd: Union[MX, list] = None, + w: Union[MX, list] = None, + ): + """ + Constructor of the class from the components of the natural coordinates + """ + + if u is None: + raise ValueError("u must be a numpy array (3x1) or a list of 3 elements") + if rp is None: + raise ValueError("rp must be a numpy array (3x1) or a list of 3 elements") + if rd is None: + raise ValueError("rd must be a numpy array (3x1) or a list of 3 elements") + if w is None: + raise ValueError("w must be a numpy array (3x1) or a list of 3 elements") + + if not isinstance(u, MX): + u = MX(u) + if not isinstance(rp, MX): + rp = MX(rp) + if not isinstance(rd, MX): + rd = MX(rd) + if not isinstance(w, MX): + w = MX(w) + + if u.shape[0] != 3: + raise ValueError("u must be a 3x1 numpy array") + if rp.shape[0] != 3: + raise ValueError("rp must be a 3x1 numpy array") + if rd.shape[0] != 3: + raise ValueError("rd must be a 3x1 numpy array") + if w.shape[0] != 3: + raise ValueError("v must be a 3x1 numpy array") + + input_array = vertcat(u, rp, rd, w) + + return cls(input_array) + + def to_array(self): + return MX(self) + + @property + def u(self): + return self[0:3, :] + + @property + def rp(self): + return self[3:6, :] + + @property + def rd(self): + return self[6:9, :] + + @property + def w(self): + return self[9:12, :] + + @property + def v(self): + return self.rp - self.rd + + @property + def vector(self): + return self.to_array() + + def to_components(self): + return self.u, self.rp, self.rd, self.w + + def to_uvw(self): + return self.u, self.v, self.w + + # def to_non_orthogonal_basis(self, vector: MX) -> MX: + # """ + # This function converts a vector expressed in the global coordinate system + # to a vector expressed in a non-orthogonal coordinate system associated to the segment coordinates. + # + # Parameters + # ---------- + # vector: MX + # The vector expressed in the global coordinate system + # + # Returns + # ------- + # MX + # The vector expressed in the non-orthogonal coordinate system + # + # """ + # return vnop_array(vector - self.rp, self.u, self.v, self.w) + # + # def to_interpolation_matrix(self, vector: MX) -> MX: + # """ + # This function converts a vector expressed in the global coordinate system + # to a vector expressed in a non-orthogonal coordinate system associated to the segment coordinates. + # + # Parameters + # ---------- + # vector: MX + # The vector expressed in the global coordinate system + # + # Returns + # ------- + # MX + # The vector expressed in the non-orthogonal coordinate system + # + # """ + # return interpolate_natural_vector(vnop_array(vector - self.rp, self.u, self.v, self.w)) + + +# class NaturalCoordinates(np.ndarray): +# def __new__(cls, input_array: np.ndarray): +# """ +# Create a new instance of the class. +# """ +# +# return np.asarray(input_array).view(cls) +# +# @classmethod +# def from_qi(cls, tuple_of_Q: tuple): +# """ +# Constructor of the class. +# """ +# if not isinstance(tuple_of_Q, tuple): +# raise ValueError("tuple_of_Q must be a tuple of SegmentGeneralizedCoordinates") +# +# for Q in tuple_of_Q: +# if not isinstance(Q, SegmentNaturalCoordinates): +# raise ValueError("tuple_of_Q must be a tuple of SegmentGeneralizedCoordinates") +# +# input_array = np.concatenate(tuple_of_Q, axis=0) +# return cls(input_array) +# +# def to_array(self): +# return np.array(self).squeeze() +# +# def nb_qi(self): +# return self.shape[0] // 12 +# +# def u(self, segment_idx: int): +# array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12)[0:3] +# return self[array_idx, :].to_array() +# +# def rp(self, segment_idx: int): +# array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12)[3:6] +# return self[array_idx, :].to_array() +# +# def rd(self, segment_idx: int): +# array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12)[6:9] +# return self[array_idx, :].to_array() +# +# def w(self, segment_idx: int): +# array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12)[9:12] +# return self[array_idx, :].to_array() +# +# def v(self, segment_idx: int): +# return self.rp(segment_idx) - self.rd(segment_idx) +# +# def vector(self, segment_idx: int): +# array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12) +# return SegmentNaturalCoordinates(self[array_idx, :].to_array()) diff --git a/bionc/math_interface/using_numpy/__init__.py b/bionc/math_interface/using_numpy/__init__.py new file mode 100644 index 00000000..0ef3875e --- /dev/null +++ b/bionc/math_interface/using_numpy/__init__.py @@ -0,0 +1 @@ +from .natural_coordinates import SegmentNaturalCoordinates diff --git a/bionc/math_interface/using_numpy/natural_coordinates.py b/bionc/math_interface/using_numpy/natural_coordinates.py new file mode 100644 index 00000000..24a41d67 --- /dev/null +++ b/bionc/math_interface/using_numpy/natural_coordinates.py @@ -0,0 +1,190 @@ +import numpy as np +from typing import Union +from bionc.utils.vnop_array import vnop_array +from bionc.utils.interpolation_matrix import interpolate_natural_vector + + +class SegmentNaturalCoordinates(np.ndarray): + """ + This class is made to handle Generalized Coordinates of a Segment + """ + + def __new__(cls, input_array: Union[np.ndarray, list, tuple]): + """ + Create a new instance of the class. + """ + + obj = np.asarray(input_array).view(cls) + + if obj.shape.__len__() == 1: + obj = obj[:, np.newaxis] + + return obj + + @classmethod + def from_components( + cls, + u: Union[np.ndarray, list] = None, + rp: Union[np.ndarray, list] = None, + rd: Union[np.ndarray, list] = None, + w: Union[np.ndarray, list] = None, + ): + """ + Constructor of the class from the components of the natural coordinates + """ + + if u is None: + raise ValueError("u must be a numpy array (3x1) or a list of 3 elements") + if rp is None: + raise ValueError("rp must be a numpy array (3x1) or a list of 3 elements") + if rd is None: + raise ValueError("rd must be a numpy array (3x1) or a list of 3 elements") + if w is None: + raise ValueError("w must be a numpy array (3x1) or a list of 3 elements") + + if not isinstance(u, np.ndarray): + u = np.array(u) + if not isinstance(rp, np.ndarray): + rp = np.array(rp) + if not isinstance(rd, np.ndarray): + rd = np.array(rd) + if not isinstance(w, np.ndarray): + w = np.array(w) + + if u.shape[0] != 3: + raise ValueError("u must be a 3x1 numpy array") + if rp.shape[0] != 3: + raise ValueError("rp must be a 3x1 numpy array") + if rd.shape[0] != 3: + raise ValueError("rd must be a 3x1 numpy array") + if w.shape[0] != 3: + raise ValueError("v must be a 3x1 numpy array") + + input_array = np.concatenate((u, rp, rd, w), axis=0) + + if input_array.shape.__len__() == 1: + input_array = input_array[:, np.newaxis] + + return cls(input_array) + + def to_array(self): + return np.array(self).squeeze() + + @property + def u(self): + return self[0:3, :].to_array() + + @property + def rp(self): + return self[3:6, :].to_array() + + @property + def rd(self): + return self[6:9, :].to_array() + + @property + def w(self): + return self[9:12, :].to_array() + + @property + def v(self): + return self.rp - self.rd + + @property + def vector(self): + return self.to_array() + + def to_components(self): + return self.u, self.rp, self.rd, self.w + + def to_uvw(self): + return self.u, self.v, self.w + + def to_non_orthogonal_basis(self, vector: np.ndarray) -> np.ndarray: + """ + This function converts a vector expressed in the global coordinate system + to a vector expressed in a non-orthogonal coordinate system associated to the segment coordinates. + + Parameters + ---------- + vector: np.ndarray + The vector expressed in the global coordinate system + + Returns + ------- + np.ndarray + The vector expressed in the non-orthogonal coordinate system + + """ + return vnop_array(vector - self.rp, self.u, self.v, self.w) + + def to_interpolation_matrix(self, vector: np.ndarray) -> np.ndarray: + """ + This function converts a vector expressed in the global coordinate system + to a vector expressed in a non-orthogonal coordinate system associated to the segment coordinates. + + Parameters + ---------- + vector: np.ndarray + The vector expressed in the global coordinate system + + Returns + ------- + np.ndarray + The vector expressed in the non-orthogonal coordinate system + + """ + return interpolate_natural_vector(vnop_array(vector - self.rp, self.u, self.v, self.w)) + + +# class NaturalCoordinates(np.ndarray): +# def __new__(cls, input_array: np.ndarray): +# """ +# Create a new instance of the class. +# """ +# +# return np.asarray(input_array).view(cls) +# +# @classmethod +# def from_qi(cls, tuple_of_Q: tuple): +# """ +# Constructor of the class. +# """ +# if not isinstance(tuple_of_Q, tuple): +# raise ValueError("tuple_of_Q must be a tuple of SegmentGeneralizedCoordinates") +# +# for Q in tuple_of_Q: +# if not isinstance(Q, SegmentNaturalCoordinates): +# raise ValueError("tuple_of_Q must be a tuple of SegmentGeneralizedCoordinates") +# +# input_array = np.concatenate(tuple_of_Q, axis=0) +# return cls(input_array) +# +# def to_array(self): +# return np.array(self).squeeze() +# +# def nb_qi(self): +# return self.shape[0] // 12 +# +# def u(self, segment_idx: int): +# array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12)[0:3] +# return self[array_idx, :].to_array() +# +# def rp(self, segment_idx: int): +# array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12)[3:6] +# return self[array_idx, :].to_array() +# +# def rd(self, segment_idx: int): +# array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12)[6:9] +# return self[array_idx, :].to_array() +# +# def w(self, segment_idx: int): +# array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12)[9:12] +# return self[array_idx, :].to_array() +# +# def v(self, segment_idx: int): +# return self.rp(segment_idx) - self.rd(segment_idx) +# +# def vector(self, segment_idx: int): +# array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12) +# return SegmentNaturalCoordinates(self[array_idx, :].to_array()) diff --git a/bionc/model_computations/natural_marker.py b/bionc/model_computations/natural_marker.py index aecddaf5..5322cddc 100644 --- a/bionc/model_computations/natural_marker.py +++ b/bionc/model_computations/natural_marker.py @@ -5,8 +5,9 @@ from .biomechanical_model import BiomechanicalModel from ..model_creation.protocols import Data from ..utils.interpolation_matrix import interpolate_natural_vector, to_natural_vector -from ..utils.natural_coordinates import SegmentNaturalCoordinates +# from ..utils.natural_coordinates import SegmentNaturalCoordinates +from ..math_interface.internal import SegmentNaturalCoordinates # todo: need a list of markers MarkerList diff --git a/bionc/model_computations/natural_segment.py b/bionc/model_computations/natural_segment.py index fe5db76a..52e48bad 100644 --- a/bionc/model_computations/natural_segment.py +++ b/bionc/model_computations/natural_segment.py @@ -5,7 +5,8 @@ from numpy import cos, sin, matmul, eye, zeros, sum, ones from numpy.linalg import inv -from ..utils.natural_coordinates import SegmentNaturalCoordinates +# from ..utils.natural_coordinates import SegmentNaturalCoordinates +from ..math_interface.internal import SegmentNaturalCoordinates from ..utils.natural_velocities import SegmentNaturalVelocities from ..utils.natural_accelerations import SegmentNaturalAccelerations from ..utils.homogenous_transform import HomogeneousTransform diff --git a/bionc/model_creation/marker_template.py b/bionc/model_creation/marker_template.py index 60ac7a66..0922b0ae 100644 --- a/bionc/model_creation/marker_template.py +++ b/bionc/model_creation/marker_template.py @@ -8,7 +8,8 @@ # from .biomechanical_model_template import BiomechanicalModelTemplate from .protocols import Data from ..model_computations.natural_segment import NaturalSegment -from ..utils.natural_coordinates import SegmentNaturalCoordinates +# from ..utils.natural_coordinates import SegmentNaturalCoordinates +from ..math_interface.internal import SegmentNaturalCoordinates class MarkerTemplate: diff --git a/bionc/model_creation/natural_segment_template.py b/bionc/model_creation/natural_segment_template.py index 58ea1385..4aacfefd 100644 --- a/bionc/model_creation/natural_segment_template.py +++ b/bionc/model_creation/natural_segment_template.py @@ -3,7 +3,8 @@ from ..model_computations.natural_axis import Axis from .natural_axis_template import AxisTemplate from ..model_computations.biomechanical_model import BiomechanicalModel -from ..utils.natural_coordinates import SegmentNaturalCoordinates +# from ..utils.natural_coordinates import SegmentNaturalCoordinates +from ..math_interface.internal import SegmentNaturalCoordinates from .marker_template import MarkerTemplate from .protocols import Data from ..model_computations.natural_segment import NaturalSegment diff --git a/bionc/utils/__init__.py b/bionc/utils/__init__.py index 5935fe08..04f3bde2 100644 --- a/bionc/utils/__init__.py +++ b/bionc/utils/__init__.py @@ -1,4 +1,6 @@ from .homogenous_transform import HomogeneousTransform -from .natural_coordinates import SegmentNaturalCoordinates, NaturalCoordinates +from .natural_coordinates import ( + # SegmentNaturalCoordinates, \ + NaturalCoordinates) from .natural_velocities import SegmentNaturalVelocities, NaturalVelocities from .natural_accelerations import SegmentNaturalAccelerations, NaturalAccelerations diff --git a/bionc/utils/natural_coordinates.py b/bionc/utils/natural_coordinates.py index 81b90b01..c9d729ee 100644 --- a/bionc/utils/natural_coordinates.py +++ b/bionc/utils/natural_coordinates.py @@ -4,137 +4,137 @@ from bionc.utils.interpolation_matrix import interpolate_natural_vector -class SegmentNaturalCoordinates(np.ndarray): - """ - This class is made to handle Generalized Coordinates of a Segment - """ - - def __new__(cls, input_array: Union[np.ndarray, list, tuple]): - """ - Create a new instance of the class. - """ - - obj = np.asarray(input_array).view(cls) - - if obj.shape.__len__() == 1: - obj = obj[:, np.newaxis] - - return obj - - @classmethod - def from_components( - cls, - u: Union[np.ndarray, list] = None, - rp: Union[np.ndarray, list] = None, - rd: Union[np.ndarray, list] = None, - w: Union[np.ndarray, list] = None, - ): - """ - Constructor of the class from the components of the natural coordinates - """ - - if u is None: - raise ValueError("u must be a numpy array (3x1) or a list of 3 elements") - if rp is None: - raise ValueError("rp must be a numpy array (3x1) or a list of 3 elements") - if rd is None: - raise ValueError("rd must be a numpy array (3x1) or a list of 3 elements") - if w is None: - raise ValueError("w must be a numpy array (3x1) or a list of 3 elements") - - if not isinstance(u, np.ndarray): - u = np.array(u) - if not isinstance(rp, np.ndarray): - rp = np.array(rp) - if not isinstance(rd, np.ndarray): - rd = np.array(rd) - if not isinstance(w, np.ndarray): - w = np.array(w) - - if u.shape[0] != 3: - raise ValueError("u must be a 3x1 numpy array") - if rp.shape[0] != 3: - raise ValueError("rp must be a 3x1 numpy array") - if rd.shape[0] != 3: - raise ValueError("rd must be a 3x1 numpy array") - if w.shape[0] != 3: - raise ValueError("v must be a 3x1 numpy array") - - input_array = np.concatenate((u, rp, rd, w), axis=0) - - if input_array.shape.__len__() == 1: - input_array = input_array[:, np.newaxis] - - return cls(input_array) - - def to_array(self): - return np.array(self).squeeze() - - @property - def u(self): - return self[0:3, :].to_array() - - @property - def rp(self): - return self[3:6, :].to_array() - - @property - def rd(self): - return self[6:9, :].to_array() - - @property - def w(self): - return self[9:12, :].to_array() - - @property - def v(self): - return self.rp - self.rd - - @property - def vector(self): - return self.to_array() - - def to_components(self): - return self.u, self.rp, self.rd, self.w - - def to_uvw(self): - return self.u, self.v, self.w - - def to_non_orthogonal_basis(self, vector: np.ndarray) -> np.ndarray: - """ - This function converts a vector expressed in the global coordinate system - to a vector expressed in a non-orthogonal coordinate system associated to the segment coordinates. - - Parameters - ---------- - vector: np.ndarray - The vector expressed in the global coordinate system - - Returns - ------- - np.ndarray - The vector expressed in the non-orthogonal coordinate system - - """ - return vnop_array(vector - self.rp, self.u, self.v, self.w) - - def to_interpolation_matrix(self, vector: np.ndarray) -> np.ndarray: - """ - This function converts a vector expressed in the global coordinate system - to a vector expressed in a non-orthogonal coordinate system associated to the segment coordinates. - - Parameters - ---------- - vector: np.ndarray - The vector expressed in the global coordinate system - - Returns - ------- - np.ndarray - The vector expressed in the non-orthogonal coordinate system - - """ - return interpolate_natural_vector(vnop_array(vector - self.rp, self.u, self.v, self.w)) +# class SegmentNaturalCoordinates(np.ndarray): +# """ +# This class is made to handle Generalized Coordinates of a Segment +# """ +# +# def __new__(cls, input_array: Union[np.ndarray, list, tuple]): +# """ +# Create a new instance of the class. +# """ +# +# obj = np.asarray(input_array).view(cls) +# +# if obj.shape.__len__() == 1: +# obj = obj[:, np.newaxis] +# +# return obj +# +# @classmethod +# def from_components( +# cls, +# u: Union[np.ndarray, list] = None, +# rp: Union[np.ndarray, list] = None, +# rd: Union[np.ndarray, list] = None, +# w: Union[np.ndarray, list] = None, +# ): +# """ +# Constructor of the class from the components of the natural coordinates +# """ +# +# if u is None: +# raise ValueError("u must be a numpy array (3x1) or a list of 3 elements") +# if rp is None: +# raise ValueError("rp must be a numpy array (3x1) or a list of 3 elements") +# if rd is None: +# raise ValueError("rd must be a numpy array (3x1) or a list of 3 elements") +# if w is None: +# raise ValueError("w must be a numpy array (3x1) or a list of 3 elements") +# +# if not isinstance(u, np.ndarray): +# u = np.array(u) +# if not isinstance(rp, np.ndarray): +# rp = np.array(rp) +# if not isinstance(rd, np.ndarray): +# rd = np.array(rd) +# if not isinstance(w, np.ndarray): +# w = np.array(w) +# +# if u.shape[0] != 3: +# raise ValueError("u must be a 3x1 numpy array") +# if rp.shape[0] != 3: +# raise ValueError("rp must be a 3x1 numpy array") +# if rd.shape[0] != 3: +# raise ValueError("rd must be a 3x1 numpy array") +# if w.shape[0] != 3: +# raise ValueError("v must be a 3x1 numpy array") +# +# input_array = np.concatenate((u, rp, rd, w), axis=0) +# +# if input_array.shape.__len__() == 1: +# input_array = input_array[:, np.newaxis] +# +# return cls(input_array) +# +# def to_array(self): +# return np.array(self).squeeze() +# +# @property +# def u(self): +# return self[0:3, :].to_array() +# +# @property +# def rp(self): +# return self[3:6, :].to_array() +# +# @property +# def rd(self): +# return self[6:9, :].to_array() +# +# @property +# def w(self): +# return self[9:12, :].to_array() +# +# @property +# def v(self): +# return self.rp - self.rd +# +# @property +# def vector(self): +# return self.to_array() +# +# def to_components(self): +# return self.u, self.rp, self.rd, self.w +# +# def to_uvw(self): +# return self.u, self.v, self.w +# +# def to_non_orthogonal_basis(self, vector: np.ndarray) -> np.ndarray: +# """ +# This function converts a vector expressed in the global coordinate system +# to a vector expressed in a non-orthogonal coordinate system associated to the segment coordinates. +# +# Parameters +# ---------- +# vector: np.ndarray +# The vector expressed in the global coordinate system +# +# Returns +# ------- +# np.ndarray +# The vector expressed in the non-orthogonal coordinate system +# +# """ +# return vnop_array(vector - self.rp, self.u, self.v, self.w) +# +# def to_interpolation_matrix(self, vector: np.ndarray) -> np.ndarray: +# """ +# This function converts a vector expressed in the global coordinate system +# to a vector expressed in a non-orthogonal coordinate system associated to the segment coordinates. +# +# Parameters +# ---------- +# vector: np.ndarray +# The vector expressed in the global coordinate system +# +# Returns +# ------- +# np.ndarray +# The vector expressed in the non-orthogonal coordinate system +# +# """ +# return interpolate_natural_vector(vnop_array(vector - self.rp, self.u, self.v, self.w)) class NaturalCoordinates(np.ndarray): diff --git a/examples/natural_coordinates.py b/examples/natural_coordinates.py new file mode 100644 index 00000000..4dd0146f --- /dev/null +++ b/examples/natural_coordinates.py @@ -0,0 +1,42 @@ +import numpy as np +from casadi import MX + +from bionc import array +from bionc import casadi_type +from bionc import bionc_casadi + +# CASADI EXAMPLE +print("CASADI EXAMPLE") +u = array(np.array([1, 2, 3]), instance_type=casadi_type) +rp = array([2, 2, 3], instance_type=casadi_type) +rd = MX.sym("rd", 3) +w = array([4, 2, 3], instance_type=casadi_type) +Q = bionc_casadi.SegmentNaturalCoordinates.from_components(u=u, rp=rp, rd=rd, w=w) + +print(Q) +print(Q.to_array()) +print(Q.u) +print(Q.rp) +print(Q.rp.shape) +print(Q.to_components()) +print("done") + +# NUMPY EXAMPLE +print("NUMPY EXAMPLE") +from bionc import numpy_type +from bionc import bionc_numpy + +u = array(np.array([1, 2, 3]), instance_type=numpy_type) +rp = array([2, 2, 3], instance_type=numpy_type) +rd = np.array([1, 2, 3]) +w = array([4, 2, 3], instance_type=numpy_type) +Q = bionc_numpy.SegmentNaturalCoordinates.from_components(u=u, rp=rp, rd=rd, w=w) + +print(Q) +print(Q.to_array()) +print(Q.u) +print(Q.rp) +print(Q.rp.shape) +print(Q.to_components()) +print("done") + From 2ddbba3ea3e27d28b62defc03855a0f77990cb82 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Wed, 9 Nov 2022 16:56:23 -0500 Subject: [PATCH 03/30] blacked --- bionc/__init__.py | 3 +-- bionc/math_interface/internal.py | 25 ++++++++----------- bionc/math_interface/math_interface.py | 4 +-- .../using_casadi/natural_coordinates.py | 1 + bionc/model_creation/marker_template.py | 1 + .../natural_segment_template.py | 1 + bionc/utils/__init__.py | 3 ++- examples/natural_coordinates.py | 1 - 8 files changed, 19 insertions(+), 20 deletions(-) diff --git a/bionc/__init__.py b/bionc/__init__.py index 14abadda..1afca94b 100644 --- a/bionc/__init__.py +++ b/bionc/__init__.py @@ -48,8 +48,7 @@ from casadi.casadi import MX as MX_type from numpy import ndarray + # global variable to store the type of the math interface casadi_type = MX_type numpy_type = ndarray - - diff --git a/bionc/math_interface/internal.py b/bionc/math_interface/internal.py index f6e5c2bf..f948fb1a 100644 --- a/bionc/math_interface/internal.py +++ b/bionc/math_interface/internal.py @@ -7,38 +7,38 @@ class SegmentNaturalCoordinates(Protocol): """ def __new__(cls, input_array): - """ Create a new instance of the class. """ + """Create a new instance of the class.""" @classmethod def from_components(cls, u, rp, rd, w): - """ Constructor of the class from the components of the natural coordinates """ + """Constructor of the class from the components of the natural coordinates""" def to_array(self): - """ This function returns the array of the natural coordinates """ + """This function returns the array of the natural coordinates""" def u(self): - """ This property returns the u vector of the natural coordinates """ + """This property returns the u vector of the natural coordinates""" def rp(self): - """ This property returns the rp vector of the natural coordinates """ + """This property returns the rp vector of the natural coordinates""" def rd(self): - """ This property returns the rd vector of the natural coordinates """ + """This property returns the rd vector of the natural coordinates""" def w(self): - """ This property returns the w vector of the natural coordinates """ + """This property returns the w vector of the natural coordinates""" def v(self): - """ This property returns the v vector of the natural coordinates """ + """This property returns the v vector of the natural coordinates""" def vector(self): - """ This property returns the vector of the natural coordinates """ + """This property returns the vector of the natural coordinates""" def to_components(self): - """ This function returns the components of the natural coordinates """ + """This function returns the components of the natural coordinates""" def to_uvw(self): - """ This function returns the uvw vector of the natural coordinates """ + """This function returns the uvw vector of the natural coordinates""" def to_non_orthogonal_basis(self, vector): """ @@ -51,6 +51,3 @@ def to_interpolation_matrix(self, vector): This function converts a vector expressed in the global coordinate system to a vector expressed in a non-orthogonal coordinate system associated to the segment coordinates. """ - - - diff --git a/bionc/math_interface/math_interface.py b/bionc/math_interface/math_interface.py index ae7c80b6..ca42da35 100644 --- a/bionc/math_interface/math_interface.py +++ b/bionc/math_interface/math_interface.py @@ -28,8 +28,8 @@ def array(data, instance_type: type) -> ndarray | MX: def symmetrize_upp(A, instance_type: type) -> ndarray | MX: """ - This function symmetrizes a matrix by copying the upper triangular part - to the lower triangular part conserving the diagonal. + This function symmetrizes a matrix by copying the upper triangular part + to the lower triangular part conserving the diagonal. """ if instance_type == ndarray: return np.tril(A) + np.tril(A, -1).T diff --git a/bionc/math_interface/using_casadi/natural_coordinates.py b/bionc/math_interface/using_casadi/natural_coordinates.py index 75d5546a..d7e18e68 100644 --- a/bionc/math_interface/using_casadi/natural_coordinates.py +++ b/bionc/math_interface/using_casadi/natural_coordinates.py @@ -1,5 +1,6 @@ from casadi import MX, vertcat from typing import Union + # from bionc.utils.vnop_array import vnop_array # from bionc.utils.interpolation_matrix import interpolate_natural_vector diff --git a/bionc/model_creation/marker_template.py b/bionc/model_creation/marker_template.py index 0922b0ae..3ee6baa2 100644 --- a/bionc/model_creation/marker_template.py +++ b/bionc/model_creation/marker_template.py @@ -8,6 +8,7 @@ # from .biomechanical_model_template import BiomechanicalModelTemplate from .protocols import Data from ..model_computations.natural_segment import NaturalSegment + # from ..utils.natural_coordinates import SegmentNaturalCoordinates from ..math_interface.internal import SegmentNaturalCoordinates diff --git a/bionc/model_creation/natural_segment_template.py b/bionc/model_creation/natural_segment_template.py index 4aacfefd..e46c601d 100644 --- a/bionc/model_creation/natural_segment_template.py +++ b/bionc/model_creation/natural_segment_template.py @@ -3,6 +3,7 @@ from ..model_computations.natural_axis import Axis from .natural_axis_template import AxisTemplate from ..model_computations.biomechanical_model import BiomechanicalModel + # from ..utils.natural_coordinates import SegmentNaturalCoordinates from ..math_interface.internal import SegmentNaturalCoordinates from .marker_template import MarkerTemplate diff --git a/bionc/utils/__init__.py b/bionc/utils/__init__.py index 04f3bde2..c04c57c5 100644 --- a/bionc/utils/__init__.py +++ b/bionc/utils/__init__.py @@ -1,6 +1,7 @@ from .homogenous_transform import HomogeneousTransform from .natural_coordinates import ( # SegmentNaturalCoordinates, \ - NaturalCoordinates) + NaturalCoordinates, +) from .natural_velocities import SegmentNaturalVelocities, NaturalVelocities from .natural_accelerations import SegmentNaturalAccelerations, NaturalAccelerations diff --git a/examples/natural_coordinates.py b/examples/natural_coordinates.py index 4dd0146f..d5e1c27e 100644 --- a/examples/natural_coordinates.py +++ b/examples/natural_coordinates.py @@ -39,4 +39,3 @@ print(Q.rp.shape) print(Q.to_components()) print("done") - From 90b720d8ae46c11972e0e2cf724556b326dc0944 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Wed, 9 Nov 2022 17:35:21 -0500 Subject: [PATCH 04/30] go for it --- bionc/__init__.py | 10 +- bionc/math_interface/internal.py | 36 ++++++ bionc/math_interface/using_casadi/__init__.py | 2 +- .../using_casadi/natural_coordinates.py | 111 ++++++++++-------- bionc/math_interface/using_numpy/__init__.py | 2 +- .../using_numpy/natural_coordinates.py | 102 ++++++++-------- bionc/model_computations/joint.py | 3 +- .../natural_segment_template.py | 2 +- bionc/utils/natural_coordinates.py | 1 + tests/test_biomech_model.py | 12 +- 10 files changed, 165 insertions(+), 116 deletions(-) diff --git a/bionc/__init__.py b/bionc/__init__.py index 1afca94b..4190fdc0 100644 --- a/bionc/__init__.py +++ b/bionc/__init__.py @@ -1,7 +1,7 @@ -from .utils.natural_coordinates import ( - # SegmentNaturalCoordinates, - NaturalCoordinates, -) +# from .utils.natural_coordinates import ( +# # SegmentNaturalCoordinates, +# # NaturalCoordinates, +# ) from .utils.natural_velocities import SegmentNaturalVelocities, NaturalVelocities from .utils.natural_accelerations import SegmentNaturalAccelerations, NaturalAccelerations from .utils.homogenous_transform import HomogeneousTransform @@ -44,7 +44,7 @@ from .math_interface import using_numpy as bionc_numpy # I don't know if it's useful to import the following yet -from .math_interface.internal import SegmentNaturalCoordinates +from .math_interface.internal import SegmentNaturalCoordinates, NaturalCoordinates from casadi.casadi import MX as MX_type from numpy import ndarray diff --git a/bionc/math_interface/internal.py b/bionc/math_interface/internal.py index f948fb1a..8306920c 100644 --- a/bionc/math_interface/internal.py +++ b/bionc/math_interface/internal.py @@ -51,3 +51,39 @@ def to_interpolation_matrix(self, vector): This function converts a vector expressed in the global coordinate system to a vector expressed in a non-orthogonal coordinate system associated to the segment coordinates. """ + + +class NaturalCoordinates(Protocol): + """ + This class is made to handle Natural coordinates of several segments + """ + + def __new__(cls, input_array): + """Create a new instance of the class.""" + + @classmethod + def from_qi(cls, tuple_of_Q): + """Constructor of the class from the components of the natural coordinates""" + + def to_array(self): + """This function returns the array of the natural coordinates""" + + def nb_qi(self): + """This function returns the number of qi""" + def u(self, segment_index): + """This property returns the u vector of the natural coordinates""" + + def rp(self, segment_index): + """This property returns the rp vector of the natural coordinates""" + + def rd(self, segment_index): + """This property returns the rd vector of the natural coordinates""" + + def w(self, segment_index): + """This property returns the w vector of the natural coordinates""" + + def v(self, segment_index): + """This property returns the v vector of the natural coordinates""" + + def vector(self): + """This property returns the vector of the natural coordinates""" diff --git a/bionc/math_interface/using_casadi/__init__.py b/bionc/math_interface/using_casadi/__init__.py index b40eae23..1a705bd6 100644 --- a/bionc/math_interface/using_casadi/__init__.py +++ b/bionc/math_interface/using_casadi/__init__.py @@ -1 +1 @@ -from .natural_coordinates import SegmentNaturalCoordinates +from .natural_coordinates import SegmentNaturalCoordinates, NaturalCoordinates diff --git a/bionc/math_interface/using_casadi/natural_coordinates.py b/bionc/math_interface/using_casadi/natural_coordinates.py index d7e18e68..1dcd8da6 100644 --- a/bionc/math_interface/using_casadi/natural_coordinates.py +++ b/bionc/math_interface/using_casadi/natural_coordinates.py @@ -1,3 +1,4 @@ +import numpy as np from casadi import MX, vertcat from typing import Union @@ -132,54 +133,62 @@ def to_uvw(self): # return interpolate_natural_vector(vnop_array(vector - self.rp, self.u, self.v, self.w)) -# class NaturalCoordinates(np.ndarray): -# def __new__(cls, input_array: np.ndarray): -# """ -# Create a new instance of the class. -# """ -# -# return np.asarray(input_array).view(cls) -# -# @classmethod -# def from_qi(cls, tuple_of_Q: tuple): -# """ -# Constructor of the class. -# """ -# if not isinstance(tuple_of_Q, tuple): -# raise ValueError("tuple_of_Q must be a tuple of SegmentGeneralizedCoordinates") -# -# for Q in tuple_of_Q: -# if not isinstance(Q, SegmentNaturalCoordinates): -# raise ValueError("tuple_of_Q must be a tuple of SegmentGeneralizedCoordinates") -# -# input_array = np.concatenate(tuple_of_Q, axis=0) -# return cls(input_array) -# -# def to_array(self): -# return np.array(self).squeeze() -# -# def nb_qi(self): -# return self.shape[0] // 12 -# -# def u(self, segment_idx: int): -# array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12)[0:3] -# return self[array_idx, :].to_array() -# -# def rp(self, segment_idx: int): -# array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12)[3:6] -# return self[array_idx, :].to_array() -# -# def rd(self, segment_idx: int): -# array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12)[6:9] -# return self[array_idx, :].to_array() -# -# def w(self, segment_idx: int): -# array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12)[9:12] -# return self[array_idx, :].to_array() -# -# def v(self, segment_idx: int): -# return self.rp(segment_idx) - self.rd(segment_idx) -# -# def vector(self, segment_idx: int): -# array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12) -# return SegmentNaturalCoordinates(self[array_idx, :].to_array()) +class NaturalCoordinates(MX): + def __new__(cls, input_array: MX): + """ + Create a new instance of the class. + """ + + if input_array.shape[0] % 12 != 0: + raise ValueError("input_array must be a column vector of size 12 x n elements") + + obj = MX.__new__(cls) + + return obj + + @classmethod + def from_qi(cls, tuple_of_Q: tuple): + """ + Constructor of the class. + """ + if not isinstance(tuple_of_Q, tuple): + raise ValueError("tuple_of_Q must be a tuple of SegmentGeneralizedCoordinates") + + for Q in tuple_of_Q: + if not isinstance(Q, SegmentNaturalCoordinates): + raise ValueError("tuple_of_Q must be a tuple of SegmentGeneralizedCoordinates") + + input_array = vertcat(*tuple_of_Q) + return cls(input_array) + + def to_array(self): + return self + + def nb_qi(self): + return self.shape[0] // 12 + + def u(self, segment_idx: int): + array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12)[0:3] + return self[array_idx, :] + + def rp(self, segment_idx: int): + array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12)[3:6] + return self[array_idx, :] + + def rd(self, segment_idx: int): + array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12)[6:9] + return self[array_idx, :] + + def w(self, segment_idx: int): + array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12)[9:12] + return self[array_idx, :] + + def v(self, segment_idx: int): + return self.rp(segment_idx) - self.rd(segment_idx) + + def vector(self, segment_idx: int = None): + if segment_idx is None: + return self[:] + else: + array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12).tolist() + return SegmentNaturalCoordinates.MX(self[array_idx]) diff --git a/bionc/math_interface/using_numpy/__init__.py b/bionc/math_interface/using_numpy/__init__.py index 0ef3875e..005f29e8 100644 --- a/bionc/math_interface/using_numpy/__init__.py +++ b/bionc/math_interface/using_numpy/__init__.py @@ -1 +1 @@ -from .natural_coordinates import SegmentNaturalCoordinates +from .natural_coordinates import SegmentNaturalCoordinates, NaturalCoordinates diff --git a/bionc/math_interface/using_numpy/natural_coordinates.py b/bionc/math_interface/using_numpy/natural_coordinates.py index 24a41d67..81b90b01 100644 --- a/bionc/math_interface/using_numpy/natural_coordinates.py +++ b/bionc/math_interface/using_numpy/natural_coordinates.py @@ -137,54 +137,54 @@ def to_interpolation_matrix(self, vector: np.ndarray) -> np.ndarray: return interpolate_natural_vector(vnop_array(vector - self.rp, self.u, self.v, self.w)) -# class NaturalCoordinates(np.ndarray): -# def __new__(cls, input_array: np.ndarray): -# """ -# Create a new instance of the class. -# """ -# -# return np.asarray(input_array).view(cls) -# -# @classmethod -# def from_qi(cls, tuple_of_Q: tuple): -# """ -# Constructor of the class. -# """ -# if not isinstance(tuple_of_Q, tuple): -# raise ValueError("tuple_of_Q must be a tuple of SegmentGeneralizedCoordinates") -# -# for Q in tuple_of_Q: -# if not isinstance(Q, SegmentNaturalCoordinates): -# raise ValueError("tuple_of_Q must be a tuple of SegmentGeneralizedCoordinates") -# -# input_array = np.concatenate(tuple_of_Q, axis=0) -# return cls(input_array) -# -# def to_array(self): -# return np.array(self).squeeze() -# -# def nb_qi(self): -# return self.shape[0] // 12 -# -# def u(self, segment_idx: int): -# array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12)[0:3] -# return self[array_idx, :].to_array() -# -# def rp(self, segment_idx: int): -# array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12)[3:6] -# return self[array_idx, :].to_array() -# -# def rd(self, segment_idx: int): -# array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12)[6:9] -# return self[array_idx, :].to_array() -# -# def w(self, segment_idx: int): -# array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12)[9:12] -# return self[array_idx, :].to_array() -# -# def v(self, segment_idx: int): -# return self.rp(segment_idx) - self.rd(segment_idx) -# -# def vector(self, segment_idx: int): -# array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12) -# return SegmentNaturalCoordinates(self[array_idx, :].to_array()) +class NaturalCoordinates(np.ndarray): + def __new__(cls, input_array: np.ndarray): + """ + Create a new instance of the class. + """ + + return np.asarray(input_array).view(cls) + + @classmethod + def from_qi(cls, tuple_of_Q: tuple): + """ + Constructor of the class. + """ + if not isinstance(tuple_of_Q, tuple): + raise ValueError("tuple_of_Q must be a tuple of SegmentGeneralizedCoordinates") + + for Q in tuple_of_Q: + if not isinstance(Q, SegmentNaturalCoordinates): + raise ValueError("tuple_of_Q must be a tuple of SegmentGeneralizedCoordinates") + + input_array = np.concatenate(tuple_of_Q, axis=0) + return cls(input_array) + + def to_array(self): + return np.array(self).squeeze() + + def nb_qi(self): + return self.shape[0] // 12 + + def u(self, segment_idx: int): + array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12)[0:3] + return self[array_idx, :].to_array() + + def rp(self, segment_idx: int): + array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12)[3:6] + return self[array_idx, :].to_array() + + def rd(self, segment_idx: int): + array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12)[6:9] + return self[array_idx, :].to_array() + + def w(self, segment_idx: int): + array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12)[9:12] + return self[array_idx, :].to_array() + + def v(self, segment_idx: int): + return self.rp(segment_idx) - self.rd(segment_idx) + + def vector(self, segment_idx: int): + array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12) + return SegmentNaturalCoordinates(self[array_idx, :].to_array()) diff --git a/bionc/model_computations/joint.py b/bionc/model_computations/joint.py index 0b104fbe..fb6e1a64 100644 --- a/bionc/model_computations/joint.py +++ b/bionc/model_computations/joint.py @@ -3,7 +3,8 @@ import numpy as np from .natural_segment import NaturalSegment -from ..utils.natural_coordinates import NaturalCoordinates, SegmentNaturalCoordinates +from ..utils.natural_coordinates import NaturalCoordinates +from ..math_interface.internal import SegmentNaturalCoordinates class JointBase(ABC): diff --git a/bionc/model_creation/natural_segment_template.py b/bionc/model_creation/natural_segment_template.py index e46c601d..03282b93 100644 --- a/bionc/model_creation/natural_segment_template.py +++ b/bionc/model_creation/natural_segment_template.py @@ -61,7 +61,7 @@ def experimental_Q(self, data: Data, kinematic_chain: BiomechanicalModel) -> Seg SegmentNaturalCoordinates The Segment Natural Coordinates Q (12 x n_frames) """ - + from ..math_interface.using_numpy import SegmentNaturalCoordinates self.Q = SegmentNaturalCoordinates.from_components( u=self.u_axis.to_axis(data, kinematic_chain).axis()[:3, :], rp=self.proximal_point.to_marker(data, kinematic_chain).position[:3, :], diff --git a/bionc/utils/natural_coordinates.py b/bionc/utils/natural_coordinates.py index c9d729ee..6ab35f5e 100644 --- a/bionc/utils/natural_coordinates.py +++ b/bionc/utils/natural_coordinates.py @@ -2,6 +2,7 @@ from typing import Union from bionc.utils.vnop_array import vnop_array from bionc.utils.interpolation_matrix import interpolate_natural_vector +from ..math_interface.internal import SegmentNaturalCoordinates # class SegmentNaturalCoordinates(np.ndarray): diff --git a/tests/test_biomech_model.py b/tests/test_biomech_model.py index 2747320c..84088d72 100644 --- a/tests/test_biomech_model.py +++ b/tests/test_biomech_model.py @@ -3,7 +3,9 @@ from .utils import TestUtils -from bionc import SegmentNaturalCoordinates, NaturalCoordinates, SegmentNaturalVelocities, NaturalVelocities +from bionc import SegmentNaturalCoordinates, NaturalCoordinates, \ + SegmentNaturalVelocities, NaturalVelocities +from bionc import bionc_numpy as bionc_np def test_biomech_model(): @@ -29,26 +31,26 @@ def test_biomech_model(): assert natural_model.nb_Qddot() == 36 # Test rigid body constraints - Q1 = SegmentNaturalCoordinates.from_components( + Q1 = bionc_np.SegmentNaturalCoordinates.from_components( u=[1, 2, 3.05], rp=[1.1, 1, 3.1], rd=[1.2, 2, 4.1], w=[1.3, 2, 5.1], ) - Q2 = SegmentNaturalCoordinates.from_components( + Q2 = bionc_np.SegmentNaturalCoordinates.from_components( u=[1.4, 2, 3.2], rp=[1.5, 1, 3.2], rd=[1.6, 2, 4.2], w=[1.7, 2, 5.2], ) - Q3 = SegmentNaturalCoordinates.from_components( + Q3 = bionc_np.SegmentNaturalCoordinates.from_components( u=[1.8, 2, 3.3], rp=[1.9, 1, 3.3], rd=[2.1, 2, 4.3], w=[2.2, 2, 5.3], ) - Q = NaturalCoordinates.from_qi((Q1, Q2, Q3)) + Q = bionc_np.NaturalCoordinates.from_qi((Q1, Q2, Q3)) np.testing.assert_array_almost_equal( natural_model.rigid_body_constraints(Q), From 26538f7f61b736b66d1e374130fd4dc3a18681bc Mon Sep 17 00:00:00 2001 From: Ipuch Date: Wed, 9 Nov 2022 17:36:09 -0500 Subject: [PATCH 05/30] remove if isinstance I had to remove so much if, i'm not able to check the types anymore --- bionc/model_computations/biomechanical_model.py | 8 -------- bionc/model_computations/natural_segment.py | 15 ++++----------- 2 files changed, 4 insertions(+), 19 deletions(-) diff --git a/bionc/model_computations/biomechanical_model.py b/bionc/model_computations/biomechanical_model.py index 8b068eed..93abd4fd 100644 --- a/bionc/model_computations/biomechanical_model.py +++ b/bionc/model_computations/biomechanical_model.py @@ -65,9 +65,6 @@ def rigid_body_constraints(self, Q: NaturalCoordinates) -> np.ndarray: Rigid body constraints of the segment [6 * nb_segments, 1] """ - if not isinstance(Q, NaturalCoordinates): - Q = NaturalCoordinates(Q) - Phi_r = np.zeros(6 * self.nb_segments()) for i, segment_name in enumerate(self.segments): idx = slice(6 * i, 6 * (i + 1)) @@ -85,8 +82,6 @@ def rigid_body_constraints_jacobian(self, Q: NaturalCoordinates) -> np.ndarray: np.ndarray Rigid body constraints of the segment [6 * nb_segments, nbQ] """ - if not isinstance(Q, NaturalCoordinates): - Q = NaturalCoordinates(Q) K_r = np.zeros((6 * self.nb_segments(), Q.shape[0])) for i, segment_name in enumerate(self.segments): @@ -111,9 +106,6 @@ def rigid_body_constraint_jacobian_derivative(self, Qdot: NaturalVelocities) -> The derivative of the Jacobian matrix of the rigid body constraints [6, 12] """ - if not isinstance(Qdot, NaturalVelocities): - Qdot = NaturalVelocities(Qdot) - Kr_dot = np.zeros((6 * self.nb_segments(), Qdot.shape[0])) for i, segment_name in enumerate(self.segments): idx_row = slice(6 * i, 6 * (i + 1)) diff --git a/bionc/model_computations/natural_segment.py b/bionc/model_computations/natural_segment.py index 52e48bad..daf9005e 100644 --- a/bionc/model_computations/natural_segment.py +++ b/bionc/model_computations/natural_segment.py @@ -162,9 +162,8 @@ def parameters_from_Q(Q: SegmentNaturalCoordinates) -> tuple: tuple The parameters of the segment (alpha, beta, gamma, length) """ - - if not isinstance(Q, SegmentNaturalCoordinates): - Q = SegmentNaturalCoordinates(Q) + from ..math_interface.using_numpy import SegmentNaturalCoordinates + Q = SegmentNaturalCoordinates(Q) u, rp, rd, w = Q.to_components() @@ -307,8 +306,8 @@ def rigid_body_constraint(self, Qi: Union[SegmentNaturalCoordinates, np.ndarray] np.ndarray Rigid body constraints of the segment [6 x 1 x N_frame] """ - if not isinstance(Qi, SegmentNaturalCoordinates): - Qi = SegmentNaturalCoordinates(Qi) + # if not isinstance(Qi, SegmentNaturalCoordinates): + # Qi = SegmentNaturalCoordinates(Qi) phir = zeros(6) # phir[0] = sum(Qi.u**2, 0) - 1 @@ -372,12 +371,6 @@ def rigid_body_constraint_jacobian_derivative(Qdoti: SegmentNaturalVelocities) - Kr_dot : np.ndarray derivative of the Jacobian matrix of the rigid body constraints denoted Kr_dot [6 x 12 ] """ - if isinstance(Qdoti, SegmentNaturalCoordinates): - raise TypeError("Qdoti should be a SegmentNaturalVelocities object") - # not able to check if Qdoti is a SegmentNaturalVelocities if Qdoti is a np.ndarray - if not isinstance(Qdoti, SegmentNaturalVelocities): - Qdoti = SegmentNaturalVelocities(Qdoti) - # initialisation Kr_dot = zeros((6, 12)) From 2c7daddc32793423a369ab496d7c5bb6d5ed5e58 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Wed, 9 Nov 2022 17:41:09 -0500 Subject: [PATCH 06/30] all tests works --- tests/test_marker.py | 11 ++++++----- tests/test_natural_coordinates.py | 13 +++++++------ tests/test_natural_segment.py | 3 ++- 3 files changed, 15 insertions(+), 12 deletions(-) diff --git a/tests/test_marker.py b/tests/test_marker.py index 0bfe0895..d5a01c49 100644 --- a/tests/test_marker.py +++ b/tests/test_marker.py @@ -2,12 +2,13 @@ import numpy as np from bionc import SegmentMarker, Marker, SegmentNaturalCoordinates +from bionc import bionc_numpy as bionc_np def test_segment_marker(): with pytest.raises(ValueError, match="Either a position or an interpolation matrix must be provided"): - segment_marker = SegmentMarker( + SegmentMarker( name="my_marker", parent_name="Thigh", position=None, @@ -17,7 +18,7 @@ def test_segment_marker(): ) with pytest.raises(ValueError, match="The position must be a 3d vector"): - segment_marker = SegmentMarker( + SegmentMarker( name="my_marker", parent_name="Thigh", position=np.zeros(2), @@ -27,7 +28,7 @@ def test_segment_marker(): ) with pytest.raises(ValueError, match="The interpolation matrix must be a 3x12 matrix"): - segment_marker = SegmentMarker( + SegmentMarker( name="my_marker", parent_name="Thigh", position=None, @@ -37,7 +38,7 @@ def test_segment_marker(): ) with pytest.raises(ValueError, match="position and interpolation matrix cannot both be provided"): - segment_marker = SegmentMarker( + SegmentMarker( name="my_marker", parent_name="Thigh", position=np.zeros(3), @@ -69,7 +70,7 @@ def test_segment_marker(): assert not segment_marker.is_anatomical marker_location = np.array([1, 2, 3]) - Qi = SegmentNaturalCoordinates.from_components( + Qi = bionc_np.SegmentNaturalCoordinates.from_components( u=[1, 2, 3], rp=[1, 1, 3], rd=[1, 2, 4], diff --git a/tests/test_natural_coordinates.py b/tests/test_natural_coordinates.py index 6dc6f2fc..3b205de4 100644 --- a/tests/test_natural_coordinates.py +++ b/tests/test_natural_coordinates.py @@ -7,12 +7,13 @@ SegmentNaturalVelocities, NaturalAccelerations, SegmentNaturalAccelerations, + bionc_numpy as bionc_np, ) def test_SegmentNaturalCoordinatesCreator(): - Qi = SegmentNaturalCoordinates.from_components( + Qi = bionc_np.SegmentNaturalCoordinates.from_components( u=np.array([0, 0, 0]), rp=np.array([4, 5, 6]), rd=np.array([7, 8, 9]), @@ -42,13 +43,13 @@ def test_NaturalAccelerationsCreator(): def test_concatenate(): - Q1 = SegmentNaturalCoordinates.from_components( + Q1 = bionc_np.SegmentNaturalCoordinates.from_components( u=np.array([1, 2, 3]), rp=np.array([4, 5, 6]), rd=np.array([7, 8, 9]), w=np.array([10, 11, 12]), ) - Q2 = SegmentNaturalCoordinates.from_components( + Q2 = bionc_np.SegmentNaturalCoordinates.from_components( u=np.array([11, 22, 33]), rp=np.array([4, 5, 6]), rd=np.array([7, 8, 9]), @@ -83,20 +84,20 @@ def test_concatenate_accelerations(): # Build a class called GeneralizedCoordinates to handle the concatenation of SegmentGeneralizedCoordinates def test_NaturalCoordinatesConstructor(): - Q1 = SegmentNaturalCoordinates.from_components( + Q1 = bionc_np.SegmentNaturalCoordinates.from_components( u=np.array([1, 2, 3]), rp=np.array([4, 5, 6]), rd=np.array([7, 8, 9]), w=np.array([10, 11, 12]), ) print(Q1.v) - Q2 = SegmentNaturalCoordinates.from_components( + Q2 = bionc_np.SegmentNaturalCoordinates.from_components( u=np.array([11, 22, 33]), rp=np.array([4, 5, 6]), rd=np.array([7, 8, 9]), w=np.array([10, 11, 12]), ) - Q = NaturalCoordinates.from_qi((Q1, Q2)) + Q = bionc_np.NaturalCoordinates.from_qi((Q1, Q2)) np.testing.assert_equal(Q.u(0), np.array([1, 2, 3])) np.testing.assert_equal(Q.u(1), np.array([11, 22, 33])) np.testing.assert_equal(Q.v(0), -np.array([7, 8, 9]) + np.array([4, 5, 6])) diff --git a/tests/test_natural_segment.py b/tests/test_natural_segment.py index f0122784..b293601b 100644 --- a/tests/test_natural_segment.py +++ b/tests/test_natural_segment.py @@ -2,6 +2,7 @@ NaturalSegment, SegmentMarker, SegmentNaturalCoordinates, + bionc_numpy as bionc_np, ) import numpy as np import pytest @@ -61,7 +62,7 @@ def test_marker_features(): my_segment.add_marker(marker1) my_segment.add_marker(marker2) - Qi = SegmentNaturalCoordinates.from_components( + Qi = bionc_np.SegmentNaturalCoordinates.from_components( u=[1, 2, 3], rp=[1, 1, 3], rd=[1, 2, 4], From 21cde21763d8e6039fc4908761cec86700a560e2 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Wed, 9 Nov 2022 17:44:54 -0500 Subject: [PATCH 07/30] unecessary import --- tests/test_marker.py | 2 +- tests/test_natural_coordinates.py | 4 ---- tests/test_natural_segment.py | 1 - 3 files changed, 1 insertion(+), 6 deletions(-) diff --git a/tests/test_marker.py b/tests/test_marker.py index d5a01c49..7d22fb58 100644 --- a/tests/test_marker.py +++ b/tests/test_marker.py @@ -1,7 +1,7 @@ import pytest import numpy as np -from bionc import SegmentMarker, Marker, SegmentNaturalCoordinates +from bionc import SegmentMarker from bionc import bionc_numpy as bionc_np diff --git a/tests/test_natural_coordinates.py b/tests/test_natural_coordinates.py index 3b205de4..2545dc11 100644 --- a/tests/test_natural_coordinates.py +++ b/tests/test_natural_coordinates.py @@ -1,10 +1,6 @@ import numpy as np import pytest from bionc import ( - NaturalCoordinates, - SegmentNaturalCoordinates, - NaturalVelocities, - SegmentNaturalVelocities, NaturalAccelerations, SegmentNaturalAccelerations, bionc_numpy as bionc_np, diff --git a/tests/test_natural_segment.py b/tests/test_natural_segment.py index b293601b..a87d438f 100644 --- a/tests/test_natural_segment.py +++ b/tests/test_natural_segment.py @@ -1,7 +1,6 @@ from bionc import ( NaturalSegment, SegmentMarker, - SegmentNaturalCoordinates, bionc_numpy as bionc_np, ) import numpy as np From 37b510e20fc18ba6025b32535618712e0efcfc92 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Wed, 9 Nov 2022 17:45:26 -0500 Subject: [PATCH 08/30] black --- bionc/math_interface/internal.py | 1 + bionc/model_computations/natural_segment.py | 1 + bionc/model_creation/natural_segment_template.py | 1 + tests/test_biomech_model.py | 3 +-- 4 files changed, 4 insertions(+), 2 deletions(-) diff --git a/bionc/math_interface/internal.py b/bionc/math_interface/internal.py index 8306920c..dd8e1159 100644 --- a/bionc/math_interface/internal.py +++ b/bionc/math_interface/internal.py @@ -70,6 +70,7 @@ def to_array(self): def nb_qi(self): """This function returns the number of qi""" + def u(self, segment_index): """This property returns the u vector of the natural coordinates""" diff --git a/bionc/model_computations/natural_segment.py b/bionc/model_computations/natural_segment.py index daf9005e..12d65b5a 100644 --- a/bionc/model_computations/natural_segment.py +++ b/bionc/model_computations/natural_segment.py @@ -163,6 +163,7 @@ def parameters_from_Q(Q: SegmentNaturalCoordinates) -> tuple: The parameters of the segment (alpha, beta, gamma, length) """ from ..math_interface.using_numpy import SegmentNaturalCoordinates + Q = SegmentNaturalCoordinates(Q) u, rp, rd, w = Q.to_components() diff --git a/bionc/model_creation/natural_segment_template.py b/bionc/model_creation/natural_segment_template.py index 03282b93..123fbeac 100644 --- a/bionc/model_creation/natural_segment_template.py +++ b/bionc/model_creation/natural_segment_template.py @@ -62,6 +62,7 @@ def experimental_Q(self, data: Data, kinematic_chain: BiomechanicalModel) -> Seg The Segment Natural Coordinates Q (12 x n_frames) """ from ..math_interface.using_numpy import SegmentNaturalCoordinates + self.Q = SegmentNaturalCoordinates.from_components( u=self.u_axis.to_axis(data, kinematic_chain).axis()[:3, :], rp=self.proximal_point.to_marker(data, kinematic_chain).position[:3, :], diff --git a/tests/test_biomech_model.py b/tests/test_biomech_model.py index 84088d72..87cbbee0 100644 --- a/tests/test_biomech_model.py +++ b/tests/test_biomech_model.py @@ -3,8 +3,7 @@ from .utils import TestUtils -from bionc import SegmentNaturalCoordinates, NaturalCoordinates, \ - SegmentNaturalVelocities, NaturalVelocities +from bionc import SegmentNaturalCoordinates, NaturalCoordinates, SegmentNaturalVelocities, NaturalVelocities from bionc import bionc_numpy as bionc_np From 74c74e3e4ee71c4b9e8f42a82d56a79b33b12e5d Mon Sep 17 00:00:00 2001 From: Ipuch Date: Wed, 9 Nov 2022 18:21:38 -0500 Subject: [PATCH 09/30] moving to math_interface with protocols --- bionc/__init__.py | 6 +- bionc/math_interface/protocols/__init__.py | 0 .../protocols/natural_accelerations.py | 95 ++++++++++++ .../natural_coordinates.py} | 24 +++ .../protocols/natural_velocities.py | 95 ++++++++++++ .../using_casadi/natural_accelerations.py | 146 ++++++++++++++++++ .../using_casadi/natural_velocities.py | 145 +++++++++++++++++ .../using_numpy/natural_accelerations.py | 140 +++++++++++++++++ .../using_numpy/natural_velocities.py | 139 +++++++++++++++++ bionc/model_computations/joint.py | 3 +- bionc/model_computations/natural_marker.py | 2 +- bionc/model_computations/natural_segment.py | 7 +- bionc/model_creation/marker_template.py | 2 +- .../natural_segment_template.py | 3 +- bionc/utils/natural_coordinates.py | 5 +- 15 files changed, 794 insertions(+), 18 deletions(-) create mode 100644 bionc/math_interface/protocols/__init__.py create mode 100644 bionc/math_interface/protocols/natural_accelerations.py rename bionc/math_interface/{internal.py => protocols/natural_coordinates.py} (89%) create mode 100644 bionc/math_interface/protocols/natural_velocities.py create mode 100644 bionc/math_interface/using_casadi/natural_accelerations.py create mode 100644 bionc/math_interface/using_casadi/natural_velocities.py create mode 100644 bionc/math_interface/using_numpy/natural_accelerations.py create mode 100644 bionc/math_interface/using_numpy/natural_velocities.py diff --git a/bionc/__init__.py b/bionc/__init__.py index 4190fdc0..b7ee0699 100644 --- a/bionc/__init__.py +++ b/bionc/__init__.py @@ -6,7 +6,6 @@ from .utils.natural_accelerations import SegmentNaturalAccelerations, NaturalAccelerations from .utils.homogenous_transform import HomogeneousTransform from .utils.interpolation_matrix import interpolate_natural_vector, to_natural_vector -from .utils.natural_coordinates import vnop_array from .model_creation import ( AxisTemplate, @@ -38,13 +37,12 @@ horzcat, ) - -from .math_interface import internal +from .math_interface.protocols import natural_coordinates from .math_interface import using_casadi as bionc_casadi from .math_interface import using_numpy as bionc_numpy # I don't know if it's useful to import the following yet -from .math_interface.internal import SegmentNaturalCoordinates, NaturalCoordinates +from bionc.math_interface.protocols.natural_coordinates import SegmentNaturalCoordinates, NaturalCoordinates from casadi.casadi import MX as MX_type from numpy import ndarray diff --git a/bionc/math_interface/protocols/__init__.py b/bionc/math_interface/protocols/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/bionc/math_interface/protocols/natural_accelerations.py b/bionc/math_interface/protocols/natural_accelerations.py new file mode 100644 index 00000000..3e6a6dcc --- /dev/null +++ b/bionc/math_interface/protocols/natural_accelerations.py @@ -0,0 +1,95 @@ +from typing import Protocol + + +class SegmentNaturalAccelerations(Protocol): + """ + This class is made to handle Generalized Coordinates of a Segment + """ + + def __new__(cls, input_array): + """Create a new instance of the class.""" + ... + + @classmethod + def from_components(cls, uddot, rpddot, rdddot, wddot): + """Constructor of the class from the components of the natural coordinates""" + ... + + def to_array(self): + """This function returns the array of the natural coordinates""" + ... + + def uddot(self): + """This property returns the u vector of the natural coordinates""" + ... + + def rpddot(self): + """This property returns the rp vector of the natural coordinates""" + ... + + def rdddot(self): + """This property returns the rd vector of the natural coordinates""" + ... + + def wddot(self): + """This property returns the w vector of the natural coordinates""" + ... + + def vddot(self): + """This property returns the v vector of the natural coordinates""" + ... + + def vector(self): + """This property returns the vector of the natural coordinates""" + ... + + def to_components(self): + """This function returns the components of the natural coordinates""" + ... + + +class NaturalAccelerations(Protocol): + """ + This class is made to handle Natural coordinates of several segments + """ + + def __new__(cls, input_array): + """Create a new instance of the class.""" + ... + + @classmethod + def from_qi(cls, tuple_of_Q): + """Constructor of the class from the components of the natural coordinates""" + ... + + def to_array(self): + """This function returns the array of the natural coordinates""" + ... + + def nb_qddoti(self): + """This function returns the number of qi""" + ... + + def uddot(self, segment_index): + """This property returns the u vector of the natural coordinates""" + ... + + def rpddot(self, segment_index): + """This property returns the rp vector of the natural coordinates""" + ... + + def rdddot(self, segment_index): + """This property returns the rd vector of the natural coordinates""" + ... + + def wddot(self, segment_index): + """This property returns the w vector of the natural coordinates""" + ... + + def vddot(self, segment_index): + """This property returns the v vector of the natural coordinates""" + ... + + def vector(self): + """This property returns the vector of the natural coordinates""" + ... diff --git a/bionc/math_interface/internal.py b/bionc/math_interface/protocols/natural_coordinates.py similarity index 89% rename from bionc/math_interface/internal.py rename to bionc/math_interface/protocols/natural_coordinates.py index dd8e1159..402d38e4 100644 --- a/bionc/math_interface/internal.py +++ b/bionc/math_interface/protocols/natural_coordinates.py @@ -8,37 +8,48 @@ class SegmentNaturalCoordinates(Protocol): def __new__(cls, input_array): """Create a new instance of the class.""" + ... @classmethod def from_components(cls, u, rp, rd, w): """Constructor of the class from the components of the natural coordinates""" + ... def to_array(self): """This function returns the array of the natural coordinates""" + ... def u(self): """This property returns the u vector of the natural coordinates""" + ... def rp(self): """This property returns the rp vector of the natural coordinates""" + ... def rd(self): """This property returns the rd vector of the natural coordinates""" + ... def w(self): """This property returns the w vector of the natural coordinates""" + ... def v(self): """This property returns the v vector of the natural coordinates""" + ... def vector(self): """This property returns the vector of the natural coordinates""" + ... def to_components(self): """This function returns the components of the natural coordinates""" + ... def to_uvw(self): """This function returns the uvw vector of the natural coordinates""" + ... def to_non_orthogonal_basis(self, vector): """ @@ -46,11 +57,14 @@ def to_non_orthogonal_basis(self, vector): to a vector expressed in a non-orthogonal coordinate system associated to the segment coordinates. """ + ... + def to_interpolation_matrix(self, vector): """ This function converts a vector expressed in the global coordinate system to a vector expressed in a non-orthogonal coordinate system associated to the segment coordinates. """ + ... class NaturalCoordinates(Protocol): @@ -60,31 +74,41 @@ class NaturalCoordinates(Protocol): def __new__(cls, input_array): """Create a new instance of the class.""" + ... @classmethod def from_qi(cls, tuple_of_Q): """Constructor of the class from the components of the natural coordinates""" + ... def to_array(self): """This function returns the array of the natural coordinates""" + ... def nb_qi(self): """This function returns the number of qi""" + ... def u(self, segment_index): """This property returns the u vector of the natural coordinates""" + ... def rp(self, segment_index): """This property returns the rp vector of the natural coordinates""" + ... def rd(self, segment_index): """This property returns the rd vector of the natural coordinates""" + ... def w(self, segment_index): """This property returns the w vector of the natural coordinates""" + ... def v(self, segment_index): """This property returns the v vector of the natural coordinates""" + ... def vector(self): """This property returns the vector of the natural coordinates""" + ... diff --git a/bionc/math_interface/protocols/natural_velocities.py b/bionc/math_interface/protocols/natural_velocities.py new file mode 100644 index 00000000..6b4d15cc --- /dev/null +++ b/bionc/math_interface/protocols/natural_velocities.py @@ -0,0 +1,95 @@ +from typing import Protocol + + +class SegmentNaturalVelocities(Protocol): + """ + This class is made to handle Generalized Coordinates of a Segment + """ + + def __new__(cls, input_array): + """Create a new instance of the class.""" + ... + + @classmethod + def from_components(cls, udot, rpdot, rddot, wdot): + """Constructor of the class from the components of the natural coordinates""" + ... + + def to_array(self): + """This function returns the array of the natural coordinates""" + ... + + def udot(self): + """This property returns the u vector of the natural coordinates""" + ... + + def rpdot(self): + """This property returns the rp vector of the natural coordinates""" + ... + + def rddot(self): + """This property returns the rd vector of the natural coordinates""" + ... + + def wdot(self): + """This property returns the w vector of the natural coordinates""" + ... + + def vdot(self): + """This property returns the v vector of the natural coordinates""" + ... + + def vector(self): + """This property returns the vector of the natural coordinates""" + ... + + def to_components(self): + """This function returns the components of the natural coordinates""" + ... + + +class NaturalVelocities(Protocol): + """ + This class is made to handle Natural coordinates of several segments + """ + + def __new__(cls, input_array): + """Create a new instance of the class.""" + ... + + @classmethod + def from_qi(cls, tuple_of_Q): + """Constructor of the class from the components of the natural coordinates""" + ... + + def to_array(self): + """This function returns the array of the natural coordinates""" + ... + + def nb_qdoti(self): + """This function returns the number of qi""" + ... + + def udot(self, segment_index): + """This property returns the u vector of the natural coordinates""" + ... + + def rpdot(self, segment_index): + """This property returns the rp vector of the natural coordinates""" + ... + + def rddot(self, segment_index): + """This property returns the rd vector of the natural coordinates""" + ... + + def wdot(self, segment_index): + """This property returns the w vector of the natural coordinates""" + ... + + def vdot(self, segment_index): + """This property returns the v vector of the natural coordinates""" + ... + + def vector(self): + """This property returns the vector of the natural coordinates""" + ... diff --git a/bionc/math_interface/using_casadi/natural_accelerations.py b/bionc/math_interface/using_casadi/natural_accelerations.py new file mode 100644 index 00000000..8c3844a9 --- /dev/null +++ b/bionc/math_interface/using_casadi/natural_accelerations.py @@ -0,0 +1,146 @@ +import numpy as np +from casadi import MX, vertcat +from typing import Union + + +class SegmentNaturalAccelerations(MX): + """ + This class is made to handle Generalized Coordinates of a Segment + """ + + def __new__(cls, input_array: Union[MX, list, tuple]): + """ + Create a new instance of the class. + """ + + obj = np.asarray(input_array).view(cls) + + return obj + + @classmethod + def from_components( + cls, + uddot: Union[MX, list] = None, + rpddot: Union[MX, list] = None, + rdddot: Union[MX, list] = None, + wddot: Union[MX, list] = None, + ): + if uddot is None: + raise ValueError("uddot must be a numpy array (3x1) or a list of 3 elements") + if rpddot is None: + raise ValueError("rpddot must be a numpy array (3x1) or a list of 3 elements") + if rdddot is None: + raise ValueError("rdddot must be a numpy array (3x1) or a list of 3 elements") + if wddot is None: + raise ValueError("wddot must be a numpy array (3x1) or a list of 3 elements") + + if not isinstance(uddot, MX): + uddot = np.array(uddot) + if not isinstance(rpddot, MX): + rpddot = np.array(rpddot) + if not isinstance(rdddot, MX): + rdddot = np.array(rdddot) + if not isinstance(wddot, MX): + wddot = np.array(wddot) + + if uddot.shape[0] != 3: + raise ValueError("uddot must be a 3x1 array") + if rpddot.shape[0] != 3: + raise ValueError("rpddot must be a 3x1 array") + if rdddot.shape[0] != 3: + raise ValueError("rdddot must be a 3x1 array") + if wddot.shape[0] != 3: + raise ValueError("vddot must be a 3x1 array") + + input_array = vertcat(*(uddot, rpddot, rdddot, wddot)) + + return cls(input_array) + + def to_array(self): + return self + + @property + def uddot(self): + return self[0:3].to_array() + + @property + def rpddot(self): + return self[3:6].to_array() + + @property + def rdddot(self): + return self[6:9].to_array() + + @property + def wddot(self): + return self[9:12].to_array() + + @property + def vddot(self): + return self.rpddot - self.rdddot + + @property + def vector(self): + return self.to_array() + + @property + def to_components(self): + return self.uddot, self.rpddot, self.rdddot, self.wddot + + +class NaturalAccelerations(MX): + def __new__(cls, input_array: MX): + """ + Create a new instance of the class. + """ + + if input_array.shape[0] % 12 != 0: + raise ValueError("input_array must be a column vector of size 12 x n elements") + + obj = MX.__new__(cls) + + return obj + + @classmethod + def from_qddoti(cls, tuple_of_Q: tuple): + """ + Constructor of the class + """ + if not isinstance(tuple_of_Q, tuple): + raise ValueError("tuple_of_Q must be a tuple of SegmentGeneralizedCoordinates") + + for Q in tuple_of_Q: + if not isinstance(Q, SegmentNaturalAccelerations): + raise ValueError("tuple_of_Q must be a tuple of SegmentGeneralizedCoordinates") + + input_array = vertcat(*tuple_of_Q) + return cls(input_array) + + def to_array(self): + return self + + def nb_qddoti(self): + return self.shape[0] // 12 + + def uddot(self, segment_idx: int): + array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12)[0:3] + return self[array_idx].to_array() + + def rpddot(self, segment_idx: int): + array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12)[3:6] + return self[array_idx].to_array() + + def rdddot(self, segment_idx: int): + array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12)[6:9] + return self[array_idx].to_array() + + def wddot(self, segment_idx: int): + array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12)[9:12] + return self[array_idx].to_array() + + def vddot(self, segment_idx: int): + return self.rpddot(segment_idx) - self.rdddot(segment_idx) + + def vector(self, segment_idx: int): + array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12) + return SegmentNaturalAccelerations(self[array_idx].to_array()) diff --git a/bionc/math_interface/using_casadi/natural_velocities.py b/bionc/math_interface/using_casadi/natural_velocities.py new file mode 100644 index 00000000..a1df0366 --- /dev/null +++ b/bionc/math_interface/using_casadi/natural_velocities.py @@ -0,0 +1,145 @@ +import numpy as np +from casadi import MX, vertcat +from typing import Union + + +class SegmentNaturalVelocities(MX): + """ + This class is made to handle Generalized Coordinates of a Segment + """ + + def __new__(cls, input_array: MX): + """ + Create a new instance of the class. + """ + + obj = MX.__new__(cls) + + return obj + + @classmethod + def from_components( + cls, + udot: Union[MX, list] = None, + rpdot: Union[MX, list] = None, + rddot: Union[MX, list] = None, + wdot: Union[MX, list] = None, + ): + if udot is None: + raise ValueError("u must be a array (3x1) or a list of 3 elements") + if rpdot is None: + raise ValueError("rp must be a array (3x1) or a list of 3 elements") + if rddot is None: + raise ValueError("rd must be a array (3x1) or a list of 3 elements") + if wdot is None: + raise ValueError("w must be a array (3x1) or a list of 3 elements") + + if not isinstance(udot, MX): + udot = MX(udot) + if not isinstance(rpdot, MX): + rpdot = MX(rpdot) + if not isinstance(rddot, MX): + rddot = MX(rddot) + if not isinstance(wdot, MX): + wdot = MX(wdot) + + if udot.shape[0] != 3: + raise ValueError("u must be a 3x1 array") + if rpdot.shape[0] != 3: + raise ValueError("rp must be a 3x1 array") + if rddot.shape[0] != 3: + raise ValueError("rd must be a 3x1 array") + if wdot.shape[0] != 3: + raise ValueError("v must be a 3x1 array") + + input_array = vertcat(*(udot, rpdot, rddot, wdot)) + return cls(input_array) + + def to_array(self): + return self + + @property + def udot(self): + return self[0:3].to_array() + + @property + def rpdot(self): + return self[3:6].to_array() + + @property + def rddot(self): + return self[6:9].to_array() + + @property + def wdot(self): + return self[9:12].to_array() + + @property + def vdot(self): + return self.rpdot - self.rddot + + @property + def vector(self): + return self.to_array() + + @property + def to_components(self): + return self.udot, self.rpdot, self.rddot, self.wdot + + +class NaturalVelocities(MX): + def __new__(cls, input_array: MX): + """ + Create a new instance of the class. + """ + + if input_array.shape[0] % 12 != 0: + raise ValueError("input_array must be a column vector of size 12 x n elements") + + obj = MX.__new__(cls) + + return obj + + @classmethod + def from_qdoti(cls, tuple_of_Q: tuple): + """ + Create a new instance of the class. + """ + if not isinstance(tuple_of_Q, tuple): + raise ValueError("tuple_of_Q must be a tuple of SegmentGeneralizedCoordinates") + + for Q in tuple_of_Q: + if not isinstance(Q, SegmentNaturalVelocities): + raise ValueError("tuple_of_Q must be a tuple of SegmentGeneralizedCoordinates") + + input_array = vertcat(*tuple_of_Q) + return cls(input_array) + + def to_array(self): + return self + + def nb_qdoti(self): + return self.shape[0] // 12 + + def udot(self, segment_idx: int): + array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12)[0:3] + return self[array_idx].to_array() + + def rpdot(self, segment_idx: int): + array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12)[3:6] + return self[array_idx].to_array() + + def rddot(self, segment_idx: int): + array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12)[6:9] + return self[array_idx].to_array() + + def wdot(self, segment_idx: int): + array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12)[9:12] + return self[array_idx].to_array() + + def vdot(self, segment_idx: int): + return self.rpdot(segment_idx) - self.rddot(segment_idx) + + def vector(self, segment_idx: int): + array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12) + return SegmentNaturalVelocities(self[array_idx].to_array()) diff --git a/bionc/math_interface/using_numpy/natural_accelerations.py b/bionc/math_interface/using_numpy/natural_accelerations.py new file mode 100644 index 00000000..e9d25f36 --- /dev/null +++ b/bionc/math_interface/using_numpy/natural_accelerations.py @@ -0,0 +1,140 @@ +import numpy as np +from typing import Union + + +class SegmentNaturalAccelerations(np.ndarray): + """ + This class is made to handle Generalized Coordinates of a Segment + """ + + def __new__(cls, input_array: Union[np.ndarray, list, tuple]): + """ + Create a new instance of the class. + """ + + obj = np.asarray(input_array).view(cls) + + return obj + + @classmethod + def from_components( + cls, + uddot: Union[np.ndarray, list] = None, + rpddot: Union[np.ndarray, list] = None, + rdddot: Union[np.ndarray, list] = None, + wddot: Union[np.ndarray, list] = None, + ): + if uddot is None: + raise ValueError("uddot must be a numpy array (3x1) or a list of 3 elements") + if rpddot is None: + raise ValueError("rpddot must be a numpy array (3x1) or a list of 3 elements") + if rdddot is None: + raise ValueError("rdddot must be a numpy array (3x1) or a list of 3 elements") + if wddot is None: + raise ValueError("wddot must be a numpy array (3x1) or a list of 3 elements") + + if not isinstance(uddot, np.ndarray): + uddot = np.array(uddot) + if not isinstance(rpddot, np.ndarray): + rpddot = np.array(rpddot) + if not isinstance(rdddot, np.ndarray): + rdddot = np.array(rdddot) + if not isinstance(wddot, np.ndarray): + wddot = np.array(wddot) + + if uddot.shape[0] != 3: + raise ValueError("uddot must be a 3x1 numpy array") + if rpddot.shape[0] != 3: + raise ValueError("rpddot must be a 3x1 numpy array") + if rdddot.shape[0] != 3: + raise ValueError("rdddot must be a 3x1 numpy array") + if wddot.shape[0] != 3: + raise ValueError("vddot must be a 3x1 numpy array") + + input_array = np.concatenate((uddot, rpddot, rdddot, wddot), axis=0) + + return cls(input_array) + + def to_array(self): + return np.array(self) + + @property + def uddot(self): + return self[0:3].to_array() + + @property + def rpddot(self): + return self[3:6].to_array() + + @property + def rdddot(self): + return self[6:9].to_array() + + @property + def wddot(self): + return self[9:12].to_array() + + @property + def vddot(self): + return self.rpddot - self.rdddot + + @property + def vector(self): + return self.to_array() + + @property + def to_components(self): + return self.uddot, self.rpddot, self.rdddot, self.wddot + + +class NaturalAccelerations(np.ndarray): + def __new__(cls, input_array: np.ndarray): + """ + Create a new instance of the class. + """ + + return np.asarray(input_array).view(cls) + + @classmethod + def from_qddoti(cls, tuple_of_Q: tuple): + """ + Constructor of the class + """ + if not isinstance(tuple_of_Q, tuple): + raise ValueError("tuple_of_Q must be a tuple of SegmentGeneralizedCoordinates") + + for Q in tuple_of_Q: + if not isinstance(Q, SegmentNaturalAccelerations): + raise ValueError("tuple_of_Q must be a tuple of SegmentGeneralizedCoordinates") + + input_array = np.concatenate(tuple_of_Q, axis=0) + return cls(input_array) + + def to_array(self): + return np.array(self) + + def nb_qddoti(self): + return self.shape[0] // 12 + + def uddot(self, segment_idx: int): + array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12)[0:3] + return self[array_idx].to_array() + + def rpddot(self, segment_idx: int): + array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12)[3:6] + return self[array_idx].to_array() + + def rdddot(self, segment_idx: int): + array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12)[6:9] + return self[array_idx].to_array() + + def wddot(self, segment_idx: int): + array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12)[9:12] + return self[array_idx].to_array() + + def vddot(self, segment_idx: int): + return self.rpddot(segment_idx) - self.rdddot(segment_idx) + + def vector(self, segment_idx: int): + array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12) + return SegmentNaturalAccelerations(self[array_idx].to_array()) diff --git a/bionc/math_interface/using_numpy/natural_velocities.py b/bionc/math_interface/using_numpy/natural_velocities.py new file mode 100644 index 00000000..8a33c189 --- /dev/null +++ b/bionc/math_interface/using_numpy/natural_velocities.py @@ -0,0 +1,139 @@ +import numpy as np +from typing import Union + + +class SegmentNaturalVelocities(np.ndarray): + """ + This class is made to handle Generalized Coordinates of a Segment + """ + + def __new__(cls, input_array: Union[np.ndarray, list, tuple]): + """ + Create a new instance of the class. + """ + + obj = np.asarray(input_array).view(cls) + + return obj + + @classmethod + def from_components( + cls, + udot: Union[np.ndarray, list] = None, + rpdot: Union[np.ndarray, list] = None, + rddot: Union[np.ndarray, list] = None, + wdot: Union[np.ndarray, list] = None, + ): + if udot is None: + raise ValueError("u must be a numpy array (3x1) or a list of 3 elements") + if rpdot is None: + raise ValueError("rp must be a numpy array (3x1) or a list of 3 elements") + if rddot is None: + raise ValueError("rd must be a numpy array (3x1) or a list of 3 elements") + if wdot is None: + raise ValueError("w must be a numpy array (3x1) or a list of 3 elements") + + if not isinstance(udot, np.ndarray): + udot = np.array(udot) + if not isinstance(rpdot, np.ndarray): + rpdot = np.array(rpdot) + if not isinstance(rddot, np.ndarray): + rddot = np.array(rddot) + if not isinstance(wdot, np.ndarray): + wdot = np.array(wdot) + + if udot.shape[0] != 3: + raise ValueError("u must be a 3x1 numpy array") + if rpdot.shape[0] != 3: + raise ValueError("rp must be a 3x1 numpy array") + if rddot.shape[0] != 3: + raise ValueError("rd must be a 3x1 numpy array") + if wdot.shape[0] != 3: + raise ValueError("v must be a 3x1 numpy array") + + input_array = np.concatenate((udot, rpdot, rddot, wdot), axis=0) + return cls(input_array) + + def to_array(self): + return np.array(self) + + @property + def udot(self): + return self[0:3].to_array() + + @property + def rpdot(self): + return self[3:6].to_array() + + @property + def rddot(self): + return self[6:9].to_array() + + @property + def wdot(self): + return self[9:12].to_array() + + @property + def vdot(self): + return self.rpdot - self.rddot + + @property + def vector(self): + return self.to_array() + + @property + def to_components(self): + return self.udot, self.rpdot, self.rddot, self.wdot + + +class NaturalVelocities(np.ndarray): + def __new__(cls, input_array: np.ndarray): + """ + Create a new instance of the class. + """ + + return np.asarray(input_array).view(cls) + + @classmethod + def from_qdoti(cls, tuple_of_Q: tuple): + """ + Create a new instance of the class. + """ + if not isinstance(tuple_of_Q, tuple): + raise ValueError("tuple_of_Q must be a tuple of SegmentGeneralizedCoordinates") + + for Q in tuple_of_Q: + if not isinstance(Q, SegmentNaturalVelocities): + raise ValueError("tuple_of_Q must be a tuple of SegmentGeneralizedCoordinates") + + input_array = np.concatenate(tuple_of_Q, axis=0) + return cls(input_array) + + def to_array(self): + return np.array(self) + + def nb_qdoti(self): + return self.shape[0] // 12 + + def udot(self, segment_idx: int): + array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12)[0:3] + return self[array_idx].to_array() + + def rpdot(self, segment_idx: int): + array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12)[3:6] + return self[array_idx].to_array() + + def rddot(self, segment_idx: int): + array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12)[6:9] + return self[array_idx].to_array() + + def wdot(self, segment_idx: int): + array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12)[9:12] + return self[array_idx].to_array() + + def vdot(self, segment_idx: int): + return self.rpdot(segment_idx) - self.rddot(segment_idx) + + def vector(self, segment_idx: int): + array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12) + return SegmentNaturalVelocities(self[array_idx].to_array()) diff --git a/bionc/model_computations/joint.py b/bionc/model_computations/joint.py index fb6e1a64..3da2708d 100644 --- a/bionc/model_computations/joint.py +++ b/bionc/model_computations/joint.py @@ -3,8 +3,7 @@ import numpy as np from .natural_segment import NaturalSegment -from ..utils.natural_coordinates import NaturalCoordinates -from ..math_interface.internal import SegmentNaturalCoordinates +from bionc.math_interface.protocols.natural_coordinates import SegmentNaturalCoordinates class JointBase(ABC): diff --git a/bionc/model_computations/natural_marker.py b/bionc/model_computations/natural_marker.py index 5322cddc..fa0d8ff5 100644 --- a/bionc/model_computations/natural_marker.py +++ b/bionc/model_computations/natural_marker.py @@ -7,7 +7,7 @@ from ..utils.interpolation_matrix import interpolate_natural_vector, to_natural_vector # from ..utils.natural_coordinates import SegmentNaturalCoordinates -from ..math_interface.internal import SegmentNaturalCoordinates +from bionc.math_interface.protocols.natural_coordinates import SegmentNaturalCoordinates # todo: need a list of markers MarkerList diff --git a/bionc/model_computations/natural_segment.py b/bionc/model_computations/natural_segment.py index 12d65b5a..fc1b29af 100644 --- a/bionc/model_computations/natural_segment.py +++ b/bionc/model_computations/natural_segment.py @@ -1,16 +1,15 @@ -from copy import copy from typing import Union, Tuple import numpy as np -from numpy import cos, sin, matmul, eye, zeros, sum, ones +from numpy import cos, sin, matmul, eye, zeros, sum from numpy.linalg import inv # from ..utils.natural_coordinates import SegmentNaturalCoordinates -from ..math_interface.internal import SegmentNaturalCoordinates +from bionc.math_interface.protocols.natural_coordinates import SegmentNaturalCoordinates from ..utils.natural_velocities import SegmentNaturalVelocities from ..utils.natural_accelerations import SegmentNaturalAccelerations from ..utils.homogenous_transform import HomogeneousTransform -from ..model_computations.natural_marker import SegmentMarker, Marker +from ..model_computations.natural_marker import SegmentMarker class NaturalSegment: diff --git a/bionc/model_creation/marker_template.py b/bionc/model_creation/marker_template.py index 3ee6baa2..f8766937 100644 --- a/bionc/model_creation/marker_template.py +++ b/bionc/model_creation/marker_template.py @@ -10,7 +10,7 @@ from ..model_computations.natural_segment import NaturalSegment # from ..utils.natural_coordinates import SegmentNaturalCoordinates -from ..math_interface.internal import SegmentNaturalCoordinates +from bionc.math_interface.protocols.natural_coordinates import SegmentNaturalCoordinates class MarkerTemplate: diff --git a/bionc/model_creation/natural_segment_template.py b/bionc/model_creation/natural_segment_template.py index 123fbeac..4b16e6a1 100644 --- a/bionc/model_creation/natural_segment_template.py +++ b/bionc/model_creation/natural_segment_template.py @@ -1,11 +1,10 @@ from typing import Callable -from ..model_computations.natural_axis import Axis from .natural_axis_template import AxisTemplate from ..model_computations.biomechanical_model import BiomechanicalModel # from ..utils.natural_coordinates import SegmentNaturalCoordinates -from ..math_interface.internal import SegmentNaturalCoordinates +from bionc.math_interface.protocols.natural_coordinates import SegmentNaturalCoordinates from .marker_template import MarkerTemplate from .protocols import Data from ..model_computations.natural_segment import NaturalSegment diff --git a/bionc/utils/natural_coordinates.py b/bionc/utils/natural_coordinates.py index 6ab35f5e..dc0b8102 100644 --- a/bionc/utils/natural_coordinates.py +++ b/bionc/utils/natural_coordinates.py @@ -1,8 +1,5 @@ import numpy as np -from typing import Union -from bionc.utils.vnop_array import vnop_array -from bionc.utils.interpolation_matrix import interpolate_natural_vector -from ..math_interface.internal import SegmentNaturalCoordinates +from bionc.math_interface.protocols.natural_coordinates import SegmentNaturalCoordinates # class SegmentNaturalCoordinates(np.ndarray): From d1576dda6213dfe545d60e88cf3d1b47ff5ec962 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Wed, 9 Nov 2022 18:28:34 -0500 Subject: [PATCH 10/30] homogenous transform --- .../protocols/homogenous_transform.py | 50 ++++++++ .../using_casadi/homogenous_transform.py | 117 ++++++++++++++++++ .../using_numpy/homogenous_transform.py | 116 +++++++++++++++++ 3 files changed, 283 insertions(+) create mode 100644 bionc/math_interface/protocols/homogenous_transform.py create mode 100644 bionc/math_interface/using_casadi/homogenous_transform.py create mode 100644 bionc/math_interface/using_numpy/homogenous_transform.py diff --git a/bionc/math_interface/protocols/homogenous_transform.py b/bionc/math_interface/protocols/homogenous_transform.py new file mode 100644 index 00000000..8126a16b --- /dev/null +++ b/bionc/math_interface/protocols/homogenous_transform.py @@ -0,0 +1,50 @@ +from typing import Protocol +import numpy as np + + +class HomogeneousTransform(Protocol): + """ + Homogenous transform class + """ + + def __new__(cls, input_array): + """ + Create a new instance of the class. + """ + ... + + @classmethod + def from_components(cls, x, y, z, t): + """ + Constructor of the class from the components of the homogenous transform + """ + ... + + @classmethod + def from_rt(cls, rotation, translation): + """ + Constructor of the class from a rotation matrix and a translation vector + """ + ... + + @classmethod + def eye(cls): + """ + Returns the identity homogenous transform + """ + ... + + def rot(self): + ... + + def translation(self): + ... + + def inv(self): + """ + Returns the inverse of the homogenous transform + """ + ... + + def to_array(self): + ... diff --git a/bionc/math_interface/using_casadi/homogenous_transform.py b/bionc/math_interface/using_casadi/homogenous_transform.py new file mode 100644 index 00000000..34485b35 --- /dev/null +++ b/bionc/math_interface/using_casadi/homogenous_transform.py @@ -0,0 +1,117 @@ +from typing import Union +from casadi import MX, vertcat, horzcat, transpose +import numpy as np + + +class HomogeneousTransform(MX): + """ + Homogenous transform class + """ + def __new__(cls, input_array: MX): + """ + Create a new instance of the class. + """ + + if not isinstance(input_array, MX): + raise TypeError("input_array must be a MX") + + if input_array.shape != (4, 4): + raise ValueError("input_array must be a 4x4 array") + + obj = MX.__new__(cls) + + return obj + + @classmethod + def from_components(cls, x: MX, y: MX, z: MX, t: MX): + """ + Constructor of the class from the components of the homogenous transform + + Parameters + ---------- + x: MX + The x axis of the homogenous transform, a 3x1 array + y: MX + The y axis of the homogenous transform, a 3x1 array + z: MX + The z axis of the homogenous transform, a 3x1 array + t: MX + translation vector, a 3x1 array + """ + if not isinstance(x, MX): + raise TypeError("x must be a array") + if not isinstance(y, MX): + raise TypeError("y must be a array") + if not isinstance(z, MX): + raise TypeError("z must be a array") + if not isinstance(t, MX): + raise TypeError("t must be a array") + + if x.shape != (3, 1): + raise ValueError("x must be a 3x1 array") + if y.shape != (3, 1): + raise ValueError("y must be a 3x1 array") + if z.shape != (3, 1): + raise ValueError("z must be a 3x1 array") + if t.shape != (3, 1): + raise ValueError("t must be a 3x1 array") + + input_array = horzcat(*(x, y, z, t)) + input_array = vertcat(*(input_array, np.array([[0, 0, 0, 1]]))) + return cls(input_array) + + @classmethod + def from_rt(cls, rotation: MX, translation: MX): + """ + Constructor of the class from a rotation matrix and a translation vector + + Parameters + ---------- + rotation: MX + A 3x3 rotation matrix + translation: MX + A 3x1 translation vector + """ + if not isinstance(rotation, MX): + raise TypeError("r must be a array") + if not isinstance(translation, MX): + raise TypeError("t must be a array") + + if rotation.shape != (3, 3): + raise ValueError("r must be a 3x3 array") + if translation.shape != (3, 1): + raise ValueError("t must be a 3x1 array") + + input_array = horzcat(*(rotation, translation)) + input_array = vertcat(*(input_array, np.array([[0, 0, 0, 1]]))) + + return cls(input_array) + + @classmethod + def eye(cls): + """ + Returns the identity homogenous transform + """ + return cls(np.eye(4)) + + @property + def rot(self): + return self[:3, :3].to_array() + + @property + def translation(self): + return self[3, 0:3].to_array() + + def inv(self): + """ + Returns the inverse of the homogenous transform + """ + inv_mat = MX.zeros((4, 4)) + inv_mat[:3, :3] = transpose(self[:3, :3]) + inv_mat[:3, 3] = -inv_mat[:3, :3] @ self[:3, 3] + inv_mat[3, :] = MX([0, 0, 0, 1]) + + return HomogeneousTransform(inv_mat) + + def to_array(self): + return self diff --git a/bionc/math_interface/using_numpy/homogenous_transform.py b/bionc/math_interface/using_numpy/homogenous_transform.py new file mode 100644 index 00000000..def1f0f4 --- /dev/null +++ b/bionc/math_interface/using_numpy/homogenous_transform.py @@ -0,0 +1,116 @@ +from typing import Union +import numpy as np + + +class HomogeneousTransform(np.ndarray): + """ + Homogenous transform class + """ + + def __new__(cls, input_array: Union[np.ndarray, list, tuple]): + """ + Create a new instance of the class. + """ + if not isinstance(input_array, np.ndarray): + raise TypeError("input_array must be a numpy array") + + if input_array.shape != (4, 4): + raise ValueError("input_array must be a 4x4 numpy array") + + obj = np.asarray(input_array).view(cls) + + return obj + + @classmethod + def from_components(cls, x: np.ndarray, y: np.ndarray, z: np.ndarray, t: np.ndarray): + """ + Constructor of the class from the components of the homogenous transform + + Parameters + ---------- + x: np.ndarray + The x axis of the homogenous transform, a 3x1 numpy array + y: np.ndarray + The y axis of the homogenous transform, a 3x1 numpy array + z: np.ndarray + The z axis of the homogenous transform, a 3x1 numpy array + t: np.ndarray + translation vector, a 3x1 numpy array + """ + if not isinstance(x, np.ndarray): + raise TypeError("x must be a numpy array") + if not isinstance(y, np.ndarray): + raise TypeError("y must be a numpy array") + if not isinstance(z, np.ndarray): + raise TypeError("z must be a numpy array") + if not isinstance(t, np.ndarray): + raise TypeError("t must be a numpy array") + + if x.shape != (3, 1): + raise ValueError("x must be a 3x1 numpy array") + if y.shape != (3, 1): + raise ValueError("y must be a 3x1 numpy array") + if z.shape != (3, 1): + raise ValueError("z must be a 3x1 numpy array") + if t.shape != (3, 1): + raise ValueError("t must be a 3x1 numpy array") + + input_array = np.concatenate((x, y, z, t), axis=1) + input_array = np.concatenate((input_array, np.array([[0, 0, 0, 1]])), axis=0) + return cls(input_array) + + @classmethod + def from_rt(cls, rotation: np.ndarray, translation: np.ndarray): + """ + Constructor of the class from a rotation matrix and a translation vector + + Parameters + ---------- + rotation: np.ndarray + A 3x3 rotation matrix + translation: np.ndarray + A 3x1 translation vector + """ + if not isinstance(rotation, np.ndarray): + raise TypeError("r must be a numpy array") + if not isinstance(translation, np.ndarray): + raise TypeError("t must be a numpy array") + + if rotation.shape != (3, 3): + raise ValueError("r must be a 3x3 numpy array") + if translation.shape != (3, 1): + raise ValueError("t must be a 3x1 numpy array") + + input_array = np.concatenate((rotation, translation), axis=1) + input_array = np.concatenate((input_array, np.array([[0, 0, 0, 1]])), axis=0) + + return cls(input_array) + + @classmethod + def eye(cls): + """ + Returns the identity homogenous transform + """ + return cls(np.eye(4)) + + @property + def rot(self): + return self[:3, :3].to_array() + + @property + def translation(self): + return self[3, 0:3].to_array() + + def inv(self): + """ + Returns the inverse of the homogenous transform + """ + inv_mat = np.zeros((4, 4)) + inv_mat[:3, :3] = self[:3, :3].T + inv_mat[:3, 3] = -inv_mat[:3, :3] @ self[:3, 3] + inv_mat[3, :] = np.array([0, 0, 0, 1]) + + return HomogeneousTransform(inv_mat) + + def to_array(self): + return np.array(self) From 872e7953d3dfda4bfaeb6894b705ac8267c5e6d1 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Wed, 9 Nov 2022 18:45:20 -0500 Subject: [PATCH 11/30] All tests works fine with numpy --- bionc/__init__.py | 12 +- bionc/math_interface/using_casadi/__init__.py | 3 + .../using_casadi/natural_accelerations.py | 10 +- .../using_casadi/natural_velocities.py | 10 +- bionc/math_interface/using_numpy/__init__.py | 3 + .../model_computations/biomechanical_model.py | 5 +- bionc/model_computations/natural_segment.py | 8 +- bionc/utils/__init__.py | 7 - bionc/utils/homogenous_transform.py | 116 ----------- bionc/utils/interpolation_matrix.py | 2 - bionc/utils/natural_accelerations.py | 140 ------------- bionc/utils/natural_coordinates.py | 188 ------------------ bionc/utils/natural_velocities.py | 139 ------------- tests/test_biomech_model.py | 10 +- tests/test_homegenous_transform.py | 34 ++-- tests/test_natural_coordinates.py | 12 +- tests/test_natural_velocities.py | 30 +-- 17 files changed, 69 insertions(+), 660 deletions(-) delete mode 100644 bionc/utils/homogenous_transform.py delete mode 100644 bionc/utils/natural_accelerations.py delete mode 100644 bionc/utils/natural_coordinates.py delete mode 100644 bionc/utils/natural_velocities.py diff --git a/bionc/__init__.py b/bionc/__init__.py index b7ee0699..f5f0a036 100644 --- a/bionc/__init__.py +++ b/bionc/__init__.py @@ -1,11 +1,5 @@ -# from .utils.natural_coordinates import ( -# # SegmentNaturalCoordinates, -# # NaturalCoordinates, -# ) -from .utils.natural_velocities import SegmentNaturalVelocities, NaturalVelocities -from .utils.natural_accelerations import SegmentNaturalAccelerations, NaturalAccelerations -from .utils.homogenous_transform import HomogeneousTransform from .utils.interpolation_matrix import interpolate_natural_vector, to_natural_vector +from .utils.vnop_array import vnop_array from .model_creation import ( AxisTemplate, @@ -41,8 +35,10 @@ from .math_interface import using_casadi as bionc_casadi from .math_interface import using_numpy as bionc_numpy -# I don't know if it's useful to import the following yet from bionc.math_interface.protocols.natural_coordinates import SegmentNaturalCoordinates, NaturalCoordinates +from bionc.math_interface.protocols.natural_velocities import SegmentNaturalVelocities, NaturalVelocities +from bionc.math_interface.protocols.natural_accelerations import SegmentNaturalAccelerations, NaturalAccelerations +from bionc.math_interface.protocols.homogenous_transform import HomogeneousTransform from casadi.casadi import MX as MX_type from numpy import ndarray diff --git a/bionc/math_interface/using_casadi/__init__.py b/bionc/math_interface/using_casadi/__init__.py index 1a705bd6..e40eaabe 100644 --- a/bionc/math_interface/using_casadi/__init__.py +++ b/bionc/math_interface/using_casadi/__init__.py @@ -1 +1,4 @@ from .natural_coordinates import SegmentNaturalCoordinates, NaturalCoordinates +from .natural_velocities import SegmentNaturalVelocities, NaturalVelocities +from .natural_accelerations import SegmentNaturalAccelerations, NaturalAccelerations +from .homogenous_transform import HomogeneousTransform diff --git a/bionc/math_interface/using_casadi/natural_accelerations.py b/bionc/math_interface/using_casadi/natural_accelerations.py index 8c3844a9..f1bb0d83 100644 --- a/bionc/math_interface/using_casadi/natural_accelerations.py +++ b/bionc/math_interface/using_casadi/natural_accelerations.py @@ -61,19 +61,19 @@ def to_array(self): @property def uddot(self): - return self[0:3].to_array() + return self[0:3] @property def rpddot(self): - return self[3:6].to_array() + return self[3:6] @property def rdddot(self): - return self[6:9].to_array() + return self[6:9] @property def wddot(self): - return self[9:12].to_array() + return self[9:12] @property def vddot(self): @@ -143,4 +143,4 @@ def vddot(self, segment_idx: int): def vector(self, segment_idx: int): array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12) - return SegmentNaturalAccelerations(self[array_idx].to_array()) + return SegmentNaturalAccelerations(self[array_idx]) diff --git a/bionc/math_interface/using_casadi/natural_velocities.py b/bionc/math_interface/using_casadi/natural_velocities.py index a1df0366..a655dd86 100644 --- a/bionc/math_interface/using_casadi/natural_velocities.py +++ b/bionc/math_interface/using_casadi/natural_velocities.py @@ -60,19 +60,19 @@ def to_array(self): @property def udot(self): - return self[0:3].to_array() + return self[0:3] @property def rpdot(self): - return self[3:6].to_array() + return self[3:6] @property def rddot(self): - return self[6:9].to_array() + return self[6:9] @property def wdot(self): - return self[9:12].to_array() + return self[9:12] @property def vdot(self): @@ -142,4 +142,4 @@ def vdot(self, segment_idx: int): def vector(self, segment_idx: int): array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12) - return SegmentNaturalVelocities(self[array_idx].to_array()) + return SegmentNaturalVelocities(self[array_idx]) diff --git a/bionc/math_interface/using_numpy/__init__.py b/bionc/math_interface/using_numpy/__init__.py index 005f29e8..ef06ff9b 100644 --- a/bionc/math_interface/using_numpy/__init__.py +++ b/bionc/math_interface/using_numpy/__init__.py @@ -1 +1,4 @@ from .natural_coordinates import SegmentNaturalCoordinates, NaturalCoordinates +from .natural_velocities import SegmentNaturalVelocities, NaturalVelocities +from .natural_accelerations import SegmentNaturalAccelerations, NaturalAccelerations +from .homogenous_transform import HomogeneousTransform diff --git a/bionc/model_computations/biomechanical_model.py b/bionc/model_computations/biomechanical_model.py index 93abd4fd..a1af1844 100644 --- a/bionc/model_computations/biomechanical_model.py +++ b/bionc/model_computations/biomechanical_model.py @@ -1,8 +1,7 @@ import numpy as np -from ..utils.natural_velocities import NaturalVelocities -from ..utils.natural_coordinates import NaturalCoordinates -from ..utils.natural_accelerations import NaturalAccelerations +from ..math_interface.protocols.natural_coordinates import SegmentNaturalCoordinates, NaturalCoordinates +from ..math_interface.using_casadi.natural_velocities import SegmentNaturalVelocities, NaturalVelocities class BiomechanicalModel: diff --git a/bionc/model_computations/natural_segment.py b/bionc/model_computations/natural_segment.py index fc1b29af..1995d5b2 100644 --- a/bionc/model_computations/natural_segment.py +++ b/bionc/model_computations/natural_segment.py @@ -5,10 +5,10 @@ from numpy.linalg import inv # from ..utils.natural_coordinates import SegmentNaturalCoordinates -from bionc.math_interface.protocols.natural_coordinates import SegmentNaturalCoordinates -from ..utils.natural_velocities import SegmentNaturalVelocities -from ..utils.natural_accelerations import SegmentNaturalAccelerations -from ..utils.homogenous_transform import HomogeneousTransform +from ..math_interface.protocols.natural_coordinates import SegmentNaturalCoordinates, NaturalCoordinates +from ..math_interface.using_casadi.natural_velocities import SegmentNaturalVelocities, NaturalVelocities +from ..math_interface.using_casadi.natural_accelerations import SegmentNaturalAccelerations, NaturalAccelerations +from ..math_interface.using_casadi.homogenous_transform import HomogeneousTransform from ..model_computations.natural_marker import SegmentMarker diff --git a/bionc/utils/__init__.py b/bionc/utils/__init__.py index c04c57c5..e69de29b 100644 --- a/bionc/utils/__init__.py +++ b/bionc/utils/__init__.py @@ -1,7 +0,0 @@ -from .homogenous_transform import HomogeneousTransform -from .natural_coordinates import ( - # SegmentNaturalCoordinates, \ - NaturalCoordinates, -) -from .natural_velocities import SegmentNaturalVelocities, NaturalVelocities -from .natural_accelerations import SegmentNaturalAccelerations, NaturalAccelerations diff --git a/bionc/utils/homogenous_transform.py b/bionc/utils/homogenous_transform.py deleted file mode 100644 index def1f0f4..00000000 --- a/bionc/utils/homogenous_transform.py +++ /dev/null @@ -1,116 +0,0 @@ -from typing import Union -import numpy as np - - -class HomogeneousTransform(np.ndarray): - """ - Homogenous transform class - """ - - def __new__(cls, input_array: Union[np.ndarray, list, tuple]): - """ - Create a new instance of the class. - """ - if not isinstance(input_array, np.ndarray): - raise TypeError("input_array must be a numpy array") - - if input_array.shape != (4, 4): - raise ValueError("input_array must be a 4x4 numpy array") - - obj = np.asarray(input_array).view(cls) - - return obj - - @classmethod - def from_components(cls, x: np.ndarray, y: np.ndarray, z: np.ndarray, t: np.ndarray): - """ - Constructor of the class from the components of the homogenous transform - - Parameters - ---------- - x: np.ndarray - The x axis of the homogenous transform, a 3x1 numpy array - y: np.ndarray - The y axis of the homogenous transform, a 3x1 numpy array - z: np.ndarray - The z axis of the homogenous transform, a 3x1 numpy array - t: np.ndarray - translation vector, a 3x1 numpy array - """ - if not isinstance(x, np.ndarray): - raise TypeError("x must be a numpy array") - if not isinstance(y, np.ndarray): - raise TypeError("y must be a numpy array") - if not isinstance(z, np.ndarray): - raise TypeError("z must be a numpy array") - if not isinstance(t, np.ndarray): - raise TypeError("t must be a numpy array") - - if x.shape != (3, 1): - raise ValueError("x must be a 3x1 numpy array") - if y.shape != (3, 1): - raise ValueError("y must be a 3x1 numpy array") - if z.shape != (3, 1): - raise ValueError("z must be a 3x1 numpy array") - if t.shape != (3, 1): - raise ValueError("t must be a 3x1 numpy array") - - input_array = np.concatenate((x, y, z, t), axis=1) - input_array = np.concatenate((input_array, np.array([[0, 0, 0, 1]])), axis=0) - return cls(input_array) - - @classmethod - def from_rt(cls, rotation: np.ndarray, translation: np.ndarray): - """ - Constructor of the class from a rotation matrix and a translation vector - - Parameters - ---------- - rotation: np.ndarray - A 3x3 rotation matrix - translation: np.ndarray - A 3x1 translation vector - """ - if not isinstance(rotation, np.ndarray): - raise TypeError("r must be a numpy array") - if not isinstance(translation, np.ndarray): - raise TypeError("t must be a numpy array") - - if rotation.shape != (3, 3): - raise ValueError("r must be a 3x3 numpy array") - if translation.shape != (3, 1): - raise ValueError("t must be a 3x1 numpy array") - - input_array = np.concatenate((rotation, translation), axis=1) - input_array = np.concatenate((input_array, np.array([[0, 0, 0, 1]])), axis=0) - - return cls(input_array) - - @classmethod - def eye(cls): - """ - Returns the identity homogenous transform - """ - return cls(np.eye(4)) - - @property - def rot(self): - return self[:3, :3].to_array() - - @property - def translation(self): - return self[3, 0:3].to_array() - - def inv(self): - """ - Returns the inverse of the homogenous transform - """ - inv_mat = np.zeros((4, 4)) - inv_mat[:3, :3] = self[:3, :3].T - inv_mat[:3, 3] = -inv_mat[:3, :3] @ self[:3, 3] - inv_mat[3, :] = np.array([0, 0, 0, 1]) - - return HomogeneousTransform(inv_mat) - - def to_array(self): - return np.array(self) diff --git a/bionc/utils/interpolation_matrix.py b/bionc/utils/interpolation_matrix.py index 3f79b52d..9b887388 100644 --- a/bionc/utils/interpolation_matrix.py +++ b/bionc/utils/interpolation_matrix.py @@ -60,5 +60,3 @@ def to_natural_vector(interpolation_matrix: np.ndarray) -> np.ndarray: return vector - -# test these two functions with pytest diff --git a/bionc/utils/natural_accelerations.py b/bionc/utils/natural_accelerations.py deleted file mode 100644 index e9d25f36..00000000 --- a/bionc/utils/natural_accelerations.py +++ /dev/null @@ -1,140 +0,0 @@ -import numpy as np -from typing import Union - - -class SegmentNaturalAccelerations(np.ndarray): - """ - This class is made to handle Generalized Coordinates of a Segment - """ - - def __new__(cls, input_array: Union[np.ndarray, list, tuple]): - """ - Create a new instance of the class. - """ - - obj = np.asarray(input_array).view(cls) - - return obj - - @classmethod - def from_components( - cls, - uddot: Union[np.ndarray, list] = None, - rpddot: Union[np.ndarray, list] = None, - rdddot: Union[np.ndarray, list] = None, - wddot: Union[np.ndarray, list] = None, - ): - if uddot is None: - raise ValueError("uddot must be a numpy array (3x1) or a list of 3 elements") - if rpddot is None: - raise ValueError("rpddot must be a numpy array (3x1) or a list of 3 elements") - if rdddot is None: - raise ValueError("rdddot must be a numpy array (3x1) or a list of 3 elements") - if wddot is None: - raise ValueError("wddot must be a numpy array (3x1) or a list of 3 elements") - - if not isinstance(uddot, np.ndarray): - uddot = np.array(uddot) - if not isinstance(rpddot, np.ndarray): - rpddot = np.array(rpddot) - if not isinstance(rdddot, np.ndarray): - rdddot = np.array(rdddot) - if not isinstance(wddot, np.ndarray): - wddot = np.array(wddot) - - if uddot.shape[0] != 3: - raise ValueError("uddot must be a 3x1 numpy array") - if rpddot.shape[0] != 3: - raise ValueError("rpddot must be a 3x1 numpy array") - if rdddot.shape[0] != 3: - raise ValueError("rdddot must be a 3x1 numpy array") - if wddot.shape[0] != 3: - raise ValueError("vddot must be a 3x1 numpy array") - - input_array = np.concatenate((uddot, rpddot, rdddot, wddot), axis=0) - - return cls(input_array) - - def to_array(self): - return np.array(self) - - @property - def uddot(self): - return self[0:3].to_array() - - @property - def rpddot(self): - return self[3:6].to_array() - - @property - def rdddot(self): - return self[6:9].to_array() - - @property - def wddot(self): - return self[9:12].to_array() - - @property - def vddot(self): - return self.rpddot - self.rdddot - - @property - def vector(self): - return self.to_array() - - @property - def to_components(self): - return self.uddot, self.rpddot, self.rdddot, self.wddot - - -class NaturalAccelerations(np.ndarray): - def __new__(cls, input_array: np.ndarray): - """ - Create a new instance of the class. - """ - - return np.asarray(input_array).view(cls) - - @classmethod - def from_qddoti(cls, tuple_of_Q: tuple): - """ - Constructor of the class - """ - if not isinstance(tuple_of_Q, tuple): - raise ValueError("tuple_of_Q must be a tuple of SegmentGeneralizedCoordinates") - - for Q in tuple_of_Q: - if not isinstance(Q, SegmentNaturalAccelerations): - raise ValueError("tuple_of_Q must be a tuple of SegmentGeneralizedCoordinates") - - input_array = np.concatenate(tuple_of_Q, axis=0) - return cls(input_array) - - def to_array(self): - return np.array(self) - - def nb_qddoti(self): - return self.shape[0] // 12 - - def uddot(self, segment_idx: int): - array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12)[0:3] - return self[array_idx].to_array() - - def rpddot(self, segment_idx: int): - array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12)[3:6] - return self[array_idx].to_array() - - def rdddot(self, segment_idx: int): - array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12)[6:9] - return self[array_idx].to_array() - - def wddot(self, segment_idx: int): - array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12)[9:12] - return self[array_idx].to_array() - - def vddot(self, segment_idx: int): - return self.rpddot(segment_idx) - self.rdddot(segment_idx) - - def vector(self, segment_idx: int): - array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12) - return SegmentNaturalAccelerations(self[array_idx].to_array()) diff --git a/bionc/utils/natural_coordinates.py b/bionc/utils/natural_coordinates.py deleted file mode 100644 index dc0b8102..00000000 --- a/bionc/utils/natural_coordinates.py +++ /dev/null @@ -1,188 +0,0 @@ -import numpy as np -from bionc.math_interface.protocols.natural_coordinates import SegmentNaturalCoordinates - - -# class SegmentNaturalCoordinates(np.ndarray): -# """ -# This class is made to handle Generalized Coordinates of a Segment -# """ -# -# def __new__(cls, input_array: Union[np.ndarray, list, tuple]): -# """ -# Create a new instance of the class. -# """ -# -# obj = np.asarray(input_array).view(cls) -# -# if obj.shape.__len__() == 1: -# obj = obj[:, np.newaxis] -# -# return obj -# -# @classmethod -# def from_components( -# cls, -# u: Union[np.ndarray, list] = None, -# rp: Union[np.ndarray, list] = None, -# rd: Union[np.ndarray, list] = None, -# w: Union[np.ndarray, list] = None, -# ): -# """ -# Constructor of the class from the components of the natural coordinates -# """ -# -# if u is None: -# raise ValueError("u must be a numpy array (3x1) or a list of 3 elements") -# if rp is None: -# raise ValueError("rp must be a numpy array (3x1) or a list of 3 elements") -# if rd is None: -# raise ValueError("rd must be a numpy array (3x1) or a list of 3 elements") -# if w is None: -# raise ValueError("w must be a numpy array (3x1) or a list of 3 elements") -# -# if not isinstance(u, np.ndarray): -# u = np.array(u) -# if not isinstance(rp, np.ndarray): -# rp = np.array(rp) -# if not isinstance(rd, np.ndarray): -# rd = np.array(rd) -# if not isinstance(w, np.ndarray): -# w = np.array(w) -# -# if u.shape[0] != 3: -# raise ValueError("u must be a 3x1 numpy array") -# if rp.shape[0] != 3: -# raise ValueError("rp must be a 3x1 numpy array") -# if rd.shape[0] != 3: -# raise ValueError("rd must be a 3x1 numpy array") -# if w.shape[0] != 3: -# raise ValueError("v must be a 3x1 numpy array") -# -# input_array = np.concatenate((u, rp, rd, w), axis=0) -# -# if input_array.shape.__len__() == 1: -# input_array = input_array[:, np.newaxis] -# -# return cls(input_array) -# -# def to_array(self): -# return np.array(self).squeeze() -# -# @property -# def u(self): -# return self[0:3, :].to_array() -# -# @property -# def rp(self): -# return self[3:6, :].to_array() -# -# @property -# def rd(self): -# return self[6:9, :].to_array() -# -# @property -# def w(self): -# return self[9:12, :].to_array() -# -# @property -# def v(self): -# return self.rp - self.rd -# -# @property -# def vector(self): -# return self.to_array() -# -# def to_components(self): -# return self.u, self.rp, self.rd, self.w -# -# def to_uvw(self): -# return self.u, self.v, self.w -# -# def to_non_orthogonal_basis(self, vector: np.ndarray) -> np.ndarray: -# """ -# This function converts a vector expressed in the global coordinate system -# to a vector expressed in a non-orthogonal coordinate system associated to the segment coordinates. -# -# Parameters -# ---------- -# vector: np.ndarray -# The vector expressed in the global coordinate system -# -# Returns -# ------- -# np.ndarray -# The vector expressed in the non-orthogonal coordinate system -# -# """ -# return vnop_array(vector - self.rp, self.u, self.v, self.w) -# -# def to_interpolation_matrix(self, vector: np.ndarray) -> np.ndarray: -# """ -# This function converts a vector expressed in the global coordinate system -# to a vector expressed in a non-orthogonal coordinate system associated to the segment coordinates. -# -# Parameters -# ---------- -# vector: np.ndarray -# The vector expressed in the global coordinate system -# -# Returns -# ------- -# np.ndarray -# The vector expressed in the non-orthogonal coordinate system -# -# """ -# return interpolate_natural_vector(vnop_array(vector - self.rp, self.u, self.v, self.w)) - - -class NaturalCoordinates(np.ndarray): - def __new__(cls, input_array: np.ndarray): - """ - Create a new instance of the class. - """ - - return np.asarray(input_array).view(cls) - - @classmethod - def from_qi(cls, tuple_of_Q: tuple): - """ - Constructor of the class. - """ - if not isinstance(tuple_of_Q, tuple): - raise ValueError("tuple_of_Q must be a tuple of SegmentGeneralizedCoordinates") - - for Q in tuple_of_Q: - if not isinstance(Q, SegmentNaturalCoordinates): - raise ValueError("tuple_of_Q must be a tuple of SegmentGeneralizedCoordinates") - - input_array = np.concatenate(tuple_of_Q, axis=0) - return cls(input_array) - - def to_array(self): - return np.array(self).squeeze() - - def nb_qi(self): - return self.shape[0] // 12 - - def u(self, segment_idx: int): - array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12)[0:3] - return self[array_idx, :].to_array() - - def rp(self, segment_idx: int): - array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12)[3:6] - return self[array_idx, :].to_array() - - def rd(self, segment_idx: int): - array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12)[6:9] - return self[array_idx, :].to_array() - - def w(self, segment_idx: int): - array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12)[9:12] - return self[array_idx, :].to_array() - - def v(self, segment_idx: int): - return self.rp(segment_idx) - self.rd(segment_idx) - - def vector(self, segment_idx: int): - array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12) - return SegmentNaturalCoordinates(self[array_idx, :].to_array()) diff --git a/bionc/utils/natural_velocities.py b/bionc/utils/natural_velocities.py deleted file mode 100644 index 6f4e1b9f..00000000 --- a/bionc/utils/natural_velocities.py +++ /dev/null @@ -1,139 +0,0 @@ -import numpy as np -from typing import Union - - -class SegmentNaturalVelocities(np.ndarray): - """ - This class is made to handle Generalized Coordinates of a Segment - """ - - def __new__(cls, input_array: Union[np.ndarray, list, tuple]): - """ - Create a new instance of the class. - """ - - obj = np.asarray(input_array).view(cls) - - return obj - - @classmethod - def from_components( - cls, - udot: Union[np.ndarray, list] = None, - rpdot: Union[np.ndarray, list] = None, - rddot: Union[np.ndarray, list] = None, - wdot: Union[np.ndarray, list] = None, - ): - if udot is None: - raise ValueError("u must be a numpy array (3x1) or a list of 3 elements") - if rpdot is None: - raise ValueError("rp must be a numpy array (3x1) or a list of 3 elements") - if rddot is None: - raise ValueError("rd must be a numpy array (3x1) or a list of 3 elements") - if wdot is None: - raise ValueError("w must be a numpy array (3x1) or a list of 3 elements") - - if not isinstance(udot, np.ndarray): - udot = np.array(udot) - if not isinstance(rpdot, np.ndarray): - rpdot = np.array(rpdot) - if not isinstance(rddot, np.ndarray): - rddot = np.array(rddot) - if not isinstance(wdot, np.ndarray): - wdot = np.array(wdot) - - if udot.shape[0] != 3: - raise ValueError("u must be a 3x1 numpy array") - if rpdot.shape[0] != 3: - raise ValueError("rp must be a 3x1 numpy array") - if rddot.shape[0] != 3: - raise ValueError("rd must be a 3x1 numpy array") - if wdot.shape[0] != 3: - raise ValueError("v must be a 3x1 numpy array") - - input_array = np.concatenate((udot, rpdot, rddot, wdot), axis=0) - return cls(input_array) - - def to_array(self): - return np.array(self) - - @property - def udot(self): - return self[0:3].to_array() - - @property - def rpdot(self): - return self[3:6].to_array() - - @property - def rddot(self): - return self[6:9].to_array() - - @property - def wdot(self): - return self[9:12].to_array() - - @property - def vdot(self): - return self.rpdot - self.rddot - - @property - def vector(self): - return self.to_array() - - @property - def to_components(self): - return self.udot, self.rpdot, self.rddot, self.wdot - - -class NaturalVelocities(np.ndarray): - def __new__(cls, input_array: np.ndarray): - """ - Create a new instance of the class. - """ - - return np.asarray(input_array).view(cls) - - @classmethod - def from_qdoti(cls, tuple_of_Q: tuple): - """ - Create a new instance of the class. - """ - if not isinstance(tuple_of_Q, tuple): - raise ValueError("tuple_of_Q must be a tuple of SegmentGeneralizedCoordinates") - - for Q in tuple_of_Q: - if not isinstance(Q, SegmentNaturalVelocities): - raise ValueError("tuple_of_Q must be a tuple of SegmentGeneralizedCoordinates") - - input_array = np.concatenate(tuple_of_Q, axis=0) - return NaturalVelocities(input_array) - - def to_array(self): - return np.array(self) - - def nb_qdoti(self): - return self.shape[0] // 12 - - def udot(self, segment_idx: int): - array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12)[0:3] - return self[array_idx].to_array() - - def rpdot(self, segment_idx: int): - array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12)[3:6] - return self[array_idx].to_array() - - def rddot(self, segment_idx: int): - array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12)[6:9] - return self[array_idx].to_array() - - def wdot(self, segment_idx: int): - array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12)[9:12] - return self[array_idx].to_array() - - def vdot(self, segment_idx: int): - return self.rpdot(segment_idx) - self.rddot(segment_idx) - - def vector(self, segment_idx: int): - array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12) - return SegmentNaturalVelocities(self[array_idx].to_array()) diff --git a/tests/test_biomech_model.py b/tests/test_biomech_model.py index 87cbbee0..a983cfd5 100644 --- a/tests/test_biomech_model.py +++ b/tests/test_biomech_model.py @@ -3,7 +3,7 @@ from .utils import TestUtils -from bionc import SegmentNaturalCoordinates, NaturalCoordinates, SegmentNaturalVelocities, NaturalVelocities +from bionc import SegmentNaturalVelocities, NaturalVelocities from bionc import bionc_numpy as bionc_np @@ -772,25 +772,25 @@ def test_biomech_model(): ) # Test rigid body constraint jacobian derivative - Qdot1 = SegmentNaturalVelocities.from_components( + Qdot1 = bionc_np.SegmentNaturalVelocities.from_components( udot=[1, 2, 3.05], rpdot=[1.1, 1, 3.1], rddot=[1.2, 2, 4.1], wdot=[1.3, 2, 5.1], ) - Qdot2 = SegmentNaturalVelocities.from_components( + Qdot2 = bionc_np.SegmentNaturalVelocities.from_components( udot=[1.4, 2, 3.2], rpdot=[1.5, 1, 3.2], rddot=[1.6, 2, 4.2], wdot=[1.7, 2, 5.2], ) - Qdot3 = SegmentNaturalVelocities.from_components( + Qdot3 = bionc_np.SegmentNaturalVelocities.from_components( udot=[1.8, 2, 3.3], rpdot=[1.9, 1, 3.3], rddot=[2.1, 2, 4.3], wdot=[2.2, 2, 5.3], ) - Qdot = NaturalVelocities.from_qdoti((Qdot1, Qdot2, Qdot3)) + Qdot = bionc_np.NaturalVelocities.from_qdoti((Qdot1, Qdot2, Qdot3)) np.testing.assert_array_almost_equal( natural_model.rigid_body_constraint_jacobian_derivative(Qdot), diff --git a/tests/test_homegenous_transform.py b/tests/test_homegenous_transform.py index 9449d3a2..cdb77aa3 100644 --- a/tests/test_homegenous_transform.py +++ b/tests/test_homegenous_transform.py @@ -1,12 +1,12 @@ import numpy as np -from bionc import HomogeneousTransform +from bionc import bionc_numpy as bionc_np # generate tests for the class to be use with pytest def test_homogenous_transform(): # test constructor - ht = HomogeneousTransform(np.eye(4)) + ht = bionc_np.HomogeneousTransform(np.eye(4)) assert np.allclose(ht.rot, np.eye(3)) assert np.allclose(ht.translation, np.zeros(3)) @@ -15,29 +15,29 @@ def test_homogenous_transform(): y = np.array([[0], [1], [0]]) z = np.array([[0], [0], [1]]) t = np.array([[0], [0], [0]]) - ht = HomogeneousTransform.from_components(x, y, z, t) + ht = bionc_np.HomogeneousTransform.from_components(x, y, z, t) assert np.allclose(ht.rot, np.eye(3)) assert np.allclose(ht.translation, np.zeros(3)) # test from_rt r = np.eye(3) t = np.zeros((3, 1)) - ht = HomogeneousTransform.from_rt(r, t) + ht = bionc_np.HomogeneousTransform.from_rt(r, t) assert np.allclose(ht.rot, np.eye(3)) assert np.allclose(ht.translation, np.zeros(3)) # test eye - ht = HomogeneousTransform.eye() + ht = bionc_np.HomogeneousTransform.eye() assert np.allclose(ht.rot, np.eye(3)) assert np.allclose(ht.translation, np.zeros(3)) - assert np.allclose(ht.to_array(), HomogeneousTransform(np.eye(4))) + assert np.allclose(ht.to_array(), bionc_np.HomogeneousTransform(np.eye(4))) # test inv x = np.array([[1], [0], [0]]) y = np.array([[0], [np.sqrt(2) / 2], [-np.sqrt(2) / 2]]) z = np.array([[0], [np.sqrt(2) / 2], [np.sqrt(2) / 2]]) t = np.array([[2], [3], [1]]) - ht = HomogeneousTransform.from_components(x, y, z, t) + ht = bionc_np.HomogeneousTransform.from_components(x, y, z, t) ht_inv = ht.inv() ht_inv_np = np.linalg.inv(ht.to_array()) assert np.allclose(ht_inv.to_array(), ht_inv_np) @@ -45,14 +45,14 @@ def test_homogenous_transform(): assert np.allclose(ht_inv.translation, -ht_inv.rot @ ht.translation) # test __getitem__ - ht = HomogeneousTransform.eye() + ht = bionc_np.HomogeneousTransform.eye() assert np.allclose(ht[0, 0], 1) assert np.allclose(ht[1, 1], 1) assert np.allclose(ht[2, 2], 1) assert np.allclose(ht[3, 3], 1) # test __setitem__ - ht = HomogeneousTransform.eye() + ht = bionc_np.HomogeneousTransform.eye() ht[0, 0] = 2 ht[1, 1] = 2 ht[2, 2] = 2 @@ -63,8 +63,8 @@ def test_homogenous_transform(): assert np.allclose(ht[3, 3], 2) # test __add__ - ht1 = HomogeneousTransform.eye() - ht2 = HomogeneousTransform.eye() + ht1 = bionc_np.HomogeneousTransform.eye() + ht2 = bionc_np.HomogeneousTransform.eye() ht3 = ht1 + ht2 assert np.allclose(ht3[0, 0], 2) assert np.allclose(ht3[1, 1], 2) @@ -72,8 +72,8 @@ def test_homogenous_transform(): assert np.allclose(ht3[3, 3], 2) # test __sub__ - ht1 = HomogeneousTransform.eye() - ht2 = HomogeneousTransform.eye() + ht1 = bionc_np.HomogeneousTransform.eye() + ht2 = bionc_np.HomogeneousTransform.eye() ht3 = ht1 - ht2 assert np.allclose(ht3[0, 0], 0) assert np.allclose(ht3[1, 1], 0) @@ -81,8 +81,8 @@ def test_homogenous_transform(): assert np.allclose(ht3[3, 3], 0) # test __mul__ - ht1 = HomogeneousTransform.eye() - ht2 = HomogeneousTransform.eye() + ht1 = bionc_np.HomogeneousTransform.eye() + ht2 = bionc_np.HomogeneousTransform.eye() ht3 = ht1 * ht2 assert np.allclose(ht3[0, 0], 1) assert np.allclose(ht3[1, 1], 1) @@ -90,8 +90,8 @@ def test_homogenous_transform(): assert np.allclose(ht3[3, 3], 1) # test __rmul__ - ht1 = HomogeneousTransform.eye() - ht2 = HomogeneousTransform.eye() + ht1 = bionc_np.HomogeneousTransform.eye() + ht2 = bionc_np.HomogeneousTransform.eye() ht3 = ht1 * ht2 assert np.allclose(ht3[0, 0], 1) assert np.allclose(ht3[1, 1], 1) diff --git a/tests/test_natural_coordinates.py b/tests/test_natural_coordinates.py index 2545dc11..c940e797 100644 --- a/tests/test_natural_coordinates.py +++ b/tests/test_natural_coordinates.py @@ -25,7 +25,7 @@ def test_SegmentNaturalCoordinatesCreator(): # accelerations def test_NaturalAccelerationsCreator(): - Qddot1 = SegmentNaturalAccelerations.from_components( + Qddot1 = bionc_np.SegmentNaturalAccelerations.from_components( uddot=np.array([1, 2, 3]), wddot=np.array([4, 5, 6]), rdddot=np.array([7, 8, 9]), @@ -61,13 +61,13 @@ def test_concatenate(): def test_concatenate_accelerations(): - Qddot1 = SegmentNaturalAccelerations.from_components( + Qddot1 = bionc_np.SegmentNaturalAccelerations.from_components( uddot=np.array([1, 2, 3]), wddot=np.array([4, 5, 6]), rdddot=np.array([7, 8, 9]), rpddot=np.array([10, 11, 12]), ) - Qddot2 = SegmentNaturalAccelerations.from_components( + Qddot2 = bionc_np.SegmentNaturalAccelerations.from_components( uddot=np.array([11, 22, 33]), wddot=np.array([4, 5, 6]), rdddot=np.array([7, 82, 9]), @@ -109,19 +109,19 @@ def test_NaturalCoordinatesConstructor(): # do the same tests for NaturalAccelerations and SegmentNaturalAccelerations def test_NaturalAccelerationsConstructor(): - Qddot1 = SegmentNaturalAccelerations.from_components( + Qddot1 = bionc_np.SegmentNaturalAccelerations.from_components( uddot=np.array([1, 2, 3]), wddot=np.array([4, 5, 6]), rdddot=np.array([7, 8, 9]), rpddot=np.array([10, 11, 12]), ) - Qddot2 = SegmentNaturalAccelerations.from_components( + Qddot2 = bionc_np.SegmentNaturalAccelerations.from_components( uddot=np.array([11, 22, 33]), wddot=np.array([4, 5, 6]), rdddot=np.array([7, 82, 9]), rpddot=np.array([110, 11, 12]), ) - Qddot = NaturalAccelerations.from_qddoti((Qddot1, Qddot2)) + Qddot = bionc_np.NaturalAccelerations.from_qddoti((Qddot1, Qddot2)) np.testing.assert_equal(Qddot.uddot(0), np.array([1, 2, 3])) np.testing.assert_equal(Qddot.uddot(1), np.array([11, 22, 33])) np.testing.assert_equal(Qddot.vector(0), Qddot1) diff --git a/tests/test_natural_velocities.py b/tests/test_natural_velocities.py index ba89f96e..c929089f 100644 --- a/tests/test_natural_velocities.py +++ b/tests/test_natural_velocities.py @@ -1,4 +1,4 @@ -from bionc.utils import SegmentNaturalVelocities, NaturalVelocities +from bionc import bionc_numpy as bionc_np import numpy as np import pytest @@ -14,38 +14,38 @@ def test_natural_velocities(): # ------------------------------------------------------------------------------------------------------------------ # test None udot with pytest.raises(ValueError, match="u must be a numpy array .* or a list of 3 elements"): - SegmentNaturalVelocities.from_components(None, correct_vector, correct_vector, correct_vector) + bionc_np.SegmentNaturalVelocities.from_components(None, correct_vector, correct_vector, correct_vector) # test wrong vector udot with pytest.raises(ValueError, match="u must be a 3x1 numpy array"): - SegmentNaturalVelocities.from_components(wrong_vector, correct_vector, correct_vector, correct_vector) + bionc_np.SegmentNaturalVelocities.from_components(wrong_vector, correct_vector, correct_vector, correct_vector) # test None rpdot with pytest.raises(ValueError, match="rp must be a numpy array .* or a list of 3 elements"): - SegmentNaturalVelocities.from_components(correct_vector, None, correct_vector, correct_vector) + bionc_np.SegmentNaturalVelocities.from_components(correct_vector, None, correct_vector, correct_vector) # test wrong vector rpdot with pytest.raises(ValueError, match="rp must be a 3x1 numpy array"): - SegmentNaturalVelocities.from_components(correct_vector, wrong_vector, correct_vector, correct_vector) + bionc_np.SegmentNaturalVelocities.from_components(correct_vector, wrong_vector, correct_vector, correct_vector) # test None rddot with pytest.raises(ValueError, match="rd must be a numpy array .* or a list of 3 elements"): - SegmentNaturalVelocities.from_components(correct_vector, correct_vector, None, correct_vector) + bionc_np.SegmentNaturalVelocities.from_components(correct_vector, correct_vector, None, correct_vector) # test wrong vector rddot with pytest.raises(ValueError, match="rd must be a 3x1 numpy array"): - SegmentNaturalVelocities.from_components(correct_vector, correct_vector, wrong_vector, correct_vector) + bionc_np.SegmentNaturalVelocities.from_components(correct_vector, correct_vector, wrong_vector, correct_vector) # test None wdot with pytest.raises(ValueError, match="w must be a numpy array .* or a list of 3 elements"): - SegmentNaturalVelocities.from_components(correct_vector, correct_vector, correct_vector, None) + bionc_np.SegmentNaturalVelocities.from_components(correct_vector, correct_vector, correct_vector, None) # test wrong vector wdot with pytest.raises(ValueError, match="v must be a 3x1 numpy array"): - SegmentNaturalVelocities.from_components(correct_vector, correct_vector, correct_vector, wrong_vector) + bionc_np.SegmentNaturalVelocities.from_components(correct_vector, correct_vector, correct_vector, wrong_vector) # Test concatenate + parameters - qdot_test = SegmentNaturalVelocities.from_components( + qdot_test = bionc_np.SegmentNaturalVelocities.from_components( udot=[1, 0, 0], rpdot=[2, 0, 0], rddot=[3, 0, 0], @@ -70,13 +70,13 @@ def test_natural_velocities(): # ------------------------------------------------------------------------------------------------------------------- # NaturalVelocities # ------------------------------------------------------------------------------------------------------------------ - qdot1 = SegmentNaturalVelocities.from_components( + qdot1 = bionc_np.SegmentNaturalVelocities.from_components( udot=np.array([1, 2, 3]), wdot=np.array([4, 5, 6]), rddot=np.array([7, 8, 9]), rpdot=np.array([10, 11, 12]), ) - qdot2 = SegmentNaturalVelocities.from_components( + qdot2 = bionc_np.SegmentNaturalVelocities.from_components( udot=np.array([11, 22, 33]), wdot=np.array([4, 5, 6]), rddot=np.array([7, 82, 9]), @@ -85,13 +85,13 @@ def test_natural_velocities(): # Wrong entry with pytest.raises(ValueError, match="tuple_of_Q must be a tuple of SegmentGeneralizedCoordinates"): - NaturalVelocities.from_qdoti(1) + bionc_np.NaturalVelocities.from_qdoti(1) # One wrong entry in the list with pytest.raises(ValueError, match="tuple_of_Q must be a tuple of SegmentGeneralizedCoordinates"): - NaturalVelocities.from_qdoti((qdot1, qdot2, [0, 0])) + bionc_np.NaturalVelocities.from_qdoti((qdot1, qdot2, [0, 0])) - qdot = NaturalVelocities.from_qdoti((qdot1, qdot2)) + qdot = bionc_np.NaturalVelocities.from_qdoti((qdot1, qdot2)) # np.testing.assert_equal(qdot.udot(0), np.array([1, 2, 3])) From 7a2b8761ef16d5a9064209a165c2011b492516c0 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Wed, 9 Nov 2022 23:15:26 -0500 Subject: [PATCH 12/30] road to test bionc_mx --- .../using_casadi/natural_accelerations.py | 4 +- .../using_casadi/natural_coordinates.py | 4 +- .../using_casadi/natural_velocities.py | 12 +- .../using_numpy/natural_accelerations.py | 4 +- .../using_numpy/natural_coordinates.py | 4 +- .../using_numpy/natural_velocities.py | 4 +- tests/test_natural_coordinates.py | 4 +- tests/test_natural_velocities.py | 123 +++++++++++++++++- tests/utils.py | 24 ++++ 9 files changed, 162 insertions(+), 21 deletions(-) diff --git a/bionc/math_interface/using_casadi/natural_accelerations.py b/bionc/math_interface/using_casadi/natural_accelerations.py index f1bb0d83..096d190f 100644 --- a/bionc/math_interface/using_casadi/natural_accelerations.py +++ b/bionc/math_interface/using_casadi/natural_accelerations.py @@ -107,11 +107,11 @@ def from_qddoti(cls, tuple_of_Q: tuple): Constructor of the class """ if not isinstance(tuple_of_Q, tuple): - raise ValueError("tuple_of_Q must be a tuple of SegmentGeneralizedCoordinates") + raise ValueError("tuple_of_Q must be a tuple of SegmentNaturalAccelerations") for Q in tuple_of_Q: if not isinstance(Q, SegmentNaturalAccelerations): - raise ValueError("tuple_of_Q must be a tuple of SegmentGeneralizedCoordinates") + raise ValueError("tuple_of_Q must be a tuple of SegmentNaturalAccelerations") input_array = vertcat(*tuple_of_Q) return cls(input_array) diff --git a/bionc/math_interface/using_casadi/natural_coordinates.py b/bionc/math_interface/using_casadi/natural_coordinates.py index 1dcd8da6..84b2d7ec 100644 --- a/bionc/math_interface/using_casadi/natural_coordinates.py +++ b/bionc/math_interface/using_casadi/natural_coordinates.py @@ -152,11 +152,11 @@ def from_qi(cls, tuple_of_Q: tuple): Constructor of the class. """ if not isinstance(tuple_of_Q, tuple): - raise ValueError("tuple_of_Q must be a tuple of SegmentGeneralizedCoordinates") + raise ValueError("tuple_of_Q must be a tuple of SegmentNaturalCoordinates") for Q in tuple_of_Q: if not isinstance(Q, SegmentNaturalCoordinates): - raise ValueError("tuple_of_Q must be a tuple of SegmentGeneralizedCoordinates") + raise ValueError("tuple_of_Q must be a tuple of SegmentNaturalCoordinates") input_array = vertcat(*tuple_of_Q) return cls(input_array) diff --git a/bionc/math_interface/using_casadi/natural_velocities.py b/bionc/math_interface/using_casadi/natural_velocities.py index a655dd86..36d529b2 100644 --- a/bionc/math_interface/using_casadi/natural_velocities.py +++ b/bionc/math_interface/using_casadi/natural_velocities.py @@ -106,11 +106,11 @@ def from_qdoti(cls, tuple_of_Q: tuple): Create a new instance of the class. """ if not isinstance(tuple_of_Q, tuple): - raise ValueError("tuple_of_Q must be a tuple of SegmentGeneralizedCoordinates") + raise ValueError("tuple_of_Q must be a tuple of SegmentNaturalVelocities") for Q in tuple_of_Q: if not isinstance(Q, SegmentNaturalVelocities): - raise ValueError("tuple_of_Q must be a tuple of SegmentGeneralizedCoordinates") + raise ValueError("tuple_of_Q must be a tuple of SegmentNaturalVelocities") input_array = vertcat(*tuple_of_Q) return cls(input_array) @@ -123,19 +123,19 @@ def nb_qdoti(self): def udot(self, segment_idx: int): array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12)[0:3] - return self[array_idx].to_array() + return self[array_idx] def rpdot(self, segment_idx: int): array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12)[3:6] - return self[array_idx].to_array() + return self[array_idx] def rddot(self, segment_idx: int): array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12)[6:9] - return self[array_idx].to_array() + return self[array_idx] def wdot(self, segment_idx: int): array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12)[9:12] - return self[array_idx].to_array() + return self[array_idx] def vdot(self, segment_idx: int): return self.rpdot(segment_idx) - self.rddot(segment_idx) diff --git a/bionc/math_interface/using_numpy/natural_accelerations.py b/bionc/math_interface/using_numpy/natural_accelerations.py index e9d25f36..5a4173ba 100644 --- a/bionc/math_interface/using_numpy/natural_accelerations.py +++ b/bionc/math_interface/using_numpy/natural_accelerations.py @@ -101,11 +101,11 @@ def from_qddoti(cls, tuple_of_Q: tuple): Constructor of the class """ if not isinstance(tuple_of_Q, tuple): - raise ValueError("tuple_of_Q must be a tuple of SegmentGeneralizedCoordinates") + raise ValueError("tuple_of_Q must be a tuple of SegmentNaturalAccelerations") for Q in tuple_of_Q: if not isinstance(Q, SegmentNaturalAccelerations): - raise ValueError("tuple_of_Q must be a tuple of SegmentGeneralizedCoordinates") + raise ValueError("tuple_of_Q must be a tuple of SegmentNaturalAccelerations") input_array = np.concatenate(tuple_of_Q, axis=0) return cls(input_array) diff --git a/bionc/math_interface/using_numpy/natural_coordinates.py b/bionc/math_interface/using_numpy/natural_coordinates.py index 81b90b01..bb80daf0 100644 --- a/bionc/math_interface/using_numpy/natural_coordinates.py +++ b/bionc/math_interface/using_numpy/natural_coordinates.py @@ -151,11 +151,11 @@ def from_qi(cls, tuple_of_Q: tuple): Constructor of the class. """ if not isinstance(tuple_of_Q, tuple): - raise ValueError("tuple_of_Q must be a tuple of SegmentGeneralizedCoordinates") + raise ValueError("tuple_of_Q must be a tuple of SegmentNaturalCoordinates") for Q in tuple_of_Q: if not isinstance(Q, SegmentNaturalCoordinates): - raise ValueError("tuple_of_Q must be a tuple of SegmentGeneralizedCoordinates") + raise ValueError("tuple_of_Q must be a tuple of SegmentNaturalCoordinates") input_array = np.concatenate(tuple_of_Q, axis=0) return cls(input_array) diff --git a/bionc/math_interface/using_numpy/natural_velocities.py b/bionc/math_interface/using_numpy/natural_velocities.py index 8a33c189..f5f4948b 100644 --- a/bionc/math_interface/using_numpy/natural_velocities.py +++ b/bionc/math_interface/using_numpy/natural_velocities.py @@ -100,11 +100,11 @@ def from_qdoti(cls, tuple_of_Q: tuple): Create a new instance of the class. """ if not isinstance(tuple_of_Q, tuple): - raise ValueError("tuple_of_Q must be a tuple of SegmentGeneralizedCoordinates") + raise ValueError("tuple_of_Q must be a tuple of SegmentNaturalVelocities") for Q in tuple_of_Q: if not isinstance(Q, SegmentNaturalVelocities): - raise ValueError("tuple_of_Q must be a tuple of SegmentGeneralizedCoordinates") + raise ValueError("tuple_of_Q must be a tuple of SegmentNaturalVelocities") input_array = np.concatenate(tuple_of_Q, axis=0) return cls(input_array) diff --git a/tests/test_natural_coordinates.py b/tests/test_natural_coordinates.py index c940e797..f2eb00d8 100644 --- a/tests/test_natural_coordinates.py +++ b/tests/test_natural_coordinates.py @@ -53,7 +53,7 @@ def test_concatenate(): ) # Methods such as u, v, w - # of SegmentGeneralizedCoordinatesInterface are not inherited if Q1 and Q2 are concatenated with numpy method + # of SegmentNaturalCoordinatesInterface are not inherited if Q1 and Q2 are concatenated with numpy method Q = np.concatenate((Q1, Q2), axis=0) # this would raise an error with pytest.raises(AttributeError, match="'numpy.ndarray' object has no attribute 'u'"): @@ -78,7 +78,7 @@ def test_concatenate_accelerations(): Qddot.uddot -# Build a class called GeneralizedCoordinates to handle the concatenation of SegmentGeneralizedCoordinates +# Build a class called GeneralizedCoordinates to handle the concatenation of SegmentNaturalCoordinates def test_NaturalCoordinatesConstructor(): Q1 = bionc_np.SegmentNaturalCoordinates.from_components( u=np.array([1, 2, 3]), diff --git a/tests/test_natural_velocities.py b/tests/test_natural_velocities.py index c929089f..a0541d9c 100644 --- a/tests/test_natural_velocities.py +++ b/tests/test_natural_velocities.py @@ -1,9 +1,12 @@ from bionc import bionc_numpy as bionc_np +from bionc import bionc_casadi as bionc_mx import numpy as np +from casadi import MX import pytest +from .utils import TestUtils -def test_natural_velocities(): +def test_natural_velocities_numpy(): # ------------------------------------------------------------------------------------------------------------------- # SegmentNaturalVelocities # ------------------------------------------------------------------------------------------------------------------ @@ -84,11 +87,11 @@ def test_natural_velocities(): ) # Wrong entry - with pytest.raises(ValueError, match="tuple_of_Q must be a tuple of SegmentGeneralizedCoordinates"): + with pytest.raises(ValueError, match="tuple_of_Q must be a tuple of SegmentNaturalVelocities"): bionc_np.NaturalVelocities.from_qdoti(1) # One wrong entry in the list - with pytest.raises(ValueError, match="tuple_of_Q must be a tuple of SegmentGeneralizedCoordinates"): + with pytest.raises(ValueError, match="tuple_of_Q must be a tuple of SegmentNaturalVelocities"): bionc_np.NaturalVelocities.from_qdoti((qdot1, qdot2, [0, 0])) qdot = bionc_np.NaturalVelocities.from_qdoti((qdot1, qdot2)) @@ -119,3 +122,117 @@ def test_natural_velocities(): qdot = np.concatenate((qdot1, qdot2), axis=0) with pytest.raises(AttributeError, match="'numpy.ndarray' object has no attribute 'udot'"): qdot.udot + + +def test_natural_velocities_casadi(): + # ------------------------------------------------------------------------------------------------------------------- + # SegmentNaturalVelocities + # ------------------------------------------------------------------------------------------------------------------ + # List instead of np array to test the translation from list to np.array + correct_vector = [1, 0, 0] + wrong_vector = [1, 0] + # Test wrong entry + # ------------------------------------------------------------------------------------------------------------------ + # test None udot + with pytest.raises(ValueError, match="u must be a array .* or a list of 3 elements"): + bionc_mx.SegmentNaturalVelocities.from_components(None, correct_vector, correct_vector, correct_vector) + + # test wrong vector udot + with pytest.raises(ValueError, match="u must be a 3x1 array"): + bionc_mx.SegmentNaturalVelocities.from_components(wrong_vector, correct_vector, correct_vector, correct_vector) + + # test None rpdot + with pytest.raises(ValueError, match="rp must be a array .* or a list of 3 elements"): + bionc_mx.SegmentNaturalVelocities.from_components(correct_vector, None, correct_vector, correct_vector) + + # test wrong vector rpdot + with pytest.raises(ValueError, match="rp must be a 3x1 array"): + bionc_mx.SegmentNaturalVelocities.from_components(correct_vector, wrong_vector, correct_vector, correct_vector) + + # test None rddot + with pytest.raises(ValueError, match="rd must be a array .* or a list of 3 elements"): + bionc_mx.SegmentNaturalVelocities.from_components(correct_vector, correct_vector, None, correct_vector) + + # test wrong vector rddot + with pytest.raises(ValueError, match="rd must be a 3x1 array"): + bionc_mx.SegmentNaturalVelocities.from_components(correct_vector, correct_vector, wrong_vector, correct_vector) + + # test None wdot + with pytest.raises(ValueError, match="w must be a array .* or a list of 3 elements"): + bionc_mx.SegmentNaturalVelocities.from_components(correct_vector, correct_vector, correct_vector, None) + + # test wrong vector wdot + with pytest.raises(ValueError, match="v must be a 3x1 array"): + bionc_mx.SegmentNaturalVelocities.from_components(correct_vector, correct_vector, correct_vector, wrong_vector) + + # Test concatenate + parameters + qdot_test = bionc_mx.SegmentNaturalVelocities.from_components( + udot=[1, 0, 0], + rpdot=[2, 0, 0], + rddot=[3, 0, 0], + wdot=[4, 0, 0], + ) + + assert np.all(TestUtils.mx_to_array(qdot_test) == np.array([1, 0, 0, 2, 0, 0, 3, 0, 0, 4, 0, 0])) + assert np.all(TestUtils.mx_to_array(qdot_test.udot) == np.array([1, 0, 0])) + assert np.all(TestUtils.mx_to_array(qdot_test.rpdot) == np.array([2, 0, 0])) + assert np.all(TestUtils.mx_to_array(qdot_test.rddot) == np.array([3, 0, 0])) + assert np.all(TestUtils.mx_to_array(qdot_test.wdot) == np.array([4, 0, 0])) + + # vdot = rpdot - rddot + assert np.all(TestUtils.mx_to_array(qdot_test.vdot) == np.array([-1, 0, 0])) + + # vectors + assert np.all(TestUtils.mx_to_array(qdot_test.vector) == np.array([1, 0, 0, 2, 0, 0, 3, 0, 0, 4, 0, 0])) + + for ind, component in enumerate(qdot_test.to_components): + assert np.all(TestUtils.mx_to_array(component) == np.array([ind + 1, 0, 0])) + + # ------------------------------------------------------------------------------------------------------------------- + # NaturalVelocities + # ------------------------------------------------------------------------------------------------------------------ + qdot1 = bionc_mx.SegmentNaturalVelocities.from_components( + udot=np.array([1, 2, 3]), + wdot=np.array([4, 5, 6]), + rddot=np.array([7, 8, 9]), + rpdot=np.array([10, 11, 12]), + ) + qdot2 = bionc_mx.SegmentNaturalVelocities.from_components( + udot=np.array([11, 22, 33]), + wdot=np.array([4, 5, 6]), + rddot=np.array([7, 82, 9]), + rpdot=np.array([110, 11, 12]), + ) + + # Wrong entry + with pytest.raises(ValueError, match="tuple_of_Q must be a tuple of SegmentNaturalVelocities"): + bionc_mx.NaturalVelocities.from_qdoti(1) + + # One wrong entry in the list + with pytest.raises(ValueError, match="tuple_of_Q must be a tuple of SegmentNaturalVelocities"): + bionc_mx.NaturalVelocities.from_qdoti((qdot1, qdot2, [0, 0])) + + qdot = bionc_mx.NaturalVelocities.from_qdoti((qdot1, qdot2)) + + # + np.testing.assert_equal(TestUtils.mx_to_array(qdot.udot(0)), np.array([1, 2, 3])) + np.testing.assert_equal(TestUtils.mx_to_array(qdot.udot(1)), np.array([11, 22, 33])) + + # test nb_qdoti + np.testing.assert_equal(qdot.nb_qdoti(), 2) + + # test of the different component of vector + np.testing.assert_equal(TestUtils.mx_to_array(qdot1.vector), TestUtils.mx_to_array(qdot1)) + np.testing.assert_equal(TestUtils.mx_to_array(qdot.vector(0)), TestUtils.mx_to_array(qdot1)) + np.testing.assert_equal(TestUtils.mx_to_array(qdot.vector(1)), TestUtils.mx_to_array(qdot2)) + np.testing.assert_equal(TestUtils.mx_to_array(qdot.vector(0).udot), np.array([1, 2, 3])) + np.testing.assert_equal(TestUtils.mx_to_array(qdot.vector(1).udot), np.array([11, 22, 33])) + np.testing.assert_equal(TestUtils.mx_to_array(qdot.vector(0).wdot), np.array([4, 5, 6])) + np.testing.assert_equal(TestUtils.mx_to_array(qdot.vector(1).wdot), np.array([4, 5, 6])) + np.testing.assert_equal(TestUtils.mx_to_array(qdot.vector(0).rddot), np.array([7, 8, 9])) + np.testing.assert_equal(TestUtils.mx_to_array(qdot.vector(1).rddot), np.array([7, 82, 9])) + np.testing.assert_equal(TestUtils.mx_to_array(qdot.vector(0).rpdot), np.array([10, 11, 12])) + np.testing.assert_equal(TestUtils.mx_to_array(qdot.vector(1).rpdot), np.array([110, 11, 12])) + np.testing.assert_equal(TestUtils.mx_to_array(qdot.vector(0).vdot), np.array([3, 3, 3])) + np.testing.assert_equal(TestUtils.mx_to_array(qdot.vector(1).vdot), np.array([103, -71, 3])) + diff --git a/tests/utils.py b/tests/utils.py index 58674df6..33d5e036 100644 --- a/tests/utils.py +++ b/tests/utils.py @@ -1,6 +1,7 @@ from typing import Any from pathlib import Path import importlib.util +from casadi import MX, Function class TestUtils: @@ -18,3 +19,26 @@ def load_module(path: str) -> Any: module = importlib.util.module_from_spec(spec) spec.loader.exec_module(module) return module + + @staticmethod + def to_casadi_func(mx: MX): + return Function( + "f", + [], + [mx], + [], + ["f"], + ).expand() + + @staticmethod + def mx_to_array(mx: MX): + """ + Convert a casadi MX to a numpy array if it is only numeric values + """ + return Function( + "f", + [], + [mx], + [], + ["f"], + ).expand()()["f"].toarray().squeeze() From 208454b34d3c73a9ec900ec330cf93c310385499 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Wed, 9 Nov 2022 23:17:25 -0500 Subject: [PATCH 13/30] blacked --- .../protocols/natural_accelerations.py | 7 +++++++ .../protocols/natural_coordinates.py | 7 +++++++ .../protocols/natural_velocities.py | 7 +++++++ .../using_casadi/homogenous_transform.py | 1 + bionc/utils/interpolation_matrix.py | 1 - tests/test_natural_velocities.py | 3 +-- tests/utils.py | 19 ++++++++++++------- 7 files changed, 35 insertions(+), 10 deletions(-) diff --git a/bionc/math_interface/protocols/natural_accelerations.py b/bionc/math_interface/protocols/natural_accelerations.py index 3e6a6dcc..cd6c065a 100644 --- a/bionc/math_interface/protocols/natural_accelerations.py +++ b/bionc/math_interface/protocols/natural_accelerations.py @@ -68,28 +68,35 @@ def to_array(self): def nb_qddoti(self): """This function returns the number of qi""" + ... def uddot(self, segment_index): """This property returns the u vector of the natural coordinates""" + ... def rpddot(self, segment_index): """This property returns the rp vector of the natural coordinates""" + ... def rdddot(self, segment_index): """This property returns the rd vector of the natural coordinates""" + ... def wddot(self, segment_index): """This property returns the w vector of the natural coordinates""" + ... def vddot(self, segment_index): """This property returns the v vector of the natural coordinates""" + ... def vector(self): """This property returns the vector of the natural coordinates""" + ... diff --git a/bionc/math_interface/protocols/natural_coordinates.py b/bionc/math_interface/protocols/natural_coordinates.py index 402d38e4..7505de63 100644 --- a/bionc/math_interface/protocols/natural_coordinates.py +++ b/bionc/math_interface/protocols/natural_coordinates.py @@ -87,28 +87,35 @@ def to_array(self): def nb_qi(self): """This function returns the number of qi""" + ... def u(self, segment_index): """This property returns the u vector of the natural coordinates""" + ... def rp(self, segment_index): """This property returns the rp vector of the natural coordinates""" + ... def rd(self, segment_index): """This property returns the rd vector of the natural coordinates""" + ... def w(self, segment_index): """This property returns the w vector of the natural coordinates""" + ... def v(self, segment_index): """This property returns the v vector of the natural coordinates""" + ... def vector(self): """This property returns the vector of the natural coordinates""" + ... diff --git a/bionc/math_interface/protocols/natural_velocities.py b/bionc/math_interface/protocols/natural_velocities.py index 6b4d15cc..6e609228 100644 --- a/bionc/math_interface/protocols/natural_velocities.py +++ b/bionc/math_interface/protocols/natural_velocities.py @@ -68,28 +68,35 @@ def to_array(self): def nb_qdoti(self): """This function returns the number of qi""" + ... def udot(self, segment_index): """This property returns the u vector of the natural coordinates""" + ... def rpdot(self, segment_index): """This property returns the rp vector of the natural coordinates""" + ... def rddot(self, segment_index): """This property returns the rd vector of the natural coordinates""" + ... def wdot(self, segment_index): """This property returns the w vector of the natural coordinates""" + ... def vdot(self, segment_index): """This property returns the v vector of the natural coordinates""" + ... def vector(self): """This property returns the vector of the natural coordinates""" + ... diff --git a/bionc/math_interface/using_casadi/homogenous_transform.py b/bionc/math_interface/using_casadi/homogenous_transform.py index 34485b35..7e3a61da 100644 --- a/bionc/math_interface/using_casadi/homogenous_transform.py +++ b/bionc/math_interface/using_casadi/homogenous_transform.py @@ -7,6 +7,7 @@ class HomogeneousTransform(MX): """ Homogenous transform class """ + def __new__(cls, input_array: MX): """ Create a new instance of the class. diff --git a/bionc/utils/interpolation_matrix.py b/bionc/utils/interpolation_matrix.py index 9b887388..9f0116e4 100644 --- a/bionc/utils/interpolation_matrix.py +++ b/bionc/utils/interpolation_matrix.py @@ -59,4 +59,3 @@ def to_natural_vector(interpolation_matrix: np.ndarray) -> np.ndarray: vector[2] = interpolation_matrix[0, 9] return vector - diff --git a/tests/test_natural_velocities.py b/tests/test_natural_velocities.py index a0541d9c..260dd264 100644 --- a/tests/test_natural_velocities.py +++ b/tests/test_natural_velocities.py @@ -122,7 +122,7 @@ def test_natural_velocities_numpy(): qdot = np.concatenate((qdot1, qdot2), axis=0) with pytest.raises(AttributeError, match="'numpy.ndarray' object has no attribute 'udot'"): qdot.udot - + def test_natural_velocities_casadi(): # ------------------------------------------------------------------------------------------------------------------- @@ -235,4 +235,3 @@ def test_natural_velocities_casadi(): np.testing.assert_equal(TestUtils.mx_to_array(qdot.vector(1).rpdot), np.array([110, 11, 12])) np.testing.assert_equal(TestUtils.mx_to_array(qdot.vector(0).vdot), np.array([3, 3, 3])) np.testing.assert_equal(TestUtils.mx_to_array(qdot.vector(1).vdot), np.array([103, -71, 3])) - diff --git a/tests/utils.py b/tests/utils.py index 33d5e036..ab8039ea 100644 --- a/tests/utils.py +++ b/tests/utils.py @@ -35,10 +35,15 @@ def mx_to_array(mx: MX): """ Convert a casadi MX to a numpy array if it is only numeric values """ - return Function( - "f", - [], - [mx], - [], - ["f"], - ).expand()()["f"].toarray().squeeze() + return ( + Function( + "f", + [], + [mx], + [], + ["f"], + ) + .expand()()["f"] + .toarray() + .squeeze() + ) From 317112f0c3b4be0b974fe3119588bd0002a6fb80 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Wed, 9 Nov 2022 23:25:49 -0500 Subject: [PATCH 14/30] testing casadi constructors --- .../using_casadi/natural_accelerations.py | 12 +-- .../using_casadi/natural_coordinates.py | 2 +- tests/test_natural_coordinates.py | 88 ++++++++++++++++++- 3 files changed, 92 insertions(+), 10 deletions(-) diff --git a/bionc/math_interface/using_casadi/natural_accelerations.py b/bionc/math_interface/using_casadi/natural_accelerations.py index 096d190f..d70a65dc 100644 --- a/bionc/math_interface/using_casadi/natural_accelerations.py +++ b/bionc/math_interface/using_casadi/natural_accelerations.py @@ -8,12 +8,12 @@ class SegmentNaturalAccelerations(MX): This class is made to handle Generalized Coordinates of a Segment """ - def __new__(cls, input_array: Union[MX, list, tuple]): + def __new__(cls, input_array: MX): """ Create a new instance of the class. """ - obj = np.asarray(input_array).view(cls) + obj = MX.__new__(cls) return obj @@ -124,19 +124,19 @@ def nb_qddoti(self): def uddot(self, segment_idx: int): array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12)[0:3] - return self[array_idx].to_array() + return self[array_idx] def rpddot(self, segment_idx: int): array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12)[3:6] - return self[array_idx].to_array() + return self[array_idx] def rdddot(self, segment_idx: int): array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12)[6:9] - return self[array_idx].to_array() + return self[array_idx] def wddot(self, segment_idx: int): array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12)[9:12] - return self[array_idx].to_array() + return self[array_idx] def vddot(self, segment_idx: int): return self.rpddot(segment_idx) - self.rdddot(segment_idx) diff --git a/bionc/math_interface/using_casadi/natural_coordinates.py b/bionc/math_interface/using_casadi/natural_coordinates.py index 84b2d7ec..c8a72bb1 100644 --- a/bionc/math_interface/using_casadi/natural_coordinates.py +++ b/bionc/math_interface/using_casadi/natural_coordinates.py @@ -191,4 +191,4 @@ def vector(self, segment_idx: int = None): return self[:] else: array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12).tolist() - return SegmentNaturalCoordinates.MX(self[array_idx]) + return SegmentNaturalCoordinates(self[array_idx]) diff --git a/tests/test_natural_coordinates.py b/tests/test_natural_coordinates.py index f2eb00d8..b020518d 100644 --- a/tests/test_natural_coordinates.py +++ b/tests/test_natural_coordinates.py @@ -4,10 +4,13 @@ NaturalAccelerations, SegmentNaturalAccelerations, bionc_numpy as bionc_np, + bionc_casadi as bionc_mx, ) +from .utils import TestUtils -def test_SegmentNaturalCoordinatesCreator(): + +def test_SegmentNaturalCoordinates(): Qi = bionc_np.SegmentNaturalCoordinates.from_components( u=np.array([0, 0, 0]), @@ -23,6 +26,22 @@ def test_SegmentNaturalCoordinatesCreator(): np.testing.assert_equal(Qi.vector[:, np.newaxis], Qi) +def test_SegmentNaturalCoordinates_casadi(): + + Qi = bionc_mx.SegmentNaturalCoordinates.from_components( + u=np.array([0, 0, 0]), + rp=np.array([4, 5, 6]), + rd=np.array([7, 8, 9]), + w=np.array([10, 11, 12]), + ) + np.testing.assert_equal(TestUtils.mx_to_array(Qi.u), np.array([0, 0, 0])) + np.testing.assert_equal(TestUtils.mx_to_array(Qi.rp), np.array([4, 5, 6])) + np.testing.assert_equal(TestUtils.mx_to_array(Qi.rd), np.array([7, 8, 9])) + np.testing.assert_equal(TestUtils.mx_to_array(Qi.w), np.array([10, 11, 12])) + np.testing.assert_equal(TestUtils.mx_to_array(Qi.v), -np.array([7, 8, 9]) + np.array([4, 5, 6])) + np.testing.assert_equal(TestUtils.mx_to_array(Qi.vector), TestUtils.mx_to_array(Qi)) + + # accelerations def test_NaturalAccelerationsCreator(): Qddot1 = bionc_np.SegmentNaturalAccelerations.from_components( @@ -38,6 +57,20 @@ def test_NaturalAccelerationsCreator(): np.testing.assert_equal(Qddot1.vector, Qddot1) +def test_NaturalAccelerationsCreator_casadi(): + Qddot1 = bionc_mx.SegmentNaturalAccelerations.from_components( + uddot=np.array([1, 2, 3]), + wddot=np.array([4, 5, 6]), + rdddot=np.array([7, 8, 9]), + rpddot=np.array([10, 11, 12]), + ) + np.testing.assert_equal(TestUtils.mx_to_array(Qddot1.uddot), np.array([1, 2, 3])) + np.testing.assert_equal(TestUtils.mx_to_array(Qddot1.wddot), np.array([4, 5, 6])) + np.testing.assert_equal(TestUtils.mx_to_array(Qddot1.rdddot), np.array([7, 8, 9])) + np.testing.assert_equal(TestUtils.mx_to_array(Qddot1.rpddot), np.array([10, 11, 12])) + np.testing.assert_equal(TestUtils.mx_to_array(Qddot1.vector), TestUtils.mx_to_array(Qddot1)) + + def test_concatenate(): Q1 = bionc_np.SegmentNaturalCoordinates.from_components( u=np.array([1, 2, 3]), @@ -79,7 +112,7 @@ def test_concatenate_accelerations(): # Build a class called GeneralizedCoordinates to handle the concatenation of SegmentNaturalCoordinates -def test_NaturalCoordinatesConstructor(): +def test_NaturalCoordinates(): Q1 = bionc_np.SegmentNaturalCoordinates.from_components( u=np.array([1, 2, 3]), rp=np.array([4, 5, 6]), @@ -107,8 +140,34 @@ def test_NaturalCoordinatesConstructor(): np.testing.assert_equal(Q.nb_qi(), 2) +def test_NaturalCoordinates_casadi(): + Q1 = bionc_mx.SegmentNaturalCoordinates.from_components( + u=np.array([1, 2, 3]), + rp=np.array([4, 5, 6]), + rd=np.array([7, 8, 9]), + w=np.array([10, 11, 12]), + ) + Q2 = bionc_mx.SegmentNaturalCoordinates.from_components( + u=np.array([11, 22, 33]), + rp=np.array([4, 5, 6]), + rd=np.array([7, 8, 9]), + w=np.array([10, 11, 12]), + ) + Q = bionc_mx.NaturalCoordinates.from_qi((Q1, Q2)) + np.testing.assert_equal(TestUtils.mx_to_array(Q.u(0)), np.array([1, 2, 3])) + np.testing.assert_equal(TestUtils.mx_to_array(Q.u(1)), np.array([11, 22, 33])) + np.testing.assert_equal(TestUtils.mx_to_array(Q.v(0)), -np.array([7, 8, 9]) + np.array([4, 5, 6])) + np.testing.assert_equal(TestUtils.mx_to_array(Q.v(1)), -np.array([7, 8, 9]) + np.array([4, 5, 6])) + np.testing.assert_equal(TestUtils.mx_to_array(Q.vector(0)), TestUtils.mx_to_array(Q1)) + np.testing.assert_equal(TestUtils.mx_to_array(Q.vector(1)), TestUtils.mx_to_array(Q2)) + np.testing.assert_equal(TestUtils.mx_to_array(Q.vector(0).u), np.array([1, 2, 3])) + np.testing.assert_equal(TestUtils.mx_to_array(Q.vector(1).u), np.array([11, 22, 33])) + np.testing.assert_equal(TestUtils.mx_to_array(Q.vector(0).v), -np.array([7, 8, 9]) + np.array([4, 5, 6])) + np.testing.assert_equal(TestUtils.mx_to_array(Q.vector(1).v), -np.array([7, 8, 9]) + np.array([4, 5, 6])) + np.testing.assert_equal(Q.nb_qi(), 2) + # do the same tests for NaturalAccelerations and SegmentNaturalAccelerations -def test_NaturalAccelerationsConstructor(): +def test_NaturalAccelerations(): Qddot1 = bionc_np.SegmentNaturalAccelerations.from_components( uddot=np.array([1, 2, 3]), wddot=np.array([4, 5, 6]), @@ -129,3 +188,26 @@ def test_NaturalAccelerationsConstructor(): np.testing.assert_equal(Qddot.vector(0).uddot, np.array([1, 2, 3])) np.testing.assert_equal(Qddot.vector(1).uddot, np.array([11, 22, 33])) np.testing.assert_equal(Qddot.nb_qddoti(), 2) + + +def test_NaturalAccelerations_casadi(): + Qddot1 = bionc_mx.SegmentNaturalAccelerations.from_components( + uddot=np.array([1, 2, 3]), + wddot=np.array([4, 5, 6]), + rdddot=np.array([7, 8, 9]), + rpddot=np.array([10, 11, 12]), + ) + Qddot2 = bionc_mx.SegmentNaturalAccelerations.from_components( + uddot=np.array([11, 22, 33]), + wddot=np.array([4, 5, 6]), + rdddot=np.array([7, 82, 9]), + rpddot=np.array([110, 11, 12]), + ) + Qddot = bionc_mx.NaturalAccelerations.from_qddoti((Qddot1, Qddot2)) + np.testing.assert_equal(TestUtils.mx_to_array(Qddot.uddot(0)), np.array([1, 2, 3])) + np.testing.assert_equal(TestUtils.mx_to_array(Qddot.uddot(1)), np.array([11, 22, 33])) + np.testing.assert_equal(TestUtils.mx_to_array(Qddot.vector(0)), TestUtils.mx_to_array(Qddot1)) + np.testing.assert_equal(TestUtils.mx_to_array(Qddot.vector(1)), TestUtils.mx_to_array(Qddot2)) + np.testing.assert_equal(TestUtils.mx_to_array(Qddot.vector(0).uddot), np.array([1, 2, 3])) + np.testing.assert_equal(TestUtils.mx_to_array(Qddot.vector(1).uddot), np.array([11, 22, 33])) + np.testing.assert_equal(Qddot.nb_qddoti(), 2) \ No newline at end of file From 77dde6026ef4b43b3ec3f344ee5fa893d92bbfa3 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Wed, 9 Nov 2022 23:36:43 -0500 Subject: [PATCH 15/30] homogenoustransform casadi --- .../using_casadi/homogenous_transform.py | 34 ++++--- tests/test_homegenous_transform.py | 90 +++++++++++++++++-- 2 files changed, 106 insertions(+), 18 deletions(-) diff --git a/bionc/math_interface/using_casadi/homogenous_transform.py b/bionc/math_interface/using_casadi/homogenous_transform.py index 7e3a61da..3d32371a 100644 --- a/bionc/math_interface/using_casadi/homogenous_transform.py +++ b/bionc/math_interface/using_casadi/homogenous_transform.py @@ -13,9 +13,6 @@ def __new__(cls, input_array: MX): Create a new instance of the class. """ - if not isinstance(input_array, MX): - raise TypeError("input_array must be a MX") - if input_array.shape != (4, 4): raise ValueError("input_array must be a 4x4 array") @@ -39,14 +36,24 @@ def from_components(cls, x: MX, y: MX, z: MX, t: MX): t: MX translation vector, a 3x1 array """ + + if x is None: + raise ValueError("u must be a numpy array (3x1) or a list of 3 elements") + if y is None: + raise ValueError("rp must be a numpy array (3x1) or a list of 3 elements") + if z is None: + raise ValueError("rd must be a numpy array (3x1) or a list of 3 elements") + if t is None: + raise ValueError("w must be a numpy array (3x1) or a list of 3 elements") + if not isinstance(x, MX): - raise TypeError("x must be a array") + x = MX(x) if not isinstance(y, MX): - raise TypeError("y must be a array") + y = MX(y) if not isinstance(z, MX): - raise TypeError("z must be a array") + z = MX(z) if not isinstance(t, MX): - raise TypeError("t must be a array") + t = MX(t) if x.shape != (3, 1): raise ValueError("x must be a 3x1 array") @@ -73,10 +80,15 @@ def from_rt(cls, rotation: MX, translation: MX): translation: MX A 3x1 translation vector """ + if rotation is None: + raise ValueError("rotation must be a 3x3 array") + if translation is None: + raise ValueError("translation must be a 3x1 array") + if not isinstance(rotation, MX): - raise TypeError("r must be a array") + MX(rotation) if not isinstance(translation, MX): - raise TypeError("t must be a array") + MX(translation) if rotation.shape != (3, 3): raise ValueError("r must be a 3x3 array") @@ -97,11 +109,11 @@ def eye(cls): @property def rot(self): - return self[:3, :3].to_array() + return self[:3, :3] @property def translation(self): - return self[3, 0:3].to_array() + return self[3, 0:3] def inv(self): """ diff --git a/tests/test_homegenous_transform.py b/tests/test_homegenous_transform.py index cdb77aa3..0e6612b2 100644 --- a/tests/test_homegenous_transform.py +++ b/tests/test_homegenous_transform.py @@ -1,6 +1,8 @@ import numpy as np from bionc import bionc_numpy as bionc_np +from bionc import bionc_casadi as bionc_mx +from .utils import TestUtils # generate tests for the class to be use with pytest @@ -89,11 +91,85 @@ def test_homogenous_transform(): assert np.allclose(ht3[2, 2], 1) assert np.allclose(ht3[3, 3], 1) - # test __rmul__ - ht1 = bionc_np.HomogeneousTransform.eye() - ht2 = bionc_np.HomogeneousTransform.eye() + +def test_homogenous_transform_casadi(): + # test constructor + ht = bionc_mx.HomogeneousTransform(np.eye(4)) + assert np.allclose(TestUtils.mx_to_array(ht.rot), np.eye(3)) + assert np.allclose(TestUtils.mx_to_array(ht.translation), np.zeros(3)) + + # test from_components + x = np.array([[1], [0], [0]]) + y = np.array([[0], [1], [0]]) + z = np.array([[0], [0], [1]]) + t = np.array([[0], [0], [0]]) + ht = bionc_mx.HomogeneousTransform.from_components(x, y, z, t) + assert np.allclose(TestUtils.mx_to_array(ht.rot), np.eye(3)) + assert np.allclose(TestUtils.mx_to_array(ht.translation), np.zeros(3)) + + # test from_rt + r = np.eye(3) + t = np.zeros((3, 1)) + ht = bionc_mx.HomogeneousTransform.from_rt(r, t) + assert np.allclose(TestUtils.mx_to_array(ht.rot), np.eye(3)) + assert np.allclose(TestUtils.mx_to_array(ht.translation), np.zeros(3)) + + # test eye + ht = bionc_mx.HomogeneousTransform.eye() + assert np.allclose(TestUtils.mx_to_array(ht.rot), np.eye(3)) + assert np.allclose(TestUtils.mx_to_array(ht.translation), np.zeros(3)) + + # test inv + x = np.array([[1], [0], [0]]) + y = np.array([[0], [np.sqrt(2) / 2], [-np.sqrt(2) / 2]]) + z = np.array([[0], [np.sqrt(2) / 2], [np.sqrt(2) / 2]]) + t = np.array([[2], [3], [1]]) + ht = bionc_mx.HomogeneousTransform.from_components(x, y, z, t) + ht_inv = ht.inv() + ht_inv_array = np.linalg.inv(TestUtils.mx_to_array(ht)) + assert np.allclose(TestUtils.mx_to_array(ht_inv), ht_inv_array) + + # test __getitem__ + ht = bionc_mx.HomogeneousTransform.eye() + assert np.allclose(TestUtils.mx_to_array(ht[0, 0]), 1) + assert np.allclose(TestUtils.mx_to_array(ht[1, 1]), 1) + assert np.allclose(TestUtils.mx_to_array(ht[2, 2]), 1) + assert np.allclose(TestUtils.mx_to_array(ht[3, 3]), 1) + + # test __setitem__ + ht = bionc_mx.HomogeneousTransform.eye() + ht[0, 0] = 2 + ht[1, 1] = 2 + ht[2, 2] = 2 + ht[3, 3] = 2 + assert np.allclose(TestUtils.mx_to_array(ht[0, 0]), 2) + assert np.allclose(TestUtils.mx_to_array(ht[1, 1]), 2) + assert np.allclose(TestUtils.mx_to_array(ht[2, 2]), 2) + assert np.allclose(TestUtils.mx_to_array(ht[3, 3]), 2) + + # test __add__ + ht1 = bionc_mx.HomogeneousTransform.eye() + ht2 = bionc_mx.HomogeneousTransform.eye() + ht3 = ht1 + ht2 + assert np.allclose(TestUtils.mx_to_array(ht3[0, 0]), 2) + assert np.allclose(TestUtils.mx_to_array(ht3[1, 1]), 2) + assert np.allclose(TestUtils.mx_to_array(ht3[2, 2]), 2) + assert np.allclose(TestUtils.mx_to_array(ht3[3, 3]), 2) + + # test __sub__ + ht1 = bionc_mx.HomogeneousTransform.eye() + ht2 = bionc_mx.HomogeneousTransform.eye() + ht3 = ht1 - ht2 + assert np.allclose(TestUtils.mx_to_array(ht3[0, 0]), 0) + assert np.allclose(TestUtils.mx_to_array(ht3[1, 1]), 0) + assert np.allclose(TestUtils.mx_to_array(ht3[2, 2]), 0) + assert np.allclose(TestUtils.mx_to_array(ht3[3, 3]), 0) + + # test __mul__ + ht1 = bionc_mx.HomogeneousTransform.eye() + ht2 = bionc_mx.HomogeneousTransform.eye() ht3 = ht1 * ht2 - assert np.allclose(ht3[0, 0], 1) - assert np.allclose(ht3[1, 1], 1) - assert np.allclose(ht3[2, 2], 1) - assert np.allclose(ht3[3, 3], 1) + assert np.allclose(TestUtils.mx_to_array(ht3[0, 0]), 1) + assert np.allclose(TestUtils.mx_to_array(ht3[1, 1]), 1) + assert np.allclose(TestUtils.mx_to_array(ht3[2, 2]), 1) + assert np.allclose(TestUtils.mx_to_array(ht3[3, 3]), 1) From 3b09a164538af4bce1e5f9901635375d8808c253 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Wed, 9 Nov 2022 23:37:14 -0500 Subject: [PATCH 16/30] blacked --- tests/test_natural_coordinates.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/tests/test_natural_coordinates.py b/tests/test_natural_coordinates.py index b020518d..7b81e3a5 100644 --- a/tests/test_natural_coordinates.py +++ b/tests/test_natural_coordinates.py @@ -166,6 +166,7 @@ def test_NaturalCoordinates_casadi(): np.testing.assert_equal(TestUtils.mx_to_array(Q.vector(1).v), -np.array([7, 8, 9]) + np.array([4, 5, 6])) np.testing.assert_equal(Q.nb_qi(), 2) + # do the same tests for NaturalAccelerations and SegmentNaturalAccelerations def test_NaturalAccelerations(): Qddot1 = bionc_np.SegmentNaturalAccelerations.from_components( @@ -210,4 +211,4 @@ def test_NaturalAccelerations_casadi(): np.testing.assert_equal(TestUtils.mx_to_array(Qddot.vector(1)), TestUtils.mx_to_array(Qddot2)) np.testing.assert_equal(TestUtils.mx_to_array(Qddot.vector(0).uddot), np.array([1, 2, 3])) np.testing.assert_equal(TestUtils.mx_to_array(Qddot.vector(1).uddot), np.array([11, 22, 33])) - np.testing.assert_equal(Qddot.nb_qddoti(), 2) \ No newline at end of file + np.testing.assert_equal(Qddot.nb_qddoti(), 2) From d77717c10f20270f0f15c0534d7aeee18b5b263f Mon Sep 17 00:00:00 2001 From: Ipuch Date: Wed, 16 Nov 2022 08:43:07 -0500 Subject: [PATCH 17/30] strong refactor as bionc_numpy and bionc_casadi --- bionc/__init__.py | 16 ++++++++-------- .../using_casadi => bionc_casadi}/__init__.py | 0 .../homogenous_transform.py | 0 .../natural_accelerations.py | 0 .../natural_coordinates.py | 0 .../natural_velocities.py | 0 .../using_numpy => bionc_numpy}/__init__.py | 0 .../homogenous_transform.py | 0 .../natural_accelerations.py | 0 .../natural_coordinates.py | 0 .../natural_velocities.py | 0 bionc/{math_interface => }/math_interface.py | 0 bionc/math_interface/protocols/__init__.py | 0 bionc/model_computations/biomechanical_model.py | 4 ++-- bionc/model_computations/joint.py | 2 +- bionc/model_computations/natural_marker.py | 2 +- bionc/model_computations/natural_segment.py | 10 +++++----- bionc/model_creation/marker_template.py | 2 +- bionc/model_creation/natural_segment_template.py | 4 ++-- bionc/{math_interface => protocols}/__init__.py | 0 .../protocols/homogenous_transform.py | 0 .../protocols/natural_accelerations.py | 0 .../protocols/natural_coordinates.py | 0 .../protocols/natural_velocities.py | 0 tests/test_biomech_model.py | 2 +- 25 files changed, 21 insertions(+), 21 deletions(-) rename bionc/{math_interface/using_casadi => bionc_casadi}/__init__.py (100%) rename bionc/{math_interface/using_casadi => bionc_casadi}/homogenous_transform.py (100%) rename bionc/{math_interface/using_casadi => bionc_casadi}/natural_accelerations.py (100%) rename bionc/{math_interface/using_casadi => bionc_casadi}/natural_coordinates.py (100%) rename bionc/{math_interface/using_casadi => bionc_casadi}/natural_velocities.py (100%) rename bionc/{math_interface/using_numpy => bionc_numpy}/__init__.py (100%) rename bionc/{math_interface/using_numpy => bionc_numpy}/homogenous_transform.py (100%) rename bionc/{math_interface/using_numpy => bionc_numpy}/natural_accelerations.py (100%) rename bionc/{math_interface/using_numpy => bionc_numpy}/natural_coordinates.py (100%) rename bionc/{math_interface/using_numpy => bionc_numpy}/natural_velocities.py (100%) rename bionc/{math_interface => }/math_interface.py (100%) delete mode 100644 bionc/math_interface/protocols/__init__.py rename bionc/{math_interface => protocols}/__init__.py (100%) rename bionc/{math_interface => }/protocols/homogenous_transform.py (100%) rename bionc/{math_interface => }/protocols/natural_accelerations.py (100%) rename bionc/{math_interface => }/protocols/natural_coordinates.py (100%) rename bionc/{math_interface => }/protocols/natural_velocities.py (100%) diff --git a/bionc/__init__.py b/bionc/__init__.py index f5f0a036..8d72d9f3 100644 --- a/bionc/__init__.py +++ b/bionc/__init__.py @@ -22,7 +22,7 @@ BiomechanicalModel, ) -from .math_interface.math_interface import ( +from .math_interface import ( zeros, eye, array, @@ -31,14 +31,14 @@ horzcat, ) -from .math_interface.protocols import natural_coordinates -from .math_interface import using_casadi as bionc_casadi -from .math_interface import using_numpy as bionc_numpy +from .protocols import natural_coordinates +from bionc import bionc_casadi +from bionc import bionc_numpy -from bionc.math_interface.protocols.natural_coordinates import SegmentNaturalCoordinates, NaturalCoordinates -from bionc.math_interface.protocols.natural_velocities import SegmentNaturalVelocities, NaturalVelocities -from bionc.math_interface.protocols.natural_accelerations import SegmentNaturalAccelerations, NaturalAccelerations -from bionc.math_interface.protocols.homogenous_transform import HomogeneousTransform +from .protocols.natural_coordinates import SegmentNaturalCoordinates, NaturalCoordinates +from .protocols.natural_velocities import SegmentNaturalVelocities, NaturalVelocities +from .protocols.natural_accelerations import SegmentNaturalAccelerations, NaturalAccelerations +from .protocols.homogenous_transform import HomogeneousTransform from casadi.casadi import MX as MX_type from numpy import ndarray diff --git a/bionc/math_interface/using_casadi/__init__.py b/bionc/bionc_casadi/__init__.py similarity index 100% rename from bionc/math_interface/using_casadi/__init__.py rename to bionc/bionc_casadi/__init__.py diff --git a/bionc/math_interface/using_casadi/homogenous_transform.py b/bionc/bionc_casadi/homogenous_transform.py similarity index 100% rename from bionc/math_interface/using_casadi/homogenous_transform.py rename to bionc/bionc_casadi/homogenous_transform.py diff --git a/bionc/math_interface/using_casadi/natural_accelerations.py b/bionc/bionc_casadi/natural_accelerations.py similarity index 100% rename from bionc/math_interface/using_casadi/natural_accelerations.py rename to bionc/bionc_casadi/natural_accelerations.py diff --git a/bionc/math_interface/using_casadi/natural_coordinates.py b/bionc/bionc_casadi/natural_coordinates.py similarity index 100% rename from bionc/math_interface/using_casadi/natural_coordinates.py rename to bionc/bionc_casadi/natural_coordinates.py diff --git a/bionc/math_interface/using_casadi/natural_velocities.py b/bionc/bionc_casadi/natural_velocities.py similarity index 100% rename from bionc/math_interface/using_casadi/natural_velocities.py rename to bionc/bionc_casadi/natural_velocities.py diff --git a/bionc/math_interface/using_numpy/__init__.py b/bionc/bionc_numpy/__init__.py similarity index 100% rename from bionc/math_interface/using_numpy/__init__.py rename to bionc/bionc_numpy/__init__.py diff --git a/bionc/math_interface/using_numpy/homogenous_transform.py b/bionc/bionc_numpy/homogenous_transform.py similarity index 100% rename from bionc/math_interface/using_numpy/homogenous_transform.py rename to bionc/bionc_numpy/homogenous_transform.py diff --git a/bionc/math_interface/using_numpy/natural_accelerations.py b/bionc/bionc_numpy/natural_accelerations.py similarity index 100% rename from bionc/math_interface/using_numpy/natural_accelerations.py rename to bionc/bionc_numpy/natural_accelerations.py diff --git a/bionc/math_interface/using_numpy/natural_coordinates.py b/bionc/bionc_numpy/natural_coordinates.py similarity index 100% rename from bionc/math_interface/using_numpy/natural_coordinates.py rename to bionc/bionc_numpy/natural_coordinates.py diff --git a/bionc/math_interface/using_numpy/natural_velocities.py b/bionc/bionc_numpy/natural_velocities.py similarity index 100% rename from bionc/math_interface/using_numpy/natural_velocities.py rename to bionc/bionc_numpy/natural_velocities.py diff --git a/bionc/math_interface/math_interface.py b/bionc/math_interface.py similarity index 100% rename from bionc/math_interface/math_interface.py rename to bionc/math_interface.py diff --git a/bionc/math_interface/protocols/__init__.py b/bionc/math_interface/protocols/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/bionc/model_computations/biomechanical_model.py b/bionc/model_computations/biomechanical_model.py index a1af1844..483e73c7 100644 --- a/bionc/model_computations/biomechanical_model.py +++ b/bionc/model_computations/biomechanical_model.py @@ -1,7 +1,7 @@ import numpy as np -from ..math_interface.protocols.natural_coordinates import SegmentNaturalCoordinates, NaturalCoordinates -from ..math_interface.using_casadi.natural_velocities import SegmentNaturalVelocities, NaturalVelocities +from ..protocols.natural_coordinates import SegmentNaturalCoordinates, NaturalCoordinates +from ..bionc_casadi.natural_velocities import SegmentNaturalVelocities, NaturalVelocities class BiomechanicalModel: diff --git a/bionc/model_computations/joint.py b/bionc/model_computations/joint.py index 3da2708d..6aad3160 100644 --- a/bionc/model_computations/joint.py +++ b/bionc/model_computations/joint.py @@ -3,7 +3,7 @@ import numpy as np from .natural_segment import NaturalSegment -from bionc.math_interface.protocols.natural_coordinates import SegmentNaturalCoordinates +from ..protocols.natural_coordinates import SegmentNaturalCoordinates class JointBase(ABC): diff --git a/bionc/model_computations/natural_marker.py b/bionc/model_computations/natural_marker.py index fa0d8ff5..1cf82e8a 100644 --- a/bionc/model_computations/natural_marker.py +++ b/bionc/model_computations/natural_marker.py @@ -7,7 +7,7 @@ from ..utils.interpolation_matrix import interpolate_natural_vector, to_natural_vector # from ..utils.natural_coordinates import SegmentNaturalCoordinates -from bionc.math_interface.protocols.natural_coordinates import SegmentNaturalCoordinates +from ..protocols.natural_coordinates import SegmentNaturalCoordinates # todo: need a list of markers MarkerList diff --git a/bionc/model_computations/natural_segment.py b/bionc/model_computations/natural_segment.py index 1995d5b2..8162bbe2 100644 --- a/bionc/model_computations/natural_segment.py +++ b/bionc/model_computations/natural_segment.py @@ -5,10 +5,10 @@ from numpy.linalg import inv # from ..utils.natural_coordinates import SegmentNaturalCoordinates -from ..math_interface.protocols.natural_coordinates import SegmentNaturalCoordinates, NaturalCoordinates -from ..math_interface.using_casadi.natural_velocities import SegmentNaturalVelocities, NaturalVelocities -from ..math_interface.using_casadi.natural_accelerations import SegmentNaturalAccelerations, NaturalAccelerations -from ..math_interface.using_casadi.homogenous_transform import HomogeneousTransform +from ..protocols.natural_coordinates import SegmentNaturalCoordinates, NaturalCoordinates +from ..bionc_casadi.natural_velocities import SegmentNaturalVelocities, NaturalVelocities +from ..bionc_casadi.natural_accelerations import SegmentNaturalAccelerations, NaturalAccelerations +from ..bionc_casadi.homogenous_transform import HomogeneousTransform from ..model_computations.natural_marker import SegmentMarker @@ -161,7 +161,7 @@ def parameters_from_Q(Q: SegmentNaturalCoordinates) -> tuple: tuple The parameters of the segment (alpha, beta, gamma, length) """ - from ..math_interface.using_numpy import SegmentNaturalCoordinates + from ..bionc_numpy import SegmentNaturalCoordinates Q = SegmentNaturalCoordinates(Q) diff --git a/bionc/model_creation/marker_template.py b/bionc/model_creation/marker_template.py index f8766937..b2d5e5e3 100644 --- a/bionc/model_creation/marker_template.py +++ b/bionc/model_creation/marker_template.py @@ -10,7 +10,7 @@ from ..model_computations.natural_segment import NaturalSegment # from ..utils.natural_coordinates import SegmentNaturalCoordinates -from bionc.math_interface.protocols.natural_coordinates import SegmentNaturalCoordinates +from ..protocols.natural_coordinates import SegmentNaturalCoordinates class MarkerTemplate: diff --git a/bionc/model_creation/natural_segment_template.py b/bionc/model_creation/natural_segment_template.py index 4b16e6a1..880c634f 100644 --- a/bionc/model_creation/natural_segment_template.py +++ b/bionc/model_creation/natural_segment_template.py @@ -4,7 +4,7 @@ from ..model_computations.biomechanical_model import BiomechanicalModel # from ..utils.natural_coordinates import SegmentNaturalCoordinates -from bionc.math_interface.protocols.natural_coordinates import SegmentNaturalCoordinates +from ..protocols.natural_coordinates import SegmentNaturalCoordinates from .marker_template import MarkerTemplate from .protocols import Data from ..model_computations.natural_segment import NaturalSegment @@ -60,7 +60,7 @@ def experimental_Q(self, data: Data, kinematic_chain: BiomechanicalModel) -> Seg SegmentNaturalCoordinates The Segment Natural Coordinates Q (12 x n_frames) """ - from ..math_interface.using_numpy import SegmentNaturalCoordinates + from ..bionc_numpy import SegmentNaturalCoordinates self.Q = SegmentNaturalCoordinates.from_components( u=self.u_axis.to_axis(data, kinematic_chain).axis()[:3, :], diff --git a/bionc/math_interface/__init__.py b/bionc/protocols/__init__.py similarity index 100% rename from bionc/math_interface/__init__.py rename to bionc/protocols/__init__.py diff --git a/bionc/math_interface/protocols/homogenous_transform.py b/bionc/protocols/homogenous_transform.py similarity index 100% rename from bionc/math_interface/protocols/homogenous_transform.py rename to bionc/protocols/homogenous_transform.py diff --git a/bionc/math_interface/protocols/natural_accelerations.py b/bionc/protocols/natural_accelerations.py similarity index 100% rename from bionc/math_interface/protocols/natural_accelerations.py rename to bionc/protocols/natural_accelerations.py diff --git a/bionc/math_interface/protocols/natural_coordinates.py b/bionc/protocols/natural_coordinates.py similarity index 100% rename from bionc/math_interface/protocols/natural_coordinates.py rename to bionc/protocols/natural_coordinates.py diff --git a/bionc/math_interface/protocols/natural_velocities.py b/bionc/protocols/natural_velocities.py similarity index 100% rename from bionc/math_interface/protocols/natural_velocities.py rename to bionc/protocols/natural_velocities.py diff --git a/tests/test_biomech_model.py b/tests/test_biomech_model.py index a983cfd5..c799db72 100644 --- a/tests/test_biomech_model.py +++ b/tests/test_biomech_model.py @@ -3,7 +3,7 @@ from .utils import TestUtils -from bionc import SegmentNaturalVelocities, NaturalVelocities +from bionc.bionc_numpy import SegmentNaturalVelocities, NaturalVelocities from bionc import bionc_numpy as bionc_np From 3b580d2a5be1c55d68a1c902d2b66651274ab6b7 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Wed, 16 Nov 2022 09:39:41 -0500 Subject: [PATCH 18/30] huge refactor move all in respective folders --- bionc/__init__.py | 2 +- bionc/bionc_casadi/__init__.py | 11 + .../biomechanical_model.py | 4 +- .../inertia_parameters.py | 2 +- .../joint.py | 2 +- .../natural_axis.py | 0 .../natural_marker.py | 6 +- bionc/bionc_casadi/natural_segment.py | 674 ++++++++++++++++++ .../segment.py | 6 +- bionc/bionc_numpy/__init__.py | 11 + bionc/bionc_numpy/biomechanical_model.py | 160 +++++ bionc/bionc_numpy/inertia_parameters.py | 87 +++ bionc/bionc_numpy/joint.py | 174 +++++ bionc/bionc_numpy/natural_axis.py | 25 + bionc/bionc_numpy/natural_marker.py | 337 +++++++++ .../natural_segment.py | 8 +- bionc/bionc_numpy/segment.py | 46 ++ bionc/model_computations/__init__.py | 9 - .../biomechanical_model_template.py | 6 +- .../inertia_parameters_template.py | 6 +- bionc/model_creation/marker_template.py | 6 +- bionc/model_creation/natural_axis_template.py | 6 +- .../natural_segment_template.py | 4 +- 23 files changed, 1554 insertions(+), 38 deletions(-) rename bionc/{model_computations => bionc_casadi}/biomechanical_model.py (96%) rename bionc/{model_computations => bionc_casadi}/inertia_parameters.py (98%) rename bionc/{model_computations => bionc_casadi}/joint.py (98%) rename bionc/{model_computations => bionc_casadi}/natural_axis.py (100%) rename bionc/{model_computations => bionc_casadi}/natural_marker.py (98%) create mode 100644 bionc/bionc_casadi/natural_segment.py rename bionc/{model_computations => bionc_casadi}/segment.py (88%) create mode 100644 bionc/bionc_numpy/biomechanical_model.py create mode 100644 bionc/bionc_numpy/inertia_parameters.py create mode 100644 bionc/bionc_numpy/joint.py create mode 100644 bionc/bionc_numpy/natural_axis.py create mode 100644 bionc/bionc_numpy/natural_marker.py rename bionc/{model_computations => bionc_numpy}/natural_segment.py (98%) create mode 100644 bionc/bionc_numpy/segment.py delete mode 100644 bionc/model_computations/__init__.py diff --git a/bionc/__init__.py b/bionc/__init__.py index 8d72d9f3..e4182d33 100644 --- a/bionc/__init__.py +++ b/bionc/__init__.py @@ -12,7 +12,7 @@ Data, GenericDynamicModel, ) -from .model_computations import ( +from .bionc_numpy import ( Axis, SegmentMarker, Marker, diff --git a/bionc/bionc_casadi/__init__.py b/bionc/bionc_casadi/__init__.py index e40eaabe..389c06fb 100644 --- a/bionc/bionc_casadi/__init__.py +++ b/bionc/bionc_casadi/__init__.py @@ -2,3 +2,14 @@ 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 + +# Some classes to define the BiomechanicalModel +from .natural_axis import Axis +from .natural_marker import SegmentMarker, Marker +from .segment import Segment +from .natural_segment import NaturalSegment +from .inertia_parameters import InertiaParameters \ No newline at end of file diff --git a/bionc/model_computations/biomechanical_model.py b/bionc/bionc_casadi/biomechanical_model.py similarity index 96% rename from bionc/model_computations/biomechanical_model.py rename to bionc/bionc_casadi/biomechanical_model.py index 483e73c7..cec23fa0 100644 --- a/bionc/model_computations/biomechanical_model.py +++ b/bionc/bionc_casadi/biomechanical_model.py @@ -1,7 +1,7 @@ import numpy as np -from ..protocols.natural_coordinates import SegmentNaturalCoordinates, NaturalCoordinates -from ..bionc_casadi.natural_velocities import SegmentNaturalVelocities, NaturalVelocities +from bionc.protocols.natural_coordinates import SegmentNaturalCoordinates, NaturalCoordinates +from bionc.bionc_numpy.natural_velocities import SegmentNaturalVelocities, NaturalVelocities class BiomechanicalModel: diff --git a/bionc/model_computations/inertia_parameters.py b/bionc/bionc_casadi/inertia_parameters.py similarity index 98% rename from bionc/model_computations/inertia_parameters.py rename to bionc/bionc_casadi/inertia_parameters.py index cb571509..f05ea35d 100644 --- a/bionc/model_computations/inertia_parameters.py +++ b/bionc/bionc_casadi/inertia_parameters.py @@ -3,7 +3,7 @@ import numpy as np from .biomechanical_model import BiomechanicalModel -from ..model_creation.protocols import Data +from bionc.model_creation.protocols import Data class InertiaParameters: diff --git a/bionc/model_computations/joint.py b/bionc/bionc_casadi/joint.py similarity index 98% rename from bionc/model_computations/joint.py rename to bionc/bionc_casadi/joint.py index 6aad3160..372996c5 100644 --- a/bionc/model_computations/joint.py +++ b/bionc/bionc_casadi/joint.py @@ -3,7 +3,7 @@ import numpy as np from .natural_segment import NaturalSegment -from ..protocols.natural_coordinates import SegmentNaturalCoordinates +from bionc.protocols.natural_coordinates import SegmentNaturalCoordinates class JointBase(ABC): diff --git a/bionc/model_computations/natural_axis.py b/bionc/bionc_casadi/natural_axis.py similarity index 100% rename from bionc/model_computations/natural_axis.py rename to bionc/bionc_casadi/natural_axis.py diff --git a/bionc/model_computations/natural_marker.py b/bionc/bionc_casadi/natural_marker.py similarity index 98% rename from bionc/model_computations/natural_marker.py rename to bionc/bionc_casadi/natural_marker.py index 1cf82e8a..e61071d3 100644 --- a/bionc/model_computations/natural_marker.py +++ b/bionc/bionc_casadi/natural_marker.py @@ -3,11 +3,11 @@ import numpy as np from .biomechanical_model import BiomechanicalModel -from ..model_creation.protocols import Data -from ..utils.interpolation_matrix import interpolate_natural_vector, to_natural_vector +from bionc.model_creation.protocols import Data +from bionc.utils.interpolation_matrix import interpolate_natural_vector, to_natural_vector # from ..utils.natural_coordinates import SegmentNaturalCoordinates -from ..protocols.natural_coordinates import SegmentNaturalCoordinates +from bionc.protocols.natural_coordinates import SegmentNaturalCoordinates # todo: need a list of markers MarkerList diff --git a/bionc/bionc_casadi/natural_segment.py b/bionc/bionc_casadi/natural_segment.py new file mode 100644 index 00000000..2417052f --- /dev/null +++ b/bionc/bionc_casadi/natural_segment.py @@ -0,0 +1,674 @@ +from typing import Union, Tuple + +import numpy as np +from casadi import MX +from casadi import cos, sin, transpose, norm_2, vertcat, horzcat, mtimes, dot, cross, vertsplit, horzsplit, reshape +from numpy.linalg import inv + +from ..protocols.natural_coordinates import SegmentNaturalCoordinates, NaturalCoordinates +from ..bionc_casadi.natural_velocities import SegmentNaturalVelocities, NaturalVelocities +from ..bionc_casadi.natural_accelerations import SegmentNaturalAccelerations, NaturalAccelerations +from ..bionc_casadi.homogenous_transform import HomogeneousTransform +from ..bionc_numpy.natural_marker import SegmentMarker + + +class NaturalSegment: + """ + Class used to define anatomical segment based on natural coordinate. + + Methods + ------- + transformation_matrix() + This function returns the transformation matrix, denoted Bi + rigid_body_constraint() + This function returns the rigid body constraints of the segment, denoted phi_r + rigid_body_constraint_jacobian() + This function returns the jacobian of rigid body constraints of the segment, denoted K_r + + add_marker() + This function adds a marker to the segment + nb_markers() + This function returns the number of markers in the segment + marker_constraints() + This function returns the defects of the marker constraints of the segment, denoted Phi_m + marker_jacobian() + This function returns the jacobian of the marker constraints of the segment, denoted K_m + + Attributes + ---------- + _name : str + name of the segment + _length : float + length of the segment + _alpha : float + angle between u and w + _beta : float + angle between w and (rp-rd) + _gamma : float + angle between (rp-rd) and u + _mass : float + mass of the segment in Segment Coordinate System + _center_of_mass : np.ndarray + center of mass of the segment in Segment Coordinate System + _inertia: np.ndarray + inertia matrix of the segment in Segment Coordinate System + """ + + def __init__( + self, + name: str = None, + alpha: Union[MX, float, np.float64] = np.pi / 2, + beta: Union[MX, float, np.float64] = np.pi / 2, + gamma: Union[MX, float, np.float64] = np.pi / 2, + length: Union[MX, float, np.float64] = None, + mass: Union[MX, float, np.float64] = None, + center_of_mass: Union[MX, np.ndarray] = None, + inertia: Union[MX, np.ndarray] = None, + ): + + self._name = name + + self._length = MX(length) + self._alpha = MX(alpha) + self._beta = MX(beta) + self._gamma = MX(gamma) + + # todo: sanity check to make sure u, v or w are not collinear + # todo: implement all the transformations matrix according the Ph.D thesis of Alexandre Naaim + self._transformation_matrix = self._transformation_matrix() + + self._mass = mass + if center_of_mass is None: + self._center_of_mass = MX(center_of_mass) + self._center_of_mass_in_natural_coordinates_system = None + self._interpolation_matrix_center_of_mass = None + else: + if center_of_mass.shape[0] != 3: + raise ValueError("Center of mass must be 3x1") + self._center_of_mass = MX(center_of_mass) + self._center_of_mass_in_natural_coordinates_system = self._center_of_mass_in_natural_coordinates_system() + self._interpolation_matrix_center_of_mass = self._interpolation_matrix_center_of_mass() + + if inertia is None: + self._inertia = MX(inertia) + self._inertia_in_natural_coordinates_system = None + self._interpolation_matrix_inertia = None + self._mass_matrix = None + else: + if inertia.shape != (3, 3): + raise ValueError("Inertia matrix must be 3x3") + self._inertia = MX(inertia) + self._pseudo_inertia_matrix = self._pseudo_inertia_matrix() + self._mass_matrix = self._update_mass_matrix() + + # list of markers embedded in the segment + self._markers = [] + + def set_name(self, name: str): + """ + This function sets the name of the segment + + Parameters + ---------- + name : str + Name of the segment + """ + self._name = name + + @classmethod + def from_experimental_Q( + cls, + Qi: SegmentNaturalCoordinates, + ) -> "NaturalSegment": + """ + Parameters + ---------- + Qi : SegmentNaturalCoordinates + Experimental segment natural coordinates (12 x n_frames) + + Returns + ------- + NaturalSegment + """ + + alpha = np.zeros(Qi.shape[1]) + beta = np.zeros(Qi.shape[1]) + gamma = np.zeros(Qi.shape[1]) + length = np.zeros(Qi.shape[1]) + + for i, Qif in enumerate(Qi.vector.T): + alpha[i], beta[i], gamma[i], length[i] = cls.parameters_from_Q(Qif) + + return cls( + alpha=np.mean(alpha, axis=0), + beta=np.mean(beta, axis=0), + gamma=np.mean(gamma, axis=0), + length=np.mean(length, axis=0), + ) + + @staticmethod + def parameters_from_Q(Q: SegmentNaturalCoordinates) -> tuple: + """ + This function computes the parameters of the segment from the natural coordinates + + Parameters + ---------- + Q: SegmentNaturalCoordinates + The natural coordinates of the segment + + Returns + ------- + tuple + The parameters of the segment (alpha, beta, gamma, length) + """ + from ..bionc_numpy import SegmentNaturalCoordinates + + Q = SegmentNaturalCoordinates(Q) + + u, rp, rd, w = Q.to_components() + + length = np.linalg.norm(rp - rd) + alpha = np.arccos(np.sum((rp - rd) * w, axis=0) / length) + beta = np.arccos(np.sum(u * w, axis=0)) + gamma = np.arccos(np.sum(u * (rp - rd), axis=0) / length) + + return alpha, beta, gamma, length + + # def __str__(self): + # print("to do") + + @property + def name(self): + return self._name + + @property + def length(self): + return self._length + + @property + def alpha(self): + return self._alpha + + @property + def beta(self): + return self._beta + + @property + def gamma(self): + return self._gamma + + @property + def mass(self): + return self._mass + + @property + def center_of_mass(self): + return self._center_of_mass + + @property + def inertia(self): + return self._inertia + + def _transformation_matrix(self) -> MX: + """ + This function computes the transformation matrix, denoted Bi, + from Natural Coordinate System to point to the orthogonal Segment Coordinate System. + Example : if vector a expressed in (Pi, X, Y, Z), inv(B) * a is expressed in (Pi, ui, vi, wi) + + Returns + ------- + MX + Transformation matrix from natural coordinate to segment coordinate system [3x3] + """ + return MX(np.array( + [ + [1, 0, 0], + [self.length * cos(self.gamma), self.length * sin(self.gamma), 0], + [ + cos(self.beta), + (cos(self.alpha) - cos(self.beta) * cos(self.gamma)) / sin(self.beta), + np.sqrt( + 1 + - cos(self.beta) ** 2 + - (cos(self.alpha) - cos(self.beta) * cos(self.gamma)) / sin(self.beta) ** 2 + ), + ], + ] + )) + + @property + def transformation_matrix(self) -> MX: + """ + This function returns the transformation matrix, denoted Bi, + from Natural Coordinate System to point to the orthogonal Segment Coordinate System. + Example : if vector a expressed in (Pi, X, Y, Z), inv(B) * a is expressed in (Pi, ui, vi, wi) + + Returns + ------- + MX + Transformation matrix from natural coordinate to segment coordinate system [3x3] + """ + return self._transformation_matrix + + def segment_coordinates_system(self, Q: SegmentNaturalCoordinates) -> HomogeneousTransform: + """ + This function computes the segment coordinates from the natural coordinates + + Parameters + ---------- + Q: SegmentNaturalCoordinates + The natural coordinates of the segment + + Returns + ------- + SegmentCoordinates + The segment coordinates + """ + if not isinstance(Q, SegmentNaturalCoordinates): + Q = SegmentNaturalCoordinates(Q) + + return HomogeneousTransform.from_rt( + rotation=self._transformation_matrix @ np.concatenate((Q.u, Q.v, Q.w), axis=1), + translation=Q.rp, + ) + + def location_from_homogenous_transform( + self, T: Union[np.ndarray, HomogeneousTransform] + ) -> SegmentNaturalCoordinates: + """ + This function returns the location of the segment in natural coordinate from its homogenous transform + + Parameters + ---------- + T: np.ndarray or HomogeneousTransform + Homogenous transform of the segment Ti which transforms from the local frame (Oi, Xi, Yi, Zi) + to the global frame (Xi, Yi, Zi) + + Returns + ------- + MX + Location of the segment [3 x 1] + """ + + u = self.transformation_matrix @ T[0:3, 0] + w = self.transformation_matrix @ T[0:3, 2] + rp = self.transformation_matrix @ T[0:3, 4] + rd = (T @ MX([0, self.length, 0, 1]))[0:3] # not sure of this line. + + return SegmentNaturalCoordinates((u, rp, rd, w)) + + def rigid_body_constraint(self, Qi: Union[SegmentNaturalCoordinates, np.ndarray]) -> MX: + """ + This function returns the rigid body constraints of the segment, denoted phi_r. + + Returns + ------- + MX + Rigid body constraints of the segment [6 x 1 x N_frame] + """ + + phir = MX.zeros(6) + u, v, w = Qi.to_uvw() + + phir[0] = u**2 - 1 + phir[1] = u * v - self.length * cos(self.gamma) + phir[2] = u * Qi.w - cos(self.beta) + phir[3] = v**2 - self.length**2 + phir[4] = v * w - self.length * cos(self.alpha) + phir[5] = w**2 - 1 + + return phir + + @staticmethod + def rigid_body_constraint_jacobian(Qi: SegmentNaturalCoordinates) -> MX: + """ + This function returns the Jacobian matrix of the rigid body constraints denoted K_r + + Returns + ------- + Kr : np.ndarray + Jacobian matrix of the rigid body constraints denoted Kr [6 x 12 x N_frame] + """ + # initialisation + Kr = MX.zeros((6, 12)) + + u, v, w = Qi.to_uvw() + + Kr[0, 0:3] = 2 * u + + Kr[1, 0:3] = v + Kr[1, 3:6] = u + Kr[1, 6:9] = -u + + Kr[2, 0:3] = w + Kr[2, 9:12] = u + + Kr[3, 3:6] = 2 * v + Kr[3, 6:9] = -2 * v + + Kr[4, 3:6] = w + Kr[4, 6:9] = -w + Kr[4, 9:12] = v + + Kr[5, 9:12] = 2 * w + + return Kr + + @staticmethod + def rigid_body_constraint_jacobian_derivative(Qdoti: SegmentNaturalVelocities) -> MX: + """ + This function returns the derivative of the Jacobian matrix of the rigid body constraints denoted Kr_dot [6 x 12 x N_frame] + + Returns + ------- + Kr_dot : np.ndarray + derivative of the Jacobian matrix of the rigid body constraints denoted Kr_dot [6 x 12 ] + """ + # initialisation + Kr_dot = MX.zeros((6, 12)) + + Kr_dot[0, 0:3] = 2 * Qdoti.udot + + Kr_dot[1, 0:3] = Qdoti.vdot + Kr_dot[1, 3:6] = Qdoti.udot + Kr_dot[1, 6:9] = -Qdoti.udot + + Kr_dot[2, 0:3] = Qdoti.wdot + Kr_dot[2, 9:12] = Qdoti.udot + + Kr_dot[3, 3:6] = 2 * Qdoti.vdot + Kr_dot[3, 6:9] = -2 * Qdoti.vdot + + Kr_dot[4, 3:6] = Qdoti.wdot + Kr_dot[4, 6:9] = -Qdoti.wdot + Kr_dot[4, 9:12] = Qdoti.vdot + + Kr_dot[5, 9:12] = 2 * Qdoti.wdot + + return Kr_dot + + def _pseudo_inertia_matrix(self) -> MX: + """ + This function returns the pseudo-inertia matrix of the segment, denoted J_i. + It transforms the inertia matrix of the segment in the segment coordinate system to the natural coordinate system. + + Returns + ------- + MX + Pseudo-inertia matrix of the segment in the natural coordinate system [3x3] + """ + # todo: verify the formula + middle_block = ( + self.inertia + + self.mass * np.dot(self.center_of_mass.T, self.center_of_mass) * MX.eye(3) + - np.dot(self.center_of_mass.T, self.center_of_mass) + ) + + Binv = inv(self.transformation_matrix) + Binv_transpose = transpose(Binv) + + return Binv @ middle_block @ Binv_transpose + + @property + def pseudo_inertia_matrix(self) -> MX: + """ + This function returns the pseudo-inertia matrix of the segment, denoted J_i. + It transforms the inertia matrix of the segment in the segment coordinate system to the natural coordinate system. + + Returns + ------- + MX + Pseudo-inertia matrix of the segment in the natural coordinate system [3x3] + """ + return self._pseudo_inertia_matrix + + def _center_of_mass_in_natural_coordinates_system(self) -> MX: + """ + This function computes the center of mass of the segment in the natural coordinate system. + It transforms the center of mass of the segment in the segment coordinate system to the natural coordinate system. + + Returns + ------- + MX + Center of mass of the segment in the natural coordinate system [3x1] + """ + return inv(self.transformation_matrix) @ self.center_of_mass + + @property + def center_of_mass_in_natural_coordinates_system(self) -> MX: + """ + This function returns the center of mass of the segment in the natural coordinate system. + It transforms the center of mass of the segment in the segment coordinate system to the natural coordinate system. + + Returns + ------- + MX + Center of mass of the segment in the natural coordinate system [3x1] + """ + return self._center_of_mass_in_natural_coordinates_system + + def _update_mass_matrix(self) -> MX: + """ + This function returns the generalized mass matrix of the segment, denoted G_i. + + Returns + ------- + MX + mass matrix of the segment [12 x 12] + """ + + Ji = self.pseudo_inertia_matrix + n_ci = self.center_of_mass_in_natural_coordinates_system + + Gi = MX.zeros((12, 12)) + + Gi[0:3, 0:3] = Ji[0, 0] * MX.eye(3) + Gi[0:3, 3:6] = (self.mass * n_ci[0] + Ji[0, 1]) * MX.eye(3) + Gi[0:3, 6:9] = -Ji[0, 1] * MX.eye(3) + Gi[0:3, 9:12] = -Ji[0, 2] * MX.eye(3) + Gi[3:6, 3:6] = (self.mass + 2 * self.mass * n_ci[1] + Ji[1, 1]) * MX.eye(3) + Gi[3:6, 6:9] = -(self.mass * n_ci[1] + Ji[1, 1]) * MX.eye(3) + Gi[3:6, 9:12] = (self.mass * n_ci[2] + Ji[1, 2]) * MX.eye(3) + Gi[6:9, 6:9] = Ji[1, 1] * MX.eye(3) + Gi[6:9, 9:12] = -Ji[1, 2] * MX.eye(3) + Gi[9:12, 9:12] = Ji[2, 2] * MX.eye(3) + + # symmetrize the matrix + Gi = np.tril(Gi) + np.tril(Gi, -1).T + + return Gi + + @property + def mass_matrix(self) -> MX: + """ + This function returns the generalized mass matrix of the segment, denoted G_i. + + Returns + ------- + MX + mass matrix of the segment [12 x 12] + """ + + return self._mass_matrix + + @staticmethod + def interpolate(vector: np.ndarray) -> MX: + """ + This function interpolates the vector to get the interpolation matrix, denoted Ni + such as: + Ni * Qi = location in the global frame + + Parameters + ---------- + vector : np.ndarray + Vector in the natural coordinate system to interpolate (Pi, ui, vi, wi) + + Returns + ------- + interpolate_natural_vector: np.ndarray + Interpolation matrix [3 x 12], denoted Ni to get the location of the vector as linear combination of Q. + vector in global frame = Ni * Qi + """ + + interpolation_matrix = MX.zeros((3, 12)) + interpolation_matrix[0:3, 0:3] = vector[0] * MX.eye(3) + interpolation_matrix[0:3, 3:6] = (1 + vector[1]) * MX.eye(3) + interpolation_matrix[0:3, 6:9] = -vector[1] * MX.eye(3) + interpolation_matrix[0:3, 9:12] = vector[2] * MX.eye(3) + + return interpolation_matrix + + def _interpolation_matrix_center_of_mass(self) -> MX: + """ + This function returns the interpolation matrix for the center of mass of the segment, denoted N_i^Ci. + It allows to apply the gravity force at the center of mass of the segment. + + Returns + ------- + MX + Interpolation matrix for the center of mass of the segment in the natural coordinate system [12 x 3] + """ + n_ci = self.center_of_mass_in_natural_coordinates_system + return self.interpolate(n_ci) + + @property + def interpolation_matrix_center_of_mass(self) -> MX: + """ + This function returns the interpolation matrix for the center of mass of the segment, denoted N_i^Ci. + It allows to apply the gravity force at the center of mass of the segment. + + Returns + ------- + MX + Interpolation matrix for the center of mass of the segment in the natural coordinate system [12 x 3] + """ + return self._interpolation_matrix_center_of_mass + + def weight(self) -> MX: + """ + This function returns the weight applied on the segment through gravity force. + + Returns + ------- + MX + Weight applied on the segment through gravity force [12 x 1] + """ + + return (self.interpolation_matrix_center_of_mass.T * self.mass) @ MX([0, 0, -9.81]) + + def differential_algebraic_equation( + self, + Qi: Union[SegmentNaturalCoordinates, np.ndarray], + Qdoti: Union[SegmentNaturalVelocities, np.ndarray], + ) -> Tuple[SegmentNaturalAccelerations, np.ndarray]: + """ + This function returns the differential algebraic equation of the segment + + Parameters + ---------- + Qi: SegmentNaturalCoordinates + Natural coordinates of the segment + Qdoti: SegmentNaturalCoordinates + Derivative of the natural coordinates of the segment + + Returns + ------- + MX + Differential algebraic equation of the segment [12 x 1] + """ + if isinstance(Qi, SegmentNaturalVelocities): + raise TypeError("Qi should be of type SegmentNaturalCoordinates") + if isinstance(Qdoti, SegmentNaturalCoordinates): + raise TypeError("Qdoti should be of type SegmentNaturalVelocities") + + # not able to verify if the types of Qi and Qdoti are np.ndarray + if not isinstance(Qi, SegmentNaturalCoordinates): + Qi = SegmentNaturalCoordinates(Qi) + if not isinstance(Qdoti, SegmentNaturalVelocities): + Qdoti = SegmentNaturalVelocities(Qdoti) + + Gi = self.mass_matrix + Kr = self.rigid_body_constraint_jacobian(Qi) + Krdot = self.rigid_body_constraint_jacobian_derivative(Qdoti) + biais = Krdot @ Qdoti.vector + + A = MX.zeros((18, 18)) + A[0:12, 0:12] = Gi + A[12:18, 0:12] = Kr + A[0:12, 12:18] = Kr.T + A[12:, 12:18] = MX.zeros((6, 6)) + + B = vertcat([self.weight(), biais]) + + # solve the linear system Ax = B with numpy + raise NotImplementedError("This function is not implemented yet") + # todo in casadi + x = np.linalg.solve(A, B) + Qddoti = x[0:12] + lambda_i = x[12:] + return SegmentNaturalAccelerations(Qddoti), lambda_i + + def add_marker(self, marker: SegmentMarker): + """ + Add a new marker to the segment + + Parameters + ---------- + marker + The marker to add + """ + if marker.parent_name is not None and marker.parent_name != self.name: + raise ValueError( + "The marker name should be the same as the 'key'. Alternatively, marker.name can be left undefined" + ) + + marker.parent_name = self.name + self._markers.append(marker) + + def nb_markers(self) -> int: + """ + Returns the number of markers of the natural segment + + Returns + ------- + int + Number of markers of the segment + """ + return len(self._markers) + + def marker_constraints(self, marker_locations: np.ndarray, Qi: SegmentNaturalCoordinates) -> MX: + """ + This function returns the marker constraints of the segment + + Parameters + ---------- + marker_locations: np.ndarray + Marker locations in the global/inertial coordinate system (3 x N_markers) + Qi: SegmentNaturalCoordinates + Natural coordinates of the segment + + Returns + ------- + MX + The defects of the marker constraints of the segment (3 x N_markers) + """ + if marker_locations.shape != (3, self.nb_markers()): + raise ValueError(f"marker_locations should be of shape (3, {self.nb_markers()})") + + defects = MX.zeros((3, self.nb_markers())) + + for i, marker in enumerate(self._markers): + defects[:, i] = marker.constraint(marker_location=marker_locations[:, i], Qi=Qi) + + return defects + + def marker_jacobian(self): + """ + This function returns the marker jacobian of the segment + + Returns + ------- + MX + The jacobian of the marker constraints of the segment (3 x N_markers) + """ + return vertcat([-marker.interpolation_matrix for marker in self._markers]) diff --git a/bionc/model_computations/segment.py b/bionc/bionc_casadi/segment.py similarity index 88% rename from bionc/model_computations/segment.py rename to bionc/bionc_casadi/segment.py index 834a1f72..45b7e2e6 100644 --- a/bionc/model_computations/segment.py +++ b/bionc/bionc_casadi/segment.py @@ -1,6 +1,6 @@ -from ..model_computations.inertia_parameters import InertiaParameters -from ..model_computations.natural_marker import SegmentMarker -from ..model_computations.natural_segment import NaturalSegment +from .inertia_parameters import InertiaParameters +from .natural_marker import SegmentMarker +from .natural_segment import NaturalSegment class Segment: diff --git a/bionc/bionc_numpy/__init__.py b/bionc/bionc_numpy/__init__.py index ef06ff9b..9164076a 100644 --- a/bionc/bionc_numpy/__init__.py +++ b/bionc/bionc_numpy/__init__.py @@ -2,3 +2,14 @@ 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 + +# Some classes to define the BiomechanicalModel +from .natural_axis import Axis +from .natural_marker import SegmentMarker, Marker +from .segment import Segment +from .natural_segment import NaturalSegment +from .inertia_parameters import InertiaParameters \ No newline at end of file diff --git a/bionc/bionc_numpy/biomechanical_model.py b/bionc/bionc_numpy/biomechanical_model.py new file mode 100644 index 00000000..cec23fa0 --- /dev/null +++ b/bionc/bionc_numpy/biomechanical_model.py @@ -0,0 +1,160 @@ +import numpy as np + +from bionc.protocols.natural_coordinates import SegmentNaturalCoordinates, NaturalCoordinates +from bionc.bionc_numpy.natural_velocities import SegmentNaturalVelocities, NaturalVelocities + + +class BiomechanicalModel: + def __init__(self): + from .natural_segment import NaturalSegment # Imported here to prevent from circular imports + from .joint import Joint # Imported here to prevent from circular imports + + self.segments: dict[str:NaturalSegment, ...] = {} + self.joints: dict[str:Joint, ...] = {} + # From Pythom 3.7 the insertion order in a dict is preserved. This is important because when writing a new + # the order of the segment matters + self._mass_matrix = self._update_mass_matrix() + + def __getitem__(self, name: str): + return self.segments[name] + + def __setitem__(self, name: str, segment: "NaturalSegment"): + if segment.name == name: # Make sure the name of the segment fits the internal one + self.segments[name] = segment + self._update_mass_matrix() # Update the generalized mass matrix + else: + raise ValueError("The name of the segment does not match the name of the segment") + + def __str__(self): + out_string = "version 4\n\n" + for name in self.segments: + out_string += str(self.segments[name]) + out_string += "\n\n\n" # Give some space between segments + return out_string + + def nb_segments(self): + return len(self.segments) + + def nb_markers(self): + nb_markers = 0 + for key in self.segments: + nb_markers += self.segments[key].nb_markers() + return nb_markers + + def nb_joints(self): + return len(self.joints) + + def nb_Q(self): + return 12 * self.nb_segments() + + def nb_Qdot(self): + return 12 * self.nb_segments() + + def nb_Qddot(self): + return 12 * self.nb_segments() + + def rigid_body_constraints(self, Q: NaturalCoordinates) -> np.ndarray: + """ + This function returns the rigid body constraints of all segments, denoted Phi_r + as a function of the natural coordinates Q. + + Returns + ------- + np.ndarray + Rigid body constraints of the segment [6 * nb_segments, 1] + """ + + Phi_r = np.zeros(6 * self.nb_segments()) + for i, segment_name in enumerate(self.segments): + idx = slice(6 * i, 6 * (i + 1)) + Phi_r[idx] = self.segments[segment_name].rigid_body_constraint(Q.vector(i)) + + return Phi_r + + def rigid_body_constraints_jacobian(self, Q: NaturalCoordinates) -> np.ndarray: + """ + This function returns the rigid body constraints of all segments, denoted K_r + as a function of the natural coordinates Q. + + Returns + ------- + np.ndarray + Rigid body constraints of the segment [6 * nb_segments, nbQ] + """ + + K_r = np.zeros((6 * self.nb_segments(), Q.shape[0])) + for i, segment_name in enumerate(self.segments): + idx_row = slice(6 * i, 6 * (i + 1)) + idx_col = slice(12 * i, 12 * (i + 1)) + K_r[idx_row, idx_col] = self.segments[segment_name].rigid_body_constraint_jacobian(Q.vector(i)) + + return K_r + + def rigid_body_constraint_jacobian_derivative(self, Qdot: NaturalVelocities) -> np.ndarray: + """ + This function returns the derivative of the Jacobian matrix of the rigid body constraints denoted Kr_dot + + Parameters + ---------- + Qdot : NaturalVelocities + The natural velocities of the segment [12, 1] + + Returns + ------- + np.ndarray + The derivative of the Jacobian matrix of the rigid body constraints [6, 12] + """ + + Kr_dot = np.zeros((6 * self.nb_segments(), Qdot.shape[0])) + for i, segment_name in enumerate(self.segments): + idx_row = slice(6 * i, 6 * (i + 1)) + idx_col = slice(12 * i, 12 * (i + 1)) + Kr_dot[idx_row, idx_col] = self.segments[segment_name].rigid_body_constraint_jacobian_derivative( + Qdot.vector(i) + ) + + return Kr_dot + + def _update_mass_matrix(self): + """ + This function computes the generalized mass matrix of the system, denoted G + + Returns + ------- + np.ndarray + generalized mass matrix of the segment [12 * nbSegment x 12 * * nbSegment] + """ + G = np.zeros((12 * self.nb_segments(), 12 * self.nb_segments())) + for i, segment_name in enumerate(self.segments): + Gi = self.segments[segment_name].mass_matrix + if Gi is None: + # mass matrix is None if one the segment doesn't have any inertial properties + self._mass_matrix = None + return + idx = slice(12 * i, 12 * (i + 1)) + G[idx, idx] = self.segments[segment_name].mass_matrix + + self._mass_matrix = G + + @property + def mass_matrix(self): + """ + This function returns the generalized mass matrix of the system, denoted G + + Returns + ------- + np.ndarray + generalized mass matrix of the segment [12 * nbSegment x 12 * * nbSegment] + + """ + return self._mass_matrix + + +# def kinematicConstraints(self, Q): +# # Method to calculate the kinematic constraints + +# def forwardDynamics(self, Q, Qdot): +# +# return Qddot, lambdas + +# def inverseDynamics(self): diff --git a/bionc/bionc_numpy/inertia_parameters.py b/bionc/bionc_numpy/inertia_parameters.py new file mode 100644 index 00000000..f05ea35d --- /dev/null +++ b/bionc/bionc_numpy/inertia_parameters.py @@ -0,0 +1,87 @@ +from typing import Callable + +import numpy as np + +from .biomechanical_model import BiomechanicalModel +from bionc.model_creation.protocols import Data + + +class InertiaParameters: + def __init__( + self, + mass: float = None, + center_of_mass: np.ndarray = None, + inertia: np.ndarray = None, + ): + """ + Parameters + ---------- + mass + The mass of the segment with respect to the full body + center_of_mass + The position of the center of mass from the segment coordinate system on the main axis + inertia + The inertia xx, yy and zz parameters of the segment + """ + self.mass = mass + self.center_of_mass = center_of_mass + self.inertia = inertia + + @staticmethod + def from_data( + data: Data, + relative_mass: Callable, + center_of_mass: Callable, + inertia: Callable, + kinematic_chain: BiomechanicalModel, + parent_scs: "NaturalSegment" = None, + ): + """ + This is a constructor for the InertiaParameterReal class. + + Parameters + ---------- + data + The data to pick the data from + relative_mass + The callback function that returns the relative mass of the segment with respect to the full body + center_of_mass + The callback function that returns the position of the center of mass + from the segment coordinate system on the main axis + inertia + The callback function that returns the inertia xx, yy and zz parameters of the segment + kinematic_chain + The model as it is constructed at that particular time. It is useful if some values must be obtained from + previously computed values + parent_scs + The segment coordinate system of the parent to transform the marker from global to local + """ + + mass = relative_mass(data.values, kinematic_chain) + + p: np.ndarray = center_of_mass(data.values, kinematic_chain) + if not isinstance(p, np.ndarray): + raise RuntimeError(f"The function {center_of_mass} must return a np.ndarray of dimension 4xT (XYZ1 x time)") + if len(p.shape) == 1: + p = p[:, np.newaxis] + + if len(p.shape) != 2 or p.shape[0] != 4: + raise RuntimeError(f"The function {center_of_mass} must return a np.ndarray of dimension 4xT (XYZ1 x time)") + + p[3, :] = 1 # Do not trust user and make sure the last value is a perfect one + com = (parent_scs.transpose if parent_scs is not None else np.identity(4)) @ p + if np.isnan(com).all(): + raise RuntimeError(f"All the values for {com} returned nan which is not permitted") + + inertia: np.ndarray = inertia(data.values, kinematic_chain) + + return InertiaParameters(mass, com, inertia) + + def __str__(self): + # Define the print function, so it automatically formats things in the file properly + com = np.nanmean(self.center_of_mass, axis=1)[:3] + + out_string = f"\tmass {self.mass}\n" + out_string += f"\tCenterOfMass {com[0]:0.5f} {com[1]:0.5f} {com[2]:0.5f}\n" + out_string += f"\tinertia_xxyyzz {self.inertia[0]} {self.inertia[1]} {self.inertia[2]}\n" + return out_string diff --git a/bionc/bionc_numpy/joint.py b/bionc/bionc_numpy/joint.py new file mode 100644 index 00000000..372996c5 --- /dev/null +++ b/bionc/bionc_numpy/joint.py @@ -0,0 +1,174 @@ +from abc import ABC, abstractmethod + +import numpy as np + +from .natural_segment import NaturalSegment +from bionc.protocols.natural_coordinates import SegmentNaturalCoordinates + + +class JointBase(ABC): + """ + This class is made to handle the kinematics of a joint + + Attributes + ---------- + joint_name : str + The name of the joint + segment_parent : NaturalSegment + The parent segment of the joint + segment_child : NaturalSegment + The child segment of the joint + + Methods + ------- + constraints(self, Q_parent: NaturalCoordinates, Q_child: NaturalCoordinates) -> np.ndarray + Returns the constraints of the joint, this defect function should be zero when the joint is in a valid position + + """ + + def __init__( + self, + joint_name: str, + segment_parent: NaturalSegment, + segment_child: NaturalSegment, + ): + self.joint_name = joint_name + self.segment_parent = segment_parent + self.segment_child = segment_child + + @abstractmethod + def constraints(self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNaturalCoordinates) -> np.ndarray: + """ + This function returns the constraints of the joint, denoted Phi_k as a function of the natural coordinates Q. + + Returns + ------- + np.ndarray + Constraints of the joint + """ + + @abstractmethod + def constraintJacobians( + self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNaturalCoordinates + ) -> np.ndarray: + """ + This function returns the constraint Jacobians of the joint, denoted K_k + as a function of the natural coordinates Q_parent and Q_child. + + Returns + ------- + np.ndarray + Constraint Jacobians of the joint [3, 2 * nbQ] + """ + + +class Joint: + """ + The public interface to the different Joint classes + + """ + + class Hinge(JointBase): + """ + This joint is defined by 3 constraints to pivot around a given axis defined by two angles theta_1 and theta_2. + + """ + + def __init__( + self, + joint_name: str, + segment_parent: NaturalSegment, + segment_child: NaturalSegment, + theta_1: float, + theta_2: float, + ): + + super(Joint.Hinge, self).__init__(joint_name, segment_parent, segment_child) + self.theta_1 = theta_1 + self.theta_2 = theta_2 + + def constraint(self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNaturalCoordinates) -> np.ndarray: + """ + This function returns the kinematic constraints of the joint, denoted Phi_k + as a function of the natural coordinates Q_parent and Q_child. + + Returns + ------- + np.ndarray + Kinematic constraints of the joint [3, 1] + """ + constraint = np.zeros(3) + constraint[0] = Q_parent.rd - Q_child.rp + constraint[1] = np.dot(Q_parent.w, Q_child.rp - Q_child.rd) - self.segment_child.length * np.cos( + self.theta_1 + ) + constraint[2] = np.dot(Q_parent.w, Q_child.u) - np.cos(self.theta_2) + + return constraint + + class Universal(JointBase, ABC): + def __init__( + self, + joint_name: str, + segment_parent: NaturalSegment, + segment_child: NaturalSegment, + theta: float, + ): + + super(Joint.Universal, self).__init__(joint_name, segment_parent, segment_child) + self.theta = theta + + def constraint(self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNaturalCoordinates) -> np.ndarray: + """ + This function returns the kinematic constraints of the joint, denoted Phi_k + as a function of the natural coordinates Q_parent and Q_child. + + Returns + ------- + np.ndarray + Kinematic constraints of the joint [2, 1] + """ + N = np.zeros((3, 12)) # interpolation matrix of the given axis + + constraint = np.zeros(2) + constraint[0] = Q_parent.rd - Q_child.rp + constraint[1] = np.dot(Q_parent.w, np.matmul(N, Q_child.vector)) - np.cos(self.theta) + + return constraint + + class Spherical(JointBase): + def __init__( + self, + joint_name: str, + segment_parent: NaturalSegment, + segment_child: NaturalSegment, + point_interpolation_matrix_in_child: float = None, + ): + + super(Joint.Spherical, self).__init__(joint_name, segment_parent, segment_child) + # todo: do something better + # this thing is not none if the joint is not located at rp nor at rd and it needs to be used + self.point_interpolation_matrix_in_child = point_interpolation_matrix_in_child + + def constraint(self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNaturalCoordinates) -> np.ndarray: + """ + This function returns the kinematic constraints of the joint, denoted Phi_k + as a function of the natural coordinates Q_parent and Q_child. + + Returns + ------- + np.ndarray + Kinematic constraints of the joint [2, 1] + """ + N = np.zeros((3, 12)) # interpolation matrix of the given axis + + constraint = np.zeros(1) + if self.point_interpolation_matrix_in_child is None: + constraint[0] = Q_parent.rd - Q_child.rp + else: + constraint[0] = np.matmul(self.point_interpolation_matrix_in_child, Q_parent.vector) - Q_child.rd + + return constraint + + +# todo : more to come diff --git a/bionc/bionc_numpy/natural_axis.py b/bionc/bionc_numpy/natural_axis.py new file mode 100644 index 00000000..21639dc6 --- /dev/null +++ b/bionc/bionc_numpy/natural_axis.py @@ -0,0 +1,25 @@ +import numpy as np + +from .natural_marker import Marker + + +class Axis: + def __init__(self, start: Marker, end: Marker): + """ + Parameters + ---------- + start: + The initial SegmentMarker + end: + The final SegmentMarker + """ + self.start_point = start + self.end_point = end + + def axis(self) -> np.ndarray: + """ + Returns the axis vector + """ + start = self.start_point.position + end = self.end_point.position + return end - start diff --git a/bionc/bionc_numpy/natural_marker.py b/bionc/bionc_numpy/natural_marker.py new file mode 100644 index 00000000..e61071d3 --- /dev/null +++ b/bionc/bionc_numpy/natural_marker.py @@ -0,0 +1,337 @@ +from typing import Callable + +import numpy as np + +from .biomechanical_model import BiomechanicalModel +from bionc.model_creation.protocols import Data +from bionc.utils.interpolation_matrix import interpolate_natural_vector, to_natural_vector + +# from ..utils.natural_coordinates import SegmentNaturalCoordinates +from bionc.protocols.natural_coordinates import SegmentNaturalCoordinates + +# todo: need a list of markers MarkerList + + +class SegmentMarker: + """ + Class used to create a segment markers for the natural segments + + Methods + ------- + from_data() + Creates a segment marker from the data + constraint() + Computes the constraint for the marker given the segment natural coordinates and experimental marker location + + Attributes + ---------- + name: str + The name of the marker + parent_name: str + The name of the parent segment on which the marker is attached + position: np.ndarray + The 3d position of the marker in the non orthogonal segment coordinate system + interpolation_matrix: np.ndarray + The interpolation matrix to use for the marker + is_technical: bool + If the marker should be flagged as a technical marker + is_anatomical: bool + If the marker should be flagged as an anatomical landmark + """ + + def __init__( + self, + name: str, + parent_name: str, + position: tuple[int | float, int | float, int | float] | np.ndarray = None, + interpolation_matrix: np.ndarray = None, + is_technical: bool = True, + is_anatomical: bool = False, + ): + """ + Parameters + ---------- + name + The name of the new marker + parent_name + The name of the parent the marker is attached to + position + The 3d position of the marker in the non orthogonal segment coordinate system + interpolation_matrix + The interpolation matrix to use for the marker + is_technical + If the marker should be flagged as a technical marker + is_anatomical + If the marker should be flagged as an anatomical marker + """ + self.name = name + self.parent_name = parent_name + + if position is None and interpolation_matrix is None: + raise ValueError("Either a position or an interpolation matrix must be provided") + + elif position is not None and interpolation_matrix is None: + if position.shape[0] != 3: + raise ValueError("The position must be a 3d vector") + + self.position = position if isinstance(position, np.ndarray) else np.array(position) + self.interpolation_matrix = interpolate_natural_vector(self.position) + + elif position is None and interpolation_matrix is not None: + if interpolation_matrix.shape != (3, 12): + raise ValueError("The interpolation matrix must be a 3x12 matrix") + + self.interpolation_matrix = interpolation_matrix + self.position = to_natural_vector(self.interpolation_matrix) + + else: + raise ValueError("position and interpolation matrix cannot both be provided") + + self.is_technical = is_technical + self.is_anatomical = is_anatomical + + @classmethod + def from_data( + cls, + data: Data, + name: str, + function: Callable, + parent_name: str, + kinematic_chain: BiomechanicalModel, + Q_xp: SegmentNaturalCoordinates = None, + is_technical: bool = True, + is_anatomical: bool = False, + ): + """ + This is a constructor for the MarkerReal class. It evaluates the function that defines the marker to get an + actual position + + Parameters + ---------- + data + The data to pick the data from + name + The name of the new marker + function + The function (f(m) -> np.ndarray, where m is a dict of markers (XYZ1 x time)) that defines the marker + parent_name + The name of the parent the marker is attached to + kinematic_chain + The model as it is constructed at that particular time. It is useful if some values must be obtained from + previously computed values + Q_xp: SegmentNaturalCoordinates + The segment natural coordinates identified from data + is_technical + If the marker should be flagged as a technical marker + is_anatomical + If the marker should be flagged as an anatomical marker + """ + + # Get the position of the markers and do some sanity checks + p: np.ndarray = function(data.values, kinematic_chain) + if not isinstance(p, np.ndarray): + raise RuntimeError(f"The function {function} must return a np.ndarray of dimension 4xT (XYZ1 x time)") + if len(p.shape) == 1: + p = p[:, np.newaxis] + + if len(p.shape) != 2 or p.shape[0] != 4: + raise RuntimeError(f"The function {function} must return a np.ndarray of dimension 4xT (XYZ1 x time)") + + natural_positions = Q_xp.to_non_orthogonal_basis(vector=p[:3, :]) + # mean + natural_position = natural_positions.mean(axis=1) + + if np.isnan(natural_position).all(): + raise RuntimeError(f"All the values for {function} returned nan which is not permitted") + return cls( + name, + parent_name, + position=natural_position, + is_technical=is_technical, + is_anatomical=is_anatomical, + ) + + def constraint(self, marker_location: np.ndarray, Qi: SegmentNaturalCoordinates) -> np.ndarray: + """ + This function computes the constraint for the marker + + Parameters + ---------- + marker_location: np.ndarray + The location of the marker in the global/inertial coordinate system + Qi + The segment natural coordinates + + Returns + ------- + The constraint for the marker + """ + if marker_location.shape[0] != 3: + raise ValueError("The marker location must be a 3d vector") + if marker_location.shape.__len__() > 1: + if marker_location.shape[1] != 1: + raise ValueError("The marker location must be a 3d vector with only one column") + else: + marker_location = marker_location.squeeze() + + return (marker_location - self.interpolation_matrix @ Qi.vector).squeeze() + + def __str__(self): + # Define the print function, so it automatically formats things in the file properly + out_string = f"marker {self.name}\n" + out_string += f"\tparent {self.parent_name}\n" + + p = np.array(self.position) + p = p if len(p.shape) == 1 else np.nanmean(p, axis=1) + p = p if len(p.shape) == 1 else np.nanmean(p, axis=0) + out_string += f"\tposition {p[0]:0.4f} {p[1]:0.4f} {p[2]:0.4f}\n" + out_string += f"\ttechnical {1 if self.is_technical else 0}\n" + out_string += f"\tanatomical {1 if self.is_anatomical else 0}\n" + out_string += "endmarker\n" + return out_string + + def __add__(self, other: np.ndarray | tuple): + if isinstance(other, tuple): + other = np.array(other) + + if isinstance(other, np.ndarray): + return SegmentMarker(name=self.name, parent_name=self.parent_name, position=self.position + other) + elif isinstance(other, SegmentMarker): + return SegmentMarker(name=self.name, parent_name=self.parent_name, position=self.position + other.position) + else: + raise NotImplementedError(f"The addition for {type(other)} is not implemented") + + def __sub__(self, other): + if isinstance(other, tuple): + other = np.array(other) + + if isinstance(other, np.ndarray): + return SegmentMarker(name=self.name, parent_name=self.parent_name, position=self.position - other) + elif isinstance(other, SegmentMarker): + return SegmentMarker(name=self.name, parent_name=self.parent_name, position=self.position - other.position) + else: + raise NotImplementedError(f"The subtraction for {type(other)} is not implemented") + + +class Marker: + def __init__( + self, + name: str, + position: tuple[int | float, int | float, int | float] | np.ndarray = None, + is_technical: bool = True, + is_anatomical: bool = False, + ): + """ + Parameters + ---------- + name + The name of the new marker + position + The 3d position of the marker in the orhtogonal coordinate system (XYZ1 x time)) that defines the marker + is_technical + If the marker should be flagged as a technical marker + is_anatomical + If the marker should be flagged as an anatomical marker + """ + self.name = name + + if position is None: + raise ValueError("A position must be provided") + + if position.shape[0] != 4: + if position.shape[0] == 3: # If the position is a 3d vector, add a 1 at the bottom + position = np.vstack((position, np.ones((1, position.shape[1])))) + else: + raise ValueError("The position must be (XYZ x time) or (XYZ1 x time)") + + self.position = position + self.is_technical = is_technical + self.is_anatomical = is_anatomical + + @classmethod + def from_data( + cls, + data: Data, + name: str, + function: Callable, + kinematic_chain: BiomechanicalModel, + is_technical: bool = True, + is_anatomical: bool = False, + ): + """ + This is a constructor for the MarkerReal class. It evaluates the function that defines the marker to get an + actual position + + Parameters + ---------- + data + The data to pick the data from + name + The name of the new marker + function + The function (f(m) -> np.ndarray, where m is a dict of markers (XYZ1 x time)) that defines the marker + kinematic_chain + The model as it is constructed at that particular time. It is useful if some values must be obtained from + previously computed values + is_technical + If the marker should be flagged as a technical marker + is_anatomical + If the marker should be flagged as an anatomical marker + """ + + # Get the position of the markers and do some sanity checks + position: np.ndarray = function(data.values, kinematic_chain) + if not isinstance(position, np.ndarray): + raise RuntimeError(f"The function {function} must return a np.ndarray of dimension 4xT (XYZ1 x time)") + if len(position.shape) == 1: + position = position[:, np.newaxis] + + if len(position.shape) != 2 or position.shape[0] != 4: + raise RuntimeError(f"The function {function} must return a np.ndarray of dimension 4xT (XYZ1 x time)") + + position[3, :] = 1 # Do not trust user and make sure the last value is a perfect one + + if np.isnan(position).all(): + raise RuntimeError(f"All the values for {function} returned nan which is not permitted") + + return cls( + name, + position, + is_technical=is_technical, + is_anatomical=is_anatomical, + ) + + def __str__(self): + # Define the print function, so it automatically formats things in the file properly + out_string = f"marker {self.name}\n" + + p = np.array(self.position) + p = p if len(p.shape) == 1 else np.nanmean(p, axis=1) + p = p if len(p.shape) == 1 else np.nanmean(p, axis=0) + out_string += f"\tposition {p[0]:0.4f} {p[1]:0.4f} {p[2]:0.4f}\n" + out_string += f"\ttechnical {1 if self.is_technical else 0}\n" + out_string += f"\tanatomical {1 if self.is_anatomical else 0}\n" + out_string += "endmarker\n" + return out_string + + def __add__(self, other: np.ndarray | tuple): + if isinstance(other, tuple): + other = np.array(other) + + if isinstance(other, np.ndarray): + return Marker(name=self.name, position=self.position + other) + elif isinstance(other, Marker): + return Marker(name=self.name, position=self.position + other.position) + else: + raise NotImplementedError(f"The addition for {type(other)} is not implemented") + + def __sub__(self, other): + if isinstance(other, tuple): + other = np.array(other) + + if isinstance(other, np.ndarray): + return Marker(name=self.name, position=self.position - other) + elif isinstance(other, Marker): + return Marker(name=self.name, position=self.position - other.position) + else: + raise NotImplementedError(f"The subtraction for {type(other)} is not implemented") diff --git a/bionc/model_computations/natural_segment.py b/bionc/bionc_numpy/natural_segment.py similarity index 98% rename from bionc/model_computations/natural_segment.py rename to bionc/bionc_numpy/natural_segment.py index 8162bbe2..28ce7db4 100644 --- a/bionc/model_computations/natural_segment.py +++ b/bionc/bionc_numpy/natural_segment.py @@ -6,10 +6,10 @@ # from ..utils.natural_coordinates import SegmentNaturalCoordinates from ..protocols.natural_coordinates import SegmentNaturalCoordinates, NaturalCoordinates -from ..bionc_casadi.natural_velocities import SegmentNaturalVelocities, NaturalVelocities -from ..bionc_casadi.natural_accelerations import SegmentNaturalAccelerations, NaturalAccelerations -from ..bionc_casadi.homogenous_transform import HomogeneousTransform -from ..model_computations.natural_marker import SegmentMarker +from ..bionc_numpy.natural_velocities import SegmentNaturalVelocities, NaturalVelocities +from ..bionc_numpy.natural_accelerations import SegmentNaturalAccelerations, NaturalAccelerations +from ..bionc_numpy.homogenous_transform import HomogeneousTransform +from ..bionc_numpy.natural_marker import SegmentMarker class NaturalSegment: diff --git a/bionc/bionc_numpy/segment.py b/bionc/bionc_numpy/segment.py new file mode 100644 index 00000000..45b7e2e6 --- /dev/null +++ b/bionc/bionc_numpy/segment.py @@ -0,0 +1,46 @@ +from .inertia_parameters import InertiaParameters +from .natural_marker import SegmentMarker +from .natural_segment import NaturalSegment + + +class Segment: + def __init__( + self, + name: str = None, + parent_name: str = "", + segment_coordinate_system: NaturalSegment = None, + translations: str = "", + rotations: str = "", + inertia_parameters: InertiaParameters = None, + ): + self.name = name + self.parent_name = parent_name + self.translations = translations + self.rotations = rotations + self.markers = [] + self.segment_coordinate_system = segment_coordinate_system + self.inertia_parameters = inertia_parameters + + def add_marker(self, marker: SegmentMarker): + self.markers.append(marker) + + def __str__(self): + # Define the print function, so it automatically formats things in the file properly + out_string = f"segment {self.name}\n" + if self.parent_name: + out_string += f"\tparent {self.parent_name}\n" + if self.segment_coordinate_system: + out_string += f"\tRT {self.segment_coordinate_system}\n" + if self.translations: + out_string += f"\ttranslations {self.translations}\n" + if self.rotations: + out_string += f"\trotations {self.rotations}\n" + if self.inertia_parameters: + out_string += str(self.inertia_parameters) + out_string += "endsegment\n" + + # Also print the markers attached to the segment + if self.markers: + for marker in self.markers: + out_string += str(marker) + return out_string diff --git a/bionc/model_computations/__init__.py b/bionc/model_computations/__init__.py deleted file mode 100644 index c1456d64..00000000 --- a/bionc/model_computations/__init__.py +++ /dev/null @@ -1,9 +0,0 @@ -# The actual model to inherit from -from .biomechanical_model import BiomechanicalModel - -# Some classes to define the BiomechanicalModel -from .natural_axis import Axis -from .natural_marker import SegmentMarker, Marker -from .segment import Segment -from .natural_segment import NaturalSegment -from .inertia_parameters import InertiaParameters diff --git a/bionc/model_creation/biomechanical_model_template.py b/bionc/model_creation/biomechanical_model_template.py index 4064f244..c3640ae6 100644 --- a/bionc/model_creation/biomechanical_model_template.py +++ b/bionc/model_creation/biomechanical_model_template.py @@ -1,8 +1,8 @@ from .protocols import Data -from ..model_computations.segment import Segment +# from ..model_computations.segment import Segment from .segment_template import SegmentTemplate -from ..model_computations.natural_segment import NaturalSegment -from ..model_computations.biomechanical_model import BiomechanicalModel +# from ..model_computations.natural_segment import NaturalSegment +from bionc.bionc_numpy.biomechanical_model import BiomechanicalModel class BiomechanicalModelTemplate: diff --git a/bionc/model_creation/inertia_parameters_template.py b/bionc/model_creation/inertia_parameters_template.py index 4f698a4a..d5fd11b9 100644 --- a/bionc/model_creation/inertia_parameters_template.py +++ b/bionc/model_creation/inertia_parameters_template.py @@ -2,9 +2,9 @@ import numpy as np -from ..model_computations.inertia_parameters import InertiaParameters -from ..model_computations.biomechanical_model import BiomechanicalModel -from ..model_computations.natural_segment import NaturalSegment +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 diff --git a/bionc/model_creation/marker_template.py b/bionc/model_creation/marker_template.py index b2d5e5e3..97602d3f 100644 --- a/bionc/model_creation/marker_template.py +++ b/bionc/model_creation/marker_template.py @@ -2,12 +2,12 @@ import numpy as np -from ..model_computations.biomechanical_model import BiomechanicalModel -from ..model_computations.natural_marker import SegmentMarker, Marker +from bionc.bionc_numpy.biomechanical_model import BiomechanicalModel +from ..bionc_numpy.natural_marker import SegmentMarker, Marker # from .biomechanical_model_template import BiomechanicalModelTemplate from .protocols import Data -from ..model_computations.natural_segment import NaturalSegment +from ..bionc_numpy.natural_segment import NaturalSegment # from ..utils.natural_coordinates import SegmentNaturalCoordinates from ..protocols.natural_coordinates import SegmentNaturalCoordinates diff --git a/bionc/model_creation/natural_axis_template.py b/bionc/model_creation/natural_axis_template.py index cd8d31ec..01432409 100644 --- a/bionc/model_creation/natural_axis_template.py +++ b/bionc/model_creation/natural_axis_template.py @@ -1,10 +1,10 @@ from typing import Callable -from ..model_computations.natural_axis import Axis -from ..model_computations.biomechanical_model import BiomechanicalModel +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 ..model_computations.natural_segment import NaturalSegment +from ..bionc_numpy.natural_segment import NaturalSegment class AxisTemplate: diff --git a/bionc/model_creation/natural_segment_template.py b/bionc/model_creation/natural_segment_template.py index 880c634f..0be6c9de 100644 --- a/bionc/model_creation/natural_segment_template.py +++ b/bionc/model_creation/natural_segment_template.py @@ -1,13 +1,13 @@ from typing import Callable from .natural_axis_template import AxisTemplate -from ..model_computations.biomechanical_model import BiomechanicalModel +from bionc.bionc_numpy.biomechanical_model import BiomechanicalModel # from ..utils.natural_coordinates import SegmentNaturalCoordinates from ..protocols.natural_coordinates import SegmentNaturalCoordinates from .marker_template import MarkerTemplate from .protocols import Data -from ..model_computations.natural_segment import NaturalSegment +from ..bionc_numpy.natural_segment import NaturalSegment class NaturalSegmentTemplate: From ff91af178cb110b96c7fbee2680575bdadb79341 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Wed, 16 Nov 2022 09:50:43 -0500 Subject: [PATCH 19/30] numpy to casadi .py scripts --- bionc/__init__.py | 1 - bionc/bionc_casadi/__init__.py | 1 - bionc/bionc_casadi/biomechanical_model.py | 29 +++++++------- bionc/bionc_casadi/inertia_parameters.py | 1 + bionc/bionc_casadi/joint.py | 26 +++++++------ bionc/bionc_casadi/natural_axis.py | 4 +- bionc/bionc_casadi/natural_marker.py | 13 +++---- bionc/bionc_casadi/segment.py | 46 ----------------------- bionc/bionc_numpy/__init__.py | 1 - bionc/bionc_numpy/segment.py | 46 ----------------------- 10 files changed, 38 insertions(+), 130 deletions(-) delete mode 100644 bionc/bionc_casadi/segment.py delete mode 100644 bionc/bionc_numpy/segment.py diff --git a/bionc/__init__.py b/bionc/__init__.py index e4182d33..4c1713c1 100644 --- a/bionc/__init__.py +++ b/bionc/__init__.py @@ -16,7 +16,6 @@ Axis, SegmentMarker, Marker, - Segment, NaturalSegment, InertiaParameters, BiomechanicalModel, diff --git a/bionc/bionc_casadi/__init__.py b/bionc/bionc_casadi/__init__.py index 389c06fb..8a643570 100644 --- a/bionc/bionc_casadi/__init__.py +++ b/bionc/bionc_casadi/__init__.py @@ -10,6 +10,5 @@ # Some classes to define the BiomechanicalModel from .natural_axis import Axis from .natural_marker import SegmentMarker, Marker -from .segment import Segment from .natural_segment import NaturalSegment from .inertia_parameters import InertiaParameters \ No newline at end of file diff --git a/bionc/bionc_casadi/biomechanical_model.py b/bionc/bionc_casadi/biomechanical_model.py index cec23fa0..9554badb 100644 --- a/bionc/bionc_casadi/biomechanical_model.py +++ b/bionc/bionc_casadi/biomechanical_model.py @@ -1,7 +1,8 @@ import numpy as np +from casadi import MX -from bionc.protocols.natural_coordinates import SegmentNaturalCoordinates, NaturalCoordinates -from bionc.bionc_numpy.natural_velocities import SegmentNaturalVelocities, NaturalVelocities +from .natural_coordinates import SegmentNaturalCoordinates, NaturalCoordinates +from .natural_velocities import SegmentNaturalVelocities, NaturalVelocities class BiomechanicalModel: @@ -53,36 +54,36 @@ def nb_Qdot(self): def nb_Qddot(self): return 12 * self.nb_segments() - def rigid_body_constraints(self, Q: NaturalCoordinates) -> np.ndarray: + def rigid_body_constraints(self, Q: NaturalCoordinates) ->MX: """ This function returns the rigid body constraints of all segments, denoted Phi_r as a function of the natural coordinates Q. Returns ------- - np.ndarray + MX Rigid body constraints of the segment [6 * nb_segments, 1] """ - Phi_r = np.zeros(6 * self.nb_segments()) + Phi_r = MX.zeros(6 * self.nb_segments()) for i, segment_name in enumerate(self.segments): idx = slice(6 * i, 6 * (i + 1)) Phi_r[idx] = self.segments[segment_name].rigid_body_constraint(Q.vector(i)) return Phi_r - def rigid_body_constraints_jacobian(self, Q: NaturalCoordinates) -> np.ndarray: + def rigid_body_constraints_jacobian(self, Q: NaturalCoordinates) -> MX: """ This function returns the rigid body constraints of all segments, denoted K_r as a function of the natural coordinates Q. Returns ------- - np.ndarray + MX Rigid body constraints of the segment [6 * nb_segments, nbQ] """ - K_r = np.zeros((6 * self.nb_segments(), Q.shape[0])) + K_r = MX.zeros((6 * self.nb_segments(), Q.shape[0])) for i, segment_name in enumerate(self.segments): idx_row = slice(6 * i, 6 * (i + 1)) idx_col = slice(12 * i, 12 * (i + 1)) @@ -90,7 +91,7 @@ def rigid_body_constraints_jacobian(self, Q: NaturalCoordinates) -> np.ndarray: return K_r - def rigid_body_constraint_jacobian_derivative(self, Qdot: NaturalVelocities) -> np.ndarray: + def rigid_body_constraint_jacobian_derivative(self, Qdot: NaturalVelocities) -> MX: """ This function returns the derivative of the Jacobian matrix of the rigid body constraints denoted Kr_dot @@ -101,11 +102,11 @@ def rigid_body_constraint_jacobian_derivative(self, Qdot: NaturalVelocities) -> Returns ------- - np.ndarray + MX The derivative of the Jacobian matrix of the rigid body constraints [6, 12] """ - Kr_dot = np.zeros((6 * self.nb_segments(), Qdot.shape[0])) + Kr_dot = MX.zeros((6 * self.nb_segments(), Qdot.shape[0])) for i, segment_name in enumerate(self.segments): idx_row = slice(6 * i, 6 * (i + 1)) idx_col = slice(12 * i, 12 * (i + 1)) @@ -121,10 +122,10 @@ def _update_mass_matrix(self): Returns ------- - np.ndarray + MX generalized mass matrix of the segment [12 * nbSegment x 12 * * nbSegment] """ - G = np.zeros((12 * self.nb_segments(), 12 * self.nb_segments())) + G = MX.zeros((12 * self.nb_segments(), 12 * self.nb_segments())) for i, segment_name in enumerate(self.segments): Gi = self.segments[segment_name].mass_matrix if Gi is None: @@ -143,7 +144,7 @@ def mass_matrix(self): Returns ------- - np.ndarray + MX generalized mass matrix of the segment [12 * nbSegment x 12 * * nbSegment] """ diff --git a/bionc/bionc_casadi/inertia_parameters.py b/bionc/bionc_casadi/inertia_parameters.py index f05ea35d..09c22cc9 100644 --- a/bionc/bionc_casadi/inertia_parameters.py +++ b/bionc/bionc_casadi/inertia_parameters.py @@ -6,6 +6,7 @@ from bionc.model_creation.protocols import Data +# TODO: not used and tested yet class InertiaParameters: def __init__( self, diff --git a/bionc/bionc_casadi/joint.py b/bionc/bionc_casadi/joint.py index 372996c5..d4bcc12b 100644 --- a/bionc/bionc_casadi/joint.py +++ b/bionc/bionc_casadi/joint.py @@ -1,11 +1,13 @@ from abc import ABC, abstractmethod +from casadi import MX import numpy as np from .natural_segment import NaturalSegment from bionc.protocols.natural_coordinates import SegmentNaturalCoordinates +# TODO: not tested yet class JointBase(ABC): """ This class is made to handle the kinematics of a joint @@ -43,21 +45,21 @@ def constraints(self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNatur Returns ------- - np.ndarray + MX Constraints of the joint """ @abstractmethod def constraintJacobians( self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNaturalCoordinates - ) -> np.ndarray: + ) -> MX: """ This function returns the constraint Jacobians of the joint, denoted K_k as a function of the natural coordinates Q_parent and Q_child. Returns ------- - np.ndarray + MX Constraint Jacobians of the joint [3, 2 * nbQ] """ @@ -87,7 +89,7 @@ def __init__( self.theta_1 = theta_1 self.theta_2 = theta_2 - def constraint(self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNaturalCoordinates) -> np.ndarray: + def constraint(self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNaturalCoordinates) -> MX: """ This function returns the kinematic constraints of the joint, denoted Phi_k as a function of the natural coordinates Q_parent and Q_child. @@ -97,7 +99,7 @@ def constraint(self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNatura np.ndarray Kinematic constraints of the joint [3, 1] """ - constraint = np.zeros(3) + constraint = MX.zeros(3) constraint[0] = Q_parent.rd - Q_child.rp constraint[1] = np.dot(Q_parent.w, Q_child.rp - Q_child.rd) - self.segment_child.length * np.cos( self.theta_1 @@ -118,7 +120,7 @@ def __init__( super(Joint.Universal, self).__init__(joint_name, segment_parent, segment_child) self.theta = theta - def constraint(self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNaturalCoordinates) -> np.ndarray: + def constraint(self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNaturalCoordinates) -> MX: """ This function returns the kinematic constraints of the joint, denoted Phi_k as a function of the natural coordinates Q_parent and Q_child. @@ -128,9 +130,9 @@ def constraint(self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNatura np.ndarray Kinematic constraints of the joint [2, 1] """ - N = np.zeros((3, 12)) # interpolation matrix of the given axis + N = MX.zeros((3, 12)) # interpolation matrix of the given axis - constraint = np.zeros(2) + constraint = MX.zeros(2) constraint[0] = Q_parent.rd - Q_child.rp constraint[1] = np.dot(Q_parent.w, np.matmul(N, Q_child.vector)) - np.cos(self.theta) @@ -150,7 +152,7 @@ def __init__( # this thing is not none if the joint is not located at rp nor at rd and it needs to be used self.point_interpolation_matrix_in_child = point_interpolation_matrix_in_child - def constraint(self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNaturalCoordinates) -> np.ndarray: + def constraint(self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNaturalCoordinates) -> MX: """ This function returns the kinematic constraints of the joint, denoted Phi_k as a function of the natural coordinates Q_parent and Q_child. @@ -160,13 +162,13 @@ def constraint(self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNatura np.ndarray Kinematic constraints of the joint [2, 1] """ - N = np.zeros((3, 12)) # interpolation matrix of the given axis + N = MX.zeros((3, 12)) # interpolation matrix of the given axis - constraint = np.zeros(1) + constraint = MX.zeros(1) if self.point_interpolation_matrix_in_child is None: constraint[0] = Q_parent.rd - Q_child.rp else: - constraint[0] = np.matmul(self.point_interpolation_matrix_in_child, Q_parent.vector) - Q_child.rd + constraint[0] = self.point_interpolation_matrix_in_child @ Q_parent.vector - Q_child.rd return constraint diff --git a/bionc/bionc_casadi/natural_axis.py b/bionc/bionc_casadi/natural_axis.py index 21639dc6..0c95ce2c 100644 --- a/bionc/bionc_casadi/natural_axis.py +++ b/bionc/bionc_casadi/natural_axis.py @@ -1,5 +1,5 @@ import numpy as np - +from casadi import MX from .natural_marker import Marker @@ -16,7 +16,7 @@ def __init__(self, start: Marker, end: Marker): self.start_point = start self.end_point = end - def axis(self) -> np.ndarray: + def axis(self) -> MX: """ Returns the axis vector """ diff --git a/bionc/bionc_casadi/natural_marker.py b/bionc/bionc_casadi/natural_marker.py index e61071d3..516f5440 100644 --- a/bionc/bionc_casadi/natural_marker.py +++ b/bionc/bionc_casadi/natural_marker.py @@ -1,6 +1,7 @@ from typing import Callable import numpy as np +from casadi import MX, vertcat from .biomechanical_model import BiomechanicalModel from bionc.model_creation.protocols import Data @@ -146,12 +147,12 @@ def from_data( return cls( name, parent_name, - position=natural_position, + position=MX(natural_position), is_technical=is_technical, is_anatomical=is_anatomical, ) - def constraint(self, marker_location: np.ndarray, Qi: SegmentNaturalCoordinates) -> np.ndarray: + def constraint(self, marker_location: np.ndarray, Qi: SegmentNaturalCoordinates) -> MX: """ This function computes the constraint for the marker @@ -171,10 +172,8 @@ def constraint(self, marker_location: np.ndarray, Qi: SegmentNaturalCoordinates) if marker_location.shape.__len__() > 1: if marker_location.shape[1] != 1: raise ValueError("The marker location must be a 3d vector with only one column") - else: - marker_location = marker_location.squeeze() - return (marker_location - self.interpolation_matrix @ Qi.vector).squeeze() + return marker_location - self.interpolation_matrix @ Qi.vector def __str__(self): # Define the print function, so it automatically formats things in the file properly @@ -217,7 +216,7 @@ class Marker: def __init__( self, name: str, - position: tuple[int | float, int | float, int | float] | np.ndarray = None, + position: tuple[int | float, int | float, int | float] | np.ndarray | MX = None, is_technical: bool = True, is_anatomical: bool = False, ): @@ -296,7 +295,7 @@ def from_data( return cls( name, - position, + MX(position), is_technical=is_technical, is_anatomical=is_anatomical, ) diff --git a/bionc/bionc_casadi/segment.py b/bionc/bionc_casadi/segment.py deleted file mode 100644 index 45b7e2e6..00000000 --- a/bionc/bionc_casadi/segment.py +++ /dev/null @@ -1,46 +0,0 @@ -from .inertia_parameters import InertiaParameters -from .natural_marker import SegmentMarker -from .natural_segment import NaturalSegment - - -class Segment: - def __init__( - self, - name: str = None, - parent_name: str = "", - segment_coordinate_system: NaturalSegment = None, - translations: str = "", - rotations: str = "", - inertia_parameters: InertiaParameters = None, - ): - self.name = name - self.parent_name = parent_name - self.translations = translations - self.rotations = rotations - self.markers = [] - self.segment_coordinate_system = segment_coordinate_system - self.inertia_parameters = inertia_parameters - - def add_marker(self, marker: SegmentMarker): - self.markers.append(marker) - - def __str__(self): - # Define the print function, so it automatically formats things in the file properly - out_string = f"segment {self.name}\n" - if self.parent_name: - out_string += f"\tparent {self.parent_name}\n" - if self.segment_coordinate_system: - out_string += f"\tRT {self.segment_coordinate_system}\n" - if self.translations: - out_string += f"\ttranslations {self.translations}\n" - if self.rotations: - out_string += f"\trotations {self.rotations}\n" - if self.inertia_parameters: - out_string += str(self.inertia_parameters) - out_string += "endsegment\n" - - # Also print the markers attached to the segment - if self.markers: - for marker in self.markers: - out_string += str(marker) - return out_string diff --git a/bionc/bionc_numpy/__init__.py b/bionc/bionc_numpy/__init__.py index 9164076a..4dd71609 100644 --- a/bionc/bionc_numpy/__init__.py +++ b/bionc/bionc_numpy/__init__.py @@ -10,6 +10,5 @@ # Some classes to define the BiomechanicalModel from .natural_axis import Axis from .natural_marker import SegmentMarker, Marker -from .segment import Segment from .natural_segment import NaturalSegment from .inertia_parameters import InertiaParameters \ No newline at end of file diff --git a/bionc/bionc_numpy/segment.py b/bionc/bionc_numpy/segment.py deleted file mode 100644 index 45b7e2e6..00000000 --- a/bionc/bionc_numpy/segment.py +++ /dev/null @@ -1,46 +0,0 @@ -from .inertia_parameters import InertiaParameters -from .natural_marker import SegmentMarker -from .natural_segment import NaturalSegment - - -class Segment: - def __init__( - self, - name: str = None, - parent_name: str = "", - segment_coordinate_system: NaturalSegment = None, - translations: str = "", - rotations: str = "", - inertia_parameters: InertiaParameters = None, - ): - self.name = name - self.parent_name = parent_name - self.translations = translations - self.rotations = rotations - self.markers = [] - self.segment_coordinate_system = segment_coordinate_system - self.inertia_parameters = inertia_parameters - - def add_marker(self, marker: SegmentMarker): - self.markers.append(marker) - - def __str__(self): - # Define the print function, so it automatically formats things in the file properly - out_string = f"segment {self.name}\n" - if self.parent_name: - out_string += f"\tparent {self.parent_name}\n" - if self.segment_coordinate_system: - out_string += f"\tRT {self.segment_coordinate_system}\n" - if self.translations: - out_string += f"\ttranslations {self.translations}\n" - if self.rotations: - out_string += f"\trotations {self.rotations}\n" - if self.inertia_parameters: - out_string += str(self.inertia_parameters) - out_string += "endsegment\n" - - # Also print the markers attached to the segment - if self.markers: - for marker in self.markers: - out_string += str(marker) - return out_string From 2d92516f24d70de9d1bbc568fcbc52a93d289901 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Wed, 16 Nov 2022 09:51:00 -0500 Subject: [PATCH 20/30] blacked --- bionc/bionc_casadi/__init__.py | 2 +- bionc/bionc_casadi/biomechanical_model.py | 2 +- bionc/bionc_casadi/joint.py | 4 +-- bionc/bionc_casadi/natural_segment.py | 30 ++++++++++--------- bionc/bionc_numpy/__init__.py | 2 +- .../biomechanical_model_template.py | 2 ++ 6 files changed, 22 insertions(+), 20 deletions(-) diff --git a/bionc/bionc_casadi/__init__.py b/bionc/bionc_casadi/__init__.py index 8a643570..ff7c9eb3 100644 --- a/bionc/bionc_casadi/__init__.py +++ b/bionc/bionc_casadi/__init__.py @@ -11,4 +11,4 @@ from .natural_axis import Axis from .natural_marker import SegmentMarker, Marker from .natural_segment import NaturalSegment -from .inertia_parameters import InertiaParameters \ No newline at end of file +from .inertia_parameters import InertiaParameters diff --git a/bionc/bionc_casadi/biomechanical_model.py b/bionc/bionc_casadi/biomechanical_model.py index 9554badb..633d2aff 100644 --- a/bionc/bionc_casadi/biomechanical_model.py +++ b/bionc/bionc_casadi/biomechanical_model.py @@ -54,7 +54,7 @@ def nb_Qdot(self): def nb_Qddot(self): return 12 * self.nb_segments() - def rigid_body_constraints(self, Q: NaturalCoordinates) ->MX: + def rigid_body_constraints(self, Q: NaturalCoordinates) -> MX: """ This function returns the rigid body constraints of all segments, denoted Phi_r as a function of the natural coordinates Q. diff --git a/bionc/bionc_casadi/joint.py b/bionc/bionc_casadi/joint.py index d4bcc12b..c07d9355 100644 --- a/bionc/bionc_casadi/joint.py +++ b/bionc/bionc_casadi/joint.py @@ -50,9 +50,7 @@ def constraints(self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNatur """ @abstractmethod - def constraintJacobians( - self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNaturalCoordinates - ) -> MX: + def constraintJacobians(self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNaturalCoordinates) -> MX: """ This function returns the constraint Jacobians of the joint, denoted K_k as a function of the natural coordinates Q_parent and Q_child. diff --git a/bionc/bionc_casadi/natural_segment.py b/bionc/bionc_casadi/natural_segment.py index 2417052f..429ddafe 100644 --- a/bionc/bionc_casadi/natural_segment.py +++ b/bionc/bionc_casadi/natural_segment.py @@ -220,21 +220,23 @@ def _transformation_matrix(self) -> MX: MX Transformation matrix from natural coordinate to segment coordinate system [3x3] """ - return MX(np.array( - [ - [1, 0, 0], - [self.length * cos(self.gamma), self.length * sin(self.gamma), 0], + return MX( + np.array( [ - cos(self.beta), - (cos(self.alpha) - cos(self.beta) * cos(self.gamma)) / sin(self.beta), - np.sqrt( - 1 - - cos(self.beta) ** 2 - - (cos(self.alpha) - cos(self.beta) * cos(self.gamma)) / sin(self.beta) ** 2 - ), - ], - ] - )) + [1, 0, 0], + [self.length * cos(self.gamma), self.length * sin(self.gamma), 0], + [ + cos(self.beta), + (cos(self.alpha) - cos(self.beta) * cos(self.gamma)) / sin(self.beta), + np.sqrt( + 1 + - cos(self.beta) ** 2 + - (cos(self.alpha) - cos(self.beta) * cos(self.gamma)) / sin(self.beta) ** 2 + ), + ], + ] + ) + ) @property def transformation_matrix(self) -> MX: diff --git a/bionc/bionc_numpy/__init__.py b/bionc/bionc_numpy/__init__.py index 4dd71609..95c0976d 100644 --- a/bionc/bionc_numpy/__init__.py +++ b/bionc/bionc_numpy/__init__.py @@ -11,4 +11,4 @@ from .natural_axis import Axis from .natural_marker import SegmentMarker, Marker from .natural_segment import NaturalSegment -from .inertia_parameters import InertiaParameters \ No newline at end of file +from .inertia_parameters import InertiaParameters diff --git a/bionc/model_creation/biomechanical_model_template.py b/bionc/model_creation/biomechanical_model_template.py index c3640ae6..100584fb 100644 --- a/bionc/model_creation/biomechanical_model_template.py +++ b/bionc/model_creation/biomechanical_model_template.py @@ -1,6 +1,8 @@ from .protocols import Data + # from ..model_computations.segment import Segment from .segment_template import SegmentTemplate + # from ..model_computations.natural_segment import NaturalSegment from bionc.bionc_numpy.biomechanical_model import BiomechanicalModel From 1d07947af0336f15983eac6f37a9284cfffb9f68 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Wed, 16 Nov 2022 14:15:32 -0500 Subject: [PATCH 21/30] testing --- bionc/bionc_casadi/natural_segment.py | 26 +++--- tests/test_natural_segment_casadi.py | 97 ++++++++++++++++++++ tests/test_natural_velocities.py | 116 ----------------------- tests/test_natural_velocities_casadi.py | 117 ++++++++++++++++++++++++ 4 files changed, 225 insertions(+), 131 deletions(-) create mode 100644 tests/test_natural_segment_casadi.py create mode 100644 tests/test_natural_velocities_casadi.py diff --git a/bionc/bionc_casadi/natural_segment.py b/bionc/bionc_casadi/natural_segment.py index 429ddafe..d71b52fc 100644 --- a/bionc/bionc_casadi/natural_segment.py +++ b/bionc/bionc_casadi/natural_segment.py @@ -2,7 +2,7 @@ import numpy as np from casadi import MX -from casadi import cos, sin, transpose, norm_2, vertcat, horzcat, mtimes, dot, cross, vertsplit, horzsplit, reshape +from casadi import cos, sin, transpose, norm_2, vertcat, sqrt from numpy.linalg import inv from ..protocols.natural_coordinates import SegmentNaturalCoordinates, NaturalCoordinates @@ -220,23 +220,19 @@ def _transformation_matrix(self) -> MX: MX Transformation matrix from natural coordinate to segment coordinate system [3x3] """ - return MX( - np.array( - [ - [1, 0, 0], - [self.length * cos(self.gamma), self.length * sin(self.gamma), 0], - [ - cos(self.beta), - (cos(self.alpha) - cos(self.beta) * cos(self.gamma)) / sin(self.beta), - np.sqrt( + B = MX.zeros(3, 3) + B[0, :] = MX([1, 0, 0]) + B[1, 0] = self.length * cos(self.gamma) + B[1, 1] = self.length * sin(self.gamma) + B[1, 2] = 0 + B[2, 0] = cos(self.beta) + B[2, 1] = (cos(self.alpha) - cos(self.beta) * cos(self.gamma)) / sin(self.beta) + B[2, 2] = sqrt( 1 - cos(self.beta) ** 2 - (cos(self.alpha) - cos(self.beta) * cos(self.gamma)) / sin(self.beta) ** 2 - ), - ], - ] - ) - ) + ) + return B @property def transformation_matrix(self) -> MX: diff --git a/tests/test_natural_segment_casadi.py b/tests/test_natural_segment_casadi.py new file mode 100644 index 00000000..e2f74d8c --- /dev/null +++ b/tests/test_natural_segment_casadi.py @@ -0,0 +1,97 @@ +from bionc.bionc_casadi import NaturalSegment, SegmentMarker, SegmentNaturalCoordinates +import numpy as np +import pytest + + +def test_natural_segment_casadi(): + # Let's create a segment + my_segment = NaturalSegment( + name="box", + alpha=np.pi / 2, + beta=np.pi / 2, + gamma=np.pi / 2, + 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]]), + ) + # test the name of the segment + assert my_segment.name == "box" + np.testing.assert_equal(my_segment.alpha, np.pi / 2) + np.testing.assert_equal(my_segment.beta, np.pi / 2) + np.testing.assert_equal(my_segment.gamma, np.pi / 2) + np.testing.assert_equal(my_segment.length, 1) + np.testing.assert_equal(my_segment.mass, 1) + np.testing.assert_equal(my_segment.center_of_mass, np.array([0, 0.01, 0])) + np.testing.assert_equal(my_segment.inertia, np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]])) + + +def test_marker_features_casadi(): + # Let's create a segment + my_segment = NaturalSegment( + name="Thigh", + alpha=np.pi / 2, + beta=np.pi / 2, + gamma=np.pi / 2, + 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]]), + ) + + # Let's add a marker + marker1 = SegmentMarker( + name="my_marker1", + parent_name="Thigh", + position=np.eye(3), + is_technical=True, + is_anatomical=False, + ) + marker2 = SegmentMarker( + name="my_marker2", + parent_name="Thigh", + position=np.array([0, 1, 2]), + is_technical=True, + is_anatomical=False, + ) + my_segment.add_marker(marker1) + my_segment.add_marker(marker2) + + Qi = SegmentNaturalCoordinates.from_components( + u=[1, 2, 3], + rp=[1, 1, 3], + rd=[1, 2, 4], + w=[1, 2, 5], + ) + + np.testing.assert_array_equal(my_segment.nb_markers(), 2) + np.testing.assert_array_equal( + my_segment.marker_constraints( + marker_locations=np.array([[1, 2, 3], [1, 2, 3]]).T, + Qi=Qi, + ), + np.array([[-1, 2, -5], [-2, -2, -9]]).T, + ) + + with pytest.raises( + ValueError, + # match=f"marker_locations should be of shape (3, {my_segment.nb_markers()})" # don't know why this doesn't work + ): + my_segment.marker_constraints( + marker_locations=np.array([[1, 2, 3]]).T, + Qi=Qi, + ) + + np.testing.assert_array_equal( + my_segment.marker_jacobian(), + np.array( + [ + [-1.0, -0.0, -0.0, -1.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, -2.0, -0.0, 0.0, 1.0, 0.0, -0.0, -0.0, -0.0], + [-0.0, -0.0, -0.0, -0.0, -0.0, -1.0, 0.0, 0.0, 0.0, -0.0, -0.0, -1.0], + [-0.0, -0.0, -0.0, -2.0, -0.0, -0.0, 1.0, 0.0, 0.0, -2.0, -0.0, -0.0], + [-0.0, -0.0, -0.0, -0.0, -2.0, -0.0, 0.0, 1.0, 0.0, -0.0, -2.0, -0.0], + [-0.0, -0.0, -0.0, -0.0, -0.0, -2.0, 0.0, 0.0, 1.0, -0.0, -0.0, -2.0], + ] + ), + ) diff --git a/tests/test_natural_velocities.py b/tests/test_natural_velocities.py index 260dd264..740045e6 100644 --- a/tests/test_natural_velocities.py +++ b/tests/test_natural_velocities.py @@ -1,9 +1,6 @@ from bionc import bionc_numpy as bionc_np -from bionc import bionc_casadi as bionc_mx import numpy as np -from casadi import MX import pytest -from .utils import TestUtils def test_natural_velocities_numpy(): @@ -122,116 +119,3 @@ def test_natural_velocities_numpy(): qdot = np.concatenate((qdot1, qdot2), axis=0) with pytest.raises(AttributeError, match="'numpy.ndarray' object has no attribute 'udot'"): qdot.udot - - -def test_natural_velocities_casadi(): - # ------------------------------------------------------------------------------------------------------------------- - # SegmentNaturalVelocities - # ------------------------------------------------------------------------------------------------------------------ - # List instead of np array to test the translation from list to np.array - correct_vector = [1, 0, 0] - wrong_vector = [1, 0] - # Test wrong entry - # ------------------------------------------------------------------------------------------------------------------ - # test None udot - with pytest.raises(ValueError, match="u must be a array .* or a list of 3 elements"): - bionc_mx.SegmentNaturalVelocities.from_components(None, correct_vector, correct_vector, correct_vector) - - # test wrong vector udot - with pytest.raises(ValueError, match="u must be a 3x1 array"): - bionc_mx.SegmentNaturalVelocities.from_components(wrong_vector, correct_vector, correct_vector, correct_vector) - - # test None rpdot - with pytest.raises(ValueError, match="rp must be a array .* or a list of 3 elements"): - bionc_mx.SegmentNaturalVelocities.from_components(correct_vector, None, correct_vector, correct_vector) - - # test wrong vector rpdot - with pytest.raises(ValueError, match="rp must be a 3x1 array"): - bionc_mx.SegmentNaturalVelocities.from_components(correct_vector, wrong_vector, correct_vector, correct_vector) - - # test None rddot - with pytest.raises(ValueError, match="rd must be a array .* or a list of 3 elements"): - bionc_mx.SegmentNaturalVelocities.from_components(correct_vector, correct_vector, None, correct_vector) - - # test wrong vector rddot - with pytest.raises(ValueError, match="rd must be a 3x1 array"): - bionc_mx.SegmentNaturalVelocities.from_components(correct_vector, correct_vector, wrong_vector, correct_vector) - - # test None wdot - with pytest.raises(ValueError, match="w must be a array .* or a list of 3 elements"): - bionc_mx.SegmentNaturalVelocities.from_components(correct_vector, correct_vector, correct_vector, None) - - # test wrong vector wdot - with pytest.raises(ValueError, match="v must be a 3x1 array"): - bionc_mx.SegmentNaturalVelocities.from_components(correct_vector, correct_vector, correct_vector, wrong_vector) - - # Test concatenate + parameters - qdot_test = bionc_mx.SegmentNaturalVelocities.from_components( - udot=[1, 0, 0], - rpdot=[2, 0, 0], - rddot=[3, 0, 0], - wdot=[4, 0, 0], - ) - - assert np.all(TestUtils.mx_to_array(qdot_test) == np.array([1, 0, 0, 2, 0, 0, 3, 0, 0, 4, 0, 0])) - assert np.all(TestUtils.mx_to_array(qdot_test.udot) == np.array([1, 0, 0])) - assert np.all(TestUtils.mx_to_array(qdot_test.rpdot) == np.array([2, 0, 0])) - assert np.all(TestUtils.mx_to_array(qdot_test.rddot) == np.array([3, 0, 0])) - assert np.all(TestUtils.mx_to_array(qdot_test.wdot) == np.array([4, 0, 0])) - - # vdot = rpdot - rddot - assert np.all(TestUtils.mx_to_array(qdot_test.vdot) == np.array([-1, 0, 0])) - - # vectors - assert np.all(TestUtils.mx_to_array(qdot_test.vector) == np.array([1, 0, 0, 2, 0, 0, 3, 0, 0, 4, 0, 0])) - - for ind, component in enumerate(qdot_test.to_components): - assert np.all(TestUtils.mx_to_array(component) == np.array([ind + 1, 0, 0])) - - # ------------------------------------------------------------------------------------------------------------------- - # NaturalVelocities - # ------------------------------------------------------------------------------------------------------------------ - qdot1 = bionc_mx.SegmentNaturalVelocities.from_components( - udot=np.array([1, 2, 3]), - wdot=np.array([4, 5, 6]), - rddot=np.array([7, 8, 9]), - rpdot=np.array([10, 11, 12]), - ) - qdot2 = bionc_mx.SegmentNaturalVelocities.from_components( - udot=np.array([11, 22, 33]), - wdot=np.array([4, 5, 6]), - rddot=np.array([7, 82, 9]), - rpdot=np.array([110, 11, 12]), - ) - - # Wrong entry - with pytest.raises(ValueError, match="tuple_of_Q must be a tuple of SegmentNaturalVelocities"): - bionc_mx.NaturalVelocities.from_qdoti(1) - - # One wrong entry in the list - with pytest.raises(ValueError, match="tuple_of_Q must be a tuple of SegmentNaturalVelocities"): - bionc_mx.NaturalVelocities.from_qdoti((qdot1, qdot2, [0, 0])) - - qdot = bionc_mx.NaturalVelocities.from_qdoti((qdot1, qdot2)) - - # - np.testing.assert_equal(TestUtils.mx_to_array(qdot.udot(0)), np.array([1, 2, 3])) - np.testing.assert_equal(TestUtils.mx_to_array(qdot.udot(1)), np.array([11, 22, 33])) - - # test nb_qdoti - np.testing.assert_equal(qdot.nb_qdoti(), 2) - - # test of the different component of vector - np.testing.assert_equal(TestUtils.mx_to_array(qdot1.vector), TestUtils.mx_to_array(qdot1)) - np.testing.assert_equal(TestUtils.mx_to_array(qdot.vector(0)), TestUtils.mx_to_array(qdot1)) - np.testing.assert_equal(TestUtils.mx_to_array(qdot.vector(1)), TestUtils.mx_to_array(qdot2)) - np.testing.assert_equal(TestUtils.mx_to_array(qdot.vector(0).udot), np.array([1, 2, 3])) - np.testing.assert_equal(TestUtils.mx_to_array(qdot.vector(1).udot), np.array([11, 22, 33])) - np.testing.assert_equal(TestUtils.mx_to_array(qdot.vector(0).wdot), np.array([4, 5, 6])) - np.testing.assert_equal(TestUtils.mx_to_array(qdot.vector(1).wdot), np.array([4, 5, 6])) - np.testing.assert_equal(TestUtils.mx_to_array(qdot.vector(0).rddot), np.array([7, 8, 9])) - np.testing.assert_equal(TestUtils.mx_to_array(qdot.vector(1).rddot), np.array([7, 82, 9])) - np.testing.assert_equal(TestUtils.mx_to_array(qdot.vector(0).rpdot), np.array([10, 11, 12])) - np.testing.assert_equal(TestUtils.mx_to_array(qdot.vector(1).rpdot), np.array([110, 11, 12])) - np.testing.assert_equal(TestUtils.mx_to_array(qdot.vector(0).vdot), np.array([3, 3, 3])) - np.testing.assert_equal(TestUtils.mx_to_array(qdot.vector(1).vdot), np.array([103, -71, 3])) diff --git a/tests/test_natural_velocities_casadi.py b/tests/test_natural_velocities_casadi.py new file mode 100644 index 00000000..9890ff59 --- /dev/null +++ b/tests/test_natural_velocities_casadi.py @@ -0,0 +1,117 @@ +from bionc import bionc_casadi as bionc_mx +import numpy as np +import pytest +from .utils import TestUtils + + +def test_natural_velocities_casadi(): + # ------------------------------------------------------------------------------------------------------------------- + # SegmentNaturalVelocities + # ------------------------------------------------------------------------------------------------------------------ + # List instead of np array to test the translation from list to np.array + correct_vector = [1, 0, 0] + wrong_vector = [1, 0] + # Test wrong entry + # ------------------------------------------------------------------------------------------------------------------ + # test None udot + with pytest.raises(ValueError, match="u must be a array .* or a list of 3 elements"): + bionc_mx.SegmentNaturalVelocities.from_components(None, correct_vector, correct_vector, correct_vector) + + # test wrong vector udot + with pytest.raises(ValueError, match="u must be a 3x1 array"): + bionc_mx.SegmentNaturalVelocities.from_components(wrong_vector, correct_vector, correct_vector, correct_vector) + + # test None rpdot + with pytest.raises(ValueError, match="rp must be a array .* or a list of 3 elements"): + bionc_mx.SegmentNaturalVelocities.from_components(correct_vector, None, correct_vector, correct_vector) + + # test wrong vector rpdot + with pytest.raises(ValueError, match="rp must be a 3x1 array"): + bionc_mx.SegmentNaturalVelocities.from_components(correct_vector, wrong_vector, correct_vector, correct_vector) + + # test None rddot + with pytest.raises(ValueError, match="rd must be a array .* or a list of 3 elements"): + bionc_mx.SegmentNaturalVelocities.from_components(correct_vector, correct_vector, None, correct_vector) + + # test wrong vector rddot + with pytest.raises(ValueError, match="rd must be a 3x1 array"): + bionc_mx.SegmentNaturalVelocities.from_components(correct_vector, correct_vector, wrong_vector, correct_vector) + + # test None wdot + with pytest.raises(ValueError, match="w must be a array .* or a list of 3 elements"): + bionc_mx.SegmentNaturalVelocities.from_components(correct_vector, correct_vector, correct_vector, None) + + # test wrong vector wdot + with pytest.raises(ValueError, match="v must be a 3x1 array"): + bionc_mx.SegmentNaturalVelocities.from_components(correct_vector, correct_vector, correct_vector, wrong_vector) + + # Test concatenate + parameters + qdot_test = bionc_mx.SegmentNaturalVelocities.from_components( + udot=[1, 0, 0], + rpdot=[2, 0, 0], + rddot=[3, 0, 0], + wdot=[4, 0, 0], + ) + + assert np.all(TestUtils.mx_to_array(qdot_test) == np.array([1, 0, 0, 2, 0, 0, 3, 0, 0, 4, 0, 0])) + assert np.all(TestUtils.mx_to_array(qdot_test.udot) == np.array([1, 0, 0])) + assert np.all(TestUtils.mx_to_array(qdot_test.rpdot) == np.array([2, 0, 0])) + assert np.all(TestUtils.mx_to_array(qdot_test.rddot) == np.array([3, 0, 0])) + assert np.all(TestUtils.mx_to_array(qdot_test.wdot) == np.array([4, 0, 0])) + + # vdot = rpdot - rddot + assert np.all(TestUtils.mx_to_array(qdot_test.vdot) == np.array([-1, 0, 0])) + + # vectors + assert np.all(TestUtils.mx_to_array(qdot_test.vector) == np.array([1, 0, 0, 2, 0, 0, 3, 0, 0, 4, 0, 0])) + + for ind, component in enumerate(qdot_test.to_components): + assert np.all(TestUtils.mx_to_array(component) == np.array([ind + 1, 0, 0])) + + # ------------------------------------------------------------------------------------------------------------------- + # NaturalVelocities + # ------------------------------------------------------------------------------------------------------------------ + qdot1 = bionc_mx.SegmentNaturalVelocities.from_components( + udot=np.array([1, 2, 3]), + wdot=np.array([4, 5, 6]), + rddot=np.array([7, 8, 9]), + rpdot=np.array([10, 11, 12]), + ) + qdot2 = bionc_mx.SegmentNaturalVelocities.from_components( + udot=np.array([11, 22, 33]), + wdot=np.array([4, 5, 6]), + rddot=np.array([7, 82, 9]), + rpdot=np.array([110, 11, 12]), + ) + + # Wrong entry + with pytest.raises(ValueError, match="tuple_of_Q must be a tuple of SegmentNaturalVelocities"): + bionc_mx.NaturalVelocities.from_qdoti(1) + + # One wrong entry in the list + with pytest.raises(ValueError, match="tuple_of_Q must be a tuple of SegmentNaturalVelocities"): + bionc_mx.NaturalVelocities.from_qdoti((qdot1, qdot2, [0, 0])) + + qdot = bionc_mx.NaturalVelocities.from_qdoti((qdot1, qdot2)) + + # + np.testing.assert_equal(TestUtils.mx_to_array(qdot.udot(0)), np.array([1, 2, 3])) + np.testing.assert_equal(TestUtils.mx_to_array(qdot.udot(1)), np.array([11, 22, 33])) + + # test nb_qdoti + np.testing.assert_equal(qdot.nb_qdoti(), 2) + + # test of the different component of vector + np.testing.assert_equal(TestUtils.mx_to_array(qdot1.vector), TestUtils.mx_to_array(qdot1)) + np.testing.assert_equal(TestUtils.mx_to_array(qdot.vector(0)), TestUtils.mx_to_array(qdot1)) + np.testing.assert_equal(TestUtils.mx_to_array(qdot.vector(1)), TestUtils.mx_to_array(qdot2)) + np.testing.assert_equal(TestUtils.mx_to_array(qdot.vector(0).udot), np.array([1, 2, 3])) + np.testing.assert_equal(TestUtils.mx_to_array(qdot.vector(1).udot), np.array([11, 22, 33])) + np.testing.assert_equal(TestUtils.mx_to_array(qdot.vector(0).wdot), np.array([4, 5, 6])) + np.testing.assert_equal(TestUtils.mx_to_array(qdot.vector(1).wdot), np.array([4, 5, 6])) + np.testing.assert_equal(TestUtils.mx_to_array(qdot.vector(0).rddot), np.array([7, 8, 9])) + np.testing.assert_equal(TestUtils.mx_to_array(qdot.vector(1).rddot), np.array([7, 82, 9])) + np.testing.assert_equal(TestUtils.mx_to_array(qdot.vector(0).rpdot), np.array([10, 11, 12])) + np.testing.assert_equal(TestUtils.mx_to_array(qdot.vector(1).rpdot), np.array([110, 11, 12])) + np.testing.assert_equal(TestUtils.mx_to_array(qdot.vector(0).vdot), np.array([3, 3, 3])) + np.testing.assert_equal(TestUtils.mx_to_array(qdot.vector(1).vdot), np.array([103, -71, 3])) \ No newline at end of file From f33208658b0e78a27d2adc03c6b0c67ab2805604 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Wed, 16 Nov 2022 14:55:41 -0500 Subject: [PATCH 22/30] bug fix with natural segment and interpolation matrix works with casadi --- bionc/__init__.py | 2 +- bionc/bionc_casadi/interpolation_matrix.py | 61 +++++++++++++++++++ bionc/bionc_casadi/natural_marker.py | 13 ++-- bionc/bionc_casadi/natural_segment.py | 19 +++--- .../interpolation_matrix.py | 0 bionc/bionc_numpy/natural_coordinates.py | 2 +- bionc/bionc_numpy/natural_marker.py | 7 ++- .../biomechanical_model_template.py | 3 - tests/test_natural_segment.py | 20 ++++-- tests/test_natural_segment_casadi.py | 38 +++++++----- tests/test_natural_velocities_casadi.py | 2 +- tests/utils.py | 8 +++ 12 files changed, 131 insertions(+), 44 deletions(-) create mode 100644 bionc/bionc_casadi/interpolation_matrix.py rename bionc/{utils => bionc_numpy}/interpolation_matrix.py (100%) diff --git a/bionc/__init__.py b/bionc/__init__.py index 4c1713c1..3e02d46f 100644 --- a/bionc/__init__.py +++ b/bionc/__init__.py @@ -1,4 +1,4 @@ -from .utils.interpolation_matrix import interpolate_natural_vector, to_natural_vector +# from bionc.bionc_numpy.interpolation_matrix import interpolate_natural_vector, to_natural_vector from .utils.vnop_array import vnop_array from .model_creation import ( diff --git a/bionc/bionc_casadi/interpolation_matrix.py b/bionc/bionc_casadi/interpolation_matrix.py new file mode 100644 index 00000000..19df11c8 --- /dev/null +++ b/bionc/bionc_casadi/interpolation_matrix.py @@ -0,0 +1,61 @@ +from casadi import MX +import numpy as np + + +def interpolate_natural_vector(vector: np.ndarray | MX) -> MX: + """ + This function converts a vector expressed in a non-orthogonal coordinate system + to an interpolation matrix, denoted Ni, such as: + Ni * Qi -> location in the global frame + + Parameters + ---------- + vector : np.ndarray + Vector in the natural coordinate system to interpolate (Pi, ui, vi, wi) + + Returns + ------- + interpolate_natural_vector: MX + Interpolation matrix [3 x 12], denoted Ni to get the location of the vector as linear combination of Q. + vector in global frame = Ni * Qi + """ + + if vector.shape[0] != 3: + raise ValueError("Vector must be 3x1") + + interpolation_matrix = MX.zeros((3, 12)) + interpolation_matrix[0:3, 0:3] = vector[0] * MX.eye(3) + interpolation_matrix[0:3, 3:6] = (1 + vector[1]) * MX.eye(3) + interpolation_matrix[0:3, 6:9] = -vector[1] * MX.eye(3) + interpolation_matrix[0:3, 9:12] = vector[2] * MX.eye(3) + + return interpolation_matrix + + +def to_natural_vector(interpolation_matrix: np.ndarray | MX) -> MX: + """ + This function converts an interpolation matrix, denoted Ni, such as: + Ni * Qi -> location in the global frame + to a vector expressed in a non-orthogonal coordinate system associated to the segment coordinates. + + Parameters + ---------- + interpolation_matrix: np.ndarray + Interpolation matrix [3 x 12], denoted Ni to get the location of the vector as linear combination of Q. + vector in global frame = Ni * Qi + + Returns + ------- + MX + Vector in the natural coordinate system to interpolate (Pi, ui, vi, wi) + """ + + if interpolation_matrix.shape != (3, 12): + raise ValueError("Interpolation matrix must be 3x12") + + vector = MX.zeros(3) + vector[0] = interpolation_matrix[0, 0] + vector[1] = interpolation_matrix[0, 3] - 1 + vector[2] = interpolation_matrix[0, 9] + + return vector diff --git a/bionc/bionc_casadi/natural_marker.py b/bionc/bionc_casadi/natural_marker.py index 516f5440..f2209c32 100644 --- a/bionc/bionc_casadi/natural_marker.py +++ b/bionc/bionc_casadi/natural_marker.py @@ -1,11 +1,11 @@ from typing import Callable import numpy as np -from casadi import MX, vertcat +from casadi import MX, vertcat, horzcat from .biomechanical_model import BiomechanicalModel from bionc.model_creation.protocols import Data -from bionc.utils.interpolation_matrix import interpolate_natural_vector, to_natural_vector +from bionc.bionc_casadi.interpolation_matrix import interpolate_natural_vector, to_natural_vector # from ..utils.natural_coordinates import SegmentNaturalCoordinates from bionc.protocols.natural_coordinates import SegmentNaturalCoordinates @@ -74,15 +74,18 @@ def __init__( elif position is not None and interpolation_matrix is None: if position.shape[0] != 3: raise ValueError("The position must be a 3d vector") + if position.shape.__len__() > 1: + if position.shape[1] != 1: + raise ValueError("The position must be a 3d vector with only one column") - self.position = position if isinstance(position, np.ndarray) else np.array(position) + self.position = MX(position if isinstance(position, np.ndarray) else np.array(position)) self.interpolation_matrix = interpolate_natural_vector(self.position) elif position is None and interpolation_matrix is not None: if interpolation_matrix.shape != (3, 12): raise ValueError("The interpolation matrix must be a 3x12 matrix") - self.interpolation_matrix = interpolation_matrix + self.interpolation_matrix = MX(interpolation_matrix) self.position = to_natural_vector(self.interpolation_matrix) else: @@ -239,7 +242,7 @@ def __init__( if position.shape[0] != 4: if position.shape[0] == 3: # If the position is a 3d vector, add a 1 at the bottom - position = np.vstack((position, np.ones((1, position.shape[1])))) + position = vertcat((position, MX.ones((1, position.shape[1])))) else: raise ValueError("The position must be (XYZ x time) or (XYZ1 x time)") diff --git a/bionc/bionc_casadi/natural_segment.py b/bionc/bionc_casadi/natural_segment.py index d71b52fc..39795d5b 100644 --- a/bionc/bionc_casadi/natural_segment.py +++ b/bionc/bionc_casadi/natural_segment.py @@ -2,14 +2,13 @@ import numpy as np from casadi import MX -from casadi import cos, sin, transpose, norm_2, vertcat, sqrt -from numpy.linalg import inv +from casadi import cos, sin, transpose, norm_2, vertcat, sqrt, inv, dot, tril, tril2symm from ..protocols.natural_coordinates import SegmentNaturalCoordinates, NaturalCoordinates from ..bionc_casadi.natural_velocities import SegmentNaturalVelocities, NaturalVelocities from ..bionc_casadi.natural_accelerations import SegmentNaturalAccelerations, NaturalAccelerations from ..bionc_casadi.homogenous_transform import HomogeneousTransform -from ..bionc_numpy.natural_marker import SegmentMarker +from ..bionc_casadi.natural_marker import SegmentMarker class NaturalSegment: @@ -228,10 +227,8 @@ def _transformation_matrix(self) -> MX: B[2, 0] = cos(self.beta) B[2, 1] = (cos(self.alpha) - cos(self.beta) * cos(self.gamma)) / sin(self.beta) B[2, 2] = sqrt( - 1 - - cos(self.beta) ** 2 - - (cos(self.alpha) - cos(self.beta) * cos(self.gamma)) / sin(self.beta) ** 2 - ) + 1 - cos(self.beta) ** 2 - (cos(self.alpha) - cos(self.beta) * cos(self.gamma)) / sin(self.beta) ** 2 + ) return B @property @@ -398,8 +395,8 @@ def _pseudo_inertia_matrix(self) -> MX: # todo: verify the formula middle_block = ( self.inertia - + self.mass * np.dot(self.center_of_mass.T, self.center_of_mass) * MX.eye(3) - - np.dot(self.center_of_mass.T, self.center_of_mass) + + self.mass * dot(self.center_of_mass, self.center_of_mass) * MX.eye(3) + - dot(self.center_of_mass, self.center_of_mass) ) Binv = inv(self.transformation_matrix) @@ -472,7 +469,7 @@ def _update_mass_matrix(self) -> MX: Gi[9:12, 9:12] = Ji[2, 2] * MX.eye(3) # symmetrize the matrix - Gi = np.tril(Gi) + np.tril(Gi, -1).T + Gi = tril2symm(tril(Gi)) return Gi @@ -669,4 +666,4 @@ def marker_jacobian(self): MX The jacobian of the marker constraints of the segment (3 x N_markers) """ - return vertcat([-marker.interpolation_matrix for marker in self._markers]) + return vertcat(*[-marker.interpolation_matrix for marker in self._markers]) diff --git a/bionc/utils/interpolation_matrix.py b/bionc/bionc_numpy/interpolation_matrix.py similarity index 100% rename from bionc/utils/interpolation_matrix.py rename to bionc/bionc_numpy/interpolation_matrix.py diff --git a/bionc/bionc_numpy/natural_coordinates.py b/bionc/bionc_numpy/natural_coordinates.py index bb80daf0..99debd1a 100644 --- a/bionc/bionc_numpy/natural_coordinates.py +++ b/bionc/bionc_numpy/natural_coordinates.py @@ -1,7 +1,7 @@ import numpy as np from typing import Union from bionc.utils.vnop_array import vnop_array -from bionc.utils.interpolation_matrix import interpolate_natural_vector +from bionc.bionc_numpy.interpolation_matrix import interpolate_natural_vector class SegmentNaturalCoordinates(np.ndarray): diff --git a/bionc/bionc_numpy/natural_marker.py b/bionc/bionc_numpy/natural_marker.py index e61071d3..1c308f55 100644 --- a/bionc/bionc_numpy/natural_marker.py +++ b/bionc/bionc_numpy/natural_marker.py @@ -4,9 +4,7 @@ from .biomechanical_model import BiomechanicalModel from bionc.model_creation.protocols import Data -from bionc.utils.interpolation_matrix import interpolate_natural_vector, to_natural_vector - -# from ..utils.natural_coordinates import SegmentNaturalCoordinates +from bionc.bionc_numpy.interpolation_matrix import interpolate_natural_vector, to_natural_vector from bionc.protocols.natural_coordinates import SegmentNaturalCoordinates # todo: need a list of markers MarkerList @@ -73,6 +71,9 @@ def __init__( elif position is not None and interpolation_matrix is None: if position.shape[0] != 3: raise ValueError("The position must be a 3d vector") + if position.shape.__len__() > 1: + if position.shape[1] != 1: + raise ValueError("The position must be a 3d vector with only one column") self.position = position if isinstance(position, np.ndarray) else np.array(position) self.interpolation_matrix = interpolate_natural_vector(self.position) diff --git a/bionc/model_creation/biomechanical_model_template.py b/bionc/model_creation/biomechanical_model_template.py index 100584fb..70d8d836 100644 --- a/bionc/model_creation/biomechanical_model_template.py +++ b/bionc/model_creation/biomechanical_model_template.py @@ -1,9 +1,6 @@ from .protocols import Data -# from ..model_computations.segment import Segment from .segment_template import SegmentTemplate - -# from ..model_computations.natural_segment import NaturalSegment from bionc.bionc_numpy.biomechanical_model import BiomechanicalModel diff --git a/tests/test_natural_segment.py b/tests/test_natural_segment.py index a87d438f..75e1ac10 100644 --- a/tests/test_natural_segment.py +++ b/tests/test_natural_segment.py @@ -44,13 +44,23 @@ def test_marker_features(): ) # Let's add a marker + with pytest.raises(ValueError, match="The position must be a 3d vector with only one column"): + SegmentMarker( + name="my_marker1", + parent_name="Thigh", + position=np.eye(3), + is_technical=True, + is_anatomical=False, + ) + marker1 = SegmentMarker( name="my_marker1", parent_name="Thigh", - position=np.eye(3), + position=np.ones(3), is_technical=True, is_anatomical=False, ) + marker2 = SegmentMarker( name="my_marker2", parent_name="Thigh", @@ -74,7 +84,7 @@ def test_marker_features(): marker_locations=np.array([[1, 2, 3], [1, 2, 3]]).T, Qi=Qi, ), - np.array([[-1, 2, -5], [-2, -2, -9]]).T, + np.array([[-2, -2, -7], [-2, -2, -9]]).T, ) with pytest.raises( @@ -90,9 +100,9 @@ def test_marker_features(): my_segment.marker_jacobian(), np.array( [ - [-1.0, -0.0, -0.0, -1.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, -2.0, -0.0, 0.0, 1.0, 0.0, -0.0, -0.0, -0.0], - [-0.0, -0.0, -0.0, -0.0, -0.0, -1.0, 0.0, 0.0, 0.0, -0.0, -0.0, -1.0], + [-1.0, -0.0, -0.0, -2.0, -0.0, -0.0, 1.0, 0.0, 0.0, -1.0, -0.0, -0.0], + [-0.0, -1.0, -0.0, -0.0, -2.0, -0.0, 0.0, 1.0, 0.0, -0.0, -1.0, -0.0], + [-0.0, -0.0, -1.0, -0.0, -0.0, -2.0, 0.0, 0.0, 1.0, -0.0, -0.0, -1.0], [-0.0, -0.0, -0.0, -2.0, -0.0, -0.0, 1.0, 0.0, 0.0, -2.0, -0.0, -0.0], [-0.0, -0.0, -0.0, -0.0, -2.0, -0.0, 0.0, 1.0, 0.0, -0.0, -2.0, -0.0], [-0.0, -0.0, -0.0, -0.0, -0.0, -2.0, 0.0, 0.0, 1.0, -0.0, -0.0, -2.0], diff --git a/tests/test_natural_segment_casadi.py b/tests/test_natural_segment_casadi.py index e2f74d8c..880f0ab1 100644 --- a/tests/test_natural_segment_casadi.py +++ b/tests/test_natural_segment_casadi.py @@ -1,6 +1,7 @@ from bionc.bionc_casadi import NaturalSegment, SegmentMarker, SegmentNaturalCoordinates import numpy as np import pytest +from .utils import TestUtils def test_natural_segment_casadi(): @@ -17,13 +18,13 @@ def test_natural_segment_casadi(): ) # test the name of the segment assert my_segment.name == "box" - np.testing.assert_equal(my_segment.alpha, np.pi / 2) - np.testing.assert_equal(my_segment.beta, np.pi / 2) - np.testing.assert_equal(my_segment.gamma, np.pi / 2) - np.testing.assert_equal(my_segment.length, 1) - np.testing.assert_equal(my_segment.mass, 1) - np.testing.assert_equal(my_segment.center_of_mass, np.array([0, 0.01, 0])) - np.testing.assert_equal(my_segment.inertia, np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]])) + TestUtils.mx_assert_equal(my_segment.alpha, np.pi / 2) + TestUtils.mx_assert_equal(my_segment.beta, np.pi / 2) + TestUtils.mx_assert_equal(my_segment.gamma, np.pi / 2) + TestUtils.mx_assert_equal(my_segment.length, 1) + TestUtils.mx_assert_equal(my_segment.mass, 1) + TestUtils.mx_assert_equal(my_segment.center_of_mass, np.array([0, 0.01, 0])) + TestUtils.mx_assert_equal(my_segment.inertia, np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]])) def test_marker_features_casadi(): @@ -40,10 +41,19 @@ def test_marker_features_casadi(): ) # Let's add a marker + with pytest.raises(ValueError, match="The position must be a 3d vector with only one column"): + SegmentMarker( + name="my_marker1", + parent_name="Thigh", + position=np.eye(3), + is_technical=True, + is_anatomical=False, + ) + marker1 = SegmentMarker( name="my_marker1", parent_name="Thigh", - position=np.eye(3), + position=np.ones(3), is_technical=True, is_anatomical=False, ) @@ -65,12 +75,12 @@ def test_marker_features_casadi(): ) np.testing.assert_array_equal(my_segment.nb_markers(), 2) - np.testing.assert_array_equal( + TestUtils.mx_assert_equal( my_segment.marker_constraints( marker_locations=np.array([[1, 2, 3], [1, 2, 3]]).T, Qi=Qi, ), - np.array([[-1, 2, -5], [-2, -2, -9]]).T, + np.array([[-2, -2, -7], [-2, -2, -9]]).T, ) with pytest.raises( @@ -82,13 +92,13 @@ def test_marker_features_casadi(): Qi=Qi, ) - np.testing.assert_array_equal( + TestUtils.mx_assert_equal( my_segment.marker_jacobian(), np.array( [ - [-1.0, -0.0, -0.0, -1.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, -2.0, -0.0, 0.0, 1.0, 0.0, -0.0, -0.0, -0.0], - [-0.0, -0.0, -0.0, -0.0, -0.0, -1.0, 0.0, 0.0, 0.0, -0.0, -0.0, -1.0], + [-1.0, -0.0, -0.0, -2.0, -0.0, -0.0, 1.0, 0.0, 0.0, -1.0, -0.0, -0.0], + [-0.0, -1.0, -0.0, -0.0, -2.0, -0.0, 0.0, 1.0, 0.0, -0.0, -1.0, -0.0], + [-0.0, -0.0, -1.0, -0.0, -0.0, -2.0, 0.0, 0.0, 1.0, -0.0, -0.0, -1.0], [-0.0, -0.0, -0.0, -2.0, -0.0, -0.0, 1.0, 0.0, 0.0, -2.0, -0.0, -0.0], [-0.0, -0.0, -0.0, -0.0, -2.0, -0.0, 0.0, 1.0, 0.0, -0.0, -2.0, -0.0], [-0.0, -0.0, -0.0, -0.0, -0.0, -2.0, 0.0, 0.0, 1.0, -0.0, -0.0, -2.0], diff --git a/tests/test_natural_velocities_casadi.py b/tests/test_natural_velocities_casadi.py index 9890ff59..b335fa53 100644 --- a/tests/test_natural_velocities_casadi.py +++ b/tests/test_natural_velocities_casadi.py @@ -114,4 +114,4 @@ def test_natural_velocities_casadi(): np.testing.assert_equal(TestUtils.mx_to_array(qdot.vector(0).rpdot), np.array([10, 11, 12])) np.testing.assert_equal(TestUtils.mx_to_array(qdot.vector(1).rpdot), np.array([110, 11, 12])) np.testing.assert_equal(TestUtils.mx_to_array(qdot.vector(0).vdot), np.array([3, 3, 3])) - np.testing.assert_equal(TestUtils.mx_to_array(qdot.vector(1).vdot), np.array([103, -71, 3])) \ No newline at end of file + np.testing.assert_equal(TestUtils.mx_to_array(qdot.vector(1).vdot), np.array([103, -71, 3])) diff --git a/tests/utils.py b/tests/utils.py index ab8039ea..ea82270a 100644 --- a/tests/utils.py +++ b/tests/utils.py @@ -2,6 +2,7 @@ from pathlib import Path import importlib.util from casadi import MX, Function +import numpy as np class TestUtils: @@ -47,3 +48,10 @@ def mx_to_array(mx: MX): .toarray() .squeeze() ) + + @staticmethod + def mx_assert_equal(mx: MX, expected: Any): + """ + Assert that a casadi MX is equal to a numpy array if it is only numeric values + """ + np.testing.assert_equal(TestUtils.mx_to_array(mx), expected) From 961f957434be0391981910249dce471c0d0dd51b Mon Sep 17 00:00:00 2001 From: Ipuch Date: Wed, 16 Nov 2022 16:11:36 -0500 Subject: [PATCH 23/30] blacked and tests ok --- bionc/bionc_casadi/__init__.py | 1 + bionc/bionc_numpy/__init__.py | 1 + tests/test_marker.py | 14 ++-- tests/test_marker_casadi.py | 90 ++++++++++++++++++++++++ tests/test_maths.py | 38 +++++++++- tests/test_natural_coordinates.py | 80 --------------------- tests/test_natural_coordinates_casadi.py | 90 ++++++++++++++++++++++++ tests/test_natural_segment.py | 4 +- 8 files changed, 228 insertions(+), 90 deletions(-) create mode 100644 tests/test_marker_casadi.py create mode 100644 tests/test_natural_coordinates_casadi.py diff --git a/bionc/bionc_casadi/__init__.py b/bionc/bionc_casadi/__init__.py index ff7c9eb3..8eb41857 100644 --- a/bionc/bionc_casadi/__init__.py +++ b/bionc/bionc_casadi/__init__.py @@ -12,3 +12,4 @@ from .natural_marker import SegmentMarker, Marker from .natural_segment import NaturalSegment from .inertia_parameters import InertiaParameters +from .interpolation_matrix import interpolate_natural_vector, to_natural_vector diff --git a/bionc/bionc_numpy/__init__.py b/bionc/bionc_numpy/__init__.py index 95c0976d..390e246d 100644 --- a/bionc/bionc_numpy/__init__.py +++ b/bionc/bionc_numpy/__init__.py @@ -12,3 +12,4 @@ from .natural_marker import SegmentMarker, Marker from .natural_segment import NaturalSegment from .inertia_parameters import InertiaParameters +from .interpolation_matrix import interpolate_natural_vector, to_natural_vector diff --git a/tests/test_marker.py b/tests/test_marker.py index 7d22fb58..af70b75b 100644 --- a/tests/test_marker.py +++ b/tests/test_marker.py @@ -50,19 +50,19 @@ def test_segment_marker(): segment_marker = SegmentMarker( name="my_marker", parent_name="Thigh", - position=np.eye(3), + position=np.ones(3), is_technical=True, is_anatomical=False, ) - np.testing.assert_array_equal(segment_marker.position, np.eye(3)) + np.testing.assert_array_equal(segment_marker.position, np.ones(3)) np.testing.assert_array_equal( segment_marker.interpolation_matrix, np.array( [ - [1.0, 0.0, 0.0, 1.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, 2.0, 0.0, -0.0, -1.0, -0.0, 0.0, 0.0, 0.0], - [0.0, 0.0, 0.0, 0.0, 0.0, 1.0, -0.0, -0.0, -0.0, 0.0, 0.0, 1.0], + [1.0, 0.0, 0.0, 2.0, 0.0, 0.0, -1.0, -0.0, -0.0, 1.0, 0.0, 0.0], + [0.0, 1.0, 0.0, 0.0, 2.0, 0.0, -0.0, -1.0, -0.0, 0.0, 1.0, 0.0], + [0.0, 0.0, 1.0, 0.0, 0.0, 2.0, -0.0, -0.0, -1.0, 0.0, 0.0, 1.0], ] ), ) @@ -78,10 +78,10 @@ def test_segment_marker(): ) constraint = segment_marker.constraint(marker_location=marker_location, Qi=Qi) - np.testing.assert_array_equal(constraint, np.array([-1, 2, -5])) + np.testing.assert_array_equal(constraint, np.array([-2, -2, -7])) constraint = segment_marker.constraint(marker_location=marker_location[:, np.newaxis], Qi=Qi) - np.testing.assert_array_equal(constraint, np.array([-1, 2, -5])) + np.testing.assert_array_equal(constraint, np.array([-2, -2, -7])) with pytest.raises(ValueError, match="The marker location must be a 3d vector"): segment_marker.constraint(marker_location=np.zeros(2), Qi=Qi) diff --git a/tests/test_marker_casadi.py b/tests/test_marker_casadi.py new file mode 100644 index 00000000..377dd354 --- /dev/null +++ b/tests/test_marker_casadi.py @@ -0,0 +1,90 @@ +import pytest +import numpy as np + +from bionc.bionc_casadi import SegmentMarker, SegmentNaturalCoordinates +from .utils import TestUtils + + +def test_segment_marker(): + + with pytest.raises(ValueError, match="Either a position or an interpolation matrix must be provided"): + SegmentMarker( + name="my_marker", + parent_name="Thigh", + position=None, + interpolation_matrix=None, + is_technical=True, + is_anatomical=False, + ) + + with pytest.raises(ValueError, match="The position must be a 3d vector"): + SegmentMarker( + name="my_marker", + parent_name="Thigh", + position=np.zeros(2), + interpolation_matrix=None, + is_technical=True, + is_anatomical=False, + ) + + with pytest.raises(ValueError, match="The interpolation matrix must be a 3x12 matrix"): + SegmentMarker( + name="my_marker", + parent_name="Thigh", + position=None, + interpolation_matrix=np.zeros((1, 2)), + is_technical=True, + is_anatomical=False, + ) + + with pytest.raises(ValueError, match="position and interpolation matrix cannot both be provided"): + SegmentMarker( + name="my_marker", + parent_name="Thigh", + position=np.zeros(3), + interpolation_matrix=np.zeros((1, 2)), + is_technical=True, + is_anatomical=False, + ) + + segment_marker = SegmentMarker( + name="my_marker", + parent_name="Thigh", + position=np.ones(3), + is_technical=True, + is_anatomical=False, + ) + + TestUtils.mx_assert_equal(segment_marker.position, np.ones(3)) + TestUtils.mx_assert_equal( + segment_marker.interpolation_matrix, + np.array( + [ + [1.0, 0.0, 0.0, 2.0, 0.0, 0.0, -1.0, -0.0, -0.0, 1.0, 0.0, 0.0], + [0.0, 1.0, 0.0, 0.0, 2.0, 0.0, -0.0, -1.0, -0.0, 0.0, 1.0, 0.0], + [0.0, 0.0, 1.0, 0.0, 0.0, 2.0, -0.0, -0.0, -1.0, 0.0, 0.0, 1.0], + ] + ), + ) + assert segment_marker.is_technical + assert not segment_marker.is_anatomical + + marker_location = np.array([1, 2, 3]) + Qi = SegmentNaturalCoordinates.from_components( + u=[1, 2, 3], + rp=[1, 1, 3], + rd=[1, 2, 4], + w=[1, 2, 5], + ) + + constraint = segment_marker.constraint(marker_location=marker_location, Qi=Qi) + TestUtils.mx_assert_equal(constraint, np.array([-2, -2, -7])) + + constraint = segment_marker.constraint(marker_location=marker_location[:, np.newaxis], Qi=Qi) + TestUtils.mx_assert_equal(constraint, np.array([-2, -2, -7])) + + with pytest.raises(ValueError, match="The marker location must be a 3d vector"): + segment_marker.constraint(marker_location=np.zeros(2), Qi=Qi) + + with pytest.raises(ValueError, match="The marker location must be a 3d vector with only one column"): + segment_marker.constraint(marker_location=np.zeros((3, 2)), Qi=Qi) diff --git a/tests/test_maths.py b/tests/test_maths.py index 866fc555..c870bc8a 100644 --- a/tests/test_maths.py +++ b/tests/test_maths.py @@ -1,6 +1,7 @@ import numpy as np -from bionc import vnop_array, interpolate_natural_vector, to_natural_vector +from bionc import vnop_array import pytest +from .utils import TestUtils def test_vnop(): @@ -56,6 +57,8 @@ def test_vnop(): def test_interpolate_natural_vector(): + from bionc.bionc_numpy import interpolate_natural_vector, to_natural_vector + vector = np.array([1, 2, 3]) interpolation_matrix = interpolate_natural_vector(vector) np.testing.assert_allclose( @@ -84,3 +87,36 @@ def test_interpolate_natural_vector(): ] ) ) + + +def test_interpolate_natural_vector_casadi(): + from bionc.bionc_casadi import interpolate_natural_vector, to_natural_vector + + vector = np.array([1, 2, 3]) + interpolation_matrix = interpolate_natural_vector(vector) + TestUtils.mx_assert_equal( + interpolation_matrix, + np.array( + [ + [1.0, 0.0, 0.0, 3.0, 0.0, 0.0, -2.0, -0.0, -0.0, 3.0, 0.0, 0.0], + [0.0, 1.0, 0.0, 0.0, 3.0, 0.0, -0.0, -2.0, -0.0, 0.0, 3.0, 0.0], + [0.0, 0.0, 1.0, 0.0, 0.0, 3.0, -0.0, -0.0, -2.0, 0.0, 0.0, 3.0], + ] + ), + ) + + vector_2 = to_natural_vector(interpolation_matrix) + TestUtils.mx_assert_equal(vector_2, vector) + + # test the failure cases + with pytest.raises(ValueError, match="Vector must be 3x1"): + interpolate_natural_vector(np.array([1, 2, 3, 4])) + with pytest.raises(ValueError, match="Interpolation matrix must be 3x12"): + to_natural_vector( + np.array( + [ + [1.0, 0.0, 0.0, 3.0, 0.0, 0.0, -2.0, -0.0, -0.0, 3.0, 0.0, 0.0], + [0.0, 1.0, 0.0, 0.0, 3.0, 0.0, -0.0, -2.0, -0.0, 0.0, 3.0, 0.0], + ] + ) + ) diff --git a/tests/test_natural_coordinates.py b/tests/test_natural_coordinates.py index 7b81e3a5..5512c5d0 100644 --- a/tests/test_natural_coordinates.py +++ b/tests/test_natural_coordinates.py @@ -26,22 +26,6 @@ def test_SegmentNaturalCoordinates(): np.testing.assert_equal(Qi.vector[:, np.newaxis], Qi) -def test_SegmentNaturalCoordinates_casadi(): - - Qi = bionc_mx.SegmentNaturalCoordinates.from_components( - u=np.array([0, 0, 0]), - rp=np.array([4, 5, 6]), - rd=np.array([7, 8, 9]), - w=np.array([10, 11, 12]), - ) - np.testing.assert_equal(TestUtils.mx_to_array(Qi.u), np.array([0, 0, 0])) - np.testing.assert_equal(TestUtils.mx_to_array(Qi.rp), np.array([4, 5, 6])) - np.testing.assert_equal(TestUtils.mx_to_array(Qi.rd), np.array([7, 8, 9])) - np.testing.assert_equal(TestUtils.mx_to_array(Qi.w), np.array([10, 11, 12])) - np.testing.assert_equal(TestUtils.mx_to_array(Qi.v), -np.array([7, 8, 9]) + np.array([4, 5, 6])) - np.testing.assert_equal(TestUtils.mx_to_array(Qi.vector), TestUtils.mx_to_array(Qi)) - - # accelerations def test_NaturalAccelerationsCreator(): Qddot1 = bionc_np.SegmentNaturalAccelerations.from_components( @@ -57,20 +41,6 @@ def test_NaturalAccelerationsCreator(): np.testing.assert_equal(Qddot1.vector, Qddot1) -def test_NaturalAccelerationsCreator_casadi(): - Qddot1 = bionc_mx.SegmentNaturalAccelerations.from_components( - uddot=np.array([1, 2, 3]), - wddot=np.array([4, 5, 6]), - rdddot=np.array([7, 8, 9]), - rpddot=np.array([10, 11, 12]), - ) - np.testing.assert_equal(TestUtils.mx_to_array(Qddot1.uddot), np.array([1, 2, 3])) - np.testing.assert_equal(TestUtils.mx_to_array(Qddot1.wddot), np.array([4, 5, 6])) - np.testing.assert_equal(TestUtils.mx_to_array(Qddot1.rdddot), np.array([7, 8, 9])) - np.testing.assert_equal(TestUtils.mx_to_array(Qddot1.rpddot), np.array([10, 11, 12])) - np.testing.assert_equal(TestUtils.mx_to_array(Qddot1.vector), TestUtils.mx_to_array(Qddot1)) - - def test_concatenate(): Q1 = bionc_np.SegmentNaturalCoordinates.from_components( u=np.array([1, 2, 3]), @@ -140,33 +110,6 @@ def test_NaturalCoordinates(): np.testing.assert_equal(Q.nb_qi(), 2) -def test_NaturalCoordinates_casadi(): - Q1 = bionc_mx.SegmentNaturalCoordinates.from_components( - u=np.array([1, 2, 3]), - rp=np.array([4, 5, 6]), - rd=np.array([7, 8, 9]), - w=np.array([10, 11, 12]), - ) - Q2 = bionc_mx.SegmentNaturalCoordinates.from_components( - u=np.array([11, 22, 33]), - rp=np.array([4, 5, 6]), - rd=np.array([7, 8, 9]), - w=np.array([10, 11, 12]), - ) - Q = bionc_mx.NaturalCoordinates.from_qi((Q1, Q2)) - np.testing.assert_equal(TestUtils.mx_to_array(Q.u(0)), np.array([1, 2, 3])) - np.testing.assert_equal(TestUtils.mx_to_array(Q.u(1)), np.array([11, 22, 33])) - np.testing.assert_equal(TestUtils.mx_to_array(Q.v(0)), -np.array([7, 8, 9]) + np.array([4, 5, 6])) - np.testing.assert_equal(TestUtils.mx_to_array(Q.v(1)), -np.array([7, 8, 9]) + np.array([4, 5, 6])) - np.testing.assert_equal(TestUtils.mx_to_array(Q.vector(0)), TestUtils.mx_to_array(Q1)) - np.testing.assert_equal(TestUtils.mx_to_array(Q.vector(1)), TestUtils.mx_to_array(Q2)) - np.testing.assert_equal(TestUtils.mx_to_array(Q.vector(0).u), np.array([1, 2, 3])) - np.testing.assert_equal(TestUtils.mx_to_array(Q.vector(1).u), np.array([11, 22, 33])) - np.testing.assert_equal(TestUtils.mx_to_array(Q.vector(0).v), -np.array([7, 8, 9]) + np.array([4, 5, 6])) - np.testing.assert_equal(TestUtils.mx_to_array(Q.vector(1).v), -np.array([7, 8, 9]) + np.array([4, 5, 6])) - np.testing.assert_equal(Q.nb_qi(), 2) - - # do the same tests for NaturalAccelerations and SegmentNaturalAccelerations def test_NaturalAccelerations(): Qddot1 = bionc_np.SegmentNaturalAccelerations.from_components( @@ -189,26 +132,3 @@ def test_NaturalAccelerations(): np.testing.assert_equal(Qddot.vector(0).uddot, np.array([1, 2, 3])) np.testing.assert_equal(Qddot.vector(1).uddot, np.array([11, 22, 33])) np.testing.assert_equal(Qddot.nb_qddoti(), 2) - - -def test_NaturalAccelerations_casadi(): - Qddot1 = bionc_mx.SegmentNaturalAccelerations.from_components( - uddot=np.array([1, 2, 3]), - wddot=np.array([4, 5, 6]), - rdddot=np.array([7, 8, 9]), - rpddot=np.array([10, 11, 12]), - ) - Qddot2 = bionc_mx.SegmentNaturalAccelerations.from_components( - uddot=np.array([11, 22, 33]), - wddot=np.array([4, 5, 6]), - rdddot=np.array([7, 82, 9]), - rpddot=np.array([110, 11, 12]), - ) - Qddot = bionc_mx.NaturalAccelerations.from_qddoti((Qddot1, Qddot2)) - np.testing.assert_equal(TestUtils.mx_to_array(Qddot.uddot(0)), np.array([1, 2, 3])) - np.testing.assert_equal(TestUtils.mx_to_array(Qddot.uddot(1)), np.array([11, 22, 33])) - np.testing.assert_equal(TestUtils.mx_to_array(Qddot.vector(0)), TestUtils.mx_to_array(Qddot1)) - np.testing.assert_equal(TestUtils.mx_to_array(Qddot.vector(1)), TestUtils.mx_to_array(Qddot2)) - np.testing.assert_equal(TestUtils.mx_to_array(Qddot.vector(0).uddot), np.array([1, 2, 3])) - np.testing.assert_equal(TestUtils.mx_to_array(Qddot.vector(1).uddot), np.array([11, 22, 33])) - np.testing.assert_equal(Qddot.nb_qddoti(), 2) diff --git a/tests/test_natural_coordinates_casadi.py b/tests/test_natural_coordinates_casadi.py new file mode 100644 index 00000000..9f6fe4b7 --- /dev/null +++ b/tests/test_natural_coordinates_casadi.py @@ -0,0 +1,90 @@ +import numpy as np +import pytest +from bionc import ( + NaturalAccelerations, + SegmentNaturalAccelerations, + bionc_numpy as bionc_np, + bionc_casadi as bionc_mx, +) + +from .utils import TestUtils + + +def test_SegmentNaturalCoordinates_casadi(): + + Qi = bionc_mx.SegmentNaturalCoordinates.from_components( + u=np.array([0, 0, 0]), + rp=np.array([4, 5, 6]), + rd=np.array([7, 8, 9]), + w=np.array([10, 11, 12]), + ) + np.testing.assert_equal(TestUtils.mx_to_array(Qi.u), np.array([0, 0, 0])) + np.testing.assert_equal(TestUtils.mx_to_array(Qi.rp), np.array([4, 5, 6])) + np.testing.assert_equal(TestUtils.mx_to_array(Qi.rd), np.array([7, 8, 9])) + np.testing.assert_equal(TestUtils.mx_to_array(Qi.w), np.array([10, 11, 12])) + np.testing.assert_equal(TestUtils.mx_to_array(Qi.v), -np.array([7, 8, 9]) + np.array([4, 5, 6])) + np.testing.assert_equal(TestUtils.mx_to_array(Qi.vector), TestUtils.mx_to_array(Qi)) + + +def test_NaturalAccelerationsCreator_casadi(): + Qddot1 = bionc_mx.SegmentNaturalAccelerations.from_components( + uddot=np.array([1, 2, 3]), + wddot=np.array([4, 5, 6]), + rdddot=np.array([7, 8, 9]), + rpddot=np.array([10, 11, 12]), + ) + np.testing.assert_equal(TestUtils.mx_to_array(Qddot1.uddot), np.array([1, 2, 3])) + np.testing.assert_equal(TestUtils.mx_to_array(Qddot1.wddot), np.array([4, 5, 6])) + np.testing.assert_equal(TestUtils.mx_to_array(Qddot1.rdddot), np.array([7, 8, 9])) + np.testing.assert_equal(TestUtils.mx_to_array(Qddot1.rpddot), np.array([10, 11, 12])) + np.testing.assert_equal(TestUtils.mx_to_array(Qddot1.vector), TestUtils.mx_to_array(Qddot1)) + + +def test_NaturalCoordinates_casadi(): + Q1 = bionc_mx.SegmentNaturalCoordinates.from_components( + u=np.array([1, 2, 3]), + rp=np.array([4, 5, 6]), + rd=np.array([7, 8, 9]), + w=np.array([10, 11, 12]), + ) + Q2 = bionc_mx.SegmentNaturalCoordinates.from_components( + u=np.array([11, 22, 33]), + rp=np.array([4, 5, 6]), + rd=np.array([7, 8, 9]), + w=np.array([10, 11, 12]), + ) + Q = bionc_mx.NaturalCoordinates.from_qi((Q1, Q2)) + np.testing.assert_equal(TestUtils.mx_to_array(Q.u(0)), np.array([1, 2, 3])) + np.testing.assert_equal(TestUtils.mx_to_array(Q.u(1)), np.array([11, 22, 33])) + np.testing.assert_equal(TestUtils.mx_to_array(Q.v(0)), -np.array([7, 8, 9]) + np.array([4, 5, 6])) + np.testing.assert_equal(TestUtils.mx_to_array(Q.v(1)), -np.array([7, 8, 9]) + np.array([4, 5, 6])) + np.testing.assert_equal(TestUtils.mx_to_array(Q.vector(0)), TestUtils.mx_to_array(Q1)) + np.testing.assert_equal(TestUtils.mx_to_array(Q.vector(1)), TestUtils.mx_to_array(Q2)) + np.testing.assert_equal(TestUtils.mx_to_array(Q.vector(0).u), np.array([1, 2, 3])) + np.testing.assert_equal(TestUtils.mx_to_array(Q.vector(1).u), np.array([11, 22, 33])) + np.testing.assert_equal(TestUtils.mx_to_array(Q.vector(0).v), -np.array([7, 8, 9]) + np.array([4, 5, 6])) + np.testing.assert_equal(TestUtils.mx_to_array(Q.vector(1).v), -np.array([7, 8, 9]) + np.array([4, 5, 6])) + np.testing.assert_equal(Q.nb_qi(), 2) + + +def test_NaturalAccelerations_casadi(): + Qddot1 = bionc_mx.SegmentNaturalAccelerations.from_components( + uddot=np.array([1, 2, 3]), + wddot=np.array([4, 5, 6]), + rdddot=np.array([7, 8, 9]), + rpddot=np.array([10, 11, 12]), + ) + Qddot2 = bionc_mx.SegmentNaturalAccelerations.from_components( + uddot=np.array([11, 22, 33]), + wddot=np.array([4, 5, 6]), + rdddot=np.array([7, 82, 9]), + rpddot=np.array([110, 11, 12]), + ) + Qddot = bionc_mx.NaturalAccelerations.from_qddoti((Qddot1, Qddot2)) + np.testing.assert_equal(TestUtils.mx_to_array(Qddot.uddot(0)), np.array([1, 2, 3])) + np.testing.assert_equal(TestUtils.mx_to_array(Qddot.uddot(1)), np.array([11, 22, 33])) + np.testing.assert_equal(TestUtils.mx_to_array(Qddot.vector(0)), TestUtils.mx_to_array(Qddot1)) + np.testing.assert_equal(TestUtils.mx_to_array(Qddot.vector(1)), TestUtils.mx_to_array(Qddot2)) + np.testing.assert_equal(TestUtils.mx_to_array(Qddot.vector(0).uddot), np.array([1, 2, 3])) + np.testing.assert_equal(TestUtils.mx_to_array(Qddot.vector(1).uddot), np.array([11, 22, 33])) + np.testing.assert_equal(Qddot.nb_qddoti(), 2) diff --git a/tests/test_natural_segment.py b/tests/test_natural_segment.py index 75e1ac10..b5654dff 100644 --- a/tests/test_natural_segment.py +++ b/tests/test_natural_segment.py @@ -1,8 +1,8 @@ from bionc import ( NaturalSegment, SegmentMarker, - bionc_numpy as bionc_np, ) +from bionc.bionc_numpy import SegmentNaturalCoordinates import numpy as np import pytest @@ -71,7 +71,7 @@ def test_marker_features(): my_segment.add_marker(marker1) my_segment.add_marker(marker2) - Qi = bionc_np.SegmentNaturalCoordinates.from_components( + Qi = SegmentNaturalCoordinates.from_components( u=[1, 2, 3], rp=[1, 1, 3], rd=[1, 2, 4], From ee4206913d0411ad2d06bf40ee3a89cd13610c59 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Wed, 16 Nov 2022 16:27:31 -0500 Subject: [PATCH 24/30] fix --- bionc/model_computations/natural_segment.py | 708 -------------------- tests/test_biomech_model.py | 15 +- 2 files changed, 7 insertions(+), 716 deletions(-) delete mode 100644 bionc/model_computations/natural_segment.py diff --git a/bionc/model_computations/natural_segment.py b/bionc/model_computations/natural_segment.py deleted file mode 100644 index 92b9627f..00000000 --- a/bionc/model_computations/natural_segment.py +++ /dev/null @@ -1,708 +0,0 @@ -from copy import copy -from typing import Union, Tuple - -import numpy as np -from numpy import cos, sin, matmul, eye, zeros, sum, ones -from numpy.linalg import inv - -from ..utils.natural_coordinates import SegmentNaturalCoordinates -from ..utils.natural_velocities import SegmentNaturalVelocities -from ..utils.natural_accelerations import SegmentNaturalAccelerations -from ..utils.homogenous_transform import HomogeneousTransform -from ..model_computations.natural_axis import Axis -from ..model_computations.natural_marker import SegmentMarker, Marker - - -class NaturalSegment: - """ - Class used to define anatomical segment based on natural coordinate. - - Methods - ------- - transformation_matrix() - This function returns the transformation matrix, denoted Bi - rigid_body_constraint() - This function returns the rigid body constraints of the segment, denoted phi_r - rigid_body_constraint_jacobian() - This function returns the jacobian of rigid body constraints of the segment, denoted K_r - - add_marker() - This function adds a marker to the segment - nb_markers() - This function returns the number of markers in the segment - marker_constraints() - This function returns the defects of the marker constraints of the segment, denoted Phi_m - marker_jacobian() - This function returns the jacobian of the marker constraints of the segment, denoted K_m - - Attributes - ---------- - _name : str - name of the segment - _length : float - length of the segment - _alpha : float - angle between u and w - _beta : float - angle between w and (rp-rd) - _gamma : float - angle between (rp-rd) and u - _mass : float - mass of the segment in Segment Coordinate System - _center_of_mass : np.ndarray - center of mass of the segment in Segment Coordinate System - _inertia: np.ndarray - inertia matrix of the segment in Segment Coordinate System - """ - - def __init__( - self, - name: str = None, - alpha: Union[float, np.float64] = np.pi / 2, - beta: Union[float, np.float64] = np.pi / 2, - gamma: Union[float, np.float64] = np.pi / 2, - length: Union[float, np.float64] = None, - mass: Union[float, np.float64] = None, - center_of_mass: np.ndarray = None, - inertia: np.ndarray = None, - ): - - self._name = name - - self._length = length - self._alpha = alpha - self._beta = beta - self._gamma = gamma - - # todo: sanity check to make sure u, v or w are not collinear - # todo: implement all the transformations matrix according the Ph.D thesis of Alexandre Naaim - self._transformation_matrix = self._transformation_matrix() - - self._mass = mass - if center_of_mass is None: - self._center_of_mass = center_of_mass - self._center_of_mass_in_natural_coordinates_system = None - self._interpolation_matrix_center_of_mass = None - else: - if center_of_mass.shape[0] != 3: - raise ValueError("Center of mass must be 3x1") - self._center_of_mass = center_of_mass - self._center_of_mass_in_natural_coordinates_system = self._center_of_mass_in_natural_coordinates_system() - self._interpolation_matrix_center_of_mass = self._interpolation_matrix_center_of_mass() - - if inertia is None: - self._inertia = inertia - self._inertia_in_natural_coordinates_system = None - self._interpolation_matrix_inertia = None - self._mass_matrix = None - else: - if inertia.shape != (3, 3): - raise ValueError("Inertia matrix must be 3x3") - self._inertia = inertia - self._pseudo_inertia_matrix = self._pseudo_inertia_matrix() - self._mass_matrix = self._update_mass_matrix() - - # list of markers embedded in the segment - self._markers = [] - - def set_name(self, name: str): - """ - This function sets the name of the segment - - Parameters - ---------- - name : str - Name of the segment - """ - self._name = name - - @classmethod - def from_experimental_Q( - cls, - Qi: SegmentNaturalCoordinates, - ) -> "NaturalSegment": - """ - Parameters - ---------- - Qi : SegmentNaturalCoordinates - Experimental segment natural coordinates (12 x n_frames) - - Returns - ------- - NaturalSegment - """ - - alpha = np.zeros(Qi.shape[1]) - beta = np.zeros(Qi.shape[1]) - gamma = np.zeros(Qi.shape[1]) - length = np.zeros(Qi.shape[1]) - - for i, Qif in enumerate(Qi.vector.T): - alpha[i], beta[i], gamma[i], length[i] = cls.parameters_from_Q(Qif) - - return cls( - alpha=np.mean(alpha, axis=0), - beta=np.mean(beta, axis=0), - gamma=np.mean(gamma, axis=0), - length=np.mean(length, axis=0), - ) - - @staticmethod - def parameters_from_Q(Q: SegmentNaturalCoordinates) -> tuple: - """ - This function computes the parameters of the segment from the natural coordinates - - Parameters - ---------- - Q: SegmentNaturalCoordinates - The natural coordinates of the segment - - Returns - ------- - tuple - The parameters of the segment (alpha, beta, gamma, length) - """ - - if not isinstance(Q, SegmentNaturalCoordinates): - Q = SegmentNaturalCoordinates(Q) - - u, rp, rd, w = Q.to_components() - - length = np.linalg.norm(rp - rd, axis=0) - alpha = np.arccos(np.sum((rp - rd) * w, axis=0) / length) - beta = np.arccos(np.sum(u * w, axis=0)) - gamma = np.arccos(np.sum(u * (rp - rd), axis=0) / length) - - return alpha, beta, gamma, length - - # def __str__(self): - # print("to do") - - @property - def name(self): - return self._name - - @property - def length(self): - return self._length - - @property - def alpha(self): - return self._alpha - - @property - def beta(self): - return self._beta - - @property - def gamma(self): - return self._gamma - - @property - def mass(self): - return self._mass - - @property - def center_of_mass(self): - return self._center_of_mass - - @property - def inertia(self): - return self._inertia - - def _transformation_matrix(self) -> np.ndarray: - """ - This function computes the transformation matrix, denoted Bi, - from Natural Coordinate System to point to the orthogonal Segment Coordinate System. - Example : if vector a expressed in (Pi, X, Y, Z), inv(B) * a is expressed in (Pi, ui, vi, wi) - - Returns - ------- - np.ndarray - Transformation matrix from natural coordinate to segment coordinate system [3x3] - """ - return np.array( - [ - [1, 0, 0], - [self.length * cos(self.gamma), self.length * sin(self.gamma), 0], - [ - cos(self.beta), - (cos(self.alpha) - cos(self.beta) * cos(self.gamma)) / sin(self.beta), - np.sqrt( - 1 - - cos(self.beta) ** 2 - - (cos(self.alpha) - cos(self.beta) * cos(self.gamma)) / sin(self.beta) ** 2 - ), - ], - ] - ).T - - @property - def transformation_matrix(self) -> np.ndarray: - """ - This function returns the transformation matrix, denoted Bi, - from Natural Coordinate System to point to the orthogonal Segment Coordinate System. - Example : if vector a expressed in (Pi, X, Y, Z), inv(B) * a is expressed in (Pi, ui, vi, wi) - - Returns - ------- - np.ndarray - Transformation matrix from natural coordinate to segment coordinate system [3x3] - """ - return self._transformation_matrix - - def segment_coordinates_system(self, Q: SegmentNaturalCoordinates) -> HomogeneousTransform: - """ - This function computes the segment coordinates from the natural coordinates - - Parameters - ---------- - Q: SegmentNaturalCoordinates - The natural coordinates of the segment - - Returns - ------- - SegmentCoordinates - The segment coordinates - """ - if not isinstance(Q, SegmentNaturalCoordinates): - Q = SegmentNaturalCoordinates(Q) - - return HomogeneousTransform.from_rt( - rotation=self._transformation_matrix @ np.concatenate((Q.u, Q.v, Q.w), axis=1), - translation=Q.rp, - ) - - def location_from_homogenous_transform( - self, T: Union[np.ndarray, HomogeneousTransform] - ) -> SegmentNaturalCoordinates: - """ - This function returns the location of the segment in natural coordinate from its homogenous transform - - Parameters - ---------- - T: np.ndarray or HomogeneousTransform - Homogenous transform of the segment Ti which transforms from the local frame (Oi, Xi, Yi, Zi) - to the global frame (Xi, Yi, Zi) - - Returns - ------- - np.ndarray - Location of the segment [3 x 1] - """ - - u = self.transformation_matrix @ T[0:3, 0] - w = self.transformation_matrix @ T[0:3, 2] - rp = self.transformation_matrix @ T[0:3, 4] - rd = (T @ np.array([0, self.length, 0, 1]))[0:3] # not sure of this line. - - return SegmentNaturalCoordinates((u, rp, rd, w)) - - def rigid_body_constraint(self, Qi: Union[SegmentNaturalCoordinates, np.ndarray]) -> np.ndarray: - """ - This function returns the rigid body constraints of the segment, denoted phi_r. - - Returns - ------- - np.ndarray - Rigid body constraints of the segment [6 x 1 x N_frame] - """ - if not isinstance(Qi, SegmentNaturalCoordinates): - Qi = SegmentNaturalCoordinates(Qi) - - phir = zeros(6) - u, v, w = Qi.to_uvw() - - phir[0] = sum(u**2, 0) - 1 - phir[1] = sum(u * v, 0) - self.length * cos(self.gamma) - phir[2] = sum(u * Qi.w, 0) - cos(self.beta) - phir[3] = sum(v**2, 0) - self.length**2 - phir[4] = sum(v * w, 0) - self.length * cos(self.alpha) - phir[5] = sum(w**2, 0) - 1 - - return phir - - @staticmethod - def rigid_body_constraint_jacobian(Qi: SegmentNaturalCoordinates) -> np.ndarray: - """ - This function returns the Jacobian matrix of the rigid body constraints denoted K_r - - Returns - ------- - Kr : np.ndarray - Jacobian matrix of the rigid body constraints denoted Kr [6 x 12 x N_frame] - """ - # initialisation - Kr = zeros((6, 12)) - - if not isinstance(Qi, SegmentNaturalCoordinates): - Qi = SegmentNaturalCoordinates(Qi) - u, v, w = Qi.to_uvw() - - Kr[0, 0:3] = 2 * u - - Kr[1, 0:3] = v - Kr[1, 3:6] = u - Kr[1, 6:9] = -u - - Kr[2, 0:3] = w - Kr[2, 9:12] = u - - Kr[3, 3:6] = 2 * v - Kr[3, 6:9] = -2 * v - - Kr[4, 3:6] = w - Kr[4, 6:9] = -w - Kr[4, 9:12] = v - - Kr[5, 9:12] = 2 * w - - return Kr - - def rigid_body_constraint_derivative( - self, Qi: SegmentNaturalCoordinates, Qdoti: SegmentNaturalVelocities - ) -> np.ndarray: - """ - This function returns the derivative of the rigid body constraints denoted Phi_r_dot - - Returns - ------- - np.ndarray - Derivative of the rigid body constraints [6 x 1 x N_frame] - """ - - return self.rigid_body_constraint_jacobian(Qi) @ np.array(Qdoti) - - @staticmethod - def rigid_body_constraint_jacobian_derivative(Qdoti: SegmentNaturalVelocities) -> np.ndarray: - """ - This function returns the derivative of the Jacobian matrix of the rigid body constraints denoted Kr_dot [6 x 12 x N_frame] - - Returns - ------- - Kr_dot : np.ndarray - derivative of the Jacobian matrix of the rigid body constraints denoted Kr_dot [6 x 12 ] - """ - if isinstance(Qdoti, SegmentNaturalCoordinates): - raise TypeError("Qdoti should be a SegmentNaturalVelocities object") - # not able to check if Qdoti is a SegmentNaturalVelocities if Qdoti is a np.ndarray - if not isinstance(Qdoti, SegmentNaturalVelocities): - Qdoti = SegmentNaturalVelocities(Qdoti) - - # initialisation - Kr_dot = zeros((6, 12)) - - Kr_dot[0, 0:3] = 2 * Qdoti.udot - - Kr_dot[1, 0:3] = Qdoti.vdot - Kr_dot[1, 3:6] = Qdoti.udot - Kr_dot[1, 6:9] = -Qdoti.udot - - Kr_dot[2, 0:3] = Qdoti.wdot - Kr_dot[2, 9:12] = Qdoti.udot - - Kr_dot[3, 3:6] = 2 * Qdoti.vdot - Kr_dot[3, 6:9] = -2 * Qdoti.vdot - - Kr_dot[4, 3:6] = Qdoti.wdot - Kr_dot[4, 6:9] = -Qdoti.wdot - Kr_dot[4, 9:12] = Qdoti.vdot - - Kr_dot[5, 9:12] = 2 * Qdoti.wdot - - return Kr_dot - - def _pseudo_inertia_matrix(self) -> np.ndarray: - """ - This function returns the pseudo-inertia matrix of the segment, denoted J_i. - It transforms the inertia matrix of the segment in the segment coordinate system to the natural coordinate system. - - Returns - ------- - np.ndarray - Pseudo-inertia matrix of the segment in the natural coordinate system [3x3] - """ - # todo: verify the formula - middle_block = self.inertia + self.mass * ( - self.center_of_mass.T @ self.center_of_mass * eye(3) - - self.center_of_mass[:, np.newaxis] @ self.center_of_mass[:, np.newaxis].T - ) - - Binv = inv(self.transformation_matrix) - Binv_transpose = Binv.T - - return Binv @ (middle_block @ Binv_transpose) - - @property - def pseudo_inertia_matrix(self) -> np.ndarray: - """ - This function returns the pseudo-inertia matrix of the segment, denoted J_i. - It transforms the inertia matrix of the segment in the segment coordinate system to the natural coordinate system. - - Returns - ------- - np.ndarray - Pseudo-inertia matrix of the segment in the natural coordinate system [3x3] - """ - return self._pseudo_inertia_matrix - - def _center_of_mass_in_natural_coordinates_system(self) -> np.ndarray: - """ - This function computes the center of mass of the segment in the natural coordinate system. - It transforms the center of mass of the segment in the segment coordinate system to the natural coordinate system. - - Returns - ------- - np.ndarray - Center of mass of the segment in the natural coordinate system [3x1] - """ - return inv(self.transformation_matrix) @ self.center_of_mass - - @property - def center_of_mass_in_natural_coordinates_system(self) -> np.ndarray: - """ - This function returns the center of mass of the segment in the natural coordinate system. - It transforms the center of mass of the segment in the segment coordinate system to the natural coordinate system. - - Returns - ------- - np.ndarray - Center of mass of the segment in the natural coordinate system [3x1] - """ - return self._center_of_mass_in_natural_coordinates_system - - def _update_mass_matrix(self) -> np.ndarray: - """ - This function returns the generalized mass matrix of the segment, denoted G_i. - - Returns - ------- - np.ndarray - mass matrix of the segment [12 x 12] - """ - - Ji = self.pseudo_inertia_matrix - n_ci = self.center_of_mass_in_natural_coordinates_system - - Gi = zeros((12, 12)) - - Gi[0:3, 0:3] = Ji[0, 0] * eye(3) - Gi[0:3, 3:6] = (self.mass * n_ci[0] + Ji[0, 1]) * eye(3) - Gi[0:3, 6:9] = -Ji[0, 1] * eye(3) - Gi[0:3, 9:12] = -Ji[0, 2] * eye(3) - Gi[3:6, 3:6] = (self.mass + 2 * self.mass * n_ci[1] + Ji[1, 1]) * eye(3) - Gi[3:6, 6:9] = -(self.mass * n_ci[1] + Ji[1, 1]) * eye(3) - Gi[3:6, 9:12] = (self.mass * n_ci[2] + Ji[1, 2]) * eye(3) - Gi[6:9, 6:9] = Ji[1, 1] * eye(3) - Gi[6:9, 9:12] = -Ji[1, 2] * eye(3) - Gi[9:12, 9:12] = Ji[2, 2] * eye(3) - - # symmetrize the matrix - Gi = np.tril(Gi) + np.tril(Gi, -1).T - - return Gi - - @property - def mass_matrix(self) -> np.ndarray: - """ - This function returns the generalized mass matrix of the segment, denoted G_i. - - Returns - ------- - np.ndarray - mass matrix of the segment [12 x 12] - """ - - return self._mass_matrix - - @staticmethod - def interpolate(vector: np.ndarray) -> np.ndarray: - """ - This function interpolates the vector to get the interpolation matrix, denoted Ni - such as: - Ni * Qi = location in the global frame - - Parameters - ---------- - vector : np.ndarray - Vector in the natural coordinate system to interpolate (Pi, ui, vi, wi) - - Returns - ------- - interpolate_natural_vector: np.ndarray - Interpolation matrix [3 x 12], denoted Ni to get the location of the vector as linear combination of Q. - vector in global frame = Ni * Qi - """ - - interpolation_matrix = np.zeros((3, 12)) - interpolation_matrix[0:3, 0:3] = vector[0] * eye(3) - interpolation_matrix[0:3, 3:6] = (1 + vector[1]) * eye(3) - interpolation_matrix[0:3, 6:9] = -vector[1] * eye(3) - interpolation_matrix[0:3, 9:12] = vector[2] * eye(3) - - return interpolation_matrix - - def _interpolation_matrix_center_of_mass(self) -> np.ndarray: - """ - This function returns the interpolation matrix for the center of mass of the segment, denoted N_i^Ci. - It allows to apply the gravity force at the center of mass of the segment. - - Returns - ------- - np.ndarray - Interpolation matrix for the center of mass of the segment in the natural coordinate system [12 x 3] - """ - n_ci = self.center_of_mass_in_natural_coordinates_system - return self.interpolate(n_ci) - - @property - def interpolation_matrix_center_of_mass(self) -> np.ndarray: - """ - This function returns the interpolation matrix for the center of mass of the segment, denoted N_i^Ci. - It allows to apply the gravity force at the center of mass of the segment. - - Returns - ------- - np.ndarray - Interpolation matrix for the center of mass of the segment in the natural coordinate system [12 x 3] - """ - return self._interpolation_matrix_center_of_mass - - def weight(self) -> np.ndarray: - """ - This function returns the weight applied on the segment through gravity force. - - Returns - ------- - np.ndarray - Weight applied on the segment through gravity force [12 x 1] - """ - - return self.interpolation_matrix_center_of_mass.T * self.mass @ np.array([0, 0, -9.81]) - - def differential_algebraic_equation( - self, - Qi: Union[SegmentNaturalCoordinates, np.ndarray], - Qdoti: Union[SegmentNaturalVelocities, np.ndarray], - stabilization: dict = None, - ) -> Tuple[SegmentNaturalAccelerations, np.ndarray]: - """ - This function returns the differential algebraic equation of the segment - - Parameters - ---------- - Qi: SegmentNaturalCoordinates - Natural coordinates of the segment - Qdoti: SegmentNaturalCoordinates - Derivative of the natural coordinates of the segment - stabilization: dict - Dictionary containing the Baumgarte's stabilization parameters: - * alpha: float - Stabilization parameter for the constraint - * beta: float - Stabilization parameter for the constraint derivative - - Returns - ------- - np.ndarray - Differential algebraic equation of the segment [12 x 1] - """ - if isinstance(Qi, SegmentNaturalVelocities): - raise TypeError("Qi should be of type SegmentNaturalCoordinates") - if isinstance(Qdoti, SegmentNaturalCoordinates): - raise TypeError("Qdoti should be of type SegmentNaturalVelocities") - - # not able to verify if the types of Qi and Qdoti are np.ndarray - if not isinstance(Qi, SegmentNaturalCoordinates): - Qi = SegmentNaturalCoordinates(Qi) - if not isinstance(Qdoti, SegmentNaturalVelocities): - Qdoti = SegmentNaturalVelocities(Qdoti) - - Gi = self.mass_matrix - Kr = self.rigid_body_constraint_jacobian(Qi) - Krdot = self.rigid_body_constraint_jacobian_derivative(Qdoti) - biais = -Krdot @ Qdoti.vector - - if stabilization is not None: - biais -= stabilization["alpha"] * self.rigid_body_constraint(Qi) + stabilization[ - "beta" - ] * self.rigid_body_constraint_derivative(Qi, Qdoti) - - A = zeros((18, 18)) - A[0:12, 0:12] = Gi - A[12:18, 0:12] = Kr - A[0:12, 12:18] = Kr.T - A[12:, 12:18] = np.zeros((6, 6)) - - B = np.concatenate([self.weight(), biais], axis=0) - - # solve the linear system Ax = B with numpy - x = np.linalg.solve(A, B) - Qddoti = x[0:12] - lambda_i = x[12:] - return SegmentNaturalAccelerations(Qddoti), lambda_i - - def add_marker(self, marker: SegmentMarker): - """ - Add a new marker to the segment - - Parameters - ---------- - marker - The marker to add - """ - if marker.parent_name is not None and marker.parent_name != self.name: - raise ValueError( - "The marker name should be the same as the 'key'. Alternatively, marker.name can be left undefined" - ) - - marker.parent_name = self.name - self._markers.append(marker) - - def nb_markers(self) -> int: - """ - Returns the number of markers of the natural segment - - Returns - ------- - int - Number of markers of the segment - """ - return len(self._markers) - - def marker_constraints(self, marker_locations: np.ndarray, Qi: SegmentNaturalCoordinates) -> np.ndarray: - """ - This function returns the marker constraints of the segment - - Parameters - ---------- - marker_locations: np.ndarray - Marker locations in the global/inertial coordinate system (3 x N_markers) - Qi: SegmentNaturalCoordinates - Natural coordinates of the segment - - Returns - ------- - np.ndarray - The defects of the marker constraints of the segment (3 x N_markers) - """ - if marker_locations.shape != (3, self.nb_markers()): - raise ValueError(f"marker_locations should be of shape (3, {self.nb_markers()})") - - defects = np.zeros((3, self.nb_markers())) - - for i, marker in enumerate(self._markers): - defects[:, i] = marker.constraint(marker_location=marker_locations[:, i], Qi=Qi) - - return defects - - def marker_jacobian(self): - """ - This function returns the marker jacobian of the segment - - Returns - ------- - np.ndarray - The jacobian of the marker constraints of the segment (3 x N_markers) - """ - return np.vstack([-marker.interpolation_matrix for marker in self._markers]) diff --git a/tests/test_biomech_model.py b/tests/test_biomech_model.py index 9d81eed2..174d0214 100644 --- a/tests/test_biomech_model.py +++ b/tests/test_biomech_model.py @@ -3,8 +3,7 @@ from .utils import TestUtils -from bionc.bionc_numpy import SegmentNaturalVelocities, NaturalVelocities -from bionc import bionc_numpy as bionc_np +from bionc.bionc_numpy import SegmentNaturalVelocities, NaturalVelocities, SegmentNaturalCoordinates, NaturalCoordinates def test_biomech_model(): @@ -33,19 +32,19 @@ def test_biomech_model(): assert natural_model.nb_Qddot() == 48 # Test rigid body constraints - Q1 = bionc_np.SegmentNaturalCoordinates.from_components( + Q1 = SegmentNaturalCoordinates.from_components( u=[1, 2, 3.05], rp=[1.1, 1, 3.1], rd=[1.2, 2, 4.1], w=[1.3, 2, 5.1], ) - Q2 = bionc_np.SegmentNaturalCoordinates.from_components( + Q2 = SegmentNaturalCoordinates.from_components( u=[1.4, 2, 3.2], rp=[1.5, 1, 3.2], rd=[1.6, 2, 4.2], w=[1.7, 2, 5.2], ) - Q3 = bionc_np.SegmentNaturalCoordinates.from_components( + Q3 = SegmentNaturalCoordinates.from_components( u=[1.8, 2, 3.3], rp=[1.9, 1, 3.3], rd=[2.1, 2, 4.3], @@ -1303,19 +1302,19 @@ def test_biomech_model(): ) # Test rigid body constraint jacobian derivative - Qdot1 = bionc_np.SegmentNaturalVelocities.from_components( + Qdot1 = SegmentNaturalVelocities.from_components( udot=[1, 2, 3.05], rpdot=[1.1, 1, 3.1], rddot=[1.2, 2, 4.1], wdot=[1.3, 2, 5.1], ) - Qdot2 = bionc_np.SegmentNaturalVelocities.from_components( + Qdot2 = SegmentNaturalVelocities.from_components( udot=[1.4, 2, 3.2], rpdot=[1.5, 1, 3.2], rddot=[1.6, 2, 4.2], wdot=[1.7, 2, 5.2], ) - Qdot3 = bionc_np.SegmentNaturalVelocities.from_components( + Qdot3 = SegmentNaturalVelocities.from_components( udot=[1.8, 2, 3.3], rpdot=[1.9, 1, 3.3], rddot=[2.1, 2, 4.3], From 44adf4ee3d3b0b05ef553898d9bc286df076f00e Mon Sep 17 00:00:00 2001 From: Ipuch Date: Wed, 16 Nov 2022 16:45:42 -0500 Subject: [PATCH 25/30] abstract to set the natural segment --- bionc/protocols/natural_segment.py | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 bionc/protocols/natural_segment.py diff --git a/bionc/protocols/natural_segment.py b/bionc/protocols/natural_segment.py new file mode 100644 index 00000000..e69de29b From 18e17cf2e8126464d492e0251fa830bb7dc0248d Mon Sep 17 00:00:00 2001 From: Ipuch Date: Wed, 16 Nov 2022 16:45:52 -0500 Subject: [PATCH 26/30] abstract yeah --- bionc/bionc_casadi/natural_segment.py | 4 +- bionc/bionc_numpy/natural_segment.py | 10 +- bionc/protocols/natural_segment.py | 394 ++++++++++++++++++++++++++ 3 files changed, 403 insertions(+), 5 deletions(-) diff --git a/bionc/bionc_casadi/natural_segment.py b/bionc/bionc_casadi/natural_segment.py index 39795d5b..90bc07b7 100644 --- a/bionc/bionc_casadi/natural_segment.py +++ b/bionc/bionc_casadi/natural_segment.py @@ -10,8 +10,10 @@ from ..bionc_casadi.homogenous_transform import HomogeneousTransform from ..bionc_casadi.natural_marker import SegmentMarker +from ..protocols.natural_segment import AbstractNaturalSegment -class NaturalSegment: + +class NaturalSegment(AbstractNaturalSegment): """ Class used to define anatomical segment based on natural coordinate. diff --git a/bionc/bionc_numpy/natural_segment.py b/bionc/bionc_numpy/natural_segment.py index 28ce7db4..338edb1a 100644 --- a/bionc/bionc_numpy/natural_segment.py +++ b/bionc/bionc_numpy/natural_segment.py @@ -11,8 +11,10 @@ from ..bionc_numpy.homogenous_transform import HomogeneousTransform from ..bionc_numpy.natural_marker import SegmentMarker +from ..protocols.natural_segment import AbstractNaturalSegment -class NaturalSegment: + +class NaturalSegment(AbstractNaturalSegment): """ Class used to define anatomical segment based on natural coordinate. @@ -414,7 +416,7 @@ def _pseudo_inertia_matrix(self) -> np.ndarray: Binv = inv(self.transformation_matrix) Binv_transpose = np.transpose(Binv) - return matmul(Binv, matmul(middle_block, Binv_transpose)) + return Binv @ (middle_block @ Binv_transpose) @property def pseudo_inertia_matrix(self) -> np.ndarray: @@ -439,7 +441,7 @@ def _center_of_mass_in_natural_coordinates_system(self) -> np.ndarray: np.ndarray Center of mass of the segment in the natural coordinate system [3x1] """ - return matmul(inv(self.transformation_matrix), self.center_of_mass) + return inv(self.transformation_matrix) @ self.center_of_mass @property def center_of_mass_in_natural_coordinates_system(self) -> np.ndarray: @@ -561,7 +563,7 @@ def weight(self) -> np.ndarray: Weight applied on the segment through gravity force [12 x 1] """ - return np.matmul(self.interpolation_matrix_center_of_mass.T * self.mass, np.array([0, 0, -9.81])) + return (self.interpolation_matrix_center_of_mass.T * self.mass) @ np.array([0, 0, -9.81]) def differential_algebraic_equation( self, diff --git a/bionc/protocols/natural_segment.py b/bionc/protocols/natural_segment.py index e69de29b..8d400e6d 100644 --- a/bionc/protocols/natural_segment.py +++ b/bionc/protocols/natural_segment.py @@ -0,0 +1,394 @@ +# import ABC +from abc import ABC, abstractmethod +from typing import Tuple, Union +import numpy as np + + +class AbstractNaturalSegment(ABC): + """ + Class used to define anatomical segment based on natural coordinate. + + Methods + ------- + set_name + This method is used to set the name of the segment. + alpha + This method is used to return the alpha angle of the segment. + beta + This method is used to return the beta angle of the segment. + gamma + This method is used to return the gamma angle of the segment. + length + This method is used to return the length of the segment. + mass + This method is used to return the mass of the segment. + inertia + This method is used to return the inertia of the segment. + center_of_mass + This method is used to return the center of mass of the segment. + transformation_matrix() + This function returns the transformation matrix, denoted Bi. + rigid_body_constraint() + This function returns the rigid body constraints of the segment, denoted phi_r. + rigid_body_constraint_jacobian() + This function returns the jacobian of rigid body constraints of the segment, denoted K_r. + + + add_marker() + This function adds a marker to the segment + nb_markers() + This function returns the number of markers in the segment + marker_constraints() + This function returns the defects of the marker constraints of the segment, denoted Phi_m + marker_jacobian() + This function returns the jacobian of the marker constraints of the segment, denoted K_m + + """ + @abstractmethod + def set_name(self, name: str): + """ + This function sets the name of the segment + + Parameters + ---------- + name : str + Name of the segment + """ + + @abstractmethod + def name(self): + """ + This function returns the name of the segment + + Returns + ------- + str + Name of the segment + """ + + @abstractmethod + def length(self): + """ + This function returns the length of the segment + + Returns + ------- + float + Length of the segment + """ + + @abstractmethod + def alpha(self): + """ + This function returns the alpha angle of the segment + + Returns + ------- + float + Alpha angle of the segment + """ + + @abstractmethod + def beta(self): + """ + This function returns the beta angle of the segment + + Returns + ------- + float + Beta angle of the segment + """ + + @abstractmethod + def gamma(self): + """ + This function returns the gamma angle of the segment + + Returns + ------- + float + Gamma angle of the segment + """ + + @abstractmethod + def mass(self): + """ + This function returns the mass of the segment + + Returns + ------- + float + Mass of the segment + """ + + @abstractmethod + def center_of_mass(self): + """ + This function returns the center of mass of the segment + + Returns + ------- + np.ndarray + Center of mass of the segment + """ + + @abstractmethod + def inertia(self): + """ + This function returns the inertia matrix of the segment + + Returns + ------- + np.ndarray + Inertia matrix of the segment + """ + + @abstractmethod + def _transformation_matrix(self): + """ + This function computes the transformation matrix, denoted Bi, + from Natural Coordinate System to point to the orthogonal Segment Coordinate System. + Example : if vector a expressed in (Pi, X, Y, Z), inv(B) * a is expressed in (Pi, ui, vi, wi) + + Returns + ------- + np.ndarray + Transformation matrix from natural coordinate to segment coordinate system [3x3] + """ + + @abstractmethod + def transformation_matrix(self): + """ + This function returns the transformation matrix, denoted Bi, + from Natural Coordinate System to point to the orthogonal Segment Coordinate System. + Example : if vector a expressed in (Pi, X, Y, Z), inv(B) * a is expressed in (Pi, ui, vi, wi) + + Returns + ------- + np.ndarray + Transformation matrix from natural coordinate to segment coordinate system [3x3] + """ + + @abstractmethod + def segment_coordinates_system(self, Q): + """ + This function computes the segment coordinates from the natural coordinates + + Parameters + ---------- + Q: SegmentNaturalCoordinates + The natural coordinates of the segment + + Returns + ------- + SegmentCoordinates + The segment coordinates + """ + + @abstractmethod + def location_from_homogenous_transform( + self, T + ): + """ + This function returns the location of the segment in natural coordinate from its homogenous transform + + Parameters + ---------- + T: np.ndarray or HomogeneousTransform + Homogenous transform of the segment Ti which transforms from the local frame (Oi, Xi, Yi, Zi) + to the global frame (Xi, Yi, Zi) + + Returns + ------- + np.ndarray + Location of the segment [3 x 1] + """ + + @abstractmethod + def rigid_body_constraint(self, Qi): + """ + This function returns the rigid body constraints of the segment, denoted phi_r. + + Returns + ------- + np.ndarray + Rigid body constraints of the segment [6 x 1 x N_frame] + """ + + @abstractmethod + def _pseudo_inertia_matrix(self): + """ + This function returns the pseudo-inertia matrix of the segment, denoted J_i. + It transforms the inertia matrix of the segment in the segment coordinate system to the natural coordinate system. + + Returns + ------- + np.ndarray + Pseudo-inertia matrix of the segment in the natural coordinate system [3x3] + """ + + @abstractmethod + def pseudo_inertia_matrix(self): + """ + This function returns the pseudo-inertia matrix of the segment, denoted J_i. + It transforms the inertia matrix of the segment in the segment coordinate system to the natural coordinate system. + + Returns + ------- + np.ndarray + Pseudo-inertia matrix of the segment in the natural coordinate system [3x3] + """ + + @abstractmethod + def _center_of_mass_in_natural_coordinates_system(self): + """ + This function computes the center of mass of the segment in the natural coordinate system. + It transforms the center of mass of the segment in the segment coordinate system to the natural coordinate system. + + Returns + ------- + np.ndarray + Center of mass of the segment in the natural coordinate system [3x1] + """ + + @abstractmethod + def center_of_mass_in_natural_coordinates_system(self): + """ + This function returns the center of mass of the segment in the natural coordinate system. + It transforms the center of mass of the segment in the segment coordinate system to the natural coordinate system. + + Returns + ------- + np.ndarray + Center of mass of the segment in the natural coordinate system [3x1] + """ + + @abstractmethod + def _update_mass_matrix(self): + """ + This function returns the generalized mass matrix of the segment, denoted G_i. + + Returns + ------- + np.ndarray + mass matrix of the segment [12 x 12] + """ + + @abstractmethod + def mass_matrix(self): + """ + This function returns the generalized mass matrix of the segment, denoted G_i. + + Returns + ------- + np.ndarray + mass matrix of the segment [12 x 12] + """ + + @abstractmethod + def _interpolation_matrix_center_of_mass(self): + """ + This function returns the interpolation matrix for the center of mass of the segment, denoted N_i^Ci. + It allows to apply the gravity force at the center of mass of the segment. + + Returns + ------- + np.ndarray + Interpolation matrix for the center of mass of the segment in the natural coordinate system [12 x 3] + """ + + @abstractmethod + def interpolation_matrix_center_of_mass(self): + """ + This function returns the interpolation matrix for the center of mass of the segment, denoted N_i^Ci. + It allows to apply the gravity force at the center of mass of the segment. + + Returns + ------- + np.ndarray + Interpolation matrix for the center of mass of the segment in the natural coordinate system [12 x 3] + """ + + @abstractmethod + def weight(self): + """ + This function returns the weight applied on the segment through gravity force. + + Returns + ------- + np.ndarray + Weight applied on the segment through gravity force [12 x 1] + """ + + @abstractmethod + def differential_algebraic_equation( + self, + Qi, + Qdoti, + ): + """ + This function returns the differential algebraic equation of the segment + + Parameters + ---------- + Qi: SegmentNaturalCoordinates + Natural coordinates of the segment + Qdoti: SegmentNaturalCoordinates + Derivative of the natural coordinates of the segment + + Returns + ------- + np.ndarray + Differential algebraic equation of the segment [12 x 1] + """ + + @abstractmethod + def add_marker(self, marker): + """ + Add a new marker to the segment + + Parameters + ---------- + marker + The marker to add + """ + + @abstractmethod + def nb_markers(self): + """ + Returns the number of markers of the natural segment + + Returns + ------- + int + Number of markers of the segment + """ + + @abstractmethod + def marker_constraints(self, marker_locations, Qi): + """ + This function returns the marker constraints of the segment + + Parameters + ---------- + marker_locations: np.ndarray + Marker locations in the global/inertial coordinate system (3 x N_markers) + Qi: SegmentNaturalCoordinates + Natural coordinates of the segment + + Returns + ------- + np.ndarray + The defects of the marker constraints of the segment (3 x N_markers) + """ + + @abstractmethod + def marker_jacobian(self): + """ + This function returns the marker jacobian of the segment + + Returns + ------- + np.ndarray + The jacobian of the marker constraints of the segment (3 x N_markers) + """ From 59f42162285d37eef4238c0db5a5643594db8d17 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Wed, 16 Nov 2022 16:46:10 -0500 Subject: [PATCH 27/30] blacked --- bionc/protocols/natural_segment.py | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/bionc/protocols/natural_segment.py b/bionc/protocols/natural_segment.py index 8d400e6d..a707c43e 100644 --- a/bionc/protocols/natural_segment.py +++ b/bionc/protocols/natural_segment.py @@ -1,4 +1,4 @@ -# import ABC +# import ABC from abc import ABC, abstractmethod from typing import Tuple, Union import numpy as np @@ -44,6 +44,7 @@ class AbstractNaturalSegment(ABC): This function returns the jacobian of the marker constraints of the segment, denoted K_m """ + @abstractmethod def set_name(self, name: str): """ @@ -186,9 +187,7 @@ def segment_coordinates_system(self, Q): """ @abstractmethod - def location_from_homogenous_transform( - self, T - ): + def location_from_homogenous_transform(self, T): """ This function returns the location of the segment in natural coordinate from its homogenous transform From 2e1cd5a6cb37dc826c25028d60bdc052dc7fd769 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Wed, 16 Nov 2022 16:55:04 -0500 Subject: [PATCH 28/30] abstract biomechanical model --- bionc/bionc_casadi/biomechanical_model.py | 3 +- bionc/bionc_numpy/biomechanical_model.py | 3 +- bionc/protocols/biomechanical_model.py | 168 ++++++++++++++++++++++ 3 files changed, 172 insertions(+), 2 deletions(-) create mode 100644 bionc/protocols/biomechanical_model.py diff --git a/bionc/bionc_casadi/biomechanical_model.py b/bionc/bionc_casadi/biomechanical_model.py index 633d2aff..187f5076 100644 --- a/bionc/bionc_casadi/biomechanical_model.py +++ b/bionc/bionc_casadi/biomechanical_model.py @@ -3,9 +3,10 @@ from .natural_coordinates import SegmentNaturalCoordinates, NaturalCoordinates from .natural_velocities import SegmentNaturalVelocities, NaturalVelocities +from ..protocols.biomechanical_model import AbstractBiomechanicalModel -class BiomechanicalModel: +class BiomechanicalModel(AbstractBiomechanicalModel): def __init__(self): from .natural_segment import NaturalSegment # Imported here to prevent from circular imports from .joint import Joint # Imported here to prevent from circular imports diff --git a/bionc/bionc_numpy/biomechanical_model.py b/bionc/bionc_numpy/biomechanical_model.py index cec23fa0..4d29e3b8 100644 --- a/bionc/bionc_numpy/biomechanical_model.py +++ b/bionc/bionc_numpy/biomechanical_model.py @@ -2,9 +2,10 @@ from bionc.protocols.natural_coordinates import SegmentNaturalCoordinates, NaturalCoordinates from bionc.bionc_numpy.natural_velocities import SegmentNaturalVelocities, NaturalVelocities +from ..protocols.biomechanical_model import AbstractBiomechanicalModel -class BiomechanicalModel: +class BiomechanicalModel(AbstractBiomechanicalModel): def __init__(self): from .natural_segment import NaturalSegment # Imported here to prevent from circular imports from .joint import Joint # Imported here to prevent from circular imports diff --git a/bionc/protocols/biomechanical_model.py b/bionc/protocols/biomechanical_model.py new file mode 100644 index 00000000..d4bf991e --- /dev/null +++ b/bionc/protocols/biomechanical_model.py @@ -0,0 +1,168 @@ +import numpy as np + + +from abc import ABC, abstractmethod +from bionc.protocols.natural_coordinates import NaturalCoordinates +from bionc.protocols.natural_velocities import NaturalVelocities + + +class AbstractBiomechanicalModel: + """ + This class is the base class for all biomechanical models. It contains the segments and the joints of the model. + + Methods + ---------- + + + """ + + @abstractmethod + def __getitem__(self, name: str): + """ + This function returns the segment with the given name + + Parameters + ---------- + name : str + Name of the segment + + Returns + ------- + NaturalSegment + The segment with the given name + """ + + @abstractmethod + def __setitem__(self, name: str, segment: "NaturalSegment"): + """ + This function adds a segment to the model + + Parameters + ---------- + name : str + Name of the segment + segment : NaturalSegment + The segment to add + """ + + @abstractmethod + def __str__(self): + """ + This function returns a string representation of the model + """ + + @abstractmethod + def nb_segments(self): + """ + This function returns the number of segments in the model + """ + + @abstractmethod + def nb_markers(self): + """ + This function returns the number of markers in the model + + Returns + ------- + int + number of markers in the model + """ + + @abstractmethod + def nb_joints(self): + """ + This function returns the number of joints in the model + + Returns + ------- + int + number of joints in the model + """ + + @abstractmethod + def nb_Q(self): + """ + This function returns the number of natural coordinates of the model + """ + + @abstractmethod + def nb_Qdot(self): + """ + This function returns the number of natural velocities of the model + """ + + @abstractmethod + def nb_Qddot(self): + """ + This function returns the number of natural accelerations of the model + """ + + @abstractmethod + def rigid_body_constraints(self, Q: NaturalCoordinates): + """ + This function returns the rigid body constraints of all segments, denoted Phi_r + as a function of the natural coordinates Q. + + Returns + ------- + Rigid body constraints of the segment [6 * nb_segments, 1] + """ + + @abstractmethod + def rigid_body_constraints_jacobian(self, Q: NaturalCoordinates): + """ + This function returns the rigid body constraints of all segments, denoted K_r + as a function of the natural coordinates Q. + + Returns + ------- + Rigid body constraints of the segment [6 * nb_segments, nbQ] + """ + + @abstractmethod + def rigid_body_constraint_jacobian_derivative(self, Qdot: NaturalVelocities): + """ + This function returns the derivative of the Jacobian matrix of the rigid body constraints denoted Kr_dot + + Parameters + ---------- + Qdot : NaturalVelocities + The natural velocities of the segment [12, 1] + + Returns + ------- + The derivative of the Jacobian matrix of the rigid body constraints [6, 12] + """ + + @abstractmethod + def _update_mass_matrix(self): + """ + This function computes the generalized mass matrix of the system, denoted G + + Returns + ------- + np.ndarray + generalized mass matrix of the segment [12 * nbSegment x 12 * * nbSegment] + """ + + @abstractmethod + def mass_matrix(self): + """ + This function returns the generalized mass matrix of the system, denoted G + + Returns + ------- + np.ndarray + generalized mass matrix of the segment [12 * nbSegment x 12 * * nbSegment] + + """ + + +# def kinematicConstraints(self, Q): +# # Method to calculate the kinematic constraints + +# def forwardDynamics(self, Q, Qdot): +# +# return Qddot, lambdas + +# def inverseDynamics(self): From 240375b2f02db18bf0ab112a4b682440e8e12ebd Mon Sep 17 00:00:00 2001 From: Ipuch Date: Wed, 16 Nov 2022 16:57:20 -0500 Subject: [PATCH 29/30] delete math interface --- bionc/math_interface.py | 51 ----------------------------------------- 1 file changed, 51 deletions(-) delete mode 100644 bionc/math_interface.py diff --git a/bionc/math_interface.py b/bionc/math_interface.py deleted file mode 100644 index ca42da35..00000000 --- a/bionc/math_interface.py +++ /dev/null @@ -1,51 +0,0 @@ -from casadi import MX, tril, tril2symm -from casadi.casadi import MX as MX_type -import casadi as cas -from numpy import ndarray -import numpy as np - - -def zeros(*args, instance_type: type) -> ndarray | MX: - if instance_type == MX_type: - return MX.zeros(*args) - elif instance_type == ndarray: - return np.zeros(args) - - -def eye(n, instance_type: type) -> ndarray | MX: - if instance_type == MX_type: - return MX.eye(n) - elif instance_type == ndarray: - return np.eye(n) - - -def array(data, instance_type: type) -> ndarray | MX: - if instance_type == MX_type: - return MX(data) - elif instance_type == ndarray: - return np.array(data) - - -def symmetrize_upp(A, instance_type: type) -> ndarray | MX: - """ - This function symmetrizes a matrix by copying the upper triangular part - to the lower triangular part conserving the diagonal. - """ - if instance_type == ndarray: - return np.tril(A) + np.tril(A, -1).T - elif instance_type == MX_type: - return tril2symm(tril(A)) - - -def vertcat(*args, instance_type: type) -> ndarray | MX: - if instance_type == ndarray: - return np.vstack(args) - elif instance_type == MX_type: - return cas.vertcat(*args) - - -def horzcat(*args, instance_type: type) -> ndarray | MX: - if instance_type == ndarray: - return np.hstack(args) - elif instance_type == MX_type: - return cas.horzcat(*args) From e55acbaf88451eb2da15e5b7330d2fd60106be16 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Wed, 16 Nov 2022 17:18:53 -0500 Subject: [PATCH 30/30] restored forward dynamics --- bionc/__init__.py | 9 --- bionc/bionc_casadi/natural_segment.py | 59 ++++++++++++----- bionc/bionc_numpy/natural_segment.py | 65 +++++++++++++------ bionc/protocols/natural_segment.py | 11 ++++ examples/forward_dynamics/forward_dynamics.py | 11 ++-- examples/natural_coordinates.py | 41 ------------ 6 files changed, 106 insertions(+), 90 deletions(-) delete mode 100644 examples/natural_coordinates.py diff --git a/bionc/__init__.py b/bionc/__init__.py index 3e02d46f..937a1669 100644 --- a/bionc/__init__.py +++ b/bionc/__init__.py @@ -21,15 +21,6 @@ BiomechanicalModel, ) -from .math_interface import ( - zeros, - eye, - array, - symmetrize_upp, - vertcat, - horzcat, -) - from .protocols import natural_coordinates from bionc import bionc_casadi from bionc import bionc_numpy diff --git a/bionc/bionc_casadi/natural_segment.py b/bionc/bionc_casadi/natural_segment.py index 90bc07b7..0d239e94 100644 --- a/bionc/bionc_casadi/natural_segment.py +++ b/bionc/bionc_casadi/natural_segment.py @@ -2,7 +2,7 @@ import numpy as np from casadi import MX -from casadi import cos, sin, transpose, norm_2, vertcat, sqrt, inv, dot, tril, tril2symm +from casadi import cos, sin, transpose, norm_2, vertcat, sqrt, inv, dot, tril, tril2symm, sum1, sum2 from ..protocols.natural_coordinates import SegmentNaturalCoordinates, NaturalCoordinates from ..bionc_casadi.natural_velocities import SegmentNaturalVelocities, NaturalVelocities @@ -307,12 +307,12 @@ def rigid_body_constraint(self, Qi: Union[SegmentNaturalCoordinates, np.ndarray] phir = MX.zeros(6) u, v, w = Qi.to_uvw() - phir[0] = u**2 - 1 - phir[1] = u * v - self.length * cos(self.gamma) - phir[2] = u * Qi.w - cos(self.beta) - phir[3] = v**2 - self.length**2 - phir[4] = v * w - self.length * cos(self.alpha) - phir[5] = w**2 - 1 + phir[0] = sum1(u**2) - 1 + phir[1] = dot(u, v) - self.length * cos(self.gamma) + phir[2] = dot(u, w) - cos(self.beta) + phir[3] = sum1(v**2) - self.length**2 + phir[4] = dot(v, w) - self.length * cos(self.alpha) + phir[5] = sum1(w**2) - 1 return phir @@ -351,6 +351,29 @@ def rigid_body_constraint_jacobian(Qi: SegmentNaturalCoordinates) -> MX: return Kr + def rigid_body_constraint_derivative( + self, + Qi: SegmentNaturalCoordinates, + Qdoti: SegmentNaturalVelocities, + ) -> MX: + """ + This function returns the derivative of the rigid body constraints denoted Phi_r_dot + + Parameters + ---------- + Qi : SegmentNaturalCoordinates + The natural coordinates of the segment + Qdoti : SegmentNaturalVelocities + The natural velocities of the segment + + Returns + ------- + MX + Derivative of the rigid body constraints [6 x 1 x N_frame] + """ + + return self.rigid_body_constraint_jacobian(Qi) @ Qdoti.to_vector() + @staticmethod def rigid_body_constraint_jacobian_derivative(Qdoti: SegmentNaturalVelocities) -> MX: """ @@ -557,6 +580,7 @@ def differential_algebraic_equation( self, Qi: Union[SegmentNaturalCoordinates, np.ndarray], Qdoti: Union[SegmentNaturalVelocities, np.ndarray], + stabilization: dict = None, ) -> Tuple[SegmentNaturalAccelerations, np.ndarray]: """ This function returns the differential algebraic equation of the segment @@ -567,28 +591,29 @@ def differential_algebraic_equation( Natural coordinates of the segment Qdoti: SegmentNaturalCoordinates Derivative of the natural coordinates of the segment + stabilization: dict + Dictionary containing the Baumgarte's stabilization parameters: + * alpha: float + Stabilization parameter for the constraint + * beta: float + Stabilization parameter for the constraint derivative Returns ------- MX Differential algebraic equation of the segment [12 x 1] """ - if isinstance(Qi, SegmentNaturalVelocities): - raise TypeError("Qi should be of type SegmentNaturalCoordinates") - if isinstance(Qdoti, SegmentNaturalCoordinates): - raise TypeError("Qdoti should be of type SegmentNaturalVelocities") - - # not able to verify if the types of Qi and Qdoti are np.ndarray - if not isinstance(Qi, SegmentNaturalCoordinates): - Qi = SegmentNaturalCoordinates(Qi) - if not isinstance(Qdoti, SegmentNaturalVelocities): - Qdoti = SegmentNaturalVelocities(Qdoti) Gi = self.mass_matrix Kr = self.rigid_body_constraint_jacobian(Qi) Krdot = self.rigid_body_constraint_jacobian_derivative(Qdoti) biais = Krdot @ Qdoti.vector + if stabilization is not None: + biais -= stabilization["alpha"] * self.rigid_body_constraint(Qi) + stabilization[ + "beta" + ] * self.rigid_body_constraint_derivative(Qi, Qdoti) + A = MX.zeros((18, 18)) A[0:12, 0:12] = Gi A[12:18, 0:12] = Kr diff --git a/bionc/bionc_numpy/natural_segment.py b/bionc/bionc_numpy/natural_segment.py index 338edb1a..bc24427c 100644 --- a/bionc/bionc_numpy/natural_segment.py +++ b/bionc/bionc_numpy/natural_segment.py @@ -1,11 +1,11 @@ from typing import Union, Tuple import numpy as np -from numpy import cos, sin, matmul, eye, zeros, sum +from numpy import cos, sin, matmul, eye, zeros, sum, dot from numpy.linalg import inv # from ..utils.natural_coordinates import SegmentNaturalCoordinates -from ..protocols.natural_coordinates import SegmentNaturalCoordinates, NaturalCoordinates +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 @@ -308,23 +308,15 @@ def rigid_body_constraint(self, Qi: Union[SegmentNaturalCoordinates, np.ndarray] np.ndarray Rigid body constraints of the segment [6 x 1 x N_frame] """ - # if not isinstance(Qi, SegmentNaturalCoordinates): - # Qi = SegmentNaturalCoordinates(Qi) - phir = zeros(6) - # phir[0] = sum(Qi.u**2, 0) - 1 - # phir[1] = sum(Qi.u * (Qi.rp - Qi.rd), 0) - self.length * cos(self.gamma) - # phir[2] = sum(Qi.u * Qi.w, 0) - cos(self.beta) - # phir[3] = sum((Qi.rp - Qi.rd) ** 2, 0) - self.length**2 - # phir[4] = sum((Qi.rp - Qi.rd) * Qi.w, 0) - self.length * cos(self.alpha) - # phir[5] = sum(Qi.w**2, 0) - 1 - - phir[0] = sum(Qi.u**2, 0) - 1 - phir[1] = sum(Qi.u * Qi.v, 0) - self.length * cos(self.gamma) - phir[2] = sum(Qi.u * Qi.w, 0) - cos(self.beta) - phir[3] = sum(Qi.v**2, 0) - self.length**2 - phir[4] = sum(Qi.v * Qi.w, 0) - self.length * cos(self.alpha) - phir[5] = sum(Qi.w**2, 0) - 1 + u, v, w = Qi.to_uvw() + + phir[0] = sum(u**2) - 1 + phir[1] = dot(u, v) - self.length * cos(self.gamma) + phir[2] = dot(u, w) - cos(self.beta) + phir[3] = sum(v**2) - self.length**2 + phir[4] = dot(v, w) - self.length * cos(self.alpha) + phir[5] = sum(w**2) - 1 return phir @@ -363,6 +355,29 @@ def rigid_body_constraint_jacobian(Qi: SegmentNaturalCoordinates) -> np.ndarray: return Kr + def rigid_body_constraint_derivative( + self, + Qi: SegmentNaturalCoordinates, + Qdoti: SegmentNaturalVelocities, + ) -> np.ndarray: + """ + This function returns the derivative of the rigid body constraints denoted Phi_r_dot + + Parameters + ---------- + Qi : SegmentNaturalCoordinates + The natural coordinates of the segment + Qdoti : SegmentNaturalVelocities + The natural velocities of the segment + + Returns + ------- + np.ndarray + Derivative of the rigid body constraints [6 x 1 x N_frame] + """ + + return self.rigid_body_constraint_jacobian(Qi) @ np.array(Qdoti) + @staticmethod def rigid_body_constraint_jacobian_derivative(Qdoti: SegmentNaturalVelocities) -> np.ndarray: """ @@ -569,6 +584,7 @@ def differential_algebraic_equation( self, Qi: Union[SegmentNaturalCoordinates, np.ndarray], Qdoti: Union[SegmentNaturalVelocities, np.ndarray], + stabilization: dict = None, ) -> Tuple[SegmentNaturalAccelerations, np.ndarray]: """ This function returns the differential algebraic equation of the segment @@ -579,6 +595,12 @@ def differential_algebraic_equation( Natural coordinates of the segment Qdoti: SegmentNaturalCoordinates Derivative of the natural coordinates of the segment + stabilization: dict + Dictionary containing the Baumgarte's stabilization parameters: + * alpha: float + Stabilization parameter for the constraint + * beta: float + Stabilization parameter for the constraint derivative Returns ------- @@ -599,7 +621,12 @@ def differential_algebraic_equation( Gi = self.mass_matrix Kr = self.rigid_body_constraint_jacobian(Qi) Krdot = self.rigid_body_constraint_jacobian_derivative(Qdoti) - biais = Krdot @ Qdoti.vector + biais = -Krdot @ Qdoti.vector + + if stabilization is not None: + biais -= stabilization["alpha"] * self.rigid_body_constraint(Qi) + stabilization[ + "beta" + ] * self.rigid_body_constraint_derivative(Qi, Qdoti) A = zeros((18, 18)) A[0:12, 0:12] = Gi diff --git a/bionc/protocols/natural_segment.py b/bionc/protocols/natural_segment.py index a707c43e..013b4c3a 100644 --- a/bionc/protocols/natural_segment.py +++ b/bionc/protocols/natural_segment.py @@ -214,6 +214,17 @@ def rigid_body_constraint(self, Qi): Rigid body constraints of the segment [6 x 1 x N_frame] """ + @abstractmethod + def rigid_body_constraint_derivative(self, Qi): + """ + This function returns the rigid body constraints derivative of the segment, denoted dphi_r/dQ. + + Returns + ------- + np.ndarray + Rigid body constraints derivative of the segment [6 x 6 x N_frame] + """ + @abstractmethod def _pseudo_inertia_matrix(self): """ diff --git a/examples/forward_dynamics/forward_dynamics.py b/examples/forward_dynamics/forward_dynamics.py index c5af1bda..2f0cab7e 100644 --- a/examples/forward_dynamics/forward_dynamics.py +++ b/examples/forward_dynamics/forward_dynamics.py @@ -1,6 +1,6 @@ import numpy as np -from bionc import ( +from bionc.bionc_numpy import ( NaturalSegment, NaturalCoordinates, SegmentNaturalCoordinates, @@ -40,7 +40,7 @@ ) print(my_segment.rigid_body_constraint(Qi)) -print(my_segment.rigid_body_constraint_derivative(Qi, Qidot)) +print(my_segment.rigid_body_constraint_jacobian_derivative(Qidot)) if (my_segment.rigid_body_constraint(Qi) > 1e-6).any(): print(my_segment.rigid_body_constraint(Qi)) @@ -79,8 +79,11 @@ def dynamics(t, states): defects_dot = np.zeros((6, len(time_steps))) center_of_mass = np.zeros((3, len(time_steps))) for i in range(len(time_steps)): - defects[:, i] = my_segment.rigid_body_constraint(all_states[0:12, i]) - defects_dot[:, i] = my_segment.rigid_body_constraint_derivative(all_states[0:12, i], all_states[12:24, i]) + defects[:, i] = my_segment.rigid_body_constraint(SegmentNaturalCoordinates(all_states[0:12, i])) + defects_dot[:, i] = my_segment.rigid_body_constraint_derivative( + SegmentNaturalCoordinates(all_states[0:12, i]), + SegmentNaturalVelocities(all_states[12:24, i]), + ) all_lambdas[:, i] = dynamics(time_steps[i], all_states[:, i])[1] center_of_mass[:, i] = my_segment.interpolation_matrix_center_of_mass @ all_states[0:12, i] diff --git a/examples/natural_coordinates.py b/examples/natural_coordinates.py deleted file mode 100644 index d5e1c27e..00000000 --- a/examples/natural_coordinates.py +++ /dev/null @@ -1,41 +0,0 @@ -import numpy as np -from casadi import MX - -from bionc import array -from bionc import casadi_type -from bionc import bionc_casadi - -# CASADI EXAMPLE -print("CASADI EXAMPLE") -u = array(np.array([1, 2, 3]), instance_type=casadi_type) -rp = array([2, 2, 3], instance_type=casadi_type) -rd = MX.sym("rd", 3) -w = array([4, 2, 3], instance_type=casadi_type) -Q = bionc_casadi.SegmentNaturalCoordinates.from_components(u=u, rp=rp, rd=rd, w=w) - -print(Q) -print(Q.to_array()) -print(Q.u) -print(Q.rp) -print(Q.rp.shape) -print(Q.to_components()) -print("done") - -# NUMPY EXAMPLE -print("NUMPY EXAMPLE") -from bionc import numpy_type -from bionc import bionc_numpy - -u = array(np.array([1, 2, 3]), instance_type=numpy_type) -rp = array([2, 2, 3], instance_type=numpy_type) -rd = np.array([1, 2, 3]) -w = array([4, 2, 3], instance_type=numpy_type) -Q = bionc_numpy.SegmentNaturalCoordinates.from_components(u=u, rp=rp, rd=rd, w=w) - -print(Q) -print(Q.to_array()) -print(Q.u) -print(Q.rp) -print(Q.rp.shape) -print(Q.to_components()) -print("done")