Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	unittest/test_costs_collision.cpp	Lines:	124	127	97.6 %
Date:	2024-02-13 11:12:33	Branches:	269	578	46.5 %


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

#define BOOST_TEST_NO_MAIN
#define BOOST_TEST_ALTERNATIVE_INIT_API

#ifdef PINOCCHIO_WITH_HPP_FCL

#include "crocoddyl/multibody/data/multibody.hpp"
#include "factory/cost.hpp"
#include "unittest_common.hpp"

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

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

void test_calc_returns_a_cost(CostModelCollisionTypes::Type cost_type,
                              StateModelTypes::Type state_type) {
  // create the model
  CostModelFactory factory;
  const boost::shared_ptr<crocoddyl::CostModelAbstract>& model =
      factory.create(cost_type, state_type);

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

  // 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::CostDataAbstract>& data =
      model->createData(&shared_data);
  data->cost = 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 cost value computed by calc()
  model->calc(data, x, u);

  // Checking that calc returns a cost value
  BOOST_CHECK(!std::isnan(data->cost));
}

void test_calc_against_numdiff(CostModelCollisionTypes::Type cost_type,
                               StateModelTypes::Type state_type) {
  // create the model
  CostModelFactory factory;
  const boost::shared_ptr<crocoddyl::CostModelAbstract>& model =
      factory.create(cost_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::CostDataAbstract>& data =
      model->createData(&shared_data);

  // Create the equivalent num diff model and data.
  crocoddyl::CostModelNumDiff model_num_diff(model);
  const boost::shared_ptr<crocoddyl::CostDataAbstract>& 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->cost == data_num_diff->cost);
}

void test_partial_derivatives_against_numdiff(
    CostModelCollisionTypes::Type cost_type, StateModelTypes::Type state_type) {
  using namespace boost::placeholders;

  // create the model
  CostModelFactory factory;
  const boost::shared_ptr<crocoddyl::CostModelAbstract>& model =
      factory.create(cost_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::CostDataAbstract>& data =
      model->createData(&shared_data);

  // Create the equivalent num diff model and data.
  crocoddyl::CostModelNumDiff model_num_diff(model);
  const boost::shared_ptr<crocoddyl::CostDataAbstract>& 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);

  // set the function that needs to be called at every step of the numdiff
  std::vector<crocoddyl::CostModelNumDiff::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);
  model_num_diff.calc(data_num_diff, x, u);
  model_num_diff.calcDiff(data_num_diff, x, u);
  // 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->Lx - data_num_diff->Lx).isZero(tol));
  BOOST_CHECK((data->Lu - data_num_diff->Lu).isZero(tol));
  if (model_num_diff.get_with_gauss_approx()) {
    // The num diff is not precise enough to be tested here.
    BOOST_CHECK((data->Lxx - data_num_diff->Lxx).isZero(tol));
    BOOST_CHECK((data->Lxu - data_num_diff->Lxu).isZero(tol));
    BOOST_CHECK((data->Luu - data_num_diff->Luu).isZero(tol));
  } else {
    BOOST_CHECK((data_num_diff->Lxx).isZero(tol));
    BOOST_CHECK((data_num_diff->Lxu).isZero(tol));
    BOOST_CHECK((data_num_diff->Luu).isZero(tol));
  }
}

void test_dimensions_in_cost_sum(CostModelCollisionTypes::Type cost_type,
                                 StateModelTypes::Type state_type) {
  // create the model
  CostModelFactory factory;
  const boost::shared_ptr<crocoddyl::CostModelAbstract>& model =
      factory.create(cost_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 cost sum model
  crocoddyl::CostModelSum cost_sum(state, model->get_nu());
  cost_sum.addCost("myCost", model, 1.);

  // 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() == cost_sum.get_state()->get_nx());
  BOOST_CHECK(model->get_state()->get_ndx() == cost_sum.get_state()->get_ndx());
  BOOST_CHECK(model->get_nu() == cost_sum.get_nu());
  BOOST_CHECK(model->get_state()->get_nq() == cost_sum.get_state()->get_nq());
  BOOST_CHECK(model->get_state()->get_nv() == cost_sum.get_state()->get_nv());
  BOOST_CHECK(model->get_activation()->get_nr() == cost_sum.get_nr());
}

void test_partial_derivatives_in_cost_sum(
    CostModelCollisionTypes::Type cost_type, StateModelTypes::Type state_type) {
  // create the model
  CostModelFactory factory;
  const boost::shared_ptr<crocoddyl::CostModelAbstract>& model =
      factory.create(cost_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::CostDataAbstract>& data =
      model->createData(&shared_data);

  // create the cost sum model
  crocoddyl::CostModelSum cost_sum(state, model->get_nu());
  cost_sum.addCost("myCost", model, 1.);
  const boost::shared_ptr<crocoddyl::CostDataSum>& data_sum =
      cost_sum.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 cost derivatives
  model->calc(data, x, u);
  model->calcDiff(data, x, u);
  cost_sum.calc(data_sum, x, u);
  cost_sum.calcDiff(data_sum, x, u);

  BOOST_CHECK((data->Lx - data_sum->Lx).isZero());
  BOOST_CHECK((data->Lu - data_sum->Lu).isZero());
  BOOST_CHECK((data->Lxx - data_sum->Lxx).isZero());
  BOOST_CHECK((data->Lxu - data_sum->Lxu).isZero());
  BOOST_CHECK((data->Luu - data_sum->Luu).isZero());
}

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

void register_cost_model_unit_tests(CostModelCollisionTypes::Type cost_type,
                                    StateModelTypes::Type state_type) {
  boost::test_tools::output_test_stream test_name;
  test_name << "test_" << cost_type << "_2norm_barrier_" << 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_cost, cost_type, state_type)));
  ts->add(BOOST_TEST_CASE(
      boost::bind(&test_calc_against_numdiff, cost_type, state_type)));
  ts->add(BOOST_TEST_CASE(boost::bind(&test_partial_derivatives_against_numdiff,
                                      cost_type, state_type)));
  ts->add(BOOST_TEST_CASE(
      boost::bind(&test_dimensions_in_cost_sum, cost_type, state_type)));
  ts->add(BOOST_TEST_CASE(boost::bind(&test_partial_derivatives_in_cost_sum,
                                      cost_type, state_type)));
  framework::master_test_suite().add(ts);
}

bool init_function() {
  // Test all costs available with all the activation types with all available
  // states types.
  for (size_t cost_type = 0; cost_type < CostModelCollisionTypes::all.size();
       ++cost_type) {
    register_cost_model_unit_tests(CostModelCollisionTypes::all[cost_type],
                                   StateModelTypes::StateMultibody_HyQ);
    register_cost_model_unit_tests(
        CostModelCollisionTypes::all[cost_type],
        StateModelTypes::StateMultibody_RandomHumanoid);
  }
  return true;
}

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

#else

int main(int, char**) {}

#endif  // PINOCCHIO_WITH_HPP_FCL


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2021-2023, LAAS-CNRS, University of Edinburgh
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			#ifdef PINOCCHIO_WITH_HPP_FCL
13
14			#include "crocoddyl/multibody/data/multibody.hpp"
15			#include "factory/cost.hpp"
16			#include "unittest_common.hpp"
17
18			using namespace boost::unit_test;
19			using namespace crocoddyl::unittest;
20
21			//----------------------------------------------------------------------------//
22
23		2	void test_calc_returns_a_cost(CostModelCollisionTypes::Type cost_type,
24			StateModelTypes::Type state_type) {
25			// create the model
26	✓✗	4	CostModelFactory factory;
27			const boost::shared_ptr<crocoddyl::CostModelAbstract>& model =
28	✓✗	4	factory.create(cost_type, state_type);
29
30			// Run the print function
31	✓✗	4	std::ostringstream tmp;
32	✓✗	2	tmp << *model;
33
34			// create the corresponding data object
35			const boost::shared_ptr<crocoddyl::StateMultibody>& state =
36		4	boost::static_pointer_cast<crocoddyl::StateMultibody>(model->get_state());
37		2	pinocchio::Model& pinocchio_model = *state->get_pinocchio().get();
38	✓✗	4	pinocchio::Data pinocchio_data(pinocchio_model);
39	✓✗	4	crocoddyl::DataCollectorMultibody shared_data(&pinocchio_data);
40			const boost::shared_ptr<crocoddyl::CostDataAbstract>& data =
41	✓✗	4	model->createData(&shared_data);
42		2	data->cost = nan("");
43
44			// Generating random values for the state and control
45	✓✗	4	const Eigen::VectorXd x = model->get_state()->rand();
46	✓✗✓✗	4	const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
47
48			// Compute all the pinocchio function needed for the models.
49	✓✗✓✗	2	crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
50
51			// Getting the cost value computed by calc()
52	✓✗✓✗ ✓✗	2	model->calc(data, x, u);
53
54			// Checking that calc returns a cost value
55	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(!std::isnan(data->cost));
56		2	}
57
58		2	void test_calc_against_numdiff(CostModelCollisionTypes::Type cost_type,
59			StateModelTypes::Type state_type) {
60			// create the model
61	✓✗	4	CostModelFactory factory;
62			const boost::shared_ptr<crocoddyl::CostModelAbstract>& model =
63	✓✗	4	factory.create(cost_type, state_type);
64
65			// create the corresponding data object
66			const boost::shared_ptr<crocoddyl::StateMultibody>& state =
67		4	boost::static_pointer_cast<crocoddyl::StateMultibody>(model->get_state());
68		2	pinocchio::Model& pinocchio_model = *state->get_pinocchio().get();
69	✓✗	4	pinocchio::Data pinocchio_data(pinocchio_model);
70	✓✗	4	crocoddyl::DataCollectorMultibody shared_data(&pinocchio_data);
71			const boost::shared_ptr<crocoddyl::CostDataAbstract>& data =
72	✓✗	4	model->createData(&shared_data);
73
74			// Create the equivalent num diff model and data.
75	✓✗	4	crocoddyl::CostModelNumDiff model_num_diff(model);
76			const boost::shared_ptr<crocoddyl::CostDataAbstract>& data_num_diff =
77	✓✗	4	model_num_diff.createData(&shared_data);
78
79			// Generating random values for the state and control
80	✓✗	4	const Eigen::VectorXd x = model->get_state()->rand();
81	✓✗✓✗	4	const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
82
83			// Compute all the pinocchio function needed for the models.
84	✓✗✓✗	2	crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
85
86			// Computing the cost derivatives
87	✓✗✓✗ ✓✗	2	model->calc(data, x, u);
88	✓✗✓✗ ✓✗	2	model_num_diff.calc(data_num_diff, x, u);
89
90			// Checking the partial derivatives against NumDiff
91	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(data->cost == data_num_diff->cost);
92		2	}
93
94		2	void test_partial_derivatives_against_numdiff(
95			CostModelCollisionTypes::Type cost_type, StateModelTypes::Type state_type) {
96			using namespace boost::placeholders;
97
98			// create the model
99	✓✗	4	CostModelFactory factory;
100			const boost::shared_ptr<crocoddyl::CostModelAbstract>& model =
101	✓✗	4	factory.create(cost_type, state_type);
102
103			// create the corresponding data object
104			const boost::shared_ptr<crocoddyl::StateMultibody>& state =
105		4	boost::static_pointer_cast<crocoddyl::StateMultibody>(model->get_state());
106		2	pinocchio::Model& pinocchio_model = *state->get_pinocchio().get();
107	✓✗	4	pinocchio::Data pinocchio_data(pinocchio_model);
108	✓✗	4	crocoddyl::DataCollectorMultibody shared_data(&pinocchio_data);
109			const boost::shared_ptr<crocoddyl::CostDataAbstract>& data =
110	✓✗	4	model->createData(&shared_data);
111
112			// Create the equivalent num diff model and data.
113	✓✗	4	crocoddyl::CostModelNumDiff model_num_diff(model);
114			const boost::shared_ptr<crocoddyl::CostDataAbstract>& data_num_diff =
115	✓✗	4	model_num_diff.createData(&shared_data);
116
117			// Generating random values for the state and control
118	✓✗	4	const Eigen::VectorXd x = model->get_state()->rand();
119	✓✗✓✗	4	const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
120
121			// Compute all the pinocchio function needed for the models.
122	✓✗✓✗	2	crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
123
124			// set the function that needs to be called at every step of the numdiff
125		4	std::vector<crocoddyl::CostModelNumDiff::ReevaluationFunction> reevals;
126	✓✗✓✗ ✓✗	2	reevals.push_back(boost::bind(&crocoddyl::unittest::updateAllPinocchio,
127			&pinocchio_model, &pinocchio_data, _1, _2));
128	✓✗	2	model_num_diff.set_reevals(reevals);
129
130			// Computing the cost derivatives
131	✓✗✓✗ ✓✗	2	model->calc(data, x, u);
132	✓✗✓✗ ✓✗	2	model->calcDiff(data, x, u);
133	✓✗✓✗ ✓✗	2	model_num_diff.calc(data_num_diff, x, u);
134	✓✗✓✗ ✓✗	2	model_num_diff.calcDiff(data_num_diff, x, u);
135			// Tolerance defined as in
136			// http://www.it.uom.gr/teaching/linearalgebra/NumericalRecipiesInC/c5-7.pdf
137		2	double tol = std::pow(model_num_diff.get_disturbance(), 1. / 3.);
138	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK((data->Lx - data_num_diff->Lx).isZero(tol));
139	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK((data->Lu - data_num_diff->Lu).isZero(tol));
140	✓✗✓✗	2	if (model_num_diff.get_with_gauss_approx()) {
141			// The num diff is not precise enough to be tested here.
142	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK((data->Lxx - data_num_diff->Lxx).isZero(tol));
143	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK((data->Lxu - data_num_diff->Lxu).isZero(tol));
144	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK((data->Luu - data_num_diff->Luu).isZero(tol));
145			} else {
146			BOOST_CHECK((data_num_diff->Lxx).isZero(tol));
147			BOOST_CHECK((data_num_diff->Lxu).isZero(tol));
148			BOOST_CHECK((data_num_diff->Luu).isZero(tol));
149			}
150		2	}
151
152		2	void test_dimensions_in_cost_sum(CostModelCollisionTypes::Type cost_type,
153			StateModelTypes::Type state_type) {
154			// create the model
155	✓✗	4	CostModelFactory factory;
156			const boost::shared_ptr<crocoddyl::CostModelAbstract>& model =
157	✓✗	4	factory.create(cost_type, state_type);
158
159			// create the corresponding data object
160			const boost::shared_ptr<crocoddyl::StateMultibody>& state =
161		4	boost::static_pointer_cast<crocoddyl::StateMultibody>(model->get_state());
162		2	pinocchio::Model& pinocchio_model = *state->get_pinocchio().get();
163	✓✗	4	pinocchio::Data pinocchio_data(pinocchio_model);
164	✓✗	4	crocoddyl::DataCollectorMultibody shared_data(&pinocchio_data);
165
166			// create the cost sum model
167		4	crocoddyl::CostModelSum cost_sum(state, model->get_nu());
168	✓✗✓✗	2	cost_sum.addCost("myCost", model, 1.);
169
170			// Generating random values for the state and control
171	✓✗	4	const Eigen::VectorXd x = state->rand();
172
173			// Compute all the pinocchio function needed for the models.
174	✓✗✓✗	2	crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
175
176	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(model->get_state()->get_nx() == cost_sum.get_state()->get_nx());
177	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(model->get_state()->get_ndx() == cost_sum.get_state()->get_ndx());
178	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(model->get_nu() == cost_sum.get_nu());
179	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(model->get_state()->get_nq() == cost_sum.get_state()->get_nq());
180	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(model->get_state()->get_nv() == cost_sum.get_state()->get_nv());
181	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(model->get_activation()->get_nr() == cost_sum.get_nr());
182		2	}
183
184		2	void test_partial_derivatives_in_cost_sum(
185			CostModelCollisionTypes::Type cost_type, StateModelTypes::Type state_type) {
186			// create the model
187	✓✗	4	CostModelFactory factory;
188			const boost::shared_ptr<crocoddyl::CostModelAbstract>& model =
189	✓✗	4	factory.create(cost_type, state_type);
190
191			// create the corresponding data object
192			const boost::shared_ptr<crocoddyl::StateMultibody>& state =
193		4	boost::static_pointer_cast<crocoddyl::StateMultibody>(model->get_state());
194		2	pinocchio::Model& pinocchio_model = *state->get_pinocchio().get();
195	✓✗	4	pinocchio::Data pinocchio_data(pinocchio_model);
196	✓✗	4	crocoddyl::DataCollectorMultibody shared_data(&pinocchio_data);
197			const boost::shared_ptr<crocoddyl::CostDataAbstract>& data =
198	✓✗	4	model->createData(&shared_data);
199
200			// create the cost sum model
201		4	crocoddyl::CostModelSum cost_sum(state, model->get_nu());
202	✓✗✓✗	2	cost_sum.addCost("myCost", model, 1.);
203			const boost::shared_ptr<crocoddyl::CostDataSum>& data_sum =
204	✓✗	4	cost_sum.createData(&shared_data);
205
206			// Generating random values for the state and control
207	✓✗	4	const Eigen::VectorXd x = state->rand();
208	✓✗✓✗	4	const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
209
210			// Compute all the pinocchio function needed for the models.
211	✓✗✓✗	2	crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
212
213			// Computing the cost derivatives
214	✓✗✓✗ ✓✗	2	model->calc(data, x, u);
215	✓✗✓✗ ✓✗	2	model->calcDiff(data, x, u);
216	✓✗✓✗ ✓✗	2	cost_sum.calc(data_sum, x, u);
217	✓✗✓✗ ✓✗	2	cost_sum.calcDiff(data_sum, x, u);
218
219	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK((data->Lx - data_sum->Lx).isZero());
220	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK((data->Lu - data_sum->Lu).isZero());
221	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK((data->Lxx - data_sum->Lxx).isZero());
222	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK((data->Lxu - data_sum->Lxu).isZero());
223	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK((data->Luu - data_sum->Luu).isZero());
224		2	}
225
226			//----------------------------------------------------------------------------//
227
228		2	void register_cost_model_unit_tests(CostModelCollisionTypes::Type cost_type,
229			StateModelTypes::Type state_type) {
230	✓✗✓✗	4	boost::test_tools::output_test_stream test_name;
231	✓✗✓✗ ✓✗✓✗	2	test_name << "test_" << cost_type << "_2norm_barrier_" << state_type;
232	✓✗✓✗ ✓✗✓✗	2	std::cout << "Running " << test_name.str() << std::endl;
233	✓✗✓✗ ✓✗✓✗	2	test_suite* ts = BOOST_TEST_SUITE(test_name.str());
234	✓✗✓✗ ✓✗✓✗ ✓✗	2	ts->add(BOOST_TEST_CASE(
235			boost::bind(&test_calc_returns_a_cost, cost_type, state_type)));
236	✓✗✓✗ ✓✗✓✗ ✓✗	2	ts->add(BOOST_TEST_CASE(
237			boost::bind(&test_calc_against_numdiff, cost_type, state_type)));
238	✓✗✓✗ ✓✗✓✗ ✓✗	2	ts->add(BOOST_TEST_CASE(boost::bind(&test_partial_derivatives_against_numdiff,
239			cost_type, state_type)));
240	✓✗✓✗ ✓✗✓✗ ✓✗	2	ts->add(BOOST_TEST_CASE(
241			boost::bind(&test_dimensions_in_cost_sum, cost_type, state_type)));
242	✓✗✓✗ ✓✗✓✗ ✓✗	2	ts->add(BOOST_TEST_CASE(boost::bind(&test_partial_derivatives_in_cost_sum,
243			cost_type, state_type)));
244	✓✗✓✗ ✓✗	2	framework::master_test_suite().add(ts);
245		2	}
246
247		1	bool init_function() {
248			// Test all costs available with all the activation types with all available
249			// states types.
250	✓✓	2	for (size_t cost_type = 0; cost_type < CostModelCollisionTypes::all.size();
251			++cost_type) {
252		1	register_cost_model_unit_tests(CostModelCollisionTypes::all[cost_type],
253			StateModelTypes::StateMultibody_HyQ);
254		1	register_cost_model_unit_tests(
255		1	CostModelCollisionTypes::all[cost_type],
256			StateModelTypes::StateMultibody_RandomHumanoid);
257			}
258		1	return true;
259			}
260
261		1	int main(int argc, char** argv) {
262		1	return ::boost::unit_test::unit_test_main(&init_function, argc, argv);
263			}
264
265			#else
266
267			int main(int, char**) {}
268
269			#endif // PINOCCHIO_WITH_HPP_FCL