Merge pull request #335 from jcarpent/topic/rnea

Add RNEA with external forces algo

bindings/python/algorithm/expose-rnea.cpp

@@ -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<Force> & 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)",

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 &,

src/spatial/force.hpp

@@ -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); }

src/spatial/motion.hpp

@@ -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<Scalar>::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()); }
 

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