Commit ea646e7f by jcarpent

### [Algo] Add RNEA with external forces

parent c036f406
 ... ... @@ -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 &, ... ...
 ... ... @@ -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!