Passivity-based RNEA Algorithms

bindings/python/algorithm/expose-rnea.cpp

@@ -108,6 +108,19 @@ namespace pinocchio
         "\tmodel: model of the kinematic tree\n"
         "\tdata: data related to the model\n",
         bp::return_value_policy<bp::return_by_value>());
+
+      bp::def(
+        "passivityRNEA", &passivityRNEA<Scalar, Options, JointCollectionDefaultTpl, VectorXs, VectorXs, VectorXs, VectorXs>,
+        bp::args("model", "data", "q", "v", "v_r", "a_r"),
+        "Compute the passivity-based RNEA, store the result in 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"
+        "\tv_r: the auxiliary joint velocity vector (size model.nv)\n"
+        "\ta_r: the auxiliary joint acceleration vector (size model.nv)\n",
+        bp::return_value_policy<bp::return_by_value>());
     }
 
   } // namespace python

include/pinocchio/algorithm/rnea.hpp

@@ -221,6 +221,43 @@ namespace pinocchio
     const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
     DataTpl<Scalar, Options, JointCollectionTpl> & data);
 
+  ///
+  /// \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);
+
 } // namespace pinocchio
 
 /* --- Details -------------------------------------------------------------------- */

include/pinocchio/algorithm/rnea.hxx

@@ -659,6 +659,164 @@ namespace pinocchio
 
       return data.C;
     }
+
+    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;
+
+      typedef impl::CoriolisMatrixForwardStep<
+        Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType1>
+        CoriolisPass1;
+
+      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]);
+
+        // // option 1
+        // model.inertias[i].__mult__(data.v_r[i], data.h[i]); 
+        // // option 1
+
+        // // option 2
+        // data.B[i].setZero();
+        // model.inertias[i].__mult__(data.v[i], data.h[i]); 
+        // CoriolisPass1::addForceCrossMatrix(data.h[i], data.B[i]); 
+        // // option 2
+
+        // option 3 (Christoffel-consistent factorization)
+        data.B[i] = model.inertias[i].variation(Scalar(0.5) * data.v[i]);
+        model.inertias[i].__mult__(data.v[i], data.h[i]); 
+        CoriolisPass1::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]);
+
+        // // option 1
+        // data.f[i] += data.v[i].cross(data.h[i]); 
+        // // option 1
+
+        // // option 2
+        // data.f[i] += Force(data.B[i] * data.v_r[i].toVector()); 
+        // // option 2
+
+        // option 3 (Christoffel-consistent factorization)
+        data.f[i] += Force(data.B[i] * data.v_r[i].toVector()); 
+        // option 3 (Christoffel-consistent factorization)
+      }
+    };
+
+    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;
+    }
   } // namespace impl
   template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
   struct GetCoriolisMatrixBackwardStep
@@ -848,6 +1006,26 @@ namespace pinocchio
   {
     return impl::computeCoriolisMatrix(model, data, make_const_ref(q), make_const_ref(v));
   }
+
+  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));
+  }
 } // namespace pinocchio
 
 /// @endcond

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

@@ -80,6 +80,21 @@ namespace pinocchio
       context::Data &,
       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 & 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>> &);
   } // namespace impl
   template PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI const context::MatrixXs &
   getCoriolisMatrix<context::Scalar, context::Options, JointCollectionDefaultTpl>(

unittest/python/bindings_rnea.py

@@ -83,6 +83,14 @@ def test_coriolis_matrix(self):
         ) - pin.rnea(model, data2, self.q, self.v * 0, self.a * 0)
 
         self.assertApprox(tau_coriolis_ref, C.dot(self.v))
+
+    def test_passivity_rnea(self):
+        model = self.model
+
+        tau1 = pin.rnea(self.model, self.data, self.q, self.v, self.a)
+        tau2 = pin.passivityRNEA(self.model, self.data, self.q, self.v, self.v, self.a)
+
+        self.assertApprox(tau1, tau2)
 
 
 if __name__ == "__main__":