Momentum regressor

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::return_by_value>());
+
+      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<bp::return_by_value>());
+
     }
 
   } // namespace python

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()

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 <iostream>
+
+// 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));
+  }
+}

include/pinocchio/algorithm/regressor.hpp

@@ -372,6 +372,46 @@ namespace pinocchio
     const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
     DataTpl<Scalar, Options, JointCollectionTpl> & data,
     const Eigen::MatrixBase<ConfigVectorType> & 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<typename, int>
+    class JointCollectionTpl,
+    typename ConfigVectorType,
+    typename TangentVectorType>
+  std::pair<
+    typename DataTpl<Scalar, Options, JointCollectionTpl>::MatrixXs,
+    typename DataTpl<Scalar, Options, JointCollectionTpl>::MatrixXs>
+  computeIndirectRegressors(
+    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"
 
@@ -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,80 @@ 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>
+  computeIndirectRegressors(
+    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;
+    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);
+  }
+
 } // 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)