[Algo] Add RNEA with external forces

src/algorithm/rnea.hpp

@@ -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<Force> & 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:
   /// <CENTER> \f$ \begin{eqnarray} M \ddot{q} + b(q, \dot{q}) = \tau  \end{eqnarray} \f$ </CENTER> <BR>

src/algorithm/rnea.hxx

@@ -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<Force> & 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<NLEForwardStep>
   {
     typedef boost::fusion::vector< const se3::Model &,

unittest/rnea.cpp

@@ -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<Force> 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 ()