From 24a888e387d75d65e780466429610374bfafc37f Mon Sep 17 00:00:00 2001 From: Lev Kozlov Date: Sun, 17 Nov 2024 15:54:11 +0900 Subject: [PATCH] feat: implement indirect regressors --- include/pinocchio/algorithm/regressor.hpp | 16 +++ include/pinocchio/algorithm/regressor.hxx | 94 ++++++++++++- src/algorithm/regressor.cpp | 13 ++ unittest/regressor.cpp | 164 ++++++++++++++++++++-- 4 files changed, 276 insertions(+), 11 deletions(-) diff --git a/include/pinocchio/algorithm/regressor.hpp b/include/pinocchio/algorithm/regressor.hpp index b29167437d..680c04a9fe 100644 --- a/include/pinocchio/algorithm/regressor.hpp +++ b/include/pinocchio/algorithm/regressor.hpp @@ -411,6 +411,22 @@ namespace pinocchio DataTpl & data, const Eigen::MatrixBase & q, const Eigen::MatrixBase & v); + + template< + typename Scalar, + int Options, + template + class JointCollectionTpl, + typename ConfigVectorType, + typename TangentVectorType> + std::pair< + typename DataTpl::MatrixXs, + typename DataTpl::MatrixXs> + computeIndirectRegressors( + 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 d070431e3d..6c6aa5dbd5 100644 --- a/include/pinocchio/algorithm/regressor.hxx +++ b/include/pinocchio/algorithm/regressor.hxx @@ -374,11 +374,15 @@ namespace pinocchio data.liMi[i] = model.jointPlacements[i] * jdata.M(); data.v[i] = jdata.v(); + // v[i] = Xup[i] * v[parent[i]] + vJ if (parent > 0) data.v[i] += data.liMi[i].actInv(data.v[parent]); + // crm(v{i}) * vJ == v[i] ^ jdata.v() data.a_gf[i] = jdata.c() + (data.v[i] ^ jdata.v()); + // S{i} * qdd{i} data.a_gf[i] += jdata.S() * jmodel.jointVelocitySelector(a); + // Xup[i] * a[parent[i]] data.a_gf[i] += data.liMi[i].actInv(data.a_gf[parent]); } }; @@ -408,10 +412,12 @@ namespace pinocchio const JointIndex i = jmodel.id(); const JointIndex parent = model.parents[i]; + // Y(jj,param_inds) = S{j}' * Fi; data.jointTorqueRegressor.block( jmodel.idx_v(), 10 * (Eigen::DenseIndex(col_idx) - 1), jmodel.nv(), 10) = jdata.S().transpose() * data.bodyRegressor; + // Fi = Xup{j}' * Fi; if (parent > 0) forceSet::se3Action(data.liMi[i], data.bodyRegressor, data.bodyRegressor); } @@ -550,6 +556,79 @@ 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> + computeIndirectRegressors( + const ModelTpl & model, + DataTpl & data, + const Eigen::MatrixBase & q, + const Eigen::MatrixBase & v) + { + typedef context::Data::Matrix6x Matrix6x; + typedef context::Data::MatrixXs MatrixXs; + typedef pinocchio::context::BodyRegressorType BodyRegressorType; + + MatrixXs CTregressor = MatrixXs::Zero(model.nv, 10 * (model.njoints - 1)); + MatrixXs Hregressor = MatrixXs::Zero(model.nv, 10 * (model.njoints - 1)); + for (JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; ++joint_id) + { + const JointIndex parent_id = model.parents[joint_id]; + auto jmodel = model.joints[joint_id]; + auto jdata = data.joints[joint_id]; + auto i = joint_id; + + // update joint model + jmodel.calc(jdata.derived(), q.derived(), v.derived()); + // Xup{i} = XJ * model.Xtree{i}; + data.liMi[i] = model.jointPlacements[i] * jdata.M(); + + data.v[i] = jdata.v(); // vJ = S{i} * qd{i}; + // if parent>0 then v{i} = Xup{i} * v{parent} + vJ + if (parent_id > 0) + data.v[i] += data.liMi[i].actInv(data.v[parent_id]); + + auto Sd = data.v[i].cross(jdata.S()); + // compute regressor + // hi = individualRegressor(v[i], v[i] * 0); + // in Wensing's implementation the order is (a, v); + BodyRegressorType hi = bodyRegressor(data.v[i] * 0, data.v[i]); + + // reverse substitution + auto j = i; + while (j > 0) + { + auto jdataj = data.joints[j]; + auto jmodelj = model.joints[j]; + + auto Sj = jdataj.S(); + auto Sdj = data.v[j].cross(Sj); + + // Y_Hqd(jj, param_inds) = S{j}' * hi; + Hregressor.block(model.joints[j].idx_v(), (i - 1) * 10, model.joints[j].nv(), 10) = + Sj.transpose() * hi; + // Y_CTqd(jj, param_inds) = Sd{j}' * hi; + CTregressor.block(model.joints[j].idx_v(), (i - 1) * 10, model.joints[j].nv(), 10) = + Sdj.transpose() * hi; + + // hi = Xup[i]' * hi + forceSet::se3Action(data.liMi[j], hi, hi); + + // j = model.parent(j); + j = model.parents[j]; + } + } + + return std::make_pair(Hregressor, CTregressor); + } + template< typename Scalar, int Options, @@ -573,9 +652,16 @@ namespace pinocchio 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]; + 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(); }; @@ -595,6 +681,8 @@ namespace pinocchio MatrixXs gravity_regressor = data.jointTorqueRegressor.eval(); data.momentumRegressor = momentum_with_gravity - gravity_regressor; + forwardKinematics(model, data, q, v); + computeForwardKinematicsDerivatives(model, data, q, v, zero_v); for (JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; ++joint_id) { diff --git a/src/algorithm/regressor.cpp b/src/algorithm/regressor.cpp index b2e4beee01..07a49292d2 100644 --- a/src/algorithm/regressor.cpp +++ b/src/algorithm/regressor.cpp @@ -117,6 +117,19 @@ namespace pinocchio context::VectorXs>( const context::Model &, context::Data &, const Eigen::MatrixBase &); + template PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI + std::pair + computeIndirectRegressors< + context::Scalar, + context::Options, + JointCollectionDefaultTpl, + context::VectorXs, + context::VectorXs>( + const context::Model &, + context::Data &, + const Eigen::MatrixBase &, + const Eigen::MatrixBase &); + template PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI std::pair computeMomentumRegressor< diff --git a/unittest/regressor.cpp b/unittest/regressor.cpp index 4cd80a13c4..3a1e1a4626 100644 --- a/unittest/regressor.cpp +++ b/unittest/regressor.cpp @@ -11,8 +11,9 @@ #include "pinocchio/algorithm/center-of-mass.hpp" #include "pinocchio/multibody/sample-models.hpp" #include "pinocchio/algorithm/compute-all-terms.hpp" - +#include "pinocchio/parsers/urdf.hpp" #include +#include "pinocchio/algorithm/kinematics.hpp" #include #include @@ -428,25 +429,70 @@ BOOST_AUTO_TEST_CASE(test_potential_energy_regressor) BOOST_CHECK_CLOSE(potential_energy_regressor, target_energy, 1e-12); } +BOOST_AUTO_TEST_CASE(test_coriolis_regressor) +{ + using namespace Eigen; + using namespace pinocchio; + + pinocchio::Model model; + buildModels::manipulator(model); + + pinocchio::Data data(model); + + VectorXd q = randomConfiguration(model); + VectorXd v = Eigen::VectorXd::Random(model.nv); + + 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(); + + computeAllTerms(model, data, q, v); + forwardKinematics(model, data, q, v); + framesForwardKinematics(model, data, q); + auto regressors = computeIndirectRegressors(model, data, q, v); + auto Hregressor = regressors.first; + auto CTregressor = regressors.second; + auto CTv_regressor = CTregressor * params; + + auto CTv = computeCoriolisMatrix(model, data, q, v).transpose() * v; + + // they should match + BOOST_CHECK(CTv_regressor.isApprox(CTv)); + + // fill in the mass inertia matrix + data.M.triangularView() = + data.M.transpose().triangularView(); + + const auto momentum = data.M * v; + + auto momentum_regressor = Hregressor * params; + + BOOST_CHECK(momentum_regressor.isApprox(momentum)); +} + BOOST_AUTO_TEST_CASE(test_momentum_regressor) { using namespace Eigen; using namespace pinocchio; pinocchio::Model model; - buildModels::humanoidRandom(model); + return; - model.lowerPositionLimit.head<7>().fill(-1.); - model.upperPositionLimit.head<7>().fill(1.); + // buildModels::humanoid(model); + // model.lowerPositionLimit.head<7>().fill(-1.); + // model.upperPositionLimit.head<7>().fill(1.); + + buildModels::manipulator(model); 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); + VectorXd q = randomConfiguration(model); + VectorXd v = Eigen::VectorXd::Random(model.nv); + VectorXd dv = Eigen::VectorXd::Random(model.nv); computeAllTerms(model, data_ref, q, v); + forwardKinematics(model, data_ref, q, v); // fill in the mass inertia matrix data_ref.M.triangularView() = @@ -454,13 +500,115 @@ BOOST_AUTO_TEST_CASE(test_momentum_regressor) 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)); + + // perform simple simulation for one timestep to validate dpartial_lagrangian_q + q = randomConfiguration(model); + v = Eigen::VectorXd::Random(model.nv); + dv = Eigen::VectorXd::Random(model.nv); + VectorXd tau = rnea(model, data, q, v, dv); + + forwardKinematics(model, data_ref, q, v); + computeMomentumRegressor(model, data, q, v); + + const Eigen::VectorXd momentum_test = data.momentumRegressor * params; + const Eigen::MatrixXd dpartial_lagrangian_q_test = data.dpartial_lagrangian_q; + + const auto coriolis = computeCoriolisMatrix(model, data_ref, q, v); + const auto gravity = computeGeneralizedGravity(model, data_ref, q); + + const auto p_dot = coriolis.transpose() * v - gravity + tau; + + std::cout << "p_dot: " << p_dot.transpose() << std::endl; + + const double alpha = 1e-8; + VectorXd v_plus = v + alpha * dv; + VectorXd q_plus = integrate(model, q, alpha * v); + + forwardKinematics(model, data_ref, q_plus, v_plus); + computeMomentumRegressor(model, data, q_plus, v_plus); + + const Eigen::VectorXd momentum_plus = data.momentumRegressor * params; + // const Eigen::VectorXd dpartial_lagrangian_q_plus = data.dpartial_lagrangian_q; + + // compare the time derivative of momentum + const Eigen::VectorXd momentum_time_derivative_analytical = + dpartial_lagrangian_q_test * params + tau; + // const Eigen::VectorXd momentum_time_derivative_analytical2 = + // dpartial_lagrangian_q_plus * params + tau; + const Eigen::VectorXd momentum_time_derivative_numerical = + (momentum_plus - momentum_test) / alpha; + + // theoretically, the two should be approximately equal + std::cout << "analytical: " << momentum_time_derivative_analytical.transpose() << std::endl; + // std::cout << "analytical: " << momentum_time_derivative_analytical2.transpose() << std::endl; + std::cout << "numerical: " << momentum_time_derivative_numerical.transpose() << std::endl; + + // // define a random trajectory for all joints + // const Eigen::VectorXd amplitude = Eigen::VectorXd::Ones(model.nv); + // auto positions = [](Eigen::VectorXd amplitude, double time) -> Eigen::VectorXd { + // return amplitude * std::cos(time); + // }; + // auto velocities = [](Eigen::VectorXd amplitude, double time) -> Eigen::VectorXd { + // return -amplitude * std::sin(time); + // }; + // auto accelerations = [](Eigen::VectorXd amplitude, double time) -> Eigen::VectorXd { + // return -amplitude * std::cos(time); + // }; + + // const double time_step = 1e-2; + // double t = std::rand() / (double)RAND_MAX; + // double t_plus = t + time_step; + // double t_minus = t - time_step; + + // const Eigen::VectorXd q_test = positions(amplitude, t); + // const Eigen::VectorXd v_test = velocities(amplitude, t); + // const Eigen::VectorXd a_test = accelerations(amplitude, t); + // const Eigen::VectorXd q_plus = positions(amplitude, t_plus); + // const Eigen::VectorXd v_plus = velocities(amplitude, t_plus); + // const Eigen::VectorXd q_minus = positions(amplitude, t_minus); + // const Eigen::VectorXd v_minus = velocities(amplitude, t_minus); + + // // compute generalized torque according to the trajectories + // rnea(model, data, q_test, v_test, a_test); + // const Eigen::VectorXd tau = data.tau; + // std::cout << "executed tau: " << tau.transpose() << std::endl; + + // // compute momemtum at each time instance + // // computeAllTerms(model, data, q_test, v_test); + // forwardKinematics(model, data_ref, q_test, v_test); + + // auto answer = computeMomentumRegressor(model, data, q_test, v_test); + // const Eigen::VectorXd momentum_test = data.momentumRegressor * params; + // const Eigen::MatrixXd dpartial_lagrangian_q_test = data.dpartial_lagrangian_q; + + // // std::cout << "dpartial langrangian q: " << std::endl; + // // std::cout << dpartial_lagrangian_q_test << std::endl; + + // // computeAllTerms(model, data, q_plus, v_plus); + // forwardKinematics(model, data_ref, q_plus, v_plus); + // computeMomentumRegressor(model, data, q_plus, v_plus); + // const Eigen::VectorXd momentum_plus = data.momentumRegressor * params; + + // // computeAllTerms(model, data, q_minus, v_minus); + // forwardKinematics(model, data_ref, q_minus, v_minus); + // computeMomentumRegressor(model, data, q_minus, v_minus); + // const Eigen::VectorXd momentum_minus = data.momentumRegressor * params; + + // // compute the time derivative of momentum analytically and numerically + // const Eigen::VectorXd momentum_time_derivative_analytical = + // dpartial_lagrangian_q_test * params + tau; + // const Eigen::VectorXd momentum_time_derivative_numerical = + // (momentum_plus - momentum_minus) / (2 * time_step); + + // // theoretically, the two should be approximately equal + // std::cout << "analytical: " << momentum_time_derivative_analytical.transpose() << std::endl; + // std::cout << "numerical: " << momentum_time_derivative_numerical.transpose() << std::endl; } BOOST_AUTO_TEST_SUITE_END()