diff --git a/bindings/python/algorithm/expose-regressor.cpp b/bindings/python/algorithm/expose-regressor.cpp index fa3e8a2d05..8dc266302a 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 computeIndirectRegressors_proxy( + const context::Model & model, + context::Data & data, + const context::VectorXs & q, + const context::VectorXs & v) + { + auto result = computeIndirectRegressors(model, data, q, v); + + return boost::python::make_tuple(result.first, result.second); + } + 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( + "computeIndirectRegressors", &computeIndirectRegressors_proxy, + bp::args("model", "data", "q", "v"), + "Compute the indirect regressors of momentum and transposed coriolis matrix times velocity,\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/examples/CMakeLists.txt b/examples/CMakeLists.txt index bbb2e5427a..288736bbd9 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -59,6 +59,7 @@ if(BUILD_WITH_URDF_SUPPORT) add_pinocchio_cpp_example(kinematics-derivatives PARSERS) add_pinocchio_cpp_example(forward-dynamics-derivatives PARSERS) add_pinocchio_cpp_example(inverse-dynamics-derivatives PARSERS) + add_pinocchio_cpp_example(sysid PARSERS) if(BUILD_ADVANCED_TESTING) add_pinocchio_cpp_example(multiprecision PARSERS) endif() diff --git a/examples/sysid.cpp b/examples/sysid.cpp new file mode 100644 index 0000000000..ab94b77eee --- /dev/null +++ b/examples/sysid.cpp @@ -0,0 +1,170 @@ +#include "pinocchio/parsers/urdf.hpp" + +#include "pinocchio/algorithm/joint-configuration.hpp" +#include "pinocchio/algorithm/kinematics-derivatives.hpp" +#include "pinocchio/algorithm/regressor.hpp" +#include "pinocchio/algorithm/rnea.hpp" +#include "pinocchio/algorithm/aba.hpp" +#include "pinocchio/algorithm/energy.hpp" + +#include + +// PINOCCHIO_MODEL_DIR is defined by the CMake but you can define your own directory here. +#ifndef PINOCCHIO_MODEL_DIR + #define PINOCCHIO_MODEL_DIR "path_to_the_model_dir" +#endif +int main(int argc, char ** argv) +{ + using namespace pinocchio; + + // You should change here to set up your own URDF file or just pass it as an argument of this + // example. + const std::string urdf_filename = + (argc <= 1) ? PINOCCHIO_MODEL_DIR + + std::string("/example-robot-data/robots/ur_description/urdf/ur5_robot.urdf") + : argv[1]; + + // Load the URDF model + Model model; + pinocchio::urdf::buildModel(urdf_filename, model); + + // Build a data related to model + Data data(model); + + // In this example we explore some of the system identification tools provided by Pinocchio. + // We start by defining a vector of dynamical parameters of our dynamic model. + Eigen::VectorXd dyn_parameters = Eigen::VectorXd::Zero(model.nv * 10); + for (JointIndex jnt_idx = 1; jnt_idx < model.njoints; ++jnt_idx) + { + // We can set the inertial parameters of the joints + dyn_parameters.segment<10>((jnt_idx - 1) * 10) = model.inertias[jnt_idx].toDynamicParameters(); + } + + { + // Sample a random joint configuration as well as random joint velocity and acceleration + Eigen::VectorXd q = randomConfiguration(model); + Eigen::VectorXd v = Eigen::VectorXd::Random(model.nv); + Eigen::VectorXd a = Eigen::VectorXd::Random(model.nv); + + // Some of the dynamics quantities can be parametrized linearly with respect to the dynamical + // parameters. For instance, in RNEA algorithm, the resulting joint torques can be expressed as + // a $Y(q, v, a) \cdot \theta = \tau$ where $Y(q, v, a)$ is a so-called joint-torque regressor. + auto jointTorqueRegressor = computeJointTorqueRegressor(model, data, q, v, a); + auto regressorTau = jointTorqueRegressor * dyn_parameters; + auto rneaTau = rnea(model, data, q, v, a); + // The two torques should be equal + assert((regressorTau - rneaTau).isZero(1e-12)); + + // However, in the real-world scenario, measuring acceleration accurately is almost impossible. + // Instead, we can use other quantities such as energy or momentum to compute how well the + // parameters fit the data. + // Let's start with energy parametrization + auto kineticEnergyRegressor = computeKineticEnergyRegressor(model, data, q, v); + auto potentialEnergyRegressor = computePotentialEnergyRegressor(model, data, q); + auto regressorEnergy = kineticEnergyRegressor + potentialEnergyRegressor; + auto energy = computeKineticEnergy(model, data, q, v) + computePotentialEnergy(model, data, q); + + // The energy should be equal + assert(std::abs((regressorEnergy * dyn_parameters - energy)) < 1e-12); + } + + // However, the logical question is how we can compute the energy (which uses default parameters). + // We recall that the power is time derivative of energy. Therefore, we can use + // the torque and velocity to compute the mechanical power and integrate on horizon to get the + // energy. Let's reset the configuration and simulate the system with the sine wave of joint + // torques; + { + auto torque_fn = [&model](const double & t) -> Eigen::VectorXd { + return Eigen::VectorXd::Ones(model.nv) * std::sin(t); + }; + + // Reset the configuration + Eigen::VectorXd q = randomConfiguration(model); + Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv); + // Perform simulation for 1000 steps with dt=1e-3 + double dt = 2e-4; + const int N = 1000; + Eigen::MatrixXd history_q = Eigen::MatrixXd::Zero(model.nq, N); + Eigen::MatrixXd history_v = Eigen::MatrixXd::Zero(model.nv, N); + + for (int i = 0; i < N; ++i) + { + auto tau = torque_fn(i * dt); + auto a = aba(model, data, q, v, tau); + // simple integration + v += a * dt; + q = integrate(model, q, v * dt); + + history_q.col(i) = q; + history_v.col(i) = v; + } + + // Now we can compute the difference in energy between the initial and final states + // using the regressor and integrate the power. + auto regEnergyFn = [&model, &data, &dyn_parameters]( + const Eigen::VectorXd & q, const Eigen::VectorXd & v) -> double { + auto kineticEnergyRegressor = computeKineticEnergyRegressor(model, data, q, v); + auto potentialEnergyRegressor = computePotentialEnergyRegressor(model, data, q); + auto regressorEnergy = kineticEnergyRegressor + potentialEnergyRegressor; + return regressorEnergy * dyn_parameters; + }; + + // Compute the energy difference + auto energy_diff = regEnergyFn(history_q.col(N - 1), history_v.col(N - 1)) + - regEnergyFn(history_q.col(0), history_v.col(0)); + + // Compute the power integral + double power_integral = 0; + for (int i = 0; i < N; ++i) + { + power_integral += torque_fn(i * dt).dot(history_v.col(i)) * dt; + } + + // The energy difference should be close to the power integral + assert( + std::abs(energy_diff - power_integral) + < 1e-2); // the tolerance is high due numerical integration + } + + // Another concept we can approach is the momentum. + // Momentum can be defined as $H = M(q) \cdot v$ where $M(q)$ is the mass inertia matrix or $H = + // Y_H(q, v) \pi$ in regressor form. On the other hand, one can show that $\dot_H = \tau + C(q, + // v)^T v - g(q)$ where $C(q, v)$ is the Coriolis matrix and $g(q)$ is the gravity vector. + // Fortunately, C(q, v)^T v can be also expressed in regressor form. + { + Eigen::VectorXd q = randomConfiguration(model); + Eigen::VectorXd v = Eigen::VectorXd::Random(model.nv); + auto tau = Eigen::VectorXd::Random(model.nv); + const double dt = 1e-3; + + auto regressors1 = computeIndirectRegressors(model, data, q, v); + // compute the momentum using regressor form + Eigen::VectorXd H1 = regressors1.first * dyn_parameters; + + // integrate forward + auto v_next = v + aba(model, data, q, v, tau) * dt; + auto q_next = integrate(model, q, v_next * dt); + auto regressors2 = computeIndirectRegressors(model, data, q_next, v_next); + // compute the momentum using regressor form + Eigen::VectorXd H2 = regressors2.first * dyn_parameters; + + // compute the numerical momentum difference + Eigen::VectorXd numericalMomentumDiff = (H2 - H1); + + // Compare the C^T v term + Eigen::VectorXd CTv_regressor = regressors1.second * dyn_parameters; + Eigen::VectorXd CTv = computeCoriolisMatrix(model, data, q, v).transpose() * v; + assert((CTv_regressor - CTv).isZero(1e-12)); + + // Compare the gravity term + Eigen::VectorXd g_regressor = computePotentialEnergyRegressor(model, data, q); + Eigen::VectorXd g = computeGeneralizedGravity(model, data, q); + assert((g_regressor - g).isZero(1e-12)); + + // find analytical momentum derivative + Eigen::VectorXd analyticalMomentumDot = CTv + tau - g; + + // Verify that the numerical momentum difference is close to the analytical momentum derivative + assert((numericalMomentumDiff - analyticalMomentumDot * dt).isZero(1e-5)); + } +} diff --git a/include/pinocchio/algorithm/regressor.hpp b/include/pinocchio/algorithm/regressor.hpp index 643dbdb5c0..cf9eb70a4a 100644 --- a/include/pinocchio/algorithm/regressor.hpp +++ b/include/pinocchio/algorithm/regressor.hpp @@ -372,6 +372,46 @@ namespace pinocchio const ModelTpl & model, DataTpl & data, const Eigen::MatrixBase & q); + + /// \brief Computes the indirect regressors Y_Hqd and Y_CTqd + /// of momentum and transposed coriolis matrix times velocity. + /// + /// These regressors are such that: + /// \f$ p = Y_Hqd(q, v) \pi$ and \f$ C^T v = Y_CTqd(q, v) \pi \f$ + /// where \f$ \pi \f$ represents the vector of dynamic parameters of each link. + /// + /// This algorithm can be applied in the context of system identification based on the + /// generalized momentum \f$ p \f$. + /// \f$ \dot{p} = \tau + C^T v - g \f$ + /// + /// \tparam JointCollection Collection of Joint types. + /// \tparam ConfigVectorType Type of the joint configuration vector. + /// \tparam TangentVectorType Type of the joint velocity 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 + 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 88f7cffa51..5f619f429b 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" @@ -373,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]); } }; @@ -407,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); } @@ -548,6 +555,83 @@ 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]; + } + } + + data.HRegressor = Hregressor; + data.YCTvRegressor = CTregressor; + + return std::make_pair(Hregressor, CTregressor); + } + } // 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..d9e0249f12 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 YCTv regressor + MatrixXs YCTvRegressor; + + /// \brief Matrix related to momentum regressor + MatrixXs HRegressor; + 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..0b0556320c 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))) + , YCTvRegressor(MatrixXs::Zero(model.nv, 10 * (model.njoints - 1))) + , HRegressor(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..dfa4c90636 100644 --- a/src/algorithm/regressor.cpp +++ b/src/algorithm/regressor.cpp @@ -117,4 +117,17 @@ 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 &); + } // namespace pinocchio diff --git a/unittest/regressor.cpp b/unittest/regressor.cpp index 99db82dd2a..4f020e4db8 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,4 +429,45 @@ BOOST_AUTO_TEST_CASE(test_potential_energy_regressor) BOOST_CHECK_CLOSE(potential_energy_regressor, target_energy, 1e-12); } +BOOST_AUTO_TEST_CASE(test_indirect_regressors) +{ + 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_SUITE_END()