Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	unittest/aba.cpp	Lines:	130	130	100.0 %
Date:	2024-04-26 13:14:21	Branches:	457	906	50.4 %


//
// Copyright (c) 2016-2020 CNRS INRIA
//

#include "pinocchio/spatial/fwd.hpp"
#include "pinocchio/spatial/se3.hpp"
#include "pinocchio/multibody/visitor.hpp"
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"
#include "pinocchio/algorithm/aba.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/parsers/sample-models.hpp"

#include "pinocchio/algorithm/compute-all-terms.hpp"
#include "pinocchio/utils/timer.hpp"

#include <iostream>

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )

template<typename JointModel>
void test_joint_methods(const pinocchio::JointModelBase<JointModel> & jmodel)
{
  typedef typename pinocchio::JointModelBase<JointModel>::JointDataDerived JointData;
  typedef typename JointModel::ConfigVector_t ConfigVector_t;
  typedef typename pinocchio::LieGroup<JointModel>::type LieGroupType;

  JointData jdata = jmodel.createData();

  ConfigVector_t ql(ConfigVector_t::Constant(jmodel.nq(),-M_PI));
  ConfigVector_t qu(ConfigVector_t::Constant(jmodel.nq(),M_PI));

  ConfigVector_t q = LieGroupType().randomConfiguration(ql,qu);
  pinocchio::Inertia::Matrix6 I(pinocchio::Inertia::Random().matrix());
  pinocchio::Inertia::Matrix6 I_check = I;

  jmodel.calc(jdata,q);
  jmodel.calc_aba(jdata,I,true);

  Eigen::MatrixXd S = jdata.S.matrix();
  Eigen::MatrixXd U_check = I_check*S;
  Eigen::MatrixXd D_check = S.transpose()*U_check;
  Eigen::MatrixXd Dinv_check = D_check.inverse();
  Eigen::MatrixXd UDinv_check = U_check*Dinv_check;
  Eigen::MatrixXd update_check = U_check*Dinv_check*U_check.transpose();
  I_check -= update_check;

  BOOST_CHECK(jdata.U.isApprox(U_check));
  BOOST_CHECK(jdata.Dinv.isApprox(Dinv_check));
  BOOST_CHECK(jdata.UDinv.isApprox(UDinv_check));

  // Checking the inertia was correctly updated
  // We use isApprox as usual, except for the freeflyer,
  // where the correct result is exacly zero and isApprox would fail.
  // Only for this single case, we use the infinity norm of the difference
  if(jmodel.shortname() == "JointModelFreeFlyer")
    BOOST_CHECK((I-I_check).lpNorm<Eigen::Infinity>() < Eigen::NumTraits<double>::dummy_precision());
  else
    BOOST_CHECK(I.isApprox(I_check));
}

struct TestJointMethods{

  template <typename JointModel>
  void operator()(const pinocchio::JointModelBase<JointModel> &) const
  {
    JointModel jmodel;
    jmodel.setIndexes(0,0,0);

    test_joint_methods(jmodel);
  }

  void operator()(const pinocchio::JointModelBase<pinocchio::JointModelComposite> &) const
  {
    pinocchio::JointModelComposite jmodel_composite;
    jmodel_composite.addJoint(pinocchio::JointModelRX());
    jmodel_composite.addJoint(pinocchio::JointModelRY());
    jmodel_composite.setIndexes(0,0,0);

    //TODO: correct LieGroup
    //test_joint_methods(jmodel_composite);

  }

  void operator()(const pinocchio::JointModelBase<pinocchio::JointModelRevoluteUnaligned> &) const
  {
    pinocchio::JointModelRevoluteUnaligned jmodel(1.5, 1., 0.);
    jmodel.setIndexes(0,0,0);

    test_joint_methods(jmodel);
  }

  void operator()(const pinocchio::JointModelBase<pinocchio::JointModelPrismaticUnaligned> &) const
  {
    pinocchio::JointModelPrismaticUnaligned jmodel(1.5, 1., 0.);
    jmodel.setIndexes(0,0,0);

    test_joint_methods(jmodel);
  }

};

BOOST_AUTO_TEST_CASE( test_joint_basic )
{
  using namespace pinocchio;

  typedef boost::variant< JointModelRX, JointModelRY, JointModelRZ, JointModelRevoluteUnaligned
  , JointModelSpherical, JointModelSphericalZYX
  , JointModelPX, JointModelPY, JointModelPZ
  , JointModelPrismaticUnaligned
  , JointModelFreeFlyer
  , JointModelPlanar
  , JointModelTranslation
  , JointModelRUBX, JointModelRUBY, JointModelRUBZ
  > Variant;

  boost::mpl::for_each<Variant::types>(TestJointMethods());
}

BOOST_AUTO_TEST_CASE ( test_aba_simple )
{
  using namespace Eigen;
  using namespace pinocchio;

  pinocchio::Model model; buildModels::humanoidRandom(model);

  pinocchio::Data data(model);
  pinocchio::Data data_ref(model);

  VectorXd q = VectorXd::Ones(model.nq);
  q.segment<4>(3).normalize();
  VectorXd v = VectorXd::Ones(model.nv);
  VectorXd tau = VectorXd::Zero(model.nv);
  VectorXd a = VectorXd::Ones(model.nv);

  tau = rnea(model, data_ref, q, v, a);
  aba(model, data, q, v, tau);

  for(size_t k = 1; k < (size_t)model.njoints; ++k)
  {
    BOOST_CHECK(data_ref.liMi[k].isApprox(data.liMi[k]));
    BOOST_CHECK(data_ref.v[k].isApprox(data.v[k]));
  }

  BOOST_CHECK(data.ddq.isApprox(a, 1e-12));

}

BOOST_AUTO_TEST_CASE ( test_aba_with_fext )
{
  using namespace Eigen;
  using namespace pinocchio;

  pinocchio::Model model; buildModels::humanoidRandom(model);

  pinocchio::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);

  PINOCCHIO_ALIGNED_STD_VECTOR(Force) fext(model.joints.size(), Force::Random());

  crba(model, data, q);
  computeJointJacobians(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) {
    getJointJacobian(model, data, i, LOCAL, 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;
  using namespace pinocchio;

  pinocchio::Model model; buildModels::humanoidRandom(model);

  pinocchio::Data data(model);
  pinocchio::Data data_ref(model);

  VectorXd q = VectorXd::Ones(model.nq);
  VectorXd v = VectorXd::Ones(model.nv);
  VectorXd tau = VectorXd::Zero(model.nv);
  VectorXd a = VectorXd::Ones(model.nv);

  crba(model, data_ref, q);
  nonLinearEffects(model, data_ref, q, v);
  data_ref.M.triangularView<Eigen::StrictlyLower>()
  = data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();

  tau = data_ref.M * a + data_ref.nle;
  aba(model, data, q, v, tau);

  VectorXd tau_ref = rnea(model, data_ref, q, v, a);
  BOOST_CHECK(tau_ref.isApprox(tau, 1e-12));


  BOOST_CHECK(data.ddq.isApprox(a, 1e-12));

}

BOOST_AUTO_TEST_CASE ( test_computeMinverse )
{
  using namespace Eigen;
  using namespace pinocchio;

  pinocchio::Model model;
  buildModels::humanoidRandom(model);
  model.gravity.setZero();

  pinocchio::Data data(model);
  pinocchio::Data data_ref(model);

  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill(1.);
  VectorXd q = randomConfiguration(model);
  VectorXd v = VectorXd::Random(model.nv);

  crba(model, data_ref, q);
  data_ref.M.triangularView<Eigen::StrictlyLower>()
  = data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();
  MatrixXd Minv_ref(data_ref.M.inverse());

  computeMinverse(model, data, q);


  BOOST_CHECK(data.Minv.topRows<6>().isApprox(Minv_ref.topRows<6>()));

  data.Minv.triangularView<Eigen::StrictlyLower>()
  = data.Minv.transpose().triangularView<Eigen::StrictlyLower>();

  BOOST_CHECK(data.Minv.isApprox(Minv_ref));

//  std::cout << "Minv:\n" << data.Minv.block<10,10>(0,0) << std::endl;
//  std::cout << "Minv_ref:\n" << Minv_ref.block<10,10>(0,0) << std::endl;
//
//  std::cout << "Minv:\n" << data.Minv.bottomRows<10>() << std::endl;
//  std::cout << "Minv_ref:\n" << Minv_ref.bottomRows<10>() << std::endl;

}

BOOST_AUTO_TEST_CASE(test_multiple_calls)
{
  using namespace Eigen;
  using namespace pinocchio;

  Model model;
  buildModels::humanoidRandom(model);

  Data data1(model), data2(model);

  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill(1.);
  VectorXd q = randomConfiguration(model);

  computeMinverse(model,data1,q);
  data2 = data1;

  for(int k = 0; k < 20; ++k)
  {
    computeMinverse(model,data1,q);
  }

  BOOST_CHECK(data1.Minv.isApprox(data2.Minv));
}

BOOST_AUTO_TEST_SUITE_END ()


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2016-2020 CNRS INRIA
3			//
4
5			#include "pinocchio/spatial/fwd.hpp"
6			#include "pinocchio/spatial/se3.hpp"
7			#include "pinocchio/multibody/visitor.hpp"
8			#include "pinocchio/multibody/model.hpp"
9			#include "pinocchio/multibody/data.hpp"
10			#include "pinocchio/algorithm/aba.hpp"
11			#include "pinocchio/algorithm/rnea.hpp"
12			#include "pinocchio/algorithm/jacobian.hpp"
13			#include "pinocchio/algorithm/joint-configuration.hpp"
14			#include "pinocchio/algorithm/crba.hpp"
15			#include "pinocchio/parsers/sample-models.hpp"
16
17			#include "pinocchio/algorithm/compute-all-terms.hpp"
18			#include "pinocchio/utils/timer.hpp"
19
20			#include <iostream>
21
22			#include <boost/test/unit_test.hpp>
23			#include <boost/utility/binary.hpp>
24
25			BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
26
27			template<typename JointModel>
28		32	void test_joint_methods(const pinocchio::JointModelBase<JointModel> & jmodel)
29			{
30			typedef typename pinocchio::JointModelBase<JointModel>::JointDataDerived JointData;
31			typedef typename JointModel::ConfigVector_t ConfigVector_t;
32			typedef typename pinocchio::LieGroup<JointModel>::type LieGroupType;
33
34	✓✗	32	JointData jdata = jmodel.createData();
35
36	✓✗✓✗ ✓✗	32	ConfigVector_t ql(ConfigVector_t::Constant(jmodel.nq(),-M_PI));
37	✓✗✓✗ ✓✗	32	ConfigVector_t qu(ConfigVector_t::Constant(jmodel.nq(),M_PI));
38
39	✓✗✓✗	32	ConfigVector_t q = LieGroupType().randomConfiguration(ql,qu);
40	✓✗✓✗	32	pinocchio::Inertia::Matrix6 I(pinocchio::Inertia::Random().matrix());
41	✓✗	32	pinocchio::Inertia::Matrix6 I_check = I;
42
43	✓✗	32	jmodel.calc(jdata,q);
44	✓✗	32	jmodel.calc_aba(jdata,I,true);
45
46	✓✗✓✗	64	Eigen::MatrixXd S = jdata.S.matrix();
47	✓✗✓✗	64	Eigen::MatrixXd U_check = I_check*S;
48	✓✗✓✗ ✓✗	64	Eigen::MatrixXd D_check = S.transpose()*U_check;
49	✓✗✓✗	64	Eigen::MatrixXd Dinv_check = D_check.inverse();
50	✓✗✓✗	64	Eigen::MatrixXd UDinv_check = U_check*Dinv_check;
51	✓✗✓✗ ✓✗✓✗	64	Eigen::MatrixXd update_check = U_checkDinv_checkU_check.transpose();
52	✓✗	32	I_check -= update_check;
53
54	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	32	BOOST_CHECK(jdata.U.isApprox(U_check));
55	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	32	BOOST_CHECK(jdata.Dinv.isApprox(Dinv_check));
56	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	32	BOOST_CHECK(jdata.UDinv.isApprox(UDinv_check));
57
58			// Checking the inertia was correctly updated
59			// We use isApprox as usual, except for the freeflyer,
60			// where the correct result is exacly zero and isApprox would fail.
61			// Only for this single case, we use the infinity norm of the difference
62	✓✗✓✓	32	if(jmodel.shortname() == "JointModelFreeFlyer")
63	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK((I-I_check).lpNorm<Eigen::Infinity>() < Eigen::NumTraits<double>::dummy_precision());
64			else
65	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	30	BOOST_CHECK(I.isApprox(I_check));
66		32	}
67
68			struct TestJointMethods{
69
70			template <typename JointModel>
71		28	void operator()(const pinocchio::JointModelBase<JointModel> &) const
72			{
73	✓✗	28	JointModel jmodel;
74	✓✗	28	jmodel.setIndexes(0,0,0);
75
76	✓✗	28	test_joint_methods(jmodel);
77		28	}
78
79			void operator()(const pinocchio::JointModelBase<pinocchio::JointModelComposite> &) const
80			{
81			pinocchio::JointModelComposite jmodel_composite;
82			jmodel_composite.addJoint(pinocchio::JointModelRX());
83			jmodel_composite.addJoint(pinocchio::JointModelRY());
84			jmodel_composite.setIndexes(0,0,0);
85
86			//TODO: correct LieGroup
87			//test_joint_methods(jmodel_composite);
88
89			}
90
91		1	void operator()(const pinocchio::JointModelBase<pinocchio::JointModelRevoluteUnaligned> &) const
92			{
93	✓✗	1	pinocchio::JointModelRevoluteUnaligned jmodel(1.5, 1., 0.);
94	✓✗	1	jmodel.setIndexes(0,0,0);
95
96	✓✗	1	test_joint_methods(jmodel);
97		1	}
98
99		1	void operator()(const pinocchio::JointModelBase<pinocchio::JointModelPrismaticUnaligned> &) const
100			{
101	✓✗	1	pinocchio::JointModelPrismaticUnaligned jmodel(1.5, 1., 0.);
102	✓✗	1	jmodel.setIndexes(0,0,0);
103
104	✓✗	1	test_joint_methods(jmodel);
105		1	}
106
107			};
108
109	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE( test_joint_basic )
110			{
111			using namespace pinocchio;
112
113			typedef boost::variant< JointModelRX, JointModelRY, JointModelRZ, JointModelRevoluteUnaligned
114			, JointModelSpherical, JointModelSphericalZYX
115			, JointModelPX, JointModelPY, JointModelPZ
116			, JointModelPrismaticUnaligned
117			, JointModelFreeFlyer
118			, JointModelPlanar
119			, JointModelTranslation
120			, JointModelRUBX, JointModelRUBY, JointModelRUBZ
121			> Variant;
122
123	✓✗	2	boost::mpl::for_each<Variant::types>(TestJointMethods());
124		2	}
125
126	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE ( test_aba_simple )
127			{
128			using namespace Eigen;
129			using namespace pinocchio;
130
131	✓✗✓✗	4	pinocchio::Model model; buildModels::humanoidRandom(model);
132
133	✓✗	4	pinocchio::Data data(model);
134	✓✗	4	pinocchio::Data data_ref(model);
135
136	✓✗✓✗	4	VectorXd q = VectorXd::Ones(model.nq);
137	✓✗✓✗	2	q.segment<4>(3).normalize();
138	✓✗✓✗	4	VectorXd v = VectorXd::Ones(model.nv);
139	✓✗✓✗	4	VectorXd tau = VectorXd::Zero(model.nv);
140	✓✗✓✗	4	VectorXd a = VectorXd::Ones(model.nv);
141
142	✓✗✓✗	2	tau = rnea(model, data_ref, q, v, a);
143	✓✗	2	aba(model, data, q, v, tau);
144
145	✓✓	56	for(size_t k = 1; k < (size_t)model.njoints; ++k)
146			{
147	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	54	BOOST_CHECK(data_ref.liMi[k].isApprox(data.liMi[k]));
148	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	54	BOOST_CHECK(data_ref.v[k].isApprox(data.v[k]));
149			}
150
151	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(data.ddq.isApprox(a, 1e-12));
152
153		2	}
154
155	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE ( test_aba_with_fext )
156			{
157			using namespace Eigen;
158			using namespace pinocchio;
159
160	✓✗✓✗	4	pinocchio::Model model; buildModels::humanoidRandom(model);
161
162	✓✗	4	pinocchio::Data data(model);
163
164	✓✗✓✗	4	VectorXd q = VectorXd::Random(model.nq);
165	✓✗✓✗	2	q.segment<4>(3).normalize();
166	✓✗✓✗	4	VectorXd v = VectorXd::Random(model.nv);
167	✓✗✓✗	4	VectorXd a = VectorXd::Random(model.nv);
168
169	✓✗✓✗	4	PINOCCHIO_ALIGNED_STD_VECTOR(Force) fext(model.joints.size(), Force::Random());
170
171	✓✗	2	crba(model, data, q);
172	✓✗	2	computeJointJacobians(model, data, q);
173	✓✗	2	nonLinearEffects(model, data, q, v);
174	✓✗	2	data.M.triangularView<Eigen::StrictlyLower>()
175	✓✗✓✗ ✓✗	4	= data.M.transpose().triangularView<Eigen::StrictlyLower>();
176
177
178	✓✗✓✗ ✓✗	4	VectorXd tau = data.M * a + data.nle;
179	✓✗✓✗	4	Data::Matrix6x J = Data::Matrix6x::Zero(6, model.nv);
180	✓✓	56	for(Model::Index i=1;i<(Model::Index)model.njoints;++i) {
181	✓✗	54	getJointJacobian(model, data, i, LOCAL, J);
182	✓✗✓✗ ✓✗✓✗	54	tau -= J.transpose()*fext[i].toVector();
183	✓✗	54	J.setZero();
184			}
185	✓✗	2	aba(model, data, q, v, tau, fext);
186
187	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(data.ddq.isApprox(a, 1e-12));
188		2	}
189
190	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE ( test_aba_vs_rnea )
191			{
192			using namespace Eigen;
193			using namespace pinocchio;
194
195	✓✗✓✗	4	pinocchio::Model model; buildModels::humanoidRandom(model);
196
197	✓✗	4	pinocchio::Data data(model);
198	✓✗	4	pinocchio::Data data_ref(model);
199
200	✓✗✓✗	4	VectorXd q = VectorXd::Ones(model.nq);
201	✓✗✓✗	4	VectorXd v = VectorXd::Ones(model.nv);
202	✓✗✓✗	4	VectorXd tau = VectorXd::Zero(model.nv);
203	✓✗✓✗	4	VectorXd a = VectorXd::Ones(model.nv);
204
205	✓✗	2	crba(model, data_ref, q);
206	✓✗	2	nonLinearEffects(model, data_ref, q, v);
207	✓✗	2	data_ref.M.triangularView<Eigen::StrictlyLower>()
208	✓✗✓✗ ✓✗	4	= data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();
209
210	✓✗✓✗ ✓✗	2	tau = data_ref.M * a + data_ref.nle;
211	✓✗	2	aba(model, data, q, v, tau);
212
213	✓✗✓✗	4	VectorXd tau_ref = rnea(model, data_ref, q, v, a);
214	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(tau_ref.isApprox(tau, 1e-12));
215
216
217	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(data.ddq.isApprox(a, 1e-12));
218
219		2	}
220
221	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE ( test_computeMinverse )
222			{
223			using namespace Eigen;
224			using namespace pinocchio;
225
226	✓✗	4	pinocchio::Model model;
227	✓✗	2	buildModels::humanoidRandom(model);
228	✓✗	2	model.gravity.setZero();
229
230	✓✗	4	pinocchio::Data data(model);
231	✓✗	4	pinocchio::Data data_ref(model);
232
233	✓✗✓✗	2	model.lowerPositionLimit.head<3>().fill(-1.);
234	✓✗✓✗	2	model.upperPositionLimit.head<3>().fill(1.);
235	✓✗	4	VectorXd q = randomConfiguration(model);
236	✓✗✓✗	4	VectorXd v = VectorXd::Random(model.nv);
237
238	✓✗	2	crba(model, data_ref, q);
239	✓✗	2	data_ref.M.triangularView<Eigen::StrictlyLower>()
240	✓✗✓✗ ✓✗	4	= data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();
241	✓✗✓✗	4	MatrixXd Minv_ref(data_ref.M.inverse());
242
243	✓✗	2	computeMinverse(model, data, q);
244
245
246	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(data.Minv.topRows<6>().isApprox(Minv_ref.topRows<6>()));
247
248	✓✗	2	data.Minv.triangularView<Eigen::StrictlyLower>()
249	✓✗✓✗ ✓✗	4	= data.Minv.transpose().triangularView<Eigen::StrictlyLower>();
250
251	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(data.Minv.isApprox(Minv_ref));
252
253			// std::cout << "Minv:\n" << data.Minv.block<10,10>(0,0) << std::endl;
254			// std::cout << "Minv_ref:\n" << Minv_ref.block<10,10>(0,0) << std::endl;
255			//
256			// std::cout << "Minv:\n" << data.Minv.bottomRows<10>() << std::endl;
257			// std::cout << "Minv_ref:\n" << Minv_ref.bottomRows<10>() << std::endl;
258
259		2	}
260
261	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(test_multiple_calls)
262			{
263			using namespace Eigen;
264			using namespace pinocchio;
265
266	✓✗	4	Model model;
267	✓✗	2	buildModels::humanoidRandom(model);
268
269	✓✗✓✗	4	Data data1(model), data2(model);
270
271	✓✗✓✗	2	model.lowerPositionLimit.head<3>().fill(-1.);
272	✓✗✓✗	2	model.upperPositionLimit.head<3>().fill(1.);
273	✓✗	4	VectorXd q = randomConfiguration(model);
274
275	✓✗	2	computeMinverse(model,data1,q);
276	✓✗	2	data2 = data1;
277
278	✓✓	42	for(int k = 0; k < 20; ++k)
279			{
280	✓✗	40	computeMinverse(model,data1,q);
281			}
282
283	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(data1.Minv.isApprox(data2.Minv));
284		2	}
285
286			BOOST_AUTO_TEST_SUITE_END ()