Adding an ellipsoid joint to the joint collection #2797

Ipuch · 2025-10-23T16:11:58Z

Thanks for taking the time to make and fill out this pull request! I'll be open to any enhancement. Thanks to @MegMll and @LucasJoseph for helping out on this one !

Description

In this PR, we introduce the new ellipsoid joint to pinocchio. This implementation is based on the paper:

Seth A, Sherman M, Eastman P, Delp S. Minimal formulation of joint motion for biomechanisms. Nonlinear Dyn. 2010 Oct 1;62(1):291-303. doi: 10.1007/s11071-010-9717-3. PMID: 21170173; PMCID: PMC3002261.

available here. It has been adapted to the formalism of Pinocchio. It allows defining an ellipsoid on which a solid can glide and rotate with respect to the "normal" of the ellipsoid. The joint is driven as function of angles XYZ, meaningful for biomechanists and thoraco-scapular joint.

Capture.video.du.2025-11-06.23-35-48.webm

Checklist

I have run pre-commit run --all-files or pixi run lint
I have performed a self-review of my own code
I have commented my code where necessary
I have made corresponding changes to the doxygen documentation
I have added tests that prove my fix or feature works
I have updated the CHANGELOG or added the "no changelog" label if it's a CI-related issue
I have updated the README credits section
We have run PINNOCHIO_EIGEN_NO_MALLOC

Extra To-dos

no reverse mode with ellipsoid joint for model-graph
Finite differences for motion subspace derivative Sdot, as we have some non-zero terms in Sdot, we (with @MegMll) feel like it would be a good idea to have a test to check Sdot with finite differences as for S.

We have tested that the ellipsoid joint behave as a ZYX spherical joint when radii are zeros. We have to test the translation part as well with a composite joint.

Rotation part - The ellipsoid joint behaves as a ZYX spherical joint when radii are zeros.
Translation part - Test the ellipsoid joint with a composite joint to check the translation part.
Add doc on how to implement a joint in doc/e-development

Linting

pre-commit run --all-files

Question

Should we add references in the code for what we have implemented ? Doc added in `joint-ellipsoid.hxx
To make it pass I had to modify second Dim and second MaxDim to 1 in this line :

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

Line 52 in 468acc3

typedef Eigen::Matrix<Scalar, Dim, Dim, Options, MaxDim, MaxDim> ReturnType;
Model-graphs, need to make sure the build of the graph happens the expected way for ellipsoid joint -> we can't reverse the joint ellipsoid because the translations and the motion subspace are tight to the parent frame, and it would necessitate to have a new joint definition to get the transform in the child frame. To say it simply, we can't just modify q to express the spatial transform in the child frame.

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.

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

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

unittest/joint-ellipsoid.cpp

unittest/model-graph.cpp

unittest/joint-ellipsoid.cpp

unittest/model-graph.cpp

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

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

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

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

unittest/joint-ellipsoid.cpp

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

README.md

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

examples/ellipsoid-joint-kinematics.py

src/parsers/graph/model-graph.cpp

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

jorisv · 2025-12-04T10:39:17Z

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

+    };
+    typedef JointDataEllipsoidTpl<Scalar, Options> JointDataDerived;
+    typedef JointModelEllipsoidTpl<Scalar, Options> JointModelDerived;
+    // typedef JointMotionSubspaceTpl<3, Scalar, Options, 3> Constraint_t;


Suggested change

// typedef JointMotionSubspaceTpl<3, Scalar, Options, 3> Constraint_t;

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

Ipuch

One last problem with ConstraintForceSetOp which does not compile.

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

Ipuch · 2025-12-04T17:01:44Z

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

+    };
+    typedef JointDataEllipsoidTpl<Scalar, Options> JointDataDerived;
+    typedef JointModelEllipsoidTpl<Scalar, Options> JointModelDerived;
+    // typedef JointMotionSubspaceTpl<3, Scalar, Options, 3> Constraint_t;


forcing subspace with one column finite diff for S Sdot

changelog again

precommit again

scooting spherical ZYX clean(joint-ellipsoid): extra include removed hard precommit

Co-authored-by: Joris Vaillant <[email protected]>

precommit

This reverts commit 36e708a.

precommit

Co-authored-by: Joris Vaillant <[email protected]> precommit

precommit

jcarpent · 2025-12-08T16:43:05Z

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

+          "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.")


I would recommend changing the naming here. Something like radius_x or semi_axis_x. a, b, c is a bit meaningless.

jcarpent · 2025-12-08T16:45:47Z

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

+    {
+      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like, 3);
+      // Compute first 6 rows from first 2 columns
+      Eigen::Matrix<Scalar, 6, 1> result = S.template block<6, 2>(0, 0) * v.template segment<2>(0);


Suggested change

Eigen::Matrix<Scalar, 6, 1> result = S.template block<6, 2>(0, 0) * v.template segment<2>(0);

Eigen::Matrix<Scalar, 6, 1> result = S.template leftCols<2>() * v.template head<2>();

jcarpent · 2025-12-08T16:47:34Z

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

+    /// @brief Default constructor. Creates a sphere (0.01, 0.01, 0.01).
+    JointModelEllipsoidTpl()
+    : radius_a(Scalar(0.01))
+    , radius_b(Scalar(0.01))
+    , radius_c(Scalar(0.01))
+    {
+    }


The default constructor should not have such values. It is better to set everything to 0 or 1.

jcarpent · 2025-12-08T16:49:31Z

include/pinocchio/parsers/graph/joints.hpp

+      double radius_a = 0.01;
+      double radius_b = 0.01;
+      double radius_c = 0.01;


Default values should be changed?

jcarpent

Thanks @Ipuch @LucasJoseph and @MegMll for making this nice add-on to Pinocchio.
I made some minor remarks about member naming notably.

github-actions bot reviewed Oct 23, 2025

View reviewed changes

MegMll added build_casadi build_autodiff build_codegen build_mpfr labels Oct 23, 2025

Ipuch force-pushed the topic-ellipsoid branch 2 times, most recently from 0e2e9e8 to f401813 Compare October 26, 2025 14:35

MegMll reviewed Oct 27, 2025

View reviewed changes

Ipuch force-pushed the topic-ellipsoid branch 4 times, most recently from bf2867e to 31f949c Compare November 3, 2025 17:25

Ipuch force-pushed the topic-ellipsoid branch from 6fc4fdc to 6edb477 Compare November 6, 2025 20:46

jorisv requested changes Nov 12, 2025

View reviewed changes

Ipuch force-pushed the topic-ellipsoid branch 3 times, most recently from 0ea8243 to 4733edb Compare November 13, 2025 16:35

Ipuch marked this pull request as ready for review November 13, 2025 18:02

jorisv requested changes Nov 18, 2025

View reviewed changes

bindings/python/parsers/graph/expose-edges.cpp Show resolved Hide resolved

examples/ellipsoid-joint-kinematics.py Show resolved Hide resolved

jorisv requested changes Nov 18, 2025

View reviewed changes

src/parsers/graph/model-graph.cpp Show resolved Hide resolved

Ipuch force-pushed the topic-ellipsoid branch from 21587ba to c215a78 Compare November 19, 2025 17:01

jorisv force-pushed the topic-ellipsoid branch from c215a78 to ada9fb1 Compare November 25, 2025 12:44

Ipuch force-pushed the topic-ellipsoid branch from fa6bb97 to 25bbc67 Compare December 3, 2025 17:25

jorisv requested changes Dec 4, 2025

View reviewed changes

Ipuch force-pushed the topic-ellipsoid branch from ea5fa9e to 4d97d00 Compare December 4, 2025 17:03

Ipuch commented Dec 4, 2025

View reviewed changes

Ipuch force-pushed the topic-ellipsoid branch 2 times, most recently from 0877a59 to 2644cc0 Compare December 8, 2025 14:40

feat(joint-ellipsoid)

35d19a7

forcing subspace with one column finite diff for S Sdot

ipuch and others added 22 commits December 8, 2025 15:58

docs(joint-ellipsoid): changelog and readme

97235d0

changelog again

precommit

ee16242

precommit again

docs(joint-ellipsoid) - afeatures

fa20746

scooting spherical ZYX clean(joint-ellipsoid): extra include removed hard precommit

Update include/pinocchio/multibody/joint/joint-ellipsoid.hpp

bf23476

Co-authored-by: Joris Vaillant <[email protected]>

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

add7ffd

Co-authored-by: Joris Vaillant <[email protected]>

review(@jorisv): docs and test

c186a48

scout(freeflyer docs)

3d62589

refactor(model-graph-algo): AddJointBetweenBodies

63d012f

precommit

doc(new joint): guide for developers.

8697c01

doc: Fix doxygen doc parsing

bf4bd42

core: Avoid cast build issue

9283dee

python: Remove redundant member definition

0709bee

lint: Run linter

55b477d

examples: Try to overcome a Windows encoding issue

60f7246

Revert "examples: Try to overcome a Windows encoding issue"

6295c0c

This reverts commit 36e708a.

ci: Another try to fix greek letter output issue

6b732b3

feat/refactor(joint-ellipsoid): joint specialization

3e20234

precommit

refactor(joint-ellipsoid): review comments.

63fe853

Co-authored-by: Joris Vaillant <[email protected]> precommit

fix(joint-ellipsoid): one more test fixed

80fbd41

wontfix: test not passing

6d43121

core: Fix build and tests

567be9e

clean(joint-ellipsoid): dead code and comments

3850bf0

precommit

jorisv force-pushed the topic-ellipsoid branch from 2644cc0 to 3850bf0 Compare December 8, 2025 15:06

core: Small improvments

5712e77

jorisv approved these changes Dec 8, 2025

View reviewed changes

jcarpent reviewed Dec 8, 2025

View reviewed changes

jcarpent requested changes Dec 8, 2025

View reviewed changes

	Eigen::Matrix<Scalar, 6, 1> result = S.template block<6, 2>(0, 0) * v.template segment<2>(0);
	Eigen::Matrix<Scalar, 6, 1> result = S.template leftCols<2>() * v.template head<2>();

Adding an ellipsoid joint to the joint collection #2797

Are you sure you want to change the base?

Adding an ellipsoid joint to the joint collection #2797

Conversation

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

Uh oh!

Description

Checklist

Extra To-dos

Linting

Question

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!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

jorisv Dec 4, 2025

Choose a reason for hiding this comment

Uh oh!

Ipuch Dec 4, 2025

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Ipuch 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!

Ipuch Dec 4, 2025

Choose a reason for hiding this comment

Uh oh!

jcarpent Dec 8, 2025

Choose a reason for hiding this comment

Uh oh!

jcarpent Dec 8, 2025

Choose a reason for hiding this comment

Uh oh!

jcarpent Dec 8, 2025

Choose a reason for hiding this comment

Uh oh!

Ipuch commented Oct 23, 2025 •

edited

Loading