Adding a spline joint to the joint collection #2784

MegMll · 2025-10-10T09:36:35Z

Description

In this PR, we introduce the new spline joint to pinocchio. This implementation is based on the paper of Lee et al. available here

It allows to define a number of SE3 frames, that will define the movement and dynamics of the joint. It's very useful for biomechanics, but can be used in other applications.

For now the joint configuration has to be bounded between 0 and 1, otherwise the transformation computation will fail.

To do

Add better tests
Remove the change in act-on-set.hxx
Remove dynamic allocation in the bias computation
use the reduce sum instead of the full sum over all control frames
Make it casadi, cppad, cppadcg, boost::mpfr compatible
add it to model-graph
Need to redefine lieGroupType to avoid generating random numbers outside of bounds [0, 1]
update changelog

Future

Add a fit spline algorithm
Introduce lower and upper bounds

github-actions

👋 Hi,
This is a reminder message to assign an extra build label to this Pull Request if needed.
By default, this PR will be build with minimal build options (URDF support and Python bindings)
The possible extra labels are:

build_collision (build Pinocchio with coal support)
build_casadi (build Pinocchio with CasADi support)
build_autodiff (build Pinocchio with CppAD support)
build_codegen (build Pinocchio with CppADCodeGen support)
build_extra (build Pinocchio with extra algorithms)
build_mpfr (build Pinocchio with Boost.Multiprecision support)
build_sdf (build Pinocchio with SDF parser)
build_accelerate (build Pinocchio with APPLE Accelerate framework support)
build_all (build Pinocchio with ALL the options stated above)

Thanks.
The Pinocchio development team.

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

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

include/pinocchio/serialization/joints-data.hpp

include/pinocchio/serialization/joints-model.hpp

include/pinocchio/algorithm/splines.hpp

jorisv · 2025-11-25T14:47:22Z

include/pinocchio/parsers/graph/graph-visitor.hpp

+        SE3 operator()(const JointSpline &) const
+        {
+          PINOCCHIO_THROW_PRETTY(
+            std::invalid_argument,
+            "Graph - Joint Spline cannot have a q_ref. Please use the joint offset directly.");
+        }


Why ? It's strange but I don't get why it's not possible.

jorisv · 2025-11-25T14:52:22Z

src/parsers/graph/model-graph-algo.cpp

+          if (!edge.forward)
+            PINOCCHIO_THROW_PRETTY(
+              std::invalid_argument, "Graph - JointSpline cannot be reversed.");


We have to think about that.

Right now I'm not very confident about the reversal of this joint.
I don't think creating a ctrlFrames vector with inverse transformation inside will give us the inverse transformation.

jorisv · 2025-11-25T14:54:15Z

unittest/casadi/joints.cpp

+    PINOCCHIO_ALIGNED_STD_VECTOR(SE3) ctrlFrames;
+    ctrlFrames.push_back(SE3::Identity());
+    ctrlFrames.push_back(SE3(Matrix3s::Identity(), Vector3s(Scalar(0.), Scalar(0.), Scalar(1.))));
+    JointModel jmodel(ctrlFrames, 1);


We should create a degree 3 spline so bsplineBasisDerivative and bsplineBasisDerivative2 are called.

jorisv · 2025-11-25T14:54:38Z

unittest/casadi/joints.cpp

+    PINOCCHIO_ALIGNED_STD_VECTOR(SE3) ctrlFrames;
+    ctrlFrames.push_back(SE3::Identity());
+    ctrlFrames.push_back(SE3(Matrix3s::Identity(), Vector3s(Scalar(0.), Scalar(0.), Scalar(1.))));
+    JointModel jmodel(ctrlFrames, 1);


We should create a degree 3 spline so bsplineBasisDerivative and bsplineBasisDerivative2 are called.

jorisv · 2025-11-25T14:56:11Z

unittest/cppad/joints.cpp

+    PINOCCHIO_ALIGNED_STD_VECTOR(SE3) ctrlFrames;
+    ctrlFrames.push_back(SE3::Identity());
+    ctrlFrames.push_back(SE3(Matrix3s::Identity(), Vector3s(Scalar(0.), Scalar(0.), Scalar(1.))));
+    JointModel jmodel(ctrlFrames, 1);


We should create a degree 3 spline so bsplineBasisDerivative and bsplineBasisDerivative2 are called.

jorisv · 2025-11-25T14:57:03Z

unittest/finite-differences.cpp

+    PINOCCHIO_ALIGNED_STD_VECTOR(pinocchio::SE3) ctrlFrames;
+    ctrlFrames.push_back(pinocchio::SE3::Identity());
+    ctrlFrames.push_back(pinocchio::SE3(Eigen::Matrix3d::Identity(), Eigen::Vector3d(0., 0., 1.)));
+    JointModel jmodel(ctrlFrames, 1);


We should create a degree 3 spline so bsplineBasisDerivative and bsplineBasisDerivative2 are called.

jorisv · 2025-11-26T09:32:49Z

unittest/casadi/joints.cpp

+    ConfigVector lb(ConfigVector::Constant(jmodel.nq(), 0.));
+    ConfigVector ub(ConfigVector::Constant(jmodel.nq(), 1.));


The only difference between generic test function and specialized one for JointModelSplineTpl is the configuration bound.
Is there way to specialize RandomConfigurationStep to handle that ?
If there is no way, can generate the bounds from TestADOnJoints visitor to avoid duplicating the code.

we can specialize the RandomConfigurationStep, like it's done for CompositeJoint, indeed. I'll do it, and you let me know, what you think.

jorisv · 2025-11-26T09:37:13Z

unittest/finite-differences.cpp

+    std::cout << "name: " << jmodel.classname() << std::endl;
+    JointDataSpline jdata = jmodel.createData();
+
+    CV q = LieGroupType().randomConfiguration(CV::Zero(), CV::Ones());


The only difference here is randomConfiguration generation. Is there a way to handle it in LieGroup ?
Otherwise, it could be nice to generate it from the visitor to avoid duplicating the code.

i checked, and it requires a new lieGroup, from my understanding. So i will generate bounds from the visitors

jorisv · 2025-11-26T09:45:36Z

unittest/joint-generic.cpp

+  q1 = LieGroupType().randomConfiguration(
+    Eigen::VectorXd::Zero(jmodel.nq()), Eigen::VectorXd::Ones(jmodel.nq()));
+  q2 = LieGroupType().randomConfiguration(
+    Eigen::VectorXd::Zero(jmodel.nq()), Eigen::VectorXd::Ones(jmodel.nq()));


Same here, randomConfiguration is the only difference between generic and spline joint.
If there is no way to specialize randomConfiguration, we should add a mechanism to this test to generate a valid randomConfiguration without duplicating the test.

jorisv · 2025-11-26T09:46:09Z

unittest/joint-generic.cpp

+    PINOCCHIO_ALIGNED_STD_VECTOR(SE3) ctrlFrames;
+    ctrlFrames.push_back(SE3::Identity());
+    ctrlFrames.push_back(SE3(Eigen::Matrix3d::Identity(), Eigen::Vector3d(0., 0., 1.)));
+    JointModel jmodel(ctrlFrames, 1);


We should create an order 3 splines to test all the joint.

unittest/joint-spline.cpp

jorisv · 2025-11-26T10:14:54Z

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

Is there a way that neutral and randomConfiguration return us a valid spline configuration ?

neutral is working, because it returns 0. However if we want random, we need to redefine a lieGroup...

jorisv · 2025-11-26T10:15:15Z

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

Do we want to manage spline with abritrary bounds ?

jorisv · 2025-11-26T15:10:28Z

unittest/finite-differences.cpp


 template<typename JointModel_>
-struct init;
+struct JmodelWithBounds


Maybe we should use a more generic name (if with want to add more attribute in the future).
Maybe JointModelWithParameters.

github-actions bot reviewed Oct 10, 2025

View reviewed changes

pgraverdy added the pr status wip To not review in weekly meeting label Oct 13, 2025

MegMll force-pushed the topic/spline_joint branch from f464bc1 to 660af02 Compare October 13, 2025 11:43

MegMll added the build_all label Oct 15, 2025

jorisv requested changes Oct 16, 2025

View reviewed changes

MegMll force-pushed the topic/spline_joint branch from cfe3213 to 3a119f8 Compare October 21, 2025 07:42

jorisv added build_casadi build_autodiff build_codegen build_mpfr and removed build_all labels Oct 21, 2025

MegMll force-pushed the topic/spline_joint branch from 3c00669 to 1e5d3d3 Compare October 23, 2025 08:18

MegMll force-pushed the topic/spline_joint branch from aab0161 to bf16106 Compare October 30, 2025 14:37

MegMll added 17 commits November 25, 2025 14:44

Joints - Add a first implementation of joint spline

7a5c37e

Unittest - Add TU for spline joint

2f6dd50

Bindings - add bindings for spline joint

1fd4295

Spatial - Weird fix to make spline joint work for now (to remove)

f169e9c

Unittest - update joint unittests to include spline joints

936c1d4

SplineJoint - apply pre-commit

7109553

Spline - use the true cumulative form of the spline

d42f298

Spline - Add isEqual Method

c9c780a

Spline - Add TU with finite difference

245bc08

Unittest - fix tests for spline joint

87c9314

Spline joint - change if/else to be casadi compatible

343c4fa

Joint Spline - Remove useless comments

befe587

Joint Spline - Fix indexing

ba09e10

Joint Spline - Eigen No Malloc checked and passed

8e6fac3

Spline joints - fix casadi joints tests

835a3be

Joint Spline - Add a build method

6bf8e04

Unittest - Fix joint spline's unittests

2c73df3

MegMll added 21 commits November 25, 2025 14:44

JointSpline - Fix indexing

26bf375

JointSpline - Add a doc

5887519

JointSpline - Fix bias computation

618c495

Unittest - add a finite difference test for S and bias c for JointSpline

db32494

JointSpline - Add comments and complete isEqual function

de1ef3a

Unittest - Try to add tolerance to see if it fixes macos-intel problem

fe4b5ca

Unittest - fix compilation error

78ec286

JointSpline - add assert for configuration to be between 0 and 1

3f4b6a6

Bindings - Add graph spline joints bindings

f9c336c

Algo/Splines - remove comments

d89552b

Unittest - add find span test for q = 0

e5fbd70

Changelog - update

1097d4f

JointSpline - remove SE3 and use Transformation_t

4b42132

Unittest - remove comments

d5b210a

JointSpline - Use transformation_t and motion_t

2ffe305

Examples - Add a small examples on what the spline joint can do

a2b0703

Unittest - Specialize for joint spline

f2e5c30

Examples - add spline joint examples

4768a32

Unittest - Fix joint generic test for joint spline

079720e

Examples - Fix some linting errors

ef9b1e3

JointSpline - Disable assert

86b40d7

jorisv force-pushed the topic/spline_joint branch from b4923b1 to 86b40d7 Compare November 25, 2025 13:45

jorisv requested changes Nov 25, 2025

View reviewed changes

jorisv requested changes Nov 26, 2025

View reviewed changes

core: Apply small changes

096a5ec

jorisv force-pushed the topic/spline_joint branch from 2e60ca8 to 096a5ec Compare November 26, 2025 10:22

MegMll added 3 commits November 26, 2025 15:55

Algo/splines - add doc

a94b07b

Unittest - Avoid code duplication for randomConfig vector generation

f4421ae

Unittest - more complex and complete tests

a8c2e58

jorisv requested changes Nov 26, 2025

View reviewed changes

		ConfigVector lb(ConfigVector::Constant(jmodel.nq(), 0.));
		ConfigVector ub(ConfigVector::Constant(jmodel.nq(), 1.));

Adding a spline joint to the joint collection #2784

Are you sure you want to change the base?

Adding a spline joint to the joint collection #2784

Conversation

MegMll commented Oct 10, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Description

To do

Future

Uh oh!

github-actions bot left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants

MegMll commented Oct 10, 2025 •

edited

Loading