diff --git a/bindings/python/algorithm/expose-rnea.cpp b/bindings/python/algorithm/expose-rnea.cpp
index ad64289f7df1e6316fad5b66c15c8ecfe203550f..576d4a38f13afdbbaa4fc00ea894f6c1a0750665 100644
--- a/bindings/python/algorithm/expose-rnea.cpp
+++ b/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)",
diff --git a/src/algorithm/rnea.hpp b/src/algorithm/rnea.hpp
index c3bcbc36fd374f4035c989a53de5c34e26341908..9d2b9b5ac00a953566d54a33281cf3e6b30b49f7 100644
--- a/src/algorithm/rnea.hpp
+++ b/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>
diff --git a/src/algorithm/rnea.hxx b/src/algorithm/rnea.hxx
index f0eeaca52a6413b932737e31611c08596730a1a1..5d0524ad18a53009b1c4c8472357cc3c70401909 100644
--- a/src/algorithm/rnea.hxx
+++ b/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 &,
diff --git a/src/spatial/force.hpp b/src/spatial/force.hpp
index d161b83b307f6427e21e2b16af942039a48da298..83d93d8746bd1263b9abdcc8d6ec3bd60d8eeeb1 100644
--- a/src/spatial/force.hpp
+++ b/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); }
diff --git a/src/spatial/motion.hpp b/src/spatial/motion.hpp
index a01f584cc42b3ecba874a59975e3041f764604a3..271f9d36019ea44d68079e66e6265508481bfcec 100644
--- a/src/spatial/motion.hpp
+++ b/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()); }
diff --git a/unittest/rnea.cpp b/unittest/rnea.cpp
index 7d4990eb61541d9a6cf6d3b6e4ed92a778e24bb1..7398e97fba8f2caa7af0c288dd646c78560b06fa 100644
--- a/unittest/rnea.cpp
+++ b/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 ()