From da4aab8ec6ab8767d257b5298aba734b88598c01 Mon Sep 17 00:00:00 2001 From: ipuch Date: Mon, 20 Oct 2025 16:06:27 +0200 Subject: [PATCH 01/48] feat(joint-ellipsoid) forcing subspace with one column finite diff for S Sdot --- CHANGELOG.md | 3 + .../joint-motion-subspace-generic.hpp | 2 +- include/pinocchio/multibody/joint/fwd.hpp | 8 + .../multibody/joint/joint-collection.hpp | 8 + .../multibody/joint/joint-ellipsoid.hpp | 745 ++++++++++++++++++ include/pinocchio/multibody/joint/joints.hpp | 3 +- .../pinocchio/serialization/joints-data.hpp | 10 + .../pinocchio/serialization/joints-model.hpp | 13 + sources.cmake | 1 + unittest/CMakeLists.txt | 1 + unittest/all-joints.cpp | 18 + unittest/finite-differences.cpp | 17 + unittest/joint-ellipsoid.cpp | 362 +++++++++ unittest/joint-generic.cpp | 17 + unittest/serialization.cpp | 17 + 15 files changed, 1223 insertions(+), 2 deletions(-) create mode 100644 include/pinocchio/multibody/joint/joint-ellipsoid.hpp create mode 100644 unittest/joint-ellipsoid.cpp diff --git a/CHANGELOG.md b/CHANGELOG.md index d6f58f5bc3..3e0657f72b 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -36,6 +36,9 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/). - Remove CMake < 3.22 details ([#2790](https://github.com/stack-of-tasks/pinocchio/pull/2790)) +### Added +- Add Ellipsoid Joint to the joint collection, get ready for biomechanics. ([#....])(todo) + ## [3.8.0] - 2025-09-17 ### Added diff --git a/include/pinocchio/multibody/joint-motion-subspace-generic.hpp b/include/pinocchio/multibody/joint-motion-subspace-generic.hpp index 9906d404a8..9cc2726d99 100644 --- a/include/pinocchio/multibody/joint-motion-subspace-generic.hpp +++ b/include/pinocchio/multibody/joint-motion-subspace-generic.hpp @@ -49,7 +49,7 @@ namespace pinocchio { typedef typename traits>::DenseBase DenseBase; - typedef Eigen::Matrix ReturnType; + typedef Eigen::Matrix ReturnType; }; template diff --git a/include/pinocchio/multibody/joint/fwd.hpp b/include/pinocchio/multibody/joint/fwd.hpp index 774111bb1e..197ed2d4f1 100644 --- a/include/pinocchio/multibody/joint/fwd.hpp +++ b/include/pinocchio/multibody/joint/fwd.hpp @@ -85,6 +85,14 @@ namespace pinocchio struct JointDataSphericalZYXTpl; typedef JointDataSphericalZYXTpl JointDataSphericalZYX; + template + struct JointModelEllipsoidTpl; + typedef JointModelEllipsoidTpl JointModelEllipsoid; + + template + struct JointDataEllipsoidTpl; + typedef JointDataEllipsoidTpl JointDataEllipsoid; + template struct JointModelPrismaticTpl; template diff --git a/include/pinocchio/multibody/joint/joint-collection.hpp b/include/pinocchio/multibody/joint/joint-collection.hpp index 540d8fb3fb..0d0c7d5de5 100644 --- a/include/pinocchio/multibody/joint/joint-collection.hpp +++ b/include/pinocchio/multibody/joint/joint-collection.hpp @@ -54,6 +54,9 @@ namespace pinocchio // Joint Spherical ZYX typedef JointModelSphericalZYXTpl JointModelSphericalZYX; + // Joint Ellipsoid + typedef JointModelEllipsoidTpl JointModelEllipsoid; + // Joint Translation typedef JointModelTranslationTpl 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 JointDataSphericalZYX; + // Joint Ellipsoid + typedef JointDataEllipsoidTpl JointDataEllipsoid; + // Joint Translation typedef JointDataTranslationTpl JointDataTranslation; @@ -179,6 +186,7 @@ namespace pinocchio JointDataRevoluteUnaligned, JointDataSpherical, JointDataSphericalZYX, + JointDataEllipsoid, JointDataPX, JointDataPY, JointDataPZ, diff --git a/include/pinocchio/multibody/joint/joint-ellipsoid.hpp b/include/pinocchio/multibody/joint/joint-ellipsoid.hpp new file mode 100644 index 0000000000..c158a1655d --- /dev/null +++ b/include/pinocchio/multibody/joint/joint-ellipsoid.hpp @@ -0,0 +1,745 @@ +// +// Copyright (c) 2025 INRIA +// + +#ifndef __pinocchio_multibody_joint_ellipsoid_hpp__ +#define __pinocchio_multibody_joint_ellipsoid_hpp__ + +#include "pinocchio/macros.hpp" +#include "pinocchio/multibody/joint/joint-base.hpp" +#include "pinocchio/multibody/joint-motion-subspace.hpp" +#include "pinocchio/math/sincos.hpp" +#include "pinocchio/math/matrix.hpp" +#include "pinocchio/spatial/inertia.hpp" +#include "pinocchio/spatial/skew.hpp" +#include "pinocchio/multibody/joint-motion-subspace.hpp" // because we are dense. + +namespace pinocchio +{ + template + struct JointEllipsoidTpl; + + template + struct traits> + { + enum + { + NQ = 3, + NV = 3, + NVExtended = 3 + }; + typedef _Scalar Scalar; + enum + { + Options = _Options + }; + typedef JointDataEllipsoidTpl JointDataDerived; + typedef JointModelEllipsoidTpl JointModelDerived; + typedef JointMotionSubspaceTpl<3, Scalar, Options, 3> Constraint_t; + typedef SE3Tpl Transformation_t; + + typedef MotionTpl Motion_t; + typedef MotionTpl Bias_t; + + // [ABA] + typedef Eigen::Matrix U_t; + typedef Eigen::Matrix D_t; + typedef Eigen::Matrix UD_t; + + typedef Eigen::Matrix ConfigVector_t; + typedef Eigen::Matrix TangentVector_t; + + typedef boost::mpl::false_ is_mimicable_t; // not mimicable + + PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE + }; + + template + struct traits> + { + typedef JointEllipsoidTpl<_Scalar, _Options> JointDerived; + typedef _Scalar Scalar; + }; + + template + struct traits> + { + typedef JointEllipsoidTpl<_Scalar, _Options> JointDerived; + typedef _Scalar Scalar; + }; + + template + struct JointDataEllipsoidTpl + : public JointDataBase> + { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + typedef JointEllipsoidTpl<_Scalar, _Options> JointDerived; + PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived); + PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR + + ConfigVector_t joint_q; + TangentVector_t joint_v; + + Constraint_t S; + Constraint_t Sdot; + Transformation_t M; + Motion_t v; + Bias_t c; + + // [ABA] specific data + U_t U; + D_t Dinv; + UD_t UDinv; + D_t StU; + + JointDataEllipsoidTpl() + : joint_q(ConfigVector_t::Zero()) + , joint_v(TangentVector_t::Zero()) + , M(Transformation_t::Identity()) + , v(Motion_t::Zero()) + , c(Bias_t::Zero()) + , U(U_t::Zero()) + , Dinv(D_t::Zero()) + , UDinv(UD_t::Zero()) + , StU(D_t::Zero()) + { + } + + static std::string classname() + { + return std::string("JointDataEllipsoid"); + } + std::string shortname() const + { + return classname(); + } + + }; // struct JointDataEllipsoidTpl + + PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelEllipsoidTpl); + template + struct JointModelEllipsoidTpl + : public JointModelBase> + { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + typedef JointEllipsoidTpl<_Scalar, _Options> JointDerived; + PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived); + + typedef JointModelBase Base; + using Base::id; + using Base::idx_q; + using Base::idx_v; + using Base::idx_vExtended; + using Base::setIndexes; + Scalar radius_a; + Scalar radius_b; + Scalar radius_c; + + JointDataDerived createData() const + { + return JointDataDerived(); + } + + JointModelEllipsoidTpl() + { + radius_a = Scalar(0.01); + radius_b = Scalar(0.01); + radius_c = Scalar(0.01); + } + + explicit JointModelEllipsoidTpl( + const Scalar & a, + const Scalar & b, + const Scalar & c) + : radius_a(a), radius_b(b), radius_c(c) + { + } + + const std::vector hasConfigurationLimit() const + { + return {true, true, true}; + } + + const std::vector hasConfigurationLimitInTangent() const + { + return {true, true, true}; + } + + template + void calc(JointDataDerived & data, const typename Eigen::MatrixBase & qs) const + // qs is the full configuration vector of the multibody system + { + // C'est ici que ça commence. + // only the q of the joint is needed. + data.joint_q = qs.template segment(idx_q()); + + Scalar c0, s0; + SINCOS(data.joint_q(0), &s0, &c0); + Scalar c1, s1; + SINCOS(data.joint_q(1), &s1, &c1); + Scalar c2, s2; + SINCOS(data.joint_q(2), &s2, &c2); + + //common operations + Scalar c1c2, c1s2; + c1c2 = c1 * c2; + c1s2 = c1 * s2; + + data.M.rotation() << c1c2, -c1s2, s1, + c0 * s2 + c2 * s0 * s1, c0 * c2 - s0 * s1 * s2, -c1 * s0, + -c0 * c2 * s1 + s0 * s2, c0 * s1 * s2 + c2 * s0, c0 * c1; + + Scalar nx, ny, nz; + nx = s1; + ny = -s0 * c1; + nz = c0 * c1; + + data.M.translation() << radius_a * nx, radius_b * ny, radius_c * nz; + + Scalar dndotx_dqdot1, dndoty_dqdot0, dndoty_dqdot1, dndotz_dqdot0, dndotz_dqdot1; + dndotx_dqdot1 = c1; + dndoty_dqdot0 = - c0 * c1; + dndoty_dqdot1 = s0 * s1; + dndotz_dqdot0 = - c1 * s0; + dndotz_dqdot1 = - c0 * s1; + + Scalar S_11, S_21, S_31, S_12, S_22, S_32; + // S_11 = c1 * (-radius_b * c0 * (c0 * s2 + c2 * s0 * s1) - radius_c * s0 * (- c0 * c2 * s1 + s0 * s2)); + S_11 = dndoty_dqdot0 * radius_b * (c0 *s2 + c2 * s0 * s1) + dndotz_dqdot0 * radius_c * (- c0 * c2 * s1 + s0 * s2); + // S_21 = c1 * (radius_b * c0 * (- c0 * c2 + s0 * s1 * s2) - radius_c * s0 * (c0 * s1 * s2 + c2 * s0)); + S_21 = - dndoty_dqdot0 * radius_b * (- c0 * c2 + s0 * s1 * s2) + dndotz_dqdot0 * radius_c * (c0 * s1 * s2 + c2 * s0); + // S_31 = c0 * c1 * c1 * s0 * (radius_b - radius_c); + S_31 = c1 * (- dndoty_dqdot0 * radius_b * s0 + dndotz_dqdot0 * radius_c * c0); + + //S_12 = radius_a * c1c2 * c1 + radius_b * s0 * s1 * (c0 * s2 + c2 * s0 * s1) - radius_c * c0 * s1 * (- c0 * c2 * s1 + s0 * s2); + S_12 = dndotx_dqdot1 * radius_a * c1 * c2 + dndoty_dqdot1 * radius_b * (c0 * s2 + c2 * s0 * s1) + dndotz_dqdot1 * radius_c * (- c0 * c2 * s1 + s0 * s2); + // S_22 = - radius_a * c1s2 * c1 - radius_b * s0 * s1 * (- c0 * c2 + s0 * s1 * s2) - radius_c * c0 * s1 * (c0 * s1 * s2 + c2 * s0); + S_22 = - dndotx_dqdot1 * radius_a * c1 * s2 - dndoty_dqdot1 * radius_b * (- c0 * c2 + s0 * s1 * s2) + dndotz_dqdot1 * radius_c * (c0 * s1 * s2 + c2 * s0); + //S_32 = c1 * s1 * (radius_a - radius_b * s0 * s0 - radius_c * c0 * c0); + S_32 = dndotx_dqdot1 * radius_a * s1 - dndoty_dqdot1 * radius_b * c1 * s0 + dndotz_dqdot1 * radius_c * c0 * c1; + + data.S.matrix() << S_11, S_12, Scalar(0), + S_21, S_22, Scalar(0), + S_31, S_32, Scalar(0), + c1c2, s2, Scalar(0), + -c1s2, c2, Scalar(0), + s1, Scalar(0), Scalar(1); + } + + template + void + calc(JointDataDerived & data, const Blank, const typename Eigen::MatrixBase & vs) + const + { + data.joint_v = vs.template segment(idx_v()); + + // Compute S from already-set joint_q + Scalar c0, s0; + SINCOS(data.joint_q(0), &s0, &c0); + Scalar c1, s1; + SINCOS(data.joint_q(1), &s1, &c1); + Scalar c2, s2; + SINCOS(data.joint_q(2), &s2, &c2); + + Scalar c1c2, c1s2; + c1c2 = c1 * c2; + c1s2 = c1 * s2; + + Scalar dndotx_dqdot1, dndoty_dqdot0, dndoty_dqdot1, dndotz_dqdot0, dndotz_dqdot1; + dndotx_dqdot1 = c1; + dndoty_dqdot0 = - c0 * c1; + dndoty_dqdot1 = s0 * s1; + dndotz_dqdot0 = - c1 * s0; + dndotz_dqdot1 = - c0 * s1; + + + Scalar S_11, S_21, S_31, S_12, S_22, S_32; + // S_11 = c1 * (-radius_b * c0 * (c0 * s2 + c2 * s0 * s1) - radius_c * s0 * (- c0 * c2 * s1 + s0 * s2)); + S_11 = dndoty_dqdot0 * radius_b * (c0 *s2 + c2 * s0 * s1) + dndotz_dqdot0 * radius_c * (- c0 * c2 * s1 + s0 * s2); + // S_21 = c1 * (radius_b * c0 * (- c0 * c2 + s0 * s1 * s2) - radius_c * s0 * (c0 * s1 * s2 + c2 * s0)); + S_21 = - dndoty_dqdot0 * radius_b * (- c0 * c2 + s0 * s1 * s2) + dndotz_dqdot0 * radius_c * (c0 * s1 * s2 + c2 * s0); + // S_31 = c0 * c1 * c1 * s0 * (radius_b - radius_c); + S_31 = c1 * (- dndoty_dqdot0 * radius_b * s0 + dndotz_dqdot0 * radius_c * c0); + + //S_12 = radius_a * c1c2 * c1 + radius_b * s0 * s1 * (c0 * s2 + c2 * s0 * s1) - radius_c * c0 * s1 * (- c0 * c2 * s1 + s0 * s2); + S_12 = dndotx_dqdot1 * radius_a * c1 * c2 + dndoty_dqdot1 * radius_b * (c0 * s2 + c2 * s0 * s1) + dndotz_dqdot1 * radius_c * (- c0 * c2 * s1 + s0 * s2); + // S_22 = - radius_a * c1s2 * c1 - radius_b * s0 * s1 * (- c0 * c2 + s0 * s1 * s2) - radius_c * c0 * s1 * (c0 * s1 * s2 + c2 * s0); + S_22 = - dndotx_dqdot1 * radius_a * c1 * s2 - dndoty_dqdot1 * radius_b * (- c0 * c2 + s0 * s1 * s2) + dndotz_dqdot1 * radius_c * (c0 * s1 * s2 + c2 * s0); + //S_32 = c1 * s1 * (radius_a - radius_b * s0 * s0 - radius_c * c0 * c0); + S_32 = dndotx_dqdot1 * radius_a * s1 - dndoty_dqdot1 * radius_b * c1 * s0 + dndotz_dqdot1 * radius_c * c0 * c1; + + data.S.matrix() << S_11, S_12, Scalar(0), + S_21, S_22, Scalar(0), + S_31, S_32, Scalar(0), + c1c2, s2, Scalar(0), + -c1s2, c2, Scalar(0), + s1, Scalar(0), Scalar(1); + + // Velocity part + data.v.toVector().noalias() = data.S.matrix() * data.joint_v; + + // Compute Sdot for bias acceleration + Scalar qdot0, qdot1, qdot2; + qdot0 = data.joint_v(0); + qdot1 = data.joint_v(1); + qdot2 = data.joint_v(2); + + Scalar Sdot_11, Sdot_21, Sdot_31, Sdot_41, Sdot_51, Sdot_61; + Scalar Sdot_12, Sdot_22, Sdot_32, Sdot_42, Sdot_52, Sdot_62; + Scalar Sdot_13, Sdot_23, Sdot_33, Sdot_43, Sdot_53, Sdot_63; + + // Derivative of dndotXX_dqdot0 with respect to q0 and q1 + Scalar d_dndotx_dqdot1_dq1 = - s1; // dndotx_dqdot1 = c1; + + Scalar d_dndoty_dqdot0_dq0 = s0 * c1; // dndoty_dqdot0 = - c0 * c1; + Scalar d_dndoty_dqdot0_dq1 = c0 * s1; + + Scalar d_dndoty_dqdot1_dq0 = c0 * s1; // dndoty_dqdot1 = s0 * s1; + Scalar d_dndoty_dqdot1_dq1 = s0 * c1; + + Scalar d_dndotz_dqdot0_dq0 = - c1 * c0; // dndotz_dqdot0 = - c1 * s0; + Scalar d_dndotz_dqdot0_dq1 = s0 * s1; + + Scalar d_dndotz_dqdot1_dq0 = s0 * s1; // dndotz_dqdot1 = - c0 * s1; + Scalar d_dndotz_dqdot1_dq1 = -c0 * c1; + + + // Upper part (translation) + // Row 1, Column 1 + Sdot_11 = qdot0 * ( + -dndoty_dqdot0 * radius_b * (-c0 * c2 * s1 + s0 * s2) + + dndotz_dqdot0 * radius_c * (c0 * s2 + c2 * s0 * s1) + + radius_b * (c0 * s2 + c2 * s0 * s1) * d_dndoty_dqdot0_dq0 + + radius_c * (-c0 * c2 * s1 + s0 * s2) * d_dndotz_dqdot0_dq0 + ) + + qdot1 * ( + dndoty_dqdot0 * radius_b * c1 * c2 * s0 + - dndotz_dqdot0 * radius_c * c0 * c1 * c2 + + radius_b * (c0 * s2 + c2 * s0 * s1) * d_dndoty_dqdot0_dq1 + + radius_c * (-c0 * c2 * s1 + s0 * s2) * d_dndotz_dqdot0_dq1 + ) + - qdot2 * ( + dndoty_dqdot0 * radius_b * (-c0 * c2 + s0 * s1 * s2) + - dndotz_dqdot0 * radius_c * (c0 * s1 * s2 + c2 * s0) + ); + + // Row 1, Column 2 + Sdot_12 = qdot0 * ( + -dndoty_dqdot1 * radius_b * (-c0 * c2 * s1 + s0 * s2) + + dndotz_dqdot1 * radius_c * (c0 * s2 + c2 * s0 * s1) + + radius_b * (c0 * s2 + c2 * s0 * s1) * d_dndoty_dqdot0_dq1 + + radius_c * (-c0 * c2 * s1 + s0 * s2) * d_dndotz_dqdot0_dq1 + ) + + qdot1 * ( + -dndotx_dqdot1 * radius_a * c2 * s1 + + dndoty_dqdot1 * radius_b * c1 * c2 * s0 + - dndotz_dqdot1 * radius_c * c0 * c1 * c2 + + radius_a * c1 * c2 * d_dndotx_dqdot1_dq1 + + radius_b * (c0 * s2 + c2 * s0 * s1) * d_dndoty_dqdot1_dq1 + + radius_c * (-c0 * c2 * s1 + s0 * s2) * d_dndotz_dqdot1_dq1 + ) + - qdot2 * ( + dndotx_dqdot1 * radius_a * c1 * s2 + + dndoty_dqdot1 * radius_b * (-c0 * c2 + s0 * s1 * s2) + - dndotz_dqdot1 * radius_c * (c0 * s1 * s2 + c2 * s0) + ); + + // Row 1, Column 3 + Sdot_13 = Scalar(0); + + // Row 2, Column 1 + Sdot_21 = -qdot0 * ( + dndoty_dqdot0 * radius_b * (c0 * s1 * s2 + c2 * s0) + + dndotz_dqdot0 * radius_c * (-c0 * c2 + s0 * s1 * s2) + + radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot0_dq0 + - radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot0_dq0 + ) + - qdot1 * ( + dndoty_dqdot0 * radius_b * c1 * s0 * s2 + - dndotz_dqdot0 * radius_c * c0 * c1 * s2 + + radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot0_dq1 + - radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot0_dq1 + ) + - qdot2 * ( + dndoty_dqdot0 * radius_b * (c0 * s2 + c2 * s0 * s1) + + dndotz_dqdot0 * radius_c * (-c0 * c2 * s1 + s0 * s2) + ); + + // Row 2, Column 2 + Sdot_22 = -qdot0 * ( + dndoty_dqdot1 * radius_b * (c0 * s1 * s2 + c2 * s0) + + dndotz_dqdot1 * radius_c * (-c0 * c2 + s0 * s1 * s2) + + radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot0_dq1 + - radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot0_dq1 + ) + + qdot1 * ( + dndotx_dqdot1 * radius_a * s1 * s2 + - dndoty_dqdot1 * radius_b * c1 * s0 * s2 + + dndotz_dqdot1 * radius_c * c0 * c1 * s2 + - radius_a * c1 * s2 * d_dndotx_dqdot1_dq1 + - radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot1_dq1 + + radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot1_dq1 + ) + - qdot2 * ( + dndotx_dqdot1 * radius_a * c1 * c2 + + dndoty_dqdot1 * radius_b * (c0 * s2 + c2 * s0 * s1) + + dndotz_dqdot1 * radius_c * (-c0 * c2 * s1 + s0 * s2) + ); + + // Row 2, Column 3 + Sdot_23 = Scalar(0); + + // Row 3, Column 1 + Sdot_31 = -qdot0 * c1 * ( + dndoty_dqdot0 * radius_b * c0 + + dndotz_dqdot0 * radius_c * s0 + + radius_b * s0 * d_dndoty_dqdot0_dq0 + - radius_c * c0 * d_dndotz_dqdot0_dq0 + ) + + qdot1 * ( + -c1 * (radius_b * s0 * d_dndoty_dqdot0_dq1 - radius_c * c0 * d_dndotz_dqdot0_dq1) + + s1 * (dndoty_dqdot0 * radius_b * s0 - dndotz_dqdot0 * radius_c * c0) + ); + + // Row 3, Column 2 + Sdot_32 = -qdot0 * c1 * ( + dndoty_dqdot1 * radius_b * c0 + + dndotz_dqdot1 * radius_c * s0 + + radius_b * s0 * d_dndoty_dqdot0_dq1 + - radius_c * c0 * d_dndotz_dqdot0_dq1 + ) + + qdot1 * ( + dndotx_dqdot1 * radius_a * c1 + + dndoty_dqdot1 * radius_b * s0 * s1 + - dndotz_dqdot1 * radius_c * c0 * s1 + + radius_a * s1 * d_dndotx_dqdot1_dq1 + - radius_b * c1 * s0 * d_dndoty_dqdot1_dq1 + + radius_c * c0 * c1 * d_dndotz_dqdot1_dq1 + ); + + // Row 3, Column 3 + Sdot_33 = Scalar(0); + + // Angular part (rows 4-6) + Sdot_41 = -(qdot1 * c2 * s1 + qdot2 * c1 * s2); + Sdot_51 = qdot1 * s1 * s2 - qdot2 * c1 * c2; + Sdot_61 = qdot1 * c1; + + Sdot_42 = qdot2 * c2; + Sdot_52 = -qdot2 * s2; + Sdot_62 = Scalar(0); + + Sdot_43 = Scalar(0); + Sdot_53 = Scalar(0); + Sdot_63 = Scalar(0); + + data.Sdot.matrix() << Sdot_11, Sdot_12, Sdot_13, + Sdot_21, Sdot_22, Sdot_23, + Sdot_31, Sdot_32, Sdot_33, + Sdot_41, Sdot_42, Sdot_43, + Sdot_51, Sdot_52, Sdot_53, + Sdot_61, Sdot_62, Sdot_63; + + data.c.toVector().noalias() = data.Sdot.matrix() * data.joint_v; + } + + template + void calc( + JointDataDerived & data, + const typename Eigen::MatrixBase & qs, + const typename Eigen::MatrixBase & vs) const + { + data.joint_q = qs.template segment(idx_q()); + + Scalar c0, s0; + SINCOS(data.joint_q(0), &s0, &c0); + Scalar c1, s1; + SINCOS(data.joint_q(1), &s1, &c1); + Scalar c2, s2; + SINCOS(data.joint_q(2), &s2, &c2); + + //common operations + Scalar c1c2, c1s2; + c1c2 = c1 * c2; + c1s2 = c1 * s2; + + data.M.rotation() << c1c2, -c1s2, s1, + c0 * s2 + c2 * s0 * s1, c0 * c2 - s0 * s1 * s2, -c1 * s0, + -c0 * c2 * s1 + s0 * s2, c0 * s1 * s2 + c2 * s0, c0 * c1; + + Scalar nx, ny, nz; + nx = s1; + ny = -s0 * c1; + nz = c0 * c1; + + data.M.translation() << radius_a * nx, radius_b * ny, radius_c * nz; + + Scalar dndotx_dqdot1, dndoty_dqdot0, dndoty_dqdot1, dndotz_dqdot0, dndotz_dqdot1; + dndotx_dqdot1 = c1; + dndoty_dqdot0 = - c0 * c1; + dndoty_dqdot1 = s0 * s1; + dndotz_dqdot0 = - c1 * s0; + dndotz_dqdot1 = - c0 * s1; + + Scalar S_11, S_21, S_31, S_12, S_22, S_32; + // S_11 = c1 * (-radius_b * c0 * (c0 * s2 + c2 * s0 * s1) - radius_c * s0 * (- c0 * c2 * s1 + s0 * s2)); + S_11 = dndoty_dqdot0 * radius_b * (c0 *s2 + c2 * s0 * s1) + dndotz_dqdot0 * radius_c * (- c0 * c2 * s1 + s0 * s2); + // S_21 = c1 * (radius_b * c0 * (- c0 * c2 + s0 * s1 * s2) - radius_c * s0 * (c0 * s1 * s2 + c2 * s0)); + S_21 = - dndoty_dqdot0 * radius_b * (- c0 * c2 + s0 * s1 * s2) + dndotz_dqdot0 * radius_c * (c0 * s1 * s2 + c2 * s0); + // S_31 = c0 * c1 * c1 * s0 * (radius_b - radius_c); + S_31 = c1 * (- dndoty_dqdot0 * radius_b * s0 + dndotz_dqdot0 * radius_c * c0); + + //S_12 = radius_a * c1c2 * c1 + radius_b * s0 * s1 * (c0 * s2 + c2 * s0 * s1) - radius_c * c0 * s1 * (- c0 * c2 * s1 + s0 * s2); + S_12 = dndotx_dqdot1 * radius_a * c1 * c2 + dndoty_dqdot1 * radius_b * (c0 * s2 + c2 * s0 * s1) + dndotz_dqdot1 * radius_c * (- c0 * c2 * s1 + s0 * s2); + // S_22 = - radius_a * c1s2 * c1 - radius_b * s0 * s1 * (- c0 * c2 + s0 * s1 * s2) - radius_c * c0 * s1 * (c0 * s1 * s2 + c2 * s0); + S_22 = - dndotx_dqdot1 * radius_a * c1 * s2 - dndoty_dqdot1 * radius_b * (- c0 * c2 + s0 * s1 * s2) + dndotz_dqdot1 * radius_c * (c0 * s1 * s2 + c2 * s0); + //S_32 = c1 * s1 * (radius_a - radius_b * s0 * s0 - radius_c * c0 * c0); + S_32 = dndotx_dqdot1 * radius_a * s1 - dndoty_dqdot1 * radius_b * c1 * s0 + dndotz_dqdot1 * radius_c * c0 * c1; + + data.S.matrix() << S_11, S_12, Scalar(0), + S_21, S_22, Scalar(0), + S_31, S_32, Scalar(0), + c1c2, s2, Scalar(0), + -c1s2, c2, Scalar(0), + s1, Scalar(0), Scalar(1); + + // Velocity part + data.joint_v = vs.template segment(idx_v()); + data.v.toVector().noalias() = data.S.matrix() * data.joint_v; + + // Compute Sdot for bias acceleration + Scalar qdot0, qdot1, qdot2; + qdot0 = data.joint_v(0); + qdot1 = data.joint_v(1); + qdot2 = data.joint_v(2); + + Scalar Sdot_11, Sdot_21, Sdot_31, Sdot_41, Sdot_51, Sdot_61; + Scalar Sdot_12, Sdot_22, Sdot_32, Sdot_42, Sdot_52, Sdot_62; + Scalar Sdot_13, Sdot_23, Sdot_33, Sdot_43, Sdot_53, Sdot_63; + + // Derivative of dndotXX_dqdot0 with respect to q0 and q1 + Scalar d_dndotx_dqdot1_dq1 = - s1; // dndotx_dqdot1 = c1; + + Scalar d_dndoty_dqdot0_dq0 = s0 * c1; // dndoty_dqdot0 = - c0 * c1; + Scalar d_dndoty_dqdot0_dq1 = c0 * s1; + + Scalar d_dndoty_dqdot1_dq0 = c0 * s1; // dndoty_dqdot1 = s0 * s1; + Scalar d_dndoty_dqdot1_dq1 = s0 * c1; + + Scalar d_dndotz_dqdot0_dq0 = - c1 * c0; // dndotz_dqdot0 = - c1 * s0; + Scalar d_dndotz_dqdot0_dq1 = s0 * s1; + + Scalar d_dndotz_dqdot1_dq0 = s0 * s1; // dndotz_dqdot1 = - c0 * s1; + Scalar d_dndotz_dqdot1_dq1 = -c0 * c1; + + + // Upper part (translation) + // Row 1, Column 1 + Sdot_11 = qdot0 * ( + -dndoty_dqdot0 * radius_b * (-c0 * c2 * s1 + s0 * s2) + + dndotz_dqdot0 * radius_c * (c0 * s2 + c2 * s0 * s1) + + radius_b * (c0 * s2 + c2 * s0 * s1) * d_dndoty_dqdot0_dq0 + + radius_c * (-c0 * c2 * s1 + s0 * s2) * d_dndotz_dqdot0_dq0 + ) + + qdot1 * ( + dndoty_dqdot0 * radius_b * c1 * c2 * s0 + - dndotz_dqdot0 * radius_c * c0 * c1 * c2 + + radius_b * (c0 * s2 + c2 * s0 * s1) * d_dndoty_dqdot0_dq1 + + radius_c * (-c0 * c2 * s1 + s0 * s2) * d_dndotz_dqdot0_dq1 + ) + - qdot2 * ( + dndoty_dqdot0 * radius_b * (-c0 * c2 + s0 * s1 * s2) + - dndotz_dqdot0 * radius_c * (c0 * s1 * s2 + c2 * s0) + ); + + // Row 1, Column 2 + Sdot_12 = qdot0 * ( + -dndoty_dqdot1 * radius_b * (-c0 * c2 * s1 + s0 * s2) + + dndotz_dqdot1 * radius_c * (c0 * s2 + c2 * s0 * s1) + + radius_b * (c0 * s2 + c2 * s0 * s1) * d_dndoty_dqdot0_dq1 + + radius_c * (-c0 * c2 * s1 + s0 * s2) * d_dndotz_dqdot0_dq1 + ) + + qdot1 * ( + -dndotx_dqdot1 * radius_a * c2 * s1 + + dndoty_dqdot1 * radius_b * c1 * c2 * s0 + - dndotz_dqdot1 * radius_c * c0 * c1 * c2 + + radius_a * c1 * c2 * d_dndotx_dqdot1_dq1 + + radius_b * (c0 * s2 + c2 * s0 * s1) * d_dndoty_dqdot1_dq1 + + radius_c * (-c0 * c2 * s1 + s0 * s2) * d_dndotz_dqdot1_dq1 + ) + - qdot2 * ( + dndotx_dqdot1 * radius_a * c1 * s2 + + dndoty_dqdot1 * radius_b * (-c0 * c2 + s0 * s1 * s2) + - dndotz_dqdot1 * radius_c * (c0 * s1 * s2 + c2 * s0) + ); + + // Row 1, Column 3 + Sdot_13 = Scalar(0); + + // Row 2, Column 1 + Sdot_21 = -qdot0 * ( + dndoty_dqdot0 * radius_b * (c0 * s1 * s2 + c2 * s0) + + dndotz_dqdot0 * radius_c * (-c0 * c2 + s0 * s1 * s2) + + radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot0_dq0 + - radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot0_dq0 + ) + - qdot1 * ( + dndoty_dqdot0 * radius_b * c1 * s0 * s2 + - dndotz_dqdot0 * radius_c * c0 * c1 * s2 + + radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot0_dq1 + - radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot0_dq1 + ) + - qdot2 * ( + dndoty_dqdot0 * radius_b * (c0 * s2 + c2 * s0 * s1) + + dndotz_dqdot0 * radius_c * (-c0 * c2 * s1 + s0 * s2) + ); + + // Row 2, Column 2 + Sdot_22 = -qdot0 * ( + dndoty_dqdot1 * radius_b * (c0 * s1 * s2 + c2 * s0) + + dndotz_dqdot1 * radius_c * (-c0 * c2 + s0 * s1 * s2) + + radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot0_dq1 + - radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot0_dq1 + ) + + qdot1 * ( + dndotx_dqdot1 * radius_a * s1 * s2 + - dndoty_dqdot1 * radius_b * c1 * s0 * s2 + + dndotz_dqdot1 * radius_c * c0 * c1 * s2 + - radius_a * c1 * s2 * d_dndotx_dqdot1_dq1 + - radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot1_dq1 + + radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot1_dq1 + ) + - qdot2 * ( + dndotx_dqdot1 * radius_a * c1 * c2 + + dndoty_dqdot1 * radius_b * (c0 * s2 + c2 * s0 * s1) + + dndotz_dqdot1 * radius_c * (-c0 * c2 * s1 + s0 * s2) + ); + + // Row 2, Column 3 + Sdot_23 = Scalar(0); + + // Row 3, Column 1 + Sdot_31 = -qdot0 * c1 * ( + dndoty_dqdot0 * radius_b * c0 + + dndotz_dqdot0 * radius_c * s0 + + radius_b * s0 * d_dndoty_dqdot0_dq0 + - radius_c * c0 * d_dndotz_dqdot0_dq0 + ) + + qdot1 * ( + -c1 * (radius_b * s0 * d_dndoty_dqdot0_dq1 - radius_c * c0 * d_dndotz_dqdot0_dq1) + + s1 * (dndoty_dqdot0 * radius_b * s0 - dndotz_dqdot0 * radius_c * c0) + ); + + // Row 3, Column 2 + Sdot_32 = -qdot0 * c1 * ( + dndoty_dqdot1 * radius_b * c0 + + dndotz_dqdot1 * radius_c * s0 + + radius_b * s0 * d_dndoty_dqdot0_dq1 + - radius_c * c0 * d_dndotz_dqdot0_dq1 + ) + + qdot1 * ( + dndotx_dqdot1 * radius_a * c1 + + dndoty_dqdot1 * radius_b * s0 * s1 + - dndotz_dqdot1 * radius_c * c0 * s1 + + radius_a * s1 * d_dndotx_dqdot1_dq1 + - radius_b * c1 * s0 * d_dndoty_dqdot1_dq1 + + radius_c * c0 * c1 * d_dndotz_dqdot1_dq1 + ); + + // Row 3, Column 3 + Sdot_33 = Scalar(0); + + // Angular part (rows 4-6) + Sdot_41 = -(qdot1 * c2 * s1 + qdot2 * c1 * s2); + Sdot_51 = qdot1 * s1 * s2 - qdot2 * c1 * c2; + Sdot_61 = qdot1 * c1; + + Sdot_42 = qdot2 * c2; + Sdot_52 = -qdot2 * s2; + Sdot_62 = Scalar(0); + + Sdot_43 = Scalar(0); + Sdot_53 = Scalar(0); + Sdot_63 = Scalar(0); + + data.Sdot.matrix() << Sdot_11, Sdot_12, Sdot_13, + Sdot_21, Sdot_22, Sdot_23, + Sdot_31, Sdot_32, Sdot_33, + Sdot_41, Sdot_42, Sdot_43, + Sdot_51, Sdot_52, Sdot_53, + Sdot_61, Sdot_62, Sdot_63; + + data.c.toVector().noalias() = data.Sdot.matrix() * data.joint_v; + } + + template + void calc_aba( + JointDataDerived & data, + const Eigen::MatrixBase & armature, + const Eigen::MatrixBase & I, + const bool update_I) const + { + data.U.noalias() = I * data.S.matrix(); + data.StU.noalias() = data.S.transpose() * data.U; + data.StU.diagonal() += armature; + internal::PerformStYSInversion::run(data.StU, data.Dinv); + + data.UDinv.noalias() = data.U * data.Dinv; + + if (update_I) + PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I).noalias() -= data.UDinv * data.U.transpose(); + } + + static std::string classname() + { + return std::string("JointModelEllipsoid"); + } + std::string shortname() const + { + return classname(); + } + + /// \returns An expression of *this with the Scalar type casted to NewScalar. + template + JointModelEllipsoidTpl cast() const + { + typedef JointModelEllipsoidTpl ReturnType; + ReturnType res; + res.setIndexes(id(), idx_q(), idx_v(), idx_vExtended()); + return res; + } + + }; // struct JointModelEllipsoidTpl + +} // namespace pinocchio + +#include + +namespace boost +{ + template + struct has_nothrow_constructor<::pinocchio::JointModelEllipsoidTpl> + : public integral_constant + { + }; + + template + struct has_nothrow_copy<::pinocchio::JointModelEllipsoidTpl> + : public integral_constant + { + }; + + template + struct has_nothrow_constructor<::pinocchio::JointDataEllipsoidTpl> + : public integral_constant + { + }; + + template + struct has_nothrow_copy<::pinocchio::JointDataEllipsoidTpl> + : public integral_constant + { + }; +} // namespace boost + +#endif // ifndef __pinocchio_multibody_joint_spherical_ZYX_hpp__ diff --git a/include/pinocchio/multibody/joint/joints.hpp b/include/pinocchio/multibody/joint/joints.hpp index 546bd4e498..f7bf01e743 100644 --- a/include/pinocchio/multibody/joint/joints.hpp +++ b/include/pinocchio/multibody/joint/joints.hpp @@ -13,8 +13,9 @@ #include "pinocchio/multibody/joint/joint-revolute-unbounded.hpp" #include "pinocchio/multibody/joint/joint-revolute-unaligned.hpp" #include "pinocchio/multibody/joint/joint-revolute-unbounded-unaligned.hpp" -#include "pinocchio/multibody/joint/joint-spherical-ZYX.hpp" #include "pinocchio/multibody/joint/joint-spherical.hpp" +#include "pinocchio/multibody/joint/joint-spherical-ZYX.hpp" +#include "pinocchio/multibody/joint/joint-ellipsoid.hpp" #include "pinocchio/multibody/joint/joint-translation.hpp" #include "pinocchio/multibody/joint/joint-mimic.hpp" #include "pinocchio/multibody/joint/joint-helical.hpp" diff --git a/include/pinocchio/serialization/joints-data.hpp b/include/pinocchio/serialization/joints-data.hpp index 157d3ba7d7..efadfad364 100644 --- a/include/pinocchio/serialization/joints-data.hpp +++ b/include/pinocchio/serialization/joints-data.hpp @@ -171,6 +171,16 @@ namespace boost fix::serialize(ar, static_cast &>(joint), version); } + template + void serialize( + Archive & ar, + pinocchio::JointDataEllipsoidTpl & joint, + const unsigned int version) + { + typedef pinocchio::JointDataEllipsoidTpl JointType; + fix::serialize(ar, static_cast &>(joint), version); + } + template void serialize( Archive & ar, diff --git a/include/pinocchio/serialization/joints-model.hpp b/include/pinocchio/serialization/joints-model.hpp index cc6392a657..67991792fb 100644 --- a/include/pinocchio/serialization/joints-model.hpp +++ b/include/pinocchio/serialization/joints-model.hpp @@ -206,6 +206,19 @@ namespace boost fix::serialize(ar, *static_cast *>(&joint), version); } + template + void serialize( + Archive & ar, + pinocchio::JointModelEllipsoidTpl & joint, + const unsigned int version) + { + typedef pinocchio::JointModelEllipsoidTpl JointType; + fix::serialize(ar, *static_cast *>(&joint), version); + ar & make_nvp("radius_a", joint.radius_a); + ar & make_nvp("radius_b", joint.radius_b); + ar & make_nvp("radius_c", joint.radius_c); + } + template void serialize( Archive & ar, diff --git a/sources.cmake b/sources.cmake index 2aec1df8cc..0691194f2d 100644 --- a/sources.cmake +++ b/sources.cmake @@ -192,6 +192,7 @@ set(${PROJECT_NAME}_CORE_PUBLIC_HEADERS ${PROJECT_SOURCE_DIR}/include/pinocchio/multibody/joint/joints.hpp ${PROJECT_SOURCE_DIR}/include/pinocchio/multibody/joint/joint-spherical.hpp ${PROJECT_SOURCE_DIR}/include/pinocchio/multibody/joint/joint-spherical-ZYX.hpp + ${PROJECT_SOURCE_DIR}/include/pinocchio/multibody/joint/joint-ellipsoid.hpp ${PROJECT_SOURCE_DIR}/include/pinocchio/multibody/joint/joint-translation.hpp ${PROJECT_SOURCE_DIR}/include/pinocchio/multibody/joint/joint-universal.hpp ${PROJECT_SOURCE_DIR}/include/pinocchio/multibody/joint-motion-subspace-base.hpp diff --git a/unittest/CMakeLists.txt b/unittest/CMakeLists.txt index c470a509ae..89322fd328 100644 --- a/unittest/CMakeLists.txt +++ b/unittest/CMakeLists.txt @@ -278,6 +278,7 @@ add_pinocchio_unit_test(joint-prismatic) add_pinocchio_unit_test(joint-planar) add_pinocchio_unit_test(joint-free-flyer) add_pinocchio_unit_test(joint-spherical) +add_pinocchio_unit_test(joint-ellipsoid) add_pinocchio_unit_test(joint-translation) add_pinocchio_unit_test(joint-generic) add_pinocchio_unit_test(joint-composite) diff --git a/unittest/all-joints.cpp b/unittest/all-joints.cpp index c1d0802e48..fd53d9fd1d 100644 --- a/unittest/all-joints.cpp +++ b/unittest/all-joints.cpp @@ -152,6 +152,24 @@ struct init> } }; +template +struct init> +{ + typedef pinocchio::JointModelEllipsoidTpl JointModel; + + static JointModel run() + { + JointModel jmodel( + static_cast(0.01), + static_cast(0.02), + static_cast(0.03) + ); + + jmodel.setIndexes(0, 0, 0); + return jmodel; + } +}; + template struct init> { diff --git a/unittest/finite-differences.cpp b/unittest/finite-differences.cpp index 9b3958ef41..07e0f4bce5 100644 --- a/unittest/finite-differences.cpp +++ b/unittest/finite-differences.cpp @@ -163,6 +163,23 @@ struct init> } }; +template +struct init> +{ + typedef pinocchio::JointModelEllipsoidTpl JointModel; + + static JointModel run() + { + JointModel jmodel(Scalar(0.01), + Scalar(0.02), + Scalar(0.03) + ); + + jmodel.setIndexes(0, 0, 0); + return jmodel; + } +}; + template class JointCollection> struct init> { diff --git a/unittest/joint-ellipsoid.cpp b/unittest/joint-ellipsoid.cpp new file mode 100644 index 0000000000..2f41b31880 --- /dev/null +++ b/unittest/joint-ellipsoid.cpp @@ -0,0 +1,362 @@ +// +// Copyright (c) 2025 INRIA +// + +#include "pinocchio/math/fwd.hpp" +#include "pinocchio/multibody/joint/joints.hpp" +#include "pinocchio/algorithm/rnea.hpp" +#include "pinocchio/algorithm/aba.hpp" +#include "pinocchio/algorithm/crba.hpp" +#include "pinocchio/algorithm/jacobian.hpp" +#include "pinocchio/algorithm/compute-all-terms.hpp" +#include "pinocchio/spatial/se3.hpp" +#include "pinocchio/spatial/inertia.hpp" +#include "pinocchio/multibody/model.hpp" +#include "pinocchio/multibody/data.hpp" +#include "pinocchio/algorithm/kinematics.hpp" +#include "pinocchio/algorithm/frames.hpp" + +#include +#include + +using namespace pinocchio; + +template +void addJointAndBody( + Model & model, + const JointModelBase & jmodel, + const Model::JointIndex parent_id, + const SE3 & joint_placement, + const std::string & joint_name, + const Inertia & Y) +{ + Model::JointIndex idx; + + idx = model.addJoint(parent_id, jmodel, joint_placement, joint_name); + model.appendBodyToJoint(idx, Y); +} + +BOOST_AUTO_TEST_SUITE(JointEllipsoid) + +BOOST_AUTO_TEST_CASE(vsFreeFlyer) +{ + using namespace pinocchio; + typedef SE3::Vector3 Vector3; + typedef Eigen::Matrix Vector6; + typedef Eigen::Matrix VectorFF; + typedef SE3::Matrix3 Matrix3; + + Model modelEllipsoid, modelFreeflyer; + + Inertia inertia(1., Vector3(0.5, 0., 0.0), Matrix3::Identity()); + SE3 pos(1); + pos.translation() = SE3::LinearType(1., 0., 0.); + + addJointAndBody(modelEllipsoid, JointModelEllipsoid(1, 2,3), 0, pos, "ellipsoid", inertia); + addJointAndBody(modelFreeflyer, JointModelFreeFlyer(), 0, pos, "free-flyer", inertia); + + Data dataEllipsoid(modelEllipsoid); + + Eigen::VectorXd q = Eigen::VectorXd::Ones(modelEllipsoid.nq); + Eigen::VectorXd v = Eigen::VectorXd::Ones(modelEllipsoid.nv); + + forwardKinematics(modelEllipsoid, dataEllipsoid, q, v); + + Eigen::VectorXd tauEllipsoid = Eigen::VectorXd::Ones(modelEllipsoid.nv); + + Eigen::VectorXd aEllipsoid = Eigen::VectorXd::Ones(modelEllipsoid.nv); + + // ForwardDynamics == aba + Eigen::VectorXd aAbaEllipsoid = + aba(modelEllipsoid, dataEllipsoid, q, v, tauEllipsoid, Convention::WORLD); + + // Calculer jdata.S().transpose() * data.f[i] +} + +// BOOST_AUTO_TEST_CASE(vsSphericalZYX) +// { +// using namespace pinocchio; +// typedef SE3::Vector3 Vector3; +// typedef Eigen::Matrix Vector6; +// typedef SE3::Matrix3 Matrix3; + +// // Build models using ModelGraph to enable configuration conversion +// graph::ModelGraph g; +// Inertia inertia(1., Vector3(0.5, 0., 0.0), Matrix3::Identity()); +// SE3 pos(1); +// pos.translation() = SE3::LinearType(1., 0., 0.); + +// g.addBody("root_body", inertia); +// g.addBody("end_body", inertia); + +// // Create SphericalZYX joint (uses ZYX Euler angles) +// g.addJoint( +// "spherical_joint", +// graph::JointSphericalZYX(), +// "root_body", +// SE3::Identity(), +// "end_body", +// pos); + +// // Build SphericalZYX model +// const auto forward_build = graph::buildModelWithBuildInfo(g, "root_body", SE3::Identity()); +// const Model & modelSphericalZYX = forward_build.model; +// Data dataSphericalZYX(modelSphericalZYX); + +// Eigen::VectorXd q_zyx(3); +// q_zyx << 0.5, 1.2, -0.8; +// Eigen::VectorXd v_zyx(3); +// v_zyx << 0.1, -0.3, 0.7; +// forwardKinematics(modelSphericalZYX, dataSphericalZYX, q_zyx, v_zyx); +// updateFramePlacements(modelSphericalZYX, dataSphericalZYX); + +// auto end_index = modelSphericalZYX.getFrameId("end_body", BODY); +// auto X_end = dataSphericalZYX.oMf[end_index]; +// const auto backward_build = +// graph::buildModelWithBuildInfo(g, "end_body", X_end); + +// const Model & XYZlacirehpSledom = backward_build.model; + +// // Create converter from SphericalZYX to Ellipsoid +// auto converter = graph::createConverter( +// modelSphericalZYX, XYZlacirehpSledom, forward_build.build_info, backward_build.build_info); + +// // Convert to Ellipsoid configuration +// Eigen::VectorXd q_xyz = Eigen::VectorXd::Zero(XYZlacirehpSledom.nq); +// Eigen::VectorXd v_xyz = Eigen::VectorXd::Zero(XYZlacirehpSledom.nv); +// converter.convertConfigurationVector(q_zyx, q_xyz); +// converter.convertTangentVector(q_zyx, v_zyx, v_xyz); + +// std::cout << "\n=== Configuration Conversion ===" << std::endl; +// std::cout << "q_zyx (SphericalZYX): " << q_zyx.transpose() << std::endl; +// std::cout << "q_xyz (converted): " << q_xyz.transpose() << std::endl; +// std::cout << "v_zyx (SphericalZYX): " << v_zyx.transpose() << std::endl; +// std::cout << "v_xyz (converted): " << v_xyz.transpose() << std::endl; + +// Model modelEllipsoid; +// addJointAndBody(modelEllipsoid, JointModelEllipsoid(0, 0, 0), 0, pos, "ellipsoid", inertia); +// Data dataEllipsoid(modelEllipsoid); + +// // Test forward kinematics with converted configurations +// forwardKinematics(modelEllipsoid, dataEllipsoid, q_xyz, v_xyz); +// forwardKinematics(modelSphericalZYX, dataSphericalZYX, q_zyx, v_zyx); + +// // Check that the transformations are identical +// std::cout << "\n=== Forward Kinematics Comparison ===" << std::endl; +// std::cout << "oMi (Ellipsoid):\n" << dataEllipsoid.oMi[1] << std::endl; +// std::cout << "oMi (SphericalZYX):\n" << dataSphericalZYX.oMi[1] << std::endl; +// BOOST_CHECK(dataEllipsoid.oMi[1].isApprox(dataSphericalZYX.oMi[1])); + +// BOOST_CHECK(dataEllipsoid.liMi[1].isApprox(dataSphericalZYX.liMi[1])); + +// // Check that velocities match +// BOOST_CHECK(dataEllipsoid.v[1].toVector().isApprox(dataSphericalZYX.v[1].toVector())); + +// // Test computeAllTerms +// computeAllTerms(modelEllipsoid, dataEllipsoid, q_xyz, v_xyz); +// computeAllTerms(modelSphericalZYX, dataSphericalZYX, q_zyx, v_zyx); + +// BOOST_CHECK(dataEllipsoid.Ycrb[1].matrix().isApprox(dataSphericalZYX.Ycrb[1].matrix())); + +// BOOST_CHECK(dataEllipsoid.f[1].toVector().isApprox(dataSphericalZYX.f[1].toVector())); + +// BOOST_CHECK(dataEllipsoid.nle.isApprox(dataSphericalZYX.nle)); + +// BOOST_CHECK(dataEllipsoid.com[0].isApprox(dataSphericalZYX.com[0])); + +// // Test inverse dynamics (RNEA) +// Eigen::VectorXd aEllipsoid = Eigen::VectorXd::Ones(modelEllipsoid.nv); +// Eigen::VectorXd aSphericalZYX = Eigen::VectorXd::Ones(modelSphericalZYX.nv); + +// Eigen::VectorXd tauEllipsoid = rnea(modelEllipsoid, dataEllipsoid, q_xyz, v_xyz, aEllipsoid); +// Eigen::VectorXd tauSphericalZYX = rnea(modelSphericalZYX, dataSphericalZYX, q_zyx, v_zyx, aSphericalZYX); + +// BOOST_CHECK(tauEllipsoid.isApprox(tauSphericalZYX)); + +// // Test forward dynamics (ABA) +// Eigen::VectorXd tau = Eigen::VectorXd::Ones(modelEllipsoid.nv); + +// Eigen::VectorXd aAbaEllipsoid = aba(modelEllipsoid, dataEllipsoid, q_xyz, v_xyz, tau, Convention::WORLD); +// Eigen::VectorXd aAbaSphericalZYX = aba(modelSphericalZYX, dataSphericalZYX, q_zyx, v_zyx, tau, Convention::WORLD); + +// BOOST_CHECK(aAbaEllipsoid.isApprox(aAbaSphericalZYX)); + +// // Test with LOCAL convention +// aAbaEllipsoid = aba(modelEllipsoid, dataEllipsoid, q_xyz, v_xyz, tau, Convention::LOCAL); +// aAbaSphericalZYX = aba(modelSphericalZYX, dataSphericalZYX, q_zyx, v_zyx, tau, Convention::LOCAL); + +// BOOST_CHECK(aAbaEllipsoid.isApprox(aAbaSphericalZYX)); + +// // Test with different configurations +// q_zyx << 0.2, -0.5, 1.1; +// v_zyx << 0.3, 0.1, -0.2; + +// converter.convertConfigurationVector(q_zyx, q_xyz); +// converter.convertTangentVector(q_zyx, v_zyx, v_xyz); + +// forwardKinematics(modelEllipsoid, dataEllipsoid, q_xyz, v_xyz); +// forwardKinematics(modelSphericalZYX, dataSphericalZYX, q_zyx, v_zyx); + +// BOOST_CHECK(dataEllipsoid.oMi[1].isApprox(dataSphericalZYX.oMi[1])); + +// BOOST_CHECK(dataEllipsoid.v[1].toVector().isApprox(dataSphericalZYX.v[1].toVector())); + +// tauEllipsoid = rnea(modelEllipsoid, dataEllipsoid, q_xyz, v_xyz, aEllipsoid); +// tauSphericalZYX = rnea(modelSphericalZYX, dataSphericalZYX, q_zyx, v_zyx, aSphericalZYX); + +// BOOST_CHECK(tauEllipsoid.isApprox(tauSphericalZYX)); + +// aAbaEllipsoid = aba(modelEllipsoid, dataEllipsoid, q_xyz, v_xyz, tau, Convention::WORLD); +// aAbaSphericalZYX = aba(modelSphericalZYX, dataSphericalZYX, q_zyx, v_zyx, tau, Convention::WORLD); + +// BOOST_CHECK(aAbaEllipsoid.isApprox(aAbaSphericalZYX)); + +// } + +BOOST_AUTO_TEST_CASE(vsSphericalZYX) +{ + using namespace pinocchio; + typedef SE3::Vector3 Vector3; + typedef SE3::Matrix3 Matrix3; + + Inertia inertia(1., Vector3(0.5, 0., 0.0), Matrix3::Identity()); + SE3 pos(1); + pos.translation() = SE3::LinearType(1., 0., 0.); + + // Create both models with the same structure + Model modelEllipsoid, modelSphericalZYX; + addJointAndBody(modelEllipsoid, JointModelEllipsoid(0, 0, 0), 0, pos, "ellipsoid", inertia); + addJointAndBody(modelSphericalZYX, JointModelSphericalZYX(), 0, pos, "spherical", inertia); + + Data dataEllipsoid(modelEllipsoid); + Data dataSphericalZYX(modelSphericalZYX); + + // Start with ZYX angles + Eigen::VectorXd q_s(3); + q_s << 0.5, 1.2, -0.8; // Z=0.5, Y=1.2, X=-0.8 + Eigen::VectorXd qd_s(3); + qd_s << 0.1, -0.3, 0.7; // ZYX angle velocities + + // Compute the rotation matrix from ZYX angles + forwardKinematics(modelSphericalZYX, dataSphericalZYX, q_s, qd_s); + const Matrix3 & R = dataSphericalZYX.oMi[1].rotation(); + const Motion & spatial_vel_zyx = dataSphericalZYX.v[1]; + + std::cout << "\n=== Target Rotation Matrix (from ZYX) ===" << std::endl; + std::cout << R << std::endl; + + // Extract XYZ Euler angles from the rotation matrix + // For XYZ convention: R = Rx(x) * Ry(y) * Rz(z) + // We need to solve for x, y, z + + Eigen::Vector3d q_e; + + // From the Ellipsoid rotation matrix structure: + // R(0,2) = sin(y) + // R(1,2) = -sin(x)*cos(y) + // R(2,2) = cos(x)*cos(y) + + double sy = R(0, 2); + q_e(1) = std::asin(sy); // y angle + + double cy = std::cos(q_e(1)); + + if (std::abs(cy) > 1e-6) { + // Not at singularity + q_e(0) = std::atan2(-R(1, 2) / cy, R(2, 2) / cy); // x angle + q_e(2) = std::atan2(-R(0, 1) / cy, R(0, 0) / cy); // z angle + } else { + // Gimbal lock - choose x = 0 + q_e(0) = 0.0; + q_e(2) = std::atan2(R(1, 0), R(1, 1)); + } + + std::cout << "\n=== Configuration ===" << std::endl; + std::cout << "q_s (Z,Y,X): " << q_s.transpose() << std::endl; + std::cout << "q_e (X,Y,Z): " << q_e.transpose() << std::endl; + + // Get the motion subspace matrices (which give us the Jacobians) + JointModelSphericalZYX jmodel_s; + JointDataSphericalZYX jdata_s = jmodel_s.createData(); + jmodel_s.calc(jdata_s, q_s); + + JointModelEllipsoid jmodel_e(0, 0, 0); + JointDataEllipsoid jdata_e = jmodel_e.createData(); + jmodel_e.calc(jdata_e, q_e); + + + // The motion subspace S gives us: omega = S * v + Matrix3 S_s = jdata_s.S.matrix().bottomRows<3>(); // Angular part + Matrix3 S_e = jdata_e.S.matrix().bottomRows<3>(); // Angular part + + Eigen::Vector3d qd_e = S_e.inverse() * S_s * qd_s; + + Eigen::Vector3d w_s = S_s * qd_s; + Eigen::Vector3d w_e = S_e * qd_e; + + + std::cout << "\n=== Joint Velocities ===" << std::endl; + std::cout << "qd_s (joint velocities): " << qd_s.transpose() << std::endl; + std::cout << "qd_e (joint velocities): " << qd_e.transpose() << std::endl; + + std::cout << "\n=== Motion Subspace Matrices ===" << std::endl; + std::cout << "S_zyx (full 6x3):\n" << jdata_s.S.matrix() << std::endl; + std::cout << "\nS_xyz (full 6x3):\n" << jdata_e.S.matrix() << std::endl; + + std::cout << "\n=== Angular Velocities from Joint Velocities ===" << std::endl; + std::cout << "omega from ZYX (S_s * qd_s): " << w_s.transpose() << std::endl; + std::cout << "omega from XYZ (S_e * qd_e): " << w_e.transpose() << std::endl; + + BOOST_CHECK(w_s.isApprox(w_e)); + std::cout << "✓ Angular velocities from joint velocities match" << std::endl; + + // Compute forward kinematics with the converted configurations + forwardKinematics(modelEllipsoid, dataEllipsoid, q_e, qd_e); + + // Also get the joint data to see body-frame velocities + JointDataEllipsoid jdata_e_fk = jmodel_e.createData(); + JointDataEllipsoid jdata_e_fk2 = jmodel_e.createData(); + jmodel_e.calc(jdata_e_fk, q_e, qd_e); + jmodel_e.calc(jdata_e_fk2, q_e); + + std::cout << "\n=== Motion Subspace Matrices after FK ===" << std::endl; + std::cout << "S_e calc q:\n" << jdata_e_fk.S.matrix() << std::endl; + std::cout << "\nS_e calc q v:\n" << jdata_e_fk2.S.matrix() << std::endl; + + BOOST_CHECK(jdata_e_fk.S.matrix().isApprox(jdata_e_fk2.S.matrix())); + std::cout << "✓ Motion subspace matrices match" << std::endl; + + JointDataSphericalZYX jdata_s_fk = jmodel_s.createData(); + jmodel_s.calc(jdata_s_fk, q_s, qd_s); + + std::cout << "\n=== Joint-frame velocities (S * v) ===" << std::endl; + Eigen::Matrix joint_vel_e = jdata_e_fk.S.matrix() * qd_e; + std::cout << "joint_vel_e: " << joint_vel_e.transpose() << std::endl; + std::cout << "jdata_e_fk.v : " << jdata_e_fk.v.toVector().transpose() << std::endl; + + + + Eigen::Matrix joint_vel_s = jdata_s_fk.S.matrix() * qd_s; + std::cout << "Ellipsoid (S_xyz * qd_e): " << joint_vel_e.transpose() << std::endl; + std::cout << "SphericalZYX (S_zyx * qd_s): " << joint_vel_s.transpose() << std::endl; + + std::cout << "\n=== Rotation Matrices ===" << std::endl; + std::cout << "Ellipsoid rotation:\n" << dataEllipsoid.oMi[1].rotation() << std::endl; + std::cout << "\nSphericalZYX rotation:\n" << dataSphericalZYX.oMi[1].rotation() << std::endl; + + std::cout << "\n=== Translations ===" << std::endl; + std::cout << "Ellipsoid translation: " << dataEllipsoid.oMi[1].translation().transpose() << std::endl; + std::cout << "SphericalZYX translation: " << dataSphericalZYX.oMi[1].translation().transpose() << std::endl; + + std::cout << "\n=== Spatial Velocities ===" << std::endl; + std::cout << "Ellipsoid v:\n" << dataEllipsoid.v[1].toVector().transpose() << std::endl; + std::cout << "SphericalZYX v:\n" << dataSphericalZYX.v[1].toVector().transpose() << std::endl; + + BOOST_CHECK(dataEllipsoid.v[1].toVector().isApprox(dataSphericalZYX.v[1].toVector())); + std::cout << "✓ Spatial velocities match" << std::endl; + + BOOST_CHECK(dataEllipsoid.oMi[1].isApprox(dataSphericalZYX.oMi[1])); + std::cout << "✓ Full oMi[1] matches" << std::endl; +} + +BOOST_AUTO_TEST_SUITE_END() diff --git a/unittest/joint-generic.cpp b/unittest/joint-generic.cpp index 1f993b7bb7..6db8bc18b3 100644 --- a/unittest/joint-generic.cpp +++ b/unittest/joint-generic.cpp @@ -264,6 +264,23 @@ struct init> } }; +template +struct init> +{ + typedef pinocchio::JointModelEllipsoidTpl JointModel; + + static JointModel run() + { + JointModel jmodel(Scalar(0.01), + Scalar(0.02), + Scalar(0.03) + ); + + jmodel.setIndexes(0, 0, 0); + return jmodel; + } +}; + template struct init> { diff --git a/unittest/serialization.cpp b/unittest/serialization.cpp index fc810abb67..2bb1d24509 100644 --- a/unittest/serialization.cpp +++ b/unittest/serialization.cpp @@ -349,6 +349,23 @@ struct init> } }; +template +struct init> +{ + typedef pinocchio::JointModelEllipsoidTpl JointModel; + + static JointModel run() + { + JointModel jmodel( + static_cast(0.01), + static_cast(0.02), + static_cast(0.03)); + + jmodel.setIndexes(0, 0, 0); + return jmodel; + } +}; + template class JointCollection> struct init> { From 19c801651ff8d4755e60e14032dd2d003b8a295b Mon Sep 17 00:00:00 2001 From: ipuch Date: Thu, 23 Oct 2025 10:25:53 +0200 Subject: [PATCH 02/48] refactor(joint-ellipsoid): extra methods for S and Sdot --- .../multibody/joint/joint-ellipsoid.hpp | 631 +++++++----------- 1 file changed, 235 insertions(+), 396 deletions(-) diff --git a/include/pinocchio/multibody/joint/joint-ellipsoid.hpp b/include/pinocchio/multibody/joint/joint-ellipsoid.hpp index c158a1655d..1ec9d572b4 100644 --- a/include/pinocchio/multibody/joint/joint-ellipsoid.hpp +++ b/include/pinocchio/multibody/joint/joint-ellipsoid.hpp @@ -167,10 +167,7 @@ namespace pinocchio template void calc(JointDataDerived & data, const typename Eigen::MatrixBase & qs) const - // qs is the full configuration vector of the multibody system { - // C'est ici que ça commence. - // only the q of the joint is needed. data.joint_q = qs.template segment(idx_q()); Scalar c0, s0; @@ -196,34 +193,7 @@ namespace pinocchio data.M.translation() << radius_a * nx, radius_b * ny, radius_c * nz; - Scalar dndotx_dqdot1, dndoty_dqdot0, dndoty_dqdot1, dndotz_dqdot0, dndotz_dqdot1; - dndotx_dqdot1 = c1; - dndoty_dqdot0 = - c0 * c1; - dndoty_dqdot1 = s0 * s1; - dndotz_dqdot0 = - c1 * s0; - dndotz_dqdot1 = - c0 * s1; - - Scalar S_11, S_21, S_31, S_12, S_22, S_32; - // S_11 = c1 * (-radius_b * c0 * (c0 * s2 + c2 * s0 * s1) - radius_c * s0 * (- c0 * c2 * s1 + s0 * s2)); - S_11 = dndoty_dqdot0 * radius_b * (c0 *s2 + c2 * s0 * s1) + dndotz_dqdot0 * radius_c * (- c0 * c2 * s1 + s0 * s2); - // S_21 = c1 * (radius_b * c0 * (- c0 * c2 + s0 * s1 * s2) - radius_c * s0 * (c0 * s1 * s2 + c2 * s0)); - S_21 = - dndoty_dqdot0 * radius_b * (- c0 * c2 + s0 * s1 * s2) + dndotz_dqdot0 * radius_c * (c0 * s1 * s2 + c2 * s0); - // S_31 = c0 * c1 * c1 * s0 * (radius_b - radius_c); - S_31 = c1 * (- dndoty_dqdot0 * radius_b * s0 + dndotz_dqdot0 * radius_c * c0); - - //S_12 = radius_a * c1c2 * c1 + radius_b * s0 * s1 * (c0 * s2 + c2 * s0 * s1) - radius_c * c0 * s1 * (- c0 * c2 * s1 + s0 * s2); - S_12 = dndotx_dqdot1 * radius_a * c1 * c2 + dndoty_dqdot1 * radius_b * (c0 * s2 + c2 * s0 * s1) + dndotz_dqdot1 * radius_c * (- c0 * c2 * s1 + s0 * s2); - // S_22 = - radius_a * c1s2 * c1 - radius_b * s0 * s1 * (- c0 * c2 + s0 * s1 * s2) - radius_c * c0 * s1 * (c0 * s1 * s2 + c2 * s0); - S_22 = - dndotx_dqdot1 * radius_a * c1 * s2 - dndoty_dqdot1 * radius_b * (- c0 * c2 + s0 * s1 * s2) + dndotz_dqdot1 * radius_c * (c0 * s1 * s2 + c2 * s0); - //S_32 = c1 * s1 * (radius_a - radius_b * s0 * s0 - radius_c * c0 * c0); - S_32 = dndotx_dqdot1 * radius_a * s1 - dndoty_dqdot1 * radius_b * c1 * s0 + dndotz_dqdot1 * radius_c * c0 * c1; - - data.S.matrix() << S_11, S_12, Scalar(0), - S_21, S_22, Scalar(0), - S_31, S_32, Scalar(0), - c1c2, s2, Scalar(0), - -c1s2, c2, Scalar(0), - s1, Scalar(0), Scalar(1); + computeMotionSubspace(s0, c0, s1, c1, s2, c2, data); } template @@ -252,193 +222,12 @@ namespace pinocchio dndotz_dqdot0 = - c1 * s0; dndotz_dqdot1 = - c0 * s1; - - Scalar S_11, S_21, S_31, S_12, S_22, S_32; - // S_11 = c1 * (-radius_b * c0 * (c0 * s2 + c2 * s0 * s1) - radius_c * s0 * (- c0 * c2 * s1 + s0 * s2)); - S_11 = dndoty_dqdot0 * radius_b * (c0 *s2 + c2 * s0 * s1) + dndotz_dqdot0 * radius_c * (- c0 * c2 * s1 + s0 * s2); - // S_21 = c1 * (radius_b * c0 * (- c0 * c2 + s0 * s1 * s2) - radius_c * s0 * (c0 * s1 * s2 + c2 * s0)); - S_21 = - dndoty_dqdot0 * radius_b * (- c0 * c2 + s0 * s1 * s2) + dndotz_dqdot0 * radius_c * (c0 * s1 * s2 + c2 * s0); - // S_31 = c0 * c1 * c1 * s0 * (radius_b - radius_c); - S_31 = c1 * (- dndoty_dqdot0 * radius_b * s0 + dndotz_dqdot0 * radius_c * c0); - - //S_12 = radius_a * c1c2 * c1 + radius_b * s0 * s1 * (c0 * s2 + c2 * s0 * s1) - radius_c * c0 * s1 * (- c0 * c2 * s1 + s0 * s2); - S_12 = dndotx_dqdot1 * radius_a * c1 * c2 + dndoty_dqdot1 * radius_b * (c0 * s2 + c2 * s0 * s1) + dndotz_dqdot1 * radius_c * (- c0 * c2 * s1 + s0 * s2); - // S_22 = - radius_a * c1s2 * c1 - radius_b * s0 * s1 * (- c0 * c2 + s0 * s1 * s2) - radius_c * c0 * s1 * (c0 * s1 * s2 + c2 * s0); - S_22 = - dndotx_dqdot1 * radius_a * c1 * s2 - dndoty_dqdot1 * radius_b * (- c0 * c2 + s0 * s1 * s2) + dndotz_dqdot1 * radius_c * (c0 * s1 * s2 + c2 * s0); - //S_32 = c1 * s1 * (radius_a - radius_b * s0 * s0 - radius_c * c0 * c0); - S_32 = dndotx_dqdot1 * radius_a * s1 - dndoty_dqdot1 * radius_b * c1 * s0 + dndotz_dqdot1 * radius_c * c0 * c1; - - data.S.matrix() << S_11, S_12, Scalar(0), - S_21, S_22, Scalar(0), - S_31, S_32, Scalar(0), - c1c2, s2, Scalar(0), - -c1s2, c2, Scalar(0), - s1, Scalar(0), Scalar(1); + computeMotionSubspace(s0, c0, s1, c1, s2, c2, dndotx_dqdot1, dndoty_dqdot0, dndoty_dqdot1, dndotz_dqdot0, dndotz_dqdot1, data); // Velocity part data.v.toVector().noalias() = data.S.matrix() * data.joint_v; - // Compute Sdot for bias acceleration - Scalar qdot0, qdot1, qdot2; - qdot0 = data.joint_v(0); - qdot1 = data.joint_v(1); - qdot2 = data.joint_v(2); - - Scalar Sdot_11, Sdot_21, Sdot_31, Sdot_41, Sdot_51, Sdot_61; - Scalar Sdot_12, Sdot_22, Sdot_32, Sdot_42, Sdot_52, Sdot_62; - Scalar Sdot_13, Sdot_23, Sdot_33, Sdot_43, Sdot_53, Sdot_63; - - // Derivative of dndotXX_dqdot0 with respect to q0 and q1 - Scalar d_dndotx_dqdot1_dq1 = - s1; // dndotx_dqdot1 = c1; - - Scalar d_dndoty_dqdot0_dq0 = s0 * c1; // dndoty_dqdot0 = - c0 * c1; - Scalar d_dndoty_dqdot0_dq1 = c0 * s1; - - Scalar d_dndoty_dqdot1_dq0 = c0 * s1; // dndoty_dqdot1 = s0 * s1; - Scalar d_dndoty_dqdot1_dq1 = s0 * c1; - - Scalar d_dndotz_dqdot0_dq0 = - c1 * c0; // dndotz_dqdot0 = - c1 * s0; - Scalar d_dndotz_dqdot0_dq1 = s0 * s1; - - Scalar d_dndotz_dqdot1_dq0 = s0 * s1; // dndotz_dqdot1 = - c0 * s1; - Scalar d_dndotz_dqdot1_dq1 = -c0 * c1; - - - // Upper part (translation) - // Row 1, Column 1 - Sdot_11 = qdot0 * ( - -dndoty_dqdot0 * radius_b * (-c0 * c2 * s1 + s0 * s2) - + dndotz_dqdot0 * radius_c * (c0 * s2 + c2 * s0 * s1) - + radius_b * (c0 * s2 + c2 * s0 * s1) * d_dndoty_dqdot0_dq0 - + radius_c * (-c0 * c2 * s1 + s0 * s2) * d_dndotz_dqdot0_dq0 - ) - + qdot1 * ( - dndoty_dqdot0 * radius_b * c1 * c2 * s0 - - dndotz_dqdot0 * radius_c * c0 * c1 * c2 - + radius_b * (c0 * s2 + c2 * s0 * s1) * d_dndoty_dqdot0_dq1 - + radius_c * (-c0 * c2 * s1 + s0 * s2) * d_dndotz_dqdot0_dq1 - ) - - qdot2 * ( - dndoty_dqdot0 * radius_b * (-c0 * c2 + s0 * s1 * s2) - - dndotz_dqdot0 * radius_c * (c0 * s1 * s2 + c2 * s0) - ); - - // Row 1, Column 2 - Sdot_12 = qdot0 * ( - -dndoty_dqdot1 * radius_b * (-c0 * c2 * s1 + s0 * s2) - + dndotz_dqdot1 * radius_c * (c0 * s2 + c2 * s0 * s1) - + radius_b * (c0 * s2 + c2 * s0 * s1) * d_dndoty_dqdot0_dq1 - + radius_c * (-c0 * c2 * s1 + s0 * s2) * d_dndotz_dqdot0_dq1 - ) - + qdot1 * ( - -dndotx_dqdot1 * radius_a * c2 * s1 - + dndoty_dqdot1 * radius_b * c1 * c2 * s0 - - dndotz_dqdot1 * radius_c * c0 * c1 * c2 - + radius_a * c1 * c2 * d_dndotx_dqdot1_dq1 - + radius_b * (c0 * s2 + c2 * s0 * s1) * d_dndoty_dqdot1_dq1 - + radius_c * (-c0 * c2 * s1 + s0 * s2) * d_dndotz_dqdot1_dq1 - ) - - qdot2 * ( - dndotx_dqdot1 * radius_a * c1 * s2 - + dndoty_dqdot1 * radius_b * (-c0 * c2 + s0 * s1 * s2) - - dndotz_dqdot1 * radius_c * (c0 * s1 * s2 + c2 * s0) - ); - - // Row 1, Column 3 - Sdot_13 = Scalar(0); - - // Row 2, Column 1 - Sdot_21 = -qdot0 * ( - dndoty_dqdot0 * radius_b * (c0 * s1 * s2 + c2 * s0) - + dndotz_dqdot0 * radius_c * (-c0 * c2 + s0 * s1 * s2) - + radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot0_dq0 - - radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot0_dq0 - ) - - qdot1 * ( - dndoty_dqdot0 * radius_b * c1 * s0 * s2 - - dndotz_dqdot0 * radius_c * c0 * c1 * s2 - + radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot0_dq1 - - radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot0_dq1 - ) - - qdot2 * ( - dndoty_dqdot0 * radius_b * (c0 * s2 + c2 * s0 * s1) - + dndotz_dqdot0 * radius_c * (-c0 * c2 * s1 + s0 * s2) - ); - - // Row 2, Column 2 - Sdot_22 = -qdot0 * ( - dndoty_dqdot1 * radius_b * (c0 * s1 * s2 + c2 * s0) - + dndotz_dqdot1 * radius_c * (-c0 * c2 + s0 * s1 * s2) - + radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot0_dq1 - - radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot0_dq1 - ) - + qdot1 * ( - dndotx_dqdot1 * radius_a * s1 * s2 - - dndoty_dqdot1 * radius_b * c1 * s0 * s2 - + dndotz_dqdot1 * radius_c * c0 * c1 * s2 - - radius_a * c1 * s2 * d_dndotx_dqdot1_dq1 - - radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot1_dq1 - + radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot1_dq1 - ) - - qdot2 * ( - dndotx_dqdot1 * radius_a * c1 * c2 - + dndoty_dqdot1 * radius_b * (c0 * s2 + c2 * s0 * s1) - + dndotz_dqdot1 * radius_c * (-c0 * c2 * s1 + s0 * s2) - ); - - // Row 2, Column 3 - Sdot_23 = Scalar(0); - - // Row 3, Column 1 - Sdot_31 = -qdot0 * c1 * ( - dndoty_dqdot0 * radius_b * c0 - + dndotz_dqdot0 * radius_c * s0 - + radius_b * s0 * d_dndoty_dqdot0_dq0 - - radius_c * c0 * d_dndotz_dqdot0_dq0 - ) - + qdot1 * ( - -c1 * (radius_b * s0 * d_dndoty_dqdot0_dq1 - radius_c * c0 * d_dndotz_dqdot0_dq1) - + s1 * (dndoty_dqdot0 * radius_b * s0 - dndotz_dqdot0 * radius_c * c0) - ); - - // Row 3, Column 2 - Sdot_32 = -qdot0 * c1 * ( - dndoty_dqdot1 * radius_b * c0 - + dndotz_dqdot1 * radius_c * s0 - + radius_b * s0 * d_dndoty_dqdot0_dq1 - - radius_c * c0 * d_dndotz_dqdot0_dq1 - ) - + qdot1 * ( - dndotx_dqdot1 * radius_a * c1 - + dndoty_dqdot1 * radius_b * s0 * s1 - - dndotz_dqdot1 * radius_c * c0 * s1 - + radius_a * s1 * d_dndotx_dqdot1_dq1 - - radius_b * c1 * s0 * d_dndoty_dqdot1_dq1 - + radius_c * c0 * c1 * d_dndotz_dqdot1_dq1 - ); - - // Row 3, Column 3 - Sdot_33 = Scalar(0); - - // Angular part (rows 4-6) - Sdot_41 = -(qdot1 * c2 * s1 + qdot2 * c1 * s2); - Sdot_51 = qdot1 * s1 * s2 - qdot2 * c1 * c2; - Sdot_61 = qdot1 * c1; - - Sdot_42 = qdot2 * c2; - Sdot_52 = -qdot2 * s2; - Sdot_62 = Scalar(0); - - Sdot_43 = Scalar(0); - Sdot_53 = Scalar(0); - Sdot_63 = Scalar(0); - - data.Sdot.matrix() << Sdot_11, Sdot_12, Sdot_13, - Sdot_21, Sdot_22, Sdot_23, - Sdot_31, Sdot_32, Sdot_33, - Sdot_41, Sdot_42, Sdot_43, - Sdot_51, Sdot_52, Sdot_53, - Sdot_61, Sdot_62, Sdot_63; + computeMotionSubspaceDerivative(s0, c0, s1, c1, s2, c2, dndotx_dqdot1, dndoty_dqdot0, dndoty_dqdot1, dndotz_dqdot0, dndotz_dqdot1, data); data.c.toVector().noalias() = data.Sdot.matrix() * data.joint_v; } @@ -481,193 +270,13 @@ namespace pinocchio dndotz_dqdot0 = - c1 * s0; dndotz_dqdot1 = - c0 * s1; - Scalar S_11, S_21, S_31, S_12, S_22, S_32; - // S_11 = c1 * (-radius_b * c0 * (c0 * s2 + c2 * s0 * s1) - radius_c * s0 * (- c0 * c2 * s1 + s0 * s2)); - S_11 = dndoty_dqdot0 * radius_b * (c0 *s2 + c2 * s0 * s1) + dndotz_dqdot0 * radius_c * (- c0 * c2 * s1 + s0 * s2); - // S_21 = c1 * (radius_b * c0 * (- c0 * c2 + s0 * s1 * s2) - radius_c * s0 * (c0 * s1 * s2 + c2 * s0)); - S_21 = - dndoty_dqdot0 * radius_b * (- c0 * c2 + s0 * s1 * s2) + dndotz_dqdot0 * radius_c * (c0 * s1 * s2 + c2 * s0); - // S_31 = c0 * c1 * c1 * s0 * (radius_b - radius_c); - S_31 = c1 * (- dndoty_dqdot0 * radius_b * s0 + dndotz_dqdot0 * radius_c * c0); - - //S_12 = radius_a * c1c2 * c1 + radius_b * s0 * s1 * (c0 * s2 + c2 * s0 * s1) - radius_c * c0 * s1 * (- c0 * c2 * s1 + s0 * s2); - S_12 = dndotx_dqdot1 * radius_a * c1 * c2 + dndoty_dqdot1 * radius_b * (c0 * s2 + c2 * s0 * s1) + dndotz_dqdot1 * radius_c * (- c0 * c2 * s1 + s0 * s2); - // S_22 = - radius_a * c1s2 * c1 - radius_b * s0 * s1 * (- c0 * c2 + s0 * s1 * s2) - radius_c * c0 * s1 * (c0 * s1 * s2 + c2 * s0); - S_22 = - dndotx_dqdot1 * radius_a * c1 * s2 - dndoty_dqdot1 * radius_b * (- c0 * c2 + s0 * s1 * s2) + dndotz_dqdot1 * radius_c * (c0 * s1 * s2 + c2 * s0); - //S_32 = c1 * s1 * (radius_a - radius_b * s0 * s0 - radius_c * c0 * c0); - S_32 = dndotx_dqdot1 * radius_a * s1 - dndoty_dqdot1 * radius_b * c1 * s0 + dndotz_dqdot1 * radius_c * c0 * c1; - - data.S.matrix() << S_11, S_12, Scalar(0), - S_21, S_22, Scalar(0), - S_31, S_32, Scalar(0), - c1c2, s2, Scalar(0), - -c1s2, c2, Scalar(0), - s1, Scalar(0), Scalar(1); + computeMotionSubspace(s0, c0, s1, c1, s2, c2, dndotx_dqdot1, dndoty_dqdot0, dndoty_dqdot1, dndotz_dqdot0, dndotz_dqdot1, data); // Velocity part data.joint_v = vs.template segment(idx_v()); data.v.toVector().noalias() = data.S.matrix() * data.joint_v; - - // Compute Sdot for bias acceleration - Scalar qdot0, qdot1, qdot2; - qdot0 = data.joint_v(0); - qdot1 = data.joint_v(1); - qdot2 = data.joint_v(2); - - Scalar Sdot_11, Sdot_21, Sdot_31, Sdot_41, Sdot_51, Sdot_61; - Scalar Sdot_12, Sdot_22, Sdot_32, Sdot_42, Sdot_52, Sdot_62; - Scalar Sdot_13, Sdot_23, Sdot_33, Sdot_43, Sdot_53, Sdot_63; - - // Derivative of dndotXX_dqdot0 with respect to q0 and q1 - Scalar d_dndotx_dqdot1_dq1 = - s1; // dndotx_dqdot1 = c1; - - Scalar d_dndoty_dqdot0_dq0 = s0 * c1; // dndoty_dqdot0 = - c0 * c1; - Scalar d_dndoty_dqdot0_dq1 = c0 * s1; - - Scalar d_dndoty_dqdot1_dq0 = c0 * s1; // dndoty_dqdot1 = s0 * s1; - Scalar d_dndoty_dqdot1_dq1 = s0 * c1; - - Scalar d_dndotz_dqdot0_dq0 = - c1 * c0; // dndotz_dqdot0 = - c1 * s0; - Scalar d_dndotz_dqdot0_dq1 = s0 * s1; - - Scalar d_dndotz_dqdot1_dq0 = s0 * s1; // dndotz_dqdot1 = - c0 * s1; - Scalar d_dndotz_dqdot1_dq1 = -c0 * c1; - - // Upper part (translation) - // Row 1, Column 1 - Sdot_11 = qdot0 * ( - -dndoty_dqdot0 * radius_b * (-c0 * c2 * s1 + s0 * s2) - + dndotz_dqdot0 * radius_c * (c0 * s2 + c2 * s0 * s1) - + radius_b * (c0 * s2 + c2 * s0 * s1) * d_dndoty_dqdot0_dq0 - + radius_c * (-c0 * c2 * s1 + s0 * s2) * d_dndotz_dqdot0_dq0 - ) - + qdot1 * ( - dndoty_dqdot0 * radius_b * c1 * c2 * s0 - - dndotz_dqdot0 * radius_c * c0 * c1 * c2 - + radius_b * (c0 * s2 + c2 * s0 * s1) * d_dndoty_dqdot0_dq1 - + radius_c * (-c0 * c2 * s1 + s0 * s2) * d_dndotz_dqdot0_dq1 - ) - - qdot2 * ( - dndoty_dqdot0 * radius_b * (-c0 * c2 + s0 * s1 * s2) - - dndotz_dqdot0 * radius_c * (c0 * s1 * s2 + c2 * s0) - ); - - // Row 1, Column 2 - Sdot_12 = qdot0 * ( - -dndoty_dqdot1 * radius_b * (-c0 * c2 * s1 + s0 * s2) - + dndotz_dqdot1 * radius_c * (c0 * s2 + c2 * s0 * s1) - + radius_b * (c0 * s2 + c2 * s0 * s1) * d_dndoty_dqdot0_dq1 - + radius_c * (-c0 * c2 * s1 + s0 * s2) * d_dndotz_dqdot0_dq1 - ) - + qdot1 * ( - -dndotx_dqdot1 * radius_a * c2 * s1 - + dndoty_dqdot1 * radius_b * c1 * c2 * s0 - - dndotz_dqdot1 * radius_c * c0 * c1 * c2 - + radius_a * c1 * c2 * d_dndotx_dqdot1_dq1 - + radius_b * (c0 * s2 + c2 * s0 * s1) * d_dndoty_dqdot1_dq1 - + radius_c * (-c0 * c2 * s1 + s0 * s2) * d_dndotz_dqdot1_dq1 - ) - - qdot2 * ( - dndotx_dqdot1 * radius_a * c1 * s2 - + dndoty_dqdot1 * radius_b * (-c0 * c2 + s0 * s1 * s2) - - dndotz_dqdot1 * radius_c * (c0 * s1 * s2 + c2 * s0) - ); - - // Row 1, Column 3 - Sdot_13 = Scalar(0); - - // Row 2, Column 1 - Sdot_21 = -qdot0 * ( - dndoty_dqdot0 * radius_b * (c0 * s1 * s2 + c2 * s0) - + dndotz_dqdot0 * radius_c * (-c0 * c2 + s0 * s1 * s2) - + radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot0_dq0 - - radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot0_dq0 - ) - - qdot1 * ( - dndoty_dqdot0 * radius_b * c1 * s0 * s2 - - dndotz_dqdot0 * radius_c * c0 * c1 * s2 - + radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot0_dq1 - - radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot0_dq1 - ) - - qdot2 * ( - dndoty_dqdot0 * radius_b * (c0 * s2 + c2 * s0 * s1) - + dndotz_dqdot0 * radius_c * (-c0 * c2 * s1 + s0 * s2) - ); - - // Row 2, Column 2 - Sdot_22 = -qdot0 * ( - dndoty_dqdot1 * radius_b * (c0 * s1 * s2 + c2 * s0) - + dndotz_dqdot1 * radius_c * (-c0 * c2 + s0 * s1 * s2) - + radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot0_dq1 - - radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot0_dq1 - ) - + qdot1 * ( - dndotx_dqdot1 * radius_a * s1 * s2 - - dndoty_dqdot1 * radius_b * c1 * s0 * s2 - + dndotz_dqdot1 * radius_c * c0 * c1 * s2 - - radius_a * c1 * s2 * d_dndotx_dqdot1_dq1 - - radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot1_dq1 - + radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot1_dq1 - ) - - qdot2 * ( - dndotx_dqdot1 * radius_a * c1 * c2 - + dndoty_dqdot1 * radius_b * (c0 * s2 + c2 * s0 * s1) - + dndotz_dqdot1 * radius_c * (-c0 * c2 * s1 + s0 * s2) - ); - - // Row 2, Column 3 - Sdot_23 = Scalar(0); - - // Row 3, Column 1 - Sdot_31 = -qdot0 * c1 * ( - dndoty_dqdot0 * radius_b * c0 - + dndotz_dqdot0 * radius_c * s0 - + radius_b * s0 * d_dndoty_dqdot0_dq0 - - radius_c * c0 * d_dndotz_dqdot0_dq0 - ) - + qdot1 * ( - -c1 * (radius_b * s0 * d_dndoty_dqdot0_dq1 - radius_c * c0 * d_dndotz_dqdot0_dq1) - + s1 * (dndoty_dqdot0 * radius_b * s0 - dndotz_dqdot0 * radius_c * c0) - ); - - // Row 3, Column 2 - Sdot_32 = -qdot0 * c1 * ( - dndoty_dqdot1 * radius_b * c0 - + dndotz_dqdot1 * radius_c * s0 - + radius_b * s0 * d_dndoty_dqdot0_dq1 - - radius_c * c0 * d_dndotz_dqdot0_dq1 - ) - + qdot1 * ( - dndotx_dqdot1 * radius_a * c1 - + dndoty_dqdot1 * radius_b * s0 * s1 - - dndotz_dqdot1 * radius_c * c0 * s1 - + radius_a * s1 * d_dndotx_dqdot1_dq1 - - radius_b * c1 * s0 * d_dndoty_dqdot1_dq1 - + radius_c * c0 * c1 * d_dndotz_dqdot1_dq1 - ); - - // Row 3, Column 3 - Sdot_33 = Scalar(0); - - // Angular part (rows 4-6) - Sdot_41 = -(qdot1 * c2 * s1 + qdot2 * c1 * s2); - Sdot_51 = qdot1 * s1 * s2 - qdot2 * c1 * c2; - Sdot_61 = qdot1 * c1; - - Sdot_42 = qdot2 * c2; - Sdot_52 = -qdot2 * s2; - Sdot_62 = Scalar(0); - - Sdot_43 = Scalar(0); - Sdot_53 = Scalar(0); - Sdot_63 = Scalar(0); - - data.Sdot.matrix() << Sdot_11, Sdot_12, Sdot_13, - Sdot_21, Sdot_22, Sdot_23, - Sdot_31, Sdot_32, Sdot_33, - Sdot_41, Sdot_42, Sdot_43, - Sdot_51, Sdot_52, Sdot_53, - Sdot_61, Sdot_62, Sdot_63; + computeMotionSubspaceDerivative(s0, c0, s1, c1, s2, c2, dndotx_dqdot1, dndoty_dqdot0, dndoty_dqdot1, dndotz_dqdot0, dndotz_dqdot1, data); data.c.toVector().noalias() = data.Sdot.matrix() * data.joint_v; } @@ -709,6 +318,236 @@ namespace pinocchio return res; } + void computeMotionSubspace( + const Scalar & s0, const Scalar & c0, + const Scalar & s1, const Scalar & c1, + const Scalar & s2, const Scalar & c2, + JointDataDerived & data) const + { + // Common operations + const Scalar c1c2 = c1 * c2; + const Scalar c1s2 = c1 * s2; + + Scalar dndotx_dqdot1, dndoty_dqdot0, dndoty_dqdot1, dndotz_dqdot0, dndotz_dqdot1; + dndotx_dqdot1 = c1; + dndoty_dqdot0 = - c0 * c1; + dndoty_dqdot1 = s0 * s1; + dndotz_dqdot0 = - c1 * s0; + dndotz_dqdot1 = - c0 * s1; + + computeMotionSubspace(s0, c0, s1, c1, s2, c2, dndotx_dqdot1, dndoty_dqdot0, dndoty_dqdot1, dndotz_dqdot0, dndotz_dqdot1, data); + + } + void computeMotionSubspace( + const Scalar & s0, const Scalar & c0, + const Scalar & s1, const Scalar & c1, + const Scalar & s2, const Scalar & c2, + const Scalar & dndotx_dqdot1, + const Scalar & dndoty_dqdot0, + const Scalar & dndoty_dqdot1, + const Scalar & dndotz_dqdot0, + const Scalar & dndotz_dqdot1, + JointDataDerived & data) const + { + // Common operations + const Scalar c1c2 = c1 * c2; + const Scalar c1s2 = c1 * s2; + + Scalar S_11, S_21, S_31, S_12, S_22, S_32; + // S_11 = c1 * (-radius_b * c0 * (c0 * s2 + c2 * s0 * s1) - radius_c * s0 * (- c0 * c2 * s1 + s0 * s2)); + S_11 = dndoty_dqdot0 * radius_b * (c0 *s2 + c2 * s0 * s1) + dndotz_dqdot0 * radius_c * (- c0 * c2 * s1 + s0 * s2); + // S_21 = c1 * (radius_b * c0 * (- c0 * c2 + s0 * s1 * s2) - radius_c * s0 * (c0 * s1 * s2 + c2 * s0)); + S_21 = - dndoty_dqdot0 * radius_b * (- c0 * c2 + s0 * s1 * s2) + dndotz_dqdot0 * radius_c * (c0 * s1 * s2 + c2 * s0); + // S_31 = c0 * c1 * c1 * s0 * (radius_b - radius_c); + S_31 = c1 * (- dndoty_dqdot0 * radius_b * s0 + dndotz_dqdot0 * radius_c * c0); + + //S_12 = radius_a * c1c2 * c1 + radius_b * s0 * s1 * (c0 * s2 + c2 * s0 * s1) - radius_c * c0 * s1 * (- c0 * c2 * s1 + s0 * s2); + S_12 = dndotx_dqdot1 * radius_a * c1 * c2 + dndoty_dqdot1 * radius_b * (c0 * s2 + c2 * s0 * s1) + dndotz_dqdot1 * radius_c * (- c0 * c2 * s1 + s0 * s2); + // S_22 = - radius_a * c1s2 * c1 - radius_b * s0 * s1 * (- c0 * c2 + s0 * s1 * s2) - radius_c * c0 * s1 * (c0 * s1 * s2 + c2 * s0); + S_22 = - dndotx_dqdot1 * radius_a * c1 * s2 - dndoty_dqdot1 * radius_b * (- c0 * c2 + s0 * s1 * s2) + dndotz_dqdot1 * radius_c * (c0 * s1 * s2 + c2 * s0); + //S_32 = c1 * s1 * (radius_a - radius_b * s0 * s0 - radius_c * c0 * c0); + S_32 = dndotx_dqdot1 * radius_a * s1 - dndoty_dqdot1 * radius_b * c1 * s0 + dndotz_dqdot1 * radius_c * c0 * c1; + + data.S.matrix() << S_11, S_12, Scalar(0), + S_21, S_22, Scalar(0), + S_31, S_32, Scalar(0), + c1c2, s2, Scalar(0), + -c1s2, c2, Scalar(0), + s1, Scalar(0), Scalar(1); + } + void computeMotionSubspaceDerivative( + const Scalar & s0, const Scalar & c0, + const Scalar & s1, const Scalar & c1, + const Scalar & s2, const Scalar & c2, + const Scalar & dndotx_dqdot1, + const Scalar & dndoty_dqdot0, + const Scalar & dndoty_dqdot1, + const Scalar & dndotz_dqdot0, + const Scalar & dndotz_dqdot1, + JointDataDerived & data) const + { + // Compute Sdot for bias acceleration + Scalar qdot0, qdot1, qdot2; + qdot0 = data.joint_v(0); + qdot1 = data.joint_v(1); + qdot2 = data.joint_v(2); + + Scalar Sdot_11, Sdot_21, Sdot_31, Sdot_41, Sdot_51, Sdot_61; + Scalar Sdot_12, Sdot_22, Sdot_32, Sdot_42, Sdot_52, Sdot_62; + Scalar Sdot_13, Sdot_23, Sdot_33, Sdot_43, Sdot_53, Sdot_63; + + // Derivative of dndotXX_dqdot0 with respect to q0 and q1 + Scalar d_dndotx_dqdot1_dq1 = - s1; // dndotx_dqdot1 = c1; + + Scalar d_dndoty_dqdot0_dq0 = s0 * c1; // dndoty_dqdot0 = - c0 * c1; + Scalar d_dndoty_dqdot0_dq1 = c0 * s1; + + Scalar d_dndoty_dqdot1_dq0 = c0 * s1; // dndoty_dqdot1 = s0 * s1; + Scalar d_dndoty_dqdot1_dq1 = s0 * c1; + + Scalar d_dndotz_dqdot0_dq0 = - c1 * c0; // dndotz_dqdot0 = - c1 * s0; + Scalar d_dndotz_dqdot0_dq1 = s0 * s1; + + Scalar d_dndotz_dqdot1_dq0 = s0 * s1; // dndotz_dqdot1 = - c0 * s1; + Scalar d_dndotz_dqdot1_dq1 = -c0 * c1; + + + // Upper part (translation) + // Row 1, Column 1 + Sdot_11 = qdot0 * ( + -dndoty_dqdot0 * radius_b * (-c0 * c2 * s1 + s0 * s2) + + dndotz_dqdot0 * radius_c * (c0 * s2 + c2 * s0 * s1) + + radius_b * (c0 * s2 + c2 * s0 * s1) * d_dndoty_dqdot0_dq0 + + radius_c * (-c0 * c2 * s1 + s0 * s2) * d_dndotz_dqdot0_dq0 + ) + + qdot1 * ( + dndoty_dqdot0 * radius_b * c1 * c2 * s0 + - dndotz_dqdot0 * radius_c * c0 * c1 * c2 + + radius_b * (c0 * s2 + c2 * s0 * s1) * d_dndoty_dqdot0_dq1 + + radius_c * (-c0 * c2 * s1 + s0 * s2) * d_dndotz_dqdot0_dq1 + ) + - qdot2 * ( + dndoty_dqdot0 * radius_b * (-c0 * c2 + s0 * s1 * s2) + - dndotz_dqdot0 * radius_c * (c0 * s1 * s2 + c2 * s0) + ); + + // Row 1, Column 2 + Sdot_12 = qdot0 * ( + -dndoty_dqdot1 * radius_b * (-c0 * c2 * s1 + s0 * s2) + + dndotz_dqdot1 * radius_c * (c0 * s2 + c2 * s0 * s1) + + radius_b * (c0 * s2 + c2 * s0 * s1) * d_dndoty_dqdot0_dq1 + + radius_c * (-c0 * c2 * s1 + s0 * s2) * d_dndotz_dqdot0_dq1 + ) + + qdot1 * ( + -dndotx_dqdot1 * radius_a * c2 * s1 + + dndoty_dqdot1 * radius_b * c1 * c2 * s0 + - dndotz_dqdot1 * radius_c * c0 * c1 * c2 + + radius_a * c1 * c2 * d_dndotx_dqdot1_dq1 + + radius_b * (c0 * s2 + c2 * s0 * s1) * d_dndoty_dqdot1_dq1 + + radius_c * (-c0 * c2 * s1 + s0 * s2) * d_dndotz_dqdot1_dq1 + ) + - qdot2 * ( + dndotx_dqdot1 * radius_a * c1 * s2 + + dndoty_dqdot1 * radius_b * (-c0 * c2 + s0 * s1 * s2) + - dndotz_dqdot1 * radius_c * (c0 * s1 * s2 + c2 * s0) + ); + + // Row 1, Column 3 + Sdot_13 = Scalar(0); + + // Row 2, Column 1 + Sdot_21 = -qdot0 * ( + dndoty_dqdot0 * radius_b * (c0 * s1 * s2 + c2 * s0) + + dndotz_dqdot0 * radius_c * (-c0 * c2 + s0 * s1 * s2) + + radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot0_dq0 + - radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot0_dq0 + ) + - qdot1 * ( + dndoty_dqdot0 * radius_b * c1 * s0 * s2 + - dndotz_dqdot0 * radius_c * c0 * c1 * s2 + + radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot0_dq1 + - radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot0_dq1 + ) + - qdot2 * ( + dndoty_dqdot0 * radius_b * (c0 * s2 + c2 * s0 * s1) + + dndotz_dqdot0 * radius_c * (-c0 * c2 * s1 + s0 * s2) + ); + + // Row 2, Column 2 + Sdot_22 = -qdot0 * ( + dndoty_dqdot1 * radius_b * (c0 * s1 * s2 + c2 * s0) + + dndotz_dqdot1 * radius_c * (-c0 * c2 + s0 * s1 * s2) + + radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot0_dq1 + - radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot0_dq1 + ) + + qdot1 * ( + dndotx_dqdot1 * radius_a * s1 * s2 + - dndoty_dqdot1 * radius_b * c1 * s0 * s2 + + dndotz_dqdot1 * radius_c * c0 * c1 * s2 + - radius_a * c1 * s2 * d_dndotx_dqdot1_dq1 + - radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot1_dq1 + + radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot1_dq1 + ) + - qdot2 * ( + dndotx_dqdot1 * radius_a * c1 * c2 + + dndoty_dqdot1 * radius_b * (c0 * s2 + c2 * s0 * s1) + + dndotz_dqdot1 * radius_c * (-c0 * c2 * s1 + s0 * s2) + ); + + // Row 2, Column 3 + Sdot_23 = Scalar(0); + + // Row 3, Column 1 + Sdot_31 = -qdot0 * c1 * ( + dndoty_dqdot0 * radius_b * c0 + + dndotz_dqdot0 * radius_c * s0 + + radius_b * s0 * d_dndoty_dqdot0_dq0 + - radius_c * c0 * d_dndotz_dqdot0_dq0 + ) + + qdot1 * ( + -c1 * (radius_b * s0 * d_dndoty_dqdot0_dq1 - radius_c * c0 * d_dndotz_dqdot0_dq1) + + s1 * (dndoty_dqdot0 * radius_b * s0 - dndotz_dqdot0 * radius_c * c0) + ); + + // Row 3, Column 2 + Sdot_32 = -qdot0 * c1 * ( + dndoty_dqdot1 * radius_b * c0 + + dndotz_dqdot1 * radius_c * s0 + + radius_b * s0 * d_dndoty_dqdot0_dq1 + - radius_c * c0 * d_dndotz_dqdot0_dq1 + ) + + qdot1 * ( + dndotx_dqdot1 * radius_a * c1 + + dndoty_dqdot1 * radius_b * s0 * s1 + - dndotz_dqdot1 * radius_c * c0 * s1 + + radius_a * s1 * d_dndotx_dqdot1_dq1 + - radius_b * c1 * s0 * d_dndoty_dqdot1_dq1 + + radius_c * c0 * c1 * d_dndotz_dqdot1_dq1 + ); + + // Row 3, Column 3 + Sdot_33 = Scalar(0); + + // Angular part (rows 4-6) + Sdot_41 = -(qdot1 * c2 * s1 + qdot2 * c1 * s2); + Sdot_51 = qdot1 * s1 * s2 - qdot2 * c1 * c2; + Sdot_61 = qdot1 * c1; + + Sdot_42 = qdot2 * c2; + Sdot_52 = -qdot2 * s2; + Sdot_62 = Scalar(0); + + Sdot_43 = Scalar(0); + Sdot_53 = Scalar(0); + Sdot_63 = Scalar(0); + + data.Sdot.matrix() << Sdot_11, Sdot_12, Sdot_13, + Sdot_21, Sdot_22, Sdot_23, + Sdot_31, Sdot_32, Sdot_33, + Sdot_41, Sdot_42, Sdot_43, + Sdot_51, Sdot_52, Sdot_53, + Sdot_61, Sdot_62, Sdot_63; + } }; // struct JointModelEllipsoidTpl } // namespace pinocchio From 1749dd9be8be6fd30754a1b341bfc07d577715f4 Mon Sep 17 00:00:00 2001 From: ipuch Date: Thu, 23 Oct 2025 10:48:43 +0200 Subject: [PATCH 03/48] feat(joint-ellipsoid): bindings, model graphs fix: ellipsoid radius variables, and linting --- .../python/parsers/graph/expose-edges.cpp | 6 + .../bindings/python/context/generic.hpp | 3 + .../python/multibody/joint/joints-models.hpp | 22 + .../multibody/joint/joint-ellipsoid.hpp | 481 ++++++++---------- .../pinocchio/parsers/graph/graph-visitor.hpp | 5 + include/pinocchio/parsers/graph/joints.hpp | 25 + src/parsers/graph/model-graph-algo.cpp | 29 ++ src/parsers/graph/model-graph.cpp | 4 + unittest/all-joints.cpp | 5 +- unittest/finite-differences.cpp | 5 +- unittest/joint-ellipsoid.cpp | 220 ++++---- unittest/joint-generic.cpp | 5 +- unittest/model-graph.cpp | 60 +++ unittest/serialization.cpp | 4 +- 14 files changed, 493 insertions(+), 381 deletions(-) diff --git a/bindings/python/parsers/graph/expose-edges.cpp b/bindings/python/parsers/graph/expose-edges.cpp index c5a260a82f..ee8663c0ef 100644 --- a/bindings/python/parsers/graph/expose-edges.cpp +++ b/bindings/python/parsers/graph/expose-edges.cpp @@ -111,6 +111,12 @@ namespace pinocchio .def_readonly("nq", &JointSphericalZYX::nq, "Number of configuration variables.") .def_readonly("nv", &JointSphericalZYX::nv, "Number of tangent variables."); + bp::class_( + "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_( "JointTranslation", "Represents a translation joint.", bp::init<>(bp::args("self"), "Default constructor.")) diff --git a/include/pinocchio/bindings/python/context/generic.hpp b/include/pinocchio/bindings/python/context/generic.hpp index cf2020aa8e..bb2ca16a42 100644 --- a/include/pinocchio/bindings/python/context/generic.hpp +++ b/include/pinocchio/bindings/python/context/generic.hpp @@ -99,6 +99,9 @@ namespace pinocchio typedef JointModelSphericalZYXTpl JointModelSphericalZYX; typedef JointDataSphericalZYXTpl JointDataSphericalZYX; + typedef JointModelEllipsoidTpl JointModelEllipsoid; + typedef JointDataEllipsoidTpl JointDataEllipsoid; + typedef JointDataPrismaticTpl JointDataPX; typedef JointModelPrismaticTpl JointModelPX; diff --git a/include/pinocchio/bindings/python/multibody/joint/joints-models.hpp b/include/pinocchio/bindings/python/multibody/joint/joints-models.hpp index 0afd1017a7..6ebffbf756 100644 --- a/include/pinocchio/bindings/python/multibody/joint/joints-models.hpp +++ b/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_ & + expose_joint_model(bp::class_ & cl) + { + return cl + .def(bp::init( + 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_ & diff --git a/include/pinocchio/multibody/joint/joint-ellipsoid.hpp b/include/pinocchio/multibody/joint/joint-ellipsoid.hpp index 1ec9d572b4..4189633290 100644 --- a/include/pinocchio/multibody/joint/joint-ellipsoid.hpp +++ b/include/pinocchio/multibody/joint/joint-ellipsoid.hpp @@ -37,7 +37,7 @@ namespace pinocchio typedef JointModelEllipsoidTpl JointModelDerived; typedef JointMotionSubspaceTpl<3, Scalar, Options, 3> Constraint_t; typedef SE3Tpl Transformation_t; - + typedef MotionTpl Motion_t; typedef MotionTpl Bias_t; @@ -69,8 +69,7 @@ namespace pinocchio }; template - struct JointDataEllipsoidTpl - : public JointDataBase> + struct JointDataEllipsoidTpl : public JointDataBase> { EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef JointEllipsoidTpl<_Scalar, _Options> JointDerived; @@ -118,8 +117,7 @@ namespace pinocchio PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelEllipsoidTpl); template - struct JointModelEllipsoidTpl - : public JointModelBase> + struct JointModelEllipsoidTpl : public JointModelBase> { EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef JointEllipsoidTpl<_Scalar, _Options> JointDerived; @@ -147,11 +145,10 @@ namespace pinocchio radius_c = Scalar(0.01); } - explicit JointModelEllipsoidTpl( - const Scalar & a, - const Scalar & b, - const Scalar & c) - : radius_a(a), radius_b(b), radius_c(c) + explicit JointModelEllipsoidTpl(const Scalar & a, const Scalar & b, const Scalar & c) + : radius_a(a) + , radius_b(b) + , radius_c(c) { } @@ -177,15 +174,14 @@ namespace pinocchio Scalar c2, s2; SINCOS(data.joint_q(2), &s2, &c2); - //common operations + // common operations Scalar c1c2, c1s2; c1c2 = c1 * c2; c1s2 = c1 * s2; - data.M.rotation() << c1c2, -c1s2, s1, - c0 * s2 + c2 * s0 * s1, c0 * c2 - s0 * s1 * s2, -c1 * s0, - -c0 * c2 * s1 + s0 * s2, c0 * s1 * s2 + c2 * s0, c0 * c1; - + data.M.rotation() << c1c2, -c1s2, s1, c0 * s2 + c2 * s0 * s1, c0 * c2 - s0 * s1 * s2, + -c1 * s0, -c0 * c2 * s1 + s0 * s2, c0 * s1 * s2 + c2 * s0, c0 * c1; + Scalar nx, ny, nz; nx = s1; ny = -s0 * c1; @@ -214,20 +210,24 @@ namespace pinocchio Scalar c1c2, c1s2; c1c2 = c1 * c2; c1s2 = c1 * s2; - + Scalar dndotx_dqdot1, dndoty_dqdot0, dndoty_dqdot1, dndotz_dqdot0, dndotz_dqdot1; dndotx_dqdot1 = c1; - dndoty_dqdot0 = - c0 * c1; + dndoty_dqdot0 = -c0 * c1; dndoty_dqdot1 = s0 * s1; - dndotz_dqdot0 = - c1 * s0; - dndotz_dqdot1 = - c0 * s1; + dndotz_dqdot0 = -c1 * s0; + dndotz_dqdot1 = -c0 * s1; + + computeMotionSubspace( + s0, c0, s1, c1, s2, c2, dndotx_dqdot1, dndoty_dqdot0, dndoty_dqdot1, dndotz_dqdot0, + dndotz_dqdot1, data); - computeMotionSubspace(s0, c0, s1, c1, s2, c2, dndotx_dqdot1, dndoty_dqdot0, dndoty_dqdot1, dndotz_dqdot0, dndotz_dqdot1, data); - // Velocity part data.v.toVector().noalias() = data.S.matrix() * data.joint_v; - computeMotionSubspaceDerivative(s0, c0, s1, c1, s2, c2, dndotx_dqdot1, dndoty_dqdot0, dndoty_dqdot1, dndotz_dqdot0, dndotz_dqdot1, data); + computeMotionSubspaceDerivative( + s0, c0, s1, c1, s2, c2, dndotx_dqdot1, dndoty_dqdot0, dndoty_dqdot1, dndotz_dqdot0, + dndotz_dqdot1, data); data.c.toVector().noalias() = data.Sdot.matrix() * data.joint_v; } @@ -247,15 +247,14 @@ namespace pinocchio Scalar c2, s2; SINCOS(data.joint_q(2), &s2, &c2); - //common operations + // common operations Scalar c1c2, c1s2; c1c2 = c1 * c2; c1s2 = c1 * s2; - data.M.rotation() << c1c2, -c1s2, s1, - c0 * s2 + c2 * s0 * s1, c0 * c2 - s0 * s1 * s2, -c1 * s0, - -c0 * c2 * s1 + s0 * s2, c0 * s1 * s2 + c2 * s0, c0 * c1; - + data.M.rotation() << c1c2, -c1s2, s1, c0 * s2 + c2 * s0 * s1, c0 * c2 - s0 * s1 * s2, + -c1 * s0, -c0 * c2 * s1 + s0 * s2, c0 * s1 * s2 + c2 * s0, c0 * c1; + Scalar nx, ny, nz; nx = s1; ny = -s0 * c1; @@ -265,18 +264,22 @@ namespace pinocchio Scalar dndotx_dqdot1, dndoty_dqdot0, dndoty_dqdot1, dndotz_dqdot0, dndotz_dqdot1; dndotx_dqdot1 = c1; - dndoty_dqdot0 = - c0 * c1; + dndoty_dqdot0 = -c0 * c1; dndoty_dqdot1 = s0 * s1; - dndotz_dqdot0 = - c1 * s0; - dndotz_dqdot1 = - c0 * s1; - - computeMotionSubspace(s0, c0, s1, c1, s2, c2, dndotx_dqdot1, dndoty_dqdot0, dndoty_dqdot1, dndotz_dqdot0, dndotz_dqdot1, data); + dndotz_dqdot0 = -c1 * s0; + dndotz_dqdot1 = -c0 * s1; + + computeMotionSubspace( + s0, c0, s1, c1, s2, c2, dndotx_dqdot1, dndoty_dqdot0, dndoty_dqdot1, dndotz_dqdot0, + dndotz_dqdot1, data); // Velocity part data.joint_v = vs.template segment(idx_v()); data.v.toVector().noalias() = data.S.matrix() * data.joint_v; - - computeMotionSubspaceDerivative(s0, c0, s1, c1, s2, c2, dndotx_dqdot1, dndoty_dqdot0, dndoty_dqdot1, dndotz_dqdot0, dndotz_dqdot1, data); + + computeMotionSubspaceDerivative( + s0, c0, s1, c1, s2, c2, dndotx_dqdot1, dndoty_dqdot0, dndoty_dqdot1, dndotz_dqdot0, + dndotz_dqdot1, data); data.c.toVector().noalias() = data.Sdot.matrix() * data.joint_v; } @@ -318,236 +321,186 @@ namespace pinocchio return res; } - void computeMotionSubspace( - const Scalar & s0, const Scalar & c0, - const Scalar & s1, const Scalar & c1, - const Scalar & s2, const Scalar & c2, - JointDataDerived & data) const - { - // Common operations - const Scalar c1c2 = c1 * c2; - const Scalar c1s2 = c1 * s2; - - Scalar dndotx_dqdot1, dndoty_dqdot0, dndoty_dqdot1, dndotz_dqdot0, dndotz_dqdot1; - dndotx_dqdot1 = c1; - dndoty_dqdot0 = - c0 * c1; - dndoty_dqdot1 = s0 * s1; - dndotz_dqdot0 = - c1 * s0; - dndotz_dqdot1 = - c0 * s1; - - computeMotionSubspace(s0, c0, s1, c1, s2, c2, dndotx_dqdot1, dndoty_dqdot0, dndoty_dqdot1, dndotz_dqdot0, dndotz_dqdot1, data); - - } - void computeMotionSubspace( - const Scalar & s0, const Scalar & c0, - const Scalar & s1, const Scalar & c1, - const Scalar & s2, const Scalar & c2, - const Scalar & dndotx_dqdot1, - const Scalar & dndoty_dqdot0, - const Scalar & dndoty_dqdot1, - const Scalar & dndotz_dqdot0, - const Scalar & dndotz_dqdot1, - JointDataDerived & data) const - { - // Common operations - const Scalar c1c2 = c1 * c2; - const Scalar c1s2 = c1 * s2; - - Scalar S_11, S_21, S_31, S_12, S_22, S_32; - // S_11 = c1 * (-radius_b * c0 * (c0 * s2 + c2 * s0 * s1) - radius_c * s0 * (- c0 * c2 * s1 + s0 * s2)); - S_11 = dndoty_dqdot0 * radius_b * (c0 *s2 + c2 * s0 * s1) + dndotz_dqdot0 * radius_c * (- c0 * c2 * s1 + s0 * s2); - // S_21 = c1 * (radius_b * c0 * (- c0 * c2 + s0 * s1 * s2) - radius_c * s0 * (c0 * s1 * s2 + c2 * s0)); - S_21 = - dndoty_dqdot0 * radius_b * (- c0 * c2 + s0 * s1 * s2) + dndotz_dqdot0 * radius_c * (c0 * s1 * s2 + c2 * s0); - // S_31 = c0 * c1 * c1 * s0 * (radius_b - radius_c); - S_31 = c1 * (- dndoty_dqdot0 * radius_b * s0 + dndotz_dqdot0 * radius_c * c0); - - //S_12 = radius_a * c1c2 * c1 + radius_b * s0 * s1 * (c0 * s2 + c2 * s0 * s1) - radius_c * c0 * s1 * (- c0 * c2 * s1 + s0 * s2); - S_12 = dndotx_dqdot1 * radius_a * c1 * c2 + dndoty_dqdot1 * radius_b * (c0 * s2 + c2 * s0 * s1) + dndotz_dqdot1 * radius_c * (- c0 * c2 * s1 + s0 * s2); - // S_22 = - radius_a * c1s2 * c1 - radius_b * s0 * s1 * (- c0 * c2 + s0 * s1 * s2) - radius_c * c0 * s1 * (c0 * s1 * s2 + c2 * s0); - S_22 = - dndotx_dqdot1 * radius_a * c1 * s2 - dndoty_dqdot1 * radius_b * (- c0 * c2 + s0 * s1 * s2) + dndotz_dqdot1 * radius_c * (c0 * s1 * s2 + c2 * s0); - //S_32 = c1 * s1 * (radius_a - radius_b * s0 * s0 - radius_c * c0 * c0); - S_32 = dndotx_dqdot1 * radius_a * s1 - dndoty_dqdot1 * radius_b * c1 * s0 + dndotz_dqdot1 * radius_c * c0 * c1; - - data.S.matrix() << S_11, S_12, Scalar(0), - S_21, S_22, Scalar(0), - S_31, S_32, Scalar(0), - c1c2, s2, Scalar(0), - -c1s2, c2, Scalar(0), - s1, Scalar(0), Scalar(1); - } - void computeMotionSubspaceDerivative( - const Scalar & s0, const Scalar & c0, - const Scalar & s1, const Scalar & c1, - const Scalar & s2, const Scalar & c2, - const Scalar & dndotx_dqdot1, - const Scalar & dndoty_dqdot0, - const Scalar & dndoty_dqdot1, - const Scalar & dndotz_dqdot0, - const Scalar & dndotz_dqdot1, - JointDataDerived & data) const - { - // Compute Sdot for bias acceleration - Scalar qdot0, qdot1, qdot2; - qdot0 = data.joint_v(0); - qdot1 = data.joint_v(1); - qdot2 = data.joint_v(2); - - Scalar Sdot_11, Sdot_21, Sdot_31, Sdot_41, Sdot_51, Sdot_61; - Scalar Sdot_12, Sdot_22, Sdot_32, Sdot_42, Sdot_52, Sdot_62; - Scalar Sdot_13, Sdot_23, Sdot_33, Sdot_43, Sdot_53, Sdot_63; - - // Derivative of dndotXX_dqdot0 with respect to q0 and q1 - Scalar d_dndotx_dqdot1_dq1 = - s1; // dndotx_dqdot1 = c1; - - Scalar d_dndoty_dqdot0_dq0 = s0 * c1; // dndoty_dqdot0 = - c0 * c1; - Scalar d_dndoty_dqdot0_dq1 = c0 * s1; - - Scalar d_dndoty_dqdot1_dq0 = c0 * s1; // dndoty_dqdot1 = s0 * s1; - Scalar d_dndoty_dqdot1_dq1 = s0 * c1; - - Scalar d_dndotz_dqdot0_dq0 = - c1 * c0; // dndotz_dqdot0 = - c1 * s0; - Scalar d_dndotz_dqdot0_dq1 = s0 * s1; - - Scalar d_dndotz_dqdot1_dq0 = s0 * s1; // dndotz_dqdot1 = - c0 * s1; - Scalar d_dndotz_dqdot1_dq1 = -c0 * c1; - - - // Upper part (translation) - // Row 1, Column 1 - Sdot_11 = qdot0 * ( - -dndoty_dqdot0 * radius_b * (-c0 * c2 * s1 + s0 * s2) - + dndotz_dqdot0 * radius_c * (c0 * s2 + c2 * s0 * s1) - + radius_b * (c0 * s2 + c2 * s0 * s1) * d_dndoty_dqdot0_dq0 - + radius_c * (-c0 * c2 * s1 + s0 * s2) * d_dndotz_dqdot0_dq0 - ) - + qdot1 * ( - dndoty_dqdot0 * radius_b * c1 * c2 * s0 - - dndotz_dqdot0 * radius_c * c0 * c1 * c2 - + radius_b * (c0 * s2 + c2 * s0 * s1) * d_dndoty_dqdot0_dq1 - + radius_c * (-c0 * c2 * s1 + s0 * s2) * d_dndotz_dqdot0_dq1 - ) - - qdot2 * ( - dndoty_dqdot0 * radius_b * (-c0 * c2 + s0 * s1 * s2) - - dndotz_dqdot0 * radius_c * (c0 * s1 * s2 + c2 * s0) - ); - - // Row 1, Column 2 - Sdot_12 = qdot0 * ( - -dndoty_dqdot1 * radius_b * (-c0 * c2 * s1 + s0 * s2) - + dndotz_dqdot1 * radius_c * (c0 * s2 + c2 * s0 * s1) - + radius_b * (c0 * s2 + c2 * s0 * s1) * d_dndoty_dqdot0_dq1 - + radius_c * (-c0 * c2 * s1 + s0 * s2) * d_dndotz_dqdot0_dq1 - ) - + qdot1 * ( - -dndotx_dqdot1 * radius_a * c2 * s1 - + dndoty_dqdot1 * radius_b * c1 * c2 * s0 - - dndotz_dqdot1 * radius_c * c0 * c1 * c2 - + radius_a * c1 * c2 * d_dndotx_dqdot1_dq1 - + radius_b * (c0 * s2 + c2 * s0 * s1) * d_dndoty_dqdot1_dq1 - + radius_c * (-c0 * c2 * s1 + s0 * s2) * d_dndotz_dqdot1_dq1 - ) - - qdot2 * ( - dndotx_dqdot1 * radius_a * c1 * s2 - + dndoty_dqdot1 * radius_b * (-c0 * c2 + s0 * s1 * s2) - - dndotz_dqdot1 * radius_c * (c0 * s1 * s2 + c2 * s0) - ); - - // Row 1, Column 3 - Sdot_13 = Scalar(0); - - // Row 2, Column 1 - Sdot_21 = -qdot0 * ( - dndoty_dqdot0 * radius_b * (c0 * s1 * s2 + c2 * s0) - + dndotz_dqdot0 * radius_c * (-c0 * c2 + s0 * s1 * s2) - + radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot0_dq0 - - radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot0_dq0 - ) - - qdot1 * ( - dndoty_dqdot0 * radius_b * c1 * s0 * s2 - - dndotz_dqdot0 * radius_c * c0 * c1 * s2 - + radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot0_dq1 - - radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot0_dq1 - ) - - qdot2 * ( - dndoty_dqdot0 * radius_b * (c0 * s2 + c2 * s0 * s1) - + dndotz_dqdot0 * radius_c * (-c0 * c2 * s1 + s0 * s2) - ); - - // Row 2, Column 2 - Sdot_22 = -qdot0 * ( - dndoty_dqdot1 * radius_b * (c0 * s1 * s2 + c2 * s0) - + dndotz_dqdot1 * radius_c * (-c0 * c2 + s0 * s1 * s2) - + radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot0_dq1 - - radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot0_dq1 - ) - + qdot1 * ( - dndotx_dqdot1 * radius_a * s1 * s2 - - dndoty_dqdot1 * radius_b * c1 * s0 * s2 - + dndotz_dqdot1 * radius_c * c0 * c1 * s2 - - radius_a * c1 * s2 * d_dndotx_dqdot1_dq1 - - radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot1_dq1 - + radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot1_dq1 - ) - - qdot2 * ( - dndotx_dqdot1 * radius_a * c1 * c2 - + dndoty_dqdot1 * radius_b * (c0 * s2 + c2 * s0 * s1) - + dndotz_dqdot1 * radius_c * (-c0 * c2 * s1 + s0 * s2) - ); - - // Row 2, Column 3 - Sdot_23 = Scalar(0); - - // Row 3, Column 1 - Sdot_31 = -qdot0 * c1 * ( - dndoty_dqdot0 * radius_b * c0 - + dndotz_dqdot0 * radius_c * s0 - + radius_b * s0 * d_dndoty_dqdot0_dq0 - - radius_c * c0 * d_dndotz_dqdot0_dq0 - ) - + qdot1 * ( - -c1 * (radius_b * s0 * d_dndoty_dqdot0_dq1 - radius_c * c0 * d_dndotz_dqdot0_dq1) - + s1 * (dndoty_dqdot0 * radius_b * s0 - dndotz_dqdot0 * radius_c * c0) - ); - - // Row 3, Column 2 - Sdot_32 = -qdot0 * c1 * ( - dndoty_dqdot1 * radius_b * c0 - + dndotz_dqdot1 * radius_c * s0 - + radius_b * s0 * d_dndoty_dqdot0_dq1 - - radius_c * c0 * d_dndotz_dqdot0_dq1 - ) - + qdot1 * ( - dndotx_dqdot1 * radius_a * c1 - + dndoty_dqdot1 * radius_b * s0 * s1 - - dndotz_dqdot1 * radius_c * c0 * s1 - + radius_a * s1 * d_dndotx_dqdot1_dq1 - - radius_b * c1 * s0 * d_dndoty_dqdot1_dq1 - + radius_c * c0 * c1 * d_dndotz_dqdot1_dq1 - ); - - // Row 3, Column 3 - Sdot_33 = Scalar(0); - - // Angular part (rows 4-6) - Sdot_41 = -(qdot1 * c2 * s1 + qdot2 * c1 * s2); - Sdot_51 = qdot1 * s1 * s2 - qdot2 * c1 * c2; - Sdot_61 = qdot1 * c1; - - Sdot_42 = qdot2 * c2; - Sdot_52 = -qdot2 * s2; - Sdot_62 = Scalar(0); - - Sdot_43 = Scalar(0); - Sdot_53 = Scalar(0); - Sdot_63 = Scalar(0); - - data.Sdot.matrix() << Sdot_11, Sdot_12, Sdot_13, - Sdot_21, Sdot_22, Sdot_23, - Sdot_31, Sdot_32, Sdot_33, - Sdot_41, Sdot_42, Sdot_43, - Sdot_51, Sdot_52, Sdot_53, - Sdot_61, Sdot_62, Sdot_63; - } + void computeMotionSubspace( + const Scalar & s0, + const Scalar & c0, + const Scalar & s1, + const Scalar & c1, + const Scalar & s2, + const Scalar & c2, + JointDataDerived & data) const + { + // Common operations + const Scalar c1c2 = c1 * c2; + const Scalar c1s2 = c1 * s2; + + Scalar dndotx_dqdot1, dndoty_dqdot0, dndoty_dqdot1, dndotz_dqdot0, dndotz_dqdot1; + dndotx_dqdot1 = c1; + dndoty_dqdot0 = -c0 * c1; + dndoty_dqdot1 = s0 * s1; + dndotz_dqdot0 = -c1 * s0; + dndotz_dqdot1 = -c0 * s1; + + computeMotionSubspace( + s0, c0, s1, c1, s2, c2, dndotx_dqdot1, dndoty_dqdot0, dndoty_dqdot1, dndotz_dqdot0, + dndotz_dqdot1, data); + } + void computeMotionSubspace( + const Scalar & s0, + const Scalar & c0, + const Scalar & s1, + const Scalar & c1, + const Scalar & s2, + const Scalar & c2, + const Scalar & dndotx_dqdot1, + const Scalar & dndoty_dqdot0, + const Scalar & dndoty_dqdot1, + const Scalar & dndotz_dqdot0, + const Scalar & dndotz_dqdot1, + JointDataDerived & data) const + { + // Common operations + const Scalar c1c2 = c1 * c2; + const Scalar c1s2 = c1 * s2; + + Scalar S_11, S_21, S_31, S_12, S_22, S_32; + // S_11 = c1 * (-radius_b * c0 * (c0 * s2 + c2 * s0 * s1) - radius_c * s0 * (- c0 * c2 * s1 + + // s0 * s2)); + S_11 = dndoty_dqdot0 * radius_b * (c0 * s2 + c2 * s0 * s1) + + dndotz_dqdot0 * radius_c * (-c0 * c2 * s1 + s0 * s2); + // S_21 = c1 * (radius_b * c0 * (- c0 * c2 + s0 * s1 * s2) - radius_c * s0 * (c0 * s1 * s2 + + // c2 * s0)); + S_21 = -dndoty_dqdot0 * radius_b * (-c0 * c2 + s0 * s1 * s2) + + dndotz_dqdot0 * radius_c * (c0 * s1 * s2 + c2 * s0); + // S_31 = c0 * c1 * c1 * s0 * (radius_b - radius_c); + S_31 = c1 * (-dndoty_dqdot0 * radius_b * s0 + dndotz_dqdot0 * radius_c * c0); + + // S_12 = radius_a * c1c2 * c1 + radius_b * s0 * s1 * (c0 * s2 + c2 * s0 * s1) - radius_c * c0 + // * s1 * (- c0 * c2 * s1 + s0 * s2); + S_12 = dndotx_dqdot1 * radius_a * c1 * c2 + + dndoty_dqdot1 * radius_b * (c0 * s2 + c2 * s0 * s1) + + dndotz_dqdot1 * radius_c * (-c0 * c2 * s1 + s0 * s2); + // S_22 = - radius_a * c1s2 * c1 - radius_b * s0 * s1 * (- c0 * c2 + s0 * s1 * s2) - radius_c + // * c0 * s1 * (c0 * s1 * s2 + c2 * s0); + S_22 = -dndotx_dqdot1 * radius_a * c1 * s2 + - dndoty_dqdot1 * radius_b * (-c0 * c2 + s0 * s1 * s2) + + dndotz_dqdot1 * radius_c * (c0 * s1 * s2 + c2 * s0); + // S_32 = c1 * s1 * (radius_a - radius_b * s0 * s0 - radius_c * c0 * c0); + S_32 = dndotx_dqdot1 * radius_a * s1 - dndoty_dqdot1 * radius_b * c1 * s0 + + dndotz_dqdot1 * radius_c * c0 * c1; + + data.S.matrix() << S_11, S_12, Scalar(0), S_21, S_22, Scalar(0), S_31, S_32, Scalar(0), c1c2, + s2, Scalar(0), -c1s2, c2, Scalar(0), s1, Scalar(0), Scalar(1); + } + void computeMotionSubspaceDerivative( + const Scalar & s0, + const Scalar & c0, + const Scalar & s1, + const Scalar & c1, + const Scalar & s2, + const Scalar & c2, + const Scalar & dndotx_dqdot1, + const Scalar & dndoty_dqdot0, + const Scalar & dndoty_dqdot1, + const Scalar & dndotz_dqdot0, + const Scalar & dndotz_dqdot1, + JointDataDerived & data) const + { + // Compute Sdot for bias acceleration + Scalar qdot0, qdot1, qdot2; + qdot0 = data.joint_v(0); + qdot1 = data.joint_v(1); + qdot2 = data.joint_v(2); + + Scalar Sdot_11, Sdot_21, Sdot_31, Sdot_41, Sdot_51, Sdot_61; + Scalar Sdot_12, Sdot_22, Sdot_32, Sdot_42, Sdot_52, Sdot_62; + Scalar Sdot_13, Sdot_23, Sdot_33, Sdot_43, Sdot_53, Sdot_63; + + // Derivative of dndotXX_dqdot0 with respect to q0 and q1 + Scalar d_dndotx_dqdot1_dq1 = -s1; // dndotx_dqdot1 = c1; + + Scalar d_dndoty_dqdot0_dq0 = s0 * c1; // dndoty_dqdot0 = - c0 * c1; + Scalar d_dndoty_dqdot0_dq1 = c0 * s1; + + Scalar d_dndoty_dqdot1_dq0 = c0 * s1; // dndoty_dqdot1 = s0 * s1; + Scalar d_dndoty_dqdot1_dq1 = s0 * c1; + + Scalar d_dndotz_dqdot0_dq0 = -c1 * c0; // dndotz_dqdot0 = - c1 * s0; + Scalar d_dndotz_dqdot0_dq1 = s0 * s1; + + Scalar d_dndotz_dqdot1_dq0 = s0 * s1; // dndotz_dqdot1 = - c0 * s1; + Scalar d_dndotz_dqdot1_dq1 = -c0 * c1; + + // Upper part (translation) + // Row 1, Column 1 + Sdot_11 = + qdot0 + * (-dndoty_dqdot0 * radius_b * (-c0 * c2 * s1 + s0 * s2) + dndotz_dqdot0 * radius_c * (c0 * s2 + c2 * s0 * s1) + radius_b * (c0 * s2 + c2 * s0 * s1) * d_dndoty_dqdot0_dq0 + radius_c * (-c0 * c2 * s1 + s0 * s2) * d_dndotz_dqdot0_dq0) + + qdot1 + * (dndoty_dqdot0 * radius_b * c1 * c2 * s0 - dndotz_dqdot0 * radius_c * c0 * c1 * c2 + radius_b * (c0 * s2 + c2 * s0 * s1) * d_dndoty_dqdot0_dq1 + radius_c * (-c0 * c2 * s1 + s0 * s2) * d_dndotz_dqdot0_dq1) + - qdot2 + * (dndoty_dqdot0 * radius_b * (-c0 * c2 + s0 * s1 * s2) - dndotz_dqdot0 * radius_c * (c0 * s1 * s2 + c2 * s0)); + + // Row 1, Column 2 + Sdot_12 = + qdot0 + * (-dndoty_dqdot1 * radius_b * (-c0 * c2 * s1 + s0 * s2) + dndotz_dqdot1 * radius_c * (c0 * s2 + c2 * s0 * s1) + radius_b * (c0 * s2 + c2 * s0 * s1) * d_dndoty_dqdot0_dq1 + radius_c * (-c0 * c2 * s1 + s0 * s2) * d_dndotz_dqdot0_dq1) + + qdot1 * (-dndotx_dqdot1 * radius_a * c2 * s1 + dndoty_dqdot1 * radius_b * c1 * c2 * s0 - dndotz_dqdot1 * radius_c * c0 * c1 * c2 + radius_a * c1 * c2 * d_dndotx_dqdot1_dq1 + radius_b * (c0 * s2 + c2 * s0 * s1) * d_dndoty_dqdot1_dq1 + radius_c * (-c0 * c2 * s1 + s0 * s2) * d_dndotz_dqdot1_dq1) - qdot2 * (dndotx_dqdot1 * radius_a * c1 * s2 + dndoty_dqdot1 * radius_b * (-c0 * c2 + s0 * s1 * s2) - dndotz_dqdot1 * radius_c * (c0 * s1 * s2 + c2 * s0)); + + // Row 1, Column 3 + Sdot_13 = Scalar(0); + + // Row 2, Column 1 + Sdot_21 = + -qdot0 + * (dndoty_dqdot0 * radius_b * (c0 * s1 * s2 + c2 * s0) + dndotz_dqdot0 * radius_c * (-c0 * c2 + s0 * s1 * s2) + radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot0_dq0 - radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot0_dq0) + - qdot1 + * (dndoty_dqdot0 * radius_b * c1 * s0 * s2 - dndotz_dqdot0 * radius_c * c0 * c1 * s2 + radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot0_dq1 - radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot0_dq1) + - qdot2 + * (dndoty_dqdot0 * radius_b * (c0 * s2 + c2 * s0 * s1) + dndotz_dqdot0 * radius_c * (-c0 * c2 * s1 + s0 * s2)); + + // Row 2, Column 2 + Sdot_22 = + -qdot0 + * (dndoty_dqdot1 * radius_b * (c0 * s1 * s2 + c2 * s0) + dndotz_dqdot1 * radius_c * (-c0 * c2 + s0 * s1 * s2) + radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot0_dq1 - radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot0_dq1) + + qdot1 * (dndotx_dqdot1 * radius_a * s1 * s2 - dndoty_dqdot1 * radius_b * c1 * s0 * s2 + dndotz_dqdot1 * radius_c * c0 * c1 * s2 - radius_a * c1 * s2 * d_dndotx_dqdot1_dq1 - radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot1_dq1 + radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot1_dq1) - qdot2 * (dndotx_dqdot1 * radius_a * c1 * c2 + dndoty_dqdot1 * radius_b * (c0 * s2 + c2 * s0 * s1) + dndotz_dqdot1 * radius_c * (-c0 * c2 * s1 + s0 * s2)); + + // Row 2, Column 3 + Sdot_23 = Scalar(0); + + // Row 3, Column 1 + Sdot_31 = + -qdot0 * c1 + * (dndoty_dqdot0 * radius_b * c0 + dndotz_dqdot0 * radius_c * s0 + radius_b * s0 * d_dndoty_dqdot0_dq0 - radius_c * c0 * d_dndotz_dqdot0_dq0) + + qdot1 + * (-c1 * (radius_b * s0 * d_dndoty_dqdot0_dq1 - radius_c * c0 * d_dndotz_dqdot0_dq1) + s1 * (dndoty_dqdot0 * radius_b * s0 - dndotz_dqdot0 * radius_c * c0)); + + // Row 3, Column 2 + Sdot_32 = + -qdot0 * c1 + * (dndoty_dqdot1 * radius_b * c0 + dndotz_dqdot1 * radius_c * s0 + radius_b * s0 * d_dndoty_dqdot0_dq1 - radius_c * c0 * d_dndotz_dqdot0_dq1) + + qdot1 + * (dndotx_dqdot1 * radius_a * c1 + dndoty_dqdot1 * radius_b * s0 * s1 - dndotz_dqdot1 * radius_c * c0 * s1 + radius_a * s1 * d_dndotx_dqdot1_dq1 - radius_b * c1 * s0 * d_dndoty_dqdot1_dq1 + radius_c * c0 * c1 * d_dndotz_dqdot1_dq1); + + // Row 3, Column 3 + Sdot_33 = Scalar(0); + + // Angular part (rows 4-6) + Sdot_41 = -(qdot1 * c2 * s1 + qdot2 * c1 * s2); + Sdot_51 = qdot1 * s1 * s2 - qdot2 * c1 * c2; + Sdot_61 = qdot1 * c1; + + Sdot_42 = qdot2 * c2; + Sdot_52 = -qdot2 * s2; + Sdot_62 = Scalar(0); + + Sdot_43 = Scalar(0); + Sdot_53 = Scalar(0); + Sdot_63 = Scalar(0); + + data.Sdot.matrix() << Sdot_11, Sdot_12, Sdot_13, Sdot_21, Sdot_22, Sdot_23, Sdot_31, Sdot_32, + Sdot_33, Sdot_41, Sdot_42, Sdot_43, Sdot_51, Sdot_52, Sdot_53, Sdot_61, Sdot_62, Sdot_63; + } }; // struct JointModelEllipsoidTpl } // namespace pinocchio diff --git a/include/pinocchio/parsers/graph/graph-visitor.hpp b/include/pinocchio/parsers/graph/graph-visitor.hpp index 8c7520522d..b8d92e4860 100644 --- a/include/pinocchio/parsers/graph/graph-visitor.hpp +++ b/include/pinocchio/parsers/graph/graph-visitor.hpp @@ -113,6 +113,11 @@ namespace pinocchio return joint_calc(JointModelSphericalZYX()); } + SE3 operator()(const JointEllipsoid &) const + { + return joint_calc(JointModelEllipsoid()); + } + SE3 operator()(const JointPlanar &) const { return joint_calc(JointModelPlanar()); diff --git a/include/pinocchio/parsers/graph/joints.hpp b/include/pinocchio/parsers/graph/joints.hpp index d6dc9d2f54..1713ce3791 100644 --- a/include/pinocchio/parsers/graph/joints.hpp +++ b/include/pinocchio/parsers/graph/joints.hpp @@ -161,6 +161,30 @@ namespace pinocchio } }; + struct JointEllipsoid + { + double radius_a = 0.01; + double radius_b = 0.01; + double radius_c = 0.01; + + static constexpr int nq = 3; + static constexpr int nv = 3; + + JointEllipsoid() = default; + JointEllipsoid(const double r_a, const double r_b, const double r_c) + : radius_a(r_a) + , radius_b(r_b) + , radius_c(r_c) + { + } + + bool operator==(const JointEllipsoid & other) const + { + return radius_a == other.radius_a && radius_b == other.radius_b + && radius_c == other.radius_c; + } + }; + struct JointTranslation { static constexpr int nq = 3; @@ -241,6 +265,7 @@ namespace pinocchio JointFreeFlyer, JointSpherical, JointSphericalZYX, + JointEllipsoid, JointTranslation, JointPlanar, JointHelical, diff --git a/src/parsers/graph/model-graph-algo.cpp b/src/parsers/graph/model-graph-algo.cpp index e0f2ad7d1b..7550fae08b 100644 --- a/src/parsers/graph/model-graph-algo.cpp +++ b/src/parsers/graph/model-graph-algo.cpp @@ -90,6 +90,9 @@ namespace pinocchio // Joint Spherical ZYX typedef typename JointCollectionDefault::JointModelSphericalZYX JointModelSphericalZYX; + // Joint Ellipsoid + typedef typename JointCollectionDefault::JointModelEllipsoid JointModelEllipsoid; + // Joint Translation typedef typename JointCollectionDefault::JointModelTranslation JointModelTranslation; @@ -221,6 +224,10 @@ namespace pinocchio { return JointModelSphericalZYX(); } + ReturnType operator()(const JointEllipsoid & joint) const + { + return JointModelEllipsoid(joint.radius_a, joint.radius_b, joint.radius_c); + } ReturnType operator()(const JointUniversal & joint) const { return JointModelUniversal(joint.axis1, joint.axis2); @@ -338,6 +345,28 @@ namespace pinocchio model.addBodyFrame(target_vertex.name, j_id, body_pose); } + void operator()(const JointEllipsoid & joint, const BodyFrame & b_f) + { + if (!edge.forward) + PINOCCHIO_THROW_PRETTY( + std::invalid_argument, "Graph - JointEllipsoid cannot be reversed yet."); + + if (boost::get(&source_vertex.frame) == nullptr) + PINOCCHIO_THROW_PRETTY( + std::invalid_argument, "Graph - Invalid joint between a body and a non body frame."); + + const SE3 & joint_pose = edge.source_to_joint; + const SE3 & body_pose = edge.joint_to_target; + + const Frame previous_body = model.frames[model.getFrameId(source_vertex.name, BODY)]; + JointIndex j_id = model.addJoint( + previous_body.parentJoint, cjm(joint), previous_body.placement * joint_pose, edge.name); + + model.addJointFrame(j_id); + model.appendBodyToJoint(j_id, b_f.inertia); // check this + model.addBodyFrame(target_vertex.name, j_id, body_pose); + } + void operator()(const JointFixed & joint, const BodyFrame & b_f) { // Need to check what's vertex the edge is coming from. If it's a body, then we add diff --git a/src/parsers/graph/model-graph.cpp b/src/parsers/graph/model-graph.cpp index 02fbfdbb6f..e702a56c93 100644 --- a/src/parsers/graph/model-graph.cpp +++ b/src/parsers/graph/model-graph.cpp @@ -65,6 +65,10 @@ namespace pinocchio { return {JointSphericalZYX(), SE3::Identity()}; } + ReturnType operator()(const JointEllipsoid &) const + { + return {JointEllipsoid(), SE3::Identity()}; + } ReturnType operator()(const JointTranslation &) const { return {JointTranslation(), SE3::Identity()}; diff --git a/unittest/all-joints.cpp b/unittest/all-joints.cpp index fd53d9fd1d..eb6178bd37 100644 --- a/unittest/all-joints.cpp +++ b/unittest/all-joints.cpp @@ -160,10 +160,7 @@ struct init> static JointModel run() { JointModel jmodel( - static_cast(0.01), - static_cast(0.02), - static_cast(0.03) - ); + static_cast(0.01), static_cast(0.02), static_cast(0.03)); jmodel.setIndexes(0, 0, 0); return jmodel; diff --git a/unittest/finite-differences.cpp b/unittest/finite-differences.cpp index 07e0f4bce5..e5986659e2 100644 --- a/unittest/finite-differences.cpp +++ b/unittest/finite-differences.cpp @@ -170,10 +170,7 @@ struct init> static JointModel run() { - JointModel jmodel(Scalar(0.01), - Scalar(0.02), - Scalar(0.03) - ); + JointModel jmodel(Scalar(0.01), Scalar(0.02), Scalar(0.03)); jmodel.setIndexes(0, 0, 0); return jmodel; diff --git a/unittest/joint-ellipsoid.cpp b/unittest/joint-ellipsoid.cpp index 2f41b31880..c1424c673a 100644 --- a/unittest/joint-ellipsoid.cpp +++ b/unittest/joint-ellipsoid.cpp @@ -52,7 +52,7 @@ BOOST_AUTO_TEST_CASE(vsFreeFlyer) SE3 pos(1); pos.translation() = SE3::LinearType(1., 0., 0.); - addJointAndBody(modelEllipsoid, JointModelEllipsoid(1, 2,3), 0, pos, "ellipsoid", inertia); + addJointAndBody(modelEllipsoid, JointModelEllipsoid(1, 2, 3), 0, pos, "ellipsoid", inertia); addJointAndBody(modelFreeflyer, JointModelFreeFlyer(), 0, pos, "free-flyer", inertia); Data dataEllipsoid(modelEllipsoid); @@ -68,7 +68,7 @@ BOOST_AUTO_TEST_CASE(vsFreeFlyer) // ForwardDynamics == aba Eigen::VectorXd aAbaEllipsoid = - aba(modelEllipsoid, dataEllipsoid, q, v, tauEllipsoid, Convention::WORLD); + aba(modelEllipsoid, dataEllipsoid, q, v, tauEllipsoid, Convention::WORLD); // Calculer jdata.S().transpose() * data.f[i] } @@ -109,7 +109,7 @@ BOOST_AUTO_TEST_CASE(vsFreeFlyer) // v_zyx << 0.1, -0.3, 0.7; // forwardKinematics(modelSphericalZYX, dataSphericalZYX, q_zyx, v_zyx); // updateFramePlacements(modelSphericalZYX, dataSphericalZYX); - + // auto end_index = modelSphericalZYX.getFrameId("end_body", BODY); // auto X_end = dataSphericalZYX.oMf[end_index]; // const auto backward_build = @@ -132,7 +132,7 @@ BOOST_AUTO_TEST_CASE(vsFreeFlyer) // std::cout << "q_xyz (converted): " << q_xyz.transpose() << std::endl; // std::cout << "v_zyx (SphericalZYX): " << v_zyx.transpose() << std::endl; // std::cout << "v_xyz (converted): " << v_xyz.transpose() << std::endl; - + // Model modelEllipsoid; // addJointAndBody(modelEllipsoid, JointModelEllipsoid(0, 0, 0), 0, pos, "ellipsoid", inertia); // Data dataEllipsoid(modelEllipsoid); @@ -146,9 +146,9 @@ BOOST_AUTO_TEST_CASE(vsFreeFlyer) // std::cout << "oMi (Ellipsoid):\n" << dataEllipsoid.oMi[1] << std::endl; // std::cout << "oMi (SphericalZYX):\n" << dataSphericalZYX.oMi[1] << std::endl; // BOOST_CHECK(dataEllipsoid.oMi[1].isApprox(dataSphericalZYX.oMi[1])); - + // BOOST_CHECK(dataEllipsoid.liMi[1].isApprox(dataSphericalZYX.liMi[1])); - + // // Check that velocities match // BOOST_CHECK(dataEllipsoid.v[1].toVector().isApprox(dataSphericalZYX.v[1].toVector())); @@ -157,60 +157,64 @@ BOOST_AUTO_TEST_CASE(vsFreeFlyer) // computeAllTerms(modelSphericalZYX, dataSphericalZYX, q_zyx, v_zyx); // BOOST_CHECK(dataEllipsoid.Ycrb[1].matrix().isApprox(dataSphericalZYX.Ycrb[1].matrix())); - + // BOOST_CHECK(dataEllipsoid.f[1].toVector().isApprox(dataSphericalZYX.f[1].toVector())); - + // BOOST_CHECK(dataEllipsoid.nle.isApprox(dataSphericalZYX.nle)); - + // BOOST_CHECK(dataEllipsoid.com[0].isApprox(dataSphericalZYX.com[0])); // // Test inverse dynamics (RNEA) // Eigen::VectorXd aEllipsoid = Eigen::VectorXd::Ones(modelEllipsoid.nv); // Eigen::VectorXd aSphericalZYX = Eigen::VectorXd::Ones(modelSphericalZYX.nv); - + // Eigen::VectorXd tauEllipsoid = rnea(modelEllipsoid, dataEllipsoid, q_xyz, v_xyz, aEllipsoid); -// Eigen::VectorXd tauSphericalZYX = rnea(modelSphericalZYX, dataSphericalZYX, q_zyx, v_zyx, aSphericalZYX); - +// Eigen::VectorXd tauSphericalZYX = rnea(modelSphericalZYX, dataSphericalZYX, q_zyx, v_zyx, +// aSphericalZYX); + // BOOST_CHECK(tauEllipsoid.isApprox(tauSphericalZYX)); // // Test forward dynamics (ABA) // Eigen::VectorXd tau = Eigen::VectorXd::Ones(modelEllipsoid.nv); - -// Eigen::VectorXd aAbaEllipsoid = aba(modelEllipsoid, dataEllipsoid, q_xyz, v_xyz, tau, Convention::WORLD); -// Eigen::VectorXd aAbaSphericalZYX = aba(modelSphericalZYX, dataSphericalZYX, q_zyx, v_zyx, tau, Convention::WORLD); - + +// Eigen::VectorXd aAbaEllipsoid = aba(modelEllipsoid, dataEllipsoid, q_xyz, v_xyz, tau, +// Convention::WORLD); Eigen::VectorXd aAbaSphericalZYX = aba(modelSphericalZYX, dataSphericalZYX, +// q_zyx, v_zyx, tau, Convention::WORLD); + // BOOST_CHECK(aAbaEllipsoid.isApprox(aAbaSphericalZYX)); // // Test with LOCAL convention // aAbaEllipsoid = aba(modelEllipsoid, dataEllipsoid, q_xyz, v_xyz, tau, Convention::LOCAL); -// aAbaSphericalZYX = aba(modelSphericalZYX, dataSphericalZYX, q_zyx, v_zyx, tau, Convention::LOCAL); - +// aAbaSphericalZYX = aba(modelSphericalZYX, dataSphericalZYX, q_zyx, v_zyx, tau, +// Convention::LOCAL); + // BOOST_CHECK(aAbaEllipsoid.isApprox(aAbaSphericalZYX)); // // Test with different configurations // q_zyx << 0.2, -0.5, 1.1; // v_zyx << 0.3, 0.1, -0.2; - + // converter.convertConfigurationVector(q_zyx, q_xyz); // converter.convertTangentVector(q_zyx, v_zyx, v_xyz); - + // forwardKinematics(modelEllipsoid, dataEllipsoid, q_xyz, v_xyz); // forwardKinematics(modelSphericalZYX, dataSphericalZYX, q_zyx, v_zyx); // BOOST_CHECK(dataEllipsoid.oMi[1].isApprox(dataSphericalZYX.oMi[1])); - + // BOOST_CHECK(dataEllipsoid.v[1].toVector().isApprox(dataSphericalZYX.v[1].toVector())); // tauEllipsoid = rnea(modelEllipsoid, dataEllipsoid, q_xyz, v_xyz, aEllipsoid); // tauSphericalZYX = rnea(modelSphericalZYX, dataSphericalZYX, q_zyx, v_zyx, aSphericalZYX); - + // BOOST_CHECK(tauEllipsoid.isApprox(tauSphericalZYX)); // aAbaEllipsoid = aba(modelEllipsoid, dataEllipsoid, q_xyz, v_xyz, tau, Convention::WORLD); -// aAbaSphericalZYX = aba(modelSphericalZYX, dataSphericalZYX, q_zyx, v_zyx, tau, Convention::WORLD); - +// aAbaSphericalZYX = aba(modelSphericalZYX, dataSphericalZYX, q_zyx, v_zyx, tau, +// Convention::WORLD); + // BOOST_CHECK(aAbaEllipsoid.isApprox(aAbaSphericalZYX)); - + // } BOOST_AUTO_TEST_CASE(vsSphericalZYX) @@ -227,16 +231,17 @@ BOOST_AUTO_TEST_CASE(vsSphericalZYX) Model modelEllipsoid, modelSphericalZYX; addJointAndBody(modelEllipsoid, JointModelEllipsoid(0, 0, 0), 0, pos, "ellipsoid", inertia); addJointAndBody(modelSphericalZYX, JointModelSphericalZYX(), 0, pos, "spherical", inertia); - + Data dataEllipsoid(modelEllipsoid); Data dataSphericalZYX(modelSphericalZYX); // Start with ZYX angles Eigen::VectorXd q_s(3); - q_s << 0.5, 1.2, -0.8; // Z=0.5, Y=1.2, X=-0.8 + q_s << 0.5, 1.2, -0.8; // Z=0.5, Y=1.2, X=-0.8 Eigen::VectorXd qd_s(3); - qd_s << 0.1, -0.3, 0.7; // ZYX angle velocities - + qd_s << 0.1, -0.3, 0.7; // ZYX angle velocities + Eigen::VectorXd qdotdot_s(3); + qdotdot_s << 0.2, 0.1, -0.1; // ZYX angle accelerations // Compute the rotation matrix from ZYX angles forwardKinematics(modelSphericalZYX, dataSphericalZYX, q_s, qd_s); const Matrix3 & R = dataSphericalZYX.oMi[1].rotation(); @@ -244,119 +249,130 @@ BOOST_AUTO_TEST_CASE(vsSphericalZYX) std::cout << "\n=== Target Rotation Matrix (from ZYX) ===" << std::endl; std::cout << R << std::endl; - + // Extract XYZ Euler angles from the rotation matrix // For XYZ convention: R = Rx(x) * Ry(y) * Rz(z) // We need to solve for x, y, z - + Eigen::Vector3d q_e; - + // From the Ellipsoid rotation matrix structure: // R(0,2) = sin(y) // R(1,2) = -sin(x)*cos(y) // R(2,2) = cos(x)*cos(y) - + double sy = R(0, 2); - q_e(1) = std::asin(sy); // y angle - + q_e(1) = std::asin(sy); // y angle + double cy = std::cos(q_e(1)); - - if (std::abs(cy) > 1e-6) { + + if (std::abs(cy) > 1e-6) + { // Not at singularity - q_e(0) = std::atan2(-R(1, 2) / cy, R(2, 2) / cy); // x angle - q_e(2) = std::atan2(-R(0, 1) / cy, R(0, 0) / cy); // z angle - } else { + q_e(0) = std::atan2(-R(1, 2) / cy, R(2, 2) / cy); // x angle + q_e(2) = std::atan2(-R(0, 1) / cy, R(0, 0) / cy); // z angle + } + else + { // Gimbal lock - choose x = 0 q_e(0) = 0.0; q_e(2) = std::atan2(R(1, 0), R(1, 1)); } - - std::cout << "\n=== Configuration ===" << std::endl; - std::cout << "q_s (Z,Y,X): " << q_s.transpose() << std::endl; - std::cout << "q_e (X,Y,Z): " << q_e.transpose() << std::endl; // Get the motion subspace matrices (which give us the Jacobians) JointModelSphericalZYX jmodel_s; + jmodel_s.setIndexes(0, 0, 0); + JointDataSphericalZYX jdata_s = jmodel_s.createData(); jmodel_s.calc(jdata_s, q_s); - + JointModelEllipsoid jmodel_e(0, 0, 0); + jmodel_e.setIndexes(0, 0, 0); + JointDataEllipsoid jdata_e = jmodel_e.createData(); jmodel_e.calc(jdata_e, q_e); - // The motion subspace S gives us: omega = S * v - Matrix3 S_s = jdata_s.S.matrix().bottomRows<3>(); // Angular part - Matrix3 S_e = jdata_e.S.matrix().bottomRows<3>(); // Angular part - + Matrix3 S_s = jdata_s.S.matrix().bottomRows<3>(); // Angular part + Matrix3 S_e = jdata_e.S.matrix().bottomRows<3>(); // Angular part + Eigen::Vector3d qd_e = S_e.inverse() * S_s * qd_s; Eigen::Vector3d w_s = S_s * qd_s; Eigen::Vector3d w_e = S_e * qd_e; - - - std::cout << "\n=== Joint Velocities ===" << std::endl; - std::cout << "qd_s (joint velocities): " << qd_s.transpose() << std::endl; - std::cout << "qd_e (joint velocities): " << qd_e.transpose() << std::endl; - - std::cout << "\n=== Motion Subspace Matrices ===" << std::endl; - std::cout << "S_zyx (full 6x3):\n" << jdata_s.S.matrix() << std::endl; - std::cout << "\nS_xyz (full 6x3):\n" << jdata_e.S.matrix() << std::endl; - - std::cout << "\n=== Angular Velocities from Joint Velocities ===" << std::endl; - std::cout << "omega from ZYX (S_s * qd_s): " << w_s.transpose() << std::endl; - std::cout << "omega from XYZ (S_e * qd_e): " << w_e.transpose() << std::endl; - + BOOST_CHECK(w_s.isApprox(w_e)); std::cout << "✓ Angular velocities from joint velocities match" << std::endl; // Compute forward kinematics with the converted configurations forwardKinematics(modelEllipsoid, dataEllipsoid, q_e, qd_e); - - // Also get the joint data to see body-frame velocities - JointDataEllipsoid jdata_e_fk = jmodel_e.createData(); - JointDataEllipsoid jdata_e_fk2 = jmodel_e.createData(); - jmodel_e.calc(jdata_e_fk, q_e, qd_e); - jmodel_e.calc(jdata_e_fk2, q_e); - - std::cout << "\n=== Motion Subspace Matrices after FK ===" << std::endl; - std::cout << "S_e calc q:\n" << jdata_e_fk.S.matrix() << std::endl; - std::cout << "\nS_e calc q v:\n" << jdata_e_fk2.S.matrix() << std::endl; - - BOOST_CHECK(jdata_e_fk.S.matrix().isApprox(jdata_e_fk2.S.matrix())); - std::cout << "✓ Motion subspace matrices match" << std::endl; - - JointDataSphericalZYX jdata_s_fk = jmodel_s.createData(); - jmodel_s.calc(jdata_s_fk, q_s, qd_s); - + + // Getting S with q_e from the three calcs + JointDataEllipsoid jDataEllipsoidFK = jmodel_e.createData(); + JointDataEllipsoid jDataEllipsoidFK2 = jmodel_e.createData(); + JointDataEllipsoid jDataEllipsoidFK3 = jmodel_e.createData(); + + jmodel_e.calc(jDataEllipsoidFK, q_e, qd_e); + jmodel_e.calc(jDataEllipsoidFK2, q_e); + jmodel_e.calc(jDataEllipsoidFK3, q_e); + jmodel_e.calc(jDataEllipsoidFK3, Blank(), qd_e); + + BOOST_CHECK(jDataEllipsoidFK.S.matrix().isApprox(jDataEllipsoidFK2.S.matrix())); + BOOST_CHECK(jDataEllipsoidFK.S.matrix().isApprox(jDataEllipsoidFK3.S.matrix())); + std::cout << "✓ Motion subspace matrices match for the three calc(...) methods" << std::endl; + + JointDataSphericalZYX jDataSphereFK = jmodel_s.createData(); + jmodel_s.calc(jDataSphereFK, q_s, qd_s); + std::cout << "\n=== Joint-frame velocities (S * v) ===" << std::endl; - Eigen::Matrix joint_vel_e = jdata_e_fk.S.matrix() * qd_e; - std::cout << "joint_vel_e: " << joint_vel_e.transpose() << std::endl; - std::cout << "jdata_e_fk.v : " << jdata_e_fk.v.toVector().transpose() << std::endl; - - - - Eigen::Matrix joint_vel_s = jdata_s_fk.S.matrix() * qd_s; - std::cout << "Ellipsoid (S_xyz * qd_e): " << joint_vel_e.transpose() << std::endl; - std::cout << "SphericalZYX (S_zyx * qd_s): " << joint_vel_s.transpose() << std::endl; - - std::cout << "\n=== Rotation Matrices ===" << std::endl; - std::cout << "Ellipsoid rotation:\n" << dataEllipsoid.oMi[1].rotation() << std::endl; - std::cout << "\nSphericalZYX rotation:\n" << dataSphericalZYX.oMi[1].rotation() << std::endl; - - std::cout << "\n=== Translations ===" << std::endl; - std::cout << "Ellipsoid translation: " << dataEllipsoid.oMi[1].translation().transpose() << std::endl; - std::cout << "SphericalZYX translation: " << dataSphericalZYX.oMi[1].translation().transpose() << std::endl; - - std::cout << "\n=== Spatial Velocities ===" << std::endl; - std::cout << "Ellipsoid v:\n" << dataEllipsoid.v[1].toVector().transpose() << std::endl; - std::cout << "SphericalZYX v:\n" << dataSphericalZYX.v[1].toVector().transpose() << std::endl; + Eigen::Matrix joint_vel_e = jDataEllipsoidFK.S.matrix() * qd_e; + Eigen::Matrix manual_vel = jDataEllipsoidFK.S.matrix() * jDataEllipsoidFK.joint_v; + Eigen::Matrix joint_vel_s = jDataSphereFK.S.matrix() * qd_s; BOOST_CHECK(dataEllipsoid.v[1].toVector().isApprox(dataSphericalZYX.v[1].toVector())); std::cout << "✓ Spatial velocities match" << std::endl; - + BOOST_CHECK(dataEllipsoid.oMi[1].isApprox(dataSphericalZYX.oMi[1])); std::cout << "✓ Full oMi[1] matches" << std::endl; + + Matrix3 Sdot_e = jDataEllipsoidFK.Sdot.matrix().bottomRows<3>(); // Angular part + + // The acceleration conversion formula: wdot_s = wdot_e + // S_s * qdotdot_s + c_s.angular() = Sdot_e * qd_e + S_e * qdotdot_e + // Solving for qdotdot_e: + // S_e * qdotdot_e = c_s.angular()+ S_s * qdotdot_s - Sdot_e * qd_e + Eigen::Vector3d qdotdot_e = + S_e.inverse() * (S_s * qdotdot_s + jDataSphereFK.c.angular() - Sdot_e * qd_e); + + // Verify angular accelerations match + Eigen::Vector3d wdot_s = jDataSphereFK.c.angular() + S_s * qdotdot_s; + Eigen::Vector3d wdot_e = Sdot_e * qd_e + S_e * qdotdot_e; + BOOST_CHECK(wdot_s.isApprox(wdot_e)); + std::cout << "✓ Angular accelerations from joint accelerations match" << std::endl; + + forwardKinematics(modelEllipsoid, dataEllipsoid, q_e, qd_e, qdotdot_e); + forwardKinematics(modelSphericalZYX, dataSphericalZYX, q_s, qd_s, qdotdot_s); + + BOOST_CHECK(dataEllipsoid.a[1].toVector().isApprox(dataSphericalZYX.a[1].toVector())); + + // Test RNEA (Recursive Newton-Euler Algorithm) + std::cout << "\n=== RNEA Test ===" << std::endl; + + rnea(modelEllipsoid, dataEllipsoid, q_e, qd_e, qdotdot_e); + rnea(modelSphericalZYX, dataSphericalZYX, q_s, qd_s, qdotdot_s); + + BOOST_CHECK(dataEllipsoid.f[1].isApprox(dataSphericalZYX.f[1])); + std::cout << "✓ RNEA f match" << std::endl; + + // Test ABA (Articulated-Body Algorithm) + std::cout << "\n=== ABA Test ===" << std::endl; + Eigen::VectorXd tau = Eigen::VectorXd::Ones(modelEllipsoid.nv); + + Eigen::VectorXd aAbaEllipsoid = + aba(modelEllipsoid, dataEllipsoid, q_e, qd_e, dataEllipsoid.tau, Convention::WORLD); + + BOOST_CHECK(dataEllipsoid.ddq.isApprox(qdotdot_e)); + std::cout << "✓ ABA computed accelerations match input accelerations" << std::endl; } BOOST_AUTO_TEST_SUITE_END() diff --git a/unittest/joint-generic.cpp b/unittest/joint-generic.cpp index 6db8bc18b3..9658d94be0 100644 --- a/unittest/joint-generic.cpp +++ b/unittest/joint-generic.cpp @@ -271,10 +271,7 @@ struct init> static JointModel run() { - JointModel jmodel(Scalar(0.01), - Scalar(0.02), - Scalar(0.03) - ); + JointModel jmodel(Scalar(0.01), Scalar(0.02), Scalar(0.03)); jmodel.setIndexes(0, 0, 0); return jmodel; diff --git a/unittest/model-graph.cpp b/unittest/model-graph.cpp index ddf67bedc5..2f1776b7a7 100644 --- a/unittest/model-graph.cpp +++ b/unittest/model-graph.cpp @@ -617,6 +617,66 @@ BOOST_AUTO_TEST_CASE(test_reverse_spherical_zyx) d_f.oMf[m_forward.getFrameId("body1", pinocchio::BODY)])); } +/// @brief test if reversing of a composite joint is correct. +BOOST_AUTO_TEST_CASE(test_reverse_ellipsoid) +{ + using namespace pinocchio::graph; + + ModelGraph g = buildReversableModelGraph(JointEllipsoid(0.01, 0.01, 0.01)); + ///////////////// Model + BOOST_CHECK_THROW(buildModel(g, "body2", pinocchio::SE3::Identity()), std::invalid_argument); + + //////////////////////////////////// Forward model + pinocchio::Model m_forward = buildModel(g, "body1", pinocchio::SE3::Identity()); + pinocchio::Data d_f(m_forward); + // config vector forward model Ellipsoid + Eigen::Vector3d q(m_forward.nq); + q << M_PI / 4, M_PI, M_PI / 2; + pinocchio::framesForwardKinematics(m_forward, d_f, q); + + // Create a standalone model with the EXACT same structure as the graph model + pinocchio::Model modelEllipsoid; + pinocchio::SE3 poseBody1 = + pinocchio::SE3(Eigen::Matrix3d::Identity(), Eigen::Vector3d(2., 0., 0.)); + pinocchio::SE3 poseBody2 = + pinocchio::SE3(Eigen::Matrix3d::Identity(), Eigen::Vector3d(0., 3., 0.)); + pinocchio::Model::JointIndex idx; + + pinocchio::JointModelEllipsoid jModelEllipsoid(0.01, 0.01, 0.01); + idx = modelEllipsoid.addJoint(0, jModelEllipsoid, poseBody1, "ellipsoid"); + std::cout << "Added joint ellipsoid with index: " << idx << std::endl; + + pinocchio::Inertia body2_inertia = Inertia::Identity(); + modelEllipsoid.appendBodyToJoint(idx, body2_inertia, poseBody2); + // pinocchio::FrameIndex frame_id = modelEllipsoid.addBodyFrame("body2", -1, poseBody2); + + // This line is not working yet. + // pinocchio::FrameIndex frame_id = modelEllipsoid.addBodyFrame("body2", idx, poseBody2); + + // pinocchio::Data jDataEllipsoid(modelEllipsoid); + // pinocchio::framesForwardKinematics(modelEllipsoid, jDataEllipsoid, q); + + // DEBUG: Print both transforms + std::cout << "nb frames in new model: " << modelEllipsoid.nframes << std::endl; + // std::cout << "=== DEBUG test_reverse_ellipsoid ===" << std::endl; + // std::cout << "body2_graph translation:\n" << body2_graph.translation().transpose() << + // std::endl; std::cout << "body2_graph rotation:\n" << body2_graph.rotation() << std::endl; + // std::cout << "\nbody2_standalone translation:\n" << body2_standalone.translation().transpose() + // << std::endl; std::cout << "body2_standalone rotation:\n" << body2_standalone.rotation() << + // std::endl; std::cout << "\nTranslation difference:\n" << (body2_graph.translation() - + // body2_standalone.translation()).transpose() << std::endl; std::cout << "Translation difference + // norm: " << (body2_graph.translation() - body2_standalone.translation()).norm() << std::endl; + // std::cout << "Rotation difference (Frobenius norm): " + // << (body2_graph.rotation() - body2_standalone.rotation()).norm() << std::endl; + + // Compare the absolute placement of body2 in both models + // pinocchio::SE3 body2_graph = d_f.oMf[m_forward.getFrameId("body2", pinocchio::BODY)]; + // pinocchio::SE3 body2_standalone = + // jDataEllipsoid.oMf[modelEllipsoid.getFrameId("body2", pinocchio::BODY)]; + + // BOOST_CHECK(body2_graph.isApprox(body2_standalone)); +} + /// @brief test if reversing of a composite joint is correct. BOOST_AUTO_TEST_CASE(test_reverse_composite) { diff --git a/unittest/serialization.cpp b/unittest/serialization.cpp index 2bb1d24509..3a665e54b9 100644 --- a/unittest/serialization.cpp +++ b/unittest/serialization.cpp @@ -357,9 +357,7 @@ struct init> static JointModel run() { JointModel jmodel( - static_cast(0.01), - static_cast(0.02), - static_cast(0.03)); + static_cast(0.01), static_cast(0.02), static_cast(0.03)); jmodel.setIndexes(0, 0, 0); return jmodel; From 44a57e3e84f3fda567863caee1958726681b1675 Mon Sep 17 00:00:00 2001 From: ipuch Date: Wed, 29 Oct 2025 14:25:05 +0100 Subject: [PATCH 04/48] feat(joint-ellipsoid): extra utils for translations position, velocities, accelerations --- .../multibody/joint/joint-ellipsoid.hpp | 91 +++++++++++++++++++ 1 file changed, 91 insertions(+) diff --git a/include/pinocchio/multibody/joint/joint-ellipsoid.hpp b/include/pinocchio/multibody/joint/joint-ellipsoid.hpp index 4189633290..3c71d4d983 100644 --- a/include/pinocchio/multibody/joint/joint-ellipsoid.hpp +++ b/include/pinocchio/multibody/joint/joint-ellipsoid.hpp @@ -129,6 +129,9 @@ namespace pinocchio using Base::idx_v; using Base::idx_vExtended; using Base::setIndexes; + + typedef typename Transformation_t::Vector3 Vector3; + Scalar radius_a; Scalar radius_b; Scalar radius_c; @@ -321,6 +324,94 @@ namespace pinocchio return res; } + template + Vector3 computeTranslations( + const typename Eigen::MatrixBase & qs) const + { + Scalar c0, s0; + SINCOS(qs(0), &s0, &c0); + Scalar c1, s1; + SINCOS(qs(1), &s1, &c1); + + return computeTranslations(s0, c0, s1, c1); + } + + Vector3 computeTranslations( + const Scalar & s0, + const Scalar & c0, + const Scalar & s1, + const Scalar & c1) const + { + Scalar nx, ny, nz; + nx = s1; + ny = -s0 * c1; + nz = c0 * c1; + + return Vector3(radius_a * nx, radius_b * ny, radius_c * nz); + } + + template + Vector3 computeTranslationVelocities( + const typename Eigen::MatrixBase & qs, + const typename Eigen::MatrixBase & vs) const + { + Scalar c0, s0; + SINCOS(qs(0), &s0, &c0); + Scalar c1, s1; + SINCOS(qs(1), &s1, &c1); + + return computeTranslationVelocities(s0, c0, s1, c1, vs(0), vs(1)); + } + + Vector3 computeTranslationVelocities( + const Scalar & s0, + const Scalar & c0, + const Scalar & s1, + const Scalar & c1, + const Scalar & q0dot, + const Scalar & q1dot) const + { + Vector3 v; + v(0) = radius_a * c1 * q1dot; + v(1) = radius_b * (- c0 * c1 * q0dot + s0 * s1 * q1dot); + v(2) = radius_c * (-s0 * c1 * q0dot - c0 * s1 * q1dot); + return v; + } + + template + Vector3 computeTranslationAccelerations( + const typename Eigen::MatrixBase & qs, + const typename Eigen::MatrixBase & vs, + const typename Eigen::MatrixBase & as) const + { + Vector3 a; + Scalar c0, s0; + SINCOS(qs(0), &s0, &c0); + Scalar c1, s1; + SINCOS(qs(1), &s1, &c1); + return computeTranslationAccelerations( + s0, c0, s1, c1, vs(0), vs(1), as(0), as(1)); + } + + Vector3 computeTranslationAccelerations( + const Scalar & s0, + const Scalar & c0, + const Scalar & s1, + const Scalar & c1, + const Scalar & q0dot, + const Scalar & q1dot, + const Scalar & q0ddot, + const Scalar & q1ddot) const + { + Vector3 a; + a(0) = radius_a * (-s1 * q1dot * q1dot + c1 * q1ddot); + a(1) = radius_b * (s0 * c1 * q0dot * q0dot + c0 * s1 * q0dot * q1dot - c0 * c1 * q0ddot + + c0 * s1 * q1dot * q0dot + s0 * c1 * q1dot * q1dot + s0 * s1 * q1ddot); + a(2) = radius_c * (-c0 * c1 * q0dot * q0dot + s0 * s1 * q0dot * q1dot - s0 * c1 * q0ddot + + s0 * s1 * q1dot * q0dot - c0 * c1 * q1dot * q1dot - c0 * s1 * q1ddot); + return a; + } + void computeMotionSubspace( const Scalar & s0, const Scalar & c0, From ed8ed8a9ade14437f02b7ac8e6e3f530bbd95866 Mon Sep 17 00:00:00 2001 From: ipuch Date: Wed, 29 Oct 2025 14:26:28 +0100 Subject: [PATCH 05/48] tests(joint-ellipsoid): add composite test delete extra commented test linting(pre-comit) tests(ellipsoid): enough core testing to me. --- .../multibody/joint/joint-ellipsoid.hpp | 171 +++++----- unittest/joint-ellipsoid.cpp | 310 +++++++----------- 2 files changed, 197 insertions(+), 284 deletions(-) diff --git a/include/pinocchio/multibody/joint/joint-ellipsoid.hpp b/include/pinocchio/multibody/joint/joint-ellipsoid.hpp index 3c71d4d983..7747267679 100644 --- a/include/pinocchio/multibody/joint/joint-ellipsoid.hpp +++ b/include/pinocchio/multibody/joint/joint-ellipsoid.hpp @@ -324,93 +324,90 @@ namespace pinocchio return res; } - template - Vector3 computeTranslations( - const typename Eigen::MatrixBase & qs) const - { - Scalar c0, s0; - SINCOS(qs(0), &s0, &c0); - Scalar c1, s1; - SINCOS(qs(1), &s1, &c1); - - return computeTranslations(s0, c0, s1, c1); - } - - Vector3 computeTranslations( - const Scalar & s0, - const Scalar & c0, - const Scalar & s1, - const Scalar & c1) const - { - Scalar nx, ny, nz; - nx = s1; - ny = -s0 * c1; - nz = c0 * c1; - - return Vector3(radius_a * nx, radius_b * ny, radius_c * nz); - } - - template - Vector3 computeTranslationVelocities( - const typename Eigen::MatrixBase & qs, - const typename Eigen::MatrixBase & vs) const - { - Scalar c0, s0; - SINCOS(qs(0), &s0, &c0); - Scalar c1, s1; - SINCOS(qs(1), &s1, &c1); - - return computeTranslationVelocities(s0, c0, s1, c1, vs(0), vs(1)); - } - - Vector3 computeTranslationVelocities( - const Scalar & s0, - const Scalar & c0, - const Scalar & s1, - const Scalar & c1, - const Scalar & q0dot, - const Scalar & q1dot) const - { - Vector3 v; - v(0) = radius_a * c1 * q1dot; - v(1) = radius_b * (- c0 * c1 * q0dot + s0 * s1 * q1dot); - v(2) = radius_c * (-s0 * c1 * q0dot - c0 * s1 * q1dot); - return v; - } - - template - Vector3 computeTranslationAccelerations( - const typename Eigen::MatrixBase & qs, - const typename Eigen::MatrixBase & vs, - const typename Eigen::MatrixBase & as) const - { - Vector3 a; - Scalar c0, s0; - SINCOS(qs(0), &s0, &c0); - Scalar c1, s1; - SINCOS(qs(1), &s1, &c1); - return computeTranslationAccelerations( - s0, c0, s1, c1, vs(0), vs(1), as(0), as(1)); - } - - Vector3 computeTranslationAccelerations( - const Scalar & s0, - const Scalar & c0, - const Scalar & s1, - const Scalar & c1, - const Scalar & q0dot, - const Scalar & q1dot, - const Scalar & q0ddot, - const Scalar & q1ddot) const - { - Vector3 a; - a(0) = radius_a * (-s1 * q1dot * q1dot + c1 * q1ddot); - a(1) = radius_b * (s0 * c1 * q0dot * q0dot + c0 * s1 * q0dot * q1dot - c0 * c1 * q0ddot - + c0 * s1 * q1dot * q0dot + s0 * c1 * q1dot * q1dot + s0 * s1 * q1ddot); - a(2) = radius_c * (-c0 * c1 * q0dot * q0dot + s0 * s1 * q0dot * q1dot - s0 * c1 * q0ddot - + s0 * s1 * q1dot * q0dot - c0 * c1 * q1dot * q1dot - c0 * s1 * q1ddot); - return a; - } + template + Vector3 computeTranslations(const typename Eigen::MatrixBase & qs) const + { + Scalar c0, s0; + SINCOS(qs(0), &s0, &c0); + Scalar c1, s1; + SINCOS(qs(1), &s1, &c1); + + return computeTranslations(s0, c0, s1, c1); + } + + Vector3 computeTranslations( + const Scalar & s0, const Scalar & c0, const Scalar & s1, const Scalar & c1) const + { + Scalar nx, ny, nz; + nx = s1; + ny = -s0 * c1; + nz = c0 * c1; + + return Vector3(radius_a * nx, radius_b * ny, radius_c * nz); + } + + template + Vector3 computeTranslationVelocities( + const typename Eigen::MatrixBase & qs, + const typename Eigen::MatrixBase & vs) const + { + Scalar c0, s0; + SINCOS(qs(0), &s0, &c0); + Scalar c1, s1; + SINCOS(qs(1), &s1, &c1); + + return computeTranslationVelocities(s0, c0, s1, c1, vs(0), vs(1)); + } + + Vector3 computeTranslationVelocities( + const Scalar & s0, + const Scalar & c0, + const Scalar & s1, + const Scalar & c1, + const Scalar & q0dot, + const Scalar & q1dot) const + { + Vector3 v; + v(0) = radius_a * c1 * q1dot; + v(1) = radius_b * (-c0 * c1 * q0dot + s0 * s1 * q1dot); + v(2) = radius_c * (-s0 * c1 * q0dot - c0 * s1 * q1dot); + return v; + } + + template + Vector3 computeTranslationAccelerations( + const typename Eigen::MatrixBase & qs, + const typename Eigen::MatrixBase & vs, + const typename Eigen::MatrixBase & as) const + { + Vector3 a; + Scalar c0, s0; + SINCOS(qs(0), &s0, &c0); + Scalar c1, s1; + SINCOS(qs(1), &s1, &c1); + return computeTranslationAccelerations(s0, c0, s1, c1, vs(0), vs(1), as(0), as(1)); + } + + Vector3 computeTranslationAccelerations( + const Scalar & s0, + const Scalar & c0, + const Scalar & s1, + const Scalar & c1, + const Scalar & q0dot, + const Scalar & q1dot, + const Scalar & q0ddot, + const Scalar & q1ddot) const + { + Vector3 a; + a(0) = radius_a * (-s1 * q1dot * q1dot + c1 * q1ddot); + a(1) = + radius_b + * (s0 * c1 * q0dot * q0dot + c0 * s1 * q0dot * q1dot - c0 * c1 * q0ddot + c0 * s1 * q1dot * q0dot + s0 * c1 * q1dot * q1dot + s0 * s1 * q1ddot); + a(2) = + radius_c + * (-c0 * c1 * q0dot * q0dot + s0 * s1 * q0dot * q1dot - s0 * c1 * q0ddot + s0 * s1 * q1dot * q0dot - c0 * c1 * q1dot * q1dot - c0 * s1 * q1ddot); + return a; + } void computeMotionSubspace( const Scalar & s0, diff --git a/unittest/joint-ellipsoid.cpp b/unittest/joint-ellipsoid.cpp index c1424c673a..ec323509bc 100644 --- a/unittest/joint-ellipsoid.cpp +++ b/unittest/joint-ellipsoid.cpp @@ -38,185 +38,6 @@ void addJointAndBody( BOOST_AUTO_TEST_SUITE(JointEllipsoid) -BOOST_AUTO_TEST_CASE(vsFreeFlyer) -{ - using namespace pinocchio; - typedef SE3::Vector3 Vector3; - typedef Eigen::Matrix Vector6; - typedef Eigen::Matrix VectorFF; - typedef SE3::Matrix3 Matrix3; - - Model modelEllipsoid, modelFreeflyer; - - Inertia inertia(1., Vector3(0.5, 0., 0.0), Matrix3::Identity()); - SE3 pos(1); - pos.translation() = SE3::LinearType(1., 0., 0.); - - addJointAndBody(modelEllipsoid, JointModelEllipsoid(1, 2, 3), 0, pos, "ellipsoid", inertia); - addJointAndBody(modelFreeflyer, JointModelFreeFlyer(), 0, pos, "free-flyer", inertia); - - Data dataEllipsoid(modelEllipsoid); - - Eigen::VectorXd q = Eigen::VectorXd::Ones(modelEllipsoid.nq); - Eigen::VectorXd v = Eigen::VectorXd::Ones(modelEllipsoid.nv); - - forwardKinematics(modelEllipsoid, dataEllipsoid, q, v); - - Eigen::VectorXd tauEllipsoid = Eigen::VectorXd::Ones(modelEllipsoid.nv); - - Eigen::VectorXd aEllipsoid = Eigen::VectorXd::Ones(modelEllipsoid.nv); - - // ForwardDynamics == aba - Eigen::VectorXd aAbaEllipsoid = - aba(modelEllipsoid, dataEllipsoid, q, v, tauEllipsoid, Convention::WORLD); - - // Calculer jdata.S().transpose() * data.f[i] -} - -// BOOST_AUTO_TEST_CASE(vsSphericalZYX) -// { -// using namespace pinocchio; -// typedef SE3::Vector3 Vector3; -// typedef Eigen::Matrix Vector6; -// typedef SE3::Matrix3 Matrix3; - -// // Build models using ModelGraph to enable configuration conversion -// graph::ModelGraph g; -// Inertia inertia(1., Vector3(0.5, 0., 0.0), Matrix3::Identity()); -// SE3 pos(1); -// pos.translation() = SE3::LinearType(1., 0., 0.); - -// g.addBody("root_body", inertia); -// g.addBody("end_body", inertia); - -// // Create SphericalZYX joint (uses ZYX Euler angles) -// g.addJoint( -// "spherical_joint", -// graph::JointSphericalZYX(), -// "root_body", -// SE3::Identity(), -// "end_body", -// pos); - -// // Build SphericalZYX model -// const auto forward_build = graph::buildModelWithBuildInfo(g, "root_body", SE3::Identity()); -// const Model & modelSphericalZYX = forward_build.model; -// Data dataSphericalZYX(modelSphericalZYX); - -// Eigen::VectorXd q_zyx(3); -// q_zyx << 0.5, 1.2, -0.8; -// Eigen::VectorXd v_zyx(3); -// v_zyx << 0.1, -0.3, 0.7; -// forwardKinematics(modelSphericalZYX, dataSphericalZYX, q_zyx, v_zyx); -// updateFramePlacements(modelSphericalZYX, dataSphericalZYX); - -// auto end_index = modelSphericalZYX.getFrameId("end_body", BODY); -// auto X_end = dataSphericalZYX.oMf[end_index]; -// const auto backward_build = -// graph::buildModelWithBuildInfo(g, "end_body", X_end); - -// const Model & XYZlacirehpSledom = backward_build.model; - -// // Create converter from SphericalZYX to Ellipsoid -// auto converter = graph::createConverter( -// modelSphericalZYX, XYZlacirehpSledom, forward_build.build_info, backward_build.build_info); - -// // Convert to Ellipsoid configuration -// Eigen::VectorXd q_xyz = Eigen::VectorXd::Zero(XYZlacirehpSledom.nq); -// Eigen::VectorXd v_xyz = Eigen::VectorXd::Zero(XYZlacirehpSledom.nv); -// converter.convertConfigurationVector(q_zyx, q_xyz); -// converter.convertTangentVector(q_zyx, v_zyx, v_xyz); - -// std::cout << "\n=== Configuration Conversion ===" << std::endl; -// std::cout << "q_zyx (SphericalZYX): " << q_zyx.transpose() << std::endl; -// std::cout << "q_xyz (converted): " << q_xyz.transpose() << std::endl; -// std::cout << "v_zyx (SphericalZYX): " << v_zyx.transpose() << std::endl; -// std::cout << "v_xyz (converted): " << v_xyz.transpose() << std::endl; - -// Model modelEllipsoid; -// addJointAndBody(modelEllipsoid, JointModelEllipsoid(0, 0, 0), 0, pos, "ellipsoid", inertia); -// Data dataEllipsoid(modelEllipsoid); - -// // Test forward kinematics with converted configurations -// forwardKinematics(modelEllipsoid, dataEllipsoid, q_xyz, v_xyz); -// forwardKinematics(modelSphericalZYX, dataSphericalZYX, q_zyx, v_zyx); - -// // Check that the transformations are identical -// std::cout << "\n=== Forward Kinematics Comparison ===" << std::endl; -// std::cout << "oMi (Ellipsoid):\n" << dataEllipsoid.oMi[1] << std::endl; -// std::cout << "oMi (SphericalZYX):\n" << dataSphericalZYX.oMi[1] << std::endl; -// BOOST_CHECK(dataEllipsoid.oMi[1].isApprox(dataSphericalZYX.oMi[1])); - -// BOOST_CHECK(dataEllipsoid.liMi[1].isApprox(dataSphericalZYX.liMi[1])); - -// // Check that velocities match -// BOOST_CHECK(dataEllipsoid.v[1].toVector().isApprox(dataSphericalZYX.v[1].toVector())); - -// // Test computeAllTerms -// computeAllTerms(modelEllipsoid, dataEllipsoid, q_xyz, v_xyz); -// computeAllTerms(modelSphericalZYX, dataSphericalZYX, q_zyx, v_zyx); - -// BOOST_CHECK(dataEllipsoid.Ycrb[1].matrix().isApprox(dataSphericalZYX.Ycrb[1].matrix())); - -// BOOST_CHECK(dataEllipsoid.f[1].toVector().isApprox(dataSphericalZYX.f[1].toVector())); - -// BOOST_CHECK(dataEllipsoid.nle.isApprox(dataSphericalZYX.nle)); - -// BOOST_CHECK(dataEllipsoid.com[0].isApprox(dataSphericalZYX.com[0])); - -// // Test inverse dynamics (RNEA) -// Eigen::VectorXd aEllipsoid = Eigen::VectorXd::Ones(modelEllipsoid.nv); -// Eigen::VectorXd aSphericalZYX = Eigen::VectorXd::Ones(modelSphericalZYX.nv); - -// Eigen::VectorXd tauEllipsoid = rnea(modelEllipsoid, dataEllipsoid, q_xyz, v_xyz, aEllipsoid); -// Eigen::VectorXd tauSphericalZYX = rnea(modelSphericalZYX, dataSphericalZYX, q_zyx, v_zyx, -// aSphericalZYX); - -// BOOST_CHECK(tauEllipsoid.isApprox(tauSphericalZYX)); - -// // Test forward dynamics (ABA) -// Eigen::VectorXd tau = Eigen::VectorXd::Ones(modelEllipsoid.nv); - -// Eigen::VectorXd aAbaEllipsoid = aba(modelEllipsoid, dataEllipsoid, q_xyz, v_xyz, tau, -// Convention::WORLD); Eigen::VectorXd aAbaSphericalZYX = aba(modelSphericalZYX, dataSphericalZYX, -// q_zyx, v_zyx, tau, Convention::WORLD); - -// BOOST_CHECK(aAbaEllipsoid.isApprox(aAbaSphericalZYX)); - -// // Test with LOCAL convention -// aAbaEllipsoid = aba(modelEllipsoid, dataEllipsoid, q_xyz, v_xyz, tau, Convention::LOCAL); -// aAbaSphericalZYX = aba(modelSphericalZYX, dataSphericalZYX, q_zyx, v_zyx, tau, -// Convention::LOCAL); - -// BOOST_CHECK(aAbaEllipsoid.isApprox(aAbaSphericalZYX)); - -// // Test with different configurations -// q_zyx << 0.2, -0.5, 1.1; -// v_zyx << 0.3, 0.1, -0.2; - -// converter.convertConfigurationVector(q_zyx, q_xyz); -// converter.convertTangentVector(q_zyx, v_zyx, v_xyz); - -// forwardKinematics(modelEllipsoid, dataEllipsoid, q_xyz, v_xyz); -// forwardKinematics(modelSphericalZYX, dataSphericalZYX, q_zyx, v_zyx); - -// BOOST_CHECK(dataEllipsoid.oMi[1].isApprox(dataSphericalZYX.oMi[1])); - -// BOOST_CHECK(dataEllipsoid.v[1].toVector().isApprox(dataSphericalZYX.v[1].toVector())); - -// tauEllipsoid = rnea(modelEllipsoid, dataEllipsoid, q_xyz, v_xyz, aEllipsoid); -// tauSphericalZYX = rnea(modelSphericalZYX, dataSphericalZYX, q_zyx, v_zyx, aSphericalZYX); - -// BOOST_CHECK(tauEllipsoid.isApprox(tauSphericalZYX)); - -// aAbaEllipsoid = aba(modelEllipsoid, dataEllipsoid, q_xyz, v_xyz, tau, Convention::WORLD); -// aAbaSphericalZYX = aba(modelSphericalZYX, dataSphericalZYX, q_zyx, v_zyx, tau, -// Convention::WORLD); - -// BOOST_CHECK(aAbaEllipsoid.isApprox(aAbaSphericalZYX)); - -// } - BOOST_AUTO_TEST_CASE(vsSphericalZYX) { using namespace pinocchio; @@ -247,9 +68,6 @@ BOOST_AUTO_TEST_CASE(vsSphericalZYX) const Matrix3 & R = dataSphericalZYX.oMi[1].rotation(); const Motion & spatial_vel_zyx = dataSphericalZYX.v[1]; - std::cout << "\n=== Target Rotation Matrix (from ZYX) ===" << std::endl; - std::cout << R << std::endl; - // Extract XYZ Euler angles from the rotation matrix // For XYZ convention: R = Rx(x) * Ry(y) * Rz(z) // We need to solve for x, y, z @@ -302,8 +120,7 @@ BOOST_AUTO_TEST_CASE(vsSphericalZYX) Eigen::Vector3d w_e = S_e * qd_e; BOOST_CHECK(w_s.isApprox(w_e)); - std::cout << "✓ Angular velocities from joint velocities match" << std::endl; - + // Compute forward kinematics with the converted configurations forwardKinematics(modelEllipsoid, dataEllipsoid, q_e, qd_e); @@ -319,21 +136,17 @@ BOOST_AUTO_TEST_CASE(vsSphericalZYX) BOOST_CHECK(jDataEllipsoidFK.S.matrix().isApprox(jDataEllipsoidFK2.S.matrix())); BOOST_CHECK(jDataEllipsoidFK.S.matrix().isApprox(jDataEllipsoidFK3.S.matrix())); - std::cout << "✓ Motion subspace matrices match for the three calc(...) methods" << std::endl; JointDataSphericalZYX jDataSphereFK = jmodel_s.createData(); jmodel_s.calc(jDataSphereFK, q_s, qd_s); - std::cout << "\n=== Joint-frame velocities (S * v) ===" << std::endl; + // Joint-frame velocities (S * v) Eigen::Matrix joint_vel_e = jDataEllipsoidFK.S.matrix() * qd_e; Eigen::Matrix manual_vel = jDataEllipsoidFK.S.matrix() * jDataEllipsoidFK.joint_v; Eigen::Matrix joint_vel_s = jDataSphereFK.S.matrix() * qd_s; BOOST_CHECK(dataEllipsoid.v[1].toVector().isApprox(dataSphericalZYX.v[1].toVector())); - std::cout << "✓ Spatial velocities match" << std::endl; - BOOST_CHECK(dataEllipsoid.oMi[1].isApprox(dataSphericalZYX.oMi[1])); - std::cout << "✓ Full oMi[1] matches" << std::endl; Matrix3 Sdot_e = jDataEllipsoidFK.Sdot.matrix().bottomRows<3>(); // Angular part @@ -348,31 +161,134 @@ BOOST_AUTO_TEST_CASE(vsSphericalZYX) Eigen::Vector3d wdot_s = jDataSphereFK.c.angular() + S_s * qdotdot_s; Eigen::Vector3d wdot_e = Sdot_e * qd_e + S_e * qdotdot_e; BOOST_CHECK(wdot_s.isApprox(wdot_e)); - std::cout << "✓ Angular accelerations from joint accelerations match" << std::endl; forwardKinematics(modelEllipsoid, dataEllipsoid, q_e, qd_e, qdotdot_e); forwardKinematics(modelSphericalZYX, dataSphericalZYX, q_s, qd_s, qdotdot_s); BOOST_CHECK(dataEllipsoid.a[1].toVector().isApprox(dataSphericalZYX.a[1].toVector())); - // Test RNEA (Recursive Newton-Euler Algorithm) - std::cout << "\n=== RNEA Test ===" << std::endl; - + // Test RNEA (Recursive Newton-Euler Algorithm) - spatial forces should match rnea(modelEllipsoid, dataEllipsoid, q_e, qd_e, qdotdot_e); rnea(modelSphericalZYX, dataSphericalZYX, q_s, qd_s, qdotdot_s); BOOST_CHECK(dataEllipsoid.f[1].isApprox(dataSphericalZYX.f[1])); - std::cout << "✓ RNEA f match" << std::endl; // Test ABA (Articulated-Body Algorithm) - std::cout << "\n=== ABA Test ===" << std::endl; Eigen::VectorXd tau = Eigen::VectorXd::Ones(modelEllipsoid.nv); - Eigen::VectorXd aAbaEllipsoid = aba(modelEllipsoid, dataEllipsoid, q_e, qd_e, dataEllipsoid.tau, Convention::WORLD); BOOST_CHECK(dataEllipsoid.ddq.isApprox(qdotdot_e)); - std::cout << "✓ ABA computed accelerations match input accelerations" << std::endl; +} + +BOOST_AUTO_TEST_CASE(vsCompositeTxTyTzRxRyRz) +{ + using namespace pinocchio; + typedef SE3::Vector3 Vector3; + typedef SE3::Matrix3 Matrix3; + + // Ellipsoid parameters + double radius_a = 2.0; + double radius_b = 1.5; + double radius_c = 1.0; + + Inertia inertia = Inertia::Identity(); + SE3 pos = SE3::Identity(); + + // Create Ellipsoid model + Model modelEllipsoid; + JointModelEllipsoid jointModelEllipsoid(radius_a, radius_b, radius_c); + addJointAndBody(modelEllipsoid, jointModelEllipsoid, 0, pos, "ellipsoid", inertia); + + // Create Composite model (Tx, Ty, Tz, Rx, Ry, Rz) + Model modelComposite; + JointModelComposite jComposite; + jComposite.addJoint(JointModelPX()); + jComposite.addJoint(JointModelPY()); + jComposite.addJoint(JointModelPZ()); + jComposite.addJoint(JointModelRX()); + jComposite.addJoint(JointModelRY()); + jComposite.addJoint(JointModelRZ()); + addJointAndBody(modelComposite, jComposite, 0, pos, "composite", inertia); + + Data dataEllipsoid(modelEllipsoid); + Data dataComposite(modelComposite); + + // Test positions of ellispoid vs composite + Eigen::VectorXd q_ellipsoid(3); + q_ellipsoid << 1.0, 2.0, 3.0; // rx, ry, rz + Eigen::Vector3d t = jointModelEllipsoid.computeTranslations(q_ellipsoid); + Eigen::VectorXd q_composite(6); + q_composite << t, q_ellipsoid; + + forwardKinematics(modelEllipsoid, dataEllipsoid, q_ellipsoid); + forwardKinematics(modelComposite, dataComposite, q_composite); + + BOOST_CHECK(dataEllipsoid.oMi[1].isApprox(dataComposite.oMi[1])); + + // Velocity test + Eigen::VectorXd qdot_ellipsoid(3); + qdot_ellipsoid << 0.1, 0.2, 0.3; + + Eigen::Vector3d v_linear = + jointModelEllipsoid.computeTranslationVelocities(q_ellipsoid, qdot_ellipsoid); + Eigen::VectorXd qdot_composite(6); + qdot_composite << v_linear, qdot_ellipsoid; + + forwardKinematics(modelEllipsoid, dataEllipsoid, q_ellipsoid, qdot_ellipsoid); + forwardKinematics(modelComposite, dataComposite, q_composite, qdot_composite); + + BOOST_CHECK(dataEllipsoid.v[1].toVector().isApprox(dataComposite.v[1].toVector())); + + // Acceleration test + Eigen::VectorXd qddot_ellipsoid(3); + qddot_ellipsoid << 0.01, 0.02, 0.03; + Eigen::Vector3d a_linear = jointModelEllipsoid.computeTranslationAccelerations( + q_ellipsoid, qdot_ellipsoid, qddot_ellipsoid); + Eigen::VectorXd qddot_composite(6); + qddot_composite << a_linear, qddot_ellipsoid; + + forwardKinematics(modelEllipsoid, dataEllipsoid, q_ellipsoid, qdot_ellipsoid, qddot_ellipsoid); + forwardKinematics(modelComposite, dataComposite, q_composite, qdot_composite, qddot_composite); + + BOOST_CHECK(dataEllipsoid.a[1].toVector().isApprox(dataComposite.a[1].toVector())); + + // Test RNEA - spatial forces and torques should match + rnea(modelEllipsoid, dataEllipsoid, q_ellipsoid, qdot_ellipsoid, qddot_ellipsoid); + rnea(modelComposite, dataComposite, q_composite, qdot_composite, qddot_composite); + BOOST_CHECK(dataEllipsoid.f[1].isApprox(dataComposite.f[1])); + + // Need joint data to get both motion subspaces S_comp and S_ell + JointDataComposite jdata_c = jComposite.createData(); + jComposite.setIndexes(0, 0, 0); + jComposite.calc(jdata_c, q_composite, qdot_composite); + + JointDataEllipsoid jdata_e = jointModelEllipsoid.createData(); + jointModelEllipsoid.setIndexes(0, 0, 0); + jointModelEllipsoid.calc(jdata_e, q_ellipsoid, qdot_ellipsoid); + + const Eigen::Matrix S_comp = jdata_c.S.matrix(); // 6x6 (Tx,Ty,Tz,Rx,Ry,Rz) + const Eigen::Matrix S_ell = jdata_e.S.matrix(); // 6x3 (ellipsoid) + + // Compute the Jacobian mapping from ellipsoid generalized velocities to composite ones: + // qdot_comp = J * qdot_ell with J = (S_comp^T * S_comp)^-1 * S_comp^T * S_ell + Eigen::MatrixXd J = (S_comp.transpose() * S_comp).ldlt().solve(S_comp.transpose() * S_ell); + + // Now map the torques back (dual mapping) + // τ_ell = J^T * τ_comp + Eigen::VectorXd tau_proj = J.transpose() * dataComposite.tau; + + // Check the numerical match + BOOST_CHECK_MESSAGE( + dataEllipsoid.tau.isApprox(tau_proj, 1e-6), + "Projected composite torques do not match ellipsoid torques.\n" + << "Expected: " << dataEllipsoid.tau.transpose() << "\nGot: " << tau_proj.transpose()); + + // Test ABA (Articulated-Body Algorithm) + Eigen::VectorXd aAbaEllipsoid = aba( + modelEllipsoid, dataEllipsoid, q_ellipsoid, qdot_ellipsoid, dataEllipsoid.tau, + Convention::WORLD); + BOOST_CHECK(aAbaEllipsoid.isApprox(qddot_ellipsoid)); } BOOST_AUTO_TEST_SUITE_END() From 147008744c12197885253f28d4a55b9c44443f32 Mon Sep 17 00:00:00 2001 From: ipuch Date: Mon, 3 Nov 2025 18:10:18 +0100 Subject: [PATCH 06/48] tests(joint-ellipsoid): Sdot as finite difference pre-commit --- .../python/parsers/graph/expose-edges.cpp | 15 +++ .../multibody/joint/joint-ellipsoid.hpp | 17 +-- unittest/joint-ellipsoid.cpp | 112 ++++++++++++++---- unittest/model-graph.cpp | 2 +- 4 files changed, 109 insertions(+), 37 deletions(-) diff --git a/bindings/python/parsers/graph/expose-edges.cpp b/bindings/python/parsers/graph/expose-edges.cpp index ee8663c0ef..a567d97c01 100644 --- a/bindings/python/parsers/graph/expose-edges.cpp +++ b/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; @@ -117,6 +120,18 @@ namespace pinocchio .def_readonly("nq", &JointEllipsoid::nq, "Number of configuration variables.") .def_readonly("nv", &JointEllipsoid::nv, "Number of tangent variables."); + bp::class_( + "JointEllipsoid", "Represents an ellipsoidal joint.", + bp::init<>(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", "Represents a translation joint.", bp::init<>(bp::args("self"), "Default constructor.")) diff --git a/include/pinocchio/multibody/joint/joint-ellipsoid.hpp b/include/pinocchio/multibody/joint/joint-ellipsoid.hpp index 7747267679..028532d705 100644 --- a/include/pinocchio/multibody/joint/joint-ellipsoid.hpp +++ b/include/pinocchio/multibody/joint/joint-ellipsoid.hpp @@ -12,7 +12,7 @@ #include "pinocchio/math/matrix.hpp" #include "pinocchio/spatial/inertia.hpp" #include "pinocchio/spatial/skew.hpp" -#include "pinocchio/multibody/joint-motion-subspace.hpp" // because we are dense. +#include "pinocchio/multibody/joint-motion-subspace.hpp" namespace pinocchio { @@ -452,28 +452,23 @@ namespace pinocchio const Scalar c1s2 = c1 * s2; Scalar S_11, S_21, S_31, S_12, S_22, S_32; - // S_11 = c1 * (-radius_b * c0 * (c0 * s2 + c2 * s0 * s1) - radius_c * s0 * (- c0 * c2 * s1 + - // s0 * s2)); + S_11 = dndoty_dqdot0 * radius_b * (c0 * s2 + c2 * s0 * s1) + dndotz_dqdot0 * radius_c * (-c0 * c2 * s1 + s0 * s2); - // S_21 = c1 * (radius_b * c0 * (- c0 * c2 + s0 * s1 * s2) - radius_c * s0 * (c0 * s1 * s2 + - // c2 * s0)); + S_21 = -dndoty_dqdot0 * radius_b * (-c0 * c2 + s0 * s1 * s2) + dndotz_dqdot0 * radius_c * (c0 * s1 * s2 + c2 * s0); - // S_31 = c0 * c1 * c1 * s0 * (radius_b - radius_c); + S_31 = c1 * (-dndoty_dqdot0 * radius_b * s0 + dndotz_dqdot0 * radius_c * c0); - // S_12 = radius_a * c1c2 * c1 + radius_b * s0 * s1 * (c0 * s2 + c2 * s0 * s1) - radius_c * c0 - // * s1 * (- c0 * c2 * s1 + s0 * s2); S_12 = dndotx_dqdot1 * radius_a * c1 * c2 + dndoty_dqdot1 * radius_b * (c0 * s2 + c2 * s0 * s1) + dndotz_dqdot1 * radius_c * (-c0 * c2 * s1 + s0 * s2); - // S_22 = - radius_a * c1s2 * c1 - radius_b * s0 * s1 * (- c0 * c2 + s0 * s1 * s2) - radius_c - // * c0 * s1 * (c0 * s1 * s2 + c2 * s0); + S_22 = -dndotx_dqdot1 * radius_a * c1 * s2 - dndoty_dqdot1 * radius_b * (-c0 * c2 + s0 * s1 * s2) + dndotz_dqdot1 * radius_c * (c0 * s1 * s2 + c2 * s0); - // S_32 = c1 * s1 * (radius_a - radius_b * s0 * s0 - radius_c * c0 * c0); + S_32 = dndotx_dqdot1 * radius_a * s1 - dndoty_dqdot1 * radius_b * c1 * s0 + dndotz_dqdot1 * radius_c * c0 * c1; diff --git a/unittest/joint-ellipsoid.cpp b/unittest/joint-ellipsoid.cpp index ec323509bc..d8e772b711 100644 --- a/unittest/joint-ellipsoid.cpp +++ b/unittest/joint-ellipsoid.cpp @@ -36,6 +36,59 @@ void addJointAndBody( model.appendBodyToJoint(idx, Y); } +/// \brief Compute Sdot (motion subspace derivative) via finite differences +template +Eigen::Matrix finiteDiffSdot( + const JointModel & jmodel, + typename JointModel::JointDataDerived & jdata, + const typename JointModel::ConfigVector_t & q, + const typename JointModel::TangentVector_t & v, + double eps = 1e-8) +{ + typedef typename LieGroup::type LieGroupType; + typedef typename JointModel::ConfigVector_t ConfigVector_t; + typedef typename JointModel::TangentVector_t TangentVector_t; + + const Eigen::DenseIndex nv = jmodel.nv(); + + Eigen::Matrix Sdot_fd; + Sdot_fd.setZero(); + + ConfigVector_t q_integrated(q); + TangentVector_t v_integrate(nv); + v_integrate.setZero(); + + for (Eigen::DenseIndex k = 0; k < nv; ++k) + { + // Integrate along kth direction + v_integrate[k] = eps; + q_integrated = LieGroupType().integrate(q, v_integrate); + + // Compute S at q + eps * e_k + jmodel.calc(jdata, q_integrated); + const Eigen::Matrix S_plus = jdata.S.matrix(); + + // Integrate in negative direction + v_integrate[k] = -eps; + q_integrated = LieGroupType().integrate(q, v_integrate); + + // Compute S at q - eps * e_k + jmodel.calc(jdata, q_integrated); + const Eigen::Matrix S_minus = jdata.S.matrix(); + + // Compute dS/dq_k via central differences + Eigen::Matrix dS_dqk = (S_plus - S_minus) / (2.0 * eps); + + // Accumulate: Sdot += (dS/dq_k) * v_k + Sdot_fd += dS_dqk * v[k]; + + // Reset + v_integrate[k] = 0.; + } + + return Sdot_fd; +} + BOOST_AUTO_TEST_SUITE(JointEllipsoid) BOOST_AUTO_TEST_CASE(vsSphericalZYX) @@ -71,31 +124,8 @@ BOOST_AUTO_TEST_CASE(vsSphericalZYX) // Extract XYZ Euler angles from the rotation matrix // For XYZ convention: R = Rx(x) * Ry(y) * Rz(z) // We need to solve for x, y, z - Eigen::Vector3d q_e; - - // From the Ellipsoid rotation matrix structure: - // R(0,2) = sin(y) - // R(1,2) = -sin(x)*cos(y) - // R(2,2) = cos(x)*cos(y) - - double sy = R(0, 2); - q_e(1) = std::asin(sy); // y angle - - double cy = std::cos(q_e(1)); - - if (std::abs(cy) > 1e-6) - { - // Not at singularity - q_e(0) = std::atan2(-R(1, 2) / cy, R(2, 2) / cy); // x angle - q_e(2) = std::atan2(-R(0, 1) / cy, R(0, 0) / cy); // z angle - } - else - { - // Gimbal lock - choose x = 0 - q_e(0) = 0.0; - q_e(2) = std::atan2(R(1, 0), R(1, 1)); - } + q_e = R.eulerAngles(0, 1, 2); // XYZ convention // Get the motion subspace matrices (which give us the Jacobians) JointModelSphericalZYX jmodel_s; @@ -120,7 +150,7 @@ BOOST_AUTO_TEST_CASE(vsSphericalZYX) Eigen::Vector3d w_e = S_e * qd_e; BOOST_CHECK(w_s.isApprox(w_e)); - + // Compute forward kinematics with the converted configurations forwardKinematics(modelEllipsoid, dataEllipsoid, q_e, qd_e); @@ -291,4 +321,36 @@ BOOST_AUTO_TEST_CASE(vsCompositeTxTyTzRxRyRz) BOOST_CHECK(aAbaEllipsoid.isApprox(qddot_ellipsoid)); } +BOOST_AUTO_TEST_CASE(testSdotFiniteDifferences) +{ + using namespace pinocchio; + + // Ellipsoid parameters + double radius_a = 2.0; + double radius_b = 1.5; + double radius_c = 1.0; + + JointModelEllipsoid jmodel(radius_a, radius_b, radius_c); + jmodel.setIndexes(0, 0, 0); + + JointDataEllipsoid jdata = jmodel.createData(); + + // Test configuration and velocity + typedef JointModelEllipsoid::ConfigVector_t ConfigVector_t; + typedef JointModelEllipsoid::TangentVector_t TangentVector_t; + typedef LieGroup::type LieGroupType; + + ConfigVector_t q = LieGroupType().random(); + TangentVector_t v = TangentVector_t::Random(); + + // Compute analytical Sdot + const double eps = 1e-8; + jmodel.calc(jdata, q, v); + const Eigen::Matrix Sdot_ref = jdata.Sdot.matrix(); + + // Compute Sdot via finite differences using helper function + const Eigen::Matrix Sdot_fd = finiteDiffSdot(jmodel, jdata, q, v, eps); + + BOOST_CHECK(Sdot_ref.isApprox(Sdot_fd, sqrt(eps))); +} BOOST_AUTO_TEST_SUITE_END() diff --git a/unittest/model-graph.cpp b/unittest/model-graph.cpp index 2f1776b7a7..07e01258b9 100644 --- a/unittest/model-graph.cpp +++ b/unittest/model-graph.cpp @@ -617,7 +617,7 @@ BOOST_AUTO_TEST_CASE(test_reverse_spherical_zyx) d_f.oMf[m_forward.getFrameId("body1", pinocchio::BODY)])); } -/// @brief test if reversing of a composite joint is correct. +/// @brief test if reversing of an ellipsoid joint is correct. BOOST_AUTO_TEST_CASE(test_reverse_ellipsoid) { using namespace pinocchio::graph; From 7842a92e8b849066a8bd513fd788c1f50551217a Mon Sep 17 00:00:00 2001 From: ipuch Date: Thu, 6 Nov 2025 21:24:51 +0100 Subject: [PATCH 07/48] test(joint-ellipsoid): model-graph --- unittest/model-graph.cpp | 63 +++++++++++++--------------------------- 1 file changed, 20 insertions(+), 43 deletions(-) diff --git a/unittest/model-graph.cpp b/unittest/model-graph.cpp index 07e01258b9..e817749730 100644 --- a/unittest/model-graph.cpp +++ b/unittest/model-graph.cpp @@ -617,64 +617,41 @@ BOOST_AUTO_TEST_CASE(test_reverse_spherical_zyx) d_f.oMf[m_forward.getFrameId("body1", pinocchio::BODY)])); } -/// @brief test if reversing of an ellipsoid joint is correct. -BOOST_AUTO_TEST_CASE(test_reverse_ellipsoid) +/// @brief test if an ellipsoid joint is correct after building +BOOST_AUTO_TEST_CASE(test_ellipsoid) { using namespace pinocchio::graph; - ModelGraph g = buildReversableModelGraph(JointEllipsoid(0.01, 0.01, 0.01)); - ///////////////// Model - BOOST_CHECK_THROW(buildModel(g, "body2", pinocchio::SE3::Identity()), std::invalid_argument); + ModelGraph g = buildReversableModelGraph(JointEllipsoid(0.01, 0.02, 0.03)); //////////////////////////////////// Forward model pinocchio::Model m_forward = buildModel(g, "body1", pinocchio::SE3::Identity()); pinocchio::Data d_f(m_forward); - // config vector forward model Ellipsoid + // config vector forward model Eigen::Vector3d q(m_forward.nq); q << M_PI / 4, M_PI, M_PI / 2; pinocchio::framesForwardKinematics(m_forward, d_f, q); // Create a standalone model with the EXACT same structure as the graph model pinocchio::Model modelEllipsoid; - pinocchio::SE3 poseBody1 = - pinocchio::SE3(Eigen::Matrix3d::Identity(), Eigen::Vector3d(2., 0., 0.)); - pinocchio::SE3 poseBody2 = - pinocchio::SE3(Eigen::Matrix3d::Identity(), Eigen::Vector3d(0., 3., 0.)); pinocchio::Model::JointIndex idx; - pinocchio::JointModelEllipsoid jModelEllipsoid(0.01, 0.01, 0.01); - idx = modelEllipsoid.addJoint(0, jModelEllipsoid, poseBody1, "ellipsoid"); - std::cout << "Added joint ellipsoid with index: " << idx << std::endl; - - pinocchio::Inertia body2_inertia = Inertia::Identity(); - modelEllipsoid.appendBodyToJoint(idx, body2_inertia, poseBody2); - // pinocchio::FrameIndex frame_id = modelEllipsoid.addBodyFrame("body2", -1, poseBody2); - - // This line is not working yet. - // pinocchio::FrameIndex frame_id = modelEllipsoid.addBodyFrame("body2", idx, poseBody2); - - // pinocchio::Data jDataEllipsoid(modelEllipsoid); - // pinocchio::framesForwardKinematics(modelEllipsoid, jDataEllipsoid, q); - - // DEBUG: Print both transforms - std::cout << "nb frames in new model: " << modelEllipsoid.nframes << std::endl; - // std::cout << "=== DEBUG test_reverse_ellipsoid ===" << std::endl; - // std::cout << "body2_graph translation:\n" << body2_graph.translation().transpose() << - // std::endl; std::cout << "body2_graph rotation:\n" << body2_graph.rotation() << std::endl; - // std::cout << "\nbody2_standalone translation:\n" << body2_standalone.translation().transpose() - // << std::endl; std::cout << "body2_standalone rotation:\n" << body2_standalone.rotation() << - // std::endl; std::cout << "\nTranslation difference:\n" << (body2_graph.translation() - - // body2_standalone.translation()).transpose() << std::endl; std::cout << "Translation difference - // norm: " << (body2_graph.translation() - body2_standalone.translation()).norm() << std::endl; - // std::cout << "Rotation difference (Frobenius norm): " - // << (body2_graph.rotation() - body2_standalone.rotation()).norm() << std::endl; - - // Compare the absolute placement of body2 in both models - // pinocchio::SE3 body2_graph = d_f.oMf[m_forward.getFrameId("body2", pinocchio::BODY)]; - // pinocchio::SE3 body2_standalone = - // jDataEllipsoid.oMf[modelEllipsoid.getFrameId("body2", pinocchio::BODY)]; - - // BOOST_CHECK(body2_graph.isApprox(body2_standalone)); + pinocchio::SE3 poseBody1(Eigen::Matrix3d::Identity(), Eigen::Vector3d(2., 0., 0.)); + pinocchio::SE3 poseBody2(Eigen::Matrix3d::Identity(), Eigen::Vector3d(0., 3., 0.)); + + idx = modelEllipsoid.addJoint( + 0, pinocchio::JointModelEllipsoid(0.01, 0.02, 0.03), poseBody1, "ellipsoid"); + + modelEllipsoid.appendBodyToJoint( + idx, + pinocchio::Inertia(4., pinocchio::Inertia::Vector3(0., 2., 0.), pinocchio::Symmetric3::Zero()), + poseBody2); + + pinocchio::Data data_model(modelEllipsoid); + pinocchio::framesForwardKinematics(modelEllipsoid, data_model, q); + + // Compare the joint transformations + BOOST_CHECK(SE3isApprox(data_model.oMi[idx], d_f.oMi[m_forward.getJointId("body1_to_body2")])); } /// @brief test if reversing of a composite joint is correct. From 4f251925dee442a014e100d41a836cb74142f62b Mon Sep 17 00:00:00 2001 From: ipuch Date: Thu, 6 Nov 2025 21:26:04 +0100 Subject: [PATCH 08/48] docs(model-graphs) --- unittest/model-graph.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/unittest/model-graph.cpp b/unittest/model-graph.cpp index e817749730..333cba8606 100644 --- a/unittest/model-graph.cpp +++ b/unittest/model-graph.cpp @@ -696,7 +696,7 @@ BOOST_AUTO_TEST_CASE(test_reverse_composite) d_f.oMf[m_forward.getFrameId("body1", pinocchio::BODY)])); } -/// @brief test if reversing of a composite joint is correct. +/// @brief test if reversing of a planar joint is correct. BOOST_AUTO_TEST_CASE(test_reverse_planar) { using namespace pinocchio::graph; @@ -732,7 +732,7 @@ BOOST_AUTO_TEST_CASE(test_reverse_planar) d_f.oMf[m_forward.getFrameId("body1", pinocchio::BODY)])); } -/// @brief test if reversing of a composite joint is correct. +/// @brief test if reversing of a mimic joint is correct. BOOST_AUTO_TEST_CASE(test_reverse_mimic) { using namespace pinocchio::graph; From 2b5bf1da70e04ea63e447238aa989c73b9d6b771 Mon Sep 17 00:00:00 2001 From: ipuch Date: Thu, 6 Nov 2025 21:44:53 +0100 Subject: [PATCH 09/48] clean(joint-ellipsoid): unused variables and warnings precomitted --- .../multibody/joint/joint-ellipsoid.hpp | 39 +++++-------------- 1 file changed, 9 insertions(+), 30 deletions(-) diff --git a/include/pinocchio/multibody/joint/joint-ellipsoid.hpp b/include/pinocchio/multibody/joint/joint-ellipsoid.hpp index 028532d705..56b74846a7 100644 --- a/include/pinocchio/multibody/joint/joint-ellipsoid.hpp +++ b/include/pinocchio/multibody/joint/joint-ellipsoid.hpp @@ -142,10 +142,10 @@ namespace pinocchio } JointModelEllipsoidTpl() + : radius_a(Scalar(0.01)) + , radius_b(Scalar(0.01)) + , radius_c(Scalar(0.01)) { - radius_a = Scalar(0.01); - radius_b = Scalar(0.01); - radius_c = Scalar(0.01); } explicit JointModelEllipsoidTpl(const Scalar & a, const Scalar & b, const Scalar & c) @@ -177,12 +177,7 @@ namespace pinocchio Scalar c2, s2; SINCOS(data.joint_q(2), &s2, &c2); - // common operations - Scalar c1c2, c1s2; - c1c2 = c1 * c2; - c1s2 = c1 * s2; - - data.M.rotation() << c1c2, -c1s2, s1, c0 * s2 + c2 * s0 * s1, c0 * c2 - s0 * s1 * s2, + data.M.rotation() << c1 * c2, -c1 * s2, s1, c0 * s2 + c2 * s0 * s1, c0 * c2 - s0 * s1 * s2, -c1 * s0, -c0 * c2 * s1 + s0 * s2, c0 * s1 * s2 + c2 * s0, c0 * c1; Scalar nx, ny, nz; @@ -210,10 +205,6 @@ namespace pinocchio Scalar c2, s2; SINCOS(data.joint_q(2), &s2, &c2); - Scalar c1c2, c1s2; - c1c2 = c1 * c2; - c1s2 = c1 * s2; - Scalar dndotx_dqdot1, dndoty_dqdot0, dndoty_dqdot1, dndotz_dqdot0, dndotz_dqdot1; dndotx_dqdot1 = c1; dndoty_dqdot0 = -c0 * c1; @@ -250,12 +241,7 @@ namespace pinocchio Scalar c2, s2; SINCOS(data.joint_q(2), &s2, &c2); - // common operations - Scalar c1c2, c1s2; - c1c2 = c1 * c2; - c1s2 = c1 * s2; - - data.M.rotation() << c1c2, -c1s2, s1, c0 * s2 + c2 * s0 * s1, c0 * c2 - s0 * s1 * s2, + data.M.rotation() << c1 * c2, -c1 * s2, s1, c0 * s2 + c2 * s0 * s1, c0 * c2 - s0 * s1 * s2, -c1 * s0, -c0 * c2 * s1 + s0 * s2, c0 * s1 * s2 + c2 * s0, c0 * c1; Scalar nx, ny, nz; @@ -418,9 +404,6 @@ namespace pinocchio const Scalar & c2, JointDataDerived & data) const { - // Common operations - const Scalar c1c2 = c1 * c2; - const Scalar c1s2 = c1 * s2; Scalar dndotx_dqdot1, dndoty_dqdot0, dndoty_dqdot1, dndotz_dqdot0, dndotz_dqdot1; dndotx_dqdot1 = c1; @@ -447,10 +430,6 @@ namespace pinocchio const Scalar & dndotz_dqdot1, JointDataDerived & data) const { - // Common operations - const Scalar c1c2 = c1 * c2; - const Scalar c1s2 = c1 * s2; - Scalar S_11, S_21, S_31, S_12, S_22, S_32; S_11 = dndoty_dqdot0 * radius_b * (c0 * s2 + c2 * s0 * s1) @@ -472,8 +451,8 @@ namespace pinocchio S_32 = dndotx_dqdot1 * radius_a * s1 - dndoty_dqdot1 * radius_b * c1 * s0 + dndotz_dqdot1 * radius_c * c0 * c1; - data.S.matrix() << S_11, S_12, Scalar(0), S_21, S_22, Scalar(0), S_31, S_32, Scalar(0), c1c2, - s2, Scalar(0), -c1s2, c2, Scalar(0), s1, Scalar(0), Scalar(1); + data.S.matrix() << S_11, S_12, Scalar(0), S_21, S_22, Scalar(0), S_31, S_32, Scalar(0), + c1 * c2, s2, Scalar(0), -c1 * s2, c2, Scalar(0), s1, Scalar(0), Scalar(1); } void computeMotionSubspaceDerivative( const Scalar & s0, @@ -505,13 +484,13 @@ namespace pinocchio Scalar d_dndoty_dqdot0_dq0 = s0 * c1; // dndoty_dqdot0 = - c0 * c1; Scalar d_dndoty_dqdot0_dq1 = c0 * s1; - Scalar d_dndoty_dqdot1_dq0 = c0 * s1; // dndoty_dqdot1 = s0 * s1; + // Scalar d_dndoty_dqdot1_dq0 = c0 * s1; // dndoty_dqdot1 = s0 * s1; Scalar d_dndoty_dqdot1_dq1 = s0 * c1; Scalar d_dndotz_dqdot0_dq0 = -c1 * c0; // dndotz_dqdot0 = - c1 * s0; Scalar d_dndotz_dqdot0_dq1 = s0 * s1; - Scalar d_dndotz_dqdot1_dq0 = s0 * s1; // dndotz_dqdot1 = - c0 * s1; + // Scalar d_dndotz_dqdot1_dq0 = s0 * s1; // dndotz_dqdot1 = - c0 * s1; Scalar d_dndotz_dqdot1_dq1 = -c0 * c1; // Upper part (translation) From 52e649c73cdf6c370295fdbaad627c26f16a4529 Mon Sep 17 00:00:00 2001 From: ipuch Date: Thu, 6 Nov 2025 22:50:03 +0100 Subject: [PATCH 10/48] fix(joint-ellipsoid): binding python not scalar but double instead --- bindings/python/parsers/graph/expose-edges.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bindings/python/parsers/graph/expose-edges.cpp b/bindings/python/parsers/graph/expose-edges.cpp index a567d97c01..9f67647ccc 100644 --- a/bindings/python/parsers/graph/expose-edges.cpp +++ b/bindings/python/parsers/graph/expose-edges.cpp @@ -122,7 +122,7 @@ namespace pinocchio bp::class_( "JointEllipsoid", "Represents an ellipsoidal joint.", - bp::init<>(bp::args("self", "radius_a", "radius_b", "radius_c"), "Constructor with radii.")) + bp::init(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( From e7017f8fd7687090a262e04d409d7006227c22d2 Mon Sep 17 00:00:00 2001 From: ipuch Date: Fri, 7 Nov 2025 09:54:04 +0100 Subject: [PATCH 11/48] feat(joint-ellipsoid): extra docs for binding + S available . --- .../bindings/python/multibody/joint/joint.hpp | 1 + .../bindings/python/multibody/joint/joints-datas.hpp | 12 ++++++++++++ .../pinocchio/multibody/joint/joint-ellipsoid.hpp | 2 ++ 3 files changed, 15 insertions(+) diff --git a/include/pinocchio/bindings/python/multibody/joint/joint.hpp b/include/pinocchio/bindings/python/multibody/joint/joint.hpp index e0dd2af48c..383b5a16b4 100644 --- a/include/pinocchio/bindings/python/multibody/joint/joint.hpp +++ b/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.") diff --git a/include/pinocchio/bindings/python/multibody/joint/joints-datas.hpp b/include/pinocchio/bindings/python/multibody/joint/joints-datas.hpp index ca78e24d7a..0fd7b9c4f1 100644 --- a/include/pinocchio/bindings/python/multibody/joint/joints-datas.hpp +++ b/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_ & + expose_joint_data(bp::class_ & 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_ & expose_joint_data(bp::class_ & cl) diff --git a/include/pinocchio/multibody/joint/joint-ellipsoid.hpp b/include/pinocchio/multibody/joint/joint-ellipsoid.hpp index 56b74846a7..a87e8b29ca 100644 --- a/include/pinocchio/multibody/joint/joint-ellipsoid.hpp +++ b/include/pinocchio/multibody/joint/joint-ellipsoid.hpp @@ -94,6 +94,8 @@ namespace pinocchio JointDataEllipsoidTpl() : joint_q(ConfigVector_t::Zero()) , joint_v(TangentVector_t::Zero()) + , S() + , Sdot() , M(Transformation_t::Identity()) , v(Motion_t::Zero()) , c(Bias_t::Zero()) From b3ecb1ce5113ec636bb6738224317edd2098778b Mon Sep 17 00:00:00 2001 From: LucasJoseph Date: Mon, 10 Nov 2025 09:44:32 +0100 Subject: [PATCH 12/48] Add ellipsoid joint kinematics example script This script demonstrates the use of the ellipsoid joint in Pinocchio, showcasing kinematics, dynamics, and visualization of a robot with an ellipsoid joint. It includes functions for creating the robot model, computing kinematics and dynamics, and visualizing the motion. --- examples/ellipsoid-joint-kinematics.py | 305 +++++++++++++++++++++++++ 1 file changed, 305 insertions(+) create mode 100644 examples/ellipsoid-joint-kinematics.py diff --git a/examples/ellipsoid-joint-kinematics.py b/examples/ellipsoid-joint-kinematics.py new file mode 100644 index 0000000000..9066f0598f --- /dev/null +++ b/examples/ellipsoid-joint-kinematics.py @@ -0,0 +1,305 @@ +""" +Ellipsoid Joint Kinematics Example + +This example demonstrates the use of the ellipsoid joint in Pinocchio. +The ellipsoid joint constrains a point to move on the surface of an ellipsoid +while allowing full 3D rotational freedom (SO(3)). + +The joint has 3 DOF corresponding to 3 rotation angles that determine both: +- The position on the ellipsoid surface +- The orientation of the body + +This is useful for modeling: +- Joints with coupled translation and rotation +- Contact points on ellipsoidal surfaces +- Biomechanical joints with ellipsoidal constraints +""" + +import pinocchio as pin +import numpy as np + +# For visualization (optional) +try: + from pinocchio.visualize import MeshcatVisualizer + VISUALIZE = True +except ImportError: + print("MeshcatVisualizer not available. Skipping visualization.") + VISUALIZE = False + + +def create_ellipsoid_robot(radius_a=0.5, radius_b=0.3, radius_c=0.2): + """ + Create a simple robot with an ellipsoid joint. + + Parameters + ---------- + radius_a : float + Ellipsoid radius along x-axis + radius_b : float + Ellipsoid radius along y-axis + radius_c : float + Ellipsoid radius along z-axis + + Returns + ------- + model : pin.Model + The robot model + """ + model = pin.Model() + + # Create ellipsoid joint + joint_name = "ellipsoid" + joint_id = model.addJoint( + 0, # parent joint id (0 = universe) + pin.JointModelEllipsoid(radius_a, radius_b, radius_c), + pin.SE3.Identity(), + joint_name + ) + + # Add a body with some inertia + mass = 1.0 + lever = np.array([0.0, 0.0, 0.1]) + inertia = pin.Inertia( + mass, + lever, + np.diag([0.01, 0.01, 0.01]) + ) + + body_placement = pin.SE3.Identity() + body_placement.translation = np.array([0.0, 0.0, 0.2]) + + model.appendBodyToJoint(joint_id, inertia, body_placement) + + return model + + +def compute_kinematics_example(): + """Demonstrate forward kinematics with the ellipsoid joint.""" + print("=" * 60) + print("ELLIPSOID JOINT KINEMATICS EXAMPLE") + print("=" * 60) + + # Create model with ellipsoid radii + radius_a, radius_b, radius_c = 0.5, 0.3, 0.2 + model = create_ellipsoid_robot(radius_a, radius_b, radius_c) + data = model.createData() + + print(f"\nEllipsoid parameters:") + print(f" radius_a (x-axis): {radius_a}") + print(f" radius_b (y-axis): {radius_b}") + print(f" radius_c (z-axis): {radius_c}") + + # Test different configurations + configs = [ + ("Zero configuration", np.array([0.0, 0.0, 0.0])), + ("Rotation around x", np.array([np.pi/4, 0.0, 0.0])), + ("Rotation around y", np.array([0.0, np.pi/4, 0.0])), + ("Rotation around z", np.array([0.0, 0.0, np.pi/4])), + ("Combined rotations", np.array([np.pi/6, np.pi/4, np.pi/3])), + ] + + for name, q in configs: + pin.forwardKinematics(model, data, q) + pin.updateFramePlacements(model, data) + + placement = data.oMi[1] + position = placement.translation + + print(f"\n{name}:") + print(f" q = {q}") + print(f" Position on ellipsoid: {position}") + print(f" Distance from origin: {np.linalg.norm(position):.4f}") + + # Verify point is on ellipsoid surface + normalized_pos = position / np.array([radius_a, radius_b, radius_c]) + + # checking the quadratic form of the ellipsoid equation + on_surface = np.abs(np.linalg.norm(normalized_pos) - 1.0) < 1e-10 + print(f" On ellipsoid surface: {on_surface}") + + +def compute_dynamics_example(): + """Demonstrate dynamics computation with the ellipsoid joint.""" + print("\n" + "=" * 60) + print("ELLIPSOID JOINT DYNAMICS EXAMPLE") + print("=" * 60) + + # Create model + model = create_ellipsoid_robot(0.5, 0.3, 0.2) + data = model.createData() + + # Configuration, velocity, and acceleration + q = np.array([np.pi/6, np.pi/4, np.pi/3]) + v = np.array([0.1, 0.2, 0.3]) + a = np.array([0.01, 0.02, 0.03]) + + print(f"\nConfiguration: q = {q}") + print(f"Velocity: v = {v}") + print(f"Acceleration: a = {a}") + + # Compute dynamics with RNEA (Recursive Newton-Euler Algorithm) + # Important Note: the generalized forces dimension matches the number of DOFs (3 for ellipsoid joint) + # but they are not pure torques due to the joint's nature and because the last + # The last step of rnea is tau = S.T * f + # where S is the joint motion subspace, and f the spatial force of the joint + # So the resulting tau vector contains generalized forces that + # include contributions from both rotational and translational effects. + tau = pin.rnea(model, data, q, v, a) + print(f"\nRequired torques (RNEA): τ = {tau}") + + # separate the generalized forces for interpretability from the spatial forces + S = data.joints[1].S + f = np.array(data.f[1]) + pure_torques = S[3:, :].T @ f[3:] # Extract rotational part + translations = S[:3, :].T @ f[:3] # Extract translational part + print(f" Pure rotational torques: {pure_torques}") + print(f" Translational forces: {translations}") + + # add them back to recover the original tau + tau_reconstructed = np.zeros(3) + tau_reconstructed += pure_torques + tau_reconstructed += translations + print(f" Reconstructed τ: {tau_reconstructed}") + + + +def visualize_ellipsoid_motion(): + """Visualize the ellipsoid joint motion in MeshCat.""" + if not VISUALIZE: + print("\nVisualization skipped (MeshcatVisualizer not available)") + return + + if not hasattr(pin, 'hppfcl'): + print("\nVisualization skipped (hpp-fcl not available)") + print("Install with: conda install -c conda-forge hpp-fcl") + print("Then recompile Pinocchio with -DBUILD_WITH_HPP_FCL_SUPPORT=ON") + return + + print("\n" + "=" * 60) + print("ELLIPSOID JOINT VISUALIZATION") + print("=" * 60) + + # Create model + radius_a, radius_b, radius_c = 0.5, 0.3, 0.2 + model = create_ellipsoid_robot(radius_a, radius_b, radius_c) + + # Add visual geometry + try: + geom_model = pin.GeometryModel() + + # 1. Add the ELLIPSOID SURFACE as a visual object + ellipsoid_shape = pin.hppfcl.Ellipsoid(radius_a, radius_b, radius_c) + ellipsoid_geom = pin.GeometryObject( + "ellipsoid_surface", + 0, # Universe frame + ellipsoid_shape, + pin.SE3.Identity() + ) + ellipsoid_geom.meshColor = np.array([0.8, 0.8, 0.8, 0.3]) # Semi-transparent gray + geom_model.addGeometryObject(ellipsoid_geom) + + # 2. Add a small sphere to show the contact point on the ellipsoid + contact_sphere = pin.hppfcl.Sphere(0.03) + contact_geom = pin.GeometryObject( + "contact_point", + model.getJointId("ellipsoid"), + contact_sphere, + pin.SE3.Identity() # At the joint frame (on the ellipsoid surface) + ) + contact_geom.meshColor = np.array([1.0, 0.0, 0.0, 1.0]) # Red + geom_model.addGeometryObject(contact_geom) + + # 3. Add a box to visualize the body orientation + box = pin.hppfcl.Box(0.1, 0.1, 0.3) + box_geom = pin.GeometryObject( + "body_visual", + model.getJointId("ellipsoid"), + box, + pin.SE3(np.eye(3), np.array([0.0, 0.0, 0.15])) + ) + box_geom.meshColor = np.array([0.2, 0.6, 1.0, 1.0]) # Blue + geom_model.addGeometryObject(box_geom) + + # Initialize visualizer + viz = MeshcatVisualizer(model, geom_model, geom_model) + viz.initViewer(open=True) + viz.loadViewerModel() + + print("\n✓ Visualization initialized!") + print(" - Gray ellipsoid: The constraint surface") + print(" - Red sphere: Contact point on the surface") + print(" - Blue box: The rigid body attached to the joint") + print("\nAnimating ellipsoid joint motion...") + print("Open http://127.0.0.1:7000/static/ in your browser to see the animation.") + + # Animate through different configurations + import time + t = 0.0 + dt = 0.02 + + for i in range(500): + # Create a smooth trajectory on the ellipsoid + q = np.array([ + 0.8 * np.sin(1.0 * t), + 0.6 * np.sin(1.2 * t + 1.0), + 0.4 * np.sin(0.8 * t + 0.5) + ]) + + viz.display(q) + time.sleep(dt) + t += dt + + if i % 50 == 0: + print(f" Frame {i}/500 - Configuration: [{q[0]:.3f}, {q[1]:.3f}, {q[2]:.3f}]") + + # create extra motion that slides along the principal axes q0 + for angle in np.linspace(0, 2 * np.pi, 100): + q = np.array([ + 0.5 * np.cos(angle), + 0.0, + 0.0 + ]) + viz.display(q) + time.sleep(dt) + + # q1 axis + for angle in np.linspace(0, 2 * np.pi, 100): + q = np.array([ + 0.0, + 0.3 * np.cos(angle), + 0.0 + ]) + viz.display(q) + time.sleep(dt) + + # q2 axis + for angle in np.linspace(0, 2 * np.pi, 100): + q = np.array([ + 0.0, + 0.0, + 0.2 * np.cos(angle) + ]) + viz.display(q) + time.sleep(dt) + + print("\n✓ Animation completed!") + + except Exception as e: + print(f"\nVisualization error: {e}") + import traceback + traceback.print_exc() + + +def main(): + """Run all examples.""" + compute_kinematics_example() + compute_dynamics_example() + visualize_ellipsoid_motion() + + print("\n" + "=" * 60) + print("Examples completed!") + print("=" * 60) + + +if __name__ == "__main__": + main() From 2d434e3dfbbff79fda3292d7208ae4d4036eb312 Mon Sep 17 00:00:00 2001 From: ipuch Date: Mon, 10 Nov 2025 15:43:21 +0100 Subject: [PATCH 13/48] exemple(joint-ellipsoid) --- examples/ellipsoid-joint-kinematics.py | 91 +++++++++++++++++++------- 1 file changed, 69 insertions(+), 22 deletions(-) diff --git a/examples/ellipsoid-joint-kinematics.py b/examples/ellipsoid-joint-kinematics.py index 9066f0598f..f2eba65051 100644 --- a/examples/ellipsoid-joint-kinematics.py +++ b/examples/ellipsoid-joint-kinematics.py @@ -1,18 +1,12 @@ """ Ellipsoid Joint Kinematics Example -This example demonstrates the use of the ellipsoid joint in Pinocchio. -The ellipsoid joint constrains a point to move on the surface of an ellipsoid -while allowing full 3D rotational freedom (SO(3)). - The joint has 3 DOF corresponding to 3 rotation angles that determine both: - The position on the ellipsoid surface - The orientation of the body -This is useful for modeling: -- Joints with coupled translation and rotation -- Contact points on ellipsoidal surfaces -- Biomechanical joints with ellipsoidal constraints +Note: the joint is not purely normal to the ellipsoid surface; it translates along a ellipsoid, + but the rotational part is "normal" to a sphere. """ import pinocchio as pin @@ -72,6 +66,47 @@ def create_ellipsoid_robot(radius_a=0.5, radius_b=0.3, radius_c=0.2): return model +def create_composite_model(): + """Create a model with a composite joint (6 DOF: Tx, Ty, Tz, Rx, Ry, Rz).""" + model = pin.Model() + + # Create composite joint with 3 prismatic + 3 revolute + jcomposite = pin.JointModelComposite() + jcomposite.addJoint(pin.JointModelPX()) + jcomposite.addJoint(pin.JointModelPY()) + jcomposite.addJoint(pin.JointModelPZ()) + jcomposite.addJoint(pin.JointModelRX()) + jcomposite.addJoint(pin.JointModelRY()) + jcomposite.addJoint(pin.JointModelRZ()) + + joint_id = model.addJoint( + 0, + jcomposite, + pin.SE3.Identity(), + "composite" + ) + + body_placement = pin.SE3.Identity() + body_placement.translation = np.array([0.0, 0.0, 0.2]) + + + # Add a body with some inertia + mass = 1.0 + lever = np.array([0.0, 0.0, 0.1]) + inertia = pin.Inertia( + mass, + lever, + np.diag([0.01, 0.01, 0.01]) + ) + + model.appendBodyToJoint( + joint_id, + inertia, + body_placement, + ) + + return model + def compute_kinematics_example(): """Demonstrate forward kinematics with the ellipsoid joint.""" @@ -132,6 +167,8 @@ def compute_dynamics_example(): q = np.array([np.pi/6, np.pi/4, np.pi/3]) v = np.array([0.1, 0.2, 0.3]) a = np.array([0.01, 0.02, 0.03]) + + pin.forwardKinematics(model, data, q, v, a) print(f"\nConfiguration: q = {q}") print(f"Velocity: v = {v}") @@ -142,24 +179,34 @@ def compute_dynamics_example(): # but they are not pure torques due to the joint's nature and because the last # The last step of rnea is tau = S.T * f # where S is the joint motion subspace, and f the spatial force of the joint - # So the resulting tau vector contains generalized forces that + # So the resulting tau vector contains three generalized forces that # include contributions from both rotational and translational effects. tau = pin.rnea(model, data, q, v, a) print(f"\nRequired torques (RNEA): τ = {tau}") - # separate the generalized forces for interpretability from the spatial forces - S = data.joints[1].S - f = np.array(data.f[1]) - pure_torques = S[3:, :].T @ f[3:] # Extract rotational part - translations = S[:3, :].T @ f[:3] # Extract translational part - print(f" Pure rotational torques: {pure_torques}") - print(f" Translational forces: {translations}") + # For example tau[0] is a combination of S_11(q)* f_x + S_21(q)* f_y + S_31(q)* f_z + S_41(q)* τ_x + S_51(q)* τ_y + S_61(q)* τ_z + + # Create composite model for comparison of joint spatial forces `data.f[1]` + composite_model = create_composite_model() + composite_data = composite_model.createData() + + q_composite = np.zeros(6) + q_composite[3:] = q # Set only rotational part + q_composite[:3] = data.oMi[1].translation # Set translation to current position on ellipsoid + + v_composite = np.zeros(6) + v_composite[3:] = v + v_composite[:3] = np.array(data.v[1])[:3] # Set linear velocity part + + a_composite = np.zeros(6) + a_composite[3:] = a + a_composite[:3] = np.array(data.a[1])[:3] # Set linear acceleration part - # add them back to recover the original tau - tau_reconstructed = np.zeros(3) - tau_reconstructed += pure_torques - tau_reconstructed += translations - print(f" Reconstructed τ: {tau_reconstructed}") + tau_composite = pin.rnea(composite_model, composite_data, q_composite, v_composite, a_composite) + print(f"\nComposite joint required torques (RNEA): τ = {tau_composite}") + print(f"\nComparison of spatial forces at the joint:") + print(f" - Ellipsoid joint spatial force: f = {data.f[1]} ") + print(f" - Composite joint spatial force: f = {composite_data.f[1]} ") @@ -277,7 +324,7 @@ def visualize_ellipsoid_motion(): q = np.array([ 0.0, 0.0, - 0.2 * np.cos(angle) + 3.14 * np.cos(angle) ]) viz.display(q) time.sleep(dt) From 6b9367049d25975dc07aee51f596b7c3cfb6f9e3 Mon Sep 17 00:00:00 2001 From: ipuch Date: Mon, 10 Nov 2025 15:49:45 +0100 Subject: [PATCH 14/48] pre-commit --- .../python/parsers/graph/expose-edges.cpp | 3 +- examples/ellipsoid-joint-kinematics.py | 145 ++++++++---------- .../python/multibody/joint/joints-datas.hpp | 3 +- 3 files changed, 70 insertions(+), 81 deletions(-) diff --git a/bindings/python/parsers/graph/expose-edges.cpp b/bindings/python/parsers/graph/expose-edges.cpp index 9f67647ccc..4aaa0f20c9 100644 --- a/bindings/python/parsers/graph/expose-edges.cpp +++ b/bindings/python/parsers/graph/expose-edges.cpp @@ -122,7 +122,8 @@ namespace pinocchio bp::class_( "JointEllipsoid", "Represents an ellipsoidal joint.", - bp::init(bp::args("self", "radius_a", "radius_b", "radius_c"), "Constructor with radii.")) + bp::init( + 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( diff --git a/examples/ellipsoid-joint-kinematics.py b/examples/ellipsoid-joint-kinematics.py index f2eba65051..4a32d73628 100644 --- a/examples/ellipsoid-joint-kinematics.py +++ b/examples/ellipsoid-joint-kinematics.py @@ -15,6 +15,7 @@ # For visualization (optional) try: from pinocchio.visualize import MeshcatVisualizer + VISUALIZE = True except ImportError: print("MeshcatVisualizer not available. Skipping visualization.") @@ -24,52 +25,49 @@ def create_ellipsoid_robot(radius_a=0.5, radius_b=0.3, radius_c=0.2): """ Create a simple robot with an ellipsoid joint. - + Parameters ---------- radius_a : float Ellipsoid radius along x-axis radius_b : float - Ellipsoid radius along y-axis + Ellipsoid radius along y-axis radius_c : float Ellipsoid radius along z-axis - + Returns ------- model : pin.Model The robot model """ model = pin.Model() - + # Create ellipsoid joint joint_name = "ellipsoid" joint_id = model.addJoint( 0, # parent joint id (0 = universe) pin.JointModelEllipsoid(radius_a, radius_b, radius_c), pin.SE3.Identity(), - joint_name + joint_name, ) - + # Add a body with some inertia mass = 1.0 lever = np.array([0.0, 0.0, 0.1]) - inertia = pin.Inertia( - mass, - lever, - np.diag([0.01, 0.01, 0.01]) - ) - + inertia = pin.Inertia(mass, lever, np.diag([0.01, 0.01, 0.01])) + body_placement = pin.SE3.Identity() body_placement.translation = np.array([0.0, 0.0, 0.2]) - + model.appendBodyToJoint(joint_id, inertia, body_placement) - + return model + def create_composite_model(): """Create a model with a composite joint (6 DOF: Tx, Ty, Tz, Rx, Ry, Rz).""" model = pin.Model() - + # Create composite joint with 3 prismatic + 3 revolute jcomposite = pin.JointModelComposite() jcomposite.addJoint(pin.JointModelPX()) @@ -78,33 +76,23 @@ def create_composite_model(): jcomposite.addJoint(pin.JointModelRX()) jcomposite.addJoint(pin.JointModelRY()) jcomposite.addJoint(pin.JointModelRZ()) - - joint_id = model.addJoint( - 0, - jcomposite, - pin.SE3.Identity(), - "composite" - ) + + joint_id = model.addJoint(0, jcomposite, pin.SE3.Identity(), "composite") body_placement = pin.SE3.Identity() body_placement.translation = np.array([0.0, 0.0, 0.2]) - # Add a body with some inertia mass = 1.0 lever = np.array([0.0, 0.0, 0.1]) - inertia = pin.Inertia( - mass, - lever, - np.diag([0.01, 0.01, 0.01]) - ) + inertia = pin.Inertia(mass, lever, np.diag([0.01, 0.01, 0.01])) model.appendBodyToJoint( joint_id, inertia, body_placement, ) - + return model @@ -113,38 +101,38 @@ def compute_kinematics_example(): print("=" * 60) print("ELLIPSOID JOINT KINEMATICS EXAMPLE") print("=" * 60) - + # Create model with ellipsoid radii radius_a, radius_b, radius_c = 0.5, 0.3, 0.2 model = create_ellipsoid_robot(radius_a, radius_b, radius_c) data = model.createData() - + print(f"\nEllipsoid parameters:") print(f" radius_a (x-axis): {radius_a}") print(f" radius_b (y-axis): {radius_b}") print(f" radius_c (z-axis): {radius_c}") - + # Test different configurations configs = [ ("Zero configuration", np.array([0.0, 0.0, 0.0])), - ("Rotation around x", np.array([np.pi/4, 0.0, 0.0])), - ("Rotation around y", np.array([0.0, np.pi/4, 0.0])), - ("Rotation around z", np.array([0.0, 0.0, np.pi/4])), - ("Combined rotations", np.array([np.pi/6, np.pi/4, np.pi/3])), + ("Rotation around x", np.array([np.pi / 4, 0.0, 0.0])), + ("Rotation around y", np.array([0.0, np.pi / 4, 0.0])), + ("Rotation around z", np.array([0.0, 0.0, np.pi / 4])), + ("Combined rotations", np.array([np.pi / 6, np.pi / 4, np.pi / 3])), ] - + for name, q in configs: pin.forwardKinematics(model, data, q) pin.updateFramePlacements(model, data) - + placement = data.oMi[1] position = placement.translation - + print(f"\n{name}:") print(f" q = {q}") print(f" Position on ellipsoid: {position}") print(f" Distance from origin: {np.linalg.norm(position):.4f}") - + # Verify point is on ellipsoid surface normalized_pos = position / np.array([radius_a, radius_b, radius_c]) @@ -158,22 +146,22 @@ def compute_dynamics_example(): print("\n" + "=" * 60) print("ELLIPSOID JOINT DYNAMICS EXAMPLE") print("=" * 60) - + # Create model model = create_ellipsoid_robot(0.5, 0.3, 0.2) data = model.createData() - + # Configuration, velocity, and acceleration - q = np.array([np.pi/6, np.pi/4, np.pi/3]) + q = np.array([np.pi / 6, np.pi / 4, np.pi / 3]) v = np.array([0.1, 0.2, 0.3]) a = np.array([0.01, 0.02, 0.03]) pin.forwardKinematics(model, data, q, v, a) - + print(f"\nConfiguration: q = {q}") print(f"Velocity: v = {v}") print(f"Acceleration: a = {a}") - + # Compute dynamics with RNEA (Recursive Newton-Euler Algorithm) # Important Note: the generalized forces dimension matches the number of DOFs (3 for ellipsoid joint) # but they are not pure torques due to the joint's nature and because the last @@ -189,11 +177,13 @@ def compute_dynamics_example(): # Create composite model for comparison of joint spatial forces `data.f[1]` composite_model = create_composite_model() composite_data = composite_model.createData() - + q_composite = np.zeros(6) q_composite[3:] = q # Set only rotational part - q_composite[:3] = data.oMi[1].translation # Set translation to current position on ellipsoid - + q_composite[:3] = data.oMi[ + 1 + ].translation # Set translation to current position on ellipsoid + v_composite = np.zeros(6) v_composite[3:] = v v_composite[:3] = np.array(data.v[1])[:3] # Set linear velocity part @@ -202,21 +192,22 @@ def compute_dynamics_example(): a_composite[3:] = a a_composite[:3] = np.array(data.a[1])[:3] # Set linear acceleration part - tau_composite = pin.rnea(composite_model, composite_data, q_composite, v_composite, a_composite) + tau_composite = pin.rnea( + composite_model, composite_data, q_composite, v_composite, a_composite + ) print(f"\nComposite joint required torques (RNEA): τ = {tau_composite}") print(f"\nComparison of spatial forces at the joint:") print(f" - Ellipsoid joint spatial force: f = {data.f[1]} ") print(f" - Composite joint spatial force: f = {composite_data.f[1]} ") - def visualize_ellipsoid_motion(): """Visualize the ellipsoid joint motion in MeshCat.""" if not VISUALIZE: print("\nVisualization skipped (MeshcatVisualizer not available)") return - if not hasattr(pin, 'hppfcl'): + if not hasattr(pin, "hppfcl"): print("\nVisualization skipped (hpp-fcl not available)") print("Install with: conda install -c conda-forge hpp-fcl") print("Then recompile Pinocchio with -DBUILD_WITH_HPP_FCL_SUPPORT=ON") @@ -240,9 +231,11 @@ def visualize_ellipsoid_motion(): "ellipsoid_surface", 0, # Universe frame ellipsoid_shape, - pin.SE3.Identity() + pin.SE3.Identity(), ) - ellipsoid_geom.meshColor = np.array([0.8, 0.8, 0.8, 0.3]) # Semi-transparent gray + ellipsoid_geom.meshColor = np.array( + [0.8, 0.8, 0.8, 0.3] + ) # Semi-transparent gray geom_model.addGeometryObject(ellipsoid_geom) # 2. Add a small sphere to show the contact point on the ellipsoid @@ -251,7 +244,7 @@ def visualize_ellipsoid_motion(): "contact_point", model.getJointId("ellipsoid"), contact_sphere, - pin.SE3.Identity() # At the joint frame (on the ellipsoid surface) + pin.SE3.Identity(), # At the joint frame (on the ellipsoid surface) ) contact_geom.meshColor = np.array([1.0, 0.0, 0.0, 1.0]) # Red geom_model.addGeometryObject(contact_geom) @@ -262,7 +255,7 @@ def visualize_ellipsoid_motion(): "body_visual", model.getJointId("ellipsoid"), box, - pin.SE3(np.eye(3), np.array([0.0, 0.0, 0.15])) + pin.SE3(np.eye(3), np.array([0.0, 0.0, 0.15])), ) box_geom.meshColor = np.array([0.2, 0.6, 1.0, 1.0]) # Blue geom_model.addGeometryObject(box_geom) @@ -277,55 +270,50 @@ def visualize_ellipsoid_motion(): print(" - Red sphere: Contact point on the surface") print(" - Blue box: The rigid body attached to the joint") print("\nAnimating ellipsoid joint motion...") - print("Open http://127.0.0.1:7000/static/ in your browser to see the animation.") + print( + "Open http://127.0.0.1:7000/static/ in your browser to see the animation." + ) # Animate through different configurations import time + t = 0.0 dt = 0.02 for i in range(500): # Create a smooth trajectory on the ellipsoid - q = np.array([ - 0.8 * np.sin(1.0 * t), - 0.6 * np.sin(1.2 * t + 1.0), - 0.4 * np.sin(0.8 * t + 0.5) - ]) + q = np.array( + [ + 0.8 * np.sin(1.0 * t), + 0.6 * np.sin(1.2 * t + 1.0), + 0.4 * np.sin(0.8 * t + 0.5), + ] + ) viz.display(q) time.sleep(dt) t += dt if i % 50 == 0: - print(f" Frame {i}/500 - Configuration: [{q[0]:.3f}, {q[1]:.3f}, {q[2]:.3f}]") + print( + f" Frame {i}/500 - Configuration: [{q[0]:.3f}, {q[1]:.3f}, {q[2]:.3f}]" + ) # create extra motion that slides along the principal axes q0 for angle in np.linspace(0, 2 * np.pi, 100): - q = np.array([ - 0.5 * np.cos(angle), - 0.0, - 0.0 - ]) + q = np.array([0.5 * np.cos(angle), 0.0, 0.0]) viz.display(q) time.sleep(dt) # q1 axis for angle in np.linspace(0, 2 * np.pi, 100): - q = np.array([ - 0.0, - 0.3 * np.cos(angle), - 0.0 - ]) + q = np.array([0.0, 0.3 * np.cos(angle), 0.0]) viz.display(q) time.sleep(dt) # q2 axis for angle in np.linspace(0, 2 * np.pi, 100): - q = np.array([ - 0.0, - 0.0, - 3.14 * np.cos(angle) - ]) + q = np.array([0.0, 0.0, 3.14 * np.cos(angle)]) viz.display(q) time.sleep(dt) @@ -334,6 +322,7 @@ def visualize_ellipsoid_motion(): except Exception as e: print(f"\nVisualization error: {e}") import traceback + traceback.print_exc() @@ -342,7 +331,7 @@ def main(): compute_kinematics_example() compute_dynamics_example() visualize_ellipsoid_motion() - + print("\n" + "=" * 60) print("Examples completed!") print("=" * 60) diff --git a/include/pinocchio/bindings/python/multibody/joint/joints-datas.hpp b/include/pinocchio/bindings/python/multibody/joint/joints-datas.hpp index 0fd7b9c4f1..bf72d8f238 100644 --- a/include/pinocchio/bindings/python/multibody/joint/joints-datas.hpp +++ b/include/pinocchio/bindings/python/multibody/joint/joints-datas.hpp @@ -63,8 +63,7 @@ namespace pinocchio inline bp::class_ & expose_joint_data(bp::class_ & cl) { - return cl - .add_property("S", &JointDataEllipsoid::S) + return cl.add_property("S", &JointDataEllipsoid::S) .add_property("Sdot", &JointDataEllipsoid::M) .add_property("StU", &JointDataEllipsoid::StU) .add_property("joint_q", &JointDataEllipsoid::joint_q) From 68ba51ad840a6bbb417653959947a697add80133 Mon Sep 17 00:00:00 2001 From: Pierre Puchaud <40755537+Ipuch@users.noreply.github.com> Date: Wed, 12 Nov 2025 15:10:38 +0100 Subject: [PATCH 15/48] Update include/pinocchio/multibody/joint/joint-ellipsoid.hpp Co-authored-by: Joris Vaillant --- include/pinocchio/multibody/joint/joint-ellipsoid.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/pinocchio/multibody/joint/joint-ellipsoid.hpp b/include/pinocchio/multibody/joint/joint-ellipsoid.hpp index a87e8b29ca..31482972b3 100644 --- a/include/pinocchio/multibody/joint/joint-ellipsoid.hpp +++ b/include/pinocchio/multibody/joint/joint-ellipsoid.hpp @@ -150,7 +150,7 @@ namespace pinocchio { } - explicit JointModelEllipsoidTpl(const Scalar & a, const Scalar & b, const Scalar & c) + JointModelEllipsoidTpl(const Scalar & a, const Scalar & b, const Scalar & c) : radius_a(a) , radius_b(b) , radius_c(c) From 87fa8efac9601595455a5562fa134437a64929dd Mon Sep 17 00:00:00 2001 From: Pierre Puchaud <40755537+Ipuch@users.noreply.github.com> Date: Wed, 12 Nov 2025 15:11:32 +0100 Subject: [PATCH 16/48] refactor(joint-ellipsoid): remove useless typename Co-authored-by: Joris Vaillant --- include/pinocchio/multibody/joint/joint-ellipsoid.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/include/pinocchio/multibody/joint/joint-ellipsoid.hpp b/include/pinocchio/multibody/joint/joint-ellipsoid.hpp index 31482972b3..cf03c9ac42 100644 --- a/include/pinocchio/multibody/joint/joint-ellipsoid.hpp +++ b/include/pinocchio/multibody/joint/joint-ellipsoid.hpp @@ -364,9 +364,9 @@ namespace pinocchio template Vector3 computeTranslationAccelerations( - const typename Eigen::MatrixBase & qs, - const typename Eigen::MatrixBase & vs, - const typename Eigen::MatrixBase & as) const + const Eigen::MatrixBase & qs, + const Eigen::MatrixBase & vs, + const Eigen::MatrixBase & as) const { Vector3 a; Scalar c0, s0; From 45d8082eef936a4075b87a8669e9dac91cda2ca2 Mon Sep 17 00:00:00 2001 From: Pierre Puchaud <40755537+Ipuch@users.noreply.github.com> Date: Wed, 12 Nov 2025 15:15:42 +0100 Subject: [PATCH 17/48] refactor(joint-ellipsoid): remove useless typenames Co-authored-by: Joris Vaillant --- include/pinocchio/multibody/joint/joint-ellipsoid.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/pinocchio/multibody/joint/joint-ellipsoid.hpp b/include/pinocchio/multibody/joint/joint-ellipsoid.hpp index cf03c9ac42..61225b1cb3 100644 --- a/include/pinocchio/multibody/joint/joint-ellipsoid.hpp +++ b/include/pinocchio/multibody/joint/joint-ellipsoid.hpp @@ -336,8 +336,8 @@ namespace pinocchio template Vector3 computeTranslationVelocities( - const typename Eigen::MatrixBase & qs, - const typename Eigen::MatrixBase & vs) const + const Eigen::MatrixBase & qs, + const Eigen::MatrixBase & vs) const { Scalar c0, s0; SINCOS(qs(0), &s0, &c0); From a8a268e34b0c8b188b7c3fc4b8b666643857046d Mon Sep 17 00:00:00 2001 From: Pierre Puchaud <40755537+Ipuch@users.noreply.github.com> Date: Wed, 12 Nov 2025 15:16:13 +0100 Subject: [PATCH 18/48] refactor(joint-ellipsoid): remove useless typenames Co-authored-by: Joris Vaillant --- include/pinocchio/multibody/joint/joint-ellipsoid.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/pinocchio/multibody/joint/joint-ellipsoid.hpp b/include/pinocchio/multibody/joint/joint-ellipsoid.hpp index 61225b1cb3..fac1a3f86f 100644 --- a/include/pinocchio/multibody/joint/joint-ellipsoid.hpp +++ b/include/pinocchio/multibody/joint/joint-ellipsoid.hpp @@ -313,7 +313,7 @@ namespace pinocchio } template - Vector3 computeTranslations(const typename Eigen::MatrixBase & qs) const + Vector3 computeTranslations(const Eigen::MatrixBase & qs) const { Scalar c0, s0; SINCOS(qs(0), &s0, &c0); From c10f6ef394ab9b31d63fddc7a40846a7ebb649af Mon Sep 17 00:00:00 2001 From: ipuch Date: Thu, 13 Nov 2025 14:24:51 +0100 Subject: [PATCH 19/48] feat(joint-ellipsoid): computeBiais --- .../multibody/joint/joint-ellipsoid.hpp | 102 ++++++++++++++++++ 1 file changed, 102 insertions(+) diff --git a/include/pinocchio/multibody/joint/joint-ellipsoid.hpp b/include/pinocchio/multibody/joint/joint-ellipsoid.hpp index fac1a3f86f..4a840db037 100644 --- a/include/pinocchio/multibody/joint/joint-ellipsoid.hpp +++ b/include/pinocchio/multibody/joint/joint-ellipsoid.hpp @@ -565,6 +565,108 @@ namespace pinocchio data.Sdot.matrix() << Sdot_11, Sdot_12, Sdot_13, Sdot_21, Sdot_22, Sdot_23, Sdot_31, Sdot_32, Sdot_33, Sdot_41, Sdot_42, Sdot_43, Sdot_51, Sdot_52, Sdot_53, Sdot_61, Sdot_62, Sdot_63; } + void computeBiais( + const Scalar & s0, + const Scalar & c0, + const Scalar & s1, + const Scalar & c1, + const Scalar & s2, + const Scalar & c2, + const Scalar & dndotx_dqdot1, + const Scalar & dndoty_dqdot0, + const Scalar & dndoty_dqdot1, + const Scalar & dndotz_dqdot0, + const Scalar & dndotz_dqdot1, + JointDataDerived & data) const + { + // Compute Sdot for bias acceleration + Scalar qdot0, qdot1, qdot2; + qdot0 = data.joint_v(0); + qdot1 = data.joint_v(1); + qdot2 = data.joint_v(2); + + // last columns and last element of the second column are zero, + // so we do not compute them + Scalar Sdot_11, Sdot_21, Sdot_31, Sdot_41, Sdot_51, Sdot_61; + Scalar Sdot_12, Sdot_22, Sdot_32, Sdot_42, Sdot_52; + + + // Derivative of dndotXX_dqdot0 with respect to q0 and q1 + Scalar d_dndotx_dqdot1_dq1 = -s1; // dndotx_dqdot1 = c1; + + Scalar d_dndoty_dqdot0_dq0 = s0 * c1; // dndoty_dqdot0 = - c0 * c1; + Scalar d_dndoty_dqdot0_dq1 = c0 * s1; + + // Scalar d_dndoty_dqdot1_dq0 = c0 * s1; // dndoty_dqdot1 = s0 * s1; + Scalar d_dndoty_dqdot1_dq1 = s0 * c1; + + Scalar d_dndotz_dqdot0_dq0 = -c1 * c0; // dndotz_dqdot0 = - c1 * s0; + Scalar d_dndotz_dqdot0_dq1 = s0 * s1; + + // Scalar d_dndotz_dqdot1_dq0 = s0 * s1; // dndotz_dqdot1 = - c0 * s1; + Scalar d_dndotz_dqdot1_dq1 = -c0 * c1; + + // Upper part (translation) + // Row 1, Column 1 + Sdot_11 = + qdot0 + * (-dndoty_dqdot0 * radius_b * (-c0 * c2 * s1 + s0 * s2) + dndotz_dqdot0 * radius_c * (c0 * s2 + c2 * s0 * s1) + radius_b * (c0 * s2 + c2 * s0 * s1) * d_dndoty_dqdot0_dq0 + radius_c * (-c0 * c2 * s1 + s0 * s2) * d_dndotz_dqdot0_dq0) + + qdot1 + * (dndoty_dqdot0 * radius_b * c1 * c2 * s0 - dndotz_dqdot0 * radius_c * c0 * c1 * c2 + radius_b * (c0 * s2 + c2 * s0 * s1) * d_dndoty_dqdot0_dq1 + radius_c * (-c0 * c2 * s1 + s0 * s2) * d_dndotz_dqdot0_dq1) + - qdot2 + * (dndoty_dqdot0 * radius_b * (-c0 * c2 + s0 * s1 * s2) - dndotz_dqdot0 * radius_c * (c0 * s1 * s2 + c2 * s0)); + + // Row 1, Column 2 + Sdot_12 = + qdot0 + * (-dndoty_dqdot1 * radius_b * (-c0 * c2 * s1 + s0 * s2) + dndotz_dqdot1 * radius_c * (c0 * s2 + c2 * s0 * s1) + radius_b * (c0 * s2 + c2 * s0 * s1) * d_dndoty_dqdot0_dq1 + radius_c * (-c0 * c2 * s1 + s0 * s2) * d_dndotz_dqdot0_dq1) + + qdot1 * (-dndotx_dqdot1 * radius_a * c2 * s1 + dndoty_dqdot1 * radius_b * c1 * c2 * s0 - dndotz_dqdot1 * radius_c * c0 * c1 * c2 + radius_a * c1 * c2 * d_dndotx_dqdot1_dq1 + radius_b * (c0 * s2 + c2 * s0 * s1) * d_dndoty_dqdot1_dq1 + radius_c * (-c0 * c2 * s1 + s0 * s2) * d_dndotz_dqdot1_dq1) - qdot2 * (dndotx_dqdot1 * radius_a * c1 * s2 + dndoty_dqdot1 * radius_b * (-c0 * c2 + s0 * s1 * s2) - dndotz_dqdot1 * radius_c * (c0 * s1 * s2 + c2 * s0)); + + // Row 2, Column 1 + Sdot_21 = + -qdot0 + * (dndoty_dqdot0 * radius_b * (c0 * s1 * s2 + c2 * s0) + dndotz_dqdot0 * radius_c * (-c0 * c2 + s0 * s1 * s2) + radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot0_dq0 - radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot0_dq0) + - qdot1 + * (dndoty_dqdot0 * radius_b * c1 * s0 * s2 - dndotz_dqdot0 * radius_c * c0 * c1 * s2 + radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot0_dq1 - radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot0_dq1) + - qdot2 + * (dndoty_dqdot0 * radius_b * (c0 * s2 + c2 * s0 * s1) + dndotz_dqdot0 * radius_c * (-c0 * c2 * s1 + s0 * s2)); + + // Row 2, Column 2 + Sdot_22 = + -qdot0 + * (dndoty_dqdot1 * radius_b * (c0 * s1 * s2 + c2 * s0) + dndotz_dqdot1 * radius_c * (-c0 * c2 + s0 * s1 * s2) + radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot0_dq1 - radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot0_dq1) + + qdot1 * (dndotx_dqdot1 * radius_a * s1 * s2 - dndoty_dqdot1 * radius_b * c1 * s0 * s2 + dndotz_dqdot1 * radius_c * c0 * c1 * s2 - radius_a * c1 * s2 * d_dndotx_dqdot1_dq1 - radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot1_dq1 + radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot1_dq1) - qdot2 * (dndotx_dqdot1 * radius_a * c1 * c2 + dndoty_dqdot1 * radius_b * (c0 * s2 + c2 * s0 * s1) + dndotz_dqdot1 * radius_c * (-c0 * c2 * s1 + s0 * s2)); + + + // Row 3, Column 1 + Sdot_31 = + -qdot0 * c1 + * (dndoty_dqdot0 * radius_b * c0 + dndotz_dqdot0 * radius_c * s0 + radius_b * s0 * d_dndoty_dqdot0_dq0 - radius_c * c0 * d_dndotz_dqdot0_dq0) + + qdot1 + * (-c1 * (radius_b * s0 * d_dndoty_dqdot0_dq1 - radius_c * c0 * d_dndotz_dqdot0_dq1) + s1 * (dndoty_dqdot0 * radius_b * s0 - dndotz_dqdot0 * radius_c * c0)); + + // Row 3, Column 2 + Sdot_32 = + -qdot0 * c1 + * (dndoty_dqdot1 * radius_b * c0 + dndotz_dqdot1 * radius_c * s0 + radius_b * s0 * d_dndoty_dqdot0_dq1 - radius_c * c0 * d_dndotz_dqdot0_dq1) + + qdot1 + * (dndotx_dqdot1 * radius_a * c1 + dndoty_dqdot1 * radius_b * s0 * s1 - dndotz_dqdot1 * radius_c * c0 * s1 + radius_a * s1 * d_dndotx_dqdot1_dq1 - radius_b * c1 * s0 * d_dndoty_dqdot1_dq1 + radius_c * c0 * c1 * d_dndotz_dqdot1_dq1); + + // Angular part (rows 4-6) + Sdot_41 = -(qdot1 * c2 * s1 + qdot2 * c1 * s2); + Sdot_51 = qdot1 * s1 * s2 - qdot2 * c1 * c2; + Sdot_61 = qdot1 * c1; + + Sdot_42 = qdot2 * c2; + Sdot_52 = -qdot2 * s2; + + data.c.toVector() << Sdot_11 * qdot0 + Sdot_12 * qdot1, + Sdot_21 * qdot0 + Sdot_22 * qdot1, + Sdot_31 * qdot0 + Sdot_32 * qdot1, + Sdot_41 * qdot0 + Sdot_42 * qdot1, + Sdot_51 * qdot0 + Sdot_52 * qdot1, + Sdot_61 * qdot0; + } }; // struct JointModelEllipsoidTpl } // namespace pinocchio From 41c76617950643ac2c7766f2b7125f7ae41d8d9f Mon Sep 17 00:00:00 2001 From: Lucas Joseph Date: Thu, 13 Nov 2025 14:51:09 +0100 Subject: [PATCH 20/48] Factorized data.M computation Preventing clang-format on large matrices --- .../multibody/joint/joint-ellipsoid.hpp | 107 +++++++++--------- 1 file changed, 54 insertions(+), 53 deletions(-) diff --git a/include/pinocchio/multibody/joint/joint-ellipsoid.hpp b/include/pinocchio/multibody/joint/joint-ellipsoid.hpp index 4a840db037..48074fdf14 100644 --- a/include/pinocchio/multibody/joint/joint-ellipsoid.hpp +++ b/include/pinocchio/multibody/joint/joint-ellipsoid.hpp @@ -167,6 +167,22 @@ namespace pinocchio return {true, true, true}; } + void computeSpatialTransform( + Scalar c0, Scalar s0, Scalar c1, Scalar s1, Scalar c2, Scalar s2, JointDataDerived & data) const + { + // clang-format off + data.M.rotation() << c1 * c2, -c1 * s2, s1, c0 * s2 + c2 * s0 * s1, c0 * c2 - s0 * s1 * s2, + -c1 * s0, -c0 * c2 * s1 + s0 * s2, c0 * s1 * s2 + c2 * s0, c0 * c1; + // clang-format on + Scalar nx, ny, nz; + nx = s1; + ny = -s0 * c1; + nz = c0 * c1; + + data.M.translation() << radius_a * nx, radius_b * ny, radius_c * nz; + + } + template void calc(JointDataDerived & data, const typename Eigen::MatrixBase & qs) const { @@ -179,15 +195,7 @@ namespace pinocchio Scalar c2, s2; SINCOS(data.joint_q(2), &s2, &c2); - data.M.rotation() << c1 * c2, -c1 * s2, s1, c0 * s2 + c2 * s0 * s1, c0 * c2 - s0 * s1 * s2, - -c1 * s0, -c0 * c2 * s1 + s0 * s2, c0 * s1 * s2 + c2 * s0, c0 * c1; - - Scalar nx, ny, nz; - nx = s1; - ny = -s0 * c1; - nz = c0 * c1; - - data.M.translation() << radius_a * nx, radius_b * ny, radius_c * nz; + computeSpatialTransform(c0,s0,c1,s1,c2,s2, data); computeMotionSubspace(s0, c0, s1, c1, s2, c2, data); } @@ -243,16 +251,8 @@ namespace pinocchio Scalar c2, s2; SINCOS(data.joint_q(2), &s2, &c2); - data.M.rotation() << c1 * c2, -c1 * s2, s1, c0 * s2 + c2 * s0 * s1, c0 * c2 - s0 * s1 * s2, - -c1 * s0, -c0 * c2 * s1 + s0 * s2, c0 * s1 * s2 + c2 * s0, c0 * c1; - - Scalar nx, ny, nz; - nx = s1; - ny = -s0 * c1; - nz = c0 * c1; - - data.M.translation() << radius_a * nx, radius_b * ny, radius_c * nz; - + computeSpatialTransform(c0, s0, c1, s1, c2, s2, data); + Scalar dndotx_dqdot1, dndoty_dqdot0, dndoty_dqdot1, dndotz_dqdot0, dndotz_dqdot1; dndotx_dqdot1 = c1; dndoty_dqdot0 = -c0 * c1; @@ -433,7 +433,7 @@ namespace pinocchio JointDataDerived & data) const { Scalar S_11, S_21, S_31, S_12, S_22, S_32; - + // clang-format off S_11 = dndoty_dqdot0 * radius_b * (c0 * s2 + c2 * s0 * s1) + dndotz_dqdot0 * radius_c * (-c0 * c2 * s1 + s0 * s2); @@ -455,6 +455,7 @@ namespace pinocchio data.S.matrix() << S_11, S_12, Scalar(0), S_21, S_22, Scalar(0), S_31, S_32, Scalar(0), c1 * c2, s2, Scalar(0), -c1 * s2, c2, Scalar(0), s1, Scalar(0), Scalar(1); + // clang-format on } void computeMotionSubspaceDerivative( const Scalar & s0, @@ -567,16 +568,16 @@ namespace pinocchio } void computeBiais( const Scalar & s0, - const Scalar & c0, const Scalar & s1, - const Scalar & c1, + const Scalar & c0, const Scalar & s2, - const Scalar & c2, + const Scalar & c1, const Scalar & dndotx_dqdot1, + const Scalar & c2, const Scalar & dndoty_dqdot0, const Scalar & dndoty_dqdot1, - const Scalar & dndotz_dqdot0, const Scalar & dndotz_dqdot1, + const Scalar & dndotz_dqdot0, JointDataDerived & data) const { // Compute Sdot for bias acceleration @@ -585,13 +586,13 @@ namespace pinocchio qdot1 = data.joint_v(1); qdot2 = data.joint_v(2); - // last columns and last element of the second column are zero, // so we do not compute them + // last columns and last element of the second column are zero, Scalar Sdot_11, Sdot_21, Sdot_31, Sdot_41, Sdot_51, Sdot_61; Scalar Sdot_12, Sdot_22, Sdot_32, Sdot_42, Sdot_52; - // Derivative of dndotXX_dqdot0 with respect to q0 and q1 + Scalar d_dndotx_dqdot1_dq1 = -s1; // dndotx_dqdot1 = c1; Scalar d_dndoty_dqdot0_dq0 = s0 * c1; // dndoty_dqdot0 = - c0 * c1; @@ -600,8 +601,8 @@ namespace pinocchio // Scalar d_dndoty_dqdot1_dq0 = c0 * s1; // dndoty_dqdot1 = s0 * s1; Scalar d_dndoty_dqdot1_dq1 = s0 * c1; - Scalar d_dndotz_dqdot0_dq0 = -c1 * c0; // dndotz_dqdot0 = - c1 * s0; Scalar d_dndotz_dqdot0_dq1 = s0 * s1; + Scalar d_dndotz_dqdot0_dq0 = -c1 * c0; // dndotz_dqdot0 = - c1 * s0; // Scalar d_dndotz_dqdot1_dq0 = s0 * s1; // dndotz_dqdot1 = - c0 * s1; Scalar d_dndotz_dqdot1_dq1 = -c0 * c1; @@ -615,58 +616,58 @@ namespace pinocchio * (dndoty_dqdot0 * radius_b * c1 * c2 * s0 - dndotz_dqdot0 * radius_c * c0 * c1 * c2 + radius_b * (c0 * s2 + c2 * s0 * s1) * d_dndoty_dqdot0_dq1 + radius_c * (-c0 * c2 * s1 + s0 * s2) * d_dndotz_dqdot0_dq1) - qdot2 * (dndoty_dqdot0 * radius_b * (-c0 * c2 + s0 * s1 * s2) - dndotz_dqdot0 * radius_c * (c0 * s1 * s2 + c2 * s0)); - - // Row 1, Column 2 Sdot_12 = + // Row 1, Column 2 + qdot0 * (-dndoty_dqdot1 * radius_b * (-c0 * c2 * s1 + s0 * s2) + dndotz_dqdot1 * radius_c * (c0 * s2 + c2 * s0 * s1) + radius_b * (c0 * s2 + c2 * s0 * s1) * d_dndoty_dqdot0_dq1 + radius_c * (-c0 * c2 * s1 + s0 * s2) * d_dndotz_dqdot0_dq1) - + qdot1 * (-dndotx_dqdot1 * radius_a * c2 * s1 + dndoty_dqdot1 * radius_b * c1 * c2 * s0 - dndotz_dqdot1 * radius_c * c0 * c1 * c2 + radius_a * c1 * c2 * d_dndotx_dqdot1_dq1 + radius_b * (c0 * s2 + c2 * s0 * s1) * d_dndoty_dqdot1_dq1 + radius_c * (-c0 * c2 * s1 + s0 * s2) * d_dndotz_dqdot1_dq1) - qdot2 * (dndotx_dqdot1 * radius_a * c1 * s2 + dndoty_dqdot1 * radius_b * (-c0 * c2 + s0 * s1 * s2) - dndotz_dqdot1 * radius_c * (c0 * s1 * s2 + c2 * s0)); + + qdot1 * (-dndotx_dqdot1 * radius_a * c2 * s1 + dndoty_dqdot1 * radius_b * c1 * c2 * s0 - dndotz_dqdot1 * radius_c * c0 * c1 * c2 + radius_a * c1 * c2 * d_dndotx_dqdot1_dq1 + radius_b * (c0 * s2 + c2 * s0 * s1) * d_dndoty_dqdot1_dq1 + radius_c * (-c0 * c2 * s1 + s0 * s2) * d_dndotz_dqdot1_dq1) - qdot2 * (dndotx_dqdot1 * radius_a * c1 * s2 + dndoty_dqdot1 * radius_b * (-c0 * c2 + s0 * s1 * s2) - dndotz_dqdot1 * radius_c * (c0 * s1 * s2 + c2 * s0)); // Row 2, Column 1 Sdot_21 = - -qdot0 * (dndoty_dqdot0 * radius_b * (c0 * s1 * s2 + c2 * s0) + dndotz_dqdot0 * radius_c * (-c0 * c2 + s0 * s1 * s2) + radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot0_dq0 - radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot0_dq0) - - qdot1 * (dndoty_dqdot0 * radius_b * c1 * s0 * s2 - dndotz_dqdot0 * radius_c * c0 * c1 * s2 + radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot0_dq1 - radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot0_dq1) - - qdot2 - * (dndoty_dqdot0 * radius_b * (c0 * s2 + c2 * s0 * s1) + dndotz_dqdot0 * radius_c * (-c0 * c2 * s1 + s0 * s2)); - - // Row 2, Column 2 - Sdot_22 = - -qdot0 - * (dndoty_dqdot1 * radius_b * (c0 * s1 * s2 + c2 * s0) + dndotz_dqdot1 * radius_c * (-c0 * c2 + s0 * s1 * s2) + radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot0_dq1 - radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot0_dq1) - + qdot1 * (dndotx_dqdot1 * radius_a * s1 * s2 - dndoty_dqdot1 * radius_b * c1 * s0 * s2 + dndotz_dqdot1 * radius_c * c0 * c1 * s2 - radius_a * c1 * s2 * d_dndotx_dqdot1_dq1 - radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot1_dq1 + radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot1_dq1) - qdot2 * (dndotx_dqdot1 * radius_a * c1 * c2 + dndoty_dqdot1 * radius_b * (c0 * s2 + c2 * s0 * s1) + dndotz_dqdot1 * radius_c * (-c0 * c2 * s1 + s0 * s2)); - // Row 3, Column 1 Sdot_31 = -qdot0 * c1 + // Row 3, Column 1 + + * (dndoty_dqdot1 * radius_b * (c0 * s1 * s2 + c2 * s0) + dndotz_dqdot1 * radius_c * (-c0 * c2 + s0 * s1 * s2) + radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot0_dq1 - radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot0_dq1) + Sdot_22 = + // Row 2, Column 2 + -qdot0 + - qdot2 + * (dndoty_dqdot0 * radius_b * (c0 * s2 + c2 * s0 * s1) + dndotz_dqdot0 * radius_c * (-c0 * c2 * s1 + s0 * s2)); + - qdot1 + -qdot0 + + qdot1 * (dndotx_dqdot1 * radius_a * s1 * s2 - dndoty_dqdot1 * radius_b * c1 * s0 * s2 + dndotz_dqdot1 * radius_c * c0 * c1 * s2 - radius_a * c1 * s2 * d_dndotx_dqdot1_dq1 - radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot1_dq1 + radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot1_dq1) - qdot2 * (dndotx_dqdot1 * radius_a * c1 * c2 + dndoty_dqdot1 * radius_b * (c0 * s2 + c2 * s0 * s1) + dndotz_dqdot1 * radius_c * (-c0 * c2 * s1 + s0 * s2)); * (dndoty_dqdot0 * radius_b * c0 + dndotz_dqdot0 * radius_c * s0 + radius_b * s0 * d_dndoty_dqdot0_dq0 - radius_c * c0 * d_dndotz_dqdot0_dq0) - + qdot1 * (-c1 * (radius_b * s0 * d_dndoty_dqdot0_dq1 - radius_c * c0 * d_dndotz_dqdot0_dq1) + s1 * (dndoty_dqdot0 * radius_b * s0 - dndotz_dqdot0 * radius_c * c0)); - - // Row 3, Column 2 - Sdot_32 = - -qdot0 * c1 + Sdot_61 = qdot1 * c1; * (dndoty_dqdot1 * radius_b * c0 + dndotz_dqdot1 * radius_c * s0 + radius_b * s0 * d_dndoty_dqdot0_dq1 - radius_c * c0 * d_dndotz_dqdot0_dq1) + -qdot0 * c1 + Sdot_32 = + // Row 3, Column 2 + + + qdot1 + qdot1 - * (dndotx_dqdot1 * radius_a * c1 + dndoty_dqdot1 * radius_b * s0 * s1 - dndotz_dqdot1 * radius_c * c0 * s1 + radius_a * s1 * d_dndotx_dqdot1_dq1 - radius_b * c1 * s0 * d_dndoty_dqdot1_dq1 + radius_c * c0 * c1 * d_dndotz_dqdot1_dq1); + * (dndotx_dqdot1 * radius_a * c1 + dndoty_dqdot1 * radius_b * s0 * s1 - dndotz_dqdot1 * radius_c * c0 * s1 + radius_a * s1 * d_dndotx_dqdot1_dq1 - radius_b * c1 * s0 * d_dndoty_dqdot1_dq1 + radius_c * c0 * c1 * d_dndotz_dqdot1_dq1); // Angular part (rows 4-6) - Sdot_41 = -(qdot1 * c2 * s1 + qdot2 * c1 * s2); Sdot_51 = qdot1 * s1 * s2 - qdot2 * c1 * c2; - Sdot_61 = qdot1 * c1; + Sdot_41 = -(qdot1 * c2 * s1 + qdot2 * c1 * s2); Sdot_42 = qdot2 * c2; - Sdot_52 = -qdot2 * s2; - data.c.toVector() << Sdot_11 * qdot0 + Sdot_12 * qdot1, + + Sdot_52 = -qdot2 * s2; Sdot_21 * qdot0 + Sdot_22 * qdot1, Sdot_31 * qdot0 + Sdot_32 * qdot1, - Sdot_41 * qdot0 + Sdot_42 * qdot1, Sdot_51 * qdot0 + Sdot_52 * qdot1, - Sdot_61 * qdot0; } + Sdot_61 * qdot0; + Sdot_41 * qdot0 + Sdot_42 * qdot1, }; // struct JointModelEllipsoidTpl } // namespace pinocchio From 6e5d528d716364df142edf3856bb129b6374cb41 Mon Sep 17 00:00:00 2001 From: ipuch Date: Thu, 13 Nov 2025 14:57:30 +0100 Subject: [PATCH 21/48] fix(joint-ellispoid) : compute_biais --- .../multibody/joint/joint-ellipsoid.hpp | 67 ++++++++++--------- 1 file changed, 34 insertions(+), 33 deletions(-) diff --git a/include/pinocchio/multibody/joint/joint-ellipsoid.hpp b/include/pinocchio/multibody/joint/joint-ellipsoid.hpp index 48074fdf14..24783279ff 100644 --- a/include/pinocchio/multibody/joint/joint-ellipsoid.hpp +++ b/include/pinocchio/multibody/joint/joint-ellipsoid.hpp @@ -567,17 +567,18 @@ namespace pinocchio Sdot_33, Sdot_41, Sdot_42, Sdot_43, Sdot_51, Sdot_52, Sdot_53, Sdot_61, Sdot_62, Sdot_63; } void computeBiais( + void computeBiais( const Scalar & s0, - const Scalar & s1, const Scalar & c0, - const Scalar & s2, + const Scalar & s1, const Scalar & c1, - const Scalar & dndotx_dqdot1, + const Scalar & s2, const Scalar & c2, + const Scalar & dndotx_dqdot1, const Scalar & dndoty_dqdot0, const Scalar & dndoty_dqdot1, - const Scalar & dndotz_dqdot1, const Scalar & dndotz_dqdot0, + const Scalar & dndotz_dqdot1, JointDataDerived & data) const { // Compute Sdot for bias acceleration @@ -586,13 +587,13 @@ namespace pinocchio qdot1 = data.joint_v(1); qdot2 = data.joint_v(2); - // so we do not compute them // last columns and last element of the second column are zero, + // so we do not compute them Scalar Sdot_11, Sdot_21, Sdot_31, Sdot_41, Sdot_51, Sdot_61; Scalar Sdot_12, Sdot_22, Sdot_32, Sdot_42, Sdot_52; - // Derivative of dndotXX_dqdot0 with respect to q0 and q1 + // Derivative of dndotXX_dqdot0 with respect to q0 and q1 Scalar d_dndotx_dqdot1_dq1 = -s1; // dndotx_dqdot1 = c1; Scalar d_dndoty_dqdot0_dq0 = s0 * c1; // dndoty_dqdot0 = - c0 * c1; @@ -601,8 +602,8 @@ namespace pinocchio // Scalar d_dndoty_dqdot1_dq0 = c0 * s1; // dndoty_dqdot1 = s0 * s1; Scalar d_dndoty_dqdot1_dq1 = s0 * c1; - Scalar d_dndotz_dqdot0_dq1 = s0 * s1; Scalar d_dndotz_dqdot0_dq0 = -c1 * c0; // dndotz_dqdot0 = - c1 * s0; + Scalar d_dndotz_dqdot0_dq1 = s0 * s1; // Scalar d_dndotz_dqdot1_dq0 = s0 * s1; // dndotz_dqdot1 = - c0 * s1; Scalar d_dndotz_dqdot1_dq1 = -c0 * c1; @@ -616,58 +617,58 @@ namespace pinocchio * (dndoty_dqdot0 * radius_b * c1 * c2 * s0 - dndotz_dqdot0 * radius_c * c0 * c1 * c2 + radius_b * (c0 * s2 + c2 * s0 * s1) * d_dndoty_dqdot0_dq1 + radius_c * (-c0 * c2 * s1 + s0 * s2) * d_dndotz_dqdot0_dq1) - qdot2 * (dndoty_dqdot0 * radius_b * (-c0 * c2 + s0 * s1 * s2) - dndotz_dqdot0 * radius_c * (c0 * s1 * s2 + c2 * s0)); - Sdot_12 = - // Row 1, Column 2 + // Row 1, Column 2 + Sdot_12 = qdot0 * (-dndoty_dqdot1 * radius_b * (-c0 * c2 * s1 + s0 * s2) + dndotz_dqdot1 * radius_c * (c0 * s2 + c2 * s0 * s1) + radius_b * (c0 * s2 + c2 * s0 * s1) * d_dndoty_dqdot0_dq1 + radius_c * (-c0 * c2 * s1 + s0 * s2) * d_dndotz_dqdot0_dq1) - + qdot1 * (-dndotx_dqdot1 * radius_a * c2 * s1 + dndoty_dqdot1 * radius_b * c1 * c2 * s0 - dndotz_dqdot1 * radius_c * c0 * c1 * c2 + radius_a * c1 * c2 * d_dndotx_dqdot1_dq1 + radius_b * (c0 * s2 + c2 * s0 * s1) * d_dndoty_dqdot1_dq1 + radius_c * (-c0 * c2 * s1 + s0 * s2) * d_dndotz_dqdot1_dq1) - qdot2 * (dndotx_dqdot1 * radius_a * c1 * s2 + dndoty_dqdot1 * radius_b * (-c0 * c2 + s0 * s1 * s2) - dndotz_dqdot1 * radius_c * (c0 * s1 * s2 + c2 * s0)); + // Row 2, Column 1 Sdot_21 = + -qdot0 * (dndoty_dqdot0 * radius_b * (c0 * s1 * s2 + c2 * s0) + dndotz_dqdot0 * radius_c * (-c0 * c2 + s0 * s1 * s2) + radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot0_dq0 - radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot0_dq0) + - qdot1 * (dndoty_dqdot0 * radius_b * c1 * s0 * s2 - dndotz_dqdot0 * radius_c * c0 * c1 * s2 + radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot0_dq1 - radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot0_dq1) - - - Sdot_31 = - -qdot0 * c1 - // Row 3, Column 1 - - * (dndoty_dqdot1 * radius_b * (c0 * s1 * s2 + c2 * s0) + dndotz_dqdot1 * radius_c * (-c0 * c2 + s0 * s1 * s2) + radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot0_dq1 - radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot0_dq1) - Sdot_22 = - // Row 2, Column 2 - -qdot0 - qdot2 * (dndoty_dqdot0 * radius_b * (c0 * s2 + c2 * s0 * s1) + dndotz_dqdot0 * radius_c * (-c0 * c2 * s1 + s0 * s2)); - - qdot1 + + // Row 2, Column 2 + Sdot_22 = -qdot0 + * (dndoty_dqdot1 * radius_b * (c0 * s1 * s2 + c2 * s0) + dndotz_dqdot1 * radius_c * (-c0 * c2 + s0 * s1 * s2) + radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot0_dq1 - radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot0_dq1) + qdot1 * (dndotx_dqdot1 * radius_a * s1 * s2 - dndoty_dqdot1 * radius_b * c1 * s0 * s2 + dndotz_dqdot1 * radius_c * c0 * c1 * s2 - radius_a * c1 * s2 * d_dndotx_dqdot1_dq1 - radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot1_dq1 + radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot1_dq1) - qdot2 * (dndotx_dqdot1 * radius_a * c1 * c2 + dndoty_dqdot1 * radius_b * (c0 * s2 + c2 * s0 * s1) + dndotz_dqdot1 * radius_c * (-c0 * c2 * s1 + s0 * s2)); + + + // Row 3, Column 1 + Sdot_31 = + -qdot0 * c1 * (dndoty_dqdot0 * radius_b * c0 + dndotz_dqdot0 * radius_c * s0 + radius_b * s0 * d_dndoty_dqdot0_dq0 - radius_c * c0 * d_dndotz_dqdot0_dq0) + + qdot1 * (-c1 * (radius_b * s0 * d_dndoty_dqdot0_dq1 - radius_c * c0 * d_dndotz_dqdot0_dq1) + s1 * (dndoty_dqdot0 * radius_b * s0 - dndotz_dqdot0 * radius_c * c0)); - Sdot_61 = qdot1 * c1; - * (dndoty_dqdot1 * radius_b * c0 + dndotz_dqdot1 * radius_c * s0 + radius_b * s0 * d_dndoty_dqdot0_dq1 - radius_c * c0 * d_dndotz_dqdot0_dq1) - -qdot0 * c1 - Sdot_32 = - // Row 3, Column 2 + // Row 3, Column 2 + Sdot_32 = + -qdot0 * c1 + * (dndoty_dqdot1 * radius_b * c0 + dndotz_dqdot1 * radius_c * s0 + radius_b * s0 * d_dndoty_dqdot0_dq1 - radius_c * c0 * d_dndotz_dqdot0_dq1) + qdot1 - + qdot1 - * (dndotx_dqdot1 * radius_a * c1 + dndoty_dqdot1 * radius_b * s0 * s1 - dndotz_dqdot1 * radius_c * c0 * s1 + radius_a * s1 * d_dndotx_dqdot1_dq1 - radius_b * c1 * s0 * d_dndoty_dqdot1_dq1 + radius_c * c0 * c1 * d_dndotz_dqdot1_dq1); + // Angular part (rows 4-6) - Sdot_51 = qdot1 * s1 * s2 - qdot2 * c1 * c2; Sdot_41 = -(qdot1 * c2 * s1 + qdot2 * c1 * s2); + Sdot_51 = qdot1 * s1 * s2 - qdot2 * c1 * c2; + Sdot_61 = qdot1 * c1; Sdot_42 = qdot2 * c2; - data.c.toVector() << Sdot_11 * qdot0 + Sdot_12 * qdot1, - Sdot_52 = -qdot2 * s2; + + data.c.toVector() << Sdot_11 * qdot0 + Sdot_12 * qdot1, Sdot_21 * qdot0 + Sdot_22 * qdot1, Sdot_31 * qdot0 + Sdot_32 * qdot1, + Sdot_41 * qdot0 + Sdot_42 * qdot1, Sdot_51 * qdot0 + Sdot_52 * qdot1, - } Sdot_61 * qdot0; - Sdot_41 * qdot0 + Sdot_42 * qdot1, + } }; // struct JointModelEllipsoidTpl } // namespace pinocchio From f2910776b2f32bd41931adeced2f67b3fdd4f1a5 Mon Sep 17 00:00:00 2001 From: ipuch Date: Thu, 13 Nov 2025 15:11:30 +0100 Subject: [PATCH 22/48] refactor(joint-ellipsoid) : c instead of Sdot * qdot --- .../multibody/joint/joint-ellipsoid.hpp | 63 ++++++++++++++++++- unittest/joint-ellipsoid.cpp | 29 +++++++++ 2 files changed, 89 insertions(+), 3 deletions(-) diff --git a/include/pinocchio/multibody/joint/joint-ellipsoid.hpp b/include/pinocchio/multibody/joint/joint-ellipsoid.hpp index 24783279ff..ebe13b9624 100644 --- a/include/pinocchio/multibody/joint/joint-ellipsoid.hpp +++ b/include/pinocchio/multibody/joint/joint-ellipsoid.hpp @@ -233,7 +233,10 @@ namespace pinocchio s0, c0, s1, c1, s2, c2, dndotx_dqdot1, dndoty_dqdot0, dndoty_dqdot1, dndotz_dqdot0, dndotz_dqdot1, data); - data.c.toVector().noalias() = data.Sdot.matrix() * data.joint_v; + // data.c.toVector().noalias() = data.Sdot.matrix() * data.joint_v; + computeBiais( + s0, c0, s1, c1, s2, c2, dndotx_dqdot1, dndoty_dqdot0, dndoty_dqdot1, dndotz_dqdot0, + dndotz_dqdot1, data); } template @@ -272,7 +275,10 @@ namespace pinocchio s0, c0, s1, c1, s2, c2, dndotx_dqdot1, dndoty_dqdot0, dndoty_dqdot1, dndotz_dqdot0, dndotz_dqdot1, data); - data.c.toVector().noalias() = data.Sdot.matrix() * data.joint_v; + // data.c.toVector().noalias() = data.Sdot.matrix() * data.joint_v; + computeBiais( + s0, c0, s1, c1, s2, c2, dndotx_dqdot1, dndoty_dqdot0, dndoty_dqdot1, dndotz_dqdot0, + dndotz_dqdot1, data); } template @@ -457,6 +463,32 @@ namespace pinocchio c1 * c2, s2, Scalar(0), -c1 * s2, c2, Scalar(0), s1, Scalar(0), Scalar(1); // clang-format on } + + template + void computeMotionSubspaceDerivative( + JointDataDerived & data, + const Eigen::MatrixBase & qs, + const Eigen::MatrixBase & vs) const + { + Scalar c0, s0; + SINCOS(qs(0), &s0, &c0); + Scalar c1, s1; + SINCOS(qs(1), &s1, &c1); + Scalar c2, s2; + SINCOS(qs(2), &s2, &c2); + + Scalar dndotx_dqdot1, dndoty_dqdot0, dndoty_dqdot1, dndotz_dqdot0, dndotz_dqdot1; + dndotx_dqdot1 = c1; + dndoty_dqdot0 = -c0 * c1; + dndoty_dqdot1 = s0 * s1; + dndotz_dqdot0 = -c1 * s0; + dndotz_dqdot1 = -c0 * s1; + + computeMotionSubspaceDerivative( + s0, c0, s1, c1, s2, c2, dndotx_dqdot1, dndoty_dqdot0, dndoty_dqdot1, dndotz_dqdot0, + dndotz_dqdot1, data); + } + void computeMotionSubspaceDerivative( const Scalar & s0, const Scalar & c0, @@ -566,8 +598,33 @@ namespace pinocchio data.Sdot.matrix() << Sdot_11, Sdot_12, Sdot_13, Sdot_21, Sdot_22, Sdot_23, Sdot_31, Sdot_32, Sdot_33, Sdot_41, Sdot_42, Sdot_43, Sdot_51, Sdot_52, Sdot_53, Sdot_61, Sdot_62, Sdot_63; } + + template + void computeBiais( + JointDataDerived & data, + const Eigen::MatrixBase & qs, + const Eigen::MatrixBase & vs) const + { + Scalar c0, s0; + SINCOS(qs(0), &s0, &c0); + Scalar c1, s1; + SINCOS(qs(1), &s1, &c1); + Scalar c2, s2; + SINCOS(qs(2), &s2, &c2); + + Scalar dndotx_dqdot1, dndoty_dqdot0, dndoty_dqdot1, dndotz_dqdot0, dndotz_dqdot1; + dndotx_dqdot1 = c1; + dndoty_dqdot0 = -c0 * c1; + dndoty_dqdot1 = s0 * s1; + dndotz_dqdot0 = -c1 * s0; + dndotz_dqdot1 = -c0 * s1; + + computeBiais( + s0, c0, s1, c1, s2, c2, dndotx_dqdot1, dndoty_dqdot0, dndoty_dqdot1, dndotz_dqdot0, + dndotz_dqdot1, data); + } + void computeBiais( - void computeBiais( const Scalar & s0, const Scalar & c0, const Scalar & s1, diff --git a/unittest/joint-ellipsoid.cpp b/unittest/joint-ellipsoid.cpp index d8e772b711..5e37f1e7f5 100644 --- a/unittest/joint-ellipsoid.cpp +++ b/unittest/joint-ellipsoid.cpp @@ -353,4 +353,33 @@ BOOST_AUTO_TEST_CASE(testSdotFiniteDifferences) BOOST_CHECK(Sdot_ref.isApprox(Sdot_fd, sqrt(eps))); } +BOOST_AUTO_TEST_CASE(testBiaisVsSdotTimesVelocity) +{ + using namespace pinocchio; + + // Ellipsoid parameters + double radius_a = 2.0; + double radius_b = 1.5; + double radius_c = 1.0; + + JointModelEllipsoid jmodel(radius_a, radius_b, radius_c); + jmodel.setIndexes(0, 0, 0); + + JointDataEllipsoid jdata = jmodel.createData(); + + // Test configuration and velocity + typedef JointModelEllipsoid::ConfigVector_t ConfigVector_t; + typedef JointModelEllipsoid::TangentVector_t TangentVector_t; + typedef LieGroup::type LieGroupType; + + ConfigVector_t q = LieGroupType().random(); + TangentVector_t v = TangentVector_t::Random(); + jmodel.calc(jdata, q, v); + + jmodel.computeBiais(jdata, q, v); + jmodel.computeMotionSubspaceDerivative(jdata, q, v); // to be displaced in the core of the test instead of JointModed + + BOOST_CHECK(jdata.c.toVector().isApprox(jdata.Sdot.matrix() * v, 1e-12)); +} + BOOST_AUTO_TEST_SUITE_END() From b6bc476cd52c6f043866b89b12a099e7838c74f0 Mon Sep 17 00:00:00 2001 From: Lucas Joseph Date: Thu, 13 Nov 2025 15:14:24 +0100 Subject: [PATCH 23/48] Move ComputeTranslations and ComputeTranslationVelocities to tests --- .../multibody/joint/joint-ellipsoid.hpp | 50 ------------------- unittest/joint-ellipsoid.cpp | 40 +++++++++++++-- 2 files changed, 36 insertions(+), 54 deletions(-) diff --git a/include/pinocchio/multibody/joint/joint-ellipsoid.hpp b/include/pinocchio/multibody/joint/joint-ellipsoid.hpp index ebe13b9624..0d80a9692e 100644 --- a/include/pinocchio/multibody/joint/joint-ellipsoid.hpp +++ b/include/pinocchio/multibody/joint/joint-ellipsoid.hpp @@ -318,56 +318,6 @@ namespace pinocchio return res; } - template - Vector3 computeTranslations(const Eigen::MatrixBase & qs) const - { - Scalar c0, s0; - SINCOS(qs(0), &s0, &c0); - Scalar c1, s1; - SINCOS(qs(1), &s1, &c1); - - return computeTranslations(s0, c0, s1, c1); - } - - Vector3 computeTranslations( - const Scalar & s0, const Scalar & c0, const Scalar & s1, const Scalar & c1) const - { - Scalar nx, ny, nz; - nx = s1; - ny = -s0 * c1; - nz = c0 * c1; - - return Vector3(radius_a * nx, radius_b * ny, radius_c * nz); - } - - template - Vector3 computeTranslationVelocities( - const Eigen::MatrixBase & qs, - const Eigen::MatrixBase & vs) const - { - Scalar c0, s0; - SINCOS(qs(0), &s0, &c0); - Scalar c1, s1; - SINCOS(qs(1), &s1, &c1); - - return computeTranslationVelocities(s0, c0, s1, c1, vs(0), vs(1)); - } - - Vector3 computeTranslationVelocities( - const Scalar & s0, - const Scalar & c0, - const Scalar & s1, - const Scalar & c1, - const Scalar & q0dot, - const Scalar & q1dot) const - { - Vector3 v; - v(0) = radius_a * c1 * q1dot; - v(1) = radius_b * (-c0 * c1 * q0dot + s0 * s1 * q1dot); - v(2) = radius_c * (-s0 * c1 * q0dot - c0 * s1 * q1dot); - return v; - } - template Vector3 computeTranslationAccelerations( const Eigen::MatrixBase & qs, diff --git a/unittest/joint-ellipsoid.cpp b/unittest/joint-ellipsoid.cpp index 5e37f1e7f5..451c5fde52 100644 --- a/unittest/joint-ellipsoid.cpp +++ b/unittest/joint-ellipsoid.cpp @@ -20,7 +20,6 @@ #include using namespace pinocchio; - template void addJointAndBody( Model & model, @@ -89,6 +88,40 @@ Eigen::Matrix finiteDiffSdot( return Sdot_fd; } + +SE3::Vector3 computeTranslations(const JointModelEllipsoid & jmodel, const Eigen::VectorXd & qs) +{ + double c0, s0; + SINCOS(qs(0), &s0, &c0); + double c1, s1; + SINCOS(qs(1), &s1, &c1); + + double nx, ny, nz; + nx = s1; + ny = -s0 * c1; + nz = c0 * c1; + + return SE3::Vector3(jmodel.radius_a * nx, jmodel.radius_b * ny, jmodel.radius_c * nz); +} + +SE3::Vector3 computeTranslationVelocities( + const JointModelEllipsoid & jmodel, + const Eigen::VectorXd & qs, + const Eigen::VectorXd & vs) +{ + double c0, s0; + SINCOS(qs(0), &s0, &c0); + double c1, s1; + SINCOS(qs(1), &s1, &c1); + + SE3::Vector3 v; + v(0) = jmodel.radius_a * c1 * vs(1); + v(1) = jmodel.radius_b * (-c0 * c1 * vs(0) + s0 * s1 * vs(1)); + v(2) = jmodel.radius_c * (-s0 * c1 * vs(0) - c0 * s1 * vs(1)); + return v; + } + + BOOST_AUTO_TEST_SUITE(JointEllipsoid) BOOST_AUTO_TEST_CASE(vsSphericalZYX) @@ -247,7 +280,7 @@ BOOST_AUTO_TEST_CASE(vsCompositeTxTyTzRxRyRz) // Test positions of ellispoid vs composite Eigen::VectorXd q_ellipsoid(3); q_ellipsoid << 1.0, 2.0, 3.0; // rx, ry, rz - Eigen::Vector3d t = jointModelEllipsoid.computeTranslations(q_ellipsoid); + Eigen::Vector3d t = computeTranslations(jointModelEllipsoid, q_ellipsoid); Eigen::VectorXd q_composite(6); q_composite << t, q_ellipsoid; @@ -260,8 +293,7 @@ BOOST_AUTO_TEST_CASE(vsCompositeTxTyTzRxRyRz) Eigen::VectorXd qdot_ellipsoid(3); qdot_ellipsoid << 0.1, 0.2, 0.3; - Eigen::Vector3d v_linear = - jointModelEllipsoid.computeTranslationVelocities(q_ellipsoid, qdot_ellipsoid); + Eigen::Vector3d v_linear = computeTranslationVelocities(jointModelEllipsoid, q_ellipsoid, qdot_ellipsoid); Eigen::VectorXd qdot_composite(6); qdot_composite << v_linear, qdot_ellipsoid; From 78d171a27870f6c0a2c6891ec4732cf37985d294 Mon Sep 17 00:00:00 2001 From: Lucas Joseph Date: Thu, 13 Nov 2025 15:21:43 +0100 Subject: [PATCH 24/48] Moved computeTranslationAccelerations to tests --- .../multibody/joint/joint-ellipsoid.hpp | 35 ------------------- unittest/joint-ellipsoid.cpp | 25 +++++++++++-- 2 files changed, 23 insertions(+), 37 deletions(-) diff --git a/include/pinocchio/multibody/joint/joint-ellipsoid.hpp b/include/pinocchio/multibody/joint/joint-ellipsoid.hpp index 0d80a9692e..cb0e28cca6 100644 --- a/include/pinocchio/multibody/joint/joint-ellipsoid.hpp +++ b/include/pinocchio/multibody/joint/joint-ellipsoid.hpp @@ -318,41 +318,6 @@ namespace pinocchio return res; } - template - Vector3 computeTranslationAccelerations( - const Eigen::MatrixBase & qs, - const Eigen::MatrixBase & vs, - const Eigen::MatrixBase & as) const - { - Vector3 a; - Scalar c0, s0; - SINCOS(qs(0), &s0, &c0); - Scalar c1, s1; - SINCOS(qs(1), &s1, &c1); - return computeTranslationAccelerations(s0, c0, s1, c1, vs(0), vs(1), as(0), as(1)); - } - - Vector3 computeTranslationAccelerations( - const Scalar & s0, - const Scalar & c0, - const Scalar & s1, - const Scalar & c1, - const Scalar & q0dot, - const Scalar & q1dot, - const Scalar & q0ddot, - const Scalar & q1ddot) const - { - Vector3 a; - a(0) = radius_a * (-s1 * q1dot * q1dot + c1 * q1ddot); - a(1) = - radius_b - * (s0 * c1 * q0dot * q0dot + c0 * s1 * q0dot * q1dot - c0 * c1 * q0ddot + c0 * s1 * q1dot * q0dot + s0 * c1 * q1dot * q1dot + s0 * s1 * q1ddot); - a(2) = - radius_c - * (-c0 * c1 * q0dot * q0dot + s0 * s1 * q0dot * q1dot - s0 * c1 * q0ddot + s0 * s1 * q1dot * q0dot - c0 * c1 * q1dot * q1dot - c0 * s1 * q1ddot); - return a; - } - void computeMotionSubspace( const Scalar & s0, const Scalar & c0, diff --git a/unittest/joint-ellipsoid.cpp b/unittest/joint-ellipsoid.cpp index 451c5fde52..104ed59d57 100644 --- a/unittest/joint-ellipsoid.cpp +++ b/unittest/joint-ellipsoid.cpp @@ -119,7 +119,28 @@ SE3::Vector3 computeTranslationVelocities( v(1) = jmodel.radius_b * (-c0 * c1 * vs(0) + s0 * s1 * vs(1)); v(2) = jmodel.radius_c * (-s0 * c1 * vs(0) - c0 * s1 * vs(1)); return v; - } +} + +SE3:: Vector3 computeTranslationAccelerations( + const JointModelEllipsoid & jmodel, + const Eigen::VectorXd & qs, + const Eigen::VectorXd & vs, + const Eigen::VectorXd & as) +{ + double c0, s0; + SINCOS(qs(0), &s0, &c0); + double c1, s1; + SINCOS(qs(1), &s1, &c1); + SE3:: Vector3 a; + a(0) = jmodel.radius_a * (-s1 * vs(1) * vs(1) + c1 * as(1)); + a(1) = + jmodel.radius_b + * (s0 * c1 * vs(0) * vs(0) + c0 * s1 * vs(0) * vs(1) - c0 * c1 * as(0) + c0 * s1 * vs(1) * vs(0) + s0 * c1 * vs(1) * vs(1) + s0 * s1 * as(1)); + a(2) = + jmodel.radius_c + * (-c0 * c1 * vs(0) * vs(0) + s0 * s1 * vs(0) * vs(1) - s0 * c1 * as(0) + s0 * s1 * vs(1) * vs(0) - c0 * c1 * vs(1) * vs(1) - c0 * s1 * as(1)); + return a; +} BOOST_AUTO_TEST_SUITE(JointEllipsoid) @@ -305,7 +326,7 @@ BOOST_AUTO_TEST_CASE(vsCompositeTxTyTzRxRyRz) // Acceleration test Eigen::VectorXd qddot_ellipsoid(3); qddot_ellipsoid << 0.01, 0.02, 0.03; - Eigen::Vector3d a_linear = jointModelEllipsoid.computeTranslationAccelerations( + Eigen::Vector3d a_linear = computeTranslationAccelerations(jointModelEllipsoid, q_ellipsoid, qdot_ellipsoid, qddot_ellipsoid); Eigen::VectorXd qddot_composite(6); qddot_composite << a_linear, qddot_ellipsoid; From 60e901eed77d93d79c0d742ce5b1eeb2960f4ff7 Mon Sep 17 00:00:00 2001 From: ipuch Date: Thu, 13 Nov 2025 15:49:23 +0100 Subject: [PATCH 25/48] refactor(joint-ellipsoid) : Sdot computeMotionSubspaceDerivative in tests only y --- .../multibody/joint/joint-ellipsoid.hpp | 143 -------------- unittest/joint-ellipsoid.cpp | 180 +++++++++++++++++- 2 files changed, 171 insertions(+), 152 deletions(-) diff --git a/include/pinocchio/multibody/joint/joint-ellipsoid.hpp b/include/pinocchio/multibody/joint/joint-ellipsoid.hpp index cb0e28cca6..e49804690e 100644 --- a/include/pinocchio/multibody/joint/joint-ellipsoid.hpp +++ b/include/pinocchio/multibody/joint/joint-ellipsoid.hpp @@ -229,10 +229,6 @@ namespace pinocchio // Velocity part data.v.toVector().noalias() = data.S.matrix() * data.joint_v; - computeMotionSubspaceDerivative( - s0, c0, s1, c1, s2, c2, dndotx_dqdot1, dndoty_dqdot0, dndoty_dqdot1, dndotz_dqdot0, - dndotz_dqdot1, data); - // data.c.toVector().noalias() = data.Sdot.matrix() * data.joint_v; computeBiais( s0, c0, s1, c1, s2, c2, dndotx_dqdot1, dndoty_dqdot0, dndoty_dqdot1, dndotz_dqdot0, @@ -271,10 +267,6 @@ namespace pinocchio data.joint_v = vs.template segment(idx_v()); data.v.toVector().noalias() = data.S.matrix() * data.joint_v; - computeMotionSubspaceDerivative( - s0, c0, s1, c1, s2, c2, dndotx_dqdot1, dndoty_dqdot0, dndoty_dqdot1, dndotz_dqdot0, - dndotz_dqdot1, data); - // data.c.toVector().noalias() = data.Sdot.matrix() * data.joint_v; computeBiais( s0, c0, s1, c1, s2, c2, dndotx_dqdot1, dndoty_dqdot0, dndoty_dqdot1, dndotz_dqdot0, @@ -378,141 +370,6 @@ namespace pinocchio c1 * c2, s2, Scalar(0), -c1 * s2, c2, Scalar(0), s1, Scalar(0), Scalar(1); // clang-format on } - - template - void computeMotionSubspaceDerivative( - JointDataDerived & data, - const Eigen::MatrixBase & qs, - const Eigen::MatrixBase & vs) const - { - Scalar c0, s0; - SINCOS(qs(0), &s0, &c0); - Scalar c1, s1; - SINCOS(qs(1), &s1, &c1); - Scalar c2, s2; - SINCOS(qs(2), &s2, &c2); - - Scalar dndotx_dqdot1, dndoty_dqdot0, dndoty_dqdot1, dndotz_dqdot0, dndotz_dqdot1; - dndotx_dqdot1 = c1; - dndoty_dqdot0 = -c0 * c1; - dndoty_dqdot1 = s0 * s1; - dndotz_dqdot0 = -c1 * s0; - dndotz_dqdot1 = -c0 * s1; - - computeMotionSubspaceDerivative( - s0, c0, s1, c1, s2, c2, dndotx_dqdot1, dndoty_dqdot0, dndoty_dqdot1, dndotz_dqdot0, - dndotz_dqdot1, data); - } - - void computeMotionSubspaceDerivative( - const Scalar & s0, - const Scalar & c0, - const Scalar & s1, - const Scalar & c1, - const Scalar & s2, - const Scalar & c2, - const Scalar & dndotx_dqdot1, - const Scalar & dndoty_dqdot0, - const Scalar & dndoty_dqdot1, - const Scalar & dndotz_dqdot0, - const Scalar & dndotz_dqdot1, - JointDataDerived & data) const - { - // Compute Sdot for bias acceleration - Scalar qdot0, qdot1, qdot2; - qdot0 = data.joint_v(0); - qdot1 = data.joint_v(1); - qdot2 = data.joint_v(2); - - Scalar Sdot_11, Sdot_21, Sdot_31, Sdot_41, Sdot_51, Sdot_61; - Scalar Sdot_12, Sdot_22, Sdot_32, Sdot_42, Sdot_52, Sdot_62; - Scalar Sdot_13, Sdot_23, Sdot_33, Sdot_43, Sdot_53, Sdot_63; - - // Derivative of dndotXX_dqdot0 with respect to q0 and q1 - Scalar d_dndotx_dqdot1_dq1 = -s1; // dndotx_dqdot1 = c1; - - Scalar d_dndoty_dqdot0_dq0 = s0 * c1; // dndoty_dqdot0 = - c0 * c1; - Scalar d_dndoty_dqdot0_dq1 = c0 * s1; - - // Scalar d_dndoty_dqdot1_dq0 = c0 * s1; // dndoty_dqdot1 = s0 * s1; - Scalar d_dndoty_dqdot1_dq1 = s0 * c1; - - Scalar d_dndotz_dqdot0_dq0 = -c1 * c0; // dndotz_dqdot0 = - c1 * s0; - Scalar d_dndotz_dqdot0_dq1 = s0 * s1; - - // Scalar d_dndotz_dqdot1_dq0 = s0 * s1; // dndotz_dqdot1 = - c0 * s1; - Scalar d_dndotz_dqdot1_dq1 = -c0 * c1; - - // Upper part (translation) - // Row 1, Column 1 - Sdot_11 = - qdot0 - * (-dndoty_dqdot0 * radius_b * (-c0 * c2 * s1 + s0 * s2) + dndotz_dqdot0 * radius_c * (c0 * s2 + c2 * s0 * s1) + radius_b * (c0 * s2 + c2 * s0 * s1) * d_dndoty_dqdot0_dq0 + radius_c * (-c0 * c2 * s1 + s0 * s2) * d_dndotz_dqdot0_dq0) - + qdot1 - * (dndoty_dqdot0 * radius_b * c1 * c2 * s0 - dndotz_dqdot0 * radius_c * c0 * c1 * c2 + radius_b * (c0 * s2 + c2 * s0 * s1) * d_dndoty_dqdot0_dq1 + radius_c * (-c0 * c2 * s1 + s0 * s2) * d_dndotz_dqdot0_dq1) - - qdot2 - * (dndoty_dqdot0 * radius_b * (-c0 * c2 + s0 * s1 * s2) - dndotz_dqdot0 * radius_c * (c0 * s1 * s2 + c2 * s0)); - - // Row 1, Column 2 - Sdot_12 = - qdot0 - * (-dndoty_dqdot1 * radius_b * (-c0 * c2 * s1 + s0 * s2) + dndotz_dqdot1 * radius_c * (c0 * s2 + c2 * s0 * s1) + radius_b * (c0 * s2 + c2 * s0 * s1) * d_dndoty_dqdot0_dq1 + radius_c * (-c0 * c2 * s1 + s0 * s2) * d_dndotz_dqdot0_dq1) - + qdot1 * (-dndotx_dqdot1 * radius_a * c2 * s1 + dndoty_dqdot1 * radius_b * c1 * c2 * s0 - dndotz_dqdot1 * radius_c * c0 * c1 * c2 + radius_a * c1 * c2 * d_dndotx_dqdot1_dq1 + radius_b * (c0 * s2 + c2 * s0 * s1) * d_dndoty_dqdot1_dq1 + radius_c * (-c0 * c2 * s1 + s0 * s2) * d_dndotz_dqdot1_dq1) - qdot2 * (dndotx_dqdot1 * radius_a * c1 * s2 + dndoty_dqdot1 * radius_b * (-c0 * c2 + s0 * s1 * s2) - dndotz_dqdot1 * radius_c * (c0 * s1 * s2 + c2 * s0)); - - // Row 1, Column 3 - Sdot_13 = Scalar(0); - - // Row 2, Column 1 - Sdot_21 = - -qdot0 - * (dndoty_dqdot0 * radius_b * (c0 * s1 * s2 + c2 * s0) + dndotz_dqdot0 * radius_c * (-c0 * c2 + s0 * s1 * s2) + radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot0_dq0 - radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot0_dq0) - - qdot1 - * (dndoty_dqdot0 * radius_b * c1 * s0 * s2 - dndotz_dqdot0 * radius_c * c0 * c1 * s2 + radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot0_dq1 - radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot0_dq1) - - qdot2 - * (dndoty_dqdot0 * radius_b * (c0 * s2 + c2 * s0 * s1) + dndotz_dqdot0 * radius_c * (-c0 * c2 * s1 + s0 * s2)); - - // Row 2, Column 2 - Sdot_22 = - -qdot0 - * (dndoty_dqdot1 * radius_b * (c0 * s1 * s2 + c2 * s0) + dndotz_dqdot1 * radius_c * (-c0 * c2 + s0 * s1 * s2) + radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot0_dq1 - radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot0_dq1) - + qdot1 * (dndotx_dqdot1 * radius_a * s1 * s2 - dndoty_dqdot1 * radius_b * c1 * s0 * s2 + dndotz_dqdot1 * radius_c * c0 * c1 * s2 - radius_a * c1 * s2 * d_dndotx_dqdot1_dq1 - radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot1_dq1 + radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot1_dq1) - qdot2 * (dndotx_dqdot1 * radius_a * c1 * c2 + dndoty_dqdot1 * radius_b * (c0 * s2 + c2 * s0 * s1) + dndotz_dqdot1 * radius_c * (-c0 * c2 * s1 + s0 * s2)); - - // Row 2, Column 3 - Sdot_23 = Scalar(0); - - // Row 3, Column 1 - Sdot_31 = - -qdot0 * c1 - * (dndoty_dqdot0 * radius_b * c0 + dndotz_dqdot0 * radius_c * s0 + radius_b * s0 * d_dndoty_dqdot0_dq0 - radius_c * c0 * d_dndotz_dqdot0_dq0) - + qdot1 - * (-c1 * (radius_b * s0 * d_dndoty_dqdot0_dq1 - radius_c * c0 * d_dndotz_dqdot0_dq1) + s1 * (dndoty_dqdot0 * radius_b * s0 - dndotz_dqdot0 * radius_c * c0)); - - // Row 3, Column 2 - Sdot_32 = - -qdot0 * c1 - * (dndoty_dqdot1 * radius_b * c0 + dndotz_dqdot1 * radius_c * s0 + radius_b * s0 * d_dndoty_dqdot0_dq1 - radius_c * c0 * d_dndotz_dqdot0_dq1) - + qdot1 - * (dndotx_dqdot1 * radius_a * c1 + dndoty_dqdot1 * radius_b * s0 * s1 - dndotz_dqdot1 * radius_c * c0 * s1 + radius_a * s1 * d_dndotx_dqdot1_dq1 - radius_b * c1 * s0 * d_dndoty_dqdot1_dq1 + radius_c * c0 * c1 * d_dndotz_dqdot1_dq1); - - // Row 3, Column 3 - Sdot_33 = Scalar(0); - - // Angular part (rows 4-6) - Sdot_41 = -(qdot1 * c2 * s1 + qdot2 * c1 * s2); - Sdot_51 = qdot1 * s1 * s2 - qdot2 * c1 * c2; - Sdot_61 = qdot1 * c1; - - Sdot_42 = qdot2 * c2; - Sdot_52 = -qdot2 * s2; - Sdot_62 = Scalar(0); - - Sdot_43 = Scalar(0); - Sdot_53 = Scalar(0); - Sdot_63 = Scalar(0); - - data.Sdot.matrix() << Sdot_11, Sdot_12, Sdot_13, Sdot_21, Sdot_22, Sdot_23, Sdot_31, Sdot_32, - Sdot_33, Sdot_41, Sdot_42, Sdot_43, Sdot_51, Sdot_52, Sdot_53, Sdot_61, Sdot_62, Sdot_63; - } template void computeBiais( diff --git a/unittest/joint-ellipsoid.cpp b/unittest/joint-ellipsoid.cpp index 104ed59d57..42e176cea6 100644 --- a/unittest/joint-ellipsoid.cpp +++ b/unittest/joint-ellipsoid.cpp @@ -35,6 +35,156 @@ void addJointAndBody( model.appendBodyToJoint(idx, Y); } +/// \brief Compute motion subspace derivative Sdot analytically for JointModelEllipsoid +Eigen::Matrix computeMotionSubspaceDerivative( + const JointModelEllipsoid & jmodel, + const Eigen::VectorXd & qs, + const Eigen::VectorXd & vs) +{ + double c0, s0; + SINCOS(qs(0), &s0, &c0); + double c1, s1; + SINCOS(qs(1), &s1, &c1); + double c2, s2; + SINCOS(qs(2), &s2, &c2); + + // Extract velocities + double qdot0 = vs(0); + double qdot1 = vs(1); + double qdot2 = vs(2); + + // Get radii + const double radius_a = jmodel.radius_a; + const double radius_b = jmodel.radius_b; + const double radius_c = jmodel.radius_c; + + // Derivatives of normal vector components w.r.t. velocities + double dndotx_dqdot1 = c1; + double dndoty_dqdot0 = -c0 * c1; + double dndoty_dqdot1 = s0 * s1; + double dndotz_dqdot0 = -c1 * s0; + double dndotz_dqdot1 = -c0 * s1; + + // Second derivatives (derivatives of dndot w.r.t. configuration) + double d_dndotx_dqdot1_dq1 = -s1; + double d_dndoty_dqdot0_dq0 = s0 * c1; + double d_dndoty_dqdot0_dq1 = c0 * s1; + double d_dndoty_dqdot1_dq1 = s0 * c1; + double d_dndotz_dqdot0_dq0 = -c1 * c0; + double d_dndotz_dqdot0_dq1 = s0 * s1; + double d_dndotz_dqdot1_dq1 = -c0 * c1; + + // Translational part (rows 1-3) + double Sdot_11 = + qdot0 + * (-dndoty_dqdot0 * radius_b * (-c0 * c2 * s1 + s0 * s2) + + dndotz_dqdot0 * radius_c * (c0 * s2 + c2 * s0 * s1) + + radius_b * (c0 * s2 + c2 * s0 * s1) * d_dndoty_dqdot0_dq0 + + radius_c * (-c0 * c2 * s1 + s0 * s2) * d_dndotz_dqdot0_dq0) + + qdot1 + * (dndoty_dqdot0 * radius_b * c1 * c2 * s0 + - dndotz_dqdot0 * radius_c * c0 * c1 * c2 + + radius_b * (c0 * s2 + c2 * s0 * s1) * d_dndoty_dqdot0_dq1 + + radius_c * (-c0 * c2 * s1 + s0 * s2) * d_dndotz_dqdot0_dq1) + - qdot2 + * (dndoty_dqdot0 * radius_b * (-c0 * c2 + s0 * s1 * s2) + - dndotz_dqdot0 * radius_c * (c0 * s1 * s2 + c2 * s0)); + + double Sdot_12 = + qdot0 + * (-dndoty_dqdot1 * radius_b * (-c0 * c2 * s1 + s0 * s2) + + dndotz_dqdot1 * radius_c * (c0 * s2 + c2 * s0 * s1) + + radius_b * (c0 * s2 + c2 * s0 * s1) * d_dndoty_dqdot0_dq1 + + radius_c * (-c0 * c2 * s1 + s0 * s2) * d_dndotz_dqdot0_dq1) + + qdot1 + * (-dndotx_dqdot1 * radius_a * c2 * s1 + + dndoty_dqdot1 * radius_b * c1 * c2 * s0 + - dndotz_dqdot1 * radius_c * c0 * c1 * c2 + + radius_a * c1 * c2 * d_dndotx_dqdot1_dq1 + + radius_b * (c0 * s2 + c2 * s0 * s1) * d_dndoty_dqdot1_dq1 + + radius_c * (-c0 * c2 * s1 + s0 * s2) * d_dndotz_dqdot1_dq1) + - qdot2 + * (dndotx_dqdot1 * radius_a * c1 * s2 + + dndoty_dqdot1 * radius_b * (-c0 * c2 + s0 * s1 * s2) + - dndotz_dqdot1 * radius_c * (c0 * s1 * s2 + c2 * s0)); + + double Sdot_21 = + -qdot0 + * (dndoty_dqdot0 * radius_b * (c0 * s1 * s2 + c2 * s0) + + dndotz_dqdot0 * radius_c * (-c0 * c2 + s0 * s1 * s2) + + radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot0_dq0 + - radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot0_dq0) + - qdot1 + * (dndoty_dqdot0 * radius_b * c1 * s0 * s2 + - dndotz_dqdot0 * radius_c * c0 * c1 * s2 + + radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot0_dq1 + - radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot0_dq1) + - qdot2 + * (dndoty_dqdot0 * radius_b * (c0 * s2 + c2 * s0 * s1) + + dndotz_dqdot0 * radius_c * (-c0 * c2 * s1 + s0 * s2)); + + double Sdot_22 = + -qdot0 + * (dndoty_dqdot1 * radius_b * (c0 * s1 * s2 + c2 * s0) + + dndotz_dqdot1 * radius_c * (-c0 * c2 + s0 * s1 * s2) + + radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot0_dq1 + - radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot0_dq1) + + qdot1 + * (dndotx_dqdot1 * radius_a * s1 * s2 + - dndoty_dqdot1 * radius_b * c1 * s0 * s2 + + dndotz_dqdot1 * radius_c * c0 * c1 * s2 + - radius_a * c1 * s2 * d_dndotx_dqdot1_dq1 + - radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot1_dq1 + + radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot1_dq1) + - qdot2 + * (dndotx_dqdot1 * radius_a * c1 * c2 + + dndoty_dqdot1 * radius_b * (c0 * s2 + c2 * s0 * s1) + + dndotz_dqdot1 * radius_c * (-c0 * c2 * s1 + s0 * s2)); + + double Sdot_31 = + -qdot0 * c1 + * (dndoty_dqdot0 * radius_b * c0 + + dndotz_dqdot0 * radius_c * s0 + + radius_b * s0 * d_dndoty_dqdot0_dq0 + - radius_c * c0 * d_dndotz_dqdot0_dq0) + + qdot1 + * (-c1 * (radius_b * s0 * d_dndoty_dqdot0_dq1 - radius_c * c0 * d_dndotz_dqdot0_dq1) + + s1 * (dndoty_dqdot0 * radius_b * s0 - dndotz_dqdot0 * radius_c * c0)); + + double Sdot_32 = + -qdot0 * c1 + * (dndoty_dqdot1 * radius_b * c0 + + dndotz_dqdot1 * radius_c * s0 + + radius_b * s0 * d_dndoty_dqdot0_dq1 + - radius_c * c0 * d_dndotz_dqdot0_dq1) + + qdot1 + * (dndotx_dqdot1 * radius_a * c1 + + dndoty_dqdot1 * radius_b * s0 * s1 + - dndotz_dqdot1 * radius_c * c0 * s1 + + radius_a * s1 * d_dndotx_dqdot1_dq1 + - radius_b * c1 * s0 * d_dndoty_dqdot1_dq1 + + radius_c * c0 * c1 * d_dndotz_dqdot1_dq1); + + // Angular part (rows 4-6) + double Sdot_41 = -(qdot1 * c2 * s1 + qdot2 * c1 * s2); + double Sdot_51 = qdot1 * s1 * s2 - qdot2 * c1 * c2; + double Sdot_61 = qdot1 * c1; + + double Sdot_42 = qdot2 * c2; + double Sdot_52 = -qdot2 * s2; + + // Build and return 6x3 matrix + Eigen::Matrix Sdot; + Sdot << Sdot_11, Sdot_12, 0.0, + Sdot_21, Sdot_22, 0.0, + Sdot_31, Sdot_32, 0.0, + Sdot_41, Sdot_42, 0.0, + Sdot_51, Sdot_52, 0.0, + Sdot_61, 0.0, 0.0; + + return Sdot; +} + /// \brief Compute Sdot (motion subspace derivative) via finite differences template Eigen::Matrix finiteDiffSdot( @@ -145,6 +295,7 @@ SE3:: Vector3 computeTranslationAccelerations( BOOST_AUTO_TEST_SUITE(JointEllipsoid) +/// @brief Test the rotationnal equivalence between JointModelEllipsoid and JointModelSphericalZYX BOOST_AUTO_TEST_CASE(vsSphericalZYX) { using namespace pinocchio; @@ -232,31 +383,31 @@ BOOST_AUTO_TEST_CASE(vsSphericalZYX) BOOST_CHECK(dataEllipsoid.v[1].toVector().isApprox(dataSphericalZYX.v[1].toVector())); BOOST_CHECK(dataEllipsoid.oMi[1].isApprox(dataSphericalZYX.oMi[1])); - Matrix3 Sdot_e = jDataEllipsoidFK.Sdot.matrix().bottomRows<3>(); // Angular part + // Matrix3 Sdot_e = jDataEllipsoidFK.Sdot.matrix().bottomRows<3>(); // Angular part + Eigen::Vector3d c_e = jDataEllipsoidFK.c.angular(); // Sdot * qd_e // The acceleration conversion formula: wdot_s = wdot_e // S_s * qdotdot_s + c_s.angular() = Sdot_e * qd_e + S_e * qdotdot_e // Solving for qdotdot_e: // S_e * qdotdot_e = c_s.angular()+ S_s * qdotdot_s - Sdot_e * qd_e Eigen::Vector3d qdotdot_e = - S_e.inverse() * (S_s * qdotdot_s + jDataSphereFK.c.angular() - Sdot_e * qd_e); + S_e.inverse() * (S_s * qdotdot_s + jDataSphereFK.c.angular() - c_e); // Verify angular accelerations match Eigen::Vector3d wdot_s = jDataSphereFK.c.angular() + S_s * qdotdot_s; - Eigen::Vector3d wdot_e = Sdot_e * qd_e + S_e * qdotdot_e; + Eigen::Vector3d wdot_e = c_e + S_e * qdotdot_e; BOOST_CHECK(wdot_s.isApprox(wdot_e)); forwardKinematics(modelEllipsoid, dataEllipsoid, q_e, qd_e, qdotdot_e); forwardKinematics(modelSphericalZYX, dataSphericalZYX, q_s, qd_s, qdotdot_s); BOOST_CHECK(dataEllipsoid.a[1].toVector().isApprox(dataSphericalZYX.a[1].toVector())); - // Test RNEA (Recursive Newton-Euler Algorithm) - spatial forces should match rnea(modelEllipsoid, dataEllipsoid, q_e, qd_e, qdotdot_e); rnea(modelSphericalZYX, dataSphericalZYX, q_s, qd_s, qdotdot_s); BOOST_CHECK(dataEllipsoid.f[1].isApprox(dataSphericalZYX.f[1])); - + // Test ABA (Articulated-Body Algorithm) Eigen::VectorXd tau = Eigen::VectorXd::Ones(modelEllipsoid.nv); Eigen::VectorXd aAbaEllipsoid = @@ -334,13 +485,22 @@ BOOST_AUTO_TEST_CASE(vsCompositeTxTyTzRxRyRz) forwardKinematics(modelEllipsoid, dataEllipsoid, q_ellipsoid, qdot_ellipsoid, qddot_ellipsoid); forwardKinematics(modelComposite, dataComposite, q_composite, qdot_composite, qddot_composite); + std::cout << "Ellipsoid acceleration: " << dataEllipsoid.a[1].toVector().transpose() << std::endl; + std::cout << "Composite acceleration: " << dataComposite.a[1].toVector().transpose() << std::endl; + BOOST_CHECK(dataEllipsoid.a[1].toVector().isApprox(dataComposite.a[1].toVector())); // Test RNEA - spatial forces and torques should match rnea(modelEllipsoid, dataEllipsoid, q_ellipsoid, qdot_ellipsoid, qddot_ellipsoid); rnea(modelComposite, dataComposite, q_composite, qdot_composite, qddot_composite); + + std::cout << "Ellipsoid force: " << dataEllipsoid.f[1].toVector().transpose() << std::endl; + std::cout << "Composite force: " << dataComposite.f[1].toVector().transpose() << std::endl; + BOOST_CHECK(dataEllipsoid.f[1].isApprox(dataComposite.f[1])); + + // Need joint data to get both motion subspaces S_comp and S_ell JointDataComposite jdata_c = jComposite.createData(); jComposite.setIndexes(0, 0, 0); @@ -398,8 +558,9 @@ BOOST_AUTO_TEST_CASE(testSdotFiniteDifferences) // Compute analytical Sdot const double eps = 1e-8; - jmodel.calc(jdata, q, v); - const Eigen::Matrix Sdot_ref = jdata.Sdot.matrix(); + + // Reference Sdot with analytical formula + const Eigen::Matrix Sdot_ref = computeMotionSubspaceDerivative(jmodel, q, v); // Compute Sdot via finite differences using helper function const Eigen::Matrix Sdot_fd = finiteDiffSdot(jmodel, jdata, q, v, eps); @@ -430,9 +591,10 @@ BOOST_AUTO_TEST_CASE(testBiaisVsSdotTimesVelocity) jmodel.calc(jdata, q, v); jmodel.computeBiais(jdata, q, v); - jmodel.computeMotionSubspaceDerivative(jdata, q, v); // to be displaced in the core of the test instead of JointModed - BOOST_CHECK(jdata.c.toVector().isApprox(jdata.Sdot.matrix() * v, 1e-12)); + const Eigen::Matrix Sdot = computeMotionSubspaceDerivative(jmodel, q, v); + + BOOST_CHECK(jdata.c.toVector().isApprox(Sdot * v, 1e-12)); } BOOST_AUTO_TEST_SUITE_END() From e329f8232b4ec1b1b11b4b92c0ac930cc711a59c Mon Sep 17 00:00:00 2001 From: ipuch Date: Thu, 13 Nov 2025 15:58:24 +0100 Subject: [PATCH 26/48] clean(joint-ellipsoid) no Sdot anymore. --- .../multibody/joint/joint-ellipsoid.hpp | 30 +++++++++---------- unittest/joint-ellipsoid.cpp | 4 +-- 2 files changed, 16 insertions(+), 18 deletions(-) diff --git a/include/pinocchio/multibody/joint/joint-ellipsoid.hpp b/include/pinocchio/multibody/joint/joint-ellipsoid.hpp index e49804690e..2461b196b4 100644 --- a/include/pinocchio/multibody/joint/joint-ellipsoid.hpp +++ b/include/pinocchio/multibody/joint/joint-ellipsoid.hpp @@ -80,7 +80,6 @@ namespace pinocchio TangentVector_t joint_v; Constraint_t S; - Constraint_t Sdot; Transformation_t M; Motion_t v; Bias_t c; @@ -95,7 +94,6 @@ namespace pinocchio : joint_q(ConfigVector_t::Zero()) , joint_v(TangentVector_t::Zero()) , S() - , Sdot() , M(Transformation_t::Identity()) , v(Motion_t::Zero()) , c(Bias_t::Zero()) @@ -171,8 +169,9 @@ namespace pinocchio Scalar c0, Scalar s0, Scalar c1, Scalar s1, Scalar c2, Scalar s2, JointDataDerived & data) const { // clang-format off - data.M.rotation() << c1 * c2, -c1 * s2, s1, c0 * s2 + c2 * s0 * s1, c0 * c2 - s0 * s1 * s2, - -c1 * s0, -c0 * c2 * s1 + s0 * s2, c0 * s1 * s2 + c2 * s0, c0 * c1; + data.M.rotation() << c1 * c2 , -c1 * s2 , s1 , + c0 * s2 + c2 * s0 * s1 , c0 * c2 - s0 * s1 * s2 ,-c1 * s0 , + -c0 * c2 * s1 + s0 * s2 , c0 * s1 * s2 + c2 * s0 , c0 * c1; // clang-format on Scalar nx, ny, nz; nx = s1; @@ -229,7 +228,6 @@ namespace pinocchio // Velocity part data.v.toVector().noalias() = data.S.matrix() * data.joint_v; - // data.c.toVector().noalias() = data.Sdot.matrix() * data.joint_v; computeBiais( s0, c0, s1, c1, s2, c2, dndotx_dqdot1, dndoty_dqdot0, dndoty_dqdot1, dndotz_dqdot0, dndotz_dqdot1, data); @@ -267,7 +265,6 @@ namespace pinocchio data.joint_v = vs.template segment(idx_v()); data.v.toVector().noalias() = data.S.matrix() * data.joint_v; - // data.c.toVector().noalias() = data.Sdot.matrix() * data.joint_v; computeBiais( s0, c0, s1, c1, s2, c2, dndotx_dqdot1, dndoty_dqdot0, dndoty_dqdot1, dndotz_dqdot0, dndotz_dqdot1, data); @@ -366,8 +363,12 @@ namespace pinocchio S_32 = dndotx_dqdot1 * radius_a * s1 - dndoty_dqdot1 * radius_b * c1 * s0 + dndotz_dqdot1 * radius_c * c0 * c1; - data.S.matrix() << S_11, S_12, Scalar(0), S_21, S_22, Scalar(0), S_31, S_32, Scalar(0), - c1 * c2, s2, Scalar(0), -c1 * s2, c2, Scalar(0), s1, Scalar(0), Scalar(1); + data.S.matrix() << S_11 , S_12 , Scalar(0), + S_21 , S_22 , Scalar(0), + S_31 , S_32 , Scalar(0), + c1 * c2, s2 , Scalar(0), + -c1 * s2, c2 , Scalar(0), + s1 , Scalar(0), Scalar(1); // clang-format on } @@ -417,24 +418,21 @@ namespace pinocchio qdot2 = data.joint_v(2); // last columns and last element of the second column are zero, - // so we do not compute them + // so we do not compute them Scalar Sdot_11, Sdot_21, Sdot_31, Sdot_41, Sdot_51, Sdot_61; Scalar Sdot_12, Sdot_22, Sdot_32, Sdot_42, Sdot_52; - // Derivative of dndotXX_dqdot0 with respect to q0 and q1 - Scalar d_dndotx_dqdot1_dq1 = -s1; // dndotx_dqdot1 = c1; + Scalar d_dndotx_dqdot1_dq1 = -s1; - Scalar d_dndoty_dqdot0_dq0 = s0 * c1; // dndoty_dqdot0 = - c0 * c1; + Scalar d_dndoty_dqdot0_dq0 = s0 * c1; Scalar d_dndoty_dqdot0_dq1 = c0 * s1; - // Scalar d_dndoty_dqdot1_dq0 = c0 * s1; // dndoty_dqdot1 = s0 * s1; Scalar d_dndoty_dqdot1_dq1 = s0 * c1; - Scalar d_dndotz_dqdot0_dq0 = -c1 * c0; // dndotz_dqdot0 = - c1 * s0; + Scalar d_dndotz_dqdot0_dq0 = -c1 * c0; Scalar d_dndotz_dqdot0_dq1 = s0 * s1; - // Scalar d_dndotz_dqdot1_dq0 = s0 * s1; // dndotz_dqdot1 = - c0 * s1; Scalar d_dndotz_dqdot1_dq1 = -c0 * c1; // Upper part (translation) @@ -531,4 +529,4 @@ namespace boost }; } // namespace boost -#endif // ifndef __pinocchio_multibody_joint_spherical_ZYX_hpp__ +#endif // ifndef __pinocchio_multibody_joint_ellipsoid_hpp__ diff --git a/unittest/joint-ellipsoid.cpp b/unittest/joint-ellipsoid.cpp index 42e176cea6..1ca516d5ad 100644 --- a/unittest/joint-ellipsoid.cpp +++ b/unittest/joint-ellipsoid.cpp @@ -383,7 +383,6 @@ BOOST_AUTO_TEST_CASE(vsSphericalZYX) BOOST_CHECK(dataEllipsoid.v[1].toVector().isApprox(dataSphericalZYX.v[1].toVector())); BOOST_CHECK(dataEllipsoid.oMi[1].isApprox(dataSphericalZYX.oMi[1])); - // Matrix3 Sdot_e = jDataEllipsoidFK.Sdot.matrix().bottomRows<3>(); // Angular part Eigen::Vector3d c_e = jDataEllipsoidFK.c.angular(); // Sdot * qd_e // The acceleration conversion formula: wdot_s = wdot_e @@ -402,7 +401,8 @@ BOOST_AUTO_TEST_CASE(vsSphericalZYX) forwardKinematics(modelSphericalZYX, dataSphericalZYX, q_s, qd_s, qdotdot_s); BOOST_CHECK(dataEllipsoid.a[1].toVector().isApprox(dataSphericalZYX.a[1].toVector())); - // Test RNEA (Recursive Newton-Euler Algorithm) - spatial forces should match + + // Test RNEA (Recursive Newton-Euler Algorithm) - spatial forces should match with both joints rnea(modelEllipsoid, dataEllipsoid, q_e, qd_e, qdotdot_e); rnea(modelSphericalZYX, dataSphericalZYX, q_s, qd_s, qdotdot_s); From ad98ad08e3b7da996766d2382adba3c0bb3ebbc5 Mon Sep 17 00:00:00 2001 From: ipuch Date: Thu, 13 Nov 2025 16:28:59 +0100 Subject: [PATCH 27/48] doc(joint-ellipsoid) --- .../multibody/joint/joint-ellipsoid.hpp | 32 ++++++++++++++++++- 1 file changed, 31 insertions(+), 1 deletion(-) diff --git a/include/pinocchio/multibody/joint/joint-ellipsoid.hpp b/include/pinocchio/multibody/joint/joint-ellipsoid.hpp index 2461b196b4..6016c2fca1 100644 --- a/include/pinocchio/multibody/joint/joint-ellipsoid.hpp +++ b/include/pinocchio/multibody/joint/joint-ellipsoid.hpp @@ -68,6 +68,25 @@ namespace pinocchio typedef _Scalar Scalar; }; + /// \brief Ellipsoid joint - constrains motion to ellipsoid surface with 3-DOF. + /// + /// The configuration space uses three angles (q₀, q₁, q₂) representing: + /// - Rotation about the x-axis + /// - Rotation about the y-axis + /// - Spin about the "normal" direction + /// + /// The joint position on the ellipsoid surface is computed as: + /// \f$ \mathbf{p} = (a \sin q_1, -b \sin q_0 \cos q_1, c \cos q_0 \cos q_1) \f$ + /// + /// where \f$ a, b, c \f$ are the radii along the x, y, z axes respectively. + /// + /// \note For non-spherical ellipsoids, the third rotation axis is only approximately + /// normal to the surface. It corresponds to the normal of an equivalent sphere while + /// the translation follows the true ellipsoid surface. The "normal" direction is + /// truly normal only when all radii are equal (sphere case). + /// + /// \sa Seth et al., "Minimal formulation of joint motion for biomechanisms," + /// Nonlinear Dynamics 62(1):291-303, 2010. template struct JointDataEllipsoidTpl : public JointDataBase> { @@ -140,7 +159,8 @@ namespace pinocchio { return JointDataDerived(); } - + + /// @brief Default constructor. Creates a sphere (0.01, 0.01, 0.01). JointModelEllipsoidTpl() : radius_a(Scalar(0.01)) , radius_b(Scalar(0.01)) @@ -148,6 +168,10 @@ namespace pinocchio { } + /// @brief Constructor with specified radii. + /// @param a Semi-axis length along x-direction + /// @param b Semi-axis length along y-direction + /// @param c Semi-axis length along z-direction JointModelEllipsoidTpl(const Scalar & a, const Scalar & b, const Scalar & c) : radius_a(a) , radius_b(b) @@ -165,6 +189,11 @@ namespace pinocchio return {true, true, true}; } + /// @brief Computes the spatial transformation M(q) from joint configuration. + /// @param[in] c0, s0 Cosine and sine of q[0] + /// @param[in] c1, s1 Cosine and sine of q[1] + /// @param[in] c2, s2 Cosine and sine of q[2] + /// @param[out] data Joint data where M will be stored void computeSpatialTransform( Scalar c0, Scalar s0, Scalar c1, Scalar s1, Scalar c2, Scalar s2, JointDataDerived & data) const { @@ -372,6 +401,7 @@ namespace pinocchio // clang-format on } + /// @brief Computes the bias acceleration c(q, v) = Sdot(q)·v. template void computeBiais( JointDataDerived & data, From 5f22bfdff112b5a6ebfaae5d634509de12c129a7 Mon Sep 17 00:00:00 2001 From: ipuch Date: Thu, 13 Nov 2025 16:45:00 +0100 Subject: [PATCH 28/48] tests(joint-ellipsoid): rnea vs aba isolated --- unittest/joint-ellipsoid.cpp | 67 +++++++++++++++++++++++++----------- 1 file changed, 47 insertions(+), 20 deletions(-) diff --git a/unittest/joint-ellipsoid.cpp b/unittest/joint-ellipsoid.cpp index 1ca516d5ad..460cc351e9 100644 --- a/unittest/joint-ellipsoid.cpp +++ b/unittest/joint-ellipsoid.cpp @@ -407,15 +407,9 @@ BOOST_AUTO_TEST_CASE(vsSphericalZYX) rnea(modelSphericalZYX, dataSphericalZYX, q_s, qd_s, qdotdot_s); BOOST_CHECK(dataEllipsoid.f[1].isApprox(dataSphericalZYX.f[1])); - - // Test ABA (Articulated-Body Algorithm) - Eigen::VectorXd tau = Eigen::VectorXd::Ones(modelEllipsoid.nv); - Eigen::VectorXd aAbaEllipsoid = - aba(modelEllipsoid, dataEllipsoid, q_e, qd_e, dataEllipsoid.tau, Convention::WORLD); - - BOOST_CHECK(dataEllipsoid.ddq.isApprox(qdotdot_e)); } +/// @brief Test the equivalence between JointModelEllipsoid and a Composite joint (Tx, Ty, Tz, Rx, Ry, Rz) BOOST_AUTO_TEST_CASE(vsCompositeTxTyTzRxRyRz) { using namespace pinocchio; @@ -485,22 +479,14 @@ BOOST_AUTO_TEST_CASE(vsCompositeTxTyTzRxRyRz) forwardKinematics(modelEllipsoid, dataEllipsoid, q_ellipsoid, qdot_ellipsoid, qddot_ellipsoid); forwardKinematics(modelComposite, dataComposite, q_composite, qdot_composite, qddot_composite); - std::cout << "Ellipsoid acceleration: " << dataEllipsoid.a[1].toVector().transpose() << std::endl; - std::cout << "Composite acceleration: " << dataComposite.a[1].toVector().transpose() << std::endl; - BOOST_CHECK(dataEllipsoid.a[1].toVector().isApprox(dataComposite.a[1].toVector())); // Test RNEA - spatial forces and torques should match rnea(modelEllipsoid, dataEllipsoid, q_ellipsoid, qdot_ellipsoid, qddot_ellipsoid); rnea(modelComposite, dataComposite, q_composite, qdot_composite, qddot_composite); - std::cout << "Ellipsoid force: " << dataEllipsoid.f[1].toVector().transpose() << std::endl; - std::cout << "Composite force: " << dataComposite.f[1].toVector().transpose() << std::endl; - BOOST_CHECK(dataEllipsoid.f[1].isApprox(dataComposite.f[1])); - - // Need joint data to get both motion subspaces S_comp and S_ell JointDataComposite jdata_c = jComposite.createData(); jComposite.setIndexes(0, 0, 0); @@ -526,14 +512,53 @@ BOOST_AUTO_TEST_CASE(vsCompositeTxTyTzRxRyRz) dataEllipsoid.tau.isApprox(tau_proj, 1e-6), "Projected composite torques do not match ellipsoid torques.\n" << "Expected: " << dataEllipsoid.tau.transpose() << "\nGot: " << tau_proj.transpose()); +} - // Test ABA (Articulated-Body Algorithm) - Eigen::VectorXd aAbaEllipsoid = aba( - modelEllipsoid, dataEllipsoid, q_ellipsoid, qdot_ellipsoid, dataEllipsoid.tau, - Convention::WORLD); - BOOST_CHECK(aAbaEllipsoid.isApprox(qddot_ellipsoid)); +/// @brief Test RNEA vs ABA with multiple random configurations +BOOST_AUTO_TEST_CASE(RNEAvsABA) +{ + using namespace pinocchio; + typedef SE3::Vector3 Vector3; + typedef SE3::Matrix3 Matrix3; + + double radius_a = 2.5; + double radius_b = 1.8; + double radius_c = 1.2; + + Inertia inertia(1.0, Vector3::Zero(), Matrix3::Identity()); + SE3 pos = SE3::Identity(); + + Model model; + JointModelEllipsoid jointModel(radius_a, radius_b, radius_c); + addJointAndBody(model, jointModel, 0, pos, "ellipsoid", inertia); + + Data data(model); + Data data_aba(model); + + // Test with multiple random configurations + for (int trial = 0; trial < 10; ++trial) + { + Eigen::VectorXd q = Eigen::VectorXd::Random(model.nq); + Eigen::VectorXd v = Eigen::VectorXd::Random(model.nv); + Eigen::VectorXd a = Eigen::VectorXd::Random(model.nv); + + // RNEA + rnea(model, data, q, v, a); + Eigen::VectorXd tau_rnea = data.tau; + + // ABA + aba(model, data_aba, q, v, tau_rnea, Convention::WORLD); + + BOOST_CHECK_MESSAGE( + a.isApprox(data_aba.ddq, 1e-9), + "RNEA and ABA inconsistent at trial " << trial << "\n" + << "Configuration: " << q.transpose() << "\n" + << "Expected: " << a.transpose() << "\n" + << "Got: " << data_aba.ddq.transpose()); + } } +/// @brief Test Sdot via finite differences BOOST_AUTO_TEST_CASE(testSdotFiniteDifferences) { using namespace pinocchio; @@ -567,6 +592,8 @@ BOOST_AUTO_TEST_CASE(testSdotFiniteDifferences) BOOST_CHECK(Sdot_ref.isApprox(Sdot_fd, sqrt(eps))); } + +/// @brief Test that biais term equals Sdot * v BOOST_AUTO_TEST_CASE(testBiaisVsSdotTimesVelocity) { using namespace pinocchio; From a9504b503a7a7b54f5b3111d6fd697eacced32b7 Mon Sep 17 00:00:00 2001 From: ipuch Date: Thu, 13 Nov 2025 16:55:52 +0100 Subject: [PATCH 29/48] docs(joint-ellipsoid): changelog and readme changelog again --- CHANGELOG.md | 5 +---- README.md | 1 + 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 3e0657f72b..6e17ec6c80 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -17,6 +17,7 @@ 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)) - Add names to joints that are inside a composite joint ([#2786](https://github.com/stack-of-tasks/pinocchio/pull/2786)) - Add color support for robot meshes in Viser ([#2793](https://github.com/stack-of-tasks/pinocchio/pull/2793)) +- Add Ellipsoid Joint (3-DOF surface constraint), get ready for biomechanics ([#2797](https://github.com/stack-of-tasks/pinocchio/pull/2797)) ### Changed - Python version update ([#2802](https://github.com/stack-of-tasks/pinocchio/pull/2802)): @@ -35,10 +36,6 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/). ### Removed - Remove CMake < 3.22 details ([#2790](https://github.com/stack-of-tasks/pinocchio/pull/2790)) - -### Added -- Add Ellipsoid Joint to the joint collection, get ready for biomechanics. ([#....])(todo) - ## [3.8.0] - 2025-09-17 ### Added diff --git a/README.md b/README.md index 4b7e01e132..326833427b 100644 --- a/README.md +++ b/README.md @@ -288,6 +288,7 @@ In addition to the core dev team, the following people have also been involved i - [Lev Kozlov](https://github.com/lvjonok): Kinetic and potential energy regressors - [Simeon Nedelchev](https://github.com/simeon-ned): Pseudo inertia and Log-Cholesky parametrization - [Alexy Legrand](https://www.linkedin.com/in/alexy-legrand-125889232/): Viser color bug fixes +- [Pierre Puchaud](https://github.com/ipuch) & [Lucas Joseph](https://github.com/LucasJoseph): Ellipsoid Joint If you have participated in the development of **Pinocchio**, please add your name and contribution to this list. From c83ea117399b4492d68789a008030b2ca03366e9 Mon Sep 17 00:00:00 2001 From: ipuch Date: Thu, 13 Nov 2025 17:09:39 +0100 Subject: [PATCH 30/48] precommit precommit again --- examples/ellipsoid-joint-kinematics.py | 20 ++- .../multibody/joint/joint-ellipsoid.hpp | 40 ++--- unittest/joint-ellipsoid.cpp | 156 ++++++------------ 3 files changed, 77 insertions(+), 139 deletions(-) diff --git a/examples/ellipsoid-joint-kinematics.py b/examples/ellipsoid-joint-kinematics.py index 4a32d73628..227f651b12 100644 --- a/examples/ellipsoid-joint-kinematics.py +++ b/examples/ellipsoid-joint-kinematics.py @@ -5,12 +5,12 @@ - The position on the ellipsoid surface - The orientation of the body -Note: the joint is not purely normal to the ellipsoid surface; it translates along a ellipsoid, - but the rotational part is "normal" to a sphere. +Note: the joint is not purely normal to the ellipsoid surface; + it translates along a ellipsoid, but the rotational part is "normal" to a sphere. """ -import pinocchio as pin import numpy as np +import pinocchio as pin # For visualization (optional) try: @@ -107,7 +107,7 @@ def compute_kinematics_example(): model = create_ellipsoid_robot(radius_a, radius_b, radius_c) data = model.createData() - print(f"\nEllipsoid parameters:") + print("\nEllipsoid parameters:") print(f" radius_a (x-axis): {radius_a}") print(f" radius_b (y-axis): {radius_b}") print(f" radius_c (z-axis): {radius_c}") @@ -163,7 +163,8 @@ def compute_dynamics_example(): print(f"Acceleration: a = {a}") # Compute dynamics with RNEA (Recursive Newton-Euler Algorithm) - # Important Note: the generalized forces dimension matches the number of DOFs (3 for ellipsoid joint) + # Important Note: + # the generalized forces dimension matches the number of DOFs # but they are not pure torques due to the joint's nature and because the last # The last step of rnea is tau = S.T * f # where S is the joint motion subspace, and f the spatial force of the joint @@ -172,7 +173,9 @@ def compute_dynamics_example(): tau = pin.rnea(model, data, q, v, a) print(f"\nRequired torques (RNEA): τ = {tau}") - # For example tau[0] is a combination of S_11(q)* f_x + S_21(q)* f_y + S_31(q)* f_z + S_41(q)* τ_x + S_51(q)* τ_y + S_61(q)* τ_z + # For example tau[0] is a combination of + # S_11(q)* f_x + S_21(q)* f_y + S_31(q)* f_z + # + S_41(q)* τ_x + S_51(q)* τ_y + S_61(q)* τ_z # Create composite model for comparison of joint spatial forces `data.f[1]` composite_model = create_composite_model() @@ -196,7 +199,7 @@ def compute_dynamics_example(): composite_model, composite_data, q_composite, v_composite, a_composite ) print(f"\nComposite joint required torques (RNEA): τ = {tau_composite}") - print(f"\nComparison of spatial forces at the joint:") + print("\nComparison of spatial forces at the joint:") print(f" - Ellipsoid joint spatial force: f = {data.f[1]} ") print(f" - Composite joint spatial force: f = {composite_data.f[1]} ") @@ -296,7 +299,8 @@ def visualize_ellipsoid_motion(): if i % 50 == 0: print( - f" Frame {i}/500 - Configuration: [{q[0]:.3f}, {q[1]:.3f}, {q[2]:.3f}]" + f" Frame {i}/500 - Configuration: \n" + f" [{q[0]:.3f}, {q[1]:.3f}, {q[2]:.3f}]" ) # create extra motion that slides along the principal axes q0 diff --git a/include/pinocchio/multibody/joint/joint-ellipsoid.hpp b/include/pinocchio/multibody/joint/joint-ellipsoid.hpp index 6016c2fca1..ee02490e27 100644 --- a/include/pinocchio/multibody/joint/joint-ellipsoid.hpp +++ b/include/pinocchio/multibody/joint/joint-ellipsoid.hpp @@ -72,7 +72,7 @@ namespace pinocchio /// /// The configuration space uses three angles (q₀, q₁, q₂) representing: /// - Rotation about the x-axis - /// - Rotation about the y-axis + /// - Rotation about the y-axis /// - Spin about the "normal" direction /// /// The joint position on the ellipsoid surface is computed as: @@ -80,7 +80,7 @@ namespace pinocchio /// /// where \f$ a, b, c \f$ are the radii along the x, y, z axes respectively. /// - /// \note For non-spherical ellipsoids, the third rotation axis is only approximately + /// \note For non-spherical ellipsoids, the third rotation axis is only approximately /// normal to the surface. It corresponds to the normal of an equivalent sphere while /// the translation follows the true ellipsoid surface. The "normal" direction is /// truly normal only when all radii are equal (sphere case). @@ -159,7 +159,7 @@ namespace pinocchio { return JointDataDerived(); } - + /// @brief Default constructor. Creates a sphere (0.01, 0.01, 0.01). JointModelEllipsoidTpl() : radius_a(Scalar(0.01)) @@ -195,11 +195,12 @@ namespace pinocchio /// @param[in] c2, s2 Cosine and sine of q[2] /// @param[out] data Joint data where M will be stored void computeSpatialTransform( - Scalar c0, Scalar s0, Scalar c1, Scalar s1, Scalar c2, Scalar s2, JointDataDerived & data) const + Scalar c0, Scalar s0, Scalar c1, Scalar s1, Scalar c2, Scalar s2, JointDataDerived & data) + const { // clang-format off data.M.rotation() << c1 * c2 , -c1 * s2 , s1 , - c0 * s2 + c2 * s0 * s1 , c0 * c2 - s0 * s1 * s2 ,-c1 * s0 , + c0 * s2 + c2 * s0 * s1 , c0 * c2 - s0 * s1 * s2 ,-c1 * s0 , -c0 * c2 * s1 + s0 * s2 , c0 * s1 * s2 + c2 * s0 , c0 * c1; // clang-format on Scalar nx, ny, nz; @@ -208,7 +209,6 @@ namespace pinocchio nz = c0 * c1; data.M.translation() << radius_a * nx, radius_b * ny, radius_c * nz; - } template @@ -223,7 +223,7 @@ namespace pinocchio Scalar c2, s2; SINCOS(data.joint_q(2), &s2, &c2); - computeSpatialTransform(c0,s0,c1,s1,c2,s2, data); + computeSpatialTransform(c0, s0, c1, s1, c2, s2, data); computeMotionSubspace(s0, c0, s1, c1, s2, c2, data); } @@ -278,7 +278,7 @@ namespace pinocchio SINCOS(data.joint_q(2), &s2, &c2); computeSpatialTransform(c0, s0, c1, s1, c2, s2, data); - + Scalar dndotx_dqdot1, dndoty_dqdot0, dndoty_dqdot1, dndotz_dqdot0, dndotz_dqdot1; dndotx_dqdot1 = c1; dndoty_dqdot0 = -c0 * c1; @@ -392,15 +392,15 @@ namespace pinocchio S_32 = dndotx_dqdot1 * radius_a * s1 - dndoty_dqdot1 * radius_b * c1 * s0 + dndotz_dqdot1 * radius_c * c0 * c1; - data.S.matrix() << S_11 , S_12 , Scalar(0), - S_21 , S_22 , Scalar(0), + data.S.matrix() << S_11 , S_12 , Scalar(0), + S_21 , S_22 , Scalar(0), S_31 , S_32 , Scalar(0), - c1 * c2, s2 , Scalar(0), - -c1 * s2, c2 , Scalar(0), + c1 * c2, s2 , Scalar(0), + -c1 * s2, c2 , Scalar(0), s1 , Scalar(0), Scalar(1); // clang-format on } - + /// @brief Computes the bias acceleration c(q, v) = Sdot(q)·v. template void computeBiais( @@ -447,7 +447,7 @@ namespace pinocchio qdot1 = data.joint_v(1); qdot2 = data.joint_v(2); - // last columns and last element of the second column are zero, + // last columns and last element of the second column are zero, // so we do not compute them Scalar Sdot_11, Sdot_21, Sdot_31, Sdot_41, Sdot_51, Sdot_61; Scalar Sdot_12, Sdot_22, Sdot_32, Sdot_42, Sdot_52; @@ -496,7 +496,6 @@ namespace pinocchio * (dndoty_dqdot1 * radius_b * (c0 * s1 * s2 + c2 * s0) + dndotz_dqdot1 * radius_c * (-c0 * c2 + s0 * s1 * s2) + radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot0_dq1 - radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot0_dq1) + qdot1 * (dndotx_dqdot1 * radius_a * s1 * s2 - dndoty_dqdot1 * radius_b * c1 * s0 * s2 + dndotz_dqdot1 * radius_c * c0 * c1 * s2 - radius_a * c1 * s2 * d_dndotx_dqdot1_dq1 - radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot1_dq1 + radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot1_dq1) - qdot2 * (dndotx_dqdot1 * radius_a * c1 * c2 + dndoty_dqdot1 * radius_b * (c0 * s2 + c2 * s0 * s1) + dndotz_dqdot1 * radius_c * (-c0 * c2 * s1 + s0 * s2)); - // Row 3, Column 1 Sdot_31 = -qdot0 * c1 @@ -518,13 +517,10 @@ namespace pinocchio Sdot_42 = qdot2 * c2; Sdot_52 = -qdot2 * s2; - - data.c.toVector() << Sdot_11 * qdot0 + Sdot_12 * qdot1, - Sdot_21 * qdot0 + Sdot_22 * qdot1, - Sdot_31 * qdot0 + Sdot_32 * qdot1, - Sdot_41 * qdot0 + Sdot_42 * qdot1, - Sdot_51 * qdot0 + Sdot_52 * qdot1, - Sdot_61 * qdot0; + + data.c.toVector() << Sdot_11 * qdot0 + Sdot_12 * qdot1, Sdot_21 * qdot0 + Sdot_22 * qdot1, + Sdot_31 * qdot0 + Sdot_32 * qdot1, Sdot_41 * qdot0 + Sdot_42 * qdot1, + Sdot_51 * qdot0 + Sdot_52 * qdot1, Sdot_61 * qdot0; } }; // struct JointModelEllipsoidTpl diff --git a/unittest/joint-ellipsoid.cpp b/unittest/joint-ellipsoid.cpp index 460cc351e9..0a45d9e438 100644 --- a/unittest/joint-ellipsoid.cpp +++ b/unittest/joint-ellipsoid.cpp @@ -37,9 +37,7 @@ void addJointAndBody( /// \brief Compute motion subspace derivative Sdot analytically for JointModelEllipsoid Eigen::Matrix computeMotionSubspaceDerivative( - const JointModelEllipsoid & jmodel, - const Eigen::VectorXd & qs, - const Eigen::VectorXd & vs) + const JointModelEllipsoid & jmodel, const Eigen::VectorXd & qs, const Eigen::VectorXd & vs) { double c0, s0; SINCOS(qs(0), &s0, &c0); @@ -75,95 +73,42 @@ Eigen::Matrix computeMotionSubspaceDerivative( double d_dndotz_dqdot1_dq1 = -c0 * c1; // Translational part (rows 1-3) - double Sdot_11 = - qdot0 - * (-dndoty_dqdot0 * radius_b * (-c0 * c2 * s1 + s0 * s2) - + dndotz_dqdot0 * radius_c * (c0 * s2 + c2 * s0 * s1) - + radius_b * (c0 * s2 + c2 * s0 * s1) * d_dndoty_dqdot0_dq0 - + radius_c * (-c0 * c2 * s1 + s0 * s2) * d_dndotz_dqdot0_dq0) - + qdot1 - * (dndoty_dqdot0 * radius_b * c1 * c2 * s0 - - dndotz_dqdot0 * radius_c * c0 * c1 * c2 - + radius_b * (c0 * s2 + c2 * s0 * s1) * d_dndoty_dqdot0_dq1 - + radius_c * (-c0 * c2 * s1 + s0 * s2) * d_dndotz_dqdot0_dq1) - - qdot2 - * (dndoty_dqdot0 * radius_b * (-c0 * c2 + s0 * s1 * s2) - - dndotz_dqdot0 * radius_c * (c0 * s1 * s2 + c2 * s0)); - - double Sdot_12 = - qdot0 - * (-dndoty_dqdot1 * radius_b * (-c0 * c2 * s1 + s0 * s2) - + dndotz_dqdot1 * radius_c * (c0 * s2 + c2 * s0 * s1) - + radius_b * (c0 * s2 + c2 * s0 * s1) * d_dndoty_dqdot0_dq1 - + radius_c * (-c0 * c2 * s1 + s0 * s2) * d_dndotz_dqdot0_dq1) - + qdot1 - * (-dndotx_dqdot1 * radius_a * c2 * s1 - + dndoty_dqdot1 * radius_b * c1 * c2 * s0 - - dndotz_dqdot1 * radius_c * c0 * c1 * c2 - + radius_a * c1 * c2 * d_dndotx_dqdot1_dq1 - + radius_b * (c0 * s2 + c2 * s0 * s1) * d_dndoty_dqdot1_dq1 - + radius_c * (-c0 * c2 * s1 + s0 * s2) * d_dndotz_dqdot1_dq1) - - qdot2 - * (dndotx_dqdot1 * radius_a * c1 * s2 - + dndoty_dqdot1 * radius_b * (-c0 * c2 + s0 * s1 * s2) - - dndotz_dqdot1 * radius_c * (c0 * s1 * s2 + c2 * s0)); - - double Sdot_21 = - -qdot0 - * (dndoty_dqdot0 * radius_b * (c0 * s1 * s2 + c2 * s0) - + dndotz_dqdot0 * radius_c * (-c0 * c2 + s0 * s1 * s2) - + radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot0_dq0 - - radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot0_dq0) - - qdot1 - * (dndoty_dqdot0 * radius_b * c1 * s0 * s2 - - dndotz_dqdot0 * radius_c * c0 * c1 * s2 - + radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot0_dq1 - - radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot0_dq1) - - qdot2 - * (dndoty_dqdot0 * radius_b * (c0 * s2 + c2 * s0 * s1) - + dndotz_dqdot0 * radius_c * (-c0 * c2 * s1 + s0 * s2)); - - double Sdot_22 = - -qdot0 - * (dndoty_dqdot1 * radius_b * (c0 * s1 * s2 + c2 * s0) - + dndotz_dqdot1 * radius_c * (-c0 * c2 + s0 * s1 * s2) - + radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot0_dq1 - - radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot0_dq1) - + qdot1 - * (dndotx_dqdot1 * radius_a * s1 * s2 - - dndoty_dqdot1 * radius_b * c1 * s0 * s2 - + dndotz_dqdot1 * radius_c * c0 * c1 * s2 - - radius_a * c1 * s2 * d_dndotx_dqdot1_dq1 - - radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot1_dq1 - + radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot1_dq1) - - qdot2 - * (dndotx_dqdot1 * radius_a * c1 * c2 - + dndoty_dqdot1 * radius_b * (c0 * s2 + c2 * s0 * s1) - + dndotz_dqdot1 * radius_c * (-c0 * c2 * s1 + s0 * s2)); + double + Sdot_11 = + qdot0 + * (-dndoty_dqdot0 * radius_b * (-c0 * c2 * s1 + s0 * s2) + dndotz_dqdot0 * radius_c * (c0 * s2 + c2 * s0 * s1) + radius_b * (c0 * s2 + c2 * s0 * s1) * d_dndoty_dqdot0_dq0 + radius_c * (-c0 * c2 * s1 + s0 * s2) * d_dndotz_dqdot0_dq0) + + qdot1 * (dndoty_dqdot0 * radius_b * c1 * c2 * s0 - dndotz_dqdot0 * radius_c * c0 * c1 * c2 + radius_b * (c0 * s2 + c2 * s0 * s1) * d_dndoty_dqdot0_dq1 + radius_c * (-c0 * c2 * s1 + s0 * s2) * d_dndotz_dqdot0_dq1) + - qdot2 + * (dndoty_dqdot0 * radius_b * (-c0 * c2 + s0 * s1 * s2) - dndotz_dqdot0 * radius_c * (c0 * s1 * s2 + c2 * s0)); + + double + Sdot_12 = + qdot0 * (-dndoty_dqdot1 * radius_b * (-c0 * c2 * s1 + s0 * s2) + dndotz_dqdot1 * radius_c * (c0 * s2 + c2 * s0 * s1) + radius_b * (c0 * s2 + c2 * s0 * s1) * d_dndoty_dqdot0_dq1 + radius_c * (-c0 * c2 * s1 + s0 * s2) * d_dndotz_dqdot0_dq1) + qdot1 * (-dndotx_dqdot1 * radius_a * c2 * s1 + dndoty_dqdot1 * radius_b * c1 * c2 * s0 - dndotz_dqdot1 * radius_c * c0 * c1 * c2 + radius_a * c1 * c2 * d_dndotx_dqdot1_dq1 + radius_b * (c0 * s2 + c2 * s0 * s1) * d_dndoty_dqdot1_dq1 + radius_c * (-c0 * c2 * s1 + s0 * s2) * d_dndotz_dqdot1_dq1) + - qdot2 + * (dndotx_dqdot1 * radius_a * c1 * s2 + dndoty_dqdot1 * radius_b * (-c0 * c2 + s0 * s1 * s2) - dndotz_dqdot1 * radius_c * (c0 * s1 * s2 + c2 * s0)); + + double + Sdot_21 = -qdot0 * (dndoty_dqdot0 * radius_b * (c0 * s1 * s2 + c2 * s0) + dndotz_dqdot0 * radius_c * (-c0 * c2 + s0 * s1 * s2) + radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot0_dq0 - radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot0_dq0) - qdot1 * (dndoty_dqdot0 * radius_b * c1 * s0 * s2 - dndotz_dqdot0 * radius_c * c0 * c1 * s2 + radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot0_dq1 - radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot0_dq1) + - qdot2 + * (dndoty_dqdot0 * radius_b * (c0 * s2 + c2 * s0 * s1) + dndotz_dqdot0 * radius_c * (-c0 * c2 * s1 + s0 * s2)); + + double + Sdot_22 = + -qdot0 * (dndoty_dqdot1 * radius_b * (c0 * s1 * s2 + c2 * s0) + dndotz_dqdot1 * radius_c * (-c0 * c2 + s0 * s1 * s2) + radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot0_dq1 - radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot0_dq1) + qdot1 * (dndotx_dqdot1 * radius_a * s1 * s2 - dndoty_dqdot1 * radius_b * c1 * s0 * s2 + dndotz_dqdot1 * radius_c * c0 * c1 * s2 - radius_a * c1 * s2 * d_dndotx_dqdot1_dq1 - radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot1_dq1 + radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot1_dq1) + - qdot2 + * (dndotx_dqdot1 * radius_a * c1 * c2 + dndoty_dqdot1 * radius_b * (c0 * s2 + c2 * s0 * s1) + dndotz_dqdot1 * radius_c * (-c0 * c2 * s1 + s0 * s2)); double Sdot_31 = -qdot0 * c1 - * (dndoty_dqdot0 * radius_b * c0 - + dndotz_dqdot0 * radius_c * s0 - + radius_b * s0 * d_dndoty_dqdot0_dq0 - - radius_c * c0 * d_dndotz_dqdot0_dq0) + * (dndoty_dqdot0 * radius_b * c0 + dndotz_dqdot0 * radius_c * s0 + radius_b * s0 * d_dndoty_dqdot0_dq0 - radius_c * c0 * d_dndotz_dqdot0_dq0) + qdot1 - * (-c1 * (radius_b * s0 * d_dndoty_dqdot0_dq1 - radius_c * c0 * d_dndotz_dqdot0_dq1) - + s1 * (dndoty_dqdot0 * radius_b * s0 - dndotz_dqdot0 * radius_c * c0)); + * (-c1 * (radius_b * s0 * d_dndoty_dqdot0_dq1 - radius_c * c0 * d_dndotz_dqdot0_dq1) + s1 * (dndoty_dqdot0 * radius_b * s0 - dndotz_dqdot0 * radius_c * c0)); double Sdot_32 = -qdot0 * c1 - * (dndoty_dqdot1 * radius_b * c0 - + dndotz_dqdot1 * radius_c * s0 - + radius_b * s0 * d_dndoty_dqdot0_dq1 - - radius_c * c0 * d_dndotz_dqdot0_dq1) + * (dndoty_dqdot1 * radius_b * c0 + dndotz_dqdot1 * radius_c * s0 + radius_b * s0 * d_dndoty_dqdot0_dq1 - radius_c * c0 * d_dndotz_dqdot0_dq1) + qdot1 - * (dndotx_dqdot1 * radius_a * c1 - + dndoty_dqdot1 * radius_b * s0 * s1 - - dndotz_dqdot1 * radius_c * c0 * s1 - + radius_a * s1 * d_dndotx_dqdot1_dq1 - - radius_b * c1 * s0 * d_dndoty_dqdot1_dq1 - + radius_c * c0 * c1 * d_dndotz_dqdot1_dq1); + * (dndotx_dqdot1 * radius_a * c1 + dndoty_dqdot1 * radius_b * s0 * s1 - dndotz_dqdot1 * radius_c * c0 * s1 + radius_a * s1 * d_dndotx_dqdot1_dq1 - radius_b * c1 * s0 * d_dndoty_dqdot1_dq1 + radius_c * c0 * c1 * d_dndotz_dqdot1_dq1); // Angular part (rows 4-6) double Sdot_41 = -(qdot1 * c2 * s1 + qdot2 * c1 * s2); @@ -175,12 +120,8 @@ Eigen::Matrix computeMotionSubspaceDerivative( // Build and return 6x3 matrix Eigen::Matrix Sdot; - Sdot << Sdot_11, Sdot_12, 0.0, - Sdot_21, Sdot_22, 0.0, - Sdot_31, Sdot_32, 0.0, - Sdot_41, Sdot_42, 0.0, - Sdot_51, Sdot_52, 0.0, - Sdot_61, 0.0, 0.0; + Sdot << Sdot_11, Sdot_12, 0.0, Sdot_21, Sdot_22, 0.0, Sdot_31, Sdot_32, 0.0, Sdot_41, Sdot_42, + 0.0, Sdot_51, Sdot_52, 0.0, Sdot_61, 0.0, 0.0; return Sdot; } @@ -238,7 +179,6 @@ Eigen::Matrix finiteDiffSdot( return Sdot_fd; } - SE3::Vector3 computeTranslations(const JointModelEllipsoid & jmodel, const Eigen::VectorXd & qs) { double c0, s0; @@ -255,9 +195,7 @@ SE3::Vector3 computeTranslations(const JointModelEllipsoid & jmodel, const Eigen } SE3::Vector3 computeTranslationVelocities( - const JointModelEllipsoid & jmodel, - const Eigen::VectorXd & qs, - const Eigen::VectorXd & vs) + const JointModelEllipsoid & jmodel, const Eigen::VectorXd & qs, const Eigen::VectorXd & vs) { double c0, s0; SINCOS(qs(0), &s0, &c0); @@ -271,17 +209,17 @@ SE3::Vector3 computeTranslationVelocities( return v; } -SE3:: Vector3 computeTranslationAccelerations( +SE3::Vector3 computeTranslationAccelerations( const JointModelEllipsoid & jmodel, const Eigen::VectorXd & qs, const Eigen::VectorXd & vs, - const Eigen::VectorXd & as) + const Eigen::VectorXd & as) { double c0, s0; SINCOS(qs(0), &s0, &c0); double c1, s1; SINCOS(qs(1), &s1, &c1); - SE3:: Vector3 a; + SE3::Vector3 a; a(0) = jmodel.radius_a * (-s1 * vs(1) * vs(1) + c1 * as(1)); a(1) = jmodel.radius_b @@ -292,7 +230,6 @@ SE3:: Vector3 computeTranslationAccelerations( return a; } - BOOST_AUTO_TEST_SUITE(JointEllipsoid) /// @brief Test the rotationnal equivalence between JointModelEllipsoid and JointModelSphericalZYX @@ -389,8 +326,7 @@ BOOST_AUTO_TEST_CASE(vsSphericalZYX) // S_s * qdotdot_s + c_s.angular() = Sdot_e * qd_e + S_e * qdotdot_e // Solving for qdotdot_e: // S_e * qdotdot_e = c_s.angular()+ S_s * qdotdot_s - Sdot_e * qd_e - Eigen::Vector3d qdotdot_e = - S_e.inverse() * (S_s * qdotdot_s + jDataSphereFK.c.angular() - c_e); + Eigen::Vector3d qdotdot_e = S_e.inverse() * (S_s * qdotdot_s + jDataSphereFK.c.angular() - c_e); // Verify angular accelerations match Eigen::Vector3d wdot_s = jDataSphereFK.c.angular() + S_s * qdotdot_s; @@ -409,7 +345,8 @@ BOOST_AUTO_TEST_CASE(vsSphericalZYX) BOOST_CHECK(dataEllipsoid.f[1].isApprox(dataSphericalZYX.f[1])); } -/// @brief Test the equivalence between JointModelEllipsoid and a Composite joint (Tx, Ty, Tz, Rx, Ry, Rz) +/// @brief Test the equivalence between JointModelEllipsoid and a Composite joint (Tx, Ty, Tz, Rx, +/// Ry, Rz) BOOST_AUTO_TEST_CASE(vsCompositeTxTyTzRxRyRz) { using namespace pinocchio; @@ -459,7 +396,8 @@ BOOST_AUTO_TEST_CASE(vsCompositeTxTyTzRxRyRz) Eigen::VectorXd qdot_ellipsoid(3); qdot_ellipsoid << 0.1, 0.2, 0.3; - Eigen::Vector3d v_linear = computeTranslationVelocities(jointModelEllipsoid, q_ellipsoid, qdot_ellipsoid); + Eigen::Vector3d v_linear = + computeTranslationVelocities(jointModelEllipsoid, q_ellipsoid, qdot_ellipsoid); Eigen::VectorXd qdot_composite(6); qdot_composite << v_linear, qdot_ellipsoid; @@ -471,8 +409,8 @@ BOOST_AUTO_TEST_CASE(vsCompositeTxTyTzRxRyRz) // Acceleration test Eigen::VectorXd qddot_ellipsoid(3); qddot_ellipsoid << 0.01, 0.02, 0.03; - Eigen::Vector3d a_linear = computeTranslationAccelerations(jointModelEllipsoid, - q_ellipsoid, qdot_ellipsoid, qddot_ellipsoid); + Eigen::Vector3d a_linear = computeTranslationAccelerations( + jointModelEllipsoid, q_ellipsoid, qdot_ellipsoid, qddot_ellipsoid); Eigen::VectorXd qddot_composite(6); qddot_composite << a_linear, qddot_ellipsoid; @@ -550,11 +488,11 @@ BOOST_AUTO_TEST_CASE(RNEAvsABA) aba(model, data_aba, q, v, tau_rnea, Convention::WORLD); BOOST_CHECK_MESSAGE( - a.isApprox(data_aba.ddq, 1e-9), - "RNEA and ABA inconsistent at trial " << trial << "\n" - << "Configuration: " << q.transpose() << "\n" - << "Expected: " << a.transpose() << "\n" - << "Got: " << data_aba.ddq.transpose()); + a.isApprox(data_aba.ddq, 1e-9), "RNEA and ABA inconsistent at trial " + << trial << "\n" + << "Configuration: " << q.transpose() << "\n" + << "Expected: " << a.transpose() << "\n" + << "Got: " << data_aba.ddq.transpose()); } } From 535de5a80dbb5e2b10a8eb32b1a4b68ba7cae2a6 Mon Sep 17 00:00:00 2001 From: ipuch Date: Thu, 13 Nov 2025 17:28:41 +0100 Subject: [PATCH 31/48] docs(joint-ellipsoid) - afeatures scooting spherical ZYX clean(joint-ellipsoid): extra include removed hard precommit --- doc/a-features/c-joints.md | 6 ++++++ include/pinocchio/multibody/joint/joint-ellipsoid.hpp | 3 --- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/doc/a-features/c-joints.md b/doc/a-features/c-joints.md index c89fda9aa2..a2740cf7a5 100644 --- a/doc/a-features/c-joints.md +++ b/doc/a-features/c-joints.md @@ -9,6 +9,12 @@ belongs to one of the following categories: - **Revolute** joints, rotating around a fixed axis, either one of \f$X,Y,Z\f$ or a custom one; - **Prismatic** joints, translating along any fixed axis, as in the revolute case; - **Spherical** joints, free rotations in the 3D space; +- **Spherical ZYX** joints, free rotations parameterized by ZYX Euler angles; +- **Ellipsoid** joints, constraining motion to an ellipsoid surface with 3-DOF (2 rotations + 1 spin), + useful for biomechanics. The translation follows the ellipsoid surface \f$\left(\frac{x}{a}\right)^2 + \left(\frac{y}{b}\right)^2 + \left(\frac{z}{c}\right)^2 = 1\f$, + while the rotational part uses the normal direction of an equivalent sphere. + This approximation is exact only when \f$a = b = c\f$ (spherical case). + See Seth et al., "Minimal formulation of joint motion for biomechanisms," Nonlinear Dynamics 62(1):291-303, 2010. - **Translation** joints, for free translations in the 3D space; - **Planar** joints, for free movements in the 2D space; - **Free-floating** joints, for free movements in the 3D space. Planar and free-floating joints are meant to be diff --git a/include/pinocchio/multibody/joint/joint-ellipsoid.hpp b/include/pinocchio/multibody/joint/joint-ellipsoid.hpp index ee02490e27..71c619efef 100644 --- a/include/pinocchio/multibody/joint/joint-ellipsoid.hpp +++ b/include/pinocchio/multibody/joint/joint-ellipsoid.hpp @@ -10,9 +10,6 @@ #include "pinocchio/multibody/joint-motion-subspace.hpp" #include "pinocchio/math/sincos.hpp" #include "pinocchio/math/matrix.hpp" -#include "pinocchio/spatial/inertia.hpp" -#include "pinocchio/spatial/skew.hpp" -#include "pinocchio/multibody/joint-motion-subspace.hpp" namespace pinocchio { From 4dcfbd37a4da56a3aa124697c7623898b5332079 Mon Sep 17 00:00:00 2001 From: Pierre Puchaud <40755537+Ipuch@users.noreply.github.com> Date: Wed, 19 Nov 2025 09:08:16 +0100 Subject: [PATCH 32/48] Update include/pinocchio/multibody/joint/joint-ellipsoid.hpp Co-authored-by: Joris Vaillant --- include/pinocchio/multibody/joint/joint-ellipsoid.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/pinocchio/multibody/joint/joint-ellipsoid.hpp b/include/pinocchio/multibody/joint/joint-ellipsoid.hpp index 71c619efef..84bf0d9dc1 100644 --- a/include/pinocchio/multibody/joint/joint-ellipsoid.hpp +++ b/include/pinocchio/multibody/joint/joint-ellipsoid.hpp @@ -192,7 +192,7 @@ namespace pinocchio /// @param[in] c2, s2 Cosine and sine of q[2] /// @param[out] data Joint data where M will be stored void computeSpatialTransform( - Scalar c0, Scalar s0, Scalar c1, Scalar s1, Scalar c2, Scalar s2, JointDataDerived & data) + const Scalar& c0, const Scalar& s0, const Scalar& c1, const Scalar& s1, const Scalar& c2, const Scalar& s2, JointDataDerived & data) const const { // clang-format off From 2f6152a4849c81565c572a9f5a2b26f7b576b787 Mon Sep 17 00:00:00 2001 From: Pierre Puchaud <40755537+Ipuch@users.noreply.github.com> Date: Wed, 19 Nov 2025 09:22:18 +0100 Subject: [PATCH 33/48] Update bindings/python/parsers/graph/expose-edges.cpp Co-authored-by: Joris Vaillant --- bindings/python/parsers/graph/expose-edges.cpp | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/bindings/python/parsers/graph/expose-edges.cpp b/bindings/python/parsers/graph/expose-edges.cpp index 4aaa0f20c9..c1251c2d31 100644 --- a/bindings/python/parsers/graph/expose-edges.cpp +++ b/bindings/python/parsers/graph/expose-edges.cpp @@ -118,11 +118,8 @@ namespace pinocchio "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", "Represents an ellipsoidal joint.", - bp::init( + .def_readonly("nv", &JointEllipsoid::nv, "Number of tangent variables.") + .def(bp::init( 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.") From 9b955f7a4d0ba2fcc7d2849908c77d89fb3cef75 Mon Sep 17 00:00:00 2001 From: ipuch Date: Wed, 19 Nov 2025 09:23:51 +0100 Subject: [PATCH 34/48] review(@jorisv): docs and test --- README.md | 3 +- examples/CMakeLists.txt | 4 +- .../multibody/joint/joint-ellipsoid.hpp | 42 ++++++++++--------- 3 files changed, 27 insertions(+), 22 deletions(-) diff --git a/README.md b/README.md index 326833427b..b60411163c 100644 --- a/README.md +++ b/README.md @@ -288,7 +288,8 @@ In addition to the core dev team, the following people have also been involved i - [Lev Kozlov](https://github.com/lvjonok): Kinetic and potential energy regressors - [Simeon Nedelchev](https://github.com/simeon-ned): Pseudo inertia and Log-Cholesky parametrization - [Alexy Legrand](https://www.linkedin.com/in/alexy-legrand-125889232/): Viser color bug fixes -- [Pierre Puchaud](https://github.com/ipuch) & [Lucas Joseph](https://github.com/LucasJoseph): Ellipsoid Joint +- [Pierre Puchaud](https://github.com/ipuch): Ellipsoid Joint +- [Lucas Joseph](https://github.com/LucasJoseph): Ellipsoid Joint If you have participated in the development of **Pinocchio**, please add your name and contribution to this list. diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 3ea18a994f..7d3c22dcfa 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -96,7 +96,9 @@ if(BUILD_PYTHON_INTERFACE) forward-dynamics-derivatives inverse-dynamics-derivatives model-graph - model-configuration-converter) + model-configuration-converter + ellipsoid-joint-kinematics + ) if(BUILD_WITH_URDF_SUPPORT) list( diff --git a/include/pinocchio/multibody/joint/joint-ellipsoid.hpp b/include/pinocchio/multibody/joint/joint-ellipsoid.hpp index 84bf0d9dc1..efe9ae11dd 100644 --- a/include/pinocchio/multibody/joint/joint-ellipsoid.hpp +++ b/include/pinocchio/multibody/joint/joint-ellipsoid.hpp @@ -65,25 +65,6 @@ namespace pinocchio typedef _Scalar Scalar; }; - /// \brief Ellipsoid joint - constrains motion to ellipsoid surface with 3-DOF. - /// - /// The configuration space uses three angles (q₀, q₁, q₂) representing: - /// - Rotation about the x-axis - /// - Rotation about the y-axis - /// - Spin about the "normal" direction - /// - /// The joint position on the ellipsoid surface is computed as: - /// \f$ \mathbf{p} = (a \sin q_1, -b \sin q_0 \cos q_1, c \cos q_0 \cos q_1) \f$ - /// - /// where \f$ a, b, c \f$ are the radii along the x, y, z axes respectively. - /// - /// \note For non-spherical ellipsoids, the third rotation axis is only approximately - /// normal to the surface. It corresponds to the normal of an equivalent sphere while - /// the translation follows the true ellipsoid surface. The "normal" direction is - /// truly normal only when all radii are equal (sphere case). - /// - /// \sa Seth et al., "Minimal formulation of joint motion for biomechanisms," - /// Nonlinear Dynamics 62(1):291-303, 2010. template struct JointDataEllipsoidTpl : public JointDataBase> { @@ -131,7 +112,27 @@ namespace pinocchio }; // struct JointDataEllipsoidTpl + PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelEllipsoidTpl); + /// \brief Ellipsoid joint - constrains motion to ellipsoid surface with 3-DOF. + /// + /// The configuration space uses three angles (q₀, q₁, q₂) representing: + /// - Rotation about the x-axis + /// - Rotation about the y-axis + /// - Spin about the "normal" direction + /// + /// The joint position on the ellipsoid surface is computed as: + /// \f$ \mathbf{p} = (a \sin q_1, -b \sin q_0 \cos q_1, c \cos q_0 \cos q_1) \f$ + /// + /// where \f$ a, b, c \f$ are the radii along the x, y, z axes respectively. + /// + /// \note For non-spherical ellipsoids, the third rotation axis is only approximately + /// normal to the surface. It corresponds to the normal of an equivalent sphere while + /// the translation follows the true ellipsoid surface. The "normal" direction is + /// truly normal only when all radii are equal (sphere case). + /// + /// \sa Seth et al., "Minimal formulation of joint motion for biomechanisms," + /// Nonlinear Dynamics 62(1):291-303, 2010. template struct JointModelEllipsoidTpl : public JointModelBase> { @@ -324,11 +325,12 @@ namespace pinocchio } /// \returns An expression of *this with the Scalar type casted to NewScalar. + /// TU todo. template JointModelEllipsoidTpl cast() const { typedef JointModelEllipsoidTpl ReturnType; - ReturnType res; + ReturnType res(radius_a, radius_b, radius_c); res.setIndexes(id(), idx_q(), idx_v(), idx_vExtended()); return res; } From 31ca2302b4f93760630106fad22cbb0e7adcfc0c Mon Sep 17 00:00:00 2001 From: ipuch Date: Wed, 19 Nov 2025 09:24:02 +0100 Subject: [PATCH 35/48] scout(freeflyer docs) --- include/pinocchio/multibody/joint/joint-free-flyer.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/include/pinocchio/multibody/joint/joint-free-flyer.hpp b/include/pinocchio/multibody/joint/joint-free-flyer.hpp index 0de0c5ed53..4ac2678b09 100644 --- a/include/pinocchio/multibody/joint/joint-free-flyer.hpp +++ b/include/pinocchio/multibody/joint/joint-free-flyer.hpp @@ -256,6 +256,8 @@ namespace pinocchio }; // struct JointDataFreeFlyerTpl + + PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelFreeFlyerTpl); /// @brief Free-flyer joint in \f$SE(3)\f$. /// /// A free-flyer joint adds seven coordinates to the configuration space. @@ -280,7 +282,6 @@ namespace pinocchio /// corresponding to the angular velocity from the child frame to the /// parent frame, expressed in the child frame (body angular velocity of the /// child frame). - PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelFreeFlyerTpl); template struct JointModelFreeFlyerTpl : public JointModelBase> { From ada9fb1fe3cff4b338654102f6dcad8704e3835d Mon Sep 17 00:00:00 2001 From: ipuch Date: Wed, 19 Nov 2025 17:54:59 +0100 Subject: [PATCH 36/48] refactor(model-graph-algo): AddJointBetweenBodies precommit --- examples/CMakeLists.txt | 3 +- .../multibody/joint/joint-ellipsoid.hpp | 10 ++-- .../multibody/joint/joint-free-flyer.hpp | 1 - src/parsers/graph/model-graph-algo.cpp | 48 +++++++++---------- 4 files changed, 31 insertions(+), 31 deletions(-) diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 7d3c22dcfa..32fe2e1936 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -97,8 +97,7 @@ if(BUILD_PYTHON_INTERFACE) inverse-dynamics-derivatives model-graph model-configuration-converter - ellipsoid-joint-kinematics - ) + ellipsoid-joint-kinematics) if(BUILD_WITH_URDF_SUPPORT) list( diff --git a/include/pinocchio/multibody/joint/joint-ellipsoid.hpp b/include/pinocchio/multibody/joint/joint-ellipsoid.hpp index efe9ae11dd..d18670ed6e 100644 --- a/include/pinocchio/multibody/joint/joint-ellipsoid.hpp +++ b/include/pinocchio/multibody/joint/joint-ellipsoid.hpp @@ -112,7 +112,6 @@ namespace pinocchio }; // struct JointDataEllipsoidTpl - PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelEllipsoidTpl); /// \brief Ellipsoid joint - constrains motion to ellipsoid surface with 3-DOF. /// @@ -193,8 +192,13 @@ namespace pinocchio /// @param[in] c2, s2 Cosine and sine of q[2] /// @param[out] data Joint data where M will be stored void computeSpatialTransform( - const Scalar& c0, const Scalar& s0, const Scalar& c1, const Scalar& s1, const Scalar& c2, const Scalar& s2, JointDataDerived & data) const - const + const Scalar & c0, + const Scalar & s0, + const Scalar & c1, + const Scalar & s1, + const Scalar & c2, + const Scalar & s2, + JointDataDerived & data) const { // clang-format off data.M.rotation() << c1 * c2 , -c1 * s2 , s1 , diff --git a/include/pinocchio/multibody/joint/joint-free-flyer.hpp b/include/pinocchio/multibody/joint/joint-free-flyer.hpp index 4ac2678b09..38804f2f55 100644 --- a/include/pinocchio/multibody/joint/joint-free-flyer.hpp +++ b/include/pinocchio/multibody/joint/joint-free-flyer.hpp @@ -256,7 +256,6 @@ namespace pinocchio }; // struct JointDataFreeFlyerTpl - PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelFreeFlyerTpl); /// @brief Free-flyer joint in \f$SE(3)\f$. /// diff --git a/src/parsers/graph/model-graph-algo.cpp b/src/parsers/graph/model-graph-algo.cpp index 7550fae08b..45dd0eb1a6 100644 --- a/src/parsers/graph/model-graph-algo.cpp +++ b/src/parsers/graph/model-graph-algo.cpp @@ -267,17 +267,9 @@ namespace pinocchio { } - template - void operator()(const JointGraph & /*joint*/, const FrameGraph & /*f_*/) - { - PINOCCHIO_THROW_PRETTY( - std::invalid_argument, - "Graph - Invalid joint between non body frames. Non body frames can " - "only be added with Fixed joint"); - } - + // Default implementation for adding a joint between two body frames template - void operator()(const JointGraph & joint, const BodyFrame & b_f) + void addJointBetweenBodies(const JointGraph & joint, const BodyFrame & b_f) { if (boost::get(&source_vertex.frame) == nullptr) { @@ -295,7 +287,7 @@ namespace pinocchio edge.jlimit.friction, edge.jlimit.damping); model.addJointFrame(j_id); - model.appendBodyToJoint(j_id, b_f.inertia); // check this + model.appendBodyToJoint(j_id, b_f.inertia); model.addBodyFrame(target_vertex.name, j_id, body_pose); // armature @@ -303,6 +295,22 @@ namespace pinocchio edge.jlimit.armature; } + template + void operator()(const JointGraph & /*joint*/, const FrameGraph & /*f_*/) + { + PINOCCHIO_THROW_PRETTY( + std::invalid_argument, + "Graph - Invalid joint between non body frames. Non body frames can " + "only be added with Fixed joint"); + } + + template + void operator()(const JointGraph & joint, const BodyFrame & b_f) + { + // Call default implementation + addJointBetweenBodies(joint, b_f); + } + template void operator()(const JointFixed & joint, const FrameGraph & f_) { @@ -350,21 +358,11 @@ namespace pinocchio if (!edge.forward) PINOCCHIO_THROW_PRETTY( std::invalid_argument, "Graph - JointEllipsoid cannot be reversed yet."); + // The ellipsoid joint cannot be reversed because of the way the motion subspace is + // defined. The motion subspace is defined in the parent frame, So reversing the joint + // would require to change the motion subspace accordingly. - if (boost::get(&source_vertex.frame) == nullptr) - PINOCCHIO_THROW_PRETTY( - std::invalid_argument, "Graph - Invalid joint between a body and a non body frame."); - - const SE3 & joint_pose = edge.source_to_joint; - const SE3 & body_pose = edge.joint_to_target; - - const Frame previous_body = model.frames[model.getFrameId(source_vertex.name, BODY)]; - JointIndex j_id = model.addJoint( - previous_body.parentJoint, cjm(joint), previous_body.placement * joint_pose, edge.name); - - model.addJointFrame(j_id); - model.appendBodyToJoint(j_id, b_f.inertia); // check this - model.addBodyFrame(target_vertex.name, j_id, body_pose); + addJointBetweenBodies(joint, b_f); } void operator()(const JointFixed & joint, const BodyFrame & b_f) From 1d7b57e7d2c497c43d862a2207af6837815bd533 Mon Sep 17 00:00:00 2001 From: ipuch Date: Wed, 26 Nov 2025 11:00:57 +0100 Subject: [PATCH 37/48] doc(new joint): guide for developers. --- doc/e-development/Implement_a_new_joint.md | 1065 ++++++++++++++++++++ 1 file changed, 1065 insertions(+) create mode 100644 doc/e-development/Implement_a_new_joint.md diff --git a/doc/e-development/Implement_a_new_joint.md b/doc/e-development/Implement_a_new_joint.md new file mode 100644 index 0000000000..9a1ae11f4f --- /dev/null +++ b/doc/e-development/Implement_a_new_joint.md @@ -0,0 +1,1065 @@ +#How to implement a new joint in Pinocchio +**Tags:** #Pinocchio #tutorial #joints #implementation #motionsubspace + +This guide provides a comprehensive, step-by-step approach to implementing a new joint type in Pinocchio. It covers mathematical foundations, core implementation, parser integration, and testing requirements. + +--- +## Table of Contents + +1. [Overview](#overview) +2. [Pinocchio Directory Structure](#pinocchio-directory-structure) +3. [Prerequisites](#prerequisites) +4. [Step 1: Mathematical Foundation](#step-1-mathematical-foundation) +5. [Step 2: Core Joint Implementation](#step-2-core-joint-implementation) +6. [Step 3: Motion Subspace Specialization](#step-3-motion-subspace-specialization) +7. [Step 4: Model Graph Integration](#step-4-model-graph-integration) +8. [Step 5: Testing](#step-5-testing) +9. [Summary Checklist](#summary-checklist) + +--- +## Overview + +Implementing a new joint in Pinocchio involves three major phases: + +1. **Core Implementation**: Define the joint's kinematics and dynamics +2. **Parser Integration**: Enable graph-based model construction and reversable joints +3. **Testing**: Verify correctness through comprehensive unit tests + +This document guides you through each phase systematically. + +--- +## Pinocchio Directory Structure + +Before diving into implementation, familiarize yourself with Pinocchio's organization. The codebase is structured as follows: + +``` +pinocchio/ +├── include/pinocchio/ # C++ headers (main implementation) +│ ├── multibody/ +│ │ ├── joint/ # Joint definitions +│ │ │ ├── fwd.hpp # Forward declarations +│ │ │ ├── joints.hpp # Includes all joint headers +│ │ │ ├── joint-collection.hpp # Joint variant definition +│ │ │ ├── joint-revolute.hpp # Example: Revolute joint +│ │ │ ├── joint-spherical-ZYX.hpp # Example: Spherical ZYX joint +│ │ │ └── joint-.hpp # Your new joint goes here +│ │ ├── model.hpp # Model class +│ │ └── data.hpp # Data class +│ ├── parsers/ +│ │ └── graph/ # Graph-based model construction +│ │ ├── joints.hpp # Graph joint definitions +│ │ ├── graph-visitor.hpp # Graph visitors +│ │ └── model-configuration-converter.hxx # Config converters +│ └── serialization/ # Boost serialization support +│ ├── joints-model.hpp +│ └── joints-data.hpp +├── src/ # C++ source files +│ └── parsers/ +│ └── graph/ +│ ├── model-graph.cpp # Graph construction logic +│ └── model-graph-algo.cpp # Graph algorithms & visitors +├── bindings/python/ # Python bindings +│ ├── multibody/joint/ +│ │ ├── joints-models.hpp # Python exposure for JointModel +│ │ └── joints-datas.hpp # Python exposure for JointData +│ └── parsers/graph/ +│ └── expose-edges.cpp # Python exposure for graph joints +├── unittest/ # Unit tests +│ ├── joint-.cpp # Your joint tests go here +│ ├── model-graph.cpp # Graph tests +│ ├── model-configuration-converter.cpp +│ └── CMakeLists.txt # Add your test here +└── examples/ # Usage examples + └── -joint-kinematics.py # Your example goes here +``` + +**Key directories you'll work in:** +1. **`include/pinocchio/multibody/joint/`** - Core joint implementation (C++ headers) +2. **`include/pinocchio/parsers/graph/`** - Graph integration (for URDF support) +3. **`src/parsers/graph/`** - Graph visitor implementations (C++ source) +4. **`bindings/python/`** - Python bindings +5. **`unittest/`** - All tests + +> [!IMPORTANT] +> Most joint code is **header-only** (in `include/`), but graph visitors require **source files** (in `src/`). + +--- +## Prerequisites + +Before implementing a new joint, ensure you understand: + +- **Spatial algebra**: Featherstone's spatial notation (6D spatial vectors, spatial transforms) +- **Joint kinematics**: Configuration space, tangent space, motion subspace +- **Pinocchio's architecture**: JointModel/JointData pattern, traits system +- **C++ templates**: Pinocchio heavily uses templates for scalar types and options + +### Available Joints in Pinocchio + +Pinocchio already implements many joint types. Study these as references: + +| Joint Type | File | nq | nv | Description | +|------------|------|----|----|-------------| +| **Revolute** | `joint-revolute.hpp` | 1 | 1 | Single-axis rotation (sparse S) | +| **Prismatic** | `joint-prismatic.hpp` | 1 | 1 | Single-axis translation (sparse S) | +| **Revolute Unaligned** | `joint-revolute-unaligned.hpp` | 1 | 1 | Revolute with arbitrary axis | +| **Prismatic Unaligned** | `joint-prismatic-unaligned.hpp` | 1 | 1 | Prismatic with arbitrary axis | +| **Revolute Unbounded** | `joint-revolute-unbounded.hpp` | 2 | 1 | Revolute without configuration limit | +| **Prismatic Unbounded** | `joint-prismatic-unbounded.hpp` | 2 | 1 | Prismatic without configuration limit | +| **Spherical** | `joint-spherical.hpp` | 4 | 3 | 3-DOF rotation using quaternion | +| **Spherical ZYX** | `joint-spherical-ZYX.hpp` | 3 | 3 | 3-DOF rotation using Euler angles | +| **Free Flyer** | `joint-free-flyer.hpp` | 7 | 6 | 6-DOF (translation + rotation) | +| **Planar** | `joint-planar.hpp` | 4 | 3 | 2D translation + 1D rotation | +| **Translation** | `joint-translation.hpp` | 3 | 3 | 3-DOF translation | +| **Universal** | `joint-universal.hpp` | 2 | 2 | 2-DOF rotation (Cardan joint) | +| **Helical** | `joint-helical.hpp` | 1 | 1 | Coupled rotation + translation | +| **Ellipsoid** | `joint-ellipsoid.hpp` | 3 | 3 | Ellipsoid surface constraint | + +> [!TIP] +> - For simple joints (1-DOF), study **Revolute** or **Prismatic** +> - For Euler-angle joints, study **Spherical ZYX** +> - For constrained surfaces, study **Ellipsoid** +> - All joint headers are in `include/pinocchio/multibody/joint/` + +### Key Mathematical Concepts + +A joint is characterized by: +- **Configuration dimension** `nq`: Size of the configuration vector $q$ +- **Velocity dimension** `nv`: Size of the velocity vector $\dot{q}$ +- **Motion subspace** $S(q)$: Maps joint velocity to spatial velocity: $v_J = S(q)\dot{q}$ +- **Spatial transform** $M(q)$: Placement from parent to child frame +- **Bias acceleration** $c(q,\dot{q})$: Often written as $\dot{S}\dot{q}$ + +--- +## Step 1: Mathematical Foundation + +**What You'll Do:** Derive the mathematical equations that govern your joint's behavior. You will compute the spatial transform $M(q)$, motion subspace $S(q)$, and bias acceleration $c(q,\dot{q})$ using pen-and-paper or symbolic tools. + +**Goal:** By the end of this step, you should have closed-form expressions ready to implement in code. +### 1.1 Define the Joint Configuration + +First, determine your joint's configuration and velocity spaces: +- What are the generalized coordinates $q = [q_0, q_1, \ldots, q_{n-1}]$? +- What is the relationship between $q$ and $\dot{q}$ (are they in the same space)? + +> [!IMPORTANT] + +> For non-Euclidean joints (e.g., spherical joints using quaternions), `nq` may differ from `nv`. Ensure you understand the Lie group structure. +### 1.2 Derive the Spatial Transform + +Compute the spatial transformation matrix from parent frame **p** to child frame **c**: +$$ +{^p}X_c(q) = \begin{bmatrix} R(q) & 0 \\ -R(q)p(q)^\times & R(q) \end{bmatrix} +$$ + Where: + +- $R(q) \in SO(3)$ is the rotation matrix +- $p(q) \in \mathbb{R}^3$ is the translation vector +- $p^\times$ denotes the skew-symmetric matrix + +> [!EXAMPLE - Ellipsoid Joint] + The ellipsoid joint has translation $p(q)$ and rotation $R(q)$ that depends on ellipsoid radii $(a,b,c)$ and configuration $(q_0, q_1, q_2)$: +$$\begin{align} p(q) = \begin{bmatrix} a\sin q_1 \\ -b\sin q_0\cos q_1 \\ c\cos q_0\cos q_1 \end{bmatrix} & & R(q) = R_x(q_0) R_y(q_1) R_z(q_2) \end{align}$$ +### 1.3 Derive the Motion Subspace + + The motion subspace is the Jacobian of the spatial velocity with respect to joint velocity: +$$ +S = \frac{\partial {^p}v_J}{\partial \dot{q}} +$$ + Where the spatial velocity in Pinocchio's convention is: +$$ +{^p}v_J = \begin{bmatrix} \dot{p}(q,\dot{q}) \\ \omega(q,\dot{q}) \end{bmatrix} +$$ +> [!NOTE] + +> Pinocchio uses the convention $[v, \; \omega]^\top$ (linear, angular), while some literature (in the Featherstone book) uses $[\omega, \; v + p \times \omega]^\top$ (angular, linear). Be consistent with Pinocchio's convention. +#### Computing $\omega$ (Angular Velocity) +For rotation-based joints, extract angular velocity from: +$$ + +\omega = \text{axial}(\dot{R}(q)R(q)^T) + +$$ +Where $\text{axial}$ extracts the vector from a skew-symmetric matrix. +#### Computing $\dot{p}$ (Linear Velocity) +Differentiate the translation with respect to time: +$$ +\dot{p} = \frac{dp}{dt} = \frac{\partial p}{\partial q}\dot{q} +$$ +### 1.4 Derive the Bias Acceleration +The bias acceleration (also called velocity product) is: +$$ +c = \dot{S}\dot{q} +$$ +Where: +$$ +\dot{S} = \sum_{i=0}^{n_v-1} \frac{\partial S}{\partial q_i}\dot{q}_i +$$ +Or equivalently: +$$ + +\dot{S} = \frac{\partial^2 {^p}v_J}{\partial q \partial \dot{q}} \dot{q} + +$$ +> [!TIP] + +> Use symbolic computation tools (SymPy, Mathematica) to derive these expressions. The math can become complex quickly, especially for $\dot{S}$. + +> [!CODE EXAMPLE] +> ```python +> Sdot = sp.Matrix.zeros(6, 3) +> for i in range(3): +> for j in range(3): +> Sdot[:, i] += sp.diff(S[:, i], q[j]) * qdot[j] +> ``` +--- +## Step 2: Core Joint Implementation + +**What You'll Do:** Create C++ header files defining your joint's `JointModel` and `JointData` structures. You will translate your mathematical derivations into efficient C++ code that Pinocchio can use. + +**Files to Create/Modify:** +- `include/pinocchio/multibody/joint/joint-.hpp` (main implementation) +- `include/pinocchio/multibody/joint/fwd.hpp` (add forward declaration) +- `include/pinocchio/multibody/joint/joints.hpp` (include your header) +- `include/pinocchio/multibody/joint/joint-collection.hpp` (register in variant) +- `bindings/python/multibody/joint/joints-models.hpp` (Python bindings) +- `bindings/python/multibody/joint/joints-datas.hpp` (Python bindings) + +### 2.1 File Structure + +You will create the main joint header at this **exact path**: + +``` +pinocchio/include/pinocchio/multibody/joint/joint-.hpp +``` + +You will also update these existing files: +- `pinocchio/include/pinocchio/multibody/joint/fwd.hpp` +- `pinocchio/include/pinocchio/multibody/joint/joints.hpp` +- `pinocchio/include/pinocchio/multibody/joint/joint-collection.hpp` +### 2.2 Define the Traits Struct +In `joint-.hpp`, define the traits for your joint: + +```cpp +template +struct traits> +{ +enum { +NQ = 3, // Configuration dimension +NV = 3, // Velocity dimension +NVExtended = 3 // Extended velocity (used for some internal operations) +}; + +typedef _Scalar Scalar; +typedef JointDataMyJointTpl JointDataDerived; +typedef JointModelMyJointTpl JointModelDerived; + +// Constraint type (motion subspace) +typedef JointMotionSubspaceTpl Constraint_t; + +// Transform type +typedef SE3Tpl Transformation_t; + +// Motion type (spatial velocity) +typedef MotionTpl Motion_t; + +// Bias type +typedef MotionTpl Bias_t; + +// Configuration and tangent vector types +typedef Eigen::Matrix ConfigVector_t; +typedef Eigen::Matrix TangentVector_t; + +// and other required typedefs... +}; + +``` + +> [!IMPORTANT] + +> All typedef here determines how spatial algebra vectors are represented. Here, `JointMotionSubspaceTpl`, `SE3Tpl` and `MotionTpl` are dense matrices. +> To take advantage of sparse patterns, they can be specialized (see [Section 3](#step-3-motion-subspace-specialization) for `JointMotionSubspaceTpl` specialization). +> It's generally a good idea to implement the non-sparse version first. + +### 2.3 Define the JointData Struct + +`JointData` stores the joint's runtime state: + +```cpp +template +struct JointDataMyJointTpl : public JointDataBase> +{ + +typedef JointMyJointTpl JointDerived; +PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived); +PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR; + +// Joint state +ConfigVector_t joint_q; // Joint configuration +TangentVector_t joint_v; // Joint velocity + +// Computed quantities +Constraint_t S; // Motion subspace S(q) +Transformation_t M; // Spatial transform M(q) +Motion_t v; // Spatial velocity v = S * joint_v + +Bias_t c; // Bias acceleration c = Sdot * joint_v + +// ABA-specific quantities +U_t U; // U = I * S +Dinv_t Dinv; // Dinv = (S^T * U + armature)^{-1} +UD_t UDinv; // UDinv = U * Dinv + +// Constructor + +JointDataMyJointTpl() +: joint_q(ConfigVector_t::Zero()) +, joint_v(TangentVector_t::Zero()) +, S() +, M(Transformation_t::Identity()) +, v(Motion_t::Zero()) +, c(Bias_t::Zero()) +, U() +, Dinv() +, UDinv() +{} + +static std::string classname() { return "JointDataMyJoint"; } +std::string shortname() const { return classname(); } + +}; + +``` + +### 2.4 Define the JointModel Struct + +`JointModel` defines the joint's kinematics and dynamics: +```cpp +template +struct JointModelMyJointTpl : public JointModelBase> +{ +PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived); + +typedef JointDataMyJointTpl JointDataDerived; +using Base::id; +using Base::idx_q; +using Base::idx_v; +using Base::idx_vExtended; +using Base::setIndexes; + +// Joint parameters (IF ANY) +Scalar param_a; +Scalar param_b; + +// Constructor +JointModelMyJointTpl() +: param_a(Scalar(1.0)) +, param_b(Scalar(1.0)) + +{} +JointModelMyJointTpl(const Scalar & a, const Scalar & b) +: param_a(a) +, param_b(b) +{} + +// Create data instance +JointDataDerived createData() const +{ +return JointDataDerived(); +} + +// Configuration limits +const std::vector hasConfigurationLimit() const +{ +return {true, true, true}; // Example for 3-DOF joint +} + +const std::vector hasConfigurationLimitInTangent() const +{ +return {true, true, true}; +} + +// Forward kinematics methods (TO IMPLEMENT) +template +void calc(JointDataDerived & data, const typename Eigen::MatrixBase & qs) const; +\\TODO + +template +void calc( +JointDataDerived & data, +const typename Eigen::MatrixBase & qs, +const typename Eigen::MatrixBase & vs) const; +\\TODO + +template +void calc_aba( +JointDataDerived & data, +const Eigen::MatrixBase & armature, +const Eigen::MatrixBase & I, +const bool update_I) const; +\\TODO + +static std::string classname() { return "JointModelMyJoint"; } +std::string shortname() const { return classname(); } +}; + +``` + +### 2.5 Implement Forward Kinematics +#### calc(data, q): Compute M and S +```cpp +template + +void calc(JointDataDerived & data, const typename Eigen::MatrixBase & qs) const +{ + +// Extract joint configuration +data.joint_q = qs.template segment(idx_q()); + +// Precompute sin/cos for efficiency +Scalar c0, s0; +SINCOS(data.joint_q(0), &s0, &c0); +Scalar c1, s1; +SINCOS(data.joint_q(1), &s1, &c1); + +// ... and so on + +// Compute spatial transform M(q) +computeSpatialTransform(c0, s0, c1, s1, /* ... */, data); + +// Compute motion subspace S(q) +computeMotionSubspace(c0, s0, c1, s1, /* ... */, data); +} +``` + +#### calc(data, q, v): Compute M, S, v, and c +```cpp +template +void calc( +JointDataDerived & data, +const typename Eigen::MatrixBase & qs, +const typename Eigen::MatrixBase & vs) const +{ + +// Compute M and S +calc(data, qs); + +// Extract joint velocity +data.joint_v = vs.template segment(idx_v()); + +// Compute spatial velocity: v = S * joint_v +data.v.toVector().noalias() = data.S.matrix() * data.joint_v; + +// Compute bias acceleration: c = Sdot * joint_v +computeBias(/* ... */, data); +} +``` + +> [!TIP] + +> Use helper functions like `computeSpatialTransform`, `computeMotionSubspace`, and `computeBias` to keep your code modular and readable. Espacially is you have complex formulae. Like for an Ellipsoid Joint. +### 2.6 Implement ABA Support + +For the Articulated Body Algorithm: +```cpp +template +void calc_aba( +JointDataDerived & data, +const Eigen::MatrixBase & armature, +const Eigen::MatrixBase & I, +const bool update_I) const + +{ +// U = I * S +data.U.noalias() = I * data.S.matrix(); + +// StU = S^T * U +data.StU.noalias() = data.S.transpose() * data.U; + +// Add armature to diagonal +data.StU.diagonal() += armature; + +// Compute Dinv = (StU)^{-1} +internal::PerformStYSInversion::run(data.StU, data.Dinv); + +// UDinv = U * Dinv +data.UDinv.noalias() = data.U * data.Dinv; + +// Update articulated inertia if requested +if (update_I) + +PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I).noalias() -= data.UDinv * data.U.transpose(); +} +``` +### 2.7 Register the Joint +#### Forward Declaration +In `include/pinocchio/multibody/joint/fwd.hpp`: + +```cpp +template +struct JointModelMyJointTpl; +typedef JointModelMyJointTpl JointModelMyJoint; +``` +#### Include in Joint Collection + +In `include/pinocchio/multibody/joint/joints.hpp`: +```cpp +#include "pinocchio/multibody/joint/joint-myjoint.hpp" +``` + + +In `include/pinocchio/multibody/joint/joint-collection.hpp`: +```cpp +typedef boost::variant< +// ... existing joints ... +JointModelMyJoint, +// ... more joints ... +> JointModelVariant; +// ... +// ... more typedef ... +// ... +typedef boost::variant< +// ... existing joints ... +JointDataMyJoint, +// ... more joints ... +> JointDataVariant; +``` + +### 2.8 Add Python Bindings +In `bindings/python/multibody/joint/joints-models.hpp`: +```cpp +bp::class_( +"JointModelMyJoint", +"My custom joint model", +bp::no_init) +.def(JointModelMyJointPythonVisitor()); +``` + +In `bindings/python/multibody/joint/joints-datas.hpp`, add similar binding for `JointDataMyJoint`. + +--- +## Step 3: Motion Subspace Specialization + +**What You'll Do:** Optionally create a custom constraint class to exploit sparse structure in your motion subspace matrix. This step is **optional** but can significantly improve performance. + +**When to do this:** If your joint's $S$ matrix has many zeros or a repeating pattern (like Revolute or Prismatic joints). + +**When to skip this:** If your $S$ matrix is dense (like Ellipsoid), use the generic `JointMotionSubspaceTpl` and skip to Step 4. +### 3.1 When to Specialize + +If your joint's motion subspace has a **sparse or structured pattern**, you can define a custom constraint class to exploit this structure for performance. + +**Examples:** +- Revolute joint: $S = [0, 0, 0, 0, 0, 1]^T$ (single non-zero entry) +- Prismatic joint: $S = [1, 0, 0, 0, 0, 0]^T$ +- Spherical joint: Block-diagonal structure + +**When NOT to specialize:** +- Dense 6×n matrices without obvious structure (use `JointMotionSubspaceTpl`) +### 3.2 Implementing a Custom Constraint + +Create a new struct inheriting from `JointMotionSubspaceBase`: +```cpp +template +struct JointMotionSubspaceMyJoint : JointMotionSubspaceBase> +{ +enum { NV = 3 }; // Example +typedef Eigen::Matrix DenseBase; +typedef Eigen::Matrix ReducedSquaredMatrix; + +// Internal representation (store only what's needed) +Eigen::Matrix axis; // Example for a single-axis joint + +// Convert to dense matrix +DenseBase matrix() const +{ +DenseBase S; +S.setZero(); +// Fill in the structure +return S; +} + +// Implement spatial algebra operations +template +typename MotionDerived::MotionPlain se3Action(const SE3Tpl & m) const +{ +// Implement: m.act(S) +} + +template +typename MotionDerived::MotionPlain se3ActionInverse(const SE3Tpl & m) const +{ +// Implement: m.actInv(S) +} + +// Implement matrix-vector multiplication +template +typename Motion::MotionPlain operator*(const Eigen::MatrixBase & v) const +{ +// Implement: S * v +} +// Other required operations... +}; + +``` + +Then update your joint's traits to use this custom constraint: +```cpp +typedef JointMotionSubspaceMyJoint Constraint_t; +``` + +> [!TIP] + +> Look at existing specialized constraints like `JointModelRevoluteTpl` for inspiration. + +--- +## Step 4: Model Graph Integration + +**What You'll Do:** Enable your joint to work with Pinocchio's graph-based parser, which is used to load URDF files and build models from Python. You will create a graph joint struct and implement visitor functions. + +**Files to Modify:** +- `include/pinocchio/parsers/graph/joints.hpp` (define graph joint) +- `src/parsers/graph/model-graph-algo.cpp` (create & add visitors) +- `src/parsers/graph/model-graph.cpp` (reversal visitors, if applicable) +- `include/pinocchio/parsers/graph/graph-visitor.hpp` (pose update, if applicable) +- `include/pinocchio/parsers/graph/model-configuration-converter.hxx` (converters, if reversible) +- `bindings/python/parsers/graph/expose-edges.cpp` (Python exposure) + +**Goal:** After this step, users can create your joint from Python and use it in graph-based model construction. +### 4.1 Why ModelGraph? + +The ModelGraph parser: +- Builds kinematic trees from high-level descriptions (URDF, SDF) +- Handles **joint reversals** (when kinematic tree orientation differs from description) +- Converts between different parameterizations +### 4.2 Define the Graph Joint Struct + +**Location:** `pinocchio/include/pinocchio/parsers/graph/joints.hpp` + +Add your joint struct to this file: +```cpp +struct JointMyJoint +{ +double param_a = 1.0; +double param_b = 1.0; +static constexpr int nq = 3; +static constexpr int nv = 3; +JointMyJoint() = default; +JointMyJoint(const double a, const double b) +: param_a(a) +, param_b(b) +{} +bool operator==(const JointMyJoint & other) const +{ +return param_a == other.param_a && param_b == other.param_b; +} +}; +``` + +Add it to the `JointVariant` typedef in the same file: + +```cpp +typedef boost::variant< +// ... existing joints ... +JointMyJoint, +// ... +> JointVariant; +``` +### 4.3 Implement CreateJointModelVisitor + +**Location:** `src/parsers/graph/model-graph-algo.cpp` + +Add a visitor in the `CreateJointModelVisitor` class to convert your graph joint to a model joint: +```cpp +ReturnType operator()(const JointMyJoint & joint) const +{ +return JointModelMyJoint(joint.param_a, joint.param_b); +} +``` +### 4.4 Implement AddJointModelVisitor + +**Location:** Same file: `src/parsers/graph/model-graph-algo.cpp` +If your joint **cannot be reversed**, you need to explicitly handle this: + +```cpp +void operator()(const JointMyJoint & joint, const BodyFrame & b_f) +{ + +if (!edge.forward) +PINOCCHIO_THROW_PRETTY( +std::invalid_argument, +"Graph - JointMyJoint cannot be reversed."); +addJointBetweenBodies(joint, b_f); +} +``` + +### 4.5 Support Reversible Joints (Advanced) + +If your joint **can be reversed**, implement the following additional visitors. +#### ReverseJointGraphVisitor + +**Location:** `src/parsers/graph/model-graph.cpp` + +In `src/parsers/graph/model-graph.cpp`: +```cpp +ReturnType operator()(const JointMyJoint & joint) const +{ +// Return reversed joint parameters +// For symmetric joints, this might just be a copy +return {JointMyJoint(joint.param_a, joint.param_b), SE3::Identity()}; +} +``` +#### UpdateJointGraphReversePoseVisitor + +**Location:** `include/pinocchio/parsers/graph/graph-visitor.hpp` + +In `include/pinocchio/parsers/graph/graph-visitor.hpp`: + +```cpp +SE3 operator()(const JointMyJoint & joint) const +{ +// Compute the static pose for the reversed joint +return joint_calc(JointModelMyJoint(joint.param_a, joint.param_b)); +} +``` +#### Configuration and Tangent Converters + +**Location:** `include/pinocchio/parsers/graph/model-configuration-converter.hxx` + +Implement converters to translate configurations between the original and reversed joint parameterizations. + +> [!NOTE] +> `q_source` is the configuration in the original model, `q_target` is the configuration in the converted model (e.g., with a reversed joint). Same for velocities (`v_source`, `v_target`). + +**ConfigurationConverterVisitor:** +```cpp + +ReturnType operator()(const JointModelMyJointTpl &) const +{ +if (joint.same_direction) { + +// Copy configuration +q_target.template segment(offset_target) = +q_source.template segment(offset_source); +} else +{ +// Reverse configuration (e.g., negate angles) +q_target.template segment(offset_target) = +-q_source.template segment(offset_source); +} +} +``` + +**TangentConverterVisitor:** +```cpp +ReturnType operator()(const JointModelMyJointTpl &) const +{ +if (joint.same_direction) { + +// Copy velocity +v_target.template segment(offset_target) = +v_source.template segment(offset_source); +} else { +// Reverse velocity +v_target.template segment(offset_target) +-v_source.template segment(offset_source); +} +} +``` + +> [!WARNING] +> Joint reversal can be non-trivial. For example, the Ellipsoid joint cannot be reversed because its motion subspace is defined in a specific frame, the parent frame. Only implement reversal if you can get the spatial transform of the joint in the child frame by modifying $q$. + +### 4.6 Add Python Exposure + +**Location:** `bindings/python/parsers/graph/expose-edges.cpp` + +Expose your graph joint to Python so it can be used from the Python API: +```cpp +bp::class_("JointMyJoint", bp::init<>()) +.def(bp::init()) +.def_readwrite("param_a", &JointMyJoint::param_a) +.def_readwrite("param_b", &JointMyJoint::param_b); +``` + +--- +## Step 5: Testing + +**What You'll Do:** Write comprehensive unit tests to verify your joint works correctly. Start by testing the joint itself (kinematics, dynamics), then test graph integration. + +**Files to Create/Modify:** +- `unittest/joint-.cpp` (create new test file) +- `unittest/CMakeLists.txt` (register your test) +- `unittest/model-graph.cpp` (add graph test) +- `unittest/model-configuration-converter.cpp` (add converter test, if reversible) +- `unittest/serialization.cpp` (add serialization test, modify only if your test take parameters) +- `unittest/all-joints.cpp` (add to joint list, modify only if your test take parameters) +- `unittest/joint-generic.cpp` (add to generic tests, modify only if your test take parameters) +- `unittest/finite-differences.cpp` (add to finite-diff tests, modify only if your test take parameters) + +**Testing Priority:** +1. **First:** Test the joint in isolation (kinematics, $S$, $\dot{S}$, dynamics) +2. **Second:** Test graph integration (if you implemented Step 4) +3. **Third:** Test converters and serialization + +You should create comprehensive tests covering: + +1. **Joint kinematics**: M, S, v, c +2. **Joint dynamics**: RNEA, ABA +3. **Equivalence tests**: Compare with composite joints or analytical solutions +4. **Model graph**: Graph construction and conversion +5. **Serialization**: Ensure the joint can be saved/loaded +### 5.1 Create Joint Unit Tests + +**Location:** Create a new file at `unittest/joint-.cpp` + +This is your primary test file for the joint: + +```cpp +#include +#include .hpp> + +// ... other includes ... +using namespace pinocchio; + +BOOST_AUTO_TEST_SUITE(JointMyJoint) +BOOST_AUTO_TEST_CASE(basic_kinematics) +{ +typedef SE3::Vector3 Vector3; +JointModelMyJoint jmodel(1.0, 2.0); +JointDataMyJoint jdata = jmodel.createData(); + +// Random configuration and velocity +Eigen::VectorXd q = Eigen::VectorXd::Random(3); +Eigen::VectorXd v = Eigen::VectorXd::Random(3); + +// Compute kinematics +jmodel.calc(jdata, q, v); + +// Verify spatial velocity: v = S * joint_v +Motion v_expected; +v_expected.toVector().noalias() = jdata.S.matrix() * jdata.joint_v; +BOOST_CHECK(jdata.v.isApprox(v_expected)); + +// Add more checks... +} + +BOOST_AUTO_TEST_CASE(test_motion_subspace_derivative) +{ +// Test Sdot via finite differences +JointModelMyJoint jmodel(1.0, 2.0); +JointDataMyJoint jdata = jmodel.createData(); +Eigen::VectorXd q = Eigen::VectorXd::Random(3); +Eigen::VectorXd v = Eigen::VectorXd::Random(3); +jmodel.calc(jdata, q, v); + +// Compute Sdot analytically via bias +Motion c_analytic = jdata.c; + +// Compute Sdot numerically +Eigen::Matrix Sdot_fd = finiteDiffSdot(jmodel, jdata, q, v); +Motion c_fd; +c_fd.toVector().noalias() = Sdot_fd * jdata.joint_v; + +// Compare +BOOST_CHECK(c_analytic.isApprox(c_fd, 1e-6)); +} + + +BOOST_AUTO_TEST_CASE(test_vs_composite) +{ +// If your joint can be represented as a composition of simpler joints, +// verify equivalence +// ... +} + +BOOST_AUTO_TEST_SUITE_END() +``` + +### 5.2 Add to CMake + +**Location:** `unittest/CMakeLists.txt` + +Register your test so it gets built and run: +```cmake + +ADD_PINOCCHIO_UNIT_TEST(joint-myjoint) + +``` + +### 5.3 Test Model Graph Integration + +**Location:** `unittest/model-graph.cpp` + +Add a test case for your joint in the graph tests: + +```cpp +BOOST_AUTO_TEST_CASE(test_graph_with_myjoint) +{ +Graph graph; + +// Add vertices (bodies) +Vertex v0 = graph.addVertex("world", BodyFrame::WORLD()); +Vertex v1 = graph.addVertex("link1", BodyFrame(...)); + +// Add edge with your joint +JointMyJoint joint(1.0, 2.0); +graph.addEdge(v0, v1, joint, SE3::Identity(), JointLimits(...)); + +// Build model from graph +Model model = graph.buildModel(); + +// Verify model was built correctly +BOOST_CHECK_EQUAL(model.njoints, 2); // universe + myjoint +BOOST_CHECK_EQUAL(model.nq, 3); +BOOST_CHECK_EQUAL(model.nv, 3); +} +``` + +### 5.4 Test Configuration Converters + +**Location:** `unittest/model-configuration-converter.cpp` + +(Only if your joint is reversible): + +```cpp +BOOST_AUTO_TEST_CASE(test_myjoint_converter) +{ + +// Create model with your joint +Model model; +JointIndex idx = model.addJoint(0, JointModelMyJoint(1.0, 2.0), SE3::Identity(), "myjoint"); + +// Create a converter (e.g., for reversed model) +ModelConfigurationConverter converter(model, model_reversed); + +// Test configuration conversion +Eigen::VectorXd q = randomConfiguration(model); +Eigen::VectorXd q_converted(model_reversed.nq); +converter.convert(q, q_converted); + +// Verify correctness +// ... +} +``` +### 5.5 Test Serialization + +**Location:** `unittest/serialization.cpp` + +Ensure your joint can be serialized and deserialized: + +```cpp +BOOST_AUTO_TEST_CASE(myjoint_serialization) +{ +JointModelMyJoint jmodel(1.0, 2.0); + +// Serialize +std::stringstream ss; +{ + +boost::archive::text_oarchive oa(ss); +oa << jmodel; +} + +// Deserialize +JointModelMyJoint jmodel_loaded; +{ +boost::archive::text_iarchive ia(ss); +ia >> jmodel_loaded; +} + +// Verify +BOOST_CHECK(jmodel.param_a == jmodel_loaded.param_a); +BOOST_CHECK(jmodel.param_b == jmodel_loaded.param_b); +} +``` + +--- +## Summary Checklist + +Use this checklist to track your implementation progress: +### Mathematical Foundation + +- [ ] Define configuration space (nq) and tangent space (nv) +- [ ] Derive spatial transform $M(q)$ +- [ ] Derive motion subspace $S(q)$ +- [ ] Derive bias acceleration $c(q,\dot{q})$ or $\dot{S}(q,\dot{q})$ +- [ ] Verify equations symbolically (SymPy, Mathematica) +### Core Implementation + +- [ ] Create `include/pinocchio/multibody/joint/joint-.hpp` +- [ ] Define `traits` struct with NQ, NV, typedefs +- [ ] Implement `JointDataMyJointTpl` struct +- [ ] Implement `JointModelMyJointTpl` struct +- [ ] Implement `calc(data, q)` method +- [ ] Implement `calc(data, q, v)` method +- [ ] Implement `calc_aba(...)` method +- [ ] Add forward declaration in `fwd.hpp` +- [ ] Include in `joints.hpp` and `joint-collection.hpp` +- [ ] Add Python bindings for JointModel and JointData +### Motion Subspace Specialization (Optional) +- [ ] Determine if specialization is beneficial +- [ ] Implement custom constraint class +- [ ] Update traits to use custom constraint +- [ ] Implement spatial algebra operations +### Model Graph Integration + +- [ ] Define graph joint struct in `parsers/graph/joints.hpp` +- [ ] Add to `JointVariant` typedef +- [ ] Implement `CreateJointModelVisitor` in `model-graph-algo.cpp` +- [ ] Implement `AddJointModelVisitor` in `model-graph-algo.cpp` +- [ ] (Optional) Implement `ReverseJointGraphVisitor` in `model-graph.cpp` +- [ ] (Optional) Implement `UpdateJointGraphReversePoseVisitor` in `graph-visitor.hpp` +- [ ] (Optional) Implement `ConfigurationConverterVisitor` in `model-configuration-converter.hxx` +- [ ] (Optional) Implement `TangentConverterVisitor` in `model-configuration-converter.hxx` +- [ ] Add Python bindings for graph joint in `expose-edges.cpp` +### Testing + +- [ ] Create `unittest/joint-.cpp` +- [ ] Test basic kinematics (M, S, v) +- [ ] Test motion subspace derivative (Sdot, c) via finite differences +- [ ] Test dynamics (RNEA, ABA) +- [ ] Test equivalence with composite joints (if applicable) +- [ ] Update `unittest/model-graph.cpp` with graph tests +- [ ] Update `unittest/model-configuration-converter.cpp` with converter tests +- [ ] Update `unittest/serialization.cpp` with serialization tests +- [ ] Add joint to `unittest/all-joints.cpp` +- [ ] Add joint to `unittest/joint-generic.cpp` +- [ ] Add joint to `unittest/finite-differences.cpp` +- [ ] Update `unittest/CMakeLists.txt` + +### Documentation + +- [ ] Add docstrings to public methods +- [ ] Document joint parameters and their meanings +- [ ] Add example usage in `examples/` +- [ ] Update documentation in `doc/` +--- + + + +## Additional Resources + +- **Featherstone's Book**: *Rigid Body Dynamics Algorithms* (2008) - The definitive reference for spatial algebra +- **Pinocchio Documentation**: https://gepettoweb.laas.fr/doc/stack-of-tasks/pinocchio/master/doxygen-html/ +- +- **Existing Joint Implementations**: Study `joint-revolute.hpp`, `joint-spherical-ZYX.hpp`, `joint-free-flyer.hpp` for examples +- +- **SymPy for symbolic math**: https://www.sympy.org/ + +--- +## Example: Quick Reference for Ellipsoid Joint + +As a concrete example, the Ellipsoid joint implementation demonstrates: + +- **Configuration**: $(q_0, q_1, q_2)$ - three Euler angles (XYZ) +- **Parameters**: $(a, b, c)$ - ellipsoid semi-axes +- **Motion subspace**: Dense 6×3 matrix (uses `JointMotionSubspaceTpl`) +- **Reversal**: Not supported (motion subspace is frame-dependent) +- **Testing**: Compared against SphericalZYX for rotation equivalence + +Key files to review: +- [`joint-ellipsoid.hpp`](include/pinocchio/multibody/joint/joint-ellipsoid.hpp) +- [`unittest/joint-ellipsoid.cpp`](unittest/joint-ellipsoid.cpp) +- [`examples/ellipsoid-joint-kinematics.py`](examples/ellipsoid-joint-kinematics.py) +--- +**Happy implementing! If you have questions, consult the Pinocchio community or review existing joint implementations for guidance.** From f6b9890bf9c84e994a5bc8b7ffda46aa379ee011 Mon Sep 17 00:00:00 2001 From: Joris Vaillant Date: Wed, 26 Nov 2025 14:03:48 +0100 Subject: [PATCH 38/48] doc: Fix doxygen doc parsing --- doc/e-development/Implement_a_new_joint.md | 139 ++++++++++----------- doc/e-development/intro.md | 3 + doc/treeview.dox | 4 + 3 files changed, 76 insertions(+), 70 deletions(-) create mode 100644 doc/e-development/intro.md diff --git a/doc/e-development/Implement_a_new_joint.md b/doc/e-development/Implement_a_new_joint.md index 9a1ae11f4f..513c25c169 100644 --- a/doc/e-development/Implement_a_new_joint.md +++ b/doc/e-development/Implement_a_new_joint.md @@ -1,4 +1,5 @@ -#How to implement a new joint in Pinocchio +# How to implement a new joint in Pinocchio {#md_doc_e-dev_impl-new-joint} + **Tags:** #Pinocchio #tutorial #joints #implementation #motionsubspace This guide provides a comprehensive, step-by-step approach to implementing a new joint type in Pinocchio. It covers mathematical foundations, core implementation, parser integration, and testing requirements. @@ -104,7 +105,6 @@ Pinocchio already implements many joint types. Study these as references: | **Revolute Unaligned** | `joint-revolute-unaligned.hpp` | 1 | 1 | Revolute with arbitrary axis | | **Prismatic Unaligned** | `joint-prismatic-unaligned.hpp` | 1 | 1 | Prismatic with arbitrary axis | | **Revolute Unbounded** | `joint-revolute-unbounded.hpp` | 2 | 1 | Revolute without configuration limit | -| **Prismatic Unbounded** | `joint-prismatic-unbounded.hpp` | 2 | 1 | Prismatic without configuration limit | | **Spherical** | `joint-spherical.hpp` | 4 | 3 | 3-DOF rotation using quaternion | | **Spherical ZYX** | `joint-spherical-ZYX.hpp` | 3 | 3 | 3-DOF rotation using Euler angles | | **Free Flyer** | `joint-free-flyer.hpp` | 7 | 6 | 6-DOF (translation + rotation) | @@ -123,94 +123,93 @@ Pinocchio already implements many joint types. Study these as references: ### Key Mathematical Concepts A joint is characterized by: -- **Configuration dimension** `nq`: Size of the configuration vector $q$ -- **Velocity dimension** `nv`: Size of the velocity vector $\dot{q}$ -- **Motion subspace** $S(q)$: Maps joint velocity to spatial velocity: $v_J = S(q)\dot{q}$ -- **Spatial transform** $M(q)$: Placement from parent to child frame -- **Bias acceleration** $c(q,\dot{q})$: Often written as $\dot{S}\dot{q}$ +- **Configuration dimension** `nq`: Size of the configuration vector \f$q\f$ +- **Velocity dimension** `nv`: Size of the velocity vector \f$\dot{q}\f$ +- **Motion subspace** \f$S(q)\f$: Maps joint velocity to spatial velocity: \f$v_J = S(q)\dot{q}\f$ +- **Spatial transform** \f$M(q)\f$: Placement from parent to child frame +- **Bias acceleration** \f$c(q,\dot{q})\f$: Often written as \f$\dot{S}\dot{q}\f$ --- ## Step 1: Mathematical Foundation -**What You'll Do:** Derive the mathematical equations that govern your joint's behavior. You will compute the spatial transform $M(q)$, motion subspace $S(q)$, and bias acceleration $c(q,\dot{q})$ using pen-and-paper or symbolic tools. +**What You'll Do:** Derive the mathematical equations that govern your joint's behavior. You will compute the spatial transform \f$M(q)\f$, motion subspace \f$S(q)\f$, and bias acceleration \f$c(q,\dot{q})\f$ using pen-and-paper or symbolic tools. **Goal:** By the end of this step, you should have closed-form expressions ready to implement in code. ### 1.1 Define the Joint Configuration First, determine your joint's configuration and velocity spaces: -- What are the generalized coordinates $q = [q_0, q_1, \ldots, q_{n-1}]$? -- What is the relationship between $q$ and $\dot{q}$ (are they in the same space)? +- What are the generalized coordinates \f$q = [q_0, q_1, \ldots, q_{n-1}]\f$? +- What is the relationship between \f$q\f$ and \f$\dot{q}\f$ (are they in the same space)? > [!IMPORTANT] - > For non-Euclidean joints (e.g., spherical joints using quaternions), `nq` may differ from `nv`. Ensure you understand the Lie group structure. + ### 1.2 Derive the Spatial Transform Compute the spatial transformation matrix from parent frame **p** to child frame **c**: -$$ +\f[ {^p}X_c(q) = \begin{bmatrix} R(q) & 0 \\ -R(q)p(q)^\times & R(q) \end{bmatrix} -$$ - Where: +\f] +Where: -- $R(q) \in SO(3)$ is the rotation matrix -- $p(q) \in \mathbb{R}^3$ is the translation vector -- $p^\times$ denotes the skew-symmetric matrix +- \f$R(q) \in SO(3)\f$ is the rotation matrix +- \f$p(q) \in \mathbb{R}^3\f$ is the translation vector +- \f$p^\times\f$ denotes the skew-symmetric matrix + +> [!TIP] +> The ellipsoid joint has translation \f$p(q)\f$ and rotation \f$R(q)\f$ that depends on ellipsoid radii \f$(a,b,c)\f$ and configuration \f$(q_0, q_1, q_2)\f$: +> \f[ +> \begin{align} p(q) = \begin{bmatrix} a\sin q_1 \\ -b\sin q_0\cos q_1 \\ c\cos q_0\cos q_1 \end{bmatrix} & & R(q) = R_x(q_0) R_y(q_1) R_z(q_2) \end{align} +> \f] -> [!EXAMPLE - Ellipsoid Joint] - The ellipsoid joint has translation $p(q)$ and rotation $R(q)$ that depends on ellipsoid radii $(a,b,c)$ and configuration $(q_0, q_1, q_2)$: -$$\begin{align} p(q) = \begin{bmatrix} a\sin q_1 \\ -b\sin q_0\cos q_1 \\ c\cos q_0\cos q_1 \end{bmatrix} & & R(q) = R_x(q_0) R_y(q_1) R_z(q_2) \end{align}$$ ### 1.3 Derive the Motion Subspace - The motion subspace is the Jacobian of the spatial velocity with respect to joint velocity: -$$ +The motion subspace is the Jacobian of the spatial velocity with respect to joint velocity: +\f[ S = \frac{\partial {^p}v_J}{\partial \dot{q}} -$$ - Where the spatial velocity in Pinocchio's convention is: -$$ +\f] +Where the spatial velocity in Pinocchio's convention is: +\f[ {^p}v_J = \begin{bmatrix} \dot{p}(q,\dot{q}) \\ \omega(q,\dot{q}) \end{bmatrix} -$$ -> [!NOTE] +\f] -> Pinocchio uses the convention $[v, \; \omega]^\top$ (linear, angular), while some literature (in the Featherstone book) uses $[\omega, \; v + p \times \omega]^\top$ (angular, linear). Be consistent with Pinocchio's convention. -#### Computing $\omega$ (Angular Velocity) +> [!NOTE] +> Pinocchio uses the convention \f$[v, \; \omega]^\top\f$ (linear, angular), while some literature (in the Featherstone book) uses \f$[\omega, \; v + p \times \omega]^\top\f$ (angular, linear). Be consistent with Pinocchio's convention. +#### Computing \f$\omega\f$ (Angular Velocity) For rotation-based joints, extract angular velocity from: -$$ +\f[ \omega = \text{axial}(\dot{R}(q)R(q)^T) - -$$ -Where $\text{axial}$ extracts the vector from a skew-symmetric matrix. -#### Computing $\dot{p}$ (Linear Velocity) +\f] +Where \f$\text{axial}\f$ extracts the vector from a skew-symmetric matrix. +#### Computing \f$\dot{p}\f$ (Linear Velocity) Differentiate the translation with respect to time: -$$ +\f[ \dot{p} = \frac{dp}{dt} = \frac{\partial p}{\partial q}\dot{q} -$$ +\f] ### 1.4 Derive the Bias Acceleration The bias acceleration (also called velocity product) is: -$$ +\f[ c = \dot{S}\dot{q} -$$ +\f] Where: -$$ +\f[ \dot{S} = \sum_{i=0}^{n_v-1} \frac{\partial S}{\partial q_i}\dot{q}_i -$$ +\f] Or equivalently: -$$ - +\f[ \dot{S} = \frac{\partial^2 {^p}v_J}{\partial q \partial \dot{q}} \dot{q} +\f] -$$ > [!TIP] - -> Use symbolic computation tools (SymPy, Mathematica) to derive these expressions. The math can become complex quickly, especially for $\dot{S}$. - -> [!CODE EXAMPLE] +> Use symbolic computation tools (SymPy, Mathematica) to derive these expressions. The math can become complex quickly, especially for \f$\dot{S}\f$. > ```python > Sdot = sp.Matrix.zeros(6, 3) > for i in range(3): > for j in range(3): > Sdot[:, i] += sp.diff(S[:, i], q[j]) * qdot[j] > ``` + --- ## Step 2: Core Joint Implementation @@ -275,7 +274,6 @@ typedef Eigen::Matrix TangentVector_t; ``` > [!IMPORTANT] - > All typedef here determines how spatial algebra vectors are represented. Here, `JointMotionSubspaceTpl`, `SE3Tpl` and `MotionTpl` are dense matrices. > To take advantage of sparse patterns, they can be specialized (see [Section 3](#step-3-motion-subspace-specialization) for `JointMotionSubspaceTpl` specialization). > It's generally a good idea to implement the non-sparse version first. @@ -455,8 +453,8 @@ computeBias(/* ... */, data); ``` > [!TIP] - > Use helper functions like `computeSpatialTransform`, `computeMotionSubspace`, and `computeBias` to keep your code modular and readable. Espacially is you have complex formulae. Like for an Ellipsoid Joint. + ### 2.6 Implement ABA Support For the Articulated Body Algorithm: @@ -541,16 +539,18 @@ In `bindings/python/multibody/joint/joints-datas.hpp`, add similar binding for ` **What You'll Do:** Optionally create a custom constraint class to exploit sparse structure in your motion subspace matrix. This step is **optional** but can significantly improve performance. -**When to do this:** If your joint's $S$ matrix has many zeros or a repeating pattern (like Revolute or Prismatic joints). +**When to do this:** If your joint's \f$S\f$ matrix has many zeros or a repeating pattern (like Revolute or Prismatic joints). + +**When to skip this:** If your matrix is dense (like Ellipsoid), use the generic `JointMotionSubspaceTpl` and skip to Step 4. -**When to skip this:** If your $S$ matrix is dense (like Ellipsoid), use the generic `JointMotionSubspaceTpl` and skip to Step 4. ### 3.1 When to Specialize If your joint's motion subspace has a **sparse or structured pattern**, you can define a custom constraint class to exploit this structure for performance. **Examples:** -- Revolute joint: $S = [0, 0, 0, 0, 0, 1]^T$ (single non-zero entry) -- Prismatic joint: $S = [1, 0, 0, 0, 0, 0]^T$ + +- Revolute joint: \f$S = [0, 0, 0, 0, 0, 1]^T \f$ (single non-zero entry) +- Prismatic joint: \f$S = [1, 0, 0, 0, 0, 0]^T \f$ - Spherical joint: Block-diagonal structure **When NOT to specialize:** @@ -608,7 +608,6 @@ typedef JointMotionSubspaceMyJoint Constraint_t; ``` > [!TIP] - > Look at existing specialized constraints like `JointModelRevoluteTpl` for inspiration. --- @@ -767,7 +766,9 @@ v_target.template segment(offset_target) ``` > [!WARNING] -> Joint reversal can be non-trivial. For example, the Ellipsoid joint cannot be reversed because its motion subspace is defined in a specific frame, the parent frame. Only implement reversal if you can get the spatial transform of the joint in the child frame by modifying $q$. +> Joint reversal can be non-trivial. +> For example, the Ellipsoid joint cannot be reversed because its motion subspace is defined in a specific frame, the parent frame. +> Only implement reversal if you can get the spatial transform of the joint in the child frame by modifying \f$q\f$. ### 4.6 Add Python Exposure @@ -797,7 +798,7 @@ bp::class_("JointMyJoint", bp::init<>()) - `unittest/finite-differences.cpp` (add to finite-diff tests, modify only if your test take parameters) **Testing Priority:** -1. **First:** Test the joint in isolation (kinematics, $S$, $\dot{S}$, dynamics) +1. **First:** Test the joint in isolation (kinematics, \f$S\f$, \f$\dot{S}\f$, dynamics) 2. **Second:** Test graph integration (if you implemented Step 4) 3. **Third:** Test converters and serialization @@ -977,15 +978,15 @@ BOOST_CHECK(jmodel.param_b == jmodel_loaded.param_b); ## Summary Checklist Use this checklist to track your implementation progress: -### Mathematical Foundation +### Mathematical Foundation - [ ] Define configuration space (nq) and tangent space (nv) -- [ ] Derive spatial transform $M(q)$ -- [ ] Derive motion subspace $S(q)$ -- [ ] Derive bias acceleration $c(q,\dot{q})$ or $\dot{S}(q,\dot{q})$ +- [ ] Derive spatial transform \f$M(q)\f$ +- [ ] Derive motion subspace \f$S(q)\f$ +- [ ] Derive bias acceleration \f$c(q,\dot{q})\f$ or \f$\dot{S}(q,\dot{q})\f$ - [ ] Verify equations symbolically (SymPy, Mathematica) -### Core Implementation +### Core Implementation - [ ] Create `include/pinocchio/multibody/joint/joint-.hpp` - [ ] Define `traits` struct with NQ, NV, typedefs - [ ] Implement `JointDataMyJointTpl` struct @@ -996,13 +997,14 @@ Use this checklist to track your implementation progress: - [ ] Add forward declaration in `fwd.hpp` - [ ] Include in `joints.hpp` and `joint-collection.hpp` - [ ] Add Python bindings for JointModel and JointData + ### Motion Subspace Specialization (Optional) - [ ] Determine if specialization is beneficial - [ ] Implement custom constraint class - [ ] Update traits to use custom constraint - [ ] Implement spatial algebra operations -### Model Graph Integration +### Model Graph Integration - [ ] Define graph joint struct in `parsers/graph/joints.hpp` - [ ] Add to `JointVariant` typedef - [ ] Implement `CreateJointModelVisitor` in `model-graph-algo.cpp` @@ -1012,8 +1014,8 @@ Use this checklist to track your implementation progress: - [ ] (Optional) Implement `ConfigurationConverterVisitor` in `model-configuration-converter.hxx` - [ ] (Optional) Implement `TangentConverterVisitor` in `model-configuration-converter.hxx` - [ ] Add Python bindings for graph joint in `expose-edges.cpp` -### Testing +### Testing - [ ] Create `unittest/joint-.cpp` - [ ] Test basic kinematics (M, S, v) - [ ] Test motion subspace derivative (Sdot, c) via finite differences @@ -1028,22 +1030,18 @@ Use this checklist to track your implementation progress: - [ ] Update `unittest/CMakeLists.txt` ### Documentation - - [ ] Add docstrings to public methods - [ ] Document joint parameters and their meanings - [ ] Add example usage in `examples/` - [ ] Update documentation in `doc/` ---- - +--- ## Additional Resources - **Featherstone's Book**: *Rigid Body Dynamics Algorithms* (2008) - The definitive reference for spatial algebra - **Pinocchio Documentation**: https://gepettoweb.laas.fr/doc/stack-of-tasks/pinocchio/master/doxygen-html/ -- - **Existing Joint Implementations**: Study `joint-revolute.hpp`, `joint-spherical-ZYX.hpp`, `joint-free-flyer.hpp` for examples -- - **SymPy for symbolic math**: https://www.sympy.org/ --- @@ -1051,8 +1049,8 @@ Use this checklist to track your implementation progress: As a concrete example, the Ellipsoid joint implementation demonstrates: -- **Configuration**: $(q_0, q_1, q_2)$ - three Euler angles (XYZ) -- **Parameters**: $(a, b, c)$ - ellipsoid semi-axes +- **Configuration**: \f$(q_0, q_1, q_2)\f$ - three Euler angles (XYZ) +- **Parameters**: \f$(a, b, c)\f$ - ellipsoid semi-axes - **Motion subspace**: Dense 6×3 matrix (uses `JointMotionSubspaceTpl`) - **Reversal**: Not supported (motion subspace is frame-dependent) - **Testing**: Compared against SphericalZYX for rotation equivalence @@ -1061,5 +1059,6 @@ Key files to review: - [`joint-ellipsoid.hpp`](include/pinocchio/multibody/joint/joint-ellipsoid.hpp) - [`unittest/joint-ellipsoid.cpp`](unittest/joint-ellipsoid.cpp) - [`examples/ellipsoid-joint-kinematics.py`](examples/ellipsoid-joint-kinematics.py) + --- **Happy implementing! If you have questions, consult the Pinocchio community or review existing joint implementations for guidance.** diff --git a/doc/e-development/intro.md b/doc/e-development/intro.md new file mode 100644 index 0000000000..17664223f2 --- /dev/null +++ b/doc/e-development/intro.md @@ -0,0 +1,3 @@ +# Development {#md_doc_e-dev_intro} + +This section contains information to Pinocchio developers diff --git a/doc/treeview.dox b/doc/treeview.dox index ea2a1b3615..bc20d02c05 100644 --- a/doc/treeview.dox +++ b/doc/treeview.dox @@ -64,6 +64,10 @@ namespace pinocchio { - \subpage md_doc_d-practical-exercises_7-learn */ + /** \page md_doc_e-dev_intro Development + - \subpage md_doc_e-dev_impl-new-joint + */ + // // Modules organization // From d092321c79aaaf6275057428c5b39372c6548d23 Mon Sep 17 00:00:00 2001 From: Joris Vaillant Date: Wed, 26 Nov 2025 15:45:49 +0100 Subject: [PATCH 39/48] core: Avoid cast build issue --- include/pinocchio/multibody/joint/joint-ellipsoid.hpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/include/pinocchio/multibody/joint/joint-ellipsoid.hpp b/include/pinocchio/multibody/joint/joint-ellipsoid.hpp index d18670ed6e..7dbbcc534f 100644 --- a/include/pinocchio/multibody/joint/joint-ellipsoid.hpp +++ b/include/pinocchio/multibody/joint/joint-ellipsoid.hpp @@ -334,7 +334,10 @@ namespace pinocchio JointModelEllipsoidTpl cast() const { typedef JointModelEllipsoidTpl ReturnType; - ReturnType res(radius_a, radius_b, radius_c); + ReturnType res{ + ScalarCast::cast(radius_a), + ScalarCast::cast(radius_b), + ScalarCast::cast(radius_c)}; res.setIndexes(id(), idx_q(), idx_v(), idx_vExtended()); return res; } From 4cba8994d418cbaf28017d7f018545acf9262232 Mon Sep 17 00:00:00 2001 From: Joris Vaillant Date: Wed, 26 Nov 2025 16:07:18 +0100 Subject: [PATCH 40/48] python: Remove redundant member definition --- .../python/parsers/graph/expose-edges.cpp | 48 ++++++++++--------- .../python/multibody/joint/joints-datas.hpp | 28 +++++------ 2 files changed, 40 insertions(+), 36 deletions(-) diff --git a/bindings/python/parsers/graph/expose-edges.cpp b/bindings/python/parsers/graph/expose-edges.cpp index c1251c2d31..90ebe72727 100644 --- a/bindings/python/parsers/graph/expose-edges.cpp +++ b/bindings/python/parsers/graph/expose-edges.cpp @@ -66,8 +66,9 @@ namespace pinocchio bp::class_( "JointFixed", "Represents a fixed joint in the graph.", bp::init<>(bp::args("self"), "Default constructor.")) - .def(bp::init( - bp::args("self", "pose"), "Constructor with joint offset.")) + .def( + bp::init( + bp::args("self", "pose"), "Constructor with joint offset.")) .def_readwrite("joint_offset", &JointFixed::joint_offset, "Offset of the joint.") .def_readonly("nq", &JointFixed::nq, "Number of configuration variables.") .def_readonly("nv", &JointFixed::nv, "Number of tangent variables."); @@ -117,10 +118,9 @@ namespace pinocchio bp::class_( "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.") - .def(bp::init( - bp::args("self", "radius_a", "radius_b", "radius_c"), "Constructor with radii.")) + .def( + bp::init( + 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( @@ -163,12 +163,14 @@ namespace pinocchio bp::class_( "JointComposite", "Represents a composite joint.", bp::init<>(bp::args("self"), "Default constructor.")) - .def(bp::init( - bp::args("self", "joint_variant", "joint_pose"), - "Constructor with a single joint and its placement.")) - .def(bp::init &, const std::vector &>( - bp::args("self", "joints_variants", "joint_poses"), - "Constructor with multiple joints and their placements.")) + .def( + bp::init( + bp::args("self", "joint_variant", "joint_pose"), + "Constructor with a single joint and its placement.")) + .def( + bp::init &, const std::vector &>( + bp::args("self", "joints_variants", "joint_poses"), + "Constructor with multiple joints and their placements.")) .def_readwrite("joints", &JointComposite::joints, "List of joints in the composite joint.") .def_readwrite( "jointsPlacements", &JointComposite::jointsPlacements, @@ -187,9 +189,10 @@ namespace pinocchio bp::class_( "JointMimic", "Represents a mimic joint.", bp::init<>(bp::args("self"), "Default constructor.")) - .def(bp::init( - bp::args("self", "secondary_joint_model", "primary_name", "scaling", "offset"), - "Constructor for mimic joint.")) + .def( + bp::init( + bp::args("self", "secondary_joint_model", "primary_name", "scaling", "offset"), + "Constructor for mimic joint.")) .def_readwrite( "primary_name", &JointMimic::primary_name, "Name of the primary joint being mimicked.") .def_readwrite( @@ -235,13 +238,14 @@ namespace pinocchio bp::class_( "EdgeParameters", "Parameters for defining an edge (joint) in the ModelGraph.") .def(bp::init<>()) - .def(bp::init< - const std::string &, const std::string &, const SE3 &, const std::string &, - const SE3 &, const JointVariant &, const boost::optional>( - (bp::arg("name"), bp::arg("source_vertex"), bp::arg("source_to_joint"), - bp::arg("target_vertex"), bp::arg("joint_to_target"), bp::arg("joint"), - bp::arg("q_ref") = boost::none), - "Constructor to define an edge with specific parameters.")) + .def( + bp::init< + const std::string &, const std::string &, const SE3 &, const std::string &, const SE3 &, + const JointVariant &, const boost::optional>( + (bp::arg("name"), bp::arg("source_vertex"), bp::arg("source_to_joint"), + bp::arg("target_vertex"), bp::arg("joint_to_target"), bp::arg("joint"), + bp::arg("q_ref") = boost::none), + "Constructor to define an edge with specific parameters.")) .def_readwrite("name", &EdgeParameters::name, "Name of the edge/joint.") .def_readwrite( "source_vertex", &EdgeParameters::source_vertex, diff --git a/include/pinocchio/bindings/python/multibody/joint/joints-datas.hpp b/include/pinocchio/bindings/python/multibody/joint/joints-datas.hpp index bf72d8f238..6c25765047 100644 --- a/include/pinocchio/bindings/python/multibody/joint/joints-datas.hpp +++ b/include/pinocchio/bindings/python/multibody/joint/joints-datas.hpp @@ -23,8 +23,9 @@ namespace pinocchio inline bp::class_ & expose_joint_data(bp::class_ & cl) { - return cl.def(bp::init( - bp::args("axis"), "Init JointDataRevoluteUnaligned from an axis with x-y-z components")); + return cl.def( + bp::init( + bp::args("axis"), "Init JointDataRevoluteUnaligned from an axis with x-y-z components")); } // specialization for JointDataPrismaticUnaligned @@ -32,8 +33,9 @@ namespace pinocchio inline bp::class_ & expose_joint_data(bp::class_ & cl) { - return cl.def(bp::init( - bp::args("axis"), "Init JointDataPrismaticUnaligned from an axis with x-y-z components")); + return cl.def( + bp::init( + bp::args("axis"), "Init JointDataPrismaticUnaligned from an axis with x-y-z components")); } // specialization for JointDataHelicalUnaligned @@ -41,8 +43,9 @@ namespace pinocchio inline bp::class_ & expose_joint_data(bp::class_ & cl) { - return cl.def(bp::init( - bp::args("axis"), "Init JointDataHelicalUnaligned from an axis with x-y-z components")); + return cl.def( + bp::init( + bp::args("axis"), "Init JointDataHelicalUnaligned from an axis with x-y-z components")); } template<> @@ -63,11 +66,7 @@ namespace pinocchio inline bp::class_ & expose_joint_data(bp::class_ & 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); + return cl.add_property("StU", &JointDataEllipsoid::StU); } template<> @@ -75,9 +74,10 @@ namespace pinocchio expose_joint_data(bp::class_ & cl) { return cl - .def(bp::init( - bp::args("joint_data_vectors", "nq", "nv"), - "Init JointDataComposite from a given collection of joint data")) + .def( + bp::init( + bp::args("joint_data_vectors", "nq", "nv"), + "Init JointDataComposite from a given collection of joint data")) .add_property("joints", &JointDataComposite::joints) .add_property("iMlast", &JointDataComposite::iMlast) .add_property("pjMi", &JointDataComposite::pjMi) From abe5d3c0957f0229223dadc28ec98302f2e8a345 Mon Sep 17 00:00:00 2001 From: Joris Vaillant Date: Wed, 26 Nov 2025 16:08:04 +0100 Subject: [PATCH 41/48] lint: Run linter --- .../python/parsers/graph/expose-edges.cpp | 46 ++++++++----------- doc/e-development/Implement_a_new_joint.md | 18 ++++---- .../python/multibody/joint/joints-datas.hpp | 22 ++++----- 3 files changed, 38 insertions(+), 48 deletions(-) diff --git a/bindings/python/parsers/graph/expose-edges.cpp b/bindings/python/parsers/graph/expose-edges.cpp index 90ebe72727..48e5c1510e 100644 --- a/bindings/python/parsers/graph/expose-edges.cpp +++ b/bindings/python/parsers/graph/expose-edges.cpp @@ -66,9 +66,8 @@ namespace pinocchio bp::class_( "JointFixed", "Represents a fixed joint in the graph.", bp::init<>(bp::args("self"), "Default constructor.")) - .def( - bp::init( - bp::args("self", "pose"), "Constructor with joint offset.")) + .def(bp::init( + bp::args("self", "pose"), "Constructor with joint offset.")) .def_readwrite("joint_offset", &JointFixed::joint_offset, "Offset of the joint.") .def_readonly("nq", &JointFixed::nq, "Number of configuration variables.") .def_readonly("nv", &JointFixed::nv, "Number of tangent variables."); @@ -118,9 +117,8 @@ namespace pinocchio bp::class_( "JointEllipsoid", "Represents an ellipsoidal joint.", bp::init<>(bp::args("self"), "Default constructor.")) - .def( - bp::init( - bp::args("self", "radius_a", "radius_b", "radius_c"), "Constructor with radii.")) + .def(bp::init( + 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( @@ -163,14 +161,12 @@ namespace pinocchio bp::class_( "JointComposite", "Represents a composite joint.", bp::init<>(bp::args("self"), "Default constructor.")) - .def( - bp::init( - bp::args("self", "joint_variant", "joint_pose"), - "Constructor with a single joint and its placement.")) - .def( - bp::init &, const std::vector &>( - bp::args("self", "joints_variants", "joint_poses"), - "Constructor with multiple joints and their placements.")) + .def(bp::init( + bp::args("self", "joint_variant", "joint_pose"), + "Constructor with a single joint and its placement.")) + .def(bp::init &, const std::vector &>( + bp::args("self", "joints_variants", "joint_poses"), + "Constructor with multiple joints and their placements.")) .def_readwrite("joints", &JointComposite::joints, "List of joints in the composite joint.") .def_readwrite( "jointsPlacements", &JointComposite::jointsPlacements, @@ -189,10 +185,9 @@ namespace pinocchio bp::class_( "JointMimic", "Represents a mimic joint.", bp::init<>(bp::args("self"), "Default constructor.")) - .def( - bp::init( - bp::args("self", "secondary_joint_model", "primary_name", "scaling", "offset"), - "Constructor for mimic joint.")) + .def(bp::init( + bp::args("self", "secondary_joint_model", "primary_name", "scaling", "offset"), + "Constructor for mimic joint.")) .def_readwrite( "primary_name", &JointMimic::primary_name, "Name of the primary joint being mimicked.") .def_readwrite( @@ -238,14 +233,13 @@ namespace pinocchio bp::class_( "EdgeParameters", "Parameters for defining an edge (joint) in the ModelGraph.") .def(bp::init<>()) - .def( - bp::init< - const std::string &, const std::string &, const SE3 &, const std::string &, const SE3 &, - const JointVariant &, const boost::optional>( - (bp::arg("name"), bp::arg("source_vertex"), bp::arg("source_to_joint"), - bp::arg("target_vertex"), bp::arg("joint_to_target"), bp::arg("joint"), - bp::arg("q_ref") = boost::none), - "Constructor to define an edge with specific parameters.")) + .def(bp::init< + const std::string &, const std::string &, const SE3 &, const std::string &, + const SE3 &, const JointVariant &, const boost::optional>( + (bp::arg("name"), bp::arg("source_vertex"), bp::arg("source_to_joint"), + bp::arg("target_vertex"), bp::arg("joint_to_target"), bp::arg("joint"), + bp::arg("q_ref") = boost::none), + "Constructor to define an edge with specific parameters.")) .def_readwrite("name", &EdgeParameters::name, "Name of the edge/joint.") .def_readwrite( "source_vertex", &EdgeParameters::source_vertex, diff --git a/doc/e-development/Implement_a_new_joint.md b/doc/e-development/Implement_a_new_joint.md index 513c25c169..81ddf3579a 100644 --- a/doc/e-development/Implement_a_new_joint.md +++ b/doc/e-development/Implement_a_new_joint.md @@ -1,6 +1,6 @@ # How to implement a new joint in Pinocchio {#md_doc_e-dev_impl-new-joint} -**Tags:** #Pinocchio #tutorial #joints #implementation #motionsubspace +**Tags:** #Pinocchio #tutorial #joints #implementation #motionsubspace This guide provides a comprehensive, step-by-step approach to implementing a new joint type in Pinocchio. It covers mathematical foundations, core implementation, parser integration, and testing requirements. @@ -146,7 +146,7 @@ First, determine your joint's configuration and velocity spaces: ### 1.2 Derive the Spatial Transform -Compute the spatial transformation matrix from parent frame **p** to child frame **c**: +Compute the spatial transformation matrix from parent frame **p** to child frame **c**: \f[ {^p}X_c(q) = \begin{bmatrix} R(q) & 0 \\ -R(q)p(q)^\times & R(q) \end{bmatrix} \f] @@ -204,11 +204,11 @@ Or equivalently: > [!TIP] > Use symbolic computation tools (SymPy, Mathematica) to derive these expressions. The math can become complex quickly, especially for \f$\dot{S}\f$. > ```python -> Sdot = sp.Matrix.zeros(6, 3) -> for i in range(3): -> for j in range(3): +> Sdot = sp.Matrix.zeros(6, 3) +> for i in range(3): +> for j in range(3): > Sdot[:, i] += sp.diff(S[:, i], q[j]) * qdot[j] -> ``` +> ``` --- ## Step 2: Core Joint Implementation @@ -739,7 +739,7 @@ if (joint.same_direction) { // Copy configuration q_target.template segment(offset_target) = q_source.template segment(offset_source); -} else +} else { // Reverse configuration (e.g., negate angles) q_target.template segment(offset_target) = @@ -759,7 +759,7 @@ v_target.template segment(offset_target) = v_source.template segment(offset_source); } else { // Reverse velocity -v_target.template segment(offset_target) +v_target.template segment(offset_target) -v_source.template segment(offset_source); } } @@ -1054,7 +1054,7 @@ As a concrete example, the Ellipsoid joint implementation demonstrates: - **Motion subspace**: Dense 6×3 matrix (uses `JointMotionSubspaceTpl`) - **Reversal**: Not supported (motion subspace is frame-dependent) - **Testing**: Compared against SphericalZYX for rotation equivalence - + Key files to review: - [`joint-ellipsoid.hpp`](include/pinocchio/multibody/joint/joint-ellipsoid.hpp) - [`unittest/joint-ellipsoid.cpp`](unittest/joint-ellipsoid.cpp) diff --git a/include/pinocchio/bindings/python/multibody/joint/joints-datas.hpp b/include/pinocchio/bindings/python/multibody/joint/joints-datas.hpp index 6c25765047..35f3ad88fb 100644 --- a/include/pinocchio/bindings/python/multibody/joint/joints-datas.hpp +++ b/include/pinocchio/bindings/python/multibody/joint/joints-datas.hpp @@ -23,9 +23,8 @@ namespace pinocchio inline bp::class_ & expose_joint_data(bp::class_ & cl) { - return cl.def( - bp::init( - bp::args("axis"), "Init JointDataRevoluteUnaligned from an axis with x-y-z components")); + return cl.def(bp::init( + bp::args("axis"), "Init JointDataRevoluteUnaligned from an axis with x-y-z components")); } // specialization for JointDataPrismaticUnaligned @@ -33,9 +32,8 @@ namespace pinocchio inline bp::class_ & expose_joint_data(bp::class_ & cl) { - return cl.def( - bp::init( - bp::args("axis"), "Init JointDataPrismaticUnaligned from an axis with x-y-z components")); + return cl.def(bp::init( + bp::args("axis"), "Init JointDataPrismaticUnaligned from an axis with x-y-z components")); } // specialization for JointDataHelicalUnaligned @@ -43,9 +41,8 @@ namespace pinocchio inline bp::class_ & expose_joint_data(bp::class_ & cl) { - return cl.def( - bp::init( - bp::args("axis"), "Init JointDataHelicalUnaligned from an axis with x-y-z components")); + return cl.def(bp::init( + bp::args("axis"), "Init JointDataHelicalUnaligned from an axis with x-y-z components")); } template<> @@ -74,10 +71,9 @@ namespace pinocchio expose_joint_data(bp::class_ & cl) { return cl - .def( - bp::init( - bp::args("joint_data_vectors", "nq", "nv"), - "Init JointDataComposite from a given collection of joint data")) + .def(bp::init( + bp::args("joint_data_vectors", "nq", "nv"), + "Init JointDataComposite from a given collection of joint data")) .add_property("joints", &JointDataComposite::joints) .add_property("iMlast", &JointDataComposite::iMlast) .add_property("pjMi", &JointDataComposite::pjMi) From 36e708a9965fc76268456036c1a37e3d5c58302e Mon Sep 17 00:00:00 2001 From: Joris Vaillant Date: Thu, 27 Nov 2025 09:39:36 +0100 Subject: [PATCH 42/48] examples: Try to overcome a Windows encoding issue --- examples/ellipsoid-joint-kinematics.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/ellipsoid-joint-kinematics.py b/examples/ellipsoid-joint-kinematics.py index 227f651b12..26c31f7258 100644 --- a/examples/ellipsoid-joint-kinematics.py +++ b/examples/ellipsoid-joint-kinematics.py @@ -171,7 +171,7 @@ def compute_dynamics_example(): # So the resulting tau vector contains three generalized forces that # include contributions from both rotational and translational effects. tau = pin.rnea(model, data, q, v, a) - print(f"\nRequired torques (RNEA): τ = {tau}") + print(f"\nRequired torques (RNEA): \N{GREEK SMALL LETTER TAU} = {tau}") # For example tau[0] is a combination of # S_11(q)* f_x + S_21(q)* f_y + S_31(q)* f_z From d27d0cab89ea438aec341a1a3cae1a682bf0fbf2 Mon Sep 17 00:00:00 2001 From: Joris Vaillant Date: Thu, 27 Nov 2025 17:37:34 +0100 Subject: [PATCH 43/48] Revert "examples: Try to overcome a Windows encoding issue" This reverts commit 36e708a9965fc76268456036c1a37e3d5c58302e. --- examples/ellipsoid-joint-kinematics.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/ellipsoid-joint-kinematics.py b/examples/ellipsoid-joint-kinematics.py index 26c31f7258..227f651b12 100644 --- a/examples/ellipsoid-joint-kinematics.py +++ b/examples/ellipsoid-joint-kinematics.py @@ -171,7 +171,7 @@ def compute_dynamics_example(): # So the resulting tau vector contains three generalized forces that # include contributions from both rotational and translational effects. tau = pin.rnea(model, data, q, v, a) - print(f"\nRequired torques (RNEA): \N{GREEK SMALL LETTER TAU} = {tau}") + print(f"\nRequired torques (RNEA): τ = {tau}") # For example tau[0] is a combination of # S_11(q)* f_x + S_21(q)* f_y + S_31(q)* f_z From 1f1c10910e8c797847582c1c68ee415f8ccf09ba Mon Sep 17 00:00:00 2001 From: Joris Vaillant Date: Thu, 27 Nov 2025 17:39:50 +0100 Subject: [PATCH 44/48] ci: Another try to fix greek letter output issue --- .github/workflows/macos-linux-windows-pixi.yml | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/.github/workflows/macos-linux-windows-pixi.yml b/.github/workflows/macos-linux-windows-pixi.yml index dd2077c37b..e3e55c67b7 100644 --- a/.github/workflows/macos-linux-windows-pixi.yml +++ b/.github/workflows/macos-linux-windows-pixi.yml @@ -29,6 +29,12 @@ concurrency: group: ${{ github.workflow }}-${{ github.ref }} cancel-in-progress: true +env: + # This is needed on Windows because ellipsoid-joint-kinematics.py output + # some utf8 char. + # More detail here: https://gist.github.com/NodeJSmith/e7e37f2d3f162456869f015f842bcf15 + PYTHONIOENCODING: "utf8" + jobs: pinocchio-pixi: name: Standard - ${{ matrix.os }} - Env ${{ matrix.environment }} ${{ matrix.build_type }} ${{ matrix.compiler }} From 25bbc67a5d5350120509b887df750603cf87f161 Mon Sep 17 00:00:00 2001 From: ipuch Date: Wed, 3 Dec 2025 17:59:14 +0100 Subject: [PATCH 45/48] feat/refactor(joint-ellipsoid): joint specialization precommit --- .../multibody/joint/joint-ellipsoid.hpp | 284 +++++++++++++++++- 1 file changed, 277 insertions(+), 7 deletions(-) diff --git a/include/pinocchio/multibody/joint/joint-ellipsoid.hpp b/include/pinocchio/multibody/joint/joint-ellipsoid.hpp index 7dbbcc534f..234e323137 100644 --- a/include/pinocchio/multibody/joint/joint-ellipsoid.hpp +++ b/include/pinocchio/multibody/joint/joint-ellipsoid.hpp @@ -16,6 +16,273 @@ namespace pinocchio template struct JointEllipsoidTpl; + template + struct JointMotionSubspaceEllipsoidTpl; + + template + struct SE3GroupAction> + { + typedef Eigen::Matrix ReturnType; + }; + + template + struct MotionAlgebraAction, MotionDerived> + { + typedef Eigen::Matrix ReturnType; + }; + + template + struct ConstraintForceOp, ForceDerived> + { + typedef Eigen::Matrix ReturnType; + }; + + template + struct ConstraintForceSetOp, ForceSet> + { + typedef Eigen::Matrix ReturnType; + }; + + template + struct traits> + { + typedef _Scalar Scalar; + enum + { + Options = _Options + }; + enum + { + LINEAR = 0, + ANGULAR = 3 + }; + + typedef MotionTpl JointMotion; + typedef Eigen::Matrix JointForce; + typedef Eigen::Matrix DenseBase; + typedef Eigen::Matrix ReducedSquaredMatrix; + + typedef DenseBase MatrixReturnType; + typedef const DenseBase ConstMatrixReturnType; + + typedef typename ReducedSquaredMatrix::IdentityReturnType StDiagonalMatrixSOperationReturnType; + }; + + template + struct JointMotionSubspaceEllipsoidTpl + : JointMotionSubspaceBase> + { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(JointMotionSubspaceEllipsoidTpl) + enum + { + NV = 3 + }; + + typedef Eigen::Matrix Matrix63; + Matrix63 S; + + JointMotionSubspaceEllipsoidTpl() + { + } + + template + JointMotion __mult__(const Eigen::MatrixBase & v) const + { + // Compute first 6 rows from first 2 columns + Eigen::Matrix result = S.template block<6, 2>(0, 0) * v.template segment<2>(0); + + // Add contribution from last column (only row 5 is non-zero: S(5,2) = 1) + result[5] += v[2]; + + return JointMotion(result); + } + + template + typename SE3GroupAction::ReturnType + se3Action(const SE3Tpl & m) const + { + typedef typename SE3GroupAction::ReturnType ReturnType; + ReturnType res; + // Apply SE3 action to first two columns + motionSet::se3Action(m, S.template leftCols<2>(), res.template leftCols<2>()); + + res.template block<3, 1>(LINEAR, 2).noalias() = m.translation().cross(m.rotation().col(2)); + res.template block<3, 1>(ANGULAR, 2) = m.rotation().col(2); + + return res; + } + + template + typename SE3GroupAction::ReturnType + se3ActionInverse(const SE3Tpl & m) const + { + typedef typename SE3GroupAction::ReturnType ReturnType; + ReturnType res; + // Apply inverse SE3 action to first two columns + motionSet::se3ActionInverse(m, S.template leftCols<2>(), res.template leftCols<2>()); + + // Third column: inverse action on [0, 0, 0, 0, 0, 1]^T + // angular part: R^T * e_z + res.template block<3, 1>(ANGULAR, 2) = m.rotation().transpose().col(2); + // linear part: R^T * (t × e_z) = R^T * [-t_y, t_x, 0]^T + typedef Eigen::Matrix Vector3; + const Vector3 t_cross_ez(-m.translation()[1], m.translation()[0], Scalar(0)); + res.template block<3, 1>(LINEAR, 2).noalias() = m.rotation().transpose() * t_cross_ez; + + return res; + } + + int nv_impl() const + { + return NV; + } + + struct TransposeConst : JointMotionSubspaceTransposeBase + { + const JointMotionSubspaceEllipsoidTpl & ref; + TransposeConst(const JointMotionSubspaceEllipsoidTpl & ref) + : ref(ref) + { + } + + template + typename ConstraintForceOp::ReturnType + operator*(const ForceDense & f) const + { + typedef + typename ConstraintForceOp::ReturnType + ReturnType; + ReturnType res; + + // First two rows: S[:, 0:2]^T * f + res.template head<2>().noalias() = ref.S.template leftCols<2>().transpose() * f.toVector(); + + // Third row: [0,0,0,0,0,1]^T · f = f.angular()[2] + res[2] = f.angular()[2]; + + return res; + } + + /// [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) + template + Eigen::Matrix< + Scalar, + 3, + Derived::ColsAtCompileTime, + Options | Eigen::RowMajor, + 3, + Derived::MaxColsAtCompileTime> + operator*(const Eigen::MatrixBase & F) const + { + EIGEN_STATIC_ASSERT( + Derived::RowsAtCompileTime == 6, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE) + typedef Eigen::Matrix< + Scalar, 3, Derived::ColsAtCompileTime, Options | Eigen::RowMajor, 3, + Derived::MaxColsAtCompileTime> + ReturnType; + + ReturnType res(3, F.cols()); + + // First two rows: S[:, 0:2]^T * F + res.template topRows<2>().noalias() = + ref.S.template leftCols<2>().transpose() * F.derived(); + + // Third row: [0,0,0,0,0,1]^T · F = F.row(5) + res.row(2) = F.row(5); + + return res; + } + }; // struct TransposeConst + + TransposeConst transpose() const + { + return TransposeConst(*this); + } + + /* CRBA joint operators + * - ForceSet::Block = ForceSet + * - ForceSet operator* (Inertia Y,Constraint S) + * - MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) + * - SE3::act(ForceSet::Block) + */ + DenseBase matrix_impl() const + { + DenseBase res; + + // Columns 0-1: copy the first two columns of S (6x2) + res.template leftCols<2>() = S.template leftCols<2>(); + + // Column 2: sparse [0, 0, 0, 0, 0, 1]^T + res.template block<5, 1>(0, 2).setZero(); + res(5, 2) = Scalar(1); + + return res; + } + + template + typename MotionAlgebraAction::ReturnType + motionAction(const MotionDense & m) const + { + typedef + typename MotionAlgebraAction::ReturnType + ReturnType; + ReturnType res; + + // Motion action on first two columns + motionSet::motionAction(m, S.template leftCols<2>(), res.template leftCols<2>()); + + // Motion action on third column [0, 0, 0, 0, 0, 1]^T + // [v; w] × [0; e_z] = [v × e_z; w × e_z] + const typename MotionDense::ConstLinearType & v_lin = m.linear(); + const typename MotionDense::ConstAngularType & w = m.angular(); + + // v × e_z = [v[1], -v[0], 0] + res(LINEAR + 0, 2) = v_lin[1]; + res(LINEAR + 1, 2) = -v_lin[0]; + res(LINEAR + 2, 2) = Scalar(0); + + // w × e_z = [w[1], -w[0], 0] + res(ANGULAR + 0, 2) = w[1]; + res(ANGULAR + 1, 2) = -w[0]; + res(ANGULAR + 2, 2) = Scalar(0); + + return res; + } + + bool isEqual(const JointMotionSubspaceEllipsoidTpl & other) const + { + return S == other.S; + } + + }; // struct JointMotionSubspaceEllipsoidTpl + + /* [CRBA] ForceSet operator* (Inertia Y, Constraint S) */ + template + Eigen::Matrix + operator*(const InertiaTpl & Y, const JointMotionSubspaceEllipsoidTpl & S) + { + // Y * S where Y is 6x6 inertia and S is 6x3 motion subspace + // Returns 6x3 force set + typedef InertiaTpl Inertia; + Eigen::Matrix M; + M.noalias() = Y.matrix() * S.matrix(); + return M; + } + + /* [ABA] Y*S operator (Matrix6 Y, Constraint S) */ + template + typename MatrixMatrixProduct< + Matrix6Like, + typename JointMotionSubspaceEllipsoidTpl::DenseBase>::type + operator*( + const Eigen::MatrixBase & Y, const JointMotionSubspaceEllipsoidTpl & S) + { + EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix6Like, 6, 6); + return Y.derived() * S.matrix(); + } + template struct traits> { @@ -32,7 +299,9 @@ namespace pinocchio }; typedef JointDataEllipsoidTpl JointDataDerived; typedef JointModelEllipsoidTpl JointModelDerived; - typedef JointMotionSubspaceTpl<3, Scalar, Options, 3> Constraint_t; + // typedef JointMotionSubspaceTpl<3, Scalar, Options, 3> Constraint_t; + typedef JointMotionSubspaceEllipsoidTpl Constraint_t; + typedef SE3Tpl Transformation_t; typedef MotionTpl Motion_t; @@ -398,12 +667,13 @@ namespace pinocchio S_32 = dndotx_dqdot1 * radius_a * s1 - dndoty_dqdot1 * radius_b * c1 * s0 + dndotz_dqdot1 * radius_c * c0 * c1; - data.S.matrix() << S_11 , S_12 , Scalar(0), - S_21 , S_22 , Scalar(0), - S_31 , S_32 , Scalar(0), - c1 * c2, s2 , Scalar(0), - -c1 * s2, c2 , Scalar(0), - s1 , Scalar(0), Scalar(1); + // Write directly to the internal matrix S, not to matrix() which returns a copy + data.S.S << S_11 , S_12 , Scalar(0), + S_21 , S_22 , Scalar(0), + S_31 , S_32 , Scalar(0), + c1 * c2, s2 , Scalar(0), + -c1 * s2, c2 , Scalar(0), + s1 , Scalar(0), Scalar(1); // clang-format on } From 8bed496dd6bd3c39d7d77026dcd212634e7256a3 Mon Sep 17 00:00:00 2001 From: Pierre Puchaud <40755537+Ipuch@users.noreply.github.com> Date: Thu, 4 Dec 2025 15:15:24 +0100 Subject: [PATCH 46/48] refactor(joint-ellipsoid): review comments. Co-authored-by: Joris Vaillant precommit --- .../multibody/joint/joint-ellipsoid.hpp | 83 ++++++++++--------- 1 file changed, 46 insertions(+), 37 deletions(-) diff --git a/include/pinocchio/multibody/joint/joint-ellipsoid.hpp b/include/pinocchio/multibody/joint/joint-ellipsoid.hpp index 234e323137..29981e1952 100644 --- a/include/pinocchio/multibody/joint/joint-ellipsoid.hpp +++ b/include/pinocchio/multibody/joint/joint-ellipsoid.hpp @@ -10,6 +10,7 @@ #include "pinocchio/multibody/joint-motion-subspace.hpp" #include "pinocchio/math/sincos.hpp" #include "pinocchio/math/matrix.hpp" +#include "pinocchio/spatial/spatial-axis.hpp" namespace pinocchio { @@ -40,7 +41,9 @@ namespace pinocchio template struct ConstraintForceSetOp, ForceSet> { - typedef Eigen::Matrix ReturnType; + typedef typename traits>::DenseBase DenseBase; + typedef + typename MatrixMatrixProduct, ForceSet>::type ReturnType; }; template @@ -80,13 +83,11 @@ namespace pinocchio NV = 3 }; + typedef SpatialAxis<5> AxisRotz; + typedef Eigen::Matrix Matrix63; Matrix63 S; - JointMotionSubspaceEllipsoidTpl() - { - } - template JointMotion __mult__(const Eigen::MatrixBase & v) const { @@ -127,9 +128,8 @@ namespace pinocchio // angular part: R^T * e_z res.template block<3, 1>(ANGULAR, 2) = m.rotation().transpose().col(2); // linear part: R^T * (t × e_z) = R^T * [-t_y, t_x, 0]^T - typedef Eigen::Matrix Vector3; - const Vector3 t_cross_ez(-m.translation()[1], m.translation()[0], Scalar(0)); - res.template block<3, 1>(LINEAR, 2).noalias() = m.rotation().transpose() * t_cross_ez; + res.template block<3, 1>(LINEAR, 2).noalias() = + m.rotation().transpose() * SpatialAxis<5>::CartesianAxis3::cross(m.translation()); return res; } @@ -194,6 +194,12 @@ namespace pinocchio return res; } + // template + // typename ConstraintForceSetOp::ReturnType + // operator*(const Eigen::MatrixBase & F) + // { + // return ref.S.transpose() * F.derived(); + // } }; // struct TransposeConst TransposeConst transpose() const @@ -209,16 +215,7 @@ namespace pinocchio */ DenseBase matrix_impl() const { - DenseBase res; - - // Columns 0-1: copy the first two columns of S (6x2) - res.template leftCols<2>() = S.template leftCols<2>(); - - // Column 2: sparse [0, 0, 0, 0, 0, 1]^T - res.template block<5, 1>(0, 2).setZero(); - res(5, 2) = Scalar(1); - - return res; + return S; } template @@ -231,22 +228,28 @@ namespace pinocchio ReturnType res; // Motion action on first two columns + typedef SpatialAxis<5> AxisZ; motionSet::motionAction(m, S.template leftCols<2>(), res.template leftCols<2>()); + // res.col(2).noalias() = m.cross(()); + + // We have to use a MotionRef to deal with the output of the cross product + MotionRef v_col2(res.col(2)); + v_col2 = m.cross(AxisRotz()); - // Motion action on third column [0, 0, 0, 0, 0, 1]^T - // [v; w] × [0; e_z] = [v × e_z; w × e_z] - const typename MotionDense::ConstLinearType & v_lin = m.linear(); - const typename MotionDense::ConstAngularType & w = m.angular(); + // // Motion action on third column [0, 0, 0, 0, 0, 1]^T + // // [v; w] × [0; e_z] = [v × e_z; w × e_z] + // const typename MotionDense::ConstLinearType & v_lin = m.linear(); + // const typename MotionDense::ConstAngularType & w = m.angular(); - // v × e_z = [v[1], -v[0], 0] - res(LINEAR + 0, 2) = v_lin[1]; - res(LINEAR + 1, 2) = -v_lin[0]; - res(LINEAR + 2, 2) = Scalar(0); + // // v × e_z = [v[1], -v[0], 0] + // res(LINEAR + 0, 2) = v_lin[1]; + // res(LINEAR + 1, 2) = -v_lin[0]; + // res(LINEAR + 2, 2) = Scalar(0); - // w × e_z = [w[1], -w[0], 0] - res(ANGULAR + 0, 2) = w[1]; - res(ANGULAR + 1, 2) = -w[0]; - res(ANGULAR + 2, 2) = Scalar(0); + // // w × e_z = [w[1], -w[0], 0] + // res(ANGULAR + 0, 2) = w[1]; + // res(ANGULAR + 1, 2) = -w[0]; + // res(ANGULAR + 2, 2) = Scalar(0); return res; } @@ -260,14 +263,21 @@ namespace pinocchio /* [CRBA] ForceSet operator* (Inertia Y, Constraint S) */ template - Eigen::Matrix - operator*(const InertiaTpl & Y, const JointMotionSubspaceEllipsoidTpl & S) + Eigen::Matrix operator*( + const InertiaTpl & Y, const JointMotionSubspaceEllipsoidTpl & constraint) { - // Y * S where Y is 6x6 inertia and S is 6x3 motion subspace - // Returns 6x3 force set + // Y * S where S[:, 2] = [0,0,0,0,0,1]^T (rotation around z-axis) + // So M[:, 2] = Y * e_z = Y.matrix().col(5) + typedef Eigen::Matrix ReturnType; typedef InertiaTpl Inertia; - Eigen::Matrix M; - M.noalias() = Y.matrix() * S.matrix(); + ReturnType M; + + // Columns 0-1: Y * S[:, 0:2] + M.template leftCols<2>().noalias() = Y.matrix() * constraint.matrix().template leftCols<2>(); + + // Column 2: Y * [0,0,0,0,0,1]^T = Y.matrix().col(5) + M.col(2) = Y.matrix().col(Inertia::ANGULAR + 2); + return M; } @@ -299,7 +309,6 @@ namespace pinocchio }; typedef JointDataEllipsoidTpl JointDataDerived; typedef JointModelEllipsoidTpl JointModelDerived; - // typedef JointMotionSubspaceTpl<3, Scalar, Options, 3> Constraint_t; typedef JointMotionSubspaceEllipsoidTpl Constraint_t; typedef SE3Tpl Transformation_t; From 4d493682a707e7a1209d11e3d7a028168edf5c3e Mon Sep 17 00:00:00 2001 From: ipuch Date: Fri, 5 Dec 2025 12:17:40 +0100 Subject: [PATCH 47/48] fix(joint-ellipsoid): one more test fixed --- .../multibody/joint/joint-ellipsoid.hpp | 31 ++++++++++++++++++- 1 file changed, 30 insertions(+), 1 deletion(-) diff --git a/include/pinocchio/multibody/joint/joint-ellipsoid.hpp b/include/pinocchio/multibody/joint/joint-ellipsoid.hpp index 29981e1952..769b9af53c 100644 --- a/include/pinocchio/multibody/joint/joint-ellipsoid.hpp +++ b/include/pinocchio/multibody/joint/joint-ellipsoid.hpp @@ -68,7 +68,7 @@ namespace pinocchio typedef DenseBase MatrixReturnType; typedef const DenseBase ConstMatrixReturnType; - typedef typename ReducedSquaredMatrix::IdentityReturnType StDiagonalMatrixSOperationReturnType; + typedef ReducedSquaredMatrix StDiagonalMatrixSOperationReturnType; }; template @@ -261,6 +261,35 @@ namespace pinocchio }; // struct JointMotionSubspaceEllipsoidTpl + namespace details + { + template + struct StDiagonalMatrixSOperation> + { + typedef JointMotionSubspaceEllipsoidTpl Constraint; + typedef typename traits::StDiagonalMatrixSOperationReturnType ReturnType; + + static ReturnType run(const JointMotionSubspaceBase & S) + { + // Exploit sparse structure of last column: [0,0,0,0,0,1]^T + ReturnType res; + const typename Constraint::DenseBase & SMatrix = S.matrix(); + + // Upper-left dense 2x2 block: S^T * S + res.template topLeftCorner<2, 2>().noalias() = + SMatrix.template leftCols<2>().transpose() * SMatrix.template leftCols<2>(); + // Third column/row: S(5, 0), S(5, 1), 1 + res(0, 2) = SMatrix(5, 0); + res(1, 2) = SMatrix(5, 1); + res(2, 2) = Scalar(1); + res(2, 0) = res(0, 2); + res(2, 1) = res(1, 2); + + return res; + } + }; + } // namespace details + /* [CRBA] ForceSet operator* (Inertia Y, Constraint S) */ template Eigen::Matrix operator*( From fcfb5ba63ce91606b3198a03a61c111e9823239c Mon Sep 17 00:00:00 2001 From: ipuch Date: Fri, 5 Dec 2025 12:24:03 +0100 Subject: [PATCH 48/48] wontfix: test not passing --- .../multibody/joint/joint-ellipsoid.hpp | 26 ++++++++++++------- 1 file changed, 16 insertions(+), 10 deletions(-) diff --git a/include/pinocchio/multibody/joint/joint-ellipsoid.hpp b/include/pinocchio/multibody/joint/joint-ellipsoid.hpp index 769b9af53c..33bdececb7 100644 --- a/include/pinocchio/multibody/joint/joint-ellipsoid.hpp +++ b/include/pinocchio/multibody/joint/joint-ellipsoid.hpp @@ -293,20 +293,26 @@ namespace pinocchio /* [CRBA] ForceSet operator* (Inertia Y, Constraint S) */ template Eigen::Matrix operator*( - const InertiaTpl & Y, const JointMotionSubspaceEllipsoidTpl & constraint) + const InertiaTpl & Y, const JointMotionSubspaceEllipsoidTpl & S) { - // Y * S where S[:, 2] = [0,0,0,0,0,1]^T (rotation around z-axis) - // So M[:, 2] = Y * e_z = Y.matrix().col(5) - typedef Eigen::Matrix ReturnType; - typedef InertiaTpl Inertia; - ReturnType M; + // none of this work at this stage: I'll squash after the review to make the code disappear. + // // Y * S where last col is [0,0,0,0,0,1]^T (rotation around z-axis) + // // So last column of M is Y * e_z = Y.matrix().col(5) + // typedef Eigen::Matrix ReturnType; + // typedef InertiaTpl Inertia; + // ReturnType M; - // Columns 0-1: Y * S[:, 0:2] - M.template leftCols<2>().noalias() = Y.matrix() * constraint.matrix().template leftCols<2>(); + // // Cache Y.matrix() to avoid computing it twice + // const typename Inertia::Matrix6 Y_mat = Y.matrix(); - // Column 2: Y * [0,0,0,0,0,1]^T = Y.matrix().col(5) - M.col(2) = Y.matrix().col(Inertia::ANGULAR + 2); + // // Columns 0-1: Y * S.template leftCols<2>() + // M.template leftCols<2>().noalias() = Y_mat * S.matrix().template leftCols<2>(); + // // Column 2: Y * [0,0,0,0,0,1]^T = Y.matrix().col(5) + // M.col(2) = Y_mat.col(Inertia::ANGULAR + 2); + typedef Eigen::Matrix ReturnType; + ReturnType M; + M.noalias() = Y.matrix() * S.matrix(); return M; }