Adding a spline joint to the joint collection

CHANGELOG.md

@@ -19,6 +19,7 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/).
 - Add color support for robot meshes in Viser ([#2793](https://github.com/stack-of-tasks/pinocchio/pull/2793))
 - Add Realtime Sanitizer (RTSan) uniittest and CI to track dynamic allocations in main API ([#2809](https://github.com/stack-of-tasks/pinocchio/pull/2809))
 - Add Ellipsoid Joint (3-DOF surface constraint), get ready for biomechanics ([#2797](https://github.com/stack-of-tasks/pinocchio/pull/2797))
+- Add a new spline joint to default joint collection ([#2784](https://github.com/stack-of-tasks/pinocchio/pull/2784))
 
 ### Changed
 - Python version update ([#2802](https://github.com/stack-of-tasks/pinocchio/pull/2802)):

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

@@ -53,6 +53,9 @@ namespace pinocchio
 
     const int JointMimic::nq;
     const int JointMimic::nv;
+
+    const int JointSpline::nq;
+    const int JointSpline::nv;
   } // namespace graph
 
   namespace python
@@ -158,6 +161,21 @@ namespace pinocchio
         .def_readonly("nq", &JointUniversal::nq, "Number of configuration variables.")
         .def_readonly("nv", &JointUniversal::nv, "Number of tangent variables.");
 
+      bp::class_<JointSpline>(
+        "JointSpline", "Represents a spline-based joint.",
+        bp::init<>(bp::args("self"), "Default constructor."))
+        .def(bp::init<int>(bp::args("self", "degree"), "Constructor with degree."))
+        .def(bp::init<const SE3 &, int>(
+          bp::args("self", "ctrlFrame", "degree"),
+          "Constructor with a single control frame and degree."))
+        .def_readwrite("ctrlFrames", &JointSpline::ctrlFrames, "Control frames of the spline.")
+        .def_readwrite("degree", &JointSpline::degree, "Degree of the spline.")
+        .def_readonly("nq", &JointSpline::nq, "Number of configuration variables.")
+        .def_readonly("nv", &JointSpline::nv, "Number of tangent variables.")
+        .def(
+          "addCtrlFrame", &JointSpline::addCtrlFrame, bp::args("self", "ctrlFrame"),
+          "Add a control frame to the spline.");
+
       bp::class_<JointComposite>(
         "JointComposite", "Represents a composite joint.",
         bp::init<>(bp::args("self"), "Default constructor."))

examples/spline-joint.py

@@ -0,0 +1,94 @@
+import time
+
+import hppfcl
+import numpy as np
+import pinocchio as pin
+from pinocchio.visualize import MeshcatVisualizer
+
+
+def generate_random_se3_trajectory(num_steps, radius, num_revolutions, height):
+    """
+    Generates a random trajectory in SE(3) with rotating orientation.
+
+    Args:
+      num_keyframes: The number of random keyframes to generate.
+      num_steps_per_segment: The number of intermediate steps between each keyframe.
+
+    Returns:
+      A list of pinocchio.SE3 objects representing the trajectory.
+    """
+    trajectory = []
+    for i in range(num_steps):
+        # 1. Parameter to track progress along the helix (from 0.0 to 1.0)
+        alpha = float(i) / num_steps
+
+        # 2. Define the translational part (the helical path)
+        # The angle determines the position on the XY plane
+        angle = alpha * num_revolutions * 2 * np.pi
+
+        # Calculate the x, y, z coordinates for the helix
+        translation = np.array(
+            [radius * np.cos(angle), radius * np.sin(angle), alpha * height]
+        )
+
+        # 3. Define the rotational part (a new random orientation at each step)
+        # pin.SE3.Random().rotation generates a random 3x3 rotation matrix
+        random_rotation = pin.SE3.Random().rotation
+
+        # 4. Combine the translation and random rotation into a single SE(3) pose
+        pose = pin.SE3(random_rotation, translation)
+        trajectory.append(pose)
+
+    return trajectory
+
+
+# --- Visualization Setup ---
+
+# Generate the random trajectory
+# Parameters for the helical trajectory
+num_steps = 30
+radius = 1.0
+num_revolutions = 3
+height = 2
+
+trajectory = generate_random_se3_trajectory(num_steps, radius, num_revolutions, height)
+
+# Create a Pinocchio model with a single free-flyer joint
+model = pin.Model()
+joint_id = model.addJoint(
+    0, pin.JointModelSpline(trajectory, 3), pin.SE3.Identity(), "free_flyer"
+)
+
+# Attach a simple visual geometry (a box) to the joint
+visual_model = pin.GeometryModel()
+box_shape = hppfcl.Box(0.1, 0.2, 0.3)
+# The placement of the geometry with respect to the joint frame
+geom_placement = pin.SE3.Identity()
+geom_obj = pin.GeometryObject("box", joint_id, geom_placement, box_shape)
+# Assign a color to the geometry
+geom_obj.meshColor = np.array([1.0, 0.5, 0.5, 1.0])  # RGBA
+visual_model.addGeometryObject(geom_obj)
+
+# --- Main Execution ---
+
+# Initialize the MeshCat visualizer.
+try:
+    viz = MeshcatVisualizer(model, visual_model, visual_model)
+    viz.initViewer(open=True)
+    viz.loadViewerModel()
+except ImportError as e:
+    print("Error while initializing the viewer.")
+    print(e)
+
+
+time.sleep(0.1)
+
+q = pin.neutral(model)
+
+q_vector = np.arange(0, 1, 0.05)
+for q in q_vector:
+    # Display the new configuration.
+    viz.display(np.array([q]))
+
+    # Delay for visualization
+    time.sleep(0.05)

include/pinocchio/algorithm/splines.hpp

@@ -0,0 +1,58 @@
+//
+// Copyright (c) 2025 INRIA
+//
+
+#ifndef __pinocchio_algorithm_splines_hpp__
+#define __pinocchio_algorithm_splines_hpp__
+
+namespace pinocchio
+{
+  /// @brief Helper structure defining a range of indices.
+  /// @details This struct identifies the subset of control frames in a spline that are active
+  /// (i.e., have non-zero basis functions) for a specific spline parameter value.
+  /// Using this local support property allows for efficient computation of the joint
+  /// transformation, S, and bias c.
+  struct SpanIndexes
+  {
+    size_t start_idx;
+    size_t end_idx;
+  };
+
+  /// @brief Algorithm to locate the span for a given B-spline parameter, q.
+  /// @details This struct implements a binary search (FindSpan) to determine which knot span
+  /// a given parameter value falls into. In B-spline curves, a parameter value $u$ implies that
+  /// only $(p+1)$ control points affect the curve at that location (where $p$ is the degree).
+  template<typename Scalar, int Options>
+  struct FindSpan
+  {
+    template<typename ConfigVector, typename KnotsVector>
+    static SpanIndexes run(
+      const Eigen::MatrixBase<ConfigVector> & q,
+      const int degree,
+      const int nbCtrlFrames,
+      const Eigen::MatrixBase<KnotsVector> & knots)
+    {
+      // Edge case: if q is at or beyond the end of the spline parameterization
+      if (q[0] >= 1.0)
+        return {
+          static_cast<size_t>(nbCtrlFrames - (degree + 1)), static_cast<size_t>(nbCtrlFrames)};
+
+      int low = degree;
+      int high = nbCtrlFrames;
+      int mid;
+
+      while (low < high)
+      {
+        mid = low + (high - low) / 2;
+        if (q[0] < knots[mid])
+          high = mid;
+        else
+          low = mid + 1;
+      }
+
+      return {static_cast<size_t>(low - (degree + 1)), static_cast<size_t>(low)};
+    }
+  };
+} // namespace pinocchio
+
+#endif // __pinocchio_algorithm_splines_hpp__

include/pinocchio/autodiff/casadi.hpp

@@ -443,5 +443,6 @@ namespace Eigen
 #include "pinocchio/autodiff/casadi/math/matrix.hpp"
 #include "pinocchio/autodiff/casadi/math/quaternion.hpp"
 #include "pinocchio/autodiff/casadi/math/triangular-matrix.hpp"
+#include "pinocchio/autodiff/casadi/algorithm/splines.hpp"
 
 #endif // #ifndef __pinocchio_autodiff_casadi_hpp__

include/pinocchio/autodiff/casadi/algorithm/splines.hpp

@@ -0,0 +1,33 @@
+//
+// Copyright (c) 2025 INRIA
+//
+
+#ifndef __pinocchio_autodiff_casadi_splines_hpp__
+#define __pinocchio_autodiff_casadi_splines_hpp__
+
+#include "pinocchio/algorithm/splines.hpp"
+
+namespace pinocchio
+{
+  // Fwd Declare
+  struct SpanIndexes;
+
+  template<typename Scalar, int Options>
+  struct FindSpan;
+
+  template<int Options>
+  struct FindSpan<::casadi::SX, Options>
+  {
+    template<typename ConfigVector, typename KnotsVector>
+    static SpanIndexes run(
+      const Eigen::MatrixBase<ConfigVector> & /*q*/,
+      const int /*degree*/,
+      const int nbCtrlFrames,
+      const Eigen::MatrixBase<KnotsVector> & /*knots*/)
+    {
+      return {0, static_cast<size_t>(nbCtrlFrames)};
+    }
+  };
+} // namespace pinocchio
+
+#endif // __pinocchio_autodiff_casadi_splines_hpp__

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

@@ -135,6 +135,9 @@ namespace pinocchio
       typedef JointModelUniversalTpl<Scalar> JointModelUniversal;
       typedef JointDataUniversalTpl<Scalar> JointDataUniversal;
 
+      typedef JointModelSplineTpl<Scalar> JointModelSpline;
+      typedef JointDataSplineTpl<Scalar> JointDataSpline;
+
       typedef JointModelTranslationTpl<Scalar> JointModelTranslation;
       typedef JointDataTranslationTpl<Scalar> JointDataTranslation;
 

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

@@ -79,7 +79,6 @@ namespace pinocchio
         .add_property("pjMi", &JointDataComposite::pjMi)
         .add_property("StU", &JointDataComposite::StU);
     }
-
   } // namespace python
 } // namespace pinocchio
 

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

@@ -290,6 +290,38 @@ namespace pinocchio
           "Second rotation axis of the JointModelUniversal.");
     }
 
+    // Specialization for JointModelSpline
+    template<>
+    bp::class_<context::JointModelSpline> &
+    expose_joint_model<context::JointModelSpline>(bp::class_<context::JointModelSpline> & cl)
+    {
+      return cl
+        .def(bp::init<>(
+          bp::args("self"),
+          "Init an empty joint Spline. Default degree of spline basis function is 3."))
+        .def(bp::init<int>(
+          bp::args("self", "degree"),
+          "Init an empty joint Spline, with the degree of the future basis functions"))
+        .def(bp::init<const ::pinocchio::container::aligned_vector<context::SE3> &, int>(
+          bp::args("self", "controlFrames", "degree"),
+          "Init an empty joint Spline, with a list of controlFrames and the degree of the future "
+          "basis functions"))
+        .def(
+          "setControlFrames", &context::JointModelSpline::setControlFrames, bp::arg("frames"),
+          "Add a vector of frames to the joint. It cannot be changed afterward and no frames can "
+          "be added afterwards either")
+        .def(
+          "makeKnots", &context::JointModelSpline::makeKnots,
+          "generate the knots vector for the spline. Call after all the control frame have been "
+          "added")
+        .def(
+          "computeRelativeMotion", &context::JointModelSpline::computeRelativeMotions,
+          "compute the relative motion between the control frames. Call after all the control "
+          "frames have been added")
+        .def_readwrite(
+          "degree", &context::JointModelSpline::degree, "Degree of the spline basis functions");
+    }
+
     // specialization for JointModelComposite
 
     struct JointModelCompositeAddJointVisitor

include/pinocchio/math/multiprecision.hpp

@@ -7,6 +7,7 @@
 
 #include "pinocchio/math/fwd.hpp"
 
+#include <boost/mpl/bool_fwd.hpp>
 #include <boost/multiprecision/number.hpp>
 #include <boost/random.hpp>
 #include <Eigen/Dense>

include/pinocchio/multibody/joint/fwd.hpp

@@ -138,6 +138,14 @@ namespace pinocchio
   struct JointDataTranslationTpl;
   typedef JointDataTranslationTpl<context::Scalar> JointDataTranslation;
 
+  template<typename Scalar, int Options = context::Options>
+  struct JointModelSplineTpl;
+  typedef JointModelSplineTpl<context::Scalar> JointModelSpline;
+
+  template<typename Scalar, int Options = context::Options>
+  struct JointDataSplineTpl;
+  typedef JointDataSplineTpl<context::Scalar> JointDataSpline;
+
   template<typename Scalar, int Options = context::Options>
   struct JointCollectionDefaultTpl;
   typedef JointCollectionDefaultTpl<context::Scalar> JointCollectionDefault;

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

@@ -85,6 +85,9 @@ namespace pinocchio
     // Joint Universal
     typedef JointModelUniversalTpl<Scalar, Options> JointModelUniversal;
 
+    // Joint Spline
+    typedef JointModelSplineTpl<Scalar, Options> JointModelSpline;
+
     typedef boost::variant<
       //    JointModelVoid,
       JointModelRX,
@@ -110,6 +113,7 @@ namespace pinocchio
       JointModelHz,
       JointModelHelicalUnaligned,
       JointModelUniversal,
+      JointModelSpline,
       boost::recursive_wrapper<JointModelComposite>,
       boost::recursive_wrapper<JointModelMimic>>
       JointModelVariant;
@@ -176,6 +180,9 @@ namespace pinocchio
     // Joint Universal
     typedef JointDataUniversalTpl<Scalar, Options> JointDataUniversal;
 
+    // Joint Spline
+    typedef JointDataSplineTpl<Scalar, Options> JointDataSpline;
+
     typedef boost::variant<
       //    JointDataVoid
       JointDataRX,
@@ -201,6 +208,7 @@ namespace pinocchio
       JointDataHz,
       JointDataHelicalUnaligned,
       JointDataUniversal,
+      JointDataSpline,
       boost::recursive_wrapper<JointDataComposite>,
       boost::recursive_wrapper<JointDataMimic>>
       JointDataVariant;