diff --git a/bindings/python/algorithm/expose-rnea.cpp b/bindings/python/algorithm/expose-rnea.cpp index 6a6e60e220..2b2145e506 100644 --- a/bindings/python/algorithm/expose-rnea.cpp +++ b/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::def( + "passivityRNEA", &passivityRNEA, + 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()); } } // namespace python diff --git a/include/pinocchio/algorithm/rnea.hpp b/include/pinocchio/algorithm/rnea.hpp index 499b69a005..04598f941d 100644 --- a/include/pinocchio/algorithm/rnea.hpp +++ b/include/pinocchio/algorithm/rnea.hpp @@ -215,6 +215,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 a7412057e3..a2d9120862 100644 --- a/include/pinocchio/algorithm/rnea.hxx +++ b/include/pinocchio/algorithm/rnea.hxx @@ -666,6 +666,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 @@ -850,6 +1008,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/include/pinocchio/algorithm/rnea.txx b/include/pinocchio/algorithm/rnea.txx index 09331477f4..c14e122ecf 100644 --- a/include/pinocchio/algorithm/rnea.txx +++ b/include/pinocchio/algorithm/rnea.txx @@ -39,6 +39,21 @@ namespace pinocchio const Eigen::MatrixBase> &, const container::aligned_vector &); + extern template PINOCCHIO_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI const context::VectorXs & + passivityRNEA< + context::Scalar, + context::Options, + JointCollectionDefaultTpl, + Eigen::Ref, + Eigen::Ref, + Eigen::Ref>( + const context::Model &, + context::Data &, + const Eigen::MatrixBase> &, + const Eigen::MatrixBase> &, + const Eigen::MatrixBase> &, + const Eigen::MatrixBase> &); + extern template PINOCCHIO_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI const context::VectorXs & nonLinearEffects< context::Scalar, diff --git a/include/pinocchio/multibody/data.hpp b/include/pinocchio/multibody/data.hpp index a06ebf622f..ff8bc84a23 100644 --- a/include/pinocchio/multibody/data.hpp +++ b/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; diff --git a/include/pinocchio/multibody/data.hxx b/include/pinocchio/multibody/data.hxx index e04993971b..05c5066992 100644 --- a/include/pinocchio/multibody/data.hxx +++ b/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()) @@ -340,7 +341,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 diff --git a/include/pinocchio/serialization/data.hpp b/include/pinocchio/serialization/data.hpp index 8ac69063c5..56937dd14e 100644 --- a/include/pinocchio/serialization/data.hpp +++ b/include/pinocchio/serialization/data.hpp @@ -38,6 +38,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); diff --git a/src/algorithm/rnea.cpp b/src/algorithm/rnea.cpp index 07c27057e1..6ff35bd9ea 100644 --- a/src/algorithm/rnea.cpp +++ b/src/algorithm/rnea.cpp @@ -80,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( diff --git a/unittest/python/bindings_rnea.py b/unittest/python/bindings_rnea.py index 88c54a0f8c..f383da0bef 100644 --- a/unittest/python/bindings_rnea.py +++ b/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__": diff --git a/unittest/rnea.cpp b/unittest/rnea.cpp index cf53534be8..b2a4d2da35 100644 --- a/unittest/rnea.cpp +++ b/unittest/rnea.cpp @@ -347,6 +347,59 @@ 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::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_CASE(test_rnea_mimic) { for (int i = 0; i < pinocchio::MimicTestCases::N_CASES; i++)