Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	unittest/test_costs.cpp	Lines:	139	143	97.2 %
Date:	2024-02-13 11:12:33	Branches:	304	658	46.2 %


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2019-2023, LAAS-CNRS, New York University,
//                          Max Planck Gesellschaft, University of Edinburgh,
//                          INRIA
// 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/cost.hpp"
#include "unittest_common.hpp"

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

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

void test_calc_returns_a_cost(CostModelTypes::Type cost_type,
                              StateModelTypes::Type state_type,
                              ActivationModelTypes::Type activation_type) {
  // create the model
  CostModelFactory factory;
  const boost::shared_ptr<crocoddyl::CostModelAbstract>& model =
      factory.create(cost_type, state_type, activation_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(CostModelTypes::Type cost_type,
                               StateModelTypes::Type state_type,
                               ActivationModelTypes::Type activation_type) {
  // create the model
  CostModelFactory factory;
  const boost::shared_ptr<crocoddyl::CostModelAbstract>& model =
      factory.create(cost_type, state_type, activation_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(
    CostModelTypes::Type cost_type, StateModelTypes::Type state_type,
    ActivationModelTypes::Type activation_type) {
  using namespace boost::placeholders;

  // create the model
  CostModelFactory factory;
  const boost::shared_ptr<crocoddyl::CostModelAbstract>& model =
      factory.create(cost_type, state_type, activation_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
  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));
  }

  // 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);
  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->Lx - data_num_diff->Lx).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));
  } else {
    BOOST_CHECK((data_num_diff->Lxx).isZero(tol));
  }
}

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