Commit ea646e7f authored by jcarpent's avatar jcarpent
Browse files

[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<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 &,
......
......@@ -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 ()
Markdown is supported
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