From fecf8f8771176be28e32ec5f068e446ead784d32 Mon Sep 17 00:00:00 2001 From: Lev Kozlov Date: Mon, 9 Sep 2024 20:31:16 +0900 Subject: [PATCH 1/2] feat: init implementation --- .../python/algorithm/expose-regressor.cpp | 23 ++++++ include/pinocchio/algorithm/regressor.hpp | 17 +++++ include/pinocchio/algorithm/regressor.hxx | 71 +++++++++++++++++++ .../bindings/python/multibody/data.hpp | 3 + include/pinocchio/multibody/data.hpp | 6 ++ include/pinocchio/multibody/data.hxx | 2 + src/algorithm/regressor.cpp | 12 ++++ unittest/regressor.cpp | 35 +++++++++ 8 files changed, 169 insertions(+) diff --git a/bindings/python/algorithm/expose-regressor.cpp b/bindings/python/algorithm/expose-regressor.cpp index fa3e8a2d05..92840b7ca0 100644 --- a/bindings/python/algorithm/expose-regressor.cpp +++ b/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::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()); } } // namespace python diff --git a/include/pinocchio/algorithm/regressor.hpp b/include/pinocchio/algorithm/regressor.hpp index bdc14d3951..c941f33a33 100644 --- a/include/pinocchio/algorithm/regressor.hpp +++ b/include/pinocchio/algorithm/regressor.hpp @@ -379,6 +379,23 @@ namespace pinocchio const ModelTpl & model, DataTpl & data, const Eigen::MatrixBase & q); + + // TODO: Add documentation + template< + typename Scalar, + int Options, + template + class JointCollectionTpl, + typename ConfigVectorType, + typename TangentVectorType> + std::pair< + typename DataTpl::MatrixXs, + typename DataTpl::MatrixXs> + computeMomentumRegressor( + const ModelTpl & model, + DataTpl & data, + const Eigen::MatrixBase & q, + const Eigen::MatrixBase & v); } // namespace pinocchio /* --- Details -------------------------------------------------------------------- */ diff --git a/include/pinocchio/algorithm/regressor.hxx b/include/pinocchio/algorithm/regressor.hxx index 485447baf0..5df2294ba2 100644 --- a/include/pinocchio/algorithm/regressor.hxx +++ b/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 + class JointCollectionTpl, + typename ConfigVectorType, + typename TangentVectorType> + std::pair< + typename DataTpl::MatrixXs, + typename DataTpl::MatrixXs> + computeMomentumRegressor( + const ModelTpl & model, + DataTpl & data, + const Eigen::MatrixBase & q, + const Eigen::MatrixBase & 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 { + Eigen::Matrix 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::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__ diff --git a/include/pinocchio/bindings/python/multibody/data.hpp b/include/pinocchio/bindings/python/multibody/data.hpp index 9548a90a85..5ad77c5fef 100644 --- a/include/pinocchio/bindings/python/multibody/data.hpp +++ b/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) diff --git a/include/pinocchio/multibody/data.hpp b/include/pinocchio/multibody/data.hpp index d19468315c..3a23c8b037 100644 --- a/include/pinocchio/multibody/data.hpp +++ b/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; diff --git a/include/pinocchio/multibody/data.hxx b/include/pinocchio/multibody/data.hxx index c0aed6741c..b661c5cefd 100644 --- a/include/pinocchio/multibody/data.hxx +++ b/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)) diff --git a/src/algorithm/regressor.cpp b/src/algorithm/regressor.cpp index 6ed0615838..b2e4beee01 100644 --- a/src/algorithm/regressor.cpp +++ b/src/algorithm/regressor.cpp @@ -117,4 +117,16 @@ namespace pinocchio context::VectorXs>( const context::Model &, context::Data &, const Eigen::MatrixBase &); + template PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI + std::pair + computeMomentumRegressor< + context::Scalar, + context::Options, + JointCollectionDefaultTpl, + context::VectorXs, + context::VectorXs>( + const context::Model &, + context::Data &, + const Eigen::MatrixBase &, + const Eigen::MatrixBase &); } // namespace pinocchio diff --git a/unittest/regressor.cpp b/unittest/regressor.cpp index 99db82dd2a..4cd80a13c4 100644 --- a/unittest/regressor.cpp +++ b/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() = + data_ref.M.transpose().triangularView(); + 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() From d5dcdbd81953defe07619664d8b285a5a59326bf Mon Sep 17 00:00:00 2001 From: Lev Kozlov Date: Mon, 9 Sep 2024 20:37:21 +0900 Subject: [PATCH 2/2] docs: add proper description --- include/pinocchio/algorithm/regressor.hpp | 24 ++++++++++++++++++++++- 1 file changed, 23 insertions(+), 1 deletion(-) diff --git a/include/pinocchio/algorithm/regressor.hpp b/include/pinocchio/algorithm/regressor.hpp index c941f33a33..f0f4db8705 100644 --- a/include/pinocchio/algorithm/regressor.hpp +++ b/include/pinocchio/algorithm/regressor.hpp @@ -380,7 +380,29 @@ namespace pinocchio DataTpl & data, const Eigen::MatrixBase & q); - // TODO: Add documentation + /// + /// \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,