Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	unittest/test_contacts.cpp	Lines:	119	119	100.0 %
Date:	2024-02-13 11:12:33	Branches:	337	666	50.6 %


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2019-2023, University of Edinburgh, Heriot-Watt University
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#define BOOST_TEST_NO_MAIN
#define BOOST_TEST_ALTERNATIVE_INIT_API

#include <boost/make_shared.hpp>
#include <boost/shared_ptr.hpp>
#include <pinocchio/algorithm/frames.hpp>
#include <pinocchio/algorithm/kinematics-derivatives.hpp>

#include "crocoddyl/multibody/contacts/contact-1d.hpp"
#include "crocoddyl/multibody/contacts/contact-2d.hpp"
#include "crocoddyl/multibody/contacts/contact-3d.hpp"
#include "crocoddyl/multibody/contacts/contact-6d.hpp"
#include "factory/contact.hpp"
#include "unittest_common.hpp"

using namespace crocoddyl::unittest;
using namespace boost::unit_test;

//----------------------------------------------------------------------------//

void test_construct_data(ContactModelTypes::Type contact_type,
                         PinocchioModelTypes::Type model_type) {
  // create the model
  ContactModelFactory factory;
  boost::shared_ptr<crocoddyl::ContactModelAbstract> model =
      factory.create(contact_type, model_type, Eigen::Vector2d::Random());

  // Run the print function
  std::ostringstream tmp;
  tmp << *model;

  // create the corresponding data object
  const boost::shared_ptr<pinocchio::Model>& pinocchio_model =
      model->get_state()->get_pinocchio();
  pinocchio::Data pinocchio_data(*pinocchio_model.get());
  boost::shared_ptr<crocoddyl::ContactDataAbstract> data =
      model->createData(&pinocchio_data);
}

void test_calc_fetch_jacobians(ContactModelTypes::Type contact_type,
                               PinocchioModelTypes::Type model_type) {
  // create the model
  ContactModelFactory factory;
  boost::shared_ptr<crocoddyl::ContactModelAbstract> model =
      factory.create(contact_type, model_type, Eigen::Vector2d::Random());

  // create the corresponding data object
  const boost::shared_ptr<pinocchio::Model>& pinocchio_model =
      model->get_state()->get_pinocchio();
  pinocchio::Data pinocchio_data(*pinocchio_model.get());
  boost::shared_ptr<crocoddyl::ContactDataAbstract> data =
      model->createData(&pinocchio_data);

  // Compute the jacobian and check that the impulse model fetch it.
  Eigen::VectorXd x = model->get_state()->rand();
  crocoddyl::unittest::updateAllPinocchio(pinocchio_model.get(),
                                          &pinocchio_data, x);

  // Getting the jacobian from the model
  model->calc(data, x);

  // Check that only the Jacobian has been filled
  BOOST_CHECK(!data->Jc.isZero());
  if (model_type !=
      PinocchioModelTypes::Hector) {  // this is due to Hector is a single rigid
                                      // body system.
    BOOST_CHECK(!data->a0.isZero());
  }
  BOOST_CHECK(data->da0_dx.isZero());
  BOOST_CHECK(data->f.toVector().isZero());
  BOOST_CHECK(data->df_dx.isZero());
  BOOST_CHECK(data->df_du.isZero());
}

void test_calc_diff_fetch_derivatives(ContactModelTypes::Type contact_type,
                                      PinocchioModelTypes::Type model_type) {
  // create the model
  ContactModelFactory factory;
  boost::shared_ptr<crocoddyl::ContactModelAbstract> model =
      factory.create(contact_type, model_type, Eigen::Vector2d::Random());

  // create the corresponding data object
  const boost::shared_ptr<pinocchio::Model>& pinocchio_model =
      model->get_state()->get_pinocchio();
  pinocchio::Data pinocchio_data(*pinocchio_model.get());
  boost::shared_ptr<crocoddyl::ContactDataAbstract> data =
      model->createData(&pinocchio_data);

  // Compute the jacobian and check that the impulse model fetch it.
  Eigen::VectorXd x = model->get_state()->rand();
  crocoddyl::unittest::updateAllPinocchio(pinocchio_model.get(),
                                          &pinocchio_data, x);

  // Getting the jacobian from the model
  model->calc(data, x);
  model->calcDiff(data, x);

  // Check that nothing has been computed and that all value are initialized to
  // 0
  BOOST_CHECK(!data->Jc.isZero());
  if (model_type !=
      PinocchioModelTypes::Hector) {  // this is due to Hector is a single rigid
                                      // body system.
    BOOST_CHECK(!data->a0.isZero());
    BOOST_CHECK(!data->da0_dx.isZero());
  }
  BOOST_CHECK(data->f.toVector().isZero());
  BOOST_CHECK(data->df_dx.isZero());
  BOOST_CHECK(data->df_du.isZero());
}

void test_update_force(ContactModelTypes::Type contact_type,
                       PinocchioModelTypes::Type model_type) {
  // create the model
  ContactModelFactory factory;
  boost::shared_ptr<crocoddyl::ContactModelAbstract> model =
      factory.create(contact_type, model_type, Eigen::Vector2d::Random());

  // create the corresponding data object
  const boost::shared_ptr<pinocchio::Model>& pinocchio_model =
      model->get_state()->get_pinocchio();
  pinocchio::Data pinocchio_data(*pinocchio_model.get());
  boost::shared_ptr<crocoddyl::ContactDataAbstract> data =
      model->createData(&pinocchio_data);

  // Create a random force and update it
  Eigen::VectorXd f = Eigen::VectorXd::Random(data->Jc.rows());
  model->updateForce(data, f);
  boost::shared_ptr<crocoddyl::ContactModel3D> m =
      boost::static_pointer_cast<crocoddyl::ContactModel3D>(model);

  // Check that nothing has been computed and that all value are initialized to
  // 0
  BOOST_CHECK(data->Jc.isZero());
  BOOST_CHECK(data->a0.isZero());
  BOOST_CHECK(data->da0_dx.isZero());
  BOOST_CHECK(!data->f.toVector().isZero());
  BOOST_CHECK(!data->fext.toVector().isZero());
  BOOST_CHECK(data->df_dx.isZero());
  BOOST_CHECK(data->df_du.isZero());
}

void test_update_force_diff(ContactModelTypes::Type contact_type,
                            PinocchioModelTypes::Type model_type) {
  // create the model
  ContactModelFactory factory;
  boost::shared_ptr<crocoddyl::ContactModelAbstract> model =
      factory.create(contact_type, model_type, Eigen::Vector2d::Random());

  // create the corresponding data object
  const boost::shared_ptr<pinocchio::Model>& pinocchio_model =
      model->get_state()->get_pinocchio();
  pinocchio::Data pinocchio_data(*pinocchio_model.get());
  boost::shared_ptr<crocoddyl::ContactDataAbstract> data =
      model->createData(&pinocchio_data);

  // Create a random force and update it
  Eigen::MatrixXd df_dx =
      Eigen::MatrixXd::Random(data->df_dx.rows(), data->df_dx.cols());
  Eigen::MatrixXd df_du =
      Eigen::MatrixXd::Random(data->df_du.rows(), data->df_du.cols());
  model->updateForceDiff(data, df_dx, df_du);

  // Check that nothing has been computed and that all value are initialized to
  // 0
  BOOST_CHECK(data->Jc.isZero());
  BOOST_CHECK(data->a0.isZero());
  BOOST_CHECK(data->da0_dx.isZero());
  BOOST_CHECK(data->f.toVector().isZero());
  BOOST_CHECK(data->fext.toVector().isZero());
  BOOST_CHECK(!data->df_dx.isZero());
  BOOST_CHECK(!data->df_du.isZero());
}

void test_partial_derivatives_against_numdiff(
    ContactModelTypes::Type contact_type,
    PinocchioModelTypes::Type model_type) {
  using namespace boost::placeholders;

  // create the model
  ContactModelFactory factory;
  boost::shared_ptr<crocoddyl::ContactModelAbstract> model =
      factory.create(contact_type, model_type, Eigen::Vector2d::Random());

  // create the corresponding data object
  pinocchio::Model& pinocchio_model =
      *model->get_state()->get_pinocchio().get();
  pinocchio::Data pinocchio_data(pinocchio_model);
  boost::shared_ptr<crocoddyl::ContactDataAbstract> data =
      model->createData(&pinocchio_data);

  // Create the equivalent num diff model and data.
  crocoddyl::ContactModelNumDiff model_num_diff(model);
  const boost::shared_ptr<crocoddyl::ContactDataAbstract>& data_num_diff =
      model_num_diff.createData(&pinocchio_data);

  // Generating random values for the state
  const Eigen::VectorXd x = model->get_state()->rand();

  // Compute all the pinocchio function needed for the models.
  crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);

  // set the function that needs to be called at every step of the numdiff
  std::vector<crocoddyl::ContactModelNumDiff::ReevaluationFunction> reevals;
  reevals.push_back(boost::bind(&crocoddyl::unittest::updateAllPinocchio,
                                &pinocchio_model, &pinocchio_data, _1, _2));
  model_num_diff.set_reevals(reevals);

  // Computing the contact derivatives
  model->calc(data, x);
  model->calcDiff(data, x);
  model_num_diff.calc(data_num_diff, x);
  model_num_diff.calcDiff(data_num_diff, x);
  // Tolerance defined as in
  // http://www.it.uom.gr/teaching/linearalgebra/NumericalRecipiesInC/c5-7.pdf
  double tol = std::pow(model_num_diff.get_disturbance(), 1. / 3.);
  BOOST_CHECK((data->da0_dx - data_num_diff->da0_dx).isZero(tol));
}

//----------------------------------------------------------------------------//

void register_contact_model_unit_tests(ContactModelTypes::Type contact_type,
                                       PinocchioModelTypes::Type model_type) {
  boost::test_tools::output_test_stream test_name;
  test_name << "test_" << contact_type << "_" << model_type;
  std::cout << "Running " << test_name.str() << std::endl;
  test_suite* ts = BOOST_TEST_SUITE(test_name.str());
  ts->add(BOOST_TEST_CASE(
      boost::bind(&test_construct_data, contact_type, model_type)));
  ts->add(BOOST_TEST_CASE(
      boost::bind(&test_calc_fetch_jacobians, contact_type, model_type)));
  ts->add(BOOST_TEST_CASE(boost::bind(&test_calc_diff_fetch_derivatives,
                                      contact_type, model_type)));
  ts->add(BOOST_TEST_CASE(
      boost::bind(&test_update_force, contact_type, model_type)));
  ts->add(BOOST_TEST_CASE(
      boost::bind(&test_update_force_diff, contact_type, model_type)));
  ts->add(BOOST_TEST_CASE(boost::bind(&test_partial_derivatives_against_numdiff,
                                      contact_type, model_type)));
  framework::master_test_suite().add(ts);
}

bool init_function() {
  for (size_t contact_type = 0; contact_type < ContactModelTypes::all.size();
       ++contact_type) {
    for (size_t model_type = 0; model_type < PinocchioModelTypes::all.size();
         ++model_type) {
      register_contact_model_unit_tests(ContactModelTypes::all[contact_type],
                                        PinocchioModelTypes::all[model_type]);
    }
  }
  return true;
}

int main(int argc, char** argv) {
  return ::boost::unit_test::unit_test_main(&init_function, argc, argv);
}


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2019-2023, University of Edinburgh, Heriot-Watt University
5			// Copyright note valid unless otherwise stated in individual files.
6			// All rights reserved.
7			///////////////////////////////////////////////////////////////////////////////
8
9			#define BOOST_TEST_NO_MAIN
10			#define BOOST_TEST_ALTERNATIVE_INIT_API
11
12			#include <boost/make_shared.hpp>
13			#include <boost/shared_ptr.hpp>
14			#include <pinocchio/algorithm/frames.hpp>
15			#include <pinocchio/algorithm/kinematics-derivatives.hpp>
16
17			#include "crocoddyl/multibody/contacts/contact-1d.hpp"
18			#include "crocoddyl/multibody/contacts/contact-2d.hpp"
19			#include "crocoddyl/multibody/contacts/contact-3d.hpp"
20			#include "crocoddyl/multibody/contacts/contact-6d.hpp"
21			#include "factory/contact.hpp"
22			#include "unittest_common.hpp"
23
24			using namespace crocoddyl::unittest;
25			using namespace boost::unit_test;
26
27			//----------------------------------------------------------------------------//
28
29		50	void test_construct_data(ContactModelTypes::Type contact_type,
30			PinocchioModelTypes::Type model_type) {
31			// create the model
32	✓✗	100	ContactModelFactory factory;
33			boost::shared_ptr<crocoddyl::ContactModelAbstract> model =
34	✓✗✓✗ ✓✗✓✗	100	factory.create(contact_type, model_type, Eigen::Vector2d::Random());
35
36			// Run the print function
37	✓✗	100	std::ostringstream tmp;
38	✓✗	50	tmp << *model;
39
40			// create the corresponding data object
41			const boost::shared_ptr<pinocchio::Model>& pinocchio_model =
42		50	model->get_state()->get_pinocchio();
43	✓✗	100	pinocchio::Data pinocchio_data(*pinocchio_model.get());
44			boost::shared_ptr<crocoddyl::ContactDataAbstract> data =
45	✓✗	50	model->createData(&pinocchio_data);
46		50	}
47
48		50	void test_calc_fetch_jacobians(ContactModelTypes::Type contact_type,
49			PinocchioModelTypes::Type model_type) {
50			// create the model
51	✓✗	100	ContactModelFactory factory;
52			boost::shared_ptr<crocoddyl::ContactModelAbstract> model =
53	✓✗✓✗ ✓✗✓✗	100	factory.create(contact_type, model_type, Eigen::Vector2d::Random());
54
55			// create the corresponding data object
56			const boost::shared_ptr<pinocchio::Model>& pinocchio_model =
57		50	model->get_state()->get_pinocchio();
58	✓✗	100	pinocchio::Data pinocchio_data(*pinocchio_model.get());
59			boost::shared_ptr<crocoddyl::ContactDataAbstract> data =
60	✓✗	100	model->createData(&pinocchio_data);
61
62			// Compute the jacobian and check that the impulse model fetch it.
63	✓✗	100	Eigen::VectorXd x = model->get_state()->rand();
64	✓✗✓✗	50	crocoddyl::unittest::updateAllPinocchio(pinocchio_model.get(),
65			&pinocchio_data, x);
66
67			// Getting the jacobian from the model
68	✓✗✓✗	50	model->calc(data, x);
69
70			// Check that only the Jacobian has been filled
71	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	50	BOOST_CHECK(!data->Jc.isZero());
72	✓✓	50	if (model_type !=
73			PinocchioModelTypes::Hector) { // this is due to Hector is a single rigid
74			// body system.
75	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	40	BOOST_CHECK(!data->a0.isZero());
76			}
77	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	50	BOOST_CHECK(data->da0_dx.isZero());
78	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	50	BOOST_CHECK(data->f.toVector().isZero());
79	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	50	BOOST_CHECK(data->df_dx.isZero());
80	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	50	BOOST_CHECK(data->df_du.isZero());
81		50	}
82
83		50	void test_calc_diff_fetch_derivatives(ContactModelTypes::Type contact_type,
84			PinocchioModelTypes::Type model_type) {
85			// create the model
86	✓✗	100	ContactModelFactory factory;
87			boost::shared_ptr<crocoddyl::ContactModelAbstract> model =
88	✓✗✓✗ ✓✗✓✗	100	factory.create(contact_type, model_type, Eigen::Vector2d::Random());
89
90			// create the corresponding data object
91			const boost::shared_ptr<pinocchio::Model>& pinocchio_model =
92		50	model->get_state()->get_pinocchio();
93	✓✗	100	pinocchio::Data pinocchio_data(*pinocchio_model.get());
94			boost::shared_ptr<crocoddyl::ContactDataAbstract> data =
95	✓✗	100	model->createData(&pinocchio_data);
96
97			// Compute the jacobian and check that the impulse model fetch it.
98	✓✗	100	Eigen::VectorXd x = model->get_state()->rand();
99	✓✗✓✗	50	crocoddyl::unittest::updateAllPinocchio(pinocchio_model.get(),
100			&pinocchio_data, x);
101
102			// Getting the jacobian from the model
103	✓✗✓✗	50	model->calc(data, x);
104	✓✗✓✗	50	model->calcDiff(data, x);
105
106			// Check that nothing has been computed and that all value are initialized to
107			// 0
108	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	50	BOOST_CHECK(!data->Jc.isZero());
109	✓✓	50	if (model_type !=
110			PinocchioModelTypes::Hector) { // this is due to Hector is a single rigid
111			// body system.
112	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	40	BOOST_CHECK(!data->a0.isZero());
113	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	40	BOOST_CHECK(!data->da0_dx.isZero());
114			}
115	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	50	BOOST_CHECK(data->f.toVector().isZero());
116	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	50	BOOST_CHECK(data->df_dx.isZero());
117	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	50	BOOST_CHECK(data->df_du.isZero());
118		50	}
119
120		50	void test_update_force(ContactModelTypes::Type contact_type,
121			PinocchioModelTypes::Type model_type) {
122			// create the model
123	✓✗	100	ContactModelFactory factory;
124			boost::shared_ptr<crocoddyl::ContactModelAbstract> model =
125	✓✗✓✗ ✓✗✓✗	100	factory.create(contact_type, model_type, Eigen::Vector2d::Random());
126
127			// create the corresponding data object
128			const boost::shared_ptr<pinocchio::Model>& pinocchio_model =
129		50	model->get_state()->get_pinocchio();
130	✓✗	100	pinocchio::Data pinocchio_data(*pinocchio_model.get());
131			boost::shared_ptr<crocoddyl::ContactDataAbstract> data =
132	✓✗	100	model->createData(&pinocchio_data);
133
134			// Create a random force and update it
135	✓✗✓✗ ✓✗	100	Eigen::VectorXd f = Eigen::VectorXd::Random(data->Jc.rows());
136	✓✗	50	model->updateForce(data, f);
137			boost::shared_ptr<crocoddyl::ContactModel3D> m =
138		100	boost::static_pointer_cast<crocoddyl::ContactModel3D>(model);
139
140			// Check that nothing has been computed and that all value are initialized to
141			// 0
142	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	50	BOOST_CHECK(data->Jc.isZero());
143	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	50	BOOST_CHECK(data->a0.isZero());
144	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	50	BOOST_CHECK(data->da0_dx.isZero());
145	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	50	BOOST_CHECK(!data->f.toVector().isZero());
146	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	50	BOOST_CHECK(!data->fext.toVector().isZero());
147	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	50	BOOST_CHECK(data->df_dx.isZero());
148	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	50	BOOST_CHECK(data->df_du.isZero());
149		50	}
150
151		50	void test_update_force_diff(ContactModelTypes::Type contact_type,
152			PinocchioModelTypes::Type model_type) {
153			// create the model
154	✓✗	100	ContactModelFactory factory;
155			boost::shared_ptr<crocoddyl::ContactModelAbstract> model =
156	✓✗✓✗ ✓✗✓✗	100	factory.create(contact_type, model_type, Eigen::Vector2d::Random());
157
158			// create the corresponding data object
159			const boost::shared_ptr<pinocchio::Model>& pinocchio_model =
160		50	model->get_state()->get_pinocchio();
161	✓✗	100	pinocchio::Data pinocchio_data(*pinocchio_model.get());
162			boost::shared_ptr<crocoddyl::ContactDataAbstract> data =
163	✓✗	100	model->createData(&pinocchio_data);
164
165			// Create a random force and update it
166			Eigen::MatrixXd df_dx =
167	✓✗✓✗ ✓✗✓✗	100	Eigen::MatrixXd::Random(data->df_dx.rows(), data->df_dx.cols());
168			Eigen::MatrixXd df_du =
169	✓✗✓✗ ✓✗✓✗	100	Eigen::MatrixXd::Random(data->df_du.rows(), data->df_du.cols());
170	✓✗	50	model->updateForceDiff(data, df_dx, df_du);
171
172			// Check that nothing has been computed and that all value are initialized to
173			// 0
174	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	50	BOOST_CHECK(data->Jc.isZero());
175	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	50	BOOST_CHECK(data->a0.isZero());
176	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	50	BOOST_CHECK(data->da0_dx.isZero());
177	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	50	BOOST_CHECK(data->f.toVector().isZero());
178	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	50	BOOST_CHECK(data->fext.toVector().isZero());
179	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	50	BOOST_CHECK(!data->df_dx.isZero());
180	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	50	BOOST_CHECK(!data->df_du.isZero());
181		50	}
182
183		50	void test_partial_derivatives_against_numdiff(
184			ContactModelTypes::Type contact_type,
185			PinocchioModelTypes::Type model_type) {
186			using namespace boost::placeholders;
187
188			// create the model
189	✓✗	100	ContactModelFactory factory;
190			boost::shared_ptr<crocoddyl::ContactModelAbstract> model =
191	✓✗✓✗ ✓✗✓✗	100	factory.create(contact_type, model_type, Eigen::Vector2d::Random());
192
193			// create the corresponding data object
194			pinocchio::Model& pinocchio_model =
195		50	*model->get_state()->get_pinocchio().get();
196	✓✗	100	pinocchio::Data pinocchio_data(pinocchio_model);
197			boost::shared_ptr<crocoddyl::ContactDataAbstract> data =
198	✓✗	100	model->createData(&pinocchio_data);
199
200			// Create the equivalent num diff model and data.
201	✓✗	100	crocoddyl::ContactModelNumDiff model_num_diff(model);
202			const boost::shared_ptr<crocoddyl::ContactDataAbstract>& data_num_diff =
203	✓✗	100	model_num_diff.createData(&pinocchio_data);
204
205			// Generating random values for the state
206	✓✗	100	const Eigen::VectorXd x = model->get_state()->rand();
207
208			// Compute all the pinocchio function needed for the models.
209	✓✗✓✗	50	crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
210
211			// set the function that needs to be called at every step of the numdiff
212		100	std::vector<crocoddyl::ContactModelNumDiff::ReevaluationFunction> reevals;
213	✓✗✓✗ ✓✗	50	reevals.push_back(boost::bind(&crocoddyl::unittest::updateAllPinocchio,
214			&pinocchio_model, &pinocchio_data, _1, _2));
215	✓✗	50	model_num_diff.set_reevals(reevals);
216
217			// Computing the contact derivatives
218	✓✗✓✗	50	model->calc(data, x);
219	✓✗✓✗	50	model->calcDiff(data, x);
220	✓✗✓✗	50	model_num_diff.calc(data_num_diff, x);
221	✓✗✓✗	50	model_num_diff.calcDiff(data_num_diff, x);
222			// Tolerance defined as in
223			// http://www.it.uom.gr/teaching/linearalgebra/NumericalRecipiesInC/c5-7.pdf
224		50	double tol = std::pow(model_num_diff.get_disturbance(), 1. / 3.);
225	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	50	BOOST_CHECK((data->da0_dx - data_num_diff->da0_dx).isZero(tol));
226		50	}
227
228			//----------------------------------------------------------------------------//
229
230		50	void register_contact_model_unit_tests(ContactModelTypes::Type contact_type,
231			PinocchioModelTypes::Type model_type) {
232	✓✗✓✗	100	boost::test_tools::output_test_stream test_name;
233	✓✗✓✗ ✓✗✓✗	50	test_name << "test_" << contact_type << "_" << model_type;
234	✓✗✓✗ ✓✗✓✗	50	std::cout << "Running " << test_name.str() << std::endl;
235	✓✗✓✗ ✓✗✓✗	50	test_suite* ts = BOOST_TEST_SUITE(test_name.str());
236	✓✗✓✗ ✓✗✓✗ ✓✗	50	ts->add(BOOST_TEST_CASE(
237			boost::bind(&test_construct_data, contact_type, model_type)));
238	✓✗✓✗ ✓✗✓✗ ✓✗	50	ts->add(BOOST_TEST_CASE(
239			boost::bind(&test_calc_fetch_jacobians, contact_type, model_type)));
240	✓✗✓✗ ✓✗✓✗ ✓✗	50	ts->add(BOOST_TEST_CASE(boost::bind(&test_calc_diff_fetch_derivatives,
241			contact_type, model_type)));
242	✓✗✓✗ ✓✗✓✗ ✓✗	50	ts->add(BOOST_TEST_CASE(
243			boost::bind(&test_update_force, contact_type, model_type)));
244	✓✗✓✗ ✓✗✓✗ ✓✗	50	ts->add(BOOST_TEST_CASE(
245			boost::bind(&test_update_force_diff, contact_type, model_type)));
246	✓✗✓✗ ✓✗✓✗ ✓✗	50	ts->add(BOOST_TEST_CASE(boost::bind(&test_partial_derivatives_against_numdiff,
247			contact_type, model_type)));
248	✓✗✓✗ ✓✗	50	framework::master_test_suite().add(ts);
249		50	}
250
251		1	bool init_function() {
252	✓✓	11	for (size_t contact_type = 0; contact_type < ContactModelTypes::all.size();
253			++contact_type) {
254	✓✓	60	for (size_t model_type = 0; model_type < PinocchioModelTypes::all.size();
255			++model_type) {
256		50	register_contact_model_unit_tests(ContactModelTypes::all[contact_type],
257		50	PinocchioModelTypes::all[model_type]);
258			}
259			}
260		1	return true;
261			}
262
263		1	int main(int argc, char** argv) {
264		1	return ::boost::unit_test::unit_test_main(&init_function, argc, argv);
265			}