[WIP] #76 Add General Spherical joint

bionc/bionc_casadi/enums.py

@@ -18,6 +18,7 @@ class JointType(Enum):
     # PRISMATIC = "not implemented yet"
     UNIVERSAL = Joint.Universal
     SPHERICAL = Joint.Spherical
+    GENERAL_SPHERICAL = Joint.GeneralSpherical
     SPHERE_ON_PLANE = Joint.SphereOnPlane
 
     # PLANAR = "planar"

bionc/bionc_casadi/joint.py

@@ -412,6 +412,117 @@ def constraint_jacobian_derivative(
                 Qdot_parent, Qdot_child
             ), self.child_constraint_jacobian_derivative(Qdot_parent, Qdot_child)
 
+    class GeneralSpherical(JointBase):
+        def __init__(
+            self,
+            name: str,
+            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,
+        ):
+            super(Joint.GeneralSpherical, self).__init__(
+                name, parent, child, index, projection_basis, parent_basis, child_basis
+            )
+
+            if parent_point is None:
+                raise ValueError("parent_point must be provided")
+            if child_point is None:
+                raise ValueError("child_point must be provided")
+
+            self.nb_constraints = 3
+            self.parent_point = parent.marker_from_name(parent_point)
+            self.child_point = child.marker_from_name(child_point)
+
+        @property
+        def nb_joint_dof(self) -> int:
+            """
+            erase the parent method because remove no proper dof when looking at absolute joint rotation and torques.
+            ex : one constant length won't block rotations and translations of the child segment
+            """
+            return 3
+
+        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.
+
+            Returns
+            -------
+            MX
+                Kinematic constraints of the joint [3, 1]
+            """
+            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
+
+        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] = self.parent_point.interpolation_matrix
+
+            return K_k_parent
+
+        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] = -self.child_point.interpolation_matrix
+
+            return K_k_child
+
+        def parent_constraint_jacobian_derivative(
+            self, Qdot_parent: SegmentNaturalVelocities, Qdot_child: SegmentNaturalVelocities
+        ) -> MX:
+            K_k_parent = MX.zeros((self.nb_constraints, 12))
+            return K_k_parent
+
+        def child_constraint_jacobian_derivative(
+            self, Qdot_parent: SegmentNaturalVelocities, Qdot_child: SegmentNaturalVelocities
+        ) -> MX:
+            K_k_child = MX.zeros((self.nb_constraints, 12))
+            return K_k_child
+
+        def constraint_jacobian(
+            self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNaturalCoordinates
+        ) -> tuple[MX, MX]:
+            """
+            This function returns the kinematic constraints of the joint, denoted K_k
+            as a function of the natural coordinates Q_parent and Q_child.
+
+            Returns
+            -------
+            tuple[MX, MX]
+                Kinematic constraints of the joint [1, 12] for parent and child
+            """
+
+            return self.parent_constraint_jacobian(Q_parent, Q_child), self.child_constraint_jacobian(Q_parent, Q_child)
+
+        def constraint_jacobian_derivative(
+            self, Qdot_parent: SegmentNaturalVelocities, Qdot_child: SegmentNaturalVelocities
+        ) -> tuple[MX, MX]:
+            """
+            This function returns the kinematic constraints of the joint, denoted K_k
+            as a function of the natural coordinates Q_parent and Q_child.
+
+            Returns
+            -------
+            tuple[MX, MX]
+                Kinematic constraints of the joint [1, 12] for parent and child
+            """
+
+            return self.parent_constraint_jacobian_derivative(
+                Qdot_parent, Qdot_child
+            ), self.child_constraint_jacobian_derivative(Qdot_parent, Qdot_child)
+
     class SphereOnPlane(JointBase):
         """
         This class represents a sphere-on-plane joint: parent is the sphere, and child is the plane.

bionc/bionc_numpy/enums.py

@@ -18,6 +18,7 @@ class JointType(Enum):
     # PRISMATIC = "not implemented yet"
     UNIVERSAL = Joint.Universal
     SPHERICAL = Joint.Spherical
+    GENERAL_SPHERICAL = Joint.GeneralSpherical
     SPHERE_ON_PLANE = Joint.SphereOnPlane
 
     # PLANAR = "planar"

bionc/bionc_numpy/joint.py

@@ -479,6 +479,150 @@ def to_mx(self):
                 child_basis=self.child_basis,
             )
 
+    class GeneralSpherical(JointBase):
+        def __init__(
+            self,
+            name: str,
+            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,
+        ):
+            super(Joint.GeneralSpherical, self).__init__(
+                name,
+                parent,
+                child,
+                index,
+                projection_basis,
+                parent_basis,
+                child_basis,
+                # None # a quoi celà corresponds ici ?
+            )
+
+            if parent_point is None:
+                raise ValueError("parent_point must be provided")
+            if child_point is None:
+                raise ValueError("child_point must be provided")
+
+            self.nb_constraints = 3
+            self.parent_point = parent.marker_from_name(parent_point)
+            self.child_point = child.marker_from_name(child_point)
+
+        @property
+        def nb_joint_dof(self) -> int:
+            """
+            erase the parent method because remove no proper dof when looking at absolute joint rotation and torques.
+            ex : one constant length won't block rotations and translations of the child segment
+            """
+            return 3
+
+        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]
+            """
+            parent_point_location = self.parent_point.interpolation_matrix @ Q_parent
+            child_point_location = self.child_point.interpolation_matrix @ Q_child
+
+            constraint = parent_point_location - child_point_location
+
+            return constraint
+
+        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] = self.parent_point.interpolation_matrix
+
+            return K_k_parent
+
+        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] = -self.child_point.interpolation_matrix
+
+            return K_k_child
+
+        def parent_constraint_jacobian_derivative(
+            self, Qdot_parent: SegmentNaturalVelocities, Qdot_child: SegmentNaturalVelocities
+        ) -> np.ndarray:
+            K_k_parent_dot = np.zeros((self.nb_constraints, 12))
+
+            return K_k_parent_dot
+
+        def child_constraint_jacobian_derivative(
+            self, Qdot_parent: SegmentNaturalVelocities, Qdot_child: SegmentNaturalVelocities
+        ) -> np.ndarray:
+            K_k_child_dot = np.zeros((self.nb_constraints, 12))
+
+            return K_k_child_dot
+
+        def constraint_jacobian(
+            self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNaturalCoordinates
+        ) -> tuple[np.ndarray, np.ndarray]:
+            """
+            This function returns the kinematic constraints of the joint, denoted K_k
+            as a function of the natural coordinates Q_parent and Q_child.
+
+            Returns
+            -------
+            tuple[np.ndarray, np.ndarray]
+                Kinematic constraints of the joint [1, 12] for parent and child
+            """
+
+            return self.parent_constraint_jacobian(Q_parent, Q_child), self.child_constraint_jacobian(Q_parent, Q_child)
+
+        def constraint_jacobian_derivative(
+            self, Qdot_parent: SegmentNaturalVelocities, Qdot_child: SegmentNaturalVelocities
+        ) -> tuple[np.ndarray, np.ndarray]:
+            """
+            This function returns the kinematic constraints of the joint, denoted K_k
+            as a function of the natural coordinates Q_parent and Q_child.
+
+            Returns
+            -------
+            tuple[np.ndarray, np.ndarray]
+                Kinematic constraints of the joint [1, 12] for parent and child
+            """
+
+            return self.parent_constraint_jacobian_derivative(
+                Qdot_parent, Qdot_child
+            ), self.child_constraint_jacobian_derivative(Qdot_parent, Qdot_child)
+
+        def to_mx(self):
+            """
+            This function returns the joint as a mx joint
+
+            Returns
+            -------
+            JointBase
+                The joint as a mx joint
+            """
+            # TODO: implement this in joint casadi
+            from ..bionc_casadi.joint import Joint as CasadiJoint
+
+            return CasadiJoint.GeneralSpherical(
+                name=self.name,
+                parent=self.parent.to_mx(),
+                child=self.child.to_mx(),
+                index=self.index,
+                parent_point=self.parent_point.name,
+                child_point=self.child_point.name,
+                projection_basis=self.projection_basis,
+                parent_basis=self.parent_basis,
+                child_basis=self.child_basis,
+            )
+
     class SphereOnPlane(JointBase):
         """
         This class represents a sphere-on-plane joint: parent is the sphere, and child is the plane.