diff --git a/bionc/bionc_casadi/joint.py b/bionc/bionc_casadi/joint.py index ffba4625..ef7013ab 100644 --- a/bionc/bionc_casadi/joint.py +++ b/bionc/bionc_casadi/joint.py @@ -4,6 +4,7 @@ from .natural_segment import NaturalSegment from .natural_coordinates import SegmentNaturalCoordinates from .natural_velocities import SegmentNaturalVelocities +from .natural_marker import NaturalMarker from ..protocols.joint import JointBase from .natural_vector import NaturalVector from ..utils.enums import NaturalAxis, CartesianAxis, EulerSequence, TransformationMatrixType @@ -331,6 +332,8 @@ def __init__( parent: NaturalSegment, child: NaturalSegment, index: int, + parent_point: str = None, + child_point: str = None, projection_basis: EulerSequence = None, parent_basis: TransformationMatrixType = None, child_basis: TransformationMatrixType = None, @@ -339,6 +342,29 @@ def __init__( name, parent, child, index, projection_basis, parent_basis, child_basis ) self.nb_constraints = 3 + self.parent_point = ( + NaturalMarker( + name=f"{self.name}_parent_point", + parent_name=self.parent.name, + position=NaturalVector.distal(), + is_technical=False, + is_anatomical=True, + ) + if parent_point is None + else parent.marker_from_name(parent_point) + ) + + self.child_point = ( + NaturalMarker( + name=f"{self.name}_child_point", + parent_name=self.child.name, + position=NaturalVector.proximal(), + is_technical=False, + is_anatomical=True, + ) + if child_point is None + else child.marker_from_name(child_point) + ) def constraint(self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNaturalCoordinates) -> MX: """ @@ -350,7 +376,10 @@ def constraint(self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNatura MX Kinematic constraints of the joint [3, 1] """ - constraint = Q_parent.rd - Q_child.rp + parent_point_location = self.parent_point.position_in_global(Q_parent) + child_point_location = self.child_point.position_in_global(Q_child) + + constraint = parent_point_location - child_point_location return constraint @@ -358,7 +387,7 @@ def parent_constraint_jacobian( self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNaturalCoordinates ) -> MX: K_k_parent = MX.zeros((self.nb_constraints, 12)) - K_k_parent[:3, 6:9] = MX.eye(3) + K_k_parent[:3, :] = self.parent_point.interpolation_matrix return K_k_parent @@ -366,7 +395,7 @@ def child_constraint_jacobian( self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNaturalCoordinates ) -> MX: K_k_child = MX.zeros((self.nb_constraints, 12)) - K_k_child[:3, 3:6] = -MX.eye(3) + K_k_child[:3, :] = -self.child_point.interpolation_matrix return K_k_child diff --git a/bionc/bionc_numpy/joint.py b/bionc/bionc_numpy/joint.py index 751f37c5..16659dd8 100644 --- a/bionc/bionc_numpy/joint.py +++ b/bionc/bionc_numpy/joint.py @@ -3,6 +3,7 @@ from .natural_segment import NaturalSegment from .natural_coordinates import SegmentNaturalCoordinates from .natural_velocities import SegmentNaturalVelocities +from .natural_marker import NaturalMarker from ..protocols.joint import JointBase from ..utils.enums import NaturalAxis, CartesianAxis, EulerSequence, TransformationMatrixType from .natural_vector import NaturalVector @@ -375,6 +376,8 @@ def __init__( parent: NaturalSegment, child: NaturalSegment, index: int, + parent_point: str = None, + child_point: str = None, projection_basis: EulerSequence = None, parent_basis: TransformationMatrixType = None, child_basis: TransformationMatrixType = None, @@ -383,6 +386,32 @@ def __init__( name, parent, child, index, projection_basis, parent_basis, child_basis, None ) self.nb_constraints = 3 + self.parent_point_str = parent_point + self.child_point_str = child_point # to transfer to casadi later on + + self.parent_point = ( + NaturalMarker( + name=f"{self.name}_parent_point", + parent_name=self.parent.name, + position=NaturalVector.distal(), + is_technical=False, + is_anatomical=True, + ) + if parent_point is None + else parent.marker_from_name(parent_point) + ) + + self.child_point = ( + NaturalMarker( + name=f"{self.name}_child_point", + parent_name=self.child.name, + position=NaturalVector.proximal(), + is_technical=False, + is_anatomical=True, + ) + if child_point is None + else child.marker_from_name(child_point) + ) def constraint(self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNaturalCoordinates) -> np.ndarray: """ @@ -394,15 +423,18 @@ def constraint(self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNatura np.ndarray Kinematic constraints of the joint [3, 1] """ - constraint = Q_parent.rd - Q_child.rp + parent_point_location = self.parent_point.position_in_global(Q_parent) + child_point_location = self.child_point.position_in_global(Q_child) - return constraint + constraint = parent_point_location - child_point_location + + return constraint.squeeze() def parent_constraint_jacobian( self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNaturalCoordinates ) -> np.ndarray: K_k_parent = np.zeros((self.nb_constraints, 12)) - K_k_parent[:3, 6:9] = np.eye(3) + K_k_parent[:3, :] = self.parent_point.interpolation_matrix return K_k_parent @@ -410,7 +442,7 @@ def child_constraint_jacobian( self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNaturalCoordinates ) -> np.ndarray: K_k_child = np.zeros((self.nb_constraints, 12)) - K_k_child[:3, 3:6] = -np.eye(3) + K_k_child[:3, :] = -self.child_point.interpolation_matrix return K_k_child @@ -474,6 +506,8 @@ def to_mx(self): parent=self.parent.to_mx(), child=self.child.to_mx(), index=self.index, + parent_point=self.parent_point_str, + child_point=self.child_point_str, projection_basis=self.projection_basis, parent_basis=self.parent_basis, child_basis=self.child_basis, diff --git a/tests/test_joints.py b/tests/test_joints.py index 5c9400a9..8ebcfee9 100644 --- a/tests/test_joints.py +++ b/tests/test_joints.py @@ -1,7 +1,7 @@ import numpy as np import pytest -from bionc import JointType, NaturalAxis, CartesianAxis +from bionc import JointType, NaturalAxis, CartesianAxis, NaturalMarker from .utils import TestUtils @@ -78,14 +78,41 @@ def test_joints(bionc_type, joint_type: JointType): assert joint.nb_joint_dof == 2 assert joint.nb_constraints == 4 elif joint_type == JointType.SPHERICAL: - joint = Joint.Spherical( - name="spherical", + # Spherical joint can be defined in two different ways: + # 1. The default with no option consist in using the rd point for the parent and rp child segment + # 2. By specifying the joint location in the parent and child segment + + box.add_natural_marker_from_segment_coordinates( + name="P1", + location=[0.1, 0.2, 0.3], + is_anatomical=True, + ) + bbox.add_natural_marker_from_segment_coordinates( + name="P2", + location=[0.2, 0.04, 0.05], + is_anatomical=True, + ) + + joint_distal_proximal = Joint.Spherical( + name="joint_distal_proximal", parent=box, child=bbox, index=0, ) - assert joint.nb_joint_dof == 3 - assert joint.nb_constraints == 3 + assert joint_distal_proximal.nb_joint_dof == 3 + assert joint_distal_proximal.nb_constraints == 3 + + joint_custom_location = Joint.Spherical( + name="joint_custom_location", + parent=box, + child=bbox, + index=1, + parent_point="P1", + child_point="P2", + ) + assert joint_custom_location.nb_joint_dof == 3 + assert joint_custom_location.nb_constraints == 3 + elif joint_type == JointType.GROUND_REVOLUTE: joint = GroundJoint.Hinge( name="hinge", @@ -328,12 +355,13 @@ def test_joints(bionc_type, joint_type: JointType): ) elif joint_type == JointType.SPHERICAL: + ## Spherical joint rd rp TestUtils.assert_equal( - joint.constraint(Q1, Q2), + joint_distal_proximal.constraint(Q1, Q2), np.array([-0.3, 0.9, 0.9]), decimal=6, ) - parent_jacobian, child_jacobian = joint.constraint_jacobian(Q1, Q2) + parent_jacobian, child_jacobian = joint_distal_proximal.constraint_jacobian(Q1, Q2) TestUtils.assert_equal( parent_jacobian, np.array( @@ -357,7 +385,7 @@ def test_joints(bionc_type, joint_type: JointType): decimal=6, ) - parent_jacobian_dot, child_jacobian_dot = joint.constraint_jacobian_derivative(Q1, Q2) + parent_jacobian_dot, child_jacobian_dot = joint_distal_proximal.constraint_jacobian_derivative(Q1, Q2) TestUtils.assert_equal( parent_jacobian_dot, np.zeros((3, 12)), @@ -369,6 +397,75 @@ def test_joints(bionc_type, joint_type: JointType): decimal=6, ) + TestUtils.assert_equal( + joint_custom_location.constraint(Q1, Q2), + np.array([-0.227081, 0.071451, 0.849838]), + decimal=6, + ) + parent_jacobian, child_jacobian = joint_custom_location.constraint_jacobian(Q1, Q2) + + TestUtils.assert_equal( + parent_jacobian, + np.array( + [ + [0.1, 0.0, 0.0, 1.2, 0.0, 0.0, -0.2, 0.0, 0.0, 0.3, 0.0, 0.0], + [0.0, 0.1, 0.0, 0.0, 1.2, 0.0, 0.0, -0.2, 0.0, 0.0, 0.3, 0.0], + [0.0, 0.0, 0.1, 0.0, 0.0, 1.2, 0.0, 0.0, -0.2, 0.0, 0.0, 0.3], + ] + ), + decimal=6, + ) + TestUtils.assert_equal( + child_jacobian, + np.array( + [ + [ + -0.2, + 0.0, + 0.0, + -1.0117535131559996, + 0.0, + 0.0, + 0.01175351315599963, + 0.0, + 0.0, + -0.0107387974365452, + 0.0, + 0.0, + ], + [ + 0.0, + -0.2, + 0.0, + -0.0, + -1.0117535131559996, + 0.0, + 0.0, + 0.01175351315599963, + 0.0, + 0.0, + -0.0107387974365452, + 0.0, + ], + [ + 0.0, + 0.0, + -0.2, + 0.0, + 0.0, + -1.0117535131559996, + 0.0, + 0.0, + 0.01175351315599963, + 0.0, + 0.0, + -0.0107387974365452, + ], + ] + ), + decimal=6, + ) + elif joint_type == JointType.CONSTANT_LENGTH: TestUtils.assert_equal( joint.constraint(Q1, Q2),