Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	unittest/test_constraints.cpp	Lines:	133	133	100.0 %
Date:	2024-02-13 11:12:33	Branches:	290	576	50.3 %


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2020-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 "crocoddyl/multibody/data/multibody.hpp"
#include "factory/constraint.hpp"
#include "unittest_common.hpp"

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

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

void test_calc_returns_a_residual(ConstraintModelTypes::Type constraint_type,
                                  StateModelTypes::Type state_type) {
  // create the model
  ConstraintModelFactory factory;
  const boost::shared_ptr<crocoddyl::ConstraintModelAbstract>& model =
      factory.create(constraint_type, state_type);

  // create the corresponding data object
  const boost::shared_ptr<crocoddyl::StateMultibody>& state =
      boost::static_pointer_cast<crocoddyl::StateMultibody>(model->get_state());
  pinocchio::Model& pinocchio_model = *state->get_pinocchio().get();
  pinocchio::Data pinocchio_data(pinocchio_model);
  crocoddyl::DataCollectorMultibody shared_data(&pinocchio_data);
  const boost::shared_ptr<crocoddyl::ConstraintDataAbstract>& data =
      model->createData(&shared_data);
  data->g *= nan("");
  data->h *= nan("");

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

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

  // Getting the constraint residual computed by calc()
  model->calc(data, x, u);

  // Checking that calc returns a residual vector
  BOOST_CHECK(!data->g.hasNaN());
  BOOST_CHECK(!data->h.hasNaN());
}

void test_calc_against_numdiff(ConstraintModelTypes::Type constraint_type,
                               StateModelTypes::Type state_type) {
  // create the model
  ConstraintModelFactory factory;
  const boost::shared_ptr<crocoddyl::ConstraintModelAbstract>& model =
      factory.create(constraint_type, state_type);

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

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

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

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

  // Computing the cost derivatives
  model->calc(data, x, u);
  model_num_diff.calc(data_num_diff, x, u);

  // Checking the partial derivatives against NumDiff
  BOOST_CHECK((data->g - data_num_diff->g).isZero());
  BOOST_CHECK((data->h - data_num_diff->h).isZero());
}

void test_partial_derivatives_against_numdiff(
    ConstraintModelTypes::Type constraint_type,
    StateModelTypes::Type state_type) {
  // create the model
  ConstraintModelFactory factory;
  const boost::shared_ptr<crocoddyl::ConstraintModelAbstract>& model =
      factory.create(constraint_type, state_type);

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

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

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

  // 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
  using namespace boost::placeholders;

  std::vector<crocoddyl::ConstraintModelNumDiff::ReevaluationFunction> reevals;
  reevals.push_back(boost::bind(&crocoddyl::unittest::updateAllPinocchio,
                                &pinocchio_model, &pinocchio_data, _1, _2));
  model_num_diff.set_reevals(reevals);

  // Computing the cost derivatives
  model->calc(data, x, u);
  model->calcDiff(data, x, u);

  // Computing the cost derivatives via numerical differentiation
  model_num_diff.calc(data_num_diff, x, u);
  model_num_diff.calcDiff(data_num_diff, x, u);

  // Checking the partial derivatives against numdiff
  double tol = sqrt(model_num_diff.get_disturbance());
  BOOST_CHECK((data->Gx - data_num_diff->Gx).isZero(tol));
  BOOST_CHECK((data->Gu - data_num_diff->Gu).isZero(tol));
  BOOST_CHECK((data->Hx - data_num_diff->Hx).isZero(tol));
  BOOST_CHECK((data->Hu - data_num_diff->Hu).isZero(tol));

  // Computing the cost derivatives
  x = model->get_state()->rand();
  crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
  model->calc(data, x);
  model->calcDiff(data, x);

  // Computing the cost derivatives via numerical differentiation
  model_num_diff.calc(data_num_diff, x);
  model_num_diff.calcDiff(data_num_diff, x);

  // Checking the partial derivatives against numdiff
  BOOST_CHECK((data->Gx - data_num_diff->Gx).isZero(tol));
  BOOST_CHECK((data->Hx - data_num_diff->Hx).isZero(tol));
}

void test_dimensions_in_constraint_manager(
    ConstraintModelTypes::Type constraint_type,
    StateModelTypes::Type state_type) {
  // create the model
  ConstraintModelFactory factory;
  const boost::shared_ptr<crocoddyl::ConstraintModelAbstract>& model =
      factory.create(constraint_type, state_type);

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

  // create the constraint manager model
  crocoddyl::ConstraintModelManager constraint_man(state, model->get_nu());
  constraint_man.addConstraint("myConstraint", model);

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

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

  BOOST_CHECK(model->get_state()->get_nx() ==
              constraint_man.get_state()->get_nx());
  BOOST_CHECK(model->get_state()->get_ndx() ==
              constraint_man.get_state()->get_ndx());
  BOOST_CHECK(model->get_nu() == constraint_man.get_nu());
  BOOST_CHECK(model->get_state()->get_nq() ==
              constraint_man.get_state()->get_nq());
  BOOST_CHECK(model->get_state()->get_nv() ==
              constraint_man.get_state()->get_nv());
  BOOST_CHECK(model->get_ng() == constraint_man.get_ng());
  BOOST_CHECK(model->get_nh() == constraint_man.get_nh());
}

void test_partial_derivatives_in_constraint_manager(
    ConstraintModelTypes::Type constraint_type,
    StateModelTypes::Type state_type) {
  // create the model
  ConstraintModelFactory factory;
  const boost::shared_ptr<crocoddyl::ConstraintModelAbstract>& model =
      factory.create(constraint_type, state_type);

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

  // create the constraint manager model
  crocoddyl::ConstraintModelManager constraint_man(state, model->get_nu());
  constraint_man.addConstraint("myConstraint", model, 1.);
  const boost::shared_ptr<crocoddyl::ConstraintDataManager>& data_man =
      constraint_man.createData(&shared_data);

  // Generating random values for the state and control
  const Eigen::VectorXd x = state->rand();
  const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());

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

  // Computing the constraint derivatives
  model->calc(data, x, u);
  model->calcDiff(data, x, u);

  // Computing the constraint-manager derivatives
  constraint_man.calc(data_man, x, u);
  constraint_man.calcDiff(data_man, x, u);

  BOOST_CHECK((data->Hx - data_man->Hx).isZero());
  BOOST_CHECK((data->Hu - data_man->Hu).isZero());
}

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

void register_constraint_model_unit_tests(
    ConstraintModelTypes::Type constraint_type,
    StateModelTypes::Type state_type) {
  boost::test_tools::output_test_stream test_name;
  test_name << "test_" << constraint_type << "_" << state_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_calc_returns_a_residual, constraint_type, state_type)));
  ts->add(BOOST_TEST_CASE(
      boost::bind(&test_calc_against_numdiff, constraint_type, state_type)));
  ts->add(BOOST_TEST_CASE(boost::bind(&test_partial_derivatives_against_numdiff,
                                      constraint_type, state_type)));
  ts->add(BOOST_TEST_CASE(boost::bind(&test_dimensions_in_constraint_manager,
                                      constraint_type, state_type)));
  ts->add(BOOST_TEST_CASE(
      boost::bind(&test_partial_derivatives_in_constraint_manager,
                  constraint_type, state_type)));
  framework::master_test_suite().add(ts);
}

bool init_function() {
  // Test all constraints available with all available states types.
  for (size_t constraint_type = 0;
       constraint_type < ConstraintModelTypes::all.size(); ++constraint_type) {
    for (size_t state_type =
             StateModelTypes::all[StateModelTypes::StateMultibody_TalosArm];
         state_type < StateModelTypes::all.size(); ++state_type) {
      register_constraint_model_unit_tests(
          ConstraintModelTypes::all[constraint_type],
          StateModelTypes::all[state_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) 2020-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 "crocoddyl/multibody/data/multibody.hpp"
13			#include "factory/constraint.hpp"
14			#include "unittest_common.hpp"
15
16			using namespace boost::unit_test;
17			using namespace crocoddyl::unittest;
18
19			//----------------------------------------------------------------------------//
20
21		70	void test_calc_returns_a_residual(ConstraintModelTypes::Type constraint_type,
22			StateModelTypes::Type state_type) {
23			// create the model
24	✓✗	140	ConstraintModelFactory factory;
25			const boost::shared_ptr<crocoddyl::ConstraintModelAbstract>& model =
26	✓✗	140	factory.create(constraint_type, state_type);
27
28			// create the corresponding data object
29			const boost::shared_ptr<crocoddyl::StateMultibody>& state =
30		140	boost::static_pointer_cast<crocoddyl::StateMultibody>(model->get_state());
31		70	pinocchio::Model& pinocchio_model = *state->get_pinocchio().get();
32	✓✗	140	pinocchio::Data pinocchio_data(pinocchio_model);
33	✓✗	140	crocoddyl::DataCollectorMultibody shared_data(&pinocchio_data);
34			const boost::shared_ptr<crocoddyl::ConstraintDataAbstract>& data =
35	✓✗	140	model->createData(&shared_data);
36	✓✗	70	data->g *= nan("");
37	✓✗	70	data->h *= nan("");
38
39			// Generating random values for the state and control
40	✓✗	140	const Eigen::VectorXd x = model->get_state()->rand();
41	✓✗✓✗	140	const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
42
43			// Compute all the pinocchio function needed for the models.
44	✓✗✓✗	70	crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
45
46			// Getting the constraint residual computed by calc()
47	✓✗✓✗ ✓✗	70	model->calc(data, x, u);
48
49			// Checking that calc returns a residual vector
50	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	70	BOOST_CHECK(!data->g.hasNaN());
51	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	70	BOOST_CHECK(!data->h.hasNaN());
52		70	}
53
54		70	void test_calc_against_numdiff(ConstraintModelTypes::Type constraint_type,
55			StateModelTypes::Type state_type) {
56			// create the model
57	✓✗	140	ConstraintModelFactory factory;
58			const boost::shared_ptr<crocoddyl::ConstraintModelAbstract>& model =
59	✓✗	140	factory.create(constraint_type, state_type);
60
61			// create the corresponding data object
62			const boost::shared_ptr<crocoddyl::StateMultibody>& state =
63		140	boost::static_pointer_cast<crocoddyl::StateMultibody>(model->get_state());
64		70	pinocchio::Model& pinocchio_model = *state->get_pinocchio().get();
65	✓✗	140	pinocchio::Data pinocchio_data(pinocchio_model);
66	✓✗	140	crocoddyl::DataCollectorMultibody shared_data(&pinocchio_data);
67			const boost::shared_ptr<crocoddyl::ConstraintDataAbstract>& data =
68	✓✗	140	model->createData(&shared_data);
69
70			// Create the equivalent num diff model and data.
71	✓✗	140	crocoddyl::ConstraintModelNumDiff model_num_diff(model);
72			const boost::shared_ptr<crocoddyl::ConstraintDataAbstract>& data_num_diff =
73	✓✗	140	model_num_diff.createData(&shared_data);
74
75			// Generating random values for the state and control
76	✓✗	140	const Eigen::VectorXd x = model->get_state()->rand();
77	✓✗✓✗	140	const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
78
79			// Compute all the pinocchio function needed for the models.
80	✓✗✓✗	70	crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
81
82			// Computing the cost derivatives
83	✓✗✓✗ ✓✗	70	model->calc(data, x, u);
84	✓✗✓✗ ✓✗	70	model_num_diff.calc(data_num_diff, x, u);
85
86			// Checking the partial derivatives against NumDiff
87	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	70	BOOST_CHECK((data->g - data_num_diff->g).isZero());
88	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	70	BOOST_CHECK((data->h - data_num_diff->h).isZero());
89		70	}
90
91		70	void test_partial_derivatives_against_numdiff(
92			ConstraintModelTypes::Type constraint_type,
93			StateModelTypes::Type state_type) {
94			// create the model
95	✓✗	140	ConstraintModelFactory factory;
96			const boost::shared_ptr<crocoddyl::ConstraintModelAbstract>& model =
97	✓✗	140	factory.create(constraint_type, state_type);
98
99			// create the corresponding data object
100			const boost::shared_ptr<crocoddyl::StateMultibody>& state =
101		140	boost::static_pointer_cast<crocoddyl::StateMultibody>(model->get_state());
102		70	pinocchio::Model& pinocchio_model = *state->get_pinocchio().get();
103	✓✗	140	pinocchio::Data pinocchio_data(pinocchio_model);
104	✓✗	140	crocoddyl::DataCollectorMultibody shared_data(&pinocchio_data);
105			const boost::shared_ptr<crocoddyl::ConstraintDataAbstract>& data =
106	✓✗	140	model->createData(&shared_data);
107
108			// Create the equivalent num diff model and data.
109	✓✗	140	crocoddyl::ConstraintModelNumDiff model_num_diff(model);
110			const boost::shared_ptr<crocoddyl::ConstraintDataAbstract>& data_num_diff =
111	✓✗	140	model_num_diff.createData(&shared_data);
112
113			// Generating random values for the state and control
114	✓✗	140	Eigen::VectorXd x = model->get_state()->rand();
115	✓✗✓✗	140	const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
116
117			// Compute all the pinocchio function needed for the models.
118	✓✗✓✗	70	crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
119
120			// set the function that needs to be called at every step of the numdiff
121			using namespace boost::placeholders;
122
123		140	std::vector<crocoddyl::ConstraintModelNumDiff::ReevaluationFunction> reevals;
124	✓✗✓✗ ✓✗	70	reevals.push_back(boost::bind(&crocoddyl::unittest::updateAllPinocchio,
125			&pinocchio_model, &pinocchio_data, _1, _2));
126	✓✗	70	model_num_diff.set_reevals(reevals);
127
128			// Computing the cost derivatives
129	✓✗✓✗ ✓✗	70	model->calc(data, x, u);
130	✓✗✓✗ ✓✗	70	model->calcDiff(data, x, u);
131
132			// Computing the cost derivatives via numerical differentiation
133	✓✗✓✗ ✓✗	70	model_num_diff.calc(data_num_diff, x, u);
134	✓✗✓✗ ✓✗	70	model_num_diff.calcDiff(data_num_diff, x, u);
135
136			// Checking the partial derivatives against numdiff
137		70	double tol = sqrt(model_num_diff.get_disturbance());
138	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	70	BOOST_CHECK((data->Gx - data_num_diff->Gx).isZero(tol));
139	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	70	BOOST_CHECK((data->Gu - data_num_diff->Gu).isZero(tol));
140	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	70	BOOST_CHECK((data->Hx - data_num_diff->Hx).isZero(tol));
141	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	70	BOOST_CHECK((data->Hu - data_num_diff->Hu).isZero(tol));
142
143			// Computing the cost derivatives
144	✓✗	70	x = model->get_state()->rand();
145	✓✗✓✗	70	crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
146	✓✗✓✗	70	model->calc(data, x);
147	✓✗✓✗	70	model->calcDiff(data, x);
148
149			// Computing the cost derivatives via numerical differentiation
150	✓✗✓✗	70	model_num_diff.calc(data_num_diff, x);
151	✓✗✓✗	70	model_num_diff.calcDiff(data_num_diff, x);
152
153			// Checking the partial derivatives against numdiff
154	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	70	BOOST_CHECK((data->Gx - data_num_diff->Gx).isZero(tol));
155	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	70	BOOST_CHECK((data->Hx - data_num_diff->Hx).isZero(tol));
156		70	}
157
158		70	void test_dimensions_in_constraint_manager(
159			ConstraintModelTypes::Type constraint_type,
160			StateModelTypes::Type state_type) {
161			// create the model
162	✓✗	140	ConstraintModelFactory factory;
163			const boost::shared_ptr<crocoddyl::ConstraintModelAbstract>& model =
164	✓✗	140	factory.create(constraint_type, state_type);
165
166			// create the corresponding data object
167			const boost::shared_ptr<crocoddyl::StateMultibody>& state =
168		140	boost::static_pointer_cast<crocoddyl::StateMultibody>(model->get_state());
169		70	pinocchio::Model& pinocchio_model = *state->get_pinocchio().get();
170	✓✗	140	pinocchio::Data pinocchio_data(pinocchio_model);
171	✓✗	140	crocoddyl::DataCollectorMultibody shared_data(&pinocchio_data);
172
173			// create the constraint manager model
174	✓✗	140	crocoddyl::ConstraintModelManager constraint_man(state, model->get_nu());
175	✓✗✓✗	70	constraint_man.addConstraint("myConstraint", model);
176
177			// Generating random values for the state and control
178	✓✗	140	const Eigen::VectorXd x = state->rand();
179
180			// Compute all the pinocchio function needed for the models.
181	✓✗✓✗	70	crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
182
183	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	70	BOOST_CHECK(model->get_state()->get_nx() ==
184			constraint_man.get_state()->get_nx());
185	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	70	BOOST_CHECK(model->get_state()->get_ndx() ==
186			constraint_man.get_state()->get_ndx());
187	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	70	BOOST_CHECK(model->get_nu() == constraint_man.get_nu());
188	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	70	BOOST_CHECK(model->get_state()->get_nq() ==
189			constraint_man.get_state()->get_nq());
190	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	70	BOOST_CHECK(model->get_state()->get_nv() ==
191			constraint_man.get_state()->get_nv());
192	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	70	BOOST_CHECK(model->get_ng() == constraint_man.get_ng());
193	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	70	BOOST_CHECK(model->get_nh() == constraint_man.get_nh());
194		70	}
195
196		70	void test_partial_derivatives_in_constraint_manager(
197			ConstraintModelTypes::Type constraint_type,
198			StateModelTypes::Type state_type) {
199			// create the model
200	✓✗	140	ConstraintModelFactory factory;
201			const boost::shared_ptr<crocoddyl::ConstraintModelAbstract>& model =
202	✓✗	140	factory.create(constraint_type, state_type);
203
204			// create the corresponding data object
205			const boost::shared_ptr<crocoddyl::StateMultibody>& state =
206		140	boost::static_pointer_cast<crocoddyl::StateMultibody>(model->get_state());
207		70	pinocchio::Model& pinocchio_model = *state->get_pinocchio().get();
208	✓✗	140	pinocchio::Data pinocchio_data(pinocchio_model);
209	✓✗	140	crocoddyl::DataCollectorMultibody shared_data(&pinocchio_data);
210			const boost::shared_ptr<crocoddyl::ConstraintDataAbstract>& data =
211	✓✗	140	model->createData(&shared_data);
212
213			// create the constraint manager model
214	✓✗	140	crocoddyl::ConstraintModelManager constraint_man(state, model->get_nu());
215	✓✗✓✗	70	constraint_man.addConstraint("myConstraint", model, 1.);
216			const boost::shared_ptr<crocoddyl::ConstraintDataManager>& data_man =
217	✓✗	140	constraint_man.createData(&shared_data);
218
219			// Generating random values for the state and control
220	✓✗	140	const Eigen::VectorXd x = state->rand();
221	✓✗✓✗	140	const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
222
223			// Compute all the pinocchio function needed for the models.
224	✓✗✓✗	70	crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
225
226			// Computing the constraint derivatives
227	✓✗✓✗ ✓✗	70	model->calc(data, x, u);
228	✓✗✓✗ ✓✗	70	model->calcDiff(data, x, u);
229
230			// Computing the constraint-manager derivatives
231	✓✗✓✗ ✓✗	70	constraint_man.calc(data_man, x, u);
232	✓✗✓✗ ✓✗	70	constraint_man.calcDiff(data_man, x, u);
233
234	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	70	BOOST_CHECK((data->Hx - data_man->Hx).isZero());
235	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	70	BOOST_CHECK((data->Hu - data_man->Hu).isZero());
236		70	}
237
238			//----------------------------------------------------------------------------//
239
240		70	void register_constraint_model_unit_tests(
241			ConstraintModelTypes::Type constraint_type,
242			StateModelTypes::Type state_type) {
243	✓✗✓✗	140	boost::test_tools::output_test_stream test_name;
244	✓✗✓✗ ✓✗✓✗	70	test_name << "test_" << constraint_type << "_" << state_type;
245	✓✗✓✗ ✓✗✓✗	70	std::cout << "Running " << test_name.str() << std::endl;
246	✓✗✓✗ ✓✗✓✗	70	test_suite* ts = BOOST_TEST_SUITE(test_name.str());
247	✓✗✓✗ ✓✗✓✗ ✓✗	70	ts->add(BOOST_TEST_CASE(
248			boost::bind(&test_calc_returns_a_residual, constraint_type, state_type)));
249	✓✗✓✗ ✓✗✓✗ ✓✗	70	ts->add(BOOST_TEST_CASE(
250			boost::bind(&test_calc_against_numdiff, constraint_type, state_type)));
251	✓✗✓✗ ✓✗✓✗ ✓✗	70	ts->add(BOOST_TEST_CASE(boost::bind(&test_partial_derivatives_against_numdiff,
252			constraint_type, state_type)));
253	✓✗✓✗ ✓✗✓✗ ✓✗	70	ts->add(BOOST_TEST_CASE(boost::bind(&test_dimensions_in_constraint_manager,
254			constraint_type, state_type)));
255	✓✗✓✗ ✓✗✓✗ ✓✗	70	ts->add(BOOST_TEST_CASE(
256			boost::bind(&test_partial_derivatives_in_constraint_manager,
257			constraint_type, state_type)));
258	✓✗✓✗ ✓✗	70	framework::master_test_suite().add(ts);
259		70	}
260
261		1	bool init_function() {
262			// Test all constraints available with all available states types.
263		15	for (size_t constraint_type = 0;
264	✓✓	15	constraint_type < ConstraintModelTypes::all.size(); ++constraint_type) {
265		70	for (size_t state_type =
266		14	StateModelTypes::all[StateModelTypes::StateMultibody_TalosArm];
267	✓✓	84	state_type < StateModelTypes::all.size(); ++state_type) {
268		70	register_constraint_model_unit_tests(
269		70	ConstraintModelTypes::all[constraint_type],
270		70	StateModelTypes::all[state_type]);
271			}
272			}
273		1	return true;
274			}
275
276		1	int main(int argc, char** argv) {
277		1	return ::boost::unit_test::unit_test_main(&init_function, argc, argv);
278			}