Commit 45e1cb45 authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

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<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)",
......
......@@ -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>
......
......@@ -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 &,
......
......@@ -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<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()); }
......
......@@ -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 ()
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment