From 252effb8d818e3486ce342d86acbe92c90fffaf3 Mon Sep 17 00:00:00 2001 From: Cfather Date: Wed, 18 Sep 2024 16:13:12 +0000 Subject: [PATCH] add option 2 --- include/pinocchio/algorithm/rnea.hpp | 74 +++--- include/pinocchio/algorithm/rnea.hxx | 344 ++++++++++++++------------- src/algorithm/rnea.cpp | 30 +-- 3 files changed, 230 insertions(+), 218 deletions(-) diff --git a/include/pinocchio/algorithm/rnea.hpp b/include/pinocchio/algorithm/rnea.hpp index 55249f5dca..8c4aef13a1 100644 --- a/include/pinocchio/algorithm/rnea.hpp +++ b/include/pinocchio/algorithm/rnea.hpp @@ -80,43 +80,6 @@ namespace pinocchio const Eigen::MatrixBase & a, const container::aligned_vector & 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 - class JointCollectionTpl, - typename ConfigVectorType, - typename TangentVectorType1, - typename TangentVectorType2, - typename TangentVectorType3> - const typename DataTpl::TangentVectorType & passivityRNEA( - const ModelTpl & model, - DataTpl & data, - const Eigen::MatrixBase & q, - const Eigen::MatrixBase & v, - const Eigen::MatrixBase & v_r, - const Eigen::MatrixBase & 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:
\f$ @@ -258,6 +221,43 @@ namespace pinocchio const ModelTpl & model, DataTpl & 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 + class JointCollectionTpl, + typename ConfigVectorType, + typename TangentVectorType1, + typename TangentVectorType2, + typename TangentVectorType3> + const typename DataTpl::TangentVectorType & passivityRNEA( + const ModelTpl & model, + DataTpl & data, + const Eigen::MatrixBase & q, + const Eigen::MatrixBase & v, + const Eigen::MatrixBase & v_r, + const Eigen::MatrixBase & a_r); + } // namespace pinocchio /* --- Details -------------------------------------------------------------------- */ diff --git a/include/pinocchio/algorithm/rnea.hxx b/include/pinocchio/algorithm/rnea.hxx index 7a83fd2757..9413970d78 100644 --- a/include/pinocchio/algorithm/rnea.hxx +++ b/include/pinocchio/algorithm/rnea.hxx @@ -215,152 +215,6 @@ namespace pinocchio return data.tau; } - template< - typename Scalar, - int Options, - template - class JointCollectionTpl, - typename ConfigVectorType, - typename TangentVectorType1, - typename TangentVectorType2, - typename TangentVectorType3> - struct PassivityRneaForwardStep - : public fusion::JointUnaryVisitorBase> - { - typedef ModelTpl Model; - typedef DataTpl Data; - typedef ForceTpl Force; - - typedef boost::fusion::vector< - const Model &, - Data &, - const ConfigVectorType &, - const TangentVectorType1 &, - const TangentVectorType2 &, - const TangentVectorType3 &> - ArgsType; - - template - static void algo( - const JointModelBase & jmodel, - JointDataBase & jdata, - const Model & model, - Data & data, - const Eigen::MatrixBase & q, - const Eigen::MatrixBase & v, - const Eigen::MatrixBase & v_r, - const Eigen::MatrixBase & 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]); - - // 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) - } - - template - static void - addForceCrossMatrix(const ForceDense & f, const Eigen::MatrixBase & 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 - class JointCollectionTpl, - typename ConfigVectorType, - typename TangentVectorType1, - typename TangentVectorType2, - typename TangentVectorType3> - const typename DataTpl::TangentVectorType & passivityRNEA( - const ModelTpl & model, - DataTpl & data, - const Eigen::MatrixBase & q, - const Eigen::MatrixBase & v, - const Eigen::MatrixBase & v_r, - const Eigen::MatrixBase & 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 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 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, @@ -805,6 +659,164 @@ namespace pinocchio return data.C; } + + template< + typename Scalar, + int Options, + template + class JointCollectionTpl, + typename ConfigVectorType, + typename TangentVectorType1, + typename TangentVectorType2, + typename TangentVectorType3> + struct PassivityRneaForwardStep + : public fusion::JointUnaryVisitorBase> + { + typedef ModelTpl Model; + typedef DataTpl Data; + typedef ForceTpl 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 + static void algo( + const JointModelBase & jmodel, + JointDataBase & jdata, + const Model & model, + Data & data, + const Eigen::MatrixBase & q, + const Eigen::MatrixBase & v, + const Eigen::MatrixBase & v_r, + const Eigen::MatrixBase & 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 + class JointCollectionTpl, + typename ConfigVectorType, + typename TangentVectorType1, + typename TangentVectorType2, + typename TangentVectorType3> + const typename DataTpl::TangentVectorType & passivityRNEA( + const ModelTpl & model, + DataTpl & data, + const Eigen::MatrixBase & q, + const Eigen::MatrixBase & v, + const Eigen::MatrixBase & v_r, + const Eigen::MatrixBase & 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 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 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 class JointCollectionTpl> struct GetCoriolisMatrixBackwardStep @@ -932,26 +944,6 @@ 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 - class JointCollectionTpl, - typename ConfigVectorType, - typename TangentVectorType1, - typename TangentVectorType2, - typename TangentVectorType3> - const typename DataTpl::TangentVectorType & passivityRNEA( - const ModelTpl & model, - DataTpl & data, - const Eigen::MatrixBase & q, - const Eigen::MatrixBase & v, - const Eigen::MatrixBase & v_r, - const Eigen::MatrixBase & 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, @@ -1014,6 +1006,26 @@ namespace pinocchio { return impl::computeCoriolisMatrix(model, data, make_const_ref(q), make_const_ref(v)); } + + template< + typename Scalar, + int Options, + template + class JointCollectionTpl, + typename ConfigVectorType, + typename TangentVectorType1, + typename TangentVectorType2, + typename TangentVectorType3> + const typename DataTpl::TangentVectorType & passivityRNEA( + const ModelTpl & model, + DataTpl & data, + const Eigen::MatrixBase & q, + const Eigen::MatrixBase & v, + const Eigen::MatrixBase & v_r, + const Eigen::MatrixBase & 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 diff --git a/src/algorithm/rnea.cpp b/src/algorithm/rnea.cpp index e4211eb60a..6ff35bd9ea 100644 --- a/src/algorithm/rnea.cpp +++ b/src/algorithm/rnea.cpp @@ -36,21 +36,6 @@ namespace pinocchio const Eigen::MatrixBase> &, const container::aligned_vector &); - template PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI const context::VectorXs & passivityRNEA< - context::Scalar, - context::Options, - JointCollectionDefaultTpl, - Eigen::Ref, - Eigen::Ref, - Eigen::Ref, - Eigen::Ref>( - const context::Model &, - context::Data &, - const Eigen::MatrixBase> &, - const Eigen::MatrixBase> &, - const Eigen::MatrixBase> &, - const Eigen::MatrixBase> &); - template PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI const context::VectorXs & nonLinearEffects< context::Scalar, @@ -95,6 +80,21 @@ namespace pinocchio context::Data &, const Eigen::MatrixBase> &, const Eigen::MatrixBase> &); + + template PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI const context::VectorXs & passivityRNEA< + context::Scalar, + context::Options, + JointCollectionDefaultTpl, + Eigen::Ref, + Eigen::Ref, + Eigen::Ref, + Eigen::Ref>( + const context::Model &, + context::Data &, + const Eigen::MatrixBase> &, + const Eigen::MatrixBase> &, + const Eigen::MatrixBase> &, + const Eigen::MatrixBase> &); } // namespace impl template PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI const context::MatrixXs & getCoriolisMatrix(