Verified Commit 6cc6eb5c authored by jcarpent's avatar jcarpent Committed by Justin Carpentier
Browse files

[Test] Increase test for centroidal dynamics with wellknown equalities

parent d6f9a6d0
......@@ -196,6 +196,59 @@ BOOST_AUTO_TEST_CASE (test_dccrb)
BOOST_CHECK(dAg.isApprox(dAg_ref,sqrt(alpha)));
BOOST_CHECK(dAg.isApprox(dAg_ref_from_M,sqrt(alpha)));
}
// Compute tensor dAg/dq
{
std::vector<Data::Matrix6x> dAgdq((size_t)model.nv,Data::Matrix6x::Zero(6,model.nv));
Data data(model), data_fd(model);
Eigen::VectorXd v_fd(Eigen::VectorXd::Zero(model.nv));
ccrba(model,data_fd,q,v);
SE3 oMc_ref(SE3::Identity());
oMc_ref.translation() = data_fd.com[0];
Data::Matrix6x Ag0 = oMc_ref.toDualActionMatrix() * data_fd.Ag;
const Force hg0 = oMc_ref.act(data_fd.hg);
Data::Matrix6x Ag_fd(6,model.nv);
Force hg_fd;
const double alpha = 1e-8;
Eigen::VectorXd q_plus(model.nq);
Data::Matrix6x dhdq(6,model.nv);
for(int k = 0; k < model.nv; ++k)
{
v_fd[k] = alpha;
q_plus = integrate(model,q,v_fd);
ccrba(model,data_fd,q_plus,v);
SE3 oMc_fd(SE3::Identity());
oMc_fd.translation() = data_fd.com[0];
Ag_fd = oMc_fd.toDualActionMatrix() * data_fd.Ag;
hg_fd = oMc_fd.act(data_fd.hg);
dAgdq[(size_t)k] = (Ag_fd - Ag0)/alpha;
dhdq.col(k) = (hg_fd - hg0).toVector()/alpha;
v_fd[k] = 0.;
}
Data::Matrix6x dAg_ref(6,model.nv); dAg_ref.setZero();
for(int k = 0; k < model.nv; ++k)
{
dAg_ref += dAgdq[(size_t)k] * v[k];
}
Data::Matrix6x dAg_ref_bis(6,model.nv); dAg_ref_bis.setZero();
for(int k = 0; k < model.nv; ++k)
{
dAg_ref_bis.col(k) = dAgdq[(size_t)k] * v;
}
dccrba(model, data, q, v);
SE3 oMc(SE3::Identity());
oMc.translation() = data.com[0];
Data::Matrix6x dAg = oMc.toDualActionMatrix() * data.dAg;
BOOST_CHECK(dAg.isApprox(dAg_ref,sqrt(alpha)));
BOOST_CHECK(dhdq.isApprox(dAg_ref_bis,sqrt(alpha)));
BOOST_CHECK((dAg*v).isApprox(dhdq*v,sqrt(alpha)));
}
}
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