Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	unittest/test_costs_noFF.cpp	Lines:	151	155	97.4 %
Date:	2024-02-13 11:12:33	Branches:	322	696	46.3 %


///////////////////////////////////////////////////////////////////////////////
// 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/core/actuation-base.hpp"
#include "crocoddyl/multibody/actuations/full.hpp"
#include "crocoddyl/multibody/data/multibody.hpp"
#include "factory/actuation.hpp"
#include "factory/cost.hpp"
#include "unittest_common.hpp"

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

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

void test_calc_returns_a_cost(CostModelNoFFTypes::Type cost_type,
                              ActivationModelTypes::Type activation_type) {
  // create the model
  CostModelFactory factory;
  const boost::shared_ptr<crocoddyl::CostModelAbstract>& model =
      factory.create(cost_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);

  boost::shared_ptr<crocoddyl::ActuationModelAbstract> actuation =
      boost::make_shared<crocoddyl::ActuationModelFull>(state);
  const boost::shared_ptr<crocoddyl::ActuationDataAbstract>& actuation_data =
      actuation->createData();
  crocoddyl::DataCollectorActMultibody shared_data(&pinocchio_data,
                                                   actuation_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(CostModelNoFFTypes::Type cost_type,
                               ActivationModelTypes::Type activation_type) {
  // create the model
  CostModelFactory factory;
  const boost::shared_ptr<crocoddyl::CostModelAbstract>& model =
      factory.create(cost_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);

  boost::shared_ptr<crocoddyl::ActuationModelAbstract> actuation =
      boost::make_shared<crocoddyl::ActuationModelFull>(state);
  const boost::shared_ptr<crocoddyl::ActuationDataAbstract>& actuation_data =
      actuation->createData();
  crocoddyl::DataCollectorActMultibody shared_data(&pinocchio_data,
                                                   actuation_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(
    CostModelNoFFTypes::Type cost_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, 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);

  boost::shared_ptr<crocoddyl::ActuationModelAbstract> actuation_model =
      boost::make_shared<crocoddyl::ActuationModelFull>(state);
  const boost::shared_ptr<crocoddyl::ActuationDataAbstract>& actuation_data =
      actuation_model->createData();
  crocoddyl::DataCollectorActMultibody shared_data(&pinocchio_data,
                                                   actuation_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));
  reevals.push_back(boost::bind(&crocoddyl::unittest::updateActuation,
                                actuation_model, actuation_data, _1, _2));
  model_num_diff.set_reevals(reevals);

  // Computing the cost derivatives
  actuation_model->calc(actuation_data, x, u);
  actuation_model->calcDiff(actuation_data, x, u);
  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()) {
    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);
  actuation_model->calc(actuation_data, x);
  actuation_model->calcDiff(actuation_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()) {
    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(CostModelNoFFTypes::Type cost_type,
                                 ActivationModelTypes::Type activation_type) {
  // create the model
  CostModelFactory factory;
  const boost::shared_ptr<crocoddyl::CostModelAbstract>& model =
      factory.create(cost_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);

  boost::shared_ptr<crocoddyl::ActuationModelAbstract> actuation =
      boost::make_shared<crocoddyl::ActuationModelFull>(state);
  const boost::shared_ptr<crocoddyl::ActuationDataAbstract>& actuation_data =
      actuation->createData();
  crocoddyl::DataCollectorActMultibody shared_data(&pinocchio_data,
                                                   actuation_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(
    CostModelNoFFTypes::Type cost_type,
    ActivationModelTypes::Type activation_type) {
  // create the model
  CostModelFactory factory;
  const boost::shared_ptr<crocoddyl::CostModelAbstract>& model =
      factory.create(cost_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);

  boost::shared_ptr<crocoddyl::ActuationModelAbstract> actuation =
      boost::make_shared<crocoddyl::ActuationModelFull>(state);
  const boost::shared_ptr<crocoddyl::ActuationDataAbstract>& actuation_data =
      actuation->createData();
  crocoddyl::DataCollectorActMultibody shared_data(&pinocchio_data,
                                                   actuation_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(
    CostModelNoFFTypes::Type cost_type,
    ActivationModelTypes::Type activation_type) {
  boost::test_tools::output_test_stream test_name;
  test_name << "test_" << cost_type << "_" << activation_type
            << "_StateMultibody_TalosArm";
  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, activation_type)));
  ts->add(BOOST_TEST_CASE(
      boost::bind(&test_calc_against_numdiff, cost_type, activation_type)));
  ts->add(BOOST_TEST_CASE(boost::bind(&test_partial_derivatives_against_numdiff,
                                      cost_type, activation_type)));
  ts->add(BOOST_TEST_CASE(
      boost::bind(&test_dimensions_in_cost_sum, cost_type, activation_type)));
  ts->add(BOOST_TEST_CASE(boost::bind(&test_partial_derivatives_in_cost_sum,
                                      cost_type, activation_type)));
  framework::master_test_suite().add(ts);
}

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