Merge pull request #1 from simeon-ned/feat/log_cholesky_parameters

Feat/log cholesky parameters

Author: simeon-ned

cmake

@@ -39,8 +39,12 @@ namespace pinocchio
       typedef typename Inertia::Matrix3 Matrix3;
       typedef typename Inertia::Vector6 Vector6;
       typedef typename Inertia::Matrix6 Matrix6;
+      typedef typename Inertia::Matrix4 Matrix4;
+      typedef typename Inertia::Matrix10 Matrix10;
+      typedef typename Inertia::Vector10 Vector10;
 
       typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options> VectorXs;
+      typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options> MatrixXs;
       typedef MotionTpl<Scalar, Options> Motion;
       typedef ForceTpl<Scalar, Options> Force;
 
@@ -57,7 +61,6 @@ namespace pinocchio
               &InertiaPythonVisitor::makeFromMCI, bp::default_call_policies(),
               bp::args("mass", "lever", "inertia")),
             "Initialize from mass, lever and 3d inertia.")
-
           .def(bp::init<>(bp::arg("self"), "Default constructor."))
           .def(bp::init<const Inertia &>((bp::arg("self"), bp::arg("clone")), "Copy constructor"))
 
@@ -124,6 +127,17 @@ namespace pinocchio
             (Matrix6(Inertia::*)(const MotionDense<Motion> &) const)
               & Inertia::template variation<Motion>,
             bp::args("self", "v"), "Returns the time derivative of the inertia.")
+          .def(
+            "LogcholToDynamicParameters", &InertiaPythonVisitor::LogcholToDynamicParameters_proxy,
+            bp::args("log_cholesky"),
+            "Converts logarithmic Cholesky parameters directly to theta parameters.")
+          .staticmethod("LogcholToDynamicParameters")
+          .def(
+            "calculateLogCholeskyJacobian",
+            &InertiaPythonVisitor::calculateLogCholeskyJacobian_proxy, bp::args("log_cholesky"),
+            "Calculates the Jacobian of the dynamic parameters with respect to the log-Cholesky "
+            "parameters.")
+          .staticmethod("calculateLogCholeskyJacobian")
 
 #ifndef PINOCCHIO_PYTHON_SKIP_COMPARISON_OPERATIONS
           .def(bp::self == bp::self)
@@ -165,7 +179,32 @@ namespace pinocchio
             "I_{xy}, I_{yy}, 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.")
           .staticmethod("FromDynamicParameters")
-
+          .def(
+            "toPseudoInertia", &InertiaPythonVisitor::toPseudoInertia_proxy, bp::arg("self"),
+            "Converts the inertia to a pseudo inertia matrix."
+            "\nThe returned 4x4 pseudo inertia matrix has the form:"
+            "\n[[ -0.5*I_xx + 0.5*I_yy + 0.5*I_zz, -I_xy, -I_xz, mr_x],"
+            "\n [ -I_xy, 0.5*I_xx - 0.5*I_yy + 0.5*I_zz, -I_yz, mr_y],"
+            "\n [ -I_xz, -I_yz, 0.5*I_xx + 0.5*I_yy - 0.5*I_zz, mr_z],"
+            "\n [ mr_x, mr_y, mr_z, m ]].")
+          .def(
+            "FromPseudoInertia", &Inertia::FromPseudoInertia, bp::args("pseudo_inertia"),
+            "Builds an inertia matrix from a 4x4 pseudo inertia matrix."
+            "\nThe parameters are given as"
+            "\npseudo_inertia = [[ -0.5*I_xx + 0.5*I_yy + 0.5*I_zz, -I_xy, -I_xz, mr_x],"
+            "\n [ -I_xy, 0.5*I_xx - 0.5*I_yy + 0.5*I_zz, -I_yz, mr_y],"
+            "\n [ -I_xz, -I_yz, 0.5*I_xx + 0.5*I_yy - 0.5*I_zz, mr_z],"
+            "\n [ mr_x, mr_y, mr_z, m ]].")
+          .staticmethod("FromPseudoInertia")
+          .def(
+            "FromLogCholeskyParameters", &Inertia::template FromLogCholeskyParameters<VectorXs>,
+            bp::args("log_cholesky"),
+            "Builds an InertiaTpl from log Cholesky parameters."
+            "\nThe parameters are given as log_cholesky = [alpha, d_1, d_2, d_3, s_{12}, s_{23}, "
+            "s_{13}, t_1, t_2, t_3] "
+            "\nThe returned InertiaTpl object is constructed from the provided log Cholesky "
+            "parameters.")
+          .staticmethod("FromLogCholeskyParameters")
           .def(
             "FromSphere", &Inertia::FromSphere, bp::args("mass", "radius"),
             "Returns the Inertia of a sphere defined by a given mass and radius.")
@@ -236,6 +275,21 @@ namespace pinocchio
         return self.toDynamicParameters();
       }
 
+      static Matrix4 toPseudoInertia_proxy(const Inertia & self)
+      {
+        return self.toPseudoInertia();
+      }
+
+      static VectorXs LogcholToDynamicParameters_proxy(const VectorXs & log_cholesky)
+      {
+        return Inertia::LogcholToDynamicParameters(log_cholesky);
+      }
+
+      static MatrixXs calculateLogCholeskyJacobian_proxy(const VectorXs & log_cholesky)
+      {
+        return Inertia::calculateLogCholeskyJacobian(log_cholesky);
+      }
+
       static Inertia *
       makeFromMCI(const Scalar & mass, const Vector3 & lever, const Matrix3 & inertia)
       {

include/pinocchio/bindings/python/spatial/inertia.hpp

@@ -241,6 +241,7 @@ namespace pinocchio
     typedef Eigen::Matrix<T, 3, 3, U> Matrix3;
     typedef Eigen::Matrix<T, 4, 4, U> Matrix4;
     typedef Eigen::Matrix<T, 6, 6, U> Matrix6;
+    typedef Eigen::Matrix<T, 10, 10, U> Matrix10;
     typedef Matrix6 ActionMatrix_t;
     typedef Vector3 Angular_t;
     typedef Vector3 Linear_t;
@@ -271,6 +272,7 @@ namespace pinocchio
 
     typedef typename Symmetric3::AlphaSkewSquare AlphaSkewSquare;
     typedef typename Eigen::Matrix<Scalar, 10, 1, Options> Vector10;
+    typedef typename Eigen::Matrix<Scalar, 10, 10, Options> Matrix10;
 
     // Constructors
     InertiaTpl()
@@ -565,6 +567,220 @@ namespace pinocchio
         mass, lever, Symmetric3(params.template segment<6>(4)) + AlphaSkewSquare(mass, lever));
     }
 
+    /**
+     * Converts the inertia to a pseudo inertia matrix.
+     *
+     * @return A 4x4 pseudo inertia matrix.
+     */
+    Matrix4 toPseudoInertia() const
+    {
+      Vector10 dynamic_params = toDynamicParameters();
+      Scalar m = dynamic_params[0];
+      Vector3 h = dynamic_params.template segment<3>(1);
+      Matrix3 I_bar;
+      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];
+
+      Matrix3 Sigma = 0.5 * I_bar.trace() * Matrix3::Identity() - I_bar;
+      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) = m;
+
+      return pseudo_inertia;
+    }
+
+    /**
+     * Builds an inertia matrix from a 4x4 pseudo inertia matrix.
+     *
+     * @param[in] pseudo_inertia A 4x4 pseudo inertia matrix.
+     *
+     * @return An InertiaTpl object constructed from the provided pseudo inertia matrix.
+     */
+    static InertiaTpl FromPseudoInertia(const Matrix4 & pseudo_inertia)
+    {
+      Scalar m = pseudo_inertia(3, 3);
+      Vector3 h = pseudo_inertia.template block<3, 1>(0, 3);
+      Matrix3 Sigma = pseudo_inertia.template block<3, 3>(0, 0);
+      Matrix3 I_bar = Sigma.trace() * Matrix3::Identity() - Sigma;
+
+      Vector10 dynamic_params;
+      dynamic_params[0] = m; /*  */
+      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 FromDynamicParameters(dynamic_params);
+    }
+
+    /**
+     * Converts logarithmic Cholesky parameters directly to dynamic parameters.
+     *
+     * @param[in] log_cholesky A 10-dimensional vector containing logarithmic Cholesky parameters.
+     * The parameters are given as
+     * \f$ \eta = [\alpha, d_1, d_2, d_3, s_{12}, s_{23}, s_{13}, t_1, t_2, t_3] \f$
+     *
+     * @return A 10-dimensional vector containing mass, first moments, and inertia tensor
+     * components. The parameters are given as \f$ \theta = [m, mc_x, mc_y, mc_z, I_{xx}, I_{xy},
+     * I_{yy}, I_{xz}, I_{yz}, I_{zz}] \f$
+     */
+    template<typename Vector10Like>
+    static Vector10 LogcholToDynamicParameters(const Eigen::MatrixBase<Vector10Like> & log_cholesky)
+    {
+      PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector10Like, log_cholesky, 10, 1);
+      using Scalar = typename Vector10Like::Scalar;
+      Vector10 dynamic_params;
+
+      const Scalar alpha = log_cholesky[0];
+      const Scalar d1 = log_cholesky[1];
+      const Scalar d2 = log_cholesky[2];
+      const Scalar d3 = log_cholesky[3];
+      const Scalar s12 = log_cholesky[4];
+      const Scalar s23 = log_cholesky[5];
+      const Scalar s13 = log_cholesky[6];
+      const Scalar t1 = log_cholesky[7];
+      const Scalar t2 = log_cholesky[8];
+      const Scalar t3 = log_cholesky[9];
+
+      const Scalar exp_d1 = math::exp(d1);
+      const Scalar exp_d2 = math::exp(d2);
+      const Scalar exp_d3 = math::exp(d3);
+
+      dynamic_params[0] = 1;
+      dynamic_params[1] = t1;
+      dynamic_params[2] = t2;
+      dynamic_params[3] = t3;
+      dynamic_params[4] = s23 * s23 + t2 * t2 + t3 * t3 + exp_d2 * exp_d2 + exp_d3 * exp_d3;
+      dynamic_params[5] = -s12 * exp_d2 - s13 * s23 - t1 * t2;
+      dynamic_params[6] =
+        s12 * s12 + s13 * s13 + t1 * t1 + t3 * t3 + exp_d1 * exp_d1 + exp_d3 * exp_d3;
+      dynamic_params[7] = -s13 * exp_d3 - t1 * t3;
+      dynamic_params[8] = -s23 * exp_d3 - t2 * t3;
+      dynamic_params[9] =
+        s12 * s12 + s13 * s13 + s23 * s23 + t1 * t1 + t2 * t2 + exp_d1 * exp_d1 + exp_d2 * exp_d2;
+
+      const Scalar exp_2_alpha = math::exp(2 * alpha);
+      dynamic_params *= exp_2_alpha;
+
+      return dynamic_params;
+    }
+
+    /**
+     * Builds an Inertia from log Cholesky parameters.
+     *
+     * @param[in] log_cholesky A 10-dimensional vector containing logarithmic Cholesky parameters.
+     * The parameters are given as
+     * \f$ \eta = [\alpha, d_1, d_2, d_3, s_{12}, s_{23}, s_{13}, t_1, t_2, t_3] \f$
+     *
+     * @return An Inertia object constructed from the provided log Cholesky parameters.
+     */
+    template<typename Vector10Like>
+    static InertiaTpl
+    FromLogCholeskyParameters(const Eigen::MatrixBase<Vector10Like> & log_cholesky)
+    {
+      PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector10Like, log_cholesky, 10, 1);
+      Vector10 dynamic_params = LogcholToDynamicParameters(log_cholesky);
+      return FromDynamicParameters(dynamic_params);
+    }
+
+    /**
+     * Calculates the Jacobian of the dynamic parameters with respect to the log-Cholesky
+     * parameters.
+     *
+     * @param[in] log_cholesky A 10-dimensional vector containing the log-Cholesky parameters.
+     *
+     * @return A 10x10 matrix containing the Jacobian of dynamic parameters with respect to
+     * log-Cholesky parameters.
+     */
+    static Matrix10 calculateLogCholeskyJacobian(const Vector10 & log_cholesky)
+    {
+      Matrix10 jacobian = Matrix10::Zero();
+      const Scalar alpha = log_cholesky[0];
+      const Scalar d1 = log_cholesky[1];
+      const Scalar d2 = log_cholesky[2];
+      const Scalar d3 = log_cholesky[3];
+      const Scalar s12 = log_cholesky[4];
+      const Scalar s23 = log_cholesky[5];
+      const Scalar s13 = log_cholesky[6];
+      const Scalar t1 = log_cholesky[7];
+      const Scalar t2 = log_cholesky[8];
+      const Scalar t3 = log_cholesky[9];
+
+      const Scalar exp_2alpha = math::exp(2 * alpha);
+      const Scalar exp_2d1 = math::exp(2 * d1);
+      const Scalar exp_2d2 = math::exp(2 * d2);
+      const Scalar exp_2d3 = math::exp(2 * d3);
+      const Scalar exp_d1 = math::exp(d1);
+      const Scalar exp_d2 = math::exp(d2);
+      const Scalar exp_d3 = math::exp(d3);
+
+      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 * (s23 * s23 + t2 * t2 + t3 * t3 + 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 * (s12 * s12 + s13 * s13 + t1 * t1 + t3 * t3 + 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
+                       * (s12 * s12 + s13 * s13 + s23 * s23 + t1 * t1 + t2 * t2 + 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;
+
+      return jacobian;
+    }
+
     // Arithmetic operators
     InertiaTpl & __equl__(const InertiaTpl & clone)
     {

include/pinocchio/spatial/inertia.hpp

@@ -111,6 +111,40 @@ def test_dynamic_parameters(self):
         I2 = pin.Inertia.FromDynamicParameters(v)
         self.assertApprox(I2, I)
 
+    def test_pseudo_inertia(self):
+        I = pin.Inertia.Random()
+        pseudo = I.toPseudoInertia()
+        self.assertTrue(np.all(np.linalg.eigvals(pseudo) > 0))
+        I_back = pin.Inertia.FromPseudoInertia(pseudo)
+        self.assertApprox(I_back.mass, I.mass)
+        self.assertApprox(I_back.lever, I.lever)
+        self.assertApprox(I_back.inertia, I.inertia)
+
+    def test_log_cholesky(self):
+        eta = np.random.randn(10)
+        alpha, d1, d2, d3, s12, s23, s13, t1, t2, t3 = eta
+        # Compute the exponential terms
+        exp_alpha = np.exp(alpha)
+        exp_d1 = np.exp(d1)
+        exp_d2 = np.exp(d2)
+        exp_d3 = np.exp(d3)
+
+        # Create the matrix U
+        U = exp_alpha * np.array(
+            [
+                [exp_d1, s12, s13, t1],
+                [0, exp_d2, s23, t2],
+                [0, 0, exp_d3, t3],
+                [0, 0, 0, 1],
+            ]
+        )
+        pseudo_chol = U @ U.T
+
+        inertia = pin.Inertia.FromLogCholeskyParameters(eta)
+        pseudo = inertia.toPseudoInertia()
+        self.assertTrue(np.all(np.linalg.eigvals(pseudo) > 0))
+        self.assertApprox(pseudo, pseudo_chol)
+
     def test_array(self):
         I = pin.Inertia.Random()
         I_array = np.array(I)

unittest/python/bindings_inertia.py

@@ -631,6 +631,15 @@ BOOST_AUTO_TEST_CASE(test_Inertia)
     Symmetric3(3.79705882, 0., 3.79705882, 0., 0., 1.81176471).matrix(),
     1e-5)); // Computed with hppfcl::Capsule
 
+  // Test Inertia From Pseudo Inertia and backward
+  I1 = Inertia::FromCapsule(1., 2., 3);
+  BOOST_CHECK(I1.isApprox(Inertia::FromPseudoInertia(I1.toPseudoInertia())));
+
+  // Test Inertia From LogCholesky parameters and backward
+  Inertia::Vector10 logChol = Inertia::Vector10::Random();
+  I1 = Inertia::FromLogCholeskyParameters(logChol);
+  BOOST_CHECK(I1.isApprox(Inertia::FromDynamicParameters(I1.toDynamicParameters())));
+
   // Copy operator
   Inertia aI_copy(aI);
   BOOST_CHECK(aI_copy == aI);