Adding an ellipsoid joint to the joint collection

CHANGELOG.md

@@ -14,6 +14,9 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/).
 - Add docker images ([#2776](https://github.com/stack-of-tasks/pinocchio/pull/2776))
 - ROS: added jrl_cmakemodules dependency ([#2789](https://github.com/stack-of-tasks/pinocchio/pull/2789))
 
+### Added
+- Add Ellipsoid Joint to the joint collection, get ready for biomechanics. ([#....])(todo)
+
 ## [3.8.0] - 2025-09-17
 
 ### Added

bindings/python/parsers/graph/expose-edges.cpp

@@ -36,6 +36,9 @@ namespace pinocchio
     const int JointSphericalZYX::nq;
     const int JointSphericalZYX::nv;
 
+    const int JointEllipsoid::nq;
+    const int JointEllipsoid::nv;
+
     const int JointTranslation::nq;
     const int JointTranslation::nv;
 
@@ -111,6 +114,24 @@ namespace pinocchio
         .def_readonly("nq", &JointSphericalZYX::nq, "Number of configuration variables.")
         .def_readonly("nv", &JointSphericalZYX::nv, "Number of tangent variables.");
 
+      bp::class_<JointEllipsoid>(
+        "JointEllipsoid", "Represents an ellipsoidal joint.",
+        bp::init<>(bp::args("self"), "Default constructor."))
+        .def_readonly("nq", &JointEllipsoid::nq, "Number of configuration variables.")
+        .def_readonly("nv", &JointEllipsoid::nv, "Number of tangent variables.");
+
+      bp::class_<JointEllipsoid>(
+        "JointEllipsoid", "Represents an ellipsoidal joint.",
+        bp::init<double, double, double>(bp::args("self", "radius_a", "radius_b", "radius_c"), "Constructor with radii."))
+        .def_readwrite(
+          "radius_a", &JointEllipsoid::radius_a, "Semi-axis length in the x direction.")
+        .def_readwrite(
+          "radius_b", &JointEllipsoid::radius_b, "Semi-axis length in the y direction.")
+        .def_readwrite(
+          "radius_c", &JointEllipsoid::radius_c, "Semi-axis length in the z direction.")
+        .def_readonly("nq", &JointEllipsoid::nq, "Number of configuration variables.")
+        .def_readonly("nv", &JointEllipsoid::nv, "Number of tangent variables.");
+
       bp::class_<JointTranslation>(
         "JointTranslation", "Represents a translation joint.",
         bp::init<>(bp::args("self"), "Default constructor."))

include/pinocchio/bindings/python/context/generic.hpp

@@ -99,6 +99,9 @@ namespace pinocchio
       typedef JointModelSphericalZYXTpl<Scalar> JointModelSphericalZYX;
       typedef JointDataSphericalZYXTpl<Scalar> JointDataSphericalZYX;
 
+      typedef JointModelEllipsoidTpl<Scalar, Options> JointModelEllipsoid;
+      typedef JointDataEllipsoidTpl<Scalar, Options> JointDataEllipsoid;
+
       typedef JointDataPrismaticTpl<Scalar, Options, 0> JointDataPX;
       typedef JointModelPrismaticTpl<Scalar, Options, 0> JointModelPX;
 

include/pinocchio/bindings/python/multibody/joint/joint.hpp

@@ -57,6 +57,7 @@ namespace pinocchio
             "\n\t- JointModelPrismaticUnaligned: Prismatic joint, with translation axis not "
             "aligned with X, Y, nor Z"
             "\n\t- JointModelSphericalZYX: Spherical joint (3D rotation)"
+            "\n\t- JointModelEllipsoid: Ellipsoidal joint (3D rotation with coupled translations)"
             "\n\t- JointModelTranslation: Translation joint (3D translation)"
             "\n\t- JointModelFreeFlyer: Joint enabling 3D rotation and translations.")
 

include/pinocchio/bindings/python/multibody/joint/joints-datas.hpp

@@ -59,6 +59,18 @@ namespace pinocchio
       return cl.add_property("StU", &JointDataSphericalZYX::StU);
     }
 
+    template<>
+    inline bp::class_<JointDataEllipsoid> &
+    expose_joint_data<JointDataEllipsoid>(bp::class_<JointDataEllipsoid> & cl)
+    {
+      return cl
+        .add_property("S", &JointDataEllipsoid::S)
+        .add_property("Sdot", &JointDataEllipsoid::M)
+        .add_property("StU", &JointDataEllipsoid::StU)
+        .add_property("joint_q", &JointDataEllipsoid::joint_q)
+        .add_property("joint_v", &JointDataEllipsoid::joint_v);
+    }
+
     template<>
     inline bp::class_<JointDataComposite> &
     expose_joint_data<JointDataComposite>(bp::class_<JointDataComposite> & cl)

include/pinocchio/bindings/python/multibody/joint/joints-models.hpp

@@ -247,6 +247,28 @@ namespace pinocchio
         .def_readwrite("pitch", &context::JointModelHZ::m_pitch, "Pitch h of the JointModelHZ.");
     }
 
+    // specialization for JointModelHelical
+    template<>
+    bp::class_<context::JointModelEllipsoid> &
+    expose_joint_model<context::JointModelEllipsoid>(bp::class_<context::JointModelEllipsoid> & cl)
+    {
+      return cl
+        .def(bp::init<context::Scalar, context::Scalar, context::Scalar>(
+          bp::args("self", "radius_a", "radius_b", "radius_c"),
+          "Init JointModelEllipsoid with radii a, b and c"))
+        .def(
+          bp::init<>(bp::args("self"), "Init JointModelEllipsoid with default radii equal to 0.01"))
+        .def_readwrite(
+          "radius_a", &context::JointModelEllipsoid::radius_a,
+          "Radius a of the JointModelEllipsoid along X axis.")
+        .def_readwrite(
+          "radius_b", &context::JointModelEllipsoid::radius_b,
+          "Radius b of the JointModelEllipsoid along Y axis.")
+        .def_readwrite(
+          "radius_c", &context::JointModelEllipsoid::radius_c,
+          "Radius c of the JointModelEllipsoid along Z axis.");
+    }
+
     // specialization for JointModelUniversal
     template<>
     bp::class_<context::JointModelUniversal> &

include/pinocchio/multibody/joint-motion-subspace-generic.hpp

@@ -49,7 +49,7 @@ namespace pinocchio
   {
     typedef
       typename traits<JointMotionSubspaceTpl<Dim, Scalar, Options, MaxDim>>::DenseBase DenseBase;
-    typedef Eigen::Matrix<Scalar, Dim, Dim, Options, MaxDim, MaxDim> ReturnType;
+    typedef Eigen::Matrix<Scalar, Dim, 1, Options, MaxDim, 1> ReturnType;
   };
 
   template<int Dim, typename Scalar, int Options, int MaxDim, typename ForceSet>

include/pinocchio/multibody/joint/fwd.hpp

@@ -85,6 +85,14 @@ namespace pinocchio
   struct JointDataSphericalZYXTpl;
   typedef JointDataSphericalZYXTpl<context::Scalar> JointDataSphericalZYX;
 
+  template<typename Scalar, int Options = context::Options>
+  struct JointModelEllipsoidTpl;
+  typedef JointModelEllipsoidTpl<context::Scalar> JointModelEllipsoid;
+
+  template<typename Scalar, int Options = context::Options>
+  struct JointDataEllipsoidTpl;
+  typedef JointDataEllipsoidTpl<context::Scalar> JointDataEllipsoid;
+
   template<typename Scalar, int Options, int axis>
   struct JointModelPrismaticTpl;
   template<typename Scalar, int Options, int axis>

include/pinocchio/multibody/joint/joint-collection.hpp

@@ -54,6 +54,9 @@ namespace pinocchio
     // Joint Spherical ZYX
     typedef JointModelSphericalZYXTpl<Scalar, Options> JointModelSphericalZYX;
 
+    // Joint Ellipsoid
+    typedef JointModelEllipsoidTpl<Scalar, Options> JointModelEllipsoid;
+
     // Joint Translation
     typedef JointModelTranslationTpl<Scalar, Options> JointModelTranslation;
 
@@ -92,6 +95,7 @@ namespace pinocchio
       JointModelRevoluteUnaligned,
       JointModelSpherical,
       JointModelSphericalZYX,
+      JointModelEllipsoid,
       JointModelPX,
       JointModelPY,
       JointModelPZ,
@@ -141,6 +145,9 @@ namespace pinocchio
     // Joint Spherical ZYX
     typedef JointDataSphericalZYXTpl<Scalar, Options> JointDataSphericalZYX;
 
+    // Joint Ellipsoid
+    typedef JointDataEllipsoidTpl<Scalar, Options> JointDataEllipsoid;
+
     // Joint Translation
     typedef JointDataTranslationTpl<Scalar, Options> JointDataTranslation;
 
@@ -179,6 +186,7 @@ namespace pinocchio
       JointDataRevoluteUnaligned,
       JointDataSpherical,
       JointDataSphericalZYX,
+      JointDataEllipsoid,
       JointDataPX,
       JointDataPY,
       JointDataPZ,