Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	unittest/test_actuation.cpp	Lines:	110	111	99.1 %
Date:	2024-02-13 11:12:33	Branches:	209	406	51.5 %


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2019-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 "factory/actuation.hpp"
#include "unittest_common.hpp"

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

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

void test_construct_data(ActuationModelTypes::Type actuation_type,
                         StateModelTypes::Type state_type) {
  // create the model
  ActuationModelFactory factory;
  const boost::shared_ptr<crocoddyl::ActuationModelAbstract>& model =
      factory.create(actuation_type, state_type);

  // create the corresponding data object
  const boost::shared_ptr<crocoddyl::ActuationDataAbstract>& data =
      model->createData();
  if (!data)
    throw std::runtime_error("[test_construct_data] Data pointer is dead.");
}

void test_calc_returns_tau(ActuationModelTypes::Type actuation_type,
                           StateModelTypes::Type state_type) {
  // create the model
  ActuationModelFactory factory;
  const boost::shared_ptr<crocoddyl::ActuationModelAbstract>& model =
      factory.create(actuation_type, state_type);

  // create the corresponding data object
  const boost::shared_ptr<crocoddyl::ActuationDataAbstract>& data =
      model->createData();

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

  // Getting the state dimension from calc() call
  model->calc(data, x, u);

  BOOST_CHECK(static_cast<std::size_t>(data->tau.size()) ==
              model->get_state()->get_nv());
}

void test_actuationSet(ActuationModelTypes::Type actuation_type,
                       StateModelTypes::Type state_type) {
  // create the model
  ActuationModelFactory factory;
  const boost::shared_ptr<crocoddyl::ActuationModelAbstract>& model =
      factory.create(actuation_type, state_type);

  // create the corresponding data object and set the cost to nan
  const boost::shared_ptr<crocoddyl::ActuationDataAbstract>& data =
      model->createData();

  crocoddyl::ActuationModelNumDiff model_num_diff(model);
  const boost::shared_ptr<crocoddyl::ActuationDataAbstract>& data_num_diff =
      model_num_diff.createData();

  // 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 selection matrix
  model->calc(data, x, u);
  model_num_diff.calc(data_num_diff, x, u);
  model_num_diff.calcDiff(data_num_diff, x, u);

  const std::size_t nv = model->get_state()->get_nv();
  Eigen::MatrixXd S =
      data_num_diff->dtau_du * pseudoInverse(data_num_diff->dtau_du);
  for (std::size_t k = 0; k < nv; ++k) {
    if (fabs(S(k, k)) < std::numeric_limits<double>::epsilon()) {
      BOOST_CHECK(data->tau_set[k] == false);
    } else {
      BOOST_CHECK(data->tau_set[k] == true);
    }
  }
}

void test_partial_derivatives_against_numdiff(
    ActuationModelTypes::Type actuation_type,
    StateModelTypes::Type state_type) {
  // create the model
  ActuationModelFactory factory;
  const boost::shared_ptr<crocoddyl::ActuationModelAbstract>& model =
      factory.create(actuation_type, state_type);

  // create the corresponding data object and set the cost to nan
  const boost::shared_ptr<crocoddyl::ActuationDataAbstract>& data =
      model->createData();

  crocoddyl::ActuationModelNumDiff model_num_diff(model);
  const boost::shared_ptr<crocoddyl::ActuationDataAbstract>& data_num_diff =
      model_num_diff.createData();

  // 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 actuation 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->dtau_dx - data_num_diff->dtau_dx).isZero(tol));
  BOOST_CHECK((data->dtau_du - data_num_diff->dtau_du).isZero(tol));

  // Computing the actuation derivatives
  x = model->get_state()->rand();
  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->dtau_dx - data_num_diff->dtau_dx).isZero(tol));
}

void test_commands(ActuationModelTypes::Type actuation_type,
                   StateModelTypes::Type state_type) {
  // create the model
  ActuationModelFactory factory;
  const boost::shared_ptr<crocoddyl::ActuationModelAbstract>& model =
      factory.create(actuation_type, state_type);

  // create the corresponding data object and set the cost to nan
  const boost::shared_ptr<crocoddyl::ActuationDataAbstract>& data =
      model->createData();

  crocoddyl::ActuationModelNumDiff model_num_diff(model);
  const boost::shared_ptr<crocoddyl::ActuationDataAbstract>& data_num_diff =
      model_num_diff.createData();

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

  // Computing the actuation commands
  model->commands(data, x, tau);
  model_num_diff.commands(data_num_diff, x, tau);

  // Checking the joint torques
  double tol = sqrt(model_num_diff.get_disturbance());
  BOOST_CHECK((data->u - data_num_diff->u).isZero(tol));
}

void test_torqueTransform(ActuationModelTypes::Type actuation_type,
                          StateModelTypes::Type state_type) {
  // create the model
  ActuationModelFactory factory;
  const boost::shared_ptr<crocoddyl::ActuationModelAbstract>& model =
      factory.create(actuation_type, state_type);

  // create the corresponding data object and set the cost to nan
  const boost::shared_ptr<crocoddyl::ActuationDataAbstract>& data =
      model->createData();

  crocoddyl::ActuationModelNumDiff model_num_diff(model);
  const boost::shared_ptr<crocoddyl::ActuationDataAbstract>& data_num_diff =
      model_num_diff.createData();

  // 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 torque transform
  model->torqueTransform(data, x, u);
  model_num_diff.torqueTransform(data_num_diff, x, u);

  // Checking the torque transform
  // 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->Mtau - data_num_diff->Mtau).isZero(tol));
}

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

void register_actuation_model_unit_tests(
    ActuationModelTypes::Type actuation_type,
    StateModelTypes::Type state_type) {
  boost::test_tools::output_test_stream test_name;
  test_name << "test_" << actuation_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_construct_data, actuation_type, state_type)));
  ts->add(BOOST_TEST_CASE(
      boost::bind(&test_calc_returns_tau, actuation_type, state_type)));
  ts->add(BOOST_TEST_CASE(
      boost::bind(&test_actuationSet, actuation_type, state_type)));
  ts->add(BOOST_TEST_CASE(boost::bind(&test_partial_derivatives_against_numdiff,
                                      actuation_type, state_type)));
  ts->add(
      BOOST_TEST_CASE(boost::bind(&test_commands, actuation_type, state_type)));
  ts->add(BOOST_TEST_CASE(
      boost::bind(&test_torqueTransform, actuation_type, state_type)));
  framework::master_test_suite().add(ts);
}

bool init_function() {
  for (size_t i = 0; i < StateModelTypes::all.size(); ++i) {
    register_actuation_model_unit_tests(ActuationModelTypes::ActuationModelFull,
                                        StateModelTypes::all[i]);
  }
  for (size_t i = 0; i < StateModelTypes::all.size(); ++i) {
    if (StateModelTypes::all[i] != StateModelTypes::StateVector &&
        StateModelTypes::all[i] != StateModelTypes::StateMultibody_Hector) {
      register_actuation_model_unit_tests(
          ActuationModelTypes::ActuationModelFloatingBase,
          StateModelTypes::all[i]);
      register_actuation_model_unit_tests(
          ActuationModelTypes::ActuationModelSquashingFull,
          StateModelTypes::all[i]);
    }
  }

  register_actuation_model_unit_tests(
      ActuationModelTypes::ActuationModelFloatingBaseThrusters,
      StateModelTypes::StateMultibody_Hector);
  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-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 "factory/actuation.hpp"
13			#include "unittest_common.hpp"
14
15			using namespace boost::unit_test;
16			using namespace crocoddyl::unittest;
17
18			//----------------------------------------------------------------------------//
19
20		18	void test_construct_data(ActuationModelTypes::Type actuation_type,
21			StateModelTypes::Type state_type) {
22			// create the model
23	✓✗	36	ActuationModelFactory factory;
24			const boost::shared_ptr<crocoddyl::ActuationModelAbstract>& model =
25	✓✗	36	factory.create(actuation_type, state_type);
26
27			// create the corresponding data object
28			const boost::shared_ptr<crocoddyl::ActuationDataAbstract>& data =
29	✓✗	36	model->createData();
30	✗✓	18	if (!data)
31			throw std::runtime_error("[test_construct_data] Data pointer is dead.");
32		18	}
33
34		18	void test_calc_returns_tau(ActuationModelTypes::Type actuation_type,
35			StateModelTypes::Type state_type) {
36			// create the model
37	✓✗	36	ActuationModelFactory factory;
38			const boost::shared_ptr<crocoddyl::ActuationModelAbstract>& model =
39	✓✗	36	factory.create(actuation_type, state_type);
40
41			// create the corresponding data object
42			const boost::shared_ptr<crocoddyl::ActuationDataAbstract>& data =
43	✓✗	36	model->createData();
44
45			// Generating random state and control vectors
46	✓✗	36	const Eigen::VectorXd x = model->get_state()->rand();
47	✓✗✓✗	36	const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
48
49			// Getting the state dimension from calc() call
50	✓✗✓✗ ✓✗	18	model->calc(data, x, u);
51
52	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	18	BOOST_CHECK(static_cast<std::size_t>(data->tau.size()) ==
53			model->get_state()->get_nv());
54		18	}
55
56		18	void test_actuationSet(ActuationModelTypes::Type actuation_type,
57			StateModelTypes::Type state_type) {
58			// create the model
59	✓✗	36	ActuationModelFactory factory;
60			const boost::shared_ptr<crocoddyl::ActuationModelAbstract>& model =
61	✓✗	36	factory.create(actuation_type, state_type);
62
63			// create the corresponding data object and set the cost to nan
64			const boost::shared_ptr<crocoddyl::ActuationDataAbstract>& data =
65	✓✗	36	model->createData();
66
67	✓✗	36	crocoddyl::ActuationModelNumDiff model_num_diff(model);
68			const boost::shared_ptr<crocoddyl::ActuationDataAbstract>& data_num_diff =
69	✓✗	36	model_num_diff.createData();
70
71			// Generating random values for the state and control
72	✓✗	36	Eigen::VectorXd x = model->get_state()->rand();
73	✓✗✓✗	36	const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
74
75			// Computing the selection matrix
76	✓✗✓✗ ✓✗	18	model->calc(data, x, u);
77	✓✗✓✗ ✓✗	18	model_num_diff.calc(data_num_diff, x, u);
78	✓✗✓✗ ✓✗	18	model_num_diff.calcDiff(data_num_diff, x, u);
79
80		18	const std::size_t nv = model->get_state()->get_nv();
81			Eigen::MatrixXd S =
82	✓✗✓✗ ✓✗	36	data_num_diff->dtau_du * pseudoInverse(data_num_diff->dtau_du);
83	✓✓	376	for (std::size_t k = 0; k < nv; ++k) {
84	✓✗✓✓	358	if (fabs(S(k, k)) < std::numeric_limits<double>::epsilon()) {
85	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	22	BOOST_CHECK(data->tau_set[k] == false);
86			} else {
87	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	336	BOOST_CHECK(data->tau_set[k] == true);
88			}
89			}
90		18	}
91
92		18	void test_partial_derivatives_against_numdiff(
93			ActuationModelTypes::Type actuation_type,
94			StateModelTypes::Type state_type) {
95			// create the model
96	✓✗	36	ActuationModelFactory factory;
97			const boost::shared_ptr<crocoddyl::ActuationModelAbstract>& model =
98	✓✗	36	factory.create(actuation_type, state_type);
99
100			// create the corresponding data object and set the cost to nan
101			const boost::shared_ptr<crocoddyl::ActuationDataAbstract>& data =
102	✓✗	36	model->createData();
103
104	✓✗	36	crocoddyl::ActuationModelNumDiff model_num_diff(model);
105			const boost::shared_ptr<crocoddyl::ActuationDataAbstract>& data_num_diff =
106	✓✗	36	model_num_diff.createData();
107
108			// Generating random values for the state and control
109	✓✗	36	Eigen::VectorXd x = model->get_state()->rand();
110	✓✗✓✗	36	const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
111
112			// Computing the actuation derivatives
113	✓✗✓✗ ✓✗	18	model->calc(data, x, u);
114	✓✗✓✗ ✓✗	18	model->calcDiff(data, x, u);
115	✓✗✓✗ ✓✗	18	model_num_diff.calc(data_num_diff, x, u);
116	✓✗✓✗ ✓✗	18	model_num_diff.calcDiff(data_num_diff, x, u);
117			// Tolerance defined as in
118			// http://www.it.uom.gr/teaching/linearalgebra/NumericalRecipiesInC/c5-7.pdf
119		18	double tol = std::pow(model_num_diff.get_disturbance(), 1. / 3.);
120	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	18	BOOST_CHECK((data->dtau_dx - data_num_diff->dtau_dx).isZero(tol));
121	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	18	BOOST_CHECK((data->dtau_du - data_num_diff->dtau_du).isZero(tol));
122
123			// Computing the actuation derivatives
124	✓✗	18	x = model->get_state()->rand();
125	✓✗	18	model->calc(data, x);
126	✓✗	18	model->calcDiff(data, x);
127	✓✗✓✗	18	model_num_diff.calc(data_num_diff, x);
128	✓✗✓✗	18	model_num_diff.calcDiff(data_num_diff, x);
129
130			// Checking the partial derivatives against numdiff
131	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	18	BOOST_CHECK((data->dtau_dx - data_num_diff->dtau_dx).isZero(tol));
132		18	}
133
134		18	void test_commands(ActuationModelTypes::Type actuation_type,
135			StateModelTypes::Type state_type) {
136			// create the model
137	✓✗	36	ActuationModelFactory factory;
138			const boost::shared_ptr<crocoddyl::ActuationModelAbstract>& model =
139	✓✗	36	factory.create(actuation_type, state_type);
140
141			// create the corresponding data object and set the cost to nan
142			const boost::shared_ptr<crocoddyl::ActuationDataAbstract>& data =
143	✓✗	36	model->createData();
144
145	✓✗	36	crocoddyl::ActuationModelNumDiff model_num_diff(model);
146			const boost::shared_ptr<crocoddyl::ActuationDataAbstract>& data_num_diff =
147	✓✗	36	model_num_diff.createData();
148
149			// Generating random values for the state and control
150	✓✗	36	Eigen::VectorXd x = model->get_state()->rand();
151			const Eigen::VectorXd tau =
152	✓✗✓✗	36	Eigen::VectorXd::Random(model->get_state()->get_nv());
153
154			// Computing the actuation commands
155	✓✗✓✗ ✓✗	18	model->commands(data, x, tau);
156	✓✗✓✗ ✓✗	18	model_num_diff.commands(data_num_diff, x, tau);
157
158			// Checking the joint torques
159		18	double tol = sqrt(model_num_diff.get_disturbance());
160	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	18	BOOST_CHECK((data->u - data_num_diff->u).isZero(tol));
161		18	}
162
163		18	void test_torqueTransform(ActuationModelTypes::Type actuation_type,
164			StateModelTypes::Type state_type) {
165			// create the model
166	✓✗	36	ActuationModelFactory factory;
167			const boost::shared_ptr<crocoddyl::ActuationModelAbstract>& model =
168	✓✗	36	factory.create(actuation_type, state_type);
169
170			// create the corresponding data object and set the cost to nan
171			const boost::shared_ptr<crocoddyl::ActuationDataAbstract>& data =
172	✓✗	36	model->createData();
173
174	✓✗	36	crocoddyl::ActuationModelNumDiff model_num_diff(model);
175			const boost::shared_ptr<crocoddyl::ActuationDataAbstract>& data_num_diff =
176	✓✗	36	model_num_diff.createData();
177
178			// Generating random values for the state and control
179	✓✗	36	Eigen::VectorXd x = model->get_state()->rand();
180	✓✗✓✗	36	const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
181
182			// Computing the torque transform
183	✓✗✓✗ ✓✗	18	model->torqueTransform(data, x, u);
184	✓✗✓✗ ✓✗	18	model_num_diff.torqueTransform(data_num_diff, x, u);
185
186			// Checking the torque transform
187			// Tolerance defined as in
188			// http://www.it.uom.gr/teaching/linearalgebra/NumericalRecipiesInC/c5-7.pdf
189		18	double tol = std::pow(model_num_diff.get_disturbance(), 1. / 3.);
190	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	18	BOOST_CHECK((data->Mtau - data_num_diff->Mtau).isZero(tol));
191		18	}
192
193			//----------------------------------------------------------------------------//
194
195		18	void register_actuation_model_unit_tests(
196			ActuationModelTypes::Type actuation_type,
197			StateModelTypes::Type state_type) {
198	✓✗✓✗	36	boost::test_tools::output_test_stream test_name;
199	✓✗✓✗ ✓✗✓✗	18	test_name << "test_" << actuation_type << "_" << state_type;
200	✓✗✓✗ ✓✗✓✗	18	std::cout << "Running " << test_name.str() << std::endl;
201	✓✗✓✗ ✓✗✓✗	18	test_suite* ts = BOOST_TEST_SUITE(test_name.str());
202	✓✗✓✗ ✓✗✓✗ ✓✗	18	ts->add(BOOST_TEST_CASE(
203			boost::bind(&test_construct_data, actuation_type, state_type)));
204	✓✗✓✗ ✓✗✓✗ ✓✗	18	ts->add(BOOST_TEST_CASE(
205			boost::bind(&test_calc_returns_tau, actuation_type, state_type)));
206	✓✗✓✗ ✓✗✓✗ ✓✗	18	ts->add(BOOST_TEST_CASE(
207			boost::bind(&test_actuationSet, actuation_type, state_type)));
208	✓✗✓✗ ✓✗✓✗ ✓✗	18	ts->add(BOOST_TEST_CASE(boost::bind(&test_partial_derivatives_against_numdiff,
209			actuation_type, state_type)));
210		18	ts->add(
211	✓✗✓✗ ✓✗✓✗ ✓✗	18	BOOST_TEST_CASE(boost::bind(&test_commands, actuation_type, state_type)));
212	✓✗✓✗ ✓✗✓✗ ✓✗	18	ts->add(BOOST_TEST_CASE(
213			boost::bind(&test_torqueTransform, actuation_type, state_type)));
214	✓✗✓✗ ✓✗	18	framework::master_test_suite().add(ts);
215		18	}
216
217		1	bool init_function() {
218	✓✓	8	for (size_t i = 0; i < StateModelTypes::all.size(); ++i) {
219		7	register_actuation_model_unit_tests(ActuationModelTypes::ActuationModelFull,
220		7	StateModelTypes::all[i]);
221			}
222	✓✓	8	for (size_t i = 0; i < StateModelTypes::all.size(); ++i) {
223	✓✓✓✓	13	if (StateModelTypes::all[i] != StateModelTypes::StateVector &&
224	✓✓	6	StateModelTypes::all[i] != StateModelTypes::StateMultibody_Hector) {
225		5	register_actuation_model_unit_tests(
226			ActuationModelTypes::ActuationModelFloatingBase,
227		5	StateModelTypes::all[i]);
228		5	register_actuation_model_unit_tests(
229			ActuationModelTypes::ActuationModelSquashingFull,
230		5	StateModelTypes::all[i]);
231			}
232			}
233
234		1	register_actuation_model_unit_tests(
235			ActuationModelTypes::ActuationModelFloatingBaseThrusters,
236			StateModelTypes::StateMultibody_Hector);
237		1	return true;
238			}
239
240		1	int main(int argc, char** argv) {
241		1	return ::boost::unit_test::unit_test_main(&init_function, argc, argv);
242			}