add passivity rnea algorithm and the tests

Author: Cfather

include/pinocchio/algorithm/rnea.hpp

@@ -80,6 +80,43 @@ namespace pinocchio
     const Eigen::MatrixBase<TangentVectorType2> & a,
     const container::aligned_vector<ForceDerived> & fext);
 
+  ///
+  /// \brief The passivity-based Recursive Newton-Euler algorithm. It computes a modified inverse dynamics, aka the joint
+  /// torques according to the current state of the system and the auxiliary joint velocities and accelerations.
+  /// To be more specific, it computes H(q) * a_r + C(q, v) * v_r + g(q)
+  ///
+  /// \tparam JointCollection Collection of Joint types.
+  /// \tparam ConfigVectorType Type of the joint configuration vector.
+  /// \tparam TangentVectorType1 Type of the joint velocity vector.
+  /// \tparam TangentVectorType2 Type of the auxiliary joint velocity vector.
+  /// \tparam TangentVectorType3 Type of the auxiliary joint acceleration vector.
+  ///
+  /// \param[in] model The model structure of 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).
+  /// \param[in] v_r The auxiliary joint velocity vector (dim model.nv).
+  /// \param[in] a_r The auxiliary joint acceleration vector (dim model.nv).
+  ///
+  /// \return The desired joint torques stored in data.tau.
+  ///
+  template<
+    typename Scalar,
+    int Options,
+    template<typename, int>
+    class JointCollectionTpl,
+    typename ConfigVectorType,
+    typename TangentVectorType1,
+    typename TangentVectorType2,
+    typename TangentVectorType3>
+  const typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType & passivityRNEA(
+    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
+    DataTpl<Scalar, Options, JointCollectionTpl> & data,
+    const Eigen::MatrixBase<ConfigVectorType> & q,
+    const Eigen::MatrixBase<TangentVectorType1> & v,
+    const Eigen::MatrixBase<TangentVectorType2> & v_r,
+    const Eigen::MatrixBase<TangentVectorType3> & a_r);
+
   ///
   /// \brief Computes the non-linear effects (Corriolis, centrifual and gravitationnal effects),
   /// also called the bias terms \f$ b(q,\dot{q}) \f$ of the Lagrangian dynamics: <CENTER> \f$

include/pinocchio/algorithm/rnea.hxx

@@ -215,6 +215,159 @@ namespace pinocchio
       return data.tau;
     }
 
+    template<
+      typename Scalar,
+      int Options,
+      template<typename, int>
+      class JointCollectionTpl,
+      typename ConfigVectorType,
+      typename TangentVectorType1,
+      typename TangentVectorType2,
+      typename TangentVectorType3>
+    struct PassivityRneaForwardStep
+    : public fusion::JointUnaryVisitorBase<PassivityRneaForwardStep<
+        Scalar,
+        Options,
+        JointCollectionTpl,
+        ConfigVectorType,
+        TangentVectorType1,
+        TangentVectorType2,
+        TangentVectorType3>>
+    {
+      typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
+      typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
+      typedef ForceTpl<Scalar, Options> Force;
+
+      typedef boost::fusion::vector<
+        const Model &,
+        Data &,
+        const ConfigVectorType &,
+        const TangentVectorType1 &,
+        const TangentVectorType2 &,
+        const TangentVectorType3 &>
+        ArgsType;
+
+      template<typename JointModel>
+      static void algo(
+        const JointModelBase<JointModel> & jmodel,
+        JointDataBase<typename JointModel::JointDataDerived> & jdata,
+        const Model & model,
+        Data & data,
+        const Eigen::MatrixBase<ConfigVectorType> & q,
+        const Eigen::MatrixBase<TangentVectorType1> & v,
+        const Eigen::MatrixBase<TangentVectorType2> & v_r,
+        const Eigen::MatrixBase<TangentVectorType3> & a_r)
+      {
+        typedef typename Model::JointIndex JointIndex;
+
+        const JointIndex i = jmodel.id();
+        const JointIndex parent = model.parents[i];
+
+        jmodel.calc(jdata.derived(), q.derived(), v.derived());
+        data.v[i] = jdata.v();
+
+        jmodel.calc(jdata.derived(), q.derived(), v_r.derived());
+        data.v_r[i] = jdata.v();
+
+        data.liMi[i] = model.jointPlacements[i] * jdata.M();
+
+        if (parent > 0) {
+          data.v[i] += data.liMi[i].actInv(data.v[parent]);
+          data.v_r[i] += data.liMi[i].actInv(data.v_r[parent]);
+        }
+
+        data.a_gf[i] = jdata.c() + (data.v[i] ^ jdata.v());
+        data.a_gf[i] += jdata.S() * jmodel.jointVelocitySelector(a_r);
+        data.a_gf[i] += data.liMi[i].actInv(data.a_gf[parent]);
+        //
+        //      data.f[i] = model.inertias[i]*data.a_gf[i];// + model.inertias[i].vxiv(data.v[i]);
+        //      // -f_ext data.h[i] = model.inertias[i]*data.v[i];
+
+        // model.inertias[i].__mult__(data.v_r[i], data.h[i]); // option 1
+        data.B[i] = model.inertias[i].variation(Scalar(0.5) * data.v[i]);
+        model.inertias[i].__mult__(data.v[i], data.h[i]);
+        addForceCrossMatrix(Scalar(0.5) * data.h[i], data.B[i]); // option 3 (Christoffel-consistent factorization)
+        
+        model.inertias[i].__mult__(data.a_gf[i], data.f[i]);
+        // data.f[i] += data.v[i].cross(data.h[i]); // option 1
+        data.f[i] += Force(data.B[i] * data.v_r[i].toVector()); // option 3 (Christoffel-consistent factorization)
+
+        //      data.h[i].motionAction(data.v[i],data.f[i]);
+        //      data.f[i] = model.inertias[i].vxiv(data.v[i]);
+        //      data.f[i].setZero();
+      }
+
+      template<typename ForceDerived, typename M6>
+      static void
+      addForceCrossMatrix(const ForceDense<ForceDerived> & f, const Eigen::MatrixBase<M6> & mout)
+      {
+        M6 & mout_ = PINOCCHIO_EIGEN_CONST_CAST(M6, mout);
+        addSkew(
+          -f.linear(), mout_.template block<3, 3>(ForceDerived::LINEAR, ForceDerived::ANGULAR));
+        addSkew(
+          -f.linear(), mout_.template block<3, 3>(ForceDerived::ANGULAR, ForceDerived::LINEAR));
+        addSkew(
+          -f.angular(), mout_.template block<3, 3>(ForceDerived::ANGULAR, ForceDerived::ANGULAR));
+      }
+    };
+
+    template<
+      typename Scalar,
+      int Options,
+      template<typename, int>
+      class JointCollectionTpl,
+      typename ConfigVectorType,
+      typename TangentVectorType1,
+      typename TangentVectorType2,
+      typename TangentVectorType3>
+    const typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType & passivityRNEA(
+      const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
+      DataTpl<Scalar, Options, JointCollectionTpl> & data,
+      const Eigen::MatrixBase<ConfigVectorType> & q,
+      const Eigen::MatrixBase<TangentVectorType1> & v,
+      const Eigen::MatrixBase<TangentVectorType2> & v_r,
+      const Eigen::MatrixBase<TangentVectorType3> & a_r)
+    {
+      assert(model.check(data) && "data is not consistent with model.");
+      PINOCCHIO_CHECK_ARGUMENT_SIZE(
+        q.size(), model.nq, "The configuration vector is not of right size");
+      PINOCCHIO_CHECK_ARGUMENT_SIZE(
+        v.size(), model.nv, "The velocity vector is not of right size");
+      PINOCCHIO_CHECK_ARGUMENT_SIZE(
+        v_r.size(), model.nv, "The auxiliary velocity vector is not of right size");
+      PINOCCHIO_CHECK_ARGUMENT_SIZE(
+        a_r.size(), model.nv, "The auxiliary acceleration vector is not of right size");
+
+      typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
+      typedef typename Model::JointIndex JointIndex;
+
+      data.v[0].setZero();
+      data.v_r[0].setZero();
+      data.a_gf[0] = -model.gravity;
+
+      typedef PassivityRneaForwardStep<
+        Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType1,
+        TangentVectorType2, TangentVectorType3>
+        Pass1;
+      typename Pass1::ArgsType arg1(model, data, q.derived(), v.derived(), v_r.derived(), a_r.derived());
+      for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
+      {
+        Pass1::run(model.joints[i], data.joints[i], arg1);
+      }
+
+      typedef RneaBackwardStep<Scalar, Options, JointCollectionTpl> Pass2;
+      typename Pass2::ArgsType arg2(model, data);
+      for (JointIndex i = (JointIndex)model.njoints - 1; i > 0; --i)
+      {
+        Pass2::run(model.joints[i], data.joints[i], arg2);
+      }
+
+      // Add rotorinertia contribution
+      data.tau.array() += model.armature.array() * a_r.array(); // Check if there is memory allocation
+
+      return data.tau;
+    }
+
     template<
       typename Scalar,
       int Options,
@@ -786,6 +939,26 @@ namespace pinocchio
     return impl::rnea(model, data, make_const_ref(q), make_const_ref(v), make_const_ref(a), fext);
   }
 
+  template<
+    typename Scalar,
+    int Options,
+    template<typename, int>
+    class JointCollectionTpl,
+    typename ConfigVectorType,
+    typename TangentVectorType1,
+    typename TangentVectorType2,
+    typename TangentVectorType3>
+  const typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType & passivityRNEA(
+    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
+    DataTpl<Scalar, Options, JointCollectionTpl> & data,
+    const Eigen::MatrixBase<ConfigVectorType> & q,
+    const Eigen::MatrixBase<TangentVectorType1> & v,
+    const Eigen::MatrixBase<TangentVectorType2> & v_r,
+    const Eigen::MatrixBase<TangentVectorType3> & a_r)
+  {
+    return impl::passivityRNEA(model, data, make_const_ref(q), make_const_ref(v), make_const_ref(v_r), make_const_ref(a_r));
+  }
+
   template<
     typename Scalar,
     int Options,

include/pinocchio/algorithm/rnea.txx

@@ -39,6 +39,21 @@ namespace pinocchio
       const Eigen::MatrixBase<Eigen::Ref<const context::VectorXs>> &,
       const container::aligned_vector<context::Force> &);
 
+    extern template PINOCCHIO_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI const context::VectorXs &
+    passivityRNEA<
+      context::Scalar,
+      context::Options,
+      JointCollectionDefaultTpl,
+      Eigen::Ref<const context::VectorXs>,
+      Eigen::Ref<const context::VectorXs>,
+      Eigen::Ref<const context::VectorXs>>(
+      const context::Model &,
+      context::Data &,
+      const Eigen::MatrixBase<Eigen::Ref<const context::VectorXs>> &,
+      const Eigen::MatrixBase<Eigen::Ref<const context::VectorXs>> &,
+      const Eigen::MatrixBase<Eigen::Ref<const context::VectorXs>> &,
+      const Eigen::MatrixBase<Eigen::Ref<const context::VectorXs>> &);
+
     extern template PINOCCHIO_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI const context::VectorXs &
     nonLinearEffects<
       context::Scalar,

include/pinocchio/multibody/data.hpp

@@ -138,6 +138,9 @@ namespace pinocchio
     /// \brief Vector of joint velocities expressed in the local frame of the joint.
     PINOCCHIO_ALIGNED_STD_VECTOR(Motion) v;
 
+    /// \brief Vector of auxiliary joint velocities expressed in the local frame of the joint.
+    PINOCCHIO_ALIGNED_STD_VECTOR(Motion) v_r;
+
     /// \brief Vector of joint velocities expressed at the origin of the world.
     PINOCCHIO_ALIGNED_STD_VECTOR(Motion) ov;
 

include/pinocchio/multibody/data.hxx

@@ -26,6 +26,7 @@ namespace pinocchio
   , a_gf((std::size_t)model.njoints, Motion::Zero())
   , oa_gf((std::size_t)model.njoints, Motion::Zero())
   , v((std::size_t)model.njoints, Motion::Zero())
+  , v_r((std::size_t)model.njoints, Motion::Zero())
   , ov((std::size_t)model.njoints, Motion::Zero())
   , f((std::size_t)model.njoints, Force::Zero())
   , of((std::size_t)model.njoints, Force::Zero())
@@ -276,7 +277,7 @@ namespace pinocchio
     bool value =
       data1.joints == data2.joints && data1.a == data2.a && data1.oa == data2.oa
       && data1.oa_drift == data2.oa_drift && data1.oa_augmented == data2.oa_augmented
-      && data1.a_gf == data2.a_gf && data1.oa_gf == data2.oa_gf && data1.v == data2.v
+      && data1.a_gf == data2.a_gf && data1.oa_gf == data2.oa_gf && data1.v == data2.v && data1.v_r == data2.v_r
       && data1.ov == data2.ov && data1.f == data2.f && data1.of == data2.of
       && data1.of_augmented == data2.of_augmented && data1.h == data2.h && data1.oh == data2.oh
       && data1.oMi == data2.oMi && data1.liMi == data2.liMi && data1.tau == data2.tau

include/pinocchio/serialization/data.hpp

@@ -39,6 +39,7 @@ namespace boost
       PINOCCHIO_MAKE_DATA_NVP(ar, data, a_gf);
       PINOCCHIO_MAKE_DATA_NVP(ar, data, oa_gf);
       PINOCCHIO_MAKE_DATA_NVP(ar, data, v);
+      PINOCCHIO_MAKE_DATA_NVP(ar, data, v_r);
       PINOCCHIO_MAKE_DATA_NVP(ar, data, ov);
       PINOCCHIO_MAKE_DATA_NVP(ar, data, f);
       PINOCCHIO_MAKE_DATA_NVP(ar, data, of);

src/algorithm/rnea.cpp

@@ -36,6 +36,21 @@ namespace pinocchio
       const Eigen::MatrixBase<Eigen::Ref<const context::VectorXs>> &,
       const container::aligned_vector<context::Force> &);
 
+    template PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI const context::VectorXs & passivityRNEA<
+      context::Scalar,
+      context::Options,
+      JointCollectionDefaultTpl,
+      Eigen::Ref<const context::VectorXs>,
+      Eigen::Ref<const context::VectorXs>,
+      Eigen::Ref<const context::VectorXs>,
+      Eigen::Ref<const context::VectorXs>>(
+      const context::Model &,
+      context::Data &,
+      const Eigen::MatrixBase<Eigen::Ref<const context::VectorXs>> &,
+      const Eigen::MatrixBase<Eigen::Ref<const context::VectorXs>> &,
+      const Eigen::MatrixBase<Eigen::Ref<const context::VectorXs>> &,
+      const Eigen::MatrixBase<Eigen::Ref<const context::VectorXs>> &);
+
     template PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI const context::VectorXs &
     nonLinearEffects<
       context::Scalar,

unittest/rnea.cpp

@@ -344,4 +344,57 @@ BOOST_AUTO_TEST_CASE(test_compute_coriolis)
   }
 }
 
+BOOST_AUTO_TEST_CASE(test_passivityrnea_vs_rnea)
+{
+  using namespace Eigen;
+  using namespace pinocchio;
+
+  pinocchio::Model model;
+  buildModels::humanoidRandom(model);
+
+  model.lowerPositionLimit.head<3>().fill(-1.);
+  model.upperPositionLimit.head<3>().fill(1.);
+
+  pinocchio::Data data_passivityrnea(model);
+  pinocchio::Data data_rnea(model);
+
+  VectorXd q = randomConfiguration(model);
+  VectorXd v = VectorXd::Random(model.nv);
+  VectorXd a = VectorXd::Random(model.nv);
+  
+  VectorXd tau_passivityrnea = passivityRNEA(model, data_passivityrnea, q, v, v, a);
+  VectorXd tau_rnea = rnea(model, data_rnea, q, v, a);
+
+  BOOST_CHECK(tau_passivityrnea.isApprox(tau_rnea, 1e-12));
+}
+
+BOOST_AUTO_TEST_CASE(test_passivityrnea_compute_coriolis)
+{
+  using namespace Eigen;
+  using namespace pinocchio;
+
+  const double prec = Eigen::NumTraits<double>::dummy_precision();
+
+  Model model;
+  buildModels::humanoidRandom(model);
+
+  model.lowerPositionLimit.head<3>().fill(-1.);
+  model.upperPositionLimit.head<3>().fill(1.);
+  model.gravity.setZero();
+
+  Data data_ref(model);
+  Data data(model);
+
+  VectorXd q = randomConfiguration(model);
+
+  VectorXd v(VectorXd::Random(model.nv));
+  computeCoriolisMatrix(model, data, q, v);
+
+  VectorXd v_r(VectorXd::Random(model.nv));
+  passivityRNEA(model, data_ref, q, v, v_r, VectorXd::Zero(model.nv));
+
+  VectorXd tau = data.C * v_r;
+  BOOST_CHECK(tau.isApprox(data_ref.tau, prec));
+}
+
 BOOST_AUTO_TEST_SUITE_END()