diff --git a/CHANGELOG.md b/CHANGELOG.md index 3dd8941bcf..ed982eb641 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -14,6 +14,9 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/). - Add docker images ([#2776](https://github.com/stack-of-tasks/pinocchio/pull/2776)) - ROS: added jrl_cmakemodules dependency ([#2789](https://github.com/stack-of-tasks/pinocchio/pull/2789)) +### Added +- Add Ellipsoid Joint to the joint collection, get ready for biomechanics. ([#....])(todo) + ## [3.8.0] - 2025-09-17 ### Added diff --git a/bindings/python/parsers/graph/expose-edges.cpp b/bindings/python/parsers/graph/expose-edges.cpp index c5a260a82f..9f67647ccc 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; @@ -111,6 +114,24 @@ namespace pinocchio .def_readonly("nq", &JointSphericalZYX::nq, "Number of configuration variables.") .def_readonly("nv", &JointSphericalZYX::nv, "Number of tangent variables."); + bp::class_( + "JointEllipsoid", "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(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/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/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/bindings/python/multibody/joint/joints-models.hpp b/include/pinocchio/bindings/python/multibody/joint/joints-models.hpp index 77cca4d02d..5b54f3dceb 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-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..a87e8b29ca --- /dev/null +++ b/include/pinocchio/multibody/joint/joint-ellipsoid.hpp @@ -0,0 +1,601 @@ +// +// 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" + +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()) + , S() + , Sdot() + , 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; + + typedef typename Transformation_t::Vector3 Vector3; + + 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 + { + 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); + + 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; + + computeMotionSubspace(s0, c0, s1, c1, s2, c2, data); + } + + 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 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); + + // 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; + } + + 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); + + 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; + + 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); + + // 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); + + 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; + } + + 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, + const Scalar & s1, + const Scalar & c1, + const Scalar & s2, + const Scalar & c2, + JointDataDerived & data) const + { + + 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 + { + Scalar S_11, S_21, S_31, S_12, S_22, S_32; + + S_11 = dndoty_dqdot0 * radius_b * (c0 * s2 + c2 * s0 * s1) + + dndotz_dqdot0 * radius_c * (-c0 * c2 * s1 + s0 * s2); + + S_21 = -dndoty_dqdot0 * radius_b * (-c0 * c2 + s0 * s1 * s2) + + dndotz_dqdot0 * radius_c * (c0 * s1 * s2 + c2 * s0); + + S_31 = c1 * (-dndoty_dqdot0 * radius_b * s0 + dndotz_dqdot0 * radius_c * c0); + + 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 = -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 = 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); + } + 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 + +#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/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/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 8a9cdda041..fc3efc4ec0 100644 --- a/include/pinocchio/serialization/joints-model.hpp +++ b/include/pinocchio/serialization/joints-model.hpp @@ -205,6 +205,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/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/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..eb6178bd37 100644 --- a/unittest/all-joints.cpp +++ b/unittest/all-joints.cpp @@ -152,6 +152,21 @@ 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..e5986659e2 100644 --- a/unittest/finite-differences.cpp +++ b/unittest/finite-differences.cpp @@ -163,6 +163,20 @@ 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..d8e772b711 --- /dev/null +++ b/unittest/joint-ellipsoid.cpp @@ -0,0 +1,356 @@ +// +// 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); +} + +/// \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) +{ + 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 + 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(); + const Motion & spatial_vel_zyx = dataSphericalZYX.v[1]; + + // 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; + q_e = R.eulerAngles(0, 1, 2); // XYZ convention + + // 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 + + 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; + + BOOST_CHECK(w_s.isApprox(w_e)); + + // Compute forward kinematics with the converted configurations + forwardKinematics(modelEllipsoid, dataEllipsoid, q_e, qd_e); + + // 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())); + + JointDataSphericalZYX jDataSphereFK = jmodel_s.createData(); + jmodel_s.calc(jDataSphereFK, q_s, qd_s); + + // 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())); + BOOST_CHECK(dataEllipsoid.oMi[1].isApprox(dataSphericalZYX.oMi[1])); + + 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)); + + 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 = + aba(modelEllipsoid, dataEllipsoid, q_e, qd_e, dataEllipsoid.tau, Convention::WORLD); + + BOOST_CHECK(dataEllipsoid.ddq.isApprox(qdotdot_e)); +} + +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_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/joint-generic.cpp b/unittest/joint-generic.cpp index 1f993b7bb7..9658d94be0 100644 --- a/unittest/joint-generic.cpp +++ b/unittest/joint-generic.cpp @@ -264,6 +264,20 @@ 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/model-graph.cpp b/unittest/model-graph.cpp index ddf67bedc5..333cba8606 100644 --- a/unittest/model-graph.cpp +++ b/unittest/model-graph.cpp @@ -617,6 +617,43 @@ BOOST_AUTO_TEST_CASE(test_reverse_spherical_zyx) d_f.oMf[m_forward.getFrameId("body1", pinocchio::BODY)])); } +/// @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.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 + 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::Model::JointIndex idx; + + 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. BOOST_AUTO_TEST_CASE(test_reverse_composite) { @@ -659,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; @@ -695,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; diff --git a/unittest/serialization.cpp b/unittest/serialization.cpp index fc810abb67..3a665e54b9 100644 --- a/unittest/serialization.cpp +++ b/unittest/serialization.cpp @@ -349,6 +349,21 @@ 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> {