diff --git a/CHANGELOG.md b/CHANGELOG.md index 47bd9861d1..371a5a3750 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -14,6 +14,7 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/). - Add compatibility with NumPy 2 `__array__` API ([#2436](https://github.com/stack-of-tasks/pinocchio/pull/2436)) - Added argument to let users decide of root joint name when parsing models (urdf, mjcf, sdf) ([#2402](https://github.com/stack-of-tasks/pinocchio/pull/2402)) - Allow use of `pathlib.Path | str` for paths in python bindings ([#2431](https://github.com/stack-of-tasks/pinocchio/pull/2431)) +- Add Pseudo inertia and Log-cholesky parametrization ([#2296](https://github.com/stack-of-tasks/pinocchio/pull/2296)) ### Fixed - Fix linkage of Boost.Serialization on Windows ([#2400](https://github.com/stack-of-tasks/pinocchio/pull/2400)) @@ -22,6 +23,7 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/). - Fix class abstract error for Rviz viewer ([#2425](https://github.com/stack-of-tasks/pinocchio/pull/2425)) - Fix compilation issue with MSCV and C++17 ([#2437](https://github.com/stack-of-tasks/pinocchio/pull/2437)) - Fix `pinocchio-test-py-robot_wrapper` when building with SDF and collision support ([#2437](https://github.com/stack-of-tasks/pinocchio/pull/2437)) +- Fix crash when calling `Inertia::FromDynamicParameters` in Python with wrong vector size ([#2296](https://github.com/stack-of-tasks/pinocchio/pull/2296)) ### Changed diff --git a/README.md b/README.md index ecfdfb8a44..41b260d5e0 100644 --- a/README.md +++ b/README.md @@ -43,15 +43,19 @@ or via pip (currently only available on Linux): ## Table of contents + - [Table of contents](#table-of-contents) + - [Introducing Pinocchio 3](#introducing-pinocchio-3) - [Pinocchio main features](#pinocchio-main-features) - [Documentation](#documentation) - [Examples](#examples) - [Tutorials](#tutorials) + - [Pinocchio continuous integrations](#pinocchio-continuous-integrations) - [Performances](#performances) - [Ongoing developments](#ongoing-developments) + - [Installation](#installation) + - [ROS](#ros) - [Visualization](#visualization) - [Citing Pinocchio](#citing-pinocchio) - - [Citing specific algorithmic contributions](#citing-specific-algorithmic-contributions) - [Questions and Issues](#questions-and-issues) - [Credits](#credits) - [Open-source projects relying on Pinocchio](#open-source-projects-relying-on-pinocchio) @@ -265,7 +269,8 @@ The following people have been involved in the development of **Pinocchio** and - [Joris Vaillant](https://github.com/jorisv) (Inria): core developer and manager of the project - [Sebastian Castro](https://roboticseabass.com) (The AI Institute): MeshCat viewer feature extension - [Lev Kozlov](https://github.com/lvjonok): Kinetic and potential energy regressors -- [Megane Millan](https://github.com/MegMll) (Inria): Features extension and core developper +- [Megane Millan](https://github.com/MegMll) (Inria): Features extension and core developer +- [Simeon Nedelchev](https://github.com/simeon-ned): Pseudo inertia and Log-cholesky parametrization If you have participated in the development of **Pinocchio**, please add your name and contribution to this list. diff --git a/bindings/python/spatial/expose-inertia.cpp b/bindings/python/spatial/expose-inertia.cpp index b2063c2dab..2671b796ff 100644 --- a/bindings/python/spatial/expose-inertia.cpp +++ b/bindings/python/spatial/expose-inertia.cpp @@ -18,6 +18,8 @@ namespace pinocchio void exposeInertia() { InertiaPythonVisitor::expose(); + PseudoInertiaPythonVisitor::expose(); + LogCholeskyParametersPythonVisitor::expose(); StdAlignedVectorPythonVisitor::expose("StdVec_Inertia"); #ifndef PINOCCHIO_PYTHON_NO_SERIALIZATION serialize::vector_type>(); diff --git a/cmake b/cmake index 31e46019be..5275edfd1a 160000 --- a/cmake +++ b/cmake @@ -1 +1 @@ -Subproject commit 31e46019beda968ba9e516ad645a951c64256eed +Subproject commit 5275edfd1a8c6d45e0f90177edad93ef15b2008f diff --git a/include/pinocchio/bindings/python/context/generic.hpp b/include/pinocchio/bindings/python/context/generic.hpp index 04c7fd3f4b..9f3eb37616 100644 --- a/include/pinocchio/bindings/python/context/generic.hpp +++ b/include/pinocchio/bindings/python/context/generic.hpp @@ -54,6 +54,8 @@ namespace pinocchio typedef MotionTpl Motion; typedef ForceTpl Force; typedef InertiaTpl Inertia; + typedef PseudoInertiaTpl PseudoInertia; + typedef LogCholeskyParametersTpl LogCholeskyParameters; typedef Symmetric3Tpl Symmetric3; // Multibody diff --git a/include/pinocchio/bindings/python/spatial/inertia.hpp b/include/pinocchio/bindings/python/spatial/inertia.hpp index ec829e5619..1432c4f59b 100644 --- a/include/pinocchio/bindings/python/spatial/inertia.hpp +++ b/include/pinocchio/bindings/python/spatial/inertia.hpp @@ -19,6 +19,8 @@ #if EIGENPY_VERSION_AT_MOST(2, 8, 1) EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(pinocchio::Inertia) +EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(pinocchio::PseudoInertiaTpl) +EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(pinocchio::LogCholeskyParametersTpl) #endif namespace pinocchio @@ -49,8 +51,6 @@ namespace pinocchio void visit(PyClass & cl) const { static const Scalar dummy_precision = Eigen::NumTraits::dummy_precision(); - PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH - PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_SELF_ASSIGN_OVERLOADED cl.def( "__init__", bp::make_constructor( @@ -158,7 +158,7 @@ namespace pinocchio "I_{xz}, I_{yz}, I_{zz}]^T " "where I = I_C + mS^T(c)S(c) and I_C has its origin at the barycenter") .def( - "FromDynamicParameters", &Inertia::template FromDynamicParameters, + "FromDynamicParameters", &InertiaPythonVisitor::fromDynamicParameters_proxy, bp::args("dynamic_parameters"), "Builds and inertia matrix from a vector of dynamic parameters." "\nThe parameters are given as dynamic_parameters = [m, mc_x, mc_y, mc_z, I_{xx}, " @@ -192,6 +192,21 @@ namespace pinocchio "Z axis. Assumes a uniform density.") .staticmethod("FromCapsule") + .def( + "FromPseudoInertia", &Inertia::FromPseudoInertia, bp::args("pseudo_inertia"), + "Returns the Inertia created from a pseudo inertia object.") + .staticmethod("FromPseudoInertia") + + .def( + "toPseudoInertia", &Inertia::toPseudoInertia, bp::arg("self"), + "Returns the pseudo inertia representation of the inertia.") + + .def( + "FromLogCholeskyParameters", &Inertia::FromLogCholeskyParameters, + bp::args("log_cholesky_parameters"), + "Returns the Inertia created from log Cholesky parameters.") + .staticmethod("FromLogCholeskyParameters") + .def("__array__", (Matrix6(Inertia::*)() const) & Inertia::matrix) .def( "__array__", &__array__, @@ -200,7 +215,6 @@ namespace pinocchio .def_pickle(Pickle()) #endif ; - PINOCCHIO_COMPILER_DIAGNOSTIC_POP } static Scalar getMass(const Inertia & self) @@ -238,6 +252,18 @@ namespace pinocchio return self.toDynamicParameters(); } + static Inertia fromDynamicParameters_proxy(const VectorXs & params) + { + if (params.rows() != 10 || params.cols() != 1) + { + std::ostringstream shape; + shape << "(" << params.rows() << ", " << params.cols() << ")"; + throw std::invalid_argument( + "Wrong size: params" + shape.str() + " but should have the following shape (10, 1)"); + } + return Inertia::FromDynamicParameters(params); + } + static Inertia * makeFromMCI(const Scalar & mass, const Vector3 & lever, const Matrix3 & inertia) { @@ -295,6 +321,289 @@ namespace pinocchio }; // struct InertiaPythonVisitor + template + struct PseudoInertiaPythonVisitor + : public boost::python::def_visitor> + { + enum + { + Options = PseudoInertia::Options + }; + typedef typename PseudoInertia::Scalar Scalar; + typedef typename PseudoInertia::Vector3 Vector3; + typedef typename PseudoInertia::Matrix3 Matrix3; + typedef typename PseudoInertia::Vector10 Vector10; + typedef typename PseudoInertia::Matrix4 Matrix4; + typedef Eigen::Matrix VectorXs; + + public: + template + void visit(PyClass & cl) const + { + cl.def(bp::init( + (bp::arg("self"), bp::arg("mass"), bp::arg("h"), bp::arg("sigma")), + "Initialize from mass, vector part of the pseudo inertia and matrix part of the " + "pseudo inertia.")) + .def(bp::init( + (bp::arg("self"), bp::arg("clone")), "Copy constructor")) + .add_property( + "mass", &PseudoInertiaPythonVisitor::getMass, &PseudoInertiaPythonVisitor::setMass, + "Mass of the Pseudo Inertia.") + .add_property( + "h", &PseudoInertiaPythonVisitor::getH, &PseudoInertiaPythonVisitor::setH, + "Vector part of the Pseudo Inertia.") + .add_property( + "sigma", &PseudoInertiaPythonVisitor::getSigma, &PseudoInertiaPythonVisitor::setSigma, + "Matrix part of the Pseudo Inertia.") + + .def( + "toMatrix", &PseudoInertia::toMatrix, bp::arg("self"), + "Returns the pseudo inertia as a 4x4 matrix.") + .def( + "toDynamicParameters", &PseudoInertiaPythonVisitor::toDynamicParameters_proxy, + bp::arg("self"), "Returns the dynamic parameters representation.") + .def( + "toInertia", &PseudoInertia::toInertia, bp::arg("self"), + "Returns the inertia representation.") + .def( + "FromDynamicParameters", &PseudoInertiaPythonVisitor::fromDynamicParameters_proxy, + bp::args("dynamic_parameters"), + "Builds a pseudo inertia matrix from a vector of dynamic parameters." + "\nThe parameters are given as dynamic_parameters = [m, h_x, h_y, h_z, I_{xx}, " + "I_{xy}, I_{yy}, I_{xz}, I_{yz}, I_{zz}]^T.") + .staticmethod("FromDynamicParameters") + + .def( + "FromMatrix", &PseudoInertia::FromMatrix, bp::args("pseudo_inertia_matrix"), + "Returns the Pseudo Inertia from a 4x4 matrix.") + .staticmethod("FromMatrix") + + .def( + "FromInertia", &PseudoInertia::FromInertia, bp::args("inertia"), + "Returns the Pseudo Inertia from an Inertia object.") + .staticmethod("FromInertia") + + .def("__array__", &PseudoInertia::toMatrix) + .def( + "__array__", &__array__, + (bp::arg("self"), bp::arg("dtype") = bp::object(), bp::arg("copy") = bp::object())) +#ifndef PINOCCHIO_PYTHON_NO_SERIALIZATION + .def_pickle(Pickle()) +#endif + ; + } + + static Scalar getMass(const PseudoInertia & self) + { + return self.mass; + } + static void setMass(PseudoInertia & self, Scalar mass) + { + self.mass = mass; + } + + static Vector3 getH(const PseudoInertia & self) + { + return self.h; + } + static void setH(PseudoInertia & self, const Vector3 & h) + { + self.h = h; + } + + static Matrix3 getSigma(const PseudoInertia & self) + { + return self.sigma; + } + static void setSigma(PseudoInertia & self, const Matrix3 & sigma) + { + self.sigma = sigma; + } + + static VectorXs toDynamicParameters_proxy(const PseudoInertia & self) + { + return self.toDynamicParameters(); + } + + static PseudoInertia fromDynamicParameters_proxy(const VectorXs & params) + { + if (params.rows() != 10 || params.cols() != 1) + { + std::ostringstream shape; + shape << "(" << params.rows() << ", " << params.cols() << ")"; + throw std::invalid_argument( + "Wrong size: params" + shape.str() + " but should have the following shape (10, 1)"); + } + return PseudoInertia::FromDynamicParameters(params); + } + + static void expose() + { +#if PY_MAJOR_VERSION == 3 && PY_MINOR_VERSION == 6 && EIGENPY_VERSION_AT_LEAST(2, 9, 0) + typedef PINOCCHIO_SHARED_PTR_HOLDER_TYPE(PseudoInertia) HolderType; +#else + typedef ::boost::python::detail::not_specified HolderType; +#endif + bp::class_( + "PseudoInertia", + "This class represents a pseudo inertia matrix and it is defined by its mass, vector " + "part, and 3x3 matrix part.\n\n" + "Supported operations ...", + bp::no_init) + .def(PseudoInertiaPythonVisitor()) + .def(CopyableVisitor()) + .def(PrintableVisitor()) + .def(CastVisitor()) + .def(ExposeConstructorByCastVisitor()); + } + + private: + static Matrix4 __array__(const PseudoInertia & self, bp::object, bp::object) + { + return self.toMatrix(); + } + + struct Pickle : bp::pickle_suite + { + static boost::python::tuple getinitargs(const PseudoInertia & pi) + { + return bp::make_tuple(pi.mass, pi.h, pi.sigma); + } + + static bool getstate_manages_dict() + { + return true; + } + }; + + }; // struct PseudoInertiaPythonVisitor + + template + struct LogCholeskyParametersPythonVisitor + : public boost::python::def_visitor> + { + enum + { + Options = LogCholeskyParameters::Options + }; + typedef typename LogCholeskyParameters::Scalar Scalar; + typedef typename LogCholeskyParameters::Vector10 Vector10; + typedef typename LogCholeskyParameters::Matrix10 Matrix10; + typedef Eigen::Matrix VectorXs; + typedef Eigen::Matrix MatrixXs; + + public: + template + void visit(PyClass & cl) const + { + cl.def( + "__init__", + bp::make_constructor( + &LogCholeskyParametersPythonVisitor::makeFromParameters, bp::default_call_policies(), + bp::args("log_cholesky_parameters")), + "Initialize from log cholesky parameters.") + .def(bp::init( + (bp::arg("self"), bp::arg("clone")), "Copy constructor")) + + .add_property( + "parameters", &LogCholeskyParametersPythonVisitor::getParameters, + &LogCholeskyParametersPythonVisitor::setParameters, "Log Cholesky parameters.") + + .def( + "toDynamicParameters", &LogCholeskyParametersPythonVisitor::toDynamicParameters_proxy, + bp::arg("self"), "Returns the dynamic parameters representation.") + .def( + "toPseudoInertia", &LogCholeskyParameters::toPseudoInertia, bp::arg("self"), + "Returns the Pseudo Inertia representation.") + .def( + "toInertia", &LogCholeskyParameters::toInertia, bp::arg("self"), + "Returns the Inertia representation.") + .def( + "calculateJacobian", &LogCholeskyParametersPythonVisitor::calculateJacobian_proxy, + bp::arg("self"), "Calculates the Jacobian of the log Cholesky parameters.") + + .def("__array__", &LogCholeskyParametersPythonVisitor::getParameters) + .def( + "__array__", &__array__, + (bp::arg("self"), bp::arg("dtype") = bp::object(), bp::arg("copy") = bp::object())) +#ifndef PINOCCHIO_PYTHON_NO_SERIALIZATION + .def_pickle(Pickle()) +#endif + ; + } + + static LogCholeskyParameters * makeFromParameters(const VectorXs & params) + { + if (params.rows() != 10 || params.cols() != 1) + { + std::ostringstream shape; + shape << "(" << params.rows() << ", " << params.cols() << ")"; + throw std::invalid_argument( + "Wrong size: params" + shape.str() + " but should have the following shape (10, 1)"); + } + return new LogCholeskyParameters(params); + } + + static VectorXs getParameters(const LogCholeskyParameters & self) + { + return self.parameters; + } + static void setParameters(LogCholeskyParameters & self, const Vector10 & parameters) + { + self.parameters = parameters; + } + + static VectorXs toDynamicParameters_proxy(const LogCholeskyParameters & self) + { + return self.toDynamicParameters(); + } + + static MatrixXs calculateJacobian_proxy(const LogCholeskyParameters & self) + { + return self.calculateJacobian(); + } + + static void expose() + { +#if PY_MAJOR_VERSION == 3 && PY_MINOR_VERSION == 6 && EIGENPY_VERSION_AT_LEAST(2, 9, 0) + typedef PINOCCHIO_SHARED_PTR_HOLDER_TYPE(LogCholeskyParameters) HolderType; +#else + typedef ::boost::python::detail::not_specified HolderType; +#endif + bp::class_( + "LogCholeskyParameters", + "This class represents log Cholesky parameters.\n\n" + "Supported operations ...", + bp::no_init) + .def(LogCholeskyParametersPythonVisitor()) + .def(CopyableVisitor()) + .def(PrintableVisitor()) + .def(CastVisitor()) + .def(ExposeConstructorByCastVisitor< + LogCholeskyParameters, ::pinocchio::LogCholeskyParameters>()); + } + + private: + static VectorXs __array__(const LogCholeskyParameters & self, bp::object, bp::object) + { + return self.parameters; + } + + struct Pickle : bp::pickle_suite + { + static boost::python::tuple getinitargs(const LogCholeskyParameters & lcp) + { + return bp::make_tuple(lcp.parameters); + } + + static bool getstate_manages_dict() + { + return true; + } + }; + + }; // struct LogCholeskyParametersPythonVisitor + } // namespace python } // namespace pinocchio diff --git a/include/pinocchio/spatial/fwd.hpp b/include/pinocchio/spatial/fwd.hpp index ec0c42e919..25b5ab005b 100644 --- a/include/pinocchio/spatial/fwd.hpp +++ b/include/pinocchio/spatial/fwd.hpp @@ -58,6 +58,10 @@ namespace pinocchio struct InertiaTpl; template class Symmetric3Tpl; + template + struct PseudoInertiaTpl; + template + struct LogCholeskyParametersTpl; typedef SE3Tpl SE3; typedef MotionTpl Motion; @@ -65,6 +69,8 @@ namespace pinocchio typedef InertiaTpl Inertia; typedef Symmetric3Tpl Symmetric3; typedef MotionZeroTpl MotionZero; + typedef PseudoInertiaTpl PseudoInertia; + typedef LogCholeskyParametersTpl LogCholeskyParameters; /** * @} diff --git a/include/pinocchio/spatial/inertia.hpp b/include/pinocchio/spatial/inertia.hpp index 61fccc6065..0e89bf1898 100644 --- a/include/pinocchio/spatial/inertia.hpp +++ b/include/pinocchio/spatial/inertia.hpp @@ -14,7 +14,6 @@ namespace pinocchio { - template struct InertiaBase : NumericalBase { @@ -228,9 +227,9 @@ namespace pinocchio X.disp(os); return os; } + }; - }; // class InertiaBase - + // class InertiaBase template struct traits> { @@ -241,6 +240,7 @@ namespace pinocchio typedef Eigen::Matrix Matrix3; typedef Eigen::Matrix Matrix4; typedef Eigen::Matrix Matrix6; + typedef Eigen::Matrix Matrix10; typedef Matrix6 ActionMatrix_t; typedef Vector3 Angular_t; typedef Vector3 Linear_t; @@ -251,6 +251,8 @@ namespace pinocchio typedef ForceTpl Force; typedef MotionTpl Motion; typedef Symmetric3Tpl Symmetric3; + typedef PseudoInertiaTpl PseudoInertia; + typedef LogCholeskyParametersTpl LogCholeskyParameters; enum { LINEAR = 0, @@ -270,7 +272,10 @@ namespace pinocchio }; typedef typename Symmetric3::AlphaSkewSquare AlphaSkewSquare; - typedef typename Eigen::Matrix Vector10; + typedef Eigen::Matrix Vector10; + typedef Eigen::Matrix Matrix10; + typedef PseudoInertiaTpl PseudoInertia; + typedef LogCholeskyParametersTpl LogCholeskyParameters; // Constructors InertiaTpl() @@ -565,6 +570,43 @@ namespace pinocchio mass, lever, Symmetric3(params.template segment<6>(4)) + AlphaSkewSquare(mass, lever)); } + /** + + * @brief Create an InertiaTpl object from a PseudoInertia object. + * + * @param pseudo_inertia A PseudoInertia object. + * @return An InertiaTpl object. + */ + + static InertiaTpl FromPseudoInertia(const PseudoInertia & pseudo_inertia) + { + return pseudo_inertia.toInertia(); + } + + /** + * @brief Convert the InertiaTpl object to a 4x4 pseudo inertia matrix. + * + * @return A 4x4 pseudo inertia matrix. + */ + PseudoInertia toPseudoInertia() const + { + PseudoInertia pseudo_inertia = PseudoInertia::FromInertia(*this); + return pseudo_inertia; + } + + /** + * @brief Create an InertiaTpl object from log Cholesky parameters. + * + * @param log_cholesky A log cholesky parameters object + * + * @return An InertiaTpl object. + */ + static InertiaTpl FromLogCholeskyParameters(const LogCholeskyParameters & log_cholesky) + { + Vector10 dynamic_params = log_cholesky.toDynamicParameters(); + return FromDynamicParameters(dynamic_params); + } + // Arithmetic operators InertiaTpl & __equl__(const InertiaTpl & clone) { @@ -889,6 +931,377 @@ namespace pinocchio }; // class InertiaTpl + /** + * @brief A structure representing a pseudo inertia matrix. + * + * References: + * - Wensing, Patrick M., Sangbae Kim, and Jean-Jacques E. Slotine. "Linear matrix inequalities + * for physically consistent inertial parameter identification: A statistical perspective on the + * mass distribution." IEEE Robotics and Automation Letters 3.1 (2017): 60-67. + */ + template + struct PseudoInertiaTpl + { + typedef _Scalar Scalar; + enum + { + Options = _Options, + }; + typedef Eigen::Matrix Matrix4; + typedef Eigen::Matrix Vector3; + typedef Eigen::Matrix Matrix3; + typedef Eigen::Matrix Vector10; + typedef LogCholeskyParametersTpl LogCholeskyParameters; + + Scalar mass; ///< Mass of the pseudo inertia + Vector3 h; ///< Vector part of the pseudo inertia + Matrix3 sigma; ///< 3x3 matrix part of the pseudo inertia + + PseudoInertiaTpl(Scalar mass, const Vector3 & h, const Matrix3 & sigma) + : mass(mass) + , h(h) + , sigma(sigma) + { + } + + /** + * @brief Converts the PseudoInertiaTpl object to a 4x4 matrix. + * @return A 4x4 pseudo inertia matrix. + */ + Matrix4 toMatrix() const + { + Matrix4 pseudo_inertia = Matrix4::Zero(); + pseudo_inertia.template block<3, 3>(0, 0) = sigma; + pseudo_inertia.template block<3, 1>(0, 3) = h; + pseudo_inertia.template block<1, 3>(3, 0) = h.transpose(); + pseudo_inertia(3, 3) = mass; + return pseudo_inertia; + } + + /** + * @brief Constructs a PseudoInertiaTpl object from a 4x4 pseudo inertia matrix. + * @param pseudo_inertia A 4x4 pseudo inertia matrix. + * @return A PseudoInertiaTpl object. + */ + static PseudoInertiaTpl FromMatrix(const Matrix4 & pseudo_inertia) + { + Scalar mass = pseudo_inertia(3, 3); + Vector3 h = pseudo_inertia.template block<3, 1>(0, 3); + Matrix3 sigma = pseudo_inertia.template block<3, 3>(0, 0); + return PseudoInertiaTpl(mass, h, sigma); + } + + /** + * @brief Constructs a PseudoInertiaTpl object from dynamic parameters. + * @param dynamic_params A 10-dimensional vector of dynamic parameters. + * @return A PseudoInertiaTpl object. + */ + template + static PseudoInertiaTpl + FromDynamicParameters(const Eigen::MatrixBase & dynamic_params) + { + PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector10Like, dynamic_params, 10, 1); + Scalar mass = dynamic_params[0]; + Vector3 h = dynamic_params.template segment<3>(1); + Matrix3 I_bar; + // clang-format off + I_bar << dynamic_params[4], dynamic_params[5], dynamic_params[7], + dynamic_params[5], dynamic_params[6], dynamic_params[8], + dynamic_params[7], dynamic_params[8], dynamic_params[9]; + // clang-format on + + Matrix3 sigma = 0.5 * I_bar.trace() * Matrix3::Identity() - I_bar; + return PseudoInertiaTpl(mass, h, sigma); + } + + /** + * @brief Converts the PseudoInertiaTpl object to dynamic parameters. + * @return A 10-dimensional vector of dynamic parameters. + */ + Vector10 toDynamicParameters() const + { + Matrix3 I_bar = sigma.trace() * Matrix3::Identity() - sigma; + + Vector10 dynamic_params; + dynamic_params[0] = mass; + dynamic_params.template segment<3>(1) = h; + dynamic_params[4] = I_bar(0, 0); + dynamic_params[5] = I_bar(0, 1); + dynamic_params[6] = I_bar(1, 1); + dynamic_params[7] = I_bar(0, 2); + dynamic_params[8] = I_bar(1, 2); + dynamic_params[9] = I_bar(2, 2); + + return dynamic_params; + } + + /** + * @brief Constructs a PseudoInertiaTpl object from an InertiaTpl object. + * @param inertia An InertiaTpl object. + * @return A PseudoInertiaTpl object. + */ + static PseudoInertiaTpl FromInertia(const InertiaTpl & inertia) + { + Vector10 dynamic_params = inertia.toDynamicParameters(); + return FromDynamicParameters(dynamic_params); + } + + /** + * @brief Converts the PseudoInertiaTpl object to an InertiaTpl object. + * @return An InertiaTpl object. + */ + InertiaTpl toInertia() const + { + Vector10 dynamic_params = toDynamicParameters(); + return InertiaTpl::FromDynamicParameters(dynamic_params); + } + + /** + * @brief Constructs a PseudoInertiaTpl object from log Cholesky parameters. + * @param log_cholesky A 10-dimensional vector of log Cholesky parameters. + * @return A PseudoInertiaTpl object. + */ + static PseudoInertiaTpl FromLogCholeskyParameters(const LogCholeskyParameters & log_cholesky) + { + Vector10 dynamic_params = log_cholesky.toDynamicParameters(); + return FromDynamicParameters(dynamic_params); + } + + /// \returns An expression of *this with the Scalar type casted to NewScalar. + template + PseudoInertiaTpl cast() const + { + return PseudoInertiaTpl( + pinocchio::cast(mass), h.template cast(), + sigma.template cast()); + } + + void disp_impl(std::ostream & os) const + { + os << " m = " << mass << "\n" + << " h = " << h.transpose() << "\n" + << " sigma = \n" + << sigma << ""; + } + + friend std::ostream & operator<<(std::ostream & os, const PseudoInertiaTpl & pi) + { + pi.disp_impl(os); + return os; + } + }; + + /** + * @brief A structure representing log Cholesky parameters. + * + * References: + * - Rucker, Caleb, and Patrick M. Wensing. "Smooth parameterization of rigid-body inertia." + * IEEE Robotics and Automation Letters 7.2 (2022): 2771-2778. + */ + template + struct LogCholeskyParametersTpl + { + typedef _Scalar Scalar; + enum + { + Options = _Options, + }; + typedef Eigen::Matrix Vector10; + typedef Eigen::Matrix Matrix10; + typedef PseudoInertiaTpl PseudoInertia; + + Vector10 parameters; ///< 10-dimensional vector of log Cholesky parameters + /** + * @brief Constructor for LogCholeskyParametersTpl. + * @param log_cholesky A 10-dimensional vector of log Cholesky parameters. + */ + explicit LogCholeskyParametersTpl(const Vector10 & log_cholesky) + : parameters(log_cholesky) + { + } + /** + * @brief Converts the LogCholeskyParametersTpl object to dynamic parameters. + * @return A 10-dimensional vector of dynamic parameters. + */ + Vector10 toDynamicParameters() const + { + using math::exp; + using math::pow; + + // clang-format off + Vector10 dynamic_params; + + const Scalar alpha = parameters[0]; + const Scalar d1 = parameters[1]; + const Scalar d2 = parameters[2]; + const Scalar d3 = parameters[3]; + const Scalar s12 = parameters[4]; + const Scalar s23 = parameters[5]; + const Scalar s13 = parameters[6]; + const Scalar t1 = parameters[7]; + const Scalar t2 = parameters[8]; + const Scalar t3 = parameters[9]; + + const Scalar exp_d1 = exp(d1); + const Scalar exp_d2 = exp(d2); + const Scalar exp_d3 = exp(d3); + + dynamic_params[0] = 1; + dynamic_params[1] = t1; + dynamic_params[2] = t2; + dynamic_params[3] = t3; + dynamic_params[4] = pow(s23, 2) + pow(t2, 2) + pow(t3, 2) + pow(exp_d2, 2) + pow(exp_d3, 2); + dynamic_params[5] = -s12 * exp_d2 - s13 * s23 - t1 * t2; + dynamic_params[6] = pow(s12, 2) + pow(s13, 2) + pow(t1, 2) + pow(t3, 2) + pow(exp_d1, 2) + pow(exp_d3, 2); + dynamic_params[7] = -s13 * exp_d3 - t1 * t3; + dynamic_params[8] = -s23 * exp_d3 - t2 * t3; + dynamic_params[9] = pow(s12, 2) + pow(s13, 2) + pow(s23, 2) + pow(t1, 2) + pow(t2, 2) + pow(exp_d1, 2) + pow(exp_d2, 2); + + const Scalar exp_2_alpha = exp(2 * alpha); + dynamic_params *= exp_2_alpha; + // clang-format on + + return dynamic_params; + } + + /** + * @brief Converts the LogCholeskyParametersTpl object to a PseudoInertiaTpl object. + * @return A PseudoInertiaTpl object. + */ + PseudoInertia toPseudoInertia() const + { + Vector10 dynamic_params = toDynamicParameters(); + return PseudoInertia::FromDynamicParameters(dynamic_params); + } + + /** + * @brief Converts the LogCholeskyParametersTpl object to an InertiaTpl object. + * @return An InertiaTpl object. + */ + InertiaTpl toInertia() const + { + Vector10 dynamic_params = toDynamicParameters(); + return InertiaTpl::FromDynamicParameters(dynamic_params); + } + + /** + * @brief Calculates the Jacobian of the log Cholesky parameters. + * @return A 10x10 matrix representing the Jacobian. + */ + Matrix10 calculateJacobian() const + { + using math::exp; + using math::pow; + + Matrix10 jacobian = Matrix10::Zero(); + + const Scalar alpha = parameters[0]; + const Scalar d1 = parameters[1]; + const Scalar d2 = parameters[2]; + const Scalar d3 = parameters[3]; + const Scalar s12 = parameters[4]; + const Scalar s23 = parameters[5]; + const Scalar s13 = parameters[6]; + const Scalar t1 = parameters[7]; + const Scalar t2 = parameters[8]; + const Scalar t3 = parameters[9]; + + const Scalar exp_2alpha = exp(2 * alpha); + const Scalar exp_2d1 = exp(2 * d1); + const Scalar exp_2d2 = exp(2 * d2); + const Scalar exp_2d3 = exp(2 * d3); + const Scalar exp_d1 = exp(d1); + const Scalar exp_d2 = exp(d2); + const Scalar exp_d3 = exp(d3); + + // clang-format off + jacobian(0, 0) = 2 * exp_2alpha; + + jacobian(1, 0) = 2 * t1 * exp_2alpha; + jacobian(1, 7) = exp_2alpha; + + jacobian(2, 0) = 2 * t2 * exp_2alpha; + jacobian(2, 8) = exp_2alpha; + + jacobian(3, 0) = 2 * t3 * exp_2alpha; + jacobian(3, 9) = exp_2alpha; + + jacobian(4, 0) = 2 * (pow(s23, 2) + pow(t2, 2) + pow(t3, 2) + exp_2d2 + exp_2d3) * exp_2alpha; + jacobian(4, 2) = 2 * exp_2alpha * exp_2d2; + jacobian(4, 3) = 2 * exp_2alpha * exp_2d3; + jacobian(4, 5) = 2 * s23 * exp_2alpha; + jacobian(4, 8) = 2 * t2 * exp_2alpha; + jacobian(4, 9) = 2 * t3 * exp_2alpha; + + jacobian(5, 0) = -2 * (s12 * exp_d2 + s13 * s23 + t1 * t2) * exp_2alpha; + jacobian(5, 2) = -s12 * exp_2alpha * exp_d2; + jacobian(5, 4) = -exp_2alpha * exp_d2; + jacobian(5, 5) = -s13 * exp_2alpha; + jacobian(5, 6) = -s23 * exp_2alpha; + jacobian(5, 7) = -t2 * exp_2alpha; + jacobian(5, 8) = -t1 * exp_2alpha; + + jacobian(6, 0) = 2 * (pow(s12, 2) + pow(s13, 2) + pow(t1, 2) + pow(t3, 2) + exp_2d1 + exp_2d3) * exp_2alpha; + jacobian(6, 1) = 2 * exp_2alpha * exp_2d1; + jacobian(6, 3) = 2 * exp_2alpha * exp_2d3; + jacobian(6, 4) = 2 * s12 * exp_2alpha; + jacobian(6, 6) = 2 * s13 * exp_2alpha; + jacobian(6, 7) = 2 * t1 * exp_2alpha; + jacobian(6, 9) = 2 * t3 * exp_2alpha; + + jacobian(7, 0) = -2 * (s13 * exp_d3 + t1 * t3) * exp_2alpha; + jacobian(7, 3) = -s13 * exp_2alpha * exp_d3; + jacobian(7, 6) = -exp_2alpha * exp_d3; + jacobian(7, 7) = -t3 * exp_2alpha; + jacobian(7, 9) = -t1 * exp_2alpha; + + jacobian(8, 0) = -2 * (s23 * exp_d3 + t2 * t3) * exp_2alpha; + jacobian(8, 3) = -s23 * exp_2alpha * exp_d3; + jacobian(8, 5) = -exp_2alpha * exp_d3; + jacobian(8, 8) = -t3 * exp_2alpha; + jacobian(8, 9) = -t2 * exp_2alpha; + + jacobian(9, 0) = 2 * (pow(s12, 2) + pow(s13, 2) + pow(s23, 2) + pow(t1, 2) + pow(t2, 2) + exp_2d1 + exp_2d2) * exp_2alpha; + jacobian(9, 1) = 2 * exp_2alpha * exp_2d1; + jacobian(9, 2) = 2 * exp_2alpha * exp_2d2; + jacobian(9, 4) = 2 * s12 * exp_2alpha; + jacobian(9, 5) = 2 * s23 * exp_2alpha; + jacobian(9, 6) = 2 * s13 * exp_2alpha; + jacobian(9, 7) = 2 * t1 * exp_2alpha; + jacobian(9, 8) = 2 * t2 * exp_2alpha; + // clang-format on + + return jacobian; + } + + /// \returns An expression of *this with the Scalar type casted to NewScalar. + template + LogCholeskyParametersTpl cast() const + { + return LogCholeskyParametersTpl(parameters.template cast()); + } + + void disp_impl(std::ostream & os) const + { + os << " alpha: " << parameters[0] << "\n" + << " d1: " << parameters[1] << "\n" + << " d2: " << parameters[2] << "\n" + << " d3: " << parameters[3] << "\n" + << " s12: " << parameters[4] << "\n" + << " s23: " << parameters[5] << "\n" + << " s13: " << parameters[6] << "\n" + << " t1: " << parameters[7] << "\n" + << " t2: " << parameters[8] << "\n" + << " t3: " << parameters[9] << ""; + } + + friend std::ostream & operator<<(std::ostream & os, const LogCholeskyParametersTpl & lcp) + { + lcp.disp_impl(os); + return os; + } + }; + } // namespace pinocchio #endif // ifndef __pinocchio_spatial_inertia_hpp__ diff --git a/unittest/python/bindings_inertia.py b/unittest/python/bindings_inertia.py index 3fb9148767..416ab0820d 100644 --- a/unittest/python/bindings_inertia.py +++ b/unittest/python/bindings_inertia.py @@ -113,6 +113,94 @@ def test_dynamic_parameters(self): I2 = pin.Inertia.FromDynamicParameters(v) self.assertApprox(I2, In) + # Test FromDynamicParameters raise an exception if the wrong + # vector size is provided + with self.assertRaises(ValueError): + pin.Inertia.FromDynamicParameters(np.array([])) + + def test_pseudo_inertia(self): + In = pin.Inertia.Random() + + pseudo = In.toPseudoInertia() + + # test accessing mass, h, sigma + self.assertApprox(pseudo.mass, In.mass) + self.assertApprox(pseudo.h, In.mass * In.lever) + self.assertEqual(pseudo.sigma.shape, (3, 3)) + + # test toMatrix + _ = pseudo.toMatrix() + + # test toDynamicParameters + params = pseudo.toDynamicParameters() + + # test fromDynamicParameters + pseudo2 = pin.PseudoInertia.FromDynamicParameters(params) + self.assertApprox(pseudo.mass, pseudo2.mass) + self.assertApprox(pseudo.h, pseudo2.h) + self.assertApprox(pseudo.sigma, pseudo2.sigma) + + # Test FromDynamicParameters raise an exception if the wrong + # vector size is provided + with self.assertRaises(ValueError): + pin.PseudoInertia.FromDynamicParameters(np.array([])) + + # test fromMatrix + pseudo3 = pin.PseudoInertia.FromMatrix(pseudo.toMatrix()) + self.assertApprox(pseudo.mass, pseudo3.mass) + self.assertApprox(pseudo.h, pseudo3.h) + self.assertApprox(pseudo.sigma, pseudo3.sigma) + + # test fromInertia + pseudo4 = pin.PseudoInertia.FromInertia(In) + self.assertApprox(pseudo.mass, pseudo4.mass) + self.assertApprox(pseudo.h, pseudo4.h) + self.assertApprox(pseudo.sigma, pseudo4.sigma) + + # test toInertia + self.assertApprox(pseudo4.toInertia(), In) + + # test from PseudoInertia + pin.PseudoInertia(pseudo) + + # test array + pseudo_array = np.array(pseudo) + self.assertApprox(pseudo_array, pseudo.toMatrix()) + + def test_log_cholesky(self): + log_cholesky = pin.LogCholeskyParameters(np.random.randn(10)) + In = pin.Inertia.FromLogCholeskyParameters(log_cholesky) + + # Test constructor with wrong vector size + with self.assertRaises(ValueError): + log_cholesky = pin.LogCholeskyParameters(np.array([])) + + # test accessing parameters + self.assertEqual(log_cholesky.parameters.shape, (10,)) + + # test toDynamicParameters + params = log_cholesky.toDynamicParameters() + params2 = In.toDynamicParameters() + self.assertApprox(params, params2) + + # test toPseudoInertia + pseudo = log_cholesky.toPseudoInertia() + pseudo2 = In.toPseudoInertia() + self.assertApprox(pseudo.mass, pseudo2.mass) + self.assertApprox(pseudo.h, pseudo2.h) + self.assertApprox(pseudo.sigma, pseudo2.sigma) + + # test toInertia + In2 = log_cholesky.toInertia() + self.assertApprox(In, In2) + + # test calculateJacobian + log_cholesky.calculateJacobian() + + # test array + log_cholesky_array = np.array(log_cholesky) + self.assertApprox(log_cholesky_array, log_cholesky.parameters) + def test_array(self): In = pin.Inertia.Random() I_array = np.array(In) diff --git a/unittest/spatial.cpp b/unittest/spatial.cpp index 883f2b2922..65cb9b9279 100644 --- a/unittest/spatial.cpp +++ b/unittest/spatial.cpp @@ -737,6 +737,97 @@ BOOST_AUTO_TEST_CASE(test_Inertia) // Test disp std::cout << aI << std::endl; + + // Test Inertia parametrizations + { + typedef LogCholeskyParametersTpl LogCholeskyParameters; + typedef PseudoInertiaTpl PseudoInertia; + + Inertia::Vector10 eta; + eta.setRandom(); + LogCholeskyParameters log_cholesky = LogCholeskyParameters(eta); + + // Convert logcholesky parametrization to pseudo-inertia + PseudoInertia pseudo = log_cholesky.toPseudoInertia(); + Eigen::Matrix4d pseudo_matrix = pseudo.toMatrix(); + // Convert logcholesky to inertia tpl + Inertia I_from_log_cholesky = Inertia::FromLogCholeskyParameters(log_cholesky); + + // Check if conversion from inertia tpl to pseudo inertia gives same result as from logcholesky + // parametrization to pseudo-inertia + Eigen::Matrix4d pseudo_from_inertia = I_from_log_cholesky.toPseudoInertia().toMatrix(); + BOOST_CHECK(pseudo_matrix.isApprox(pseudo_from_inertia, 1e-10)); + + // // Check if log-cholesky parametrization to pseudo-inertia gives same result as their + // calculations + double alpha = log_cholesky.parameters[0]; + double d1 = log_cholesky.parameters[1]; + double d2 = log_cholesky.parameters[2]; + double d3 = log_cholesky.parameters[3]; + double s12 = log_cholesky.parameters[4]; + double s23 = log_cholesky.parameters[5]; + double s13 = log_cholesky.parameters[6]; + double t1 = log_cholesky.parameters[7]; + double t2 = log_cholesky.parameters[8]; + double t3 = log_cholesky.parameters[9]; + + double exp_alpha = std::exp(alpha); + double exp_d1 = std::exp(d1); + double exp_d2 = std::exp(d2); + double exp_d3 = std::exp(d3); + + Eigen::Matrix4d U; + // clang-format off + U << exp_d1, s12, s13, t1, + 0, exp_d2, s23, t2, + 0, 0, exp_d3, t3, + 0, 0, 0, 1; + // clang-format on + U *= exp_alpha; + + Eigen::Matrix4d pseudo_chol = U * U.transpose(); + BOOST_CHECK(pseudo_matrix.isApprox(pseudo_chol, 1e-10)); + + // Additional checks: Convert back from pseudo-inertia to inertia and validate + Inertia I_back = pseudo.toInertia(); + BOOST_CHECK_CLOSE(I_back.mass(), I_from_log_cholesky.mass(), 1e-12); + BOOST_CHECK(I_back.lever().isApprox(I_from_log_cholesky.lever(), 1e-12)); + BOOST_CHECK(I_back.inertia().isApprox(I_from_log_cholesky.inertia(), 1e-12)); + + // Convert Inertia to dynamic parameters + Inertia::Vector10 dynamic_params_inertia = I_from_log_cholesky.toDynamicParameters(); + + // Convert log Cholesky to dynamic parameters + Inertia::Vector10 dynamic_params_log_cholesky = log_cholesky.toDynamicParameters(); + + // Compare dynamic parameters from Inertia and log Cholesky + BOOST_CHECK(dynamic_params_inertia.isApprox(dynamic_params_log_cholesky, 1e-10)); + + // Convert Pseudo Inertia to dynamic parameters + PseudoInertia pseudo_inertia = PseudoInertia::FromMatrix(pseudo_matrix); + Inertia::Vector10 dynamic_params_pseudo_inertia = pseudo_inertia.toDynamicParameters(); + + // Compare dynamic parameters from Inertia and Pseudo Inertia + BOOST_CHECK(dynamic_params_inertia.isApprox(dynamic_params_pseudo_inertia, 1e-10)); + + // Compare dynamic parameters from log Cholesky and Pseudo Inertia + BOOST_CHECK(dynamic_params_log_cholesky.isApprox(dynamic_params_pseudo_inertia, 1e-10)); + + // Calculate Jacobian of logcholesky parametrization + Eigen::Matrix jacobian = log_cholesky.calculateJacobian(); + + // Check if determinant is non-zero + BOOST_CHECK(std::abs(jacobian.determinant()) > 1e-10); + + // Check physical consistency by positive definiteness of pseudo inertia + Eigen::SelfAdjointEigenSolver eigensolver(pseudo_matrix); + BOOST_CHECK((eigensolver.eigenvalues().array() > 0).all()); + + // Test disp + std::cout << I_from_log_cholesky << std::endl; + std::cout << log_cholesky << std::endl; + std::cout << pseudo_inertia << std::endl; + } } BOOST_AUTO_TEST_CASE(cast_inertia)