Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	unittest/test_residuals.cpp	Lines:	165	165	100.0 %
Date:	2024-02-13 11:12:33	Branches:	292	568	51.4 %


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2021-2024, 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/core/residuals/control.hpp"
#include "crocoddyl/core/residuals/joint-acceleration.hpp"
#include "crocoddyl/core/residuals/joint-effort.hpp"
#include "crocoddyl/multibody/data/multibody.hpp"
#include "crocoddyl/multibody/residuals/centroidal-momentum.hpp"
#include "crocoddyl/multibody/residuals/com-position.hpp"
#include "crocoddyl/multibody/residuals/state.hpp"
#include "factory/actuation.hpp"
#include "factory/residual.hpp"
#include "unittest_common.hpp"

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

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

void test_calc_returns_a_residual(ResidualModelTypes::Type residual_type,
                                  StateModelTypes::Type state_type,
                                  ActuationModelTypes::Type actuation_type) {
  // Create the model
  ResidualModelFactory residual_factory;
  ActuationModelFactory actuation_factory;
  boost::shared_ptr<crocoddyl::ActuationModelAbstract> actuation_model =
      actuation_factory.create(actuation_type, state_type);
  const boost::shared_ptr<crocoddyl::ResidualModelAbstract>& model =
      residual_factory.create(residual_type, state_type,
                              actuation_model->get_nu());

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

  // Create the corresponding shared data
  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);
  const boost::shared_ptr<crocoddyl::ActuationDataAbstract>& actuation_data =
      actuation_model->createData();
  crocoddyl::DataCollectorActMultibody shared_data(&pinocchio_data,
                                                   actuation_data);

  // create the residual data
  const boost::shared_ptr<crocoddyl::ResidualDataAbstract>& data =
      model->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);
  crocoddyl::unittest::updateActuation(actuation_model, actuation_data, x, u);

  // Getting the residual value computed by calc()
  data->r *= nan("");
  model->calc(data, x, u);

  // Checking that calc returns a residual value
  for (std::size_t i = 0; i < model->get_nr(); ++i)
    BOOST_CHECK(!std::isnan(data->r(i)));
}

void test_calc_against_numdiff(ResidualModelTypes::Type residual_type,
                               StateModelTypes::Type state_type,
                               ActuationModelTypes::Type actuation_type) {
  using namespace boost::placeholders;

  // Create the model
  ResidualModelFactory residual_factory;
  ActuationModelFactory actuation_factory;
  boost::shared_ptr<crocoddyl::ActuationModelAbstract> actuation_model =
      actuation_factory.create(actuation_type, state_type);
  const boost::shared_ptr<crocoddyl::ResidualModelAbstract>& model =
      residual_factory.create(residual_type, state_type,
                              actuation_model->get_nu());

  // Create the corresponding shared data
  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);
  const boost::shared_ptr<crocoddyl::ActuationDataAbstract>& actuation_data =
      actuation_model->createData();
  crocoddyl::DataCollectorActMultibody shared_data(&pinocchio_data,
                                                   actuation_data);

  // Create the residual data
  const boost::shared_ptr<crocoddyl::ResidualDataAbstract>& data =
      model->createData(&shared_data);

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

  // Computing the residual
  crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
  actuation_model->calc(actuation_data, x, u);
  model->calc(data, x, u);

  // Computing the residual from num diff
  std::vector<crocoddyl::ResidualModelNumDiff::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);
  model_num_diff.calc(data_num_diff, x, u);

  // Checking the partial derivatives against NumDiff
  BOOST_CHECK(data->r == data_num_diff->r);
}

void test_partial_derivatives_against_numdiff(
    ResidualModelTypes::Type residual_type, StateModelTypes::Type state_type,
    ActuationModelTypes::Type actuation_type) {
  using namespace boost::placeholders;

  // Create the model
  ResidualModelFactory residual_factory;
  ActuationModelFactory actuation_factory;
  boost::shared_ptr<crocoddyl::ActuationModelAbstract> actuation_model =
      actuation_factory.create(actuation_type, state_type);
  const boost::shared_ptr<crocoddyl::ResidualModelAbstract>& model =
      residual_factory.create(residual_type, state_type,
                              actuation_model->get_nu());

  // Create the corresponding shared data
  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);
  const boost::shared_ptr<crocoddyl::ActuationDataAbstract>& actuation_data =
      actuation_model->createData();
  crocoddyl::DataCollectorActMultibody shared_data(&pinocchio_data,
                                                   actuation_data);

  // Create the residual data
  const boost::shared_ptr<crocoddyl::ResidualDataAbstract>& data =
      model->createData(&shared_data);

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

  // Computing the residual derivatives
  crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
  actuation_model->calc(actuation_data, x, u);
  actuation_model->calcDiff(actuation_data, x, u);
  model->calc(data, x, u);
  model->calcDiff(data, x, u);

  // Computing the residual derivatives via numerical differentiation
  std::vector<crocoddyl::ResidualModelNumDiff::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);
  model_num_diff.calc(data_num_diff, x, u);
  model_num_diff.calcDiff(data_num_diff, x, u);

  // Checking the partial derivatives against numdiff
  // 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->Rx - data_num_diff->Rx).isZero(tol));
  BOOST_CHECK((data->Ru - data_num_diff->Ru).isZero(tol));

  // Computing the residual 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);

  // Computing the residual derivatives via numerical differentiation
  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->Rx - data_num_diff->Rx).isZero(tol));
}

void test_reference() {
  ResidualModelFactory factory;
  StateModelTypes::Type state_type = StateModelTypes::StateMultibody_Talos;
  ActuationModelTypes::Type actuation_type =
      ActuationModelTypes::ActuationModelFloatingBase;
  StateModelFactory state_factory;
  ActuationModelFactory actuation_factory;
  boost::shared_ptr<crocoddyl::StateMultibody> state =
      boost::static_pointer_cast<crocoddyl::StateMultibody>(
          state_factory.create(state_type));
  boost::shared_ptr<crocoddyl::ActuationModelAbstract> actuation =
      actuation_factory.create(actuation_type, state_type);

  const std::size_t nu = actuation->get_nu();
  const std::size_t nv = state->get_nv();

  // Test reference in state residual
  crocoddyl::ResidualModelState state_residual(state, state->rand(), nu);
  Eigen::VectorXd x_ref = state_residual.get_state()->rand();
  state_residual.set_reference(x_ref);
  BOOST_CHECK((x_ref - state_residual.get_reference()).isZero());

  // Test reference in control residual
  crocoddyl::ResidualModelControl control_residual(state, nu);
  Eigen::VectorXd u_ref = Eigen::VectorXd::Random(nu);
  control_residual.set_reference(u_ref);
  BOOST_CHECK((u_ref - control_residual.get_reference()).isZero());

  // Test reference in joint-acceleration residual
  crocoddyl::ResidualModelJointAcceleration jacc_residual(state, nu);
  Eigen::VectorXd a_ref = Eigen::VectorXd::Random(nv);
  jacc_residual.set_reference(a_ref);
  BOOST_CHECK((a_ref - jacc_residual.get_reference()).isZero());

  // Test reference in joint-effort residual
  crocoddyl::ResidualModelJointEffort jeff_residual(state, actuation, nu);
  Eigen::VectorXd tau_ref = Eigen::VectorXd::Random(nu);
  jeff_residual.set_reference(tau_ref);
  BOOST_CHECK((tau_ref - jeff_residual.get_reference()).isZero());

  // Test reference in centroidal-momentum residual
  crocoddyl::ResidualModelCentroidalMomentum cmon_residual(
      state, Eigen::Matrix<double, 6, 1>::Zero());
  Eigen::Matrix<double, 6, 1> h_ref = Eigen::Matrix<double, 6, 1>::Random();
  cmon_residual.set_reference(h_ref);
  BOOST_CHECK((h_ref - cmon_residual.get_reference()).isZero());

  // Test reference in com-position residual
  crocoddyl::ResidualModelCoMPosition c_residual(state,
                                                 Eigen::Vector3d::Zero());
  Eigen::Vector3d c_ref = Eigen::Vector3d::Random();
  c_residual.set_reference(c_ref);
  BOOST_CHECK((c_ref - c_residual.get_reference()).isZero());
}

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

void register_residual_model_unit_tests(
    ResidualModelTypes::Type residual_type, StateModelTypes::Type state_type,
    ActuationModelTypes::Type actuation_type) {
  boost::test_tools::output_test_stream test_name;
  test_name << "test_" << residual_type << "_" << state_type << "_"
            << actuation_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, residual_type,
                                  state_type, actuation_type)));
  ts->add(BOOST_TEST_CASE(boost::bind(&test_calc_against_numdiff, residual_type,
                                      state_type, actuation_type)));
  ts->add(
      BOOST_TEST_CASE(boost::bind(&test_partial_derivatives_against_numdiff,
                                  residual_type, state_type, actuation_type)));
  framework::master_test_suite().add(ts);
}

void regiter_residual_reference_unit_tests() {
  boost::test_tools::output_test_stream test_name;
  test_name << "test_reference";
  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_reference)));
  framework::master_test_suite().add(ts);
}

bool init_function() {
  // Test all residuals available with all the activation types with all
  // available states types.
  for (size_t residual_type = 0; residual_type < ResidualModelTypes::all.size();
       ++residual_type) {
    for (size_t state_type =
             StateModelTypes::all[StateModelTypes::StateMultibody_TalosArm];
         state_type < StateModelTypes::all.size(); ++state_type) {
      for (size_t actuation_type = 0;
           actuation_type < ActuationModelTypes::all.size(); ++actuation_type) {
        if (ActuationModelTypes::all[actuation_type] !=
            ActuationModelTypes::ActuationModelFloatingBaseThrusters) {
          register_residual_model_unit_tests(
              ResidualModelTypes::all[residual_type],
              StateModelTypes::all[state_type],
              ActuationModelTypes::all[actuation_type]);
        } else if (StateModelTypes::all[state_type] !=
                       StateModelTypes::StateMultibody_TalosArm &&
                   StateModelTypes::all[state_type] !=
                       StateModelTypes::StateMultibodyContact2D_TalosArm) {
          register_residual_model_unit_tests(
              ResidualModelTypes::all[residual_type],
              StateModelTypes::all[state_type],
              ActuationModelTypes::all[actuation_type]);
        }
      }
    }
  }
  regiter_residual_reference_unit_tests();
  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) 2021-2024, 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/core/residuals/control.hpp"
13			#include "crocoddyl/core/residuals/joint-acceleration.hpp"
14			#include "crocoddyl/core/residuals/joint-effort.hpp"
15			#include "crocoddyl/multibody/data/multibody.hpp"
16			#include "crocoddyl/multibody/residuals/centroidal-momentum.hpp"
17			#include "crocoddyl/multibody/residuals/com-position.hpp"
18			#include "crocoddyl/multibody/residuals/state.hpp"
19			#include "factory/actuation.hpp"
20			#include "factory/residual.hpp"
21			#include "unittest_common.hpp"
22
23			using namespace boost::unit_test;
24			using namespace crocoddyl::unittest;
25
26			//----------------------------------------------------------------------------//
27
28		180	void test_calc_returns_a_residual(ResidualModelTypes::Type residual_type,
29			StateModelTypes::Type state_type,
30			ActuationModelTypes::Type actuation_type) {
31			// Create the model
32	✓✗	360	ResidualModelFactory residual_factory;
33	✓✗	360	ActuationModelFactory actuation_factory;
34			boost::shared_ptr<crocoddyl::ActuationModelAbstract> actuation_model =
35	✓✗	360	actuation_factory.create(actuation_type, state_type);
36			const boost::shared_ptr<crocoddyl::ResidualModelAbstract>& model =
37			residual_factory.create(residual_type, state_type,
38	✓✗	360	actuation_model->get_nu());
39
40			// Run the print function
41	✓✗	360	std::ostringstream tmp;
42	✓✗	180	tmp << *model;
43
44			// Create the corresponding shared data
45			const boost::shared_ptr<crocoddyl::StateMultibody>& state =
46		360	boost::static_pointer_cast<crocoddyl::StateMultibody>(model->get_state());
47		180	pinocchio::Model& pinocchio_model = *state->get_pinocchio().get();
48	✓✗	360	pinocchio::Data pinocchio_data(pinocchio_model);
49			const boost::shared_ptr<crocoddyl::ActuationDataAbstract>& actuation_data =
50	✓✗	360	actuation_model->createData();
51			crocoddyl::DataCollectorActMultibody shared_data(&pinocchio_data,
52	✓✗	360	actuation_data);
53
54			// create the residual data
55			const boost::shared_ptr<crocoddyl::ResidualDataAbstract>& data =
56	✓✗	360	model->createData(&shared_data);
57
58			// Generating random values for the state and control
59	✓✗	360	const Eigen::VectorXd x = model->get_state()->rand();
60	✓✗✓✗	360	const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
61
62			// Compute all the pinocchio function needed for the models.
63	✓✗✓✗	180	crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
64	✓✗	180	crocoddyl::unittest::updateActuation(actuation_model, actuation_data, x, u);
65
66			// Getting the residual value computed by calc()
67	✓✗	180	data->r *= nan("");
68	✓✗✓✗ ✓✗	180	model->calc(data, x, u);
69
70			// Checking that calc returns a residual value
71	✓✓	2270	for (std::size_t i = 0; i < model->get_nr(); ++i)
72	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2090	BOOST_CHECK(!std::isnan(data->r(i)));
73		180	}
74
75		180	void test_calc_against_numdiff(ResidualModelTypes::Type residual_type,
76			StateModelTypes::Type state_type,
77			ActuationModelTypes::Type actuation_type) {
78			using namespace boost::placeholders;
79
80			// Create the model
81	✓✗	360	ResidualModelFactory residual_factory;
82	✓✗	360	ActuationModelFactory actuation_factory;
83			boost::shared_ptr<crocoddyl::ActuationModelAbstract> actuation_model =
84	✓✗	360	actuation_factory.create(actuation_type, state_type);
85			const boost::shared_ptr<crocoddyl::ResidualModelAbstract>& model =
86			residual_factory.create(residual_type, state_type,
87	✓✗	360	actuation_model->get_nu());
88
89			// Create the corresponding shared data
90			const boost::shared_ptr<crocoddyl::StateMultibody>& state =
91		360	boost::static_pointer_cast<crocoddyl::StateMultibody>(model->get_state());
92		180	pinocchio::Model& pinocchio_model = *state->get_pinocchio().get();
93	✓✗	360	pinocchio::Data pinocchio_data(pinocchio_model);
94			const boost::shared_ptr<crocoddyl::ActuationDataAbstract>& actuation_data =
95	✓✗	360	actuation_model->createData();
96			crocoddyl::DataCollectorActMultibody shared_data(&pinocchio_data,
97	✓✗	360	actuation_data);
98
99			// Create the residual data
100			const boost::shared_ptr<crocoddyl::ResidualDataAbstract>& data =
101	✓✗	360	model->createData(&shared_data);
102
103			// Create the equivalent num diff model and data.
104	✓✗	360	crocoddyl::ResidualModelNumDiff model_num_diff(model);
105			const boost::shared_ptr<crocoddyl::ResidualDataAbstract>& data_num_diff =
106	✓✗	360	model_num_diff.createData(&shared_data);
107
108			// Generating random values for the state and control
109	✓✗	360	const Eigen::VectorXd x = model->get_state()->rand();
110	✓✗✓✗	360	const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
111
112			// Computing the residual
113	✓✗✓✗	180	crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
114	✓✗✓✗ ✓✗	180	actuation_model->calc(actuation_data, x, u);
115	✓✗✓✗ ✓✗	180	model->calc(data, x, u);
116
117			// Computing the residual from num diff
118		360	std::vector<crocoddyl::ResidualModelNumDiff::ReevaluationFunction> reevals;
119	✓✗✓✗ ✓✗	180	reevals.push_back(boost::bind(&crocoddyl::unittest::updateAllPinocchio,
120			&pinocchio_model, &pinocchio_data, _1, _2));
121	✓✗✓✗ ✓✗	180	reevals.push_back(boost::bind(&crocoddyl::unittest::updateActuation,
122			actuation_model, actuation_data, _1, _2));
123	✓✗	180	model_num_diff.set_reevals(reevals);
124	✓✗✓✗ ✓✗	180	model_num_diff.calc(data_num_diff, x, u);
125
126			// Checking the partial derivatives against NumDiff
127	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	180	BOOST_CHECK(data->r == data_num_diff->r);
128		180	}
129
130		180	void test_partial_derivatives_against_numdiff(
131			ResidualModelTypes::Type residual_type, StateModelTypes::Type state_type,
132			ActuationModelTypes::Type actuation_type) {
133			using namespace boost::placeholders;
134
135			// Create the model
136	✓✗	360	ResidualModelFactory residual_factory;
137	✓✗	360	ActuationModelFactory actuation_factory;
138			boost::shared_ptr<crocoddyl::ActuationModelAbstract> actuation_model =
139	✓✗	360	actuation_factory.create(actuation_type, state_type);
140			const boost::shared_ptr<crocoddyl::ResidualModelAbstract>& model =
141			residual_factory.create(residual_type, state_type,
142	✓✗	360	actuation_model->get_nu());
143
144			// Create the corresponding shared data
145			const boost::shared_ptr<crocoddyl::StateMultibody>& state =
146		360	boost::static_pointer_cast<crocoddyl::StateMultibody>(model->get_state());
147		180	pinocchio::Model& pinocchio_model = *state->get_pinocchio().get();
148	✓✗	360	pinocchio::Data pinocchio_data(pinocchio_model);
149			const boost::shared_ptr<crocoddyl::ActuationDataAbstract>& actuation_data =
150	✓✗	360	actuation_model->createData();
151			crocoddyl::DataCollectorActMultibody shared_data(&pinocchio_data,
152	✓✗	360	actuation_data);
153
154			// Create the residual data
155			const boost::shared_ptr<crocoddyl::ResidualDataAbstract>& data =
156	✓✗	360	model->createData(&shared_data);
157
158			// Create the equivalent num diff model and data.
159	✓✗	360	crocoddyl::ResidualModelNumDiff model_num_diff(model);
160			const boost::shared_ptr<crocoddyl::ResidualDataAbstract>& data_num_diff =
161	✓✗	360	model_num_diff.createData(&shared_data);
162
163			// Generating random values for the state and control
164	✓✗	360	Eigen::VectorXd x = model->get_state()->rand();
165	✓✗✓✗	360	const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
166
167			// Computing the residual derivatives
168	✓✗✓✗	180	crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
169	✓✗✓✗ ✓✗	180	actuation_model->calc(actuation_data, x, u);
170	✓✗✓✗ ✓✗	180	actuation_model->calcDiff(actuation_data, x, u);
171	✓✗✓✗ ✓✗	180	model->calc(data, x, u);
172	✓✗✓✗ ✓✗	180	model->calcDiff(data, x, u);
173
174			// Computing the residual derivatives via numerical differentiation
175		360	std::vector<crocoddyl::ResidualModelNumDiff::ReevaluationFunction> reevals;
176	✓✗✓✗ ✓✗	180	reevals.push_back(boost::bind(&crocoddyl::unittest::updateAllPinocchio,
177			&pinocchio_model, &pinocchio_data, _1, _2));
178	✓✗✓✗ ✓✗	180	reevals.push_back(boost::bind(&crocoddyl::unittest::updateActuation,
179			actuation_model, actuation_data, _1, _2));
180	✓✗	180	model_num_diff.set_reevals(reevals);
181	✓✗✓✗ ✓✗	180	model_num_diff.calc(data_num_diff, x, u);
182	✓✗✓✗ ✓✗	180	model_num_diff.calcDiff(data_num_diff, x, u);
183
184			// Checking the partial derivatives against numdiff
185			// Tolerance defined as in
186			// http://www.it.uom.gr/teaching/linearalgebra/NumericalRecipiesInC/c5-7.pdf
187		180	double tol = std::pow(model_num_diff.get_disturbance(), 1. / 3.);
188	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	180	BOOST_CHECK((data->Rx - data_num_diff->Rx).isZero(tol));
189	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	180	BOOST_CHECK((data->Ru - data_num_diff->Ru).isZero(tol));
190
191			// Computing the residual derivatives
192	✓✗	180	x = model->get_state()->rand();
193	✓✗✓✗	180	crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
194	✓✗	180	actuation_model->calc(actuation_data, x);
195	✓✗	180	actuation_model->calcDiff(actuation_data, x);
196
197			// Computing the residual derivatives via numerical differentiation
198	✓✗✓✗	180	model->calc(data, x);
199	✓✗✓✗	180	model->calcDiff(data, x);
200	✓✗✓✗	180	model_num_diff.calc(data_num_diff, x);
201	✓✗✓✗	180	model_num_diff.calcDiff(data_num_diff, x);
202
203			// Checking the partial derivatives against numdiff
204	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	180	BOOST_CHECK((data->Rx - data_num_diff->Rx).isZero(tol));
205		180	}
206
207		1	void test_reference() {
208	✓✗	2	ResidualModelFactory factory;
209		1	StateModelTypes::Type state_type = StateModelTypes::StateMultibody_Talos;
210		1	ActuationModelTypes::Type actuation_type =
211			ActuationModelTypes::ActuationModelFloatingBase;
212	✓✗	2	StateModelFactory state_factory;
213	✓✗	2	ActuationModelFactory actuation_factory;
214			boost::shared_ptr<crocoddyl::StateMultibody> state =
215			boost::static_pointer_cast<crocoddyl::StateMultibody>(
216	✓✗	2	state_factory.create(state_type));
217			boost::shared_ptr<crocoddyl::ActuationModelAbstract> actuation =
218	✓✗	2	actuation_factory.create(actuation_type, state_type);
219
220		1	const std::size_t nu = actuation->get_nu();
221		1	const std::size_t nv = state->get_nv();
222
223			// Test reference in state residual
224	✓✗✓✗	3	crocoddyl::ResidualModelState state_residual(state, state->rand(), nu);
225	✓✗	2	Eigen::VectorXd x_ref = state_residual.get_state()->rand();
226	✓✗	1	state_residual.set_reference(x_ref);
227	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK((x_ref - state_residual.get_reference()).isZero());
228
229			// Test reference in control residual
230	✓✗	2	crocoddyl::ResidualModelControl control_residual(state, nu);
231	✓✗✓✗	2	Eigen::VectorXd u_ref = Eigen::VectorXd::Random(nu);
232	✓✗	1	control_residual.set_reference(u_ref);
233	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK((u_ref - control_residual.get_reference()).isZero());
234
235			// Test reference in joint-acceleration residual
236	✓✗	2	crocoddyl::ResidualModelJointAcceleration jacc_residual(state, nu);
237	✓✗✓✗	2	Eigen::VectorXd a_ref = Eigen::VectorXd::Random(nv);
238	✓✗	1	jacc_residual.set_reference(a_ref);
239	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK((a_ref - jacc_residual.get_reference()).isZero());
240
241			// Test reference in joint-effort residual
242	✓✗	3	crocoddyl::ResidualModelJointEffort jeff_residual(state, actuation, nu);
243	✓✗✓✗	2	Eigen::VectorXd tau_ref = Eigen::VectorXd::Random(nu);
244	✓✗	1	jeff_residual.set_reference(tau_ref);
245	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK((tau_ref - jeff_residual.get_reference()).isZero());
246
247			// Test reference in centroidal-momentum residual
248			crocoddyl::ResidualModelCentroidalMomentum cmon_residual(
249	✓✗✓✗ ✓✗	2	state, Eigen::Matrix<double, 6, 1>::Zero());
250	✓✗✓✗	1	Eigen::Matrix<double, 6, 1> h_ref = Eigen::Matrix<double, 6, 1>::Random();
251	✓✗	1	cmon_residual.set_reference(h_ref);
252	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK((h_ref - cmon_residual.get_reference()).isZero());
253
254			// Test reference in com-position residual
255			crocoddyl::ResidualModelCoMPosition c_residual(state,
256	✓✗✓✗ ✓✗	2	Eigen::Vector3d::Zero());
257	✓✗✓✗	1	Eigen::Vector3d c_ref = Eigen::Vector3d::Random();
258	✓✗	1	c_residual.set_reference(c_ref);
259	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	1	BOOST_CHECK((c_ref - c_residual.get_reference()).isZero());
260		1	}
261
262			//----------------------------------------------------------------------------//
263
264		180	void register_residual_model_unit_tests(
265			ResidualModelTypes::Type residual_type, StateModelTypes::Type state_type,
266			ActuationModelTypes::Type actuation_type) {
267	✓✗✓✗	360	boost::test_tools::output_test_stream test_name;
268	✓✗✓✗ ✓✗✓✗ ✓✗	180	test_name << "test_" << residual_type << "_" << state_type << "_"
269	✓✗	180	<< actuation_type;
270	✓✗✓✗ ✓✗✓✗	180	std::cout << "Running " << test_name.str() << std::endl;
271	✓✗✓✗ ✓✗✓✗	180	test_suite* ts = BOOST_TEST_SUITE(test_name.str());
272		180	ts->add(
273	✓✗✓✗ ✓✗✓✗ ✓✗	180	BOOST_TEST_CASE(boost::bind(&test_calc_returns_a_residual, residual_type,
274			state_type, actuation_type)));
275	✓✗✓✗ ✓✗✓✗ ✓✗	180	ts->add(BOOST_TEST_CASE(boost::bind(&test_calc_against_numdiff, residual_type,
276			state_type, actuation_type)));
277		180	ts->add(
278	✓✗✓✗ ✓✗✓✗ ✓✗	180	BOOST_TEST_CASE(boost::bind(&test_partial_derivatives_against_numdiff,
279			residual_type, state_type, actuation_type)));
280	✓✗✓✗ ✓✗	180	framework::master_test_suite().add(ts);
281		180	}
282
283		1	void regiter_residual_reference_unit_tests() {
284	✓✗✓✗	2	boost::test_tools::output_test_stream test_name;
285	✓✗	1	test_name << "test_reference";
286	✓✗✓✗ ✓✗✓✗	1	std::cout << "Running " << test_name.str() << std::endl;
287	✓✗✓✗ ✓✗✓✗	1	test_suite* ts = BOOST_TEST_SUITE(test_name.str());
288	✓✗✓✗ ✓✗✓✗ ✓✗	1	ts->add(BOOST_TEST_CASE(boost::bind(&test_reference)));
289	✓✗✓✗ ✓✗	1	framework::master_test_suite().add(ts);
290		1	}
291
292		1	bool init_function() {
293			// Test all residuals available with all the activation types with all
294			// available states types.
295	✓✓	11	for (size_t residual_type = 0; residual_type < ResidualModelTypes::all.size();
296			++residual_type) {
297		50	for (size_t state_type =
298		10	StateModelTypes::all[StateModelTypes::StateMultibody_TalosArm];
299	✓✓	60	state_type < StateModelTypes::all.size(); ++state_type) {
300		250	for (size_t actuation_type = 0;
301	✓✓	250	actuation_type < ActuationModelTypes::all.size(); ++actuation_type) {
302	✓✓	200	if (ActuationModelTypes::all[actuation_type] !=
303			ActuationModelTypes::ActuationModelFloatingBaseThrusters) {
304		150	register_residual_model_unit_tests(
305		150	ResidualModelTypes::all[residual_type],
306		150	StateModelTypes::all[state_type],
307		150	ActuationModelTypes::all[actuation_type]);
308		50	} else if (StateModelTypes::all[state_type] !=
309	✓✓✓✓	90	StateModelTypes::StateMultibody_TalosArm &&
310	✓✓	40	StateModelTypes::all[state_type] !=
311			StateModelTypes::StateMultibodyContact2D_TalosArm) {
312		30	register_residual_model_unit_tests(
313		30	ResidualModelTypes::all[residual_type],
314		30	StateModelTypes::all[state_type],
315		30	ActuationModelTypes::all[actuation_type]);
316			}
317			}
318			}
319			}
320		1	regiter_residual_reference_unit_tests();
321		1	return true;
322			}
323
324		1	int main(int argc, char** argv) {
325		1	return ::boost::unit_test::unit_test_main(&init_function, argc, argv);
326			}