feat: init implementation

Author: lvjonok

bindings/python/algorithm/expose-regressor.cpp

@@ -27,6 +27,17 @@ namespace pinocchio
       return frameBodyRegressor(model, data, frameId);
     }
 
+    boost::python::tuple computeMomentumRegressor_proxy(
+      const context::Model & model,
+      context::Data & data,
+      const context::VectorXs & q,
+      const context::VectorXs & v)
+    {
+      computeMomentumRegressor(model, data, q, v);
+
+      return boost::python::make_tuple(data.momentumRegressor, data.dpartial_lagrangian_q);
+    }
+
     void exposeRegressor()
     {
       typedef context::Scalar Scalar;
@@ -123,6 +134,18 @@ namespace pinocchio
         "\tdata: data related to the model\n"
         "\tq: the joint configuration vector (size model.nq)\n",
         bp::return_value_policy<bp::return_by_value>());
+
+      bp::def(
+        "computeMomentumRegressor", &computeMomentumRegressor_proxy,
+        bp::args("model", "data", "q", "v"),
+        "Compute the momentum regressor and the partial derivative of the Lagrangian with respect "
+        "to the configuration, store the result in context::Data and return it.\n\n"
+        "Parameters:\n"
+        "\tmodel: model of the kinematic tree\n"
+        "\tdata: data related to the model\n"
+        "\tq: the joint configuration vector (size model.nq)\n"
+        "\tv: the joint velocity vector (size model.nv)\n",
+        bp::return_value_policy<bp::return_by_value>());
     }
 
   } // namespace python

include/pinocchio/algorithm/regressor.hpp

@@ -379,6 +379,23 @@ namespace pinocchio
     const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
     DataTpl<Scalar, Options, JointCollectionTpl> & data,
     const Eigen::MatrixBase<ConfigVectorType> & q);
+
+  // TODO: Add documentation
+  template<
+    typename Scalar,
+    int Options,
+    template<typename, int>
+    class JointCollectionTpl,
+    typename ConfigVectorType,
+    typename TangentVectorType>
+  std::pair<
+    typename DataTpl<Scalar, Options, JointCollectionTpl>::MatrixXs,
+    typename DataTpl<Scalar, Options, JointCollectionTpl>::MatrixXs>
+  computeMomentumRegressor(
+    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
+    DataTpl<Scalar, Options, JointCollectionTpl> & data,
+    const Eigen::MatrixBase<ConfigVectorType> & q,
+    const Eigen::MatrixBase<TangentVectorType> & v);
 } // namespace pinocchio
 
 /* --- Details -------------------------------------------------------------------- */

include/pinocchio/algorithm/regressor.hxx

@@ -7,6 +7,7 @@
 
 #include "pinocchio/algorithm/check.hpp"
 #include "pinocchio/algorithm/kinematics.hpp"
+#include "pinocchio/algorithm/kinematics-derivatives.hpp"
 #include "pinocchio/spatial/skew.hpp"
 #include "pinocchio/spatial/symmetric3.hpp"
 
@@ -557,6 +558,76 @@ namespace pinocchio
 
     return data.potentialEnergyRegressor;
   }
+
+  template<
+    typename Scalar,
+    int Options,
+    template<typename, int>
+    class JointCollectionTpl,
+    typename ConfigVectorType,
+    typename TangentVectorType>
+  std::pair<
+    typename DataTpl<Scalar, Options, JointCollectionTpl>::MatrixXs,
+    typename DataTpl<Scalar, Options, JointCollectionTpl>::MatrixXs>
+  computeMomentumRegressor(
+    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
+    DataTpl<Scalar, Options, JointCollectionTpl> & data,
+    const Eigen::MatrixBase<ConfigVectorType> & q,
+    const Eigen::MatrixBase<TangentVectorType> & v)
+  {
+    typedef context::Data::Matrix6x Matrix6x;
+    typedef context::Data::MatrixXs MatrixXs;
+
+    // symbolic jacobian of the spatial kinetic energy
+    auto spatialKineticEnergyJacobian =
+      [](const Eigen::Vector3d & v, const Eigen::Vector3d & w) -> Eigen::Matrix<double, 6, 10> {
+      Eigen::Matrix<double, 10, 6> jacobian;
+      jacobian << v[0], v[1], v[2], 0, 0, 0, 0, w[2], -w[1], 0, -v[2], v[1], -w[2], 0, w[0], v[2],
+        0, -v[0], w[1], -w[0], 0, -v[1], v[0], 0, 0, 0, 0, w[0], 0, 0, 0, 0, 0, w[1], w[0], 0, 0, 0,
+        0, 0, w[1], 0, 0, 0, 0, w[2], 0, w[0], 0, 0, 0, 0, w[2], w[1], 0, 0, 0, 0, 0, w[2];
+
+      return jacobian.transpose();
+    };
+
+    // zero the regressors
+    data.momentumRegressor.setZero();
+    data.dpartial_lagrangian_q.setZero();
+
+    auto zero_v = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>::Zero(model.nv);
+
+    // first compute the momentum regressor with gravity terms included
+    computeJointTorqueRegressor(model, data, q, zero_v, v);
+    MatrixXs momentum_with_gravity = data.jointTorqueRegressor.eval();
+
+    // compute the gravity component of the regressor
+    computeJointTorqueRegressor(model, data, q, zero_v, zero_v);
+    MatrixXs gravity_regressor = data.jointTorqueRegressor.eval();
+
+    data.momentumRegressor = momentum_with_gravity - gravity_regressor;
+
+    for (JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; ++joint_id)
+    {
+
+      Motion v_i = getVelocity(model, data, joint_id, LOCAL);
+
+      Matrix6x partial_dq(Matrix6x::Zero(6, model.nv));
+      Matrix6x dvb_dv(Matrix6x::Zero(6, model.nv));
+
+      getJointVelocityDerivatives(model, data, joint_id, LOCAL, partial_dq, dvb_dv);
+
+      // auto dvb_dv = velocity_derivatives.second;
+      auto phik_dv = spatialKineticEnergyJacobian(v_i.linear(), v_i.angular());
+
+      auto phik_dv_joint = dvb_dv.transpose() * phik_dv;
+      data.dpartial_lagrangian_q.middleCols((joint_id - 1) * 10, 10) = phik_dv_joint;
+    }
+
+    // Subtract the static regressor to get the final result
+    data.dpartial_lagrangian_q -= gravity_regressor;
+
+    return std::make_pair(data.momentumRegressor.eval(), data.dpartial_lagrangian_q.eval());
+  }
+
 } // namespace pinocchio
 
 #endif // ifndef __pinocchio_algorithm_regressor_hxx__

include/pinocchio/bindings/python/multibody/data.hpp

@@ -256,6 +256,9 @@ namespace pinocchio
           .ADD_DATA_PROPERTY(jointTorqueRegressor, "Joint torque regressor.")
           .ADD_DATA_PROPERTY(kineticEnergyRegressor, "Kinetic energy regressor.")
           .ADD_DATA_PROPERTY(potentialEnergyRegressor, "Potential energy regressor.")
+          .ADD_DATA_PROPERTY(momentumRegressor, "Momentum regressor.")
+          .ADD_DATA_PROPERTY(
+            dpartial_lagrangian_q, "Partial Lagrangian with respect to the joint configuration.")
 
 #ifndef PINOCCHIO_PYTHON_SKIP_COMPARISON_OPERATIONS
           .def(bp::self == bp::self)

include/pinocchio/multibody/data.hpp

@@ -505,6 +505,12 @@ namespace pinocchio
     /// \brief Matrix related to potential energy regressor
     RowVectorXs potentialEnergyRegressor;
 
+    /// \brief Matrix related to momentum regressor
+    MatrixXs momentumRegressor;
+
+    /// \brief Matrix related to partial Lagrangian with respect to the joint configuration
+    MatrixXs dpartial_lagrangian_q;
+
     PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6x) KA;
     PINOCCHIO_ALIGNED_STD_VECTOR(MatrixXs) LA;
     PINOCCHIO_ALIGNED_STD_VECTOR(VectorXs) lA;

include/pinocchio/multibody/data.hxx

@@ -115,6 +115,8 @@ namespace pinocchio
   , jointTorqueRegressor(MatrixXs::Zero(model.nv, 10 * (model.njoints - 1)))
   , kineticEnergyRegressor(RowVectorXs::Zero(10 * (model.njoints - 1)))
   , potentialEnergyRegressor(RowVectorXs::Zero(10 * (model.njoints - 1)))
+  , momentumRegressor(MatrixXs::Zero(model.nv, 10 * (model.njoints - 1)))
+  , dpartial_lagrangian_q(MatrixXs::Zero(model.nv, 10 * (model.njoints - 1)))
   , KA((std::size_t)model.njoints, Matrix6x::Zero(6, 0))
   , LA((std::size_t)model.njoints, MatrixXs::Zero(0, 0))
   , lA((std::size_t)model.njoints, VectorXs::Zero(0))

src/algorithm/regressor.cpp

@@ -117,4 +117,16 @@ namespace pinocchio
     context::VectorXs>(
     const context::Model &, context::Data &, const Eigen::MatrixBase<context::VectorXs> &);
 
+  template PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI
+    std::pair<context::Data::MatrixXs, context::Data::MatrixXs>
+    computeMomentumRegressor<
+      context::Scalar,
+      context::Options,
+      JointCollectionDefaultTpl,
+      context::VectorXs,
+      context::VectorXs>(
+      const context::Model &,
+      context::Data &,
+      const Eigen::MatrixBase<context::VectorXs> &,
+      const Eigen::MatrixBase<context::VectorXs> &);
 } // namespace pinocchio

unittest/regressor.cpp

@@ -428,4 +428,39 @@ BOOST_AUTO_TEST_CASE(test_potential_energy_regressor)
   BOOST_CHECK_CLOSE(potential_energy_regressor, target_energy, 1e-12);
 }
 
+BOOST_AUTO_TEST_CASE(test_momentum_regressor)
+{
+  using namespace Eigen;
+  using namespace pinocchio;
+
+  pinocchio::Model model;
+  buildModels::humanoidRandom(model);
+
+  model.lowerPositionLimit.head<7>().fill(-1.);
+  model.upperPositionLimit.head<7>().fill(1.);
+
+  pinocchio::Data data(model);
+  pinocchio::Data data_ref(model);
+
+  const VectorXd q = randomConfiguration(model);
+  const VectorXd v = Eigen::VectorXd::Random(model.nv);
+  const VectorXd dv = Eigen::VectorXd::Random(model.nv);
+
+  computeAllTerms(model, data_ref, q, v);
+
+  // fill in the mass inertia matrix
+  data_ref.M.triangularView<Eigen::StrictlyLower>() =
+    data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();
+  const VectorXd target_momentum = data_ref.M * v;
+
+  computeMomentumRegressor(model, data, q, v);
+  std::cout << "executed momentum regressor" << std::endl;
+  Eigen::VectorXd params(10 * (model.njoints - 1));
+  for (JointIndex i = 1; i < (Model::JointIndex)model.njoints; ++i)
+    params.segment<10>(Eigen::DenseIndex((i - 1) * 10)) = model.inertias[i].toDynamicParameters();
+
+  const VectorXd momentum_regressor = data.momentumRegressor * params;
+  BOOST_CHECK(momentum_regressor.isApprox(target_momentum));
+}
+
 BOOST_AUTO_TEST_SUITE_END()