Momentum regressor

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,45 @@ namespace pinocchio
  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
  DataTpl<Scalar, Options, JointCollectionTpl> & data,
  const Eigen::MatrixBase<ConfigVectorType> & q);
+
+ ///
+ /// \brief Computes the momentum regressor and component of time derivative
+ /// of momentum regressor of the system based on the current robot state.
+ ///
+ /// The result stored in `data.momentumRegressor` corresponds to a matrix \f$ Y_p \f$ such that
+ /// \f$ P = Y_p(q, v) \pi = M v \f$ where \f$ \pi \f$ represents the vector of dynamic parameters
+ /// of each link.
+ ///
+ /// The result stored in `data.dpartial_lagrangian_q` corresponds to a matrix
+ /// \f$ Y_h \f$ such that \f$ \frac{\partial L}{\partial q} = Y_h(q, v) \f$.
+ ///
+ /// \tparam JointCollection Collection of Joint types.
+ /// \tparam ConfigVectorType Type of the joint configuration vector.
+ ///
+ /// \param[in] model The model structure representing the rigid body system.
+ /// \param[in] data The data structure of the rigid body system.
+ /// \param[in] q The joint configuration vector (dim model.nq).
+ /// \param[in] v The joint velocity vector (dim model.nv).
+ ///
+ /// \return A pair containing:
+ /// - The momentum regressor matrix.
+ /// - A matrix containing a component of the time derivative of the momentum regressor.
+ ///
+ 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()