Commit 03c0c518 authored by Rohan Budhiraja's avatar Rohan Budhiraja
Browse files

[aba] accept external forces in aba.

[unittest][aba] check aba with external force
[bindings][aba] create binding for aba with ext force
parent 41958ed0
......@@ -25,13 +25,27 @@ namespace se3
void exposeABA()
{
bp::def("aba",&aba,
typedef Eigen::VectorXd DataVector;
typedef container::aligned_vector<Force> ForceAlignedVector;
bp::def("aba",
(const DataVector& (*)(const Model&, Data&, const DataVector&, const DataVector&, const DataVector&)) &aba,
bp::args("Model","Data",
"Joint configuration q (size Model::nq)",
"Joint velocity v (size Model::nv)",
"Joint torque tau (size Model::nv)"),
"Compute ABA, put the result in Data::ddq and return it.",
bp::return_value_policy<bp::return_by_value>());
bp::def("aba",
(const DataVector& (*)(const Model&, Data&, const DataVector&, const DataVector&, const DataVector&, const ForceAlignedVector&))&aba,
bp::args("Model","Data",
"Joint configuration q (size Model::nq)",
"Joint velocity v (size Model::nv)",
"Joint torque tau (size Model::nv)",
"Vector of external forces expressed in the local frame of each joint (size Model::njoints)"),
"Compute ABA with external forces, put the result in Data::ddq and return it.",
bp::return_value_policy<bp::return_by_value>());
}
......
......@@ -41,6 +41,27 @@ namespace se3
const Eigen::VectorXd & v,
const Eigen::VectorXd & tau);
///
/// \brief The Articulated-Body algorithm. It computes the forward dynamics, aka the joint accelerations given the current state and actuation of the model.
///
/// \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] tau The joint torque vector (dim model.nv).
/// \param[in] fext Vector of external forces expressed in the local frame of the joints (dim model.njoints)
///
/// \return The current joint acceleration stored in data.ddq.
///
inline const Eigen::VectorXd &
aba(const Model & model,
Data & data,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v,
const Eigen::VectorXd & tau,
const container::aligned_vector<Force> & fext);
DEFINE_ALGO_CHECKER(ABA);
} // namespace se3
......
......@@ -221,6 +221,46 @@ namespace se3
return data.ddq;
}
inline const Eigen::VectorXd &
aba(const Model & model,
Data & data,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v,
const Eigen::VectorXd & tau,
const container::aligned_vector<Force> & fext)
{
assert(model.check(data) && "data is not consistent with model.");
data.v[0].setZero();
data.a[0] = -model.gravity;
data.u = tau;
for(Model::Index i=1;i<(Model::Index)model.njoints;++i)
{
AbaForwardStep1::run(model.joints[i],data.joints[i],
AbaForwardStep1::ArgsType(model,data,q,v));
data.f[i] -= fext[i];
}
for( Model::Index i=(Model::Index)model.njoints-1;i>0;--i )
{
AbaBackwardStep::run(model.joints[i],data.joints[i],
AbaBackwardStep::ArgsType(model,data));
}
for(Model::Index i=1;i<(Model::Index)model.njoints;++i)
{
AbaForwardStep2::run(model.joints[i],data.joints[i],
AbaForwardStep2::ArgsType(model,data));
}
return data.ddq;
}
// --- CHECKER ---------------------------------------------------------------
// --- CHECKER ---------------------------------------------------------------
// --- CHECKER ---------------------------------------------------------------
......
......@@ -21,6 +21,7 @@
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/algorithm/aba.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/parsers/sample-models.hpp"
......@@ -60,6 +61,41 @@ BOOST_AUTO_TEST_CASE ( test_aba_simple )
}
BOOST_AUTO_TEST_CASE ( test_aba_with_fext )
{
using namespace Eigen;
using namespace se3;
se3::Model model; buildModels::humanoidSimple(model);
se3::Data data(model);
VectorXd q = VectorXd::Random(model.nq);
q.segment<4>(3).normalize();
VectorXd v = VectorXd::Random(model.nv);
VectorXd a = VectorXd::Random(model.nv);
container::aligned_vector<Force> fext(model.joints.size(), Force::Random());
crba(model, data, q);
computeJacobians(model, data, q);
nonLinearEffects(model, data, q, v);
data.M.triangularView<Eigen::StrictlyLower>()
= data.M.transpose().triangularView<Eigen::StrictlyLower>();
VectorXd tau = data.M * a + data.nle;
Data::Matrix6x J = Data::Matrix6x::Zero(6, model.nv);
for(Model::Index i=1;i<(Model::Index)model.njoints;++i) {
getJacobian<true>(model, data, i, J);
tau -= J.transpose()*fext[i].toVector();
J.setZero();
}
aba(model, data, q, v, tau, fext);
BOOST_CHECK(data.ddq.isApprox(a, 1e-12));
}
BOOST_AUTO_TEST_CASE ( test_aba_vs_rnea )
{
using namespace Eigen;
......
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