Verified Commit 3afebdee authored by Justin Carpentier's avatar Justin Carpentier
Browse files

algo: add missing Ig computations

parent f454a136
Pipeline #12944 failed with stage
in 168 minutes and 41 seconds
......@@ -173,6 +173,10 @@ namespace pinocchio
data.hg.angular() += data.hg.linear().cross(data.com[0]);
data.Jcom = data.Ag.template topRows<3>()/data.mass[0];
data.Ig.mass() = data.Ycrb[0].mass();
data.Ig.lever().setZero();
data.Ig.inertia() = data.Ycrb[0].inertia();
// Energy
computeKineticEnergy(model, data);
computePotentialEnergy(model, data);
......
......@@ -233,7 +233,7 @@ namespace pinocchio
Force dhg;
/// \brief Centroidal Composite Rigid Body Inertia.
/// \note \f$ hg = Ig v_{\text{mean}}\f$ map a mean velocity to the current centroil momentum quantity.
/// \note \f$ hg = Ig v_{\text{mean}}\f$ map a mean velocity to the current centroidal momentum quantity.
Inertia Ig;
/// \brief Spatial forces set, used in CRBA and CCRBA
......
......@@ -44,12 +44,16 @@ BOOST_AUTO_TEST_CASE ( test_against_algo )
centerOfMass(model, data_other, q, v, true);
computeKineticEnergy(model, data_other, q, v);
computePotentialEnergy(model, data_other, q);
ccrba(model, data_other, q, v);
BOOST_CHECK (data.nle.isApprox(data_other.nle, 1e-12));
BOOST_CHECK (Eigen::MatrixXd(data.M.triangularView<Eigen::Upper>())
.isApprox(Eigen::MatrixXd(data_other.M.triangularView<Eigen::Upper>()), 1e-12));
BOOST_CHECK (data.J.isApprox(data_other.J, 1e-12));
BOOST_CHECK (data.Jcom.isApprox(data_other.Jcom, 1e-12));
BOOST_CHECK (data.Ag.isApprox(data_other.Ag, 1e-12));
BOOST_CHECK (data.hg.isApprox(data_other.hg, 1e-12));
BOOST_CHECK (data.Ig.isApprox(data_other.Ig, 1e-12));
for (int k=0; k<model.njoints; ++k)
{
......@@ -83,6 +87,7 @@ BOOST_AUTO_TEST_CASE ( test_against_algo )
BOOST_CHECK (data.Jcom.isApprox(data_other.Jcom, 1e-12));
BOOST_CHECK (data.Ag.isApprox(data_other.Ag, 1e-12));
BOOST_CHECK (data.hg.isApprox(data_other.hg, 1e-12));
BOOST_CHECK (data.Ig.isApprox(data_other.Ig, 1e-12));
for (int k=0; k<model.njoints; ++k)
{
......@@ -117,6 +122,7 @@ BOOST_AUTO_TEST_CASE ( test_against_algo )
BOOST_CHECK (data.Jcom.isApprox(data_other.Jcom, 1e-12));
BOOST_CHECK (data.Ag.isApprox(data_other.Ag, 1e-12));
BOOST_CHECK (data.hg.isApprox(data_other.hg, 1e-12));
BOOST_CHECK (data.Ig.isApprox(data_other.Ig, 1e-12));
for (int k=0; k<model.njoints; ++k)
{
......@@ -151,6 +157,7 @@ BOOST_AUTO_TEST_CASE ( test_against_algo )
BOOST_CHECK (data.Jcom.isApprox(data_other.Jcom, 1e-12));
BOOST_CHECK (data.Ag.isApprox(data_other.Ag, 1e-12));
BOOST_CHECK (data.hg.isApprox(data_other.hg, 1e-12));
BOOST_CHECK (data.Ig.isApprox(data_other.Ig, 1e-12));
for (int k=0; k<model.njoints; ++k)
{
......
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