Commit 45e1cb45 by Justin Carpentier Committed by GitHub

Merge pull request #335 from jcarpent/topic/rnea

Add RNEA with external forces algo
parents 01752677 eb916f1b
 ... ... @@ -30,6 +30,16 @@ namespace se3 { return rnea(*model,*data,q,v,a); } static Eigen::VectorXd rnea_fext_proxy(const ModelHandler& model, DataHandler & data, const VectorXd_fx & q, const VectorXd_fx & v, const VectorXd_fx & a, const std::vector & fext) { return rnea(*model,*data,q,v,a,fext); } static Eigen::VectorXd nle_proxy(const ModelHandler& model, DataHandler & data, ... ... @@ -47,7 +57,16 @@ namespace se3 "Velocity v (size Model::nv)", "Acceleration a (size Model::nv)"), "Compute the RNEA, put the result in Data and return it."); bp::def("rnea",rnea_fext_proxy, bp::args("Model","Data", "Configuration q (size Model::nq)", "Velocity v (size Model::nv)", "Acceleration a (size Model::nv)", "Vector of external forces expressed in the local frame of each joint (size Model::njoints)"), "Compute the RNEA with external forces, put the result in Data and return it."); bp::def("nle",nle_proxy, bp::args("Model","Data", "Configuration q (size Model::nq)", ... ...
 ... ... @@ -39,6 +39,25 @@ namespace se3 const Eigen::VectorXd & v, const Eigen::VectorXd & a); /// /// \brief The Recursive Newton-Euler algorithm. It computes the inverse dynamics, aka the joint torques according to the current state of the system, the desired joint accelerations and the external forces. /// /// \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] a The joint acceleration vector (dim model.nv). /// \param[in] fext Vector of external forces expressed in the local frame of the joints (dim model.njoints) /// /// \return The desired joint torques stored in data.tau. /// inline const Eigen::VectorXd & rnea(const Model & model, Data & data, const Eigen::VectorXd & q, const Eigen::VectorXd & v, const Eigen::VectorXd & a, const std::vector & fext); /// /// \brief Computes the non-linear effects (Corriolis, centrifual and gravitationnal effects), also called the biais terms \f$b(q,\dot{q}) \f$ of the Lagrangian dynamics: ///
\f$\begin{eqnarray} M \ddot{q} + b(q, \dot{q}) = \tau \end{eqnarray} \f$

... ...
 ... ... @@ -108,6 +108,34 @@ namespace se3 return data.tau; } inline const Eigen::VectorXd & rnea(const Model & model, Data & data, const Eigen::VectorXd & q, const Eigen::VectorXd & v, const Eigen::VectorXd & a, const std::vector & fext) { assert(fext.size() == model.joints.size()); data.v[0].setZero(); data.a_gf[0] = -model.gravity; for( Model::JointIndex i=1;i<(Model::JointIndex)model.njoints;++i ) { RneaForwardStep::run(model.joints[i],data.joints[i], RneaForwardStep::ArgsType(model,data,q,v,a)); data.f[i] -= fext[i]; } for( Model::JointIndex i=(Model::JointIndex)model.njoints-1;i>0;--i ) { RneaBackwardStep::run(model.joints[i],data.joints[i], RneaBackwardStep::ArgsType(model,data)); } return data.tau; } struct NLEForwardStep : public fusion::JointVisitor { typedef boost::fusion::vector< const se3::Model &, ... ...
 ... ... @@ -127,10 +127,17 @@ namespace se3 */ Derived_t & operator= (const Derived_t & other) { return derived().__equl__(other); } /** \brief replaces *this by *this + other. * \return a reference to *this /** * \brief Replaces *this by *this + other. * \return a reference to *this */ Derived_t & operator+= (const Derived_t & phi) { return derived().__pequ__(phi); } /** * \brief Replaces *this by *this - other. * \return a reference to *this */ Derived_t & operator-= (const Derived_t & phi) { return derived().__mequ__(phi); } /** \return an expression of the sum of *this and other */ ... ... @@ -307,6 +314,7 @@ namespace se3 ForceTpl & __equl__(const ForceTpl & other) { data = other.data; return *this; } ForceTpl & __pequ__ (const ForceTpl & phi) { data += phi.data; return *this; } ForceTpl & __mequ__ (const ForceTpl & phi) { data -= phi.data; return *this; } ForceTpl __plus__(const ForceTpl & phi) const { return ForceTpl(data + phi.data); } ForceTpl __mult__(const double a) const { return ForceTpl(a*data); } ForceTpl __minus__() const { return ForceTpl(-data); } ... ...
 ... ... @@ -62,6 +62,7 @@ namespace se3 Derived_t operator+(const Derived_t & v2) const { return derived().__plus__(v2); } Derived_t operator-(const Derived_t & v2) const { return derived().__minus__(v2); } Derived_t & operator+=(const Derived_t & v2) { return derived().__pequ__(v2); } Derived_t & operator-=(const Derived_t & v2) { return derived().__mequ__(v2); } bool isApprox (const Derived_t & other, const Scalar & prec = Eigen::NumTraits::dummy_precision()) const { return derived().isApprox_impl(other, prec);} ... ... @@ -192,6 +193,7 @@ namespace se3 MotionTpl __plus__(const MotionTpl & v2) const { return MotionTpl(data + v2.data); } MotionTpl __minus__(const MotionTpl & v2) const { return MotionTpl(data - v2.data); } MotionTpl& __pequ__(const MotionTpl & v2) { data += v2.data; return *this; } MotionTpl& __mequ__(const MotionTpl & v2) { data -= v2.data; return *this; } Scalar dot(const Force & f) const { return data.dot(f.toVector()); } ... ...
 ... ... @@ -131,4 +131,33 @@ BOOST_AUTO_TEST_CASE ( test_nle_vs_rnea ) BOOST_CHECK (tau_nle.isApprox(tau_rnea, 1e-12)); } BOOST_AUTO_TEST_CASE (test_rnea_with_fext) { using namespace Eigen; using namespace se3; Model model; buildModels::humanoidSimple(model); Data data_rnea_fext(model); Data data_rnea(model); VectorXd q (VectorXd::Random(model.nq)); q.segment<4>(3).normalize(); VectorXd v (VectorXd::Random(model.nv)); VectorXd a (VectorXd::Random(model.nv)); std::vector fext(model.joints.size(), Force::Zero()); JointIndex rf = model.getJointId("rleg6_joint"); Force Frf = Force::Random(); JointIndex lf = model.getJointId("lleg6_joint"); Force Flf = Force::Random(); rnea(model,data_rnea,q,v,a); rnea(model,data_rnea_fext,q,v,a,fext); BOOST_CHECK(data_rnea.tau.isApprox(data_rnea_fext.tau)); } BOOST_AUTO_TEST_SUITE_END ()
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!