Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	unittest/test_diff_actions.cpp	Lines:	101	105	96.2 %
Date:	2024-02-13 11:12:33	Branches:	251	546	46.0 %


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

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

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

void test_check_data(DifferentialActionModelTypes::Type action_type) {
  // create the model
  DifferentialActionModelFactory factory;
  boost::shared_ptr<crocoddyl::DifferentialActionModelAbstract> model =
      factory.create(action_type);

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

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

  BOOST_CHECK(model->checkData(data));
}

void test_calc_returns_state(DifferentialActionModelTypes::Type action_type) {
  // create the model
  DifferentialActionModelFactory factory;
  boost::shared_ptr<crocoddyl::DifferentialActionModelAbstract> model =
      factory.create(action_type);

  // create the corresponding data object
  boost::shared_ptr<crocoddyl::DifferentialActionDataAbstract> 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->xout.size()) ==
              model->get_state()->get_nv());
}

void test_calc_returns_a_cost(DifferentialActionModelTypes::Type action_type) {
  // create the model
  DifferentialActionModelFactory factory;
  boost::shared_ptr<crocoddyl::DifferentialActionModelAbstract> model =
      factory.create(action_type);

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

  // Getting the cost value computed by calc()
  const Eigen::VectorXd x = model->get_state()->rand();
  const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
  model->calc(data, x, u);

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

void test_quasi_static(DifferentialActionModelTypes::Type action_type) {
  if (action_type ==
      DifferentialActionModelTypes::
          DifferentialActionModelFreeFwdDynamics_TalosArm_Squashed)
    return;
  // create the model
  DifferentialActionModelFactory factory;
  boost::shared_ptr<crocoddyl::DifferentialActionModelAbstract> model =
      factory.create(action_type, false);

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

  // Getting the cost value computed by calc()
  Eigen::VectorXd x = model->get_state()->rand();
  x.tail(model->get_state()->get_nv()).setZero();
  Eigen::VectorXd u = Eigen::VectorXd::Zero(model->get_nu());
  model->quasiStatic(data, u, x);
  model->calc(data, x, u);

  // Check for inactive contacts
  if (action_type == DifferentialActionModelTypes::
                         DifferentialActionModelContactFwdDynamics_HyQ ||
      action_type ==
          DifferentialActionModelTypes::
              DifferentialActionModelContactFwdDynamicsWithFriction_HyQ ||
      action_type == DifferentialActionModelTypes::
                         DifferentialActionModelContactFwdDynamics_Talos ||
      action_type ==
          DifferentialActionModelTypes::
              DifferentialActionModelContactFwdDynamicsWithFriction_Talos ||
      action_type == DifferentialActionModelTypes::
                         DifferentialActionModelContactInvDynamics_HyQ ||
      action_type ==
          DifferentialActionModelTypes::
              DifferentialActionModelContactInvDynamicsWithFriction_HyQ ||
      action_type == DifferentialActionModelTypes::
                         DifferentialActionModelContactInvDynamics_Talos ||
      action_type ==
          DifferentialActionModelTypes::
              DifferentialActionModelContactInvDynamicsWithFriction_Talos) {
    boost::shared_ptr<crocoddyl::DifferentialActionModelContactFwdDynamics> m =
        boost::static_pointer_cast<
            crocoddyl::DifferentialActionModelContactFwdDynamics>(model);
    m->get_contacts()->changeContactStatus("lf", false);

    model->quasiStatic(data, u, x);
    model->calc(data, x, u);

    // Checking that the acceleration is zero as supposed to be in a quasi
    // static condition
    BOOST_CHECK(data->xout.norm() <= 1e-8);
  }
}

void test_partial_derivatives_against_numdiff(
    DifferentialActionModelTypes::Type action_type) {
  // create the model
  DifferentialActionModelFactory factory;
  boost::shared_ptr<crocoddyl::DifferentialActionModelAbstract> model =
      factory.create(action_type);

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

  crocoddyl::DifferentialActionModelNumDiff model_num_diff(model);
  boost::shared_ptr<crocoddyl::DifferentialActionDataAbstract> 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 action 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->Fx - data_num_diff->Fx).isZero(tol));
  BOOST_CHECK((data->Fu - data_num_diff->Fu).isZero(tol));
  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));
  }
  BOOST_CHECK((data->Hx - data_num_diff->Hx).isZero(tol));
  BOOST_CHECK((data->Hu - data_num_diff->Hu).isZero(tol));
  BOOST_CHECK((data->Gx - data_num_diff->Gx).isZero(tol));
  BOOST_CHECK((data->Gu - data_num_diff->Gu).isZero(tol));

  // Computing the action 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);
  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));
  }
  BOOST_CHECK((data->Hx - data_num_diff->Hx).isZero(tol));
  BOOST_CHECK((data->Gx - data_num_diff->Gx).isZero(tol));
}

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

void register_action_model_unit_tests(
    DifferentialActionModelTypes::Type action_type) {
  boost::test_tools::output_test_stream test_name;
  test_name << "test_" << action_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_check_data, action_type)));
  ts->add(BOOST_TEST_CASE(boost::bind(&test_calc_returns_state, action_type)));
  ts->add(BOOST_TEST_CASE(boost::bind(&test_calc_returns_a_cost, action_type)));
  ts->add(BOOST_TEST_CASE(
      boost::bind(&test_partial_derivatives_against_numdiff, action_type)));
  ts->add(BOOST_TEST_CASE(boost::bind(&test_quasi_static, action_type)));
  framework::master_test_suite().add(ts);
}

bool init_function() {
  for (size_t i = 0; i < DifferentialActionModelTypes::all.size(); ++i) {
    register_action_model_unit_tests(DifferentialActionModelTypes::all[i]);
  }
  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, INRIA, University of
6			// Oxford, Heriot-Watt University
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 "factory/diff_action.hpp"
15			#include "unittest_common.hpp"
16
17			using namespace boost::unit_test;
18			using namespace crocoddyl::unittest;
19
20			//----------------------------------------------------------------------------//
21
22		22	void test_check_data(DifferentialActionModelTypes::Type action_type) {
23			// create the model
24	✓✗	44	DifferentialActionModelFactory factory;
25			boost::shared_ptr<crocoddyl::DifferentialActionModelAbstract> model =
26	✓✗	44	factory.create(action_type);
27
28			// Run the print function
29	✓✗	44	std::ostringstream tmp;
30	✓✗	22	tmp << *model;
31
32			// create the corresponding data object
33			boost::shared_ptr<crocoddyl::DifferentialActionDataAbstract> data =
34	✓✗	44	model->createData();
35
36	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	22	BOOST_CHECK(model->checkData(data));
37		22	}
38
39		22	void test_calc_returns_state(DifferentialActionModelTypes::Type action_type) {
40			// create the model
41	✓✗	44	DifferentialActionModelFactory factory;
42			boost::shared_ptr<crocoddyl::DifferentialActionModelAbstract> model =
43	✓✗	44	factory.create(action_type);
44
45			// create the corresponding data object
46			boost::shared_ptr<crocoddyl::DifferentialActionDataAbstract> data =
47	✓✗	44	model->createData();
48
49			// Generating random state and control vectors
50	✓✗	44	const Eigen::VectorXd x = model->get_state()->rand();
51	✓✗✓✗	44	const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
52
53			// Getting the state dimension from calc() call
54	✓✗✓✗ ✓✗	22	model->calc(data, x, u);
55
56	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	22	BOOST_CHECK(static_cast<std::size_t>(data->xout.size()) ==
57			model->get_state()->get_nv());
58		22	}
59
60		22	void test_calc_returns_a_cost(DifferentialActionModelTypes::Type action_type) {
61			// create the model
62	✓✗	44	DifferentialActionModelFactory factory;
63			boost::shared_ptr<crocoddyl::DifferentialActionModelAbstract> model =
64	✓✗	44	factory.create(action_type);
65
66			// create the corresponding data object and set the cost to nan
67			boost::shared_ptr<crocoddyl::DifferentialActionDataAbstract> data =
68	✓✗	44	model->createData();
69		22	data->cost = nan("");
70
71			// Getting the cost value computed by calc()
72	✓✗	44	const Eigen::VectorXd x = model->get_state()->rand();
73	✓✗✓✗	44	const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
74	✓✗✓✗ ✓✗	22	model->calc(data, x, u);
75
76			// Checking that calc returns a cost value
77	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	22	BOOST_CHECK(!std::isnan(data->cost));
78		22	}
79
80		22	void test_quasi_static(DifferentialActionModelTypes::Type action_type) {
81	✓✓	22	if (action_type ==
82			DifferentialActionModelTypes::
83			DifferentialActionModelFreeFwdDynamics_TalosArm_Squashed)
84		1	return;
85			// create the model
86	✓✗	42	DifferentialActionModelFactory factory;
87			boost::shared_ptr<crocoddyl::DifferentialActionModelAbstract> model =
88	✓✗	42	factory.create(action_type, false);
89
90			// create the corresponding data object and set the cost to nan
91			boost::shared_ptr<crocoddyl::DifferentialActionDataAbstract> data =
92	✓✗	42	model->createData();
93
94			// Getting the cost value computed by calc()
95	✓✗	42	Eigen::VectorXd x = model->get_state()->rand();
96	✓✗✓✗	21	x.tail(model->get_state()->get_nv()).setZero();
97	✓✗✓✗	42	Eigen::VectorXd u = Eigen::VectorXd::Zero(model->get_nu());
98	✓✗✓✗ ✓✗	21	model->quasiStatic(data, u, x);
99	✓✗✓✗ ✓✗	21	model->calc(data, x, u);
100
101			// Check for inactive contacts
102	✓✓	21	if (action_type == DifferentialActionModelTypes::
103	✓✓	20	DifferentialActionModelContactFwdDynamics_HyQ \|\|
104			action_type ==
105			DifferentialActionModelTypes::
106	✓✓	19	DifferentialActionModelContactFwdDynamicsWithFriction_HyQ \|\|
107			action_type == DifferentialActionModelTypes::
108	✓✓	18	DifferentialActionModelContactFwdDynamics_Talos \|\|
109			action_type ==
110			DifferentialActionModelTypes::
111	✓✓	17	DifferentialActionModelContactFwdDynamicsWithFriction_Talos \|\|
112			action_type == DifferentialActionModelTypes::
113	✓✓	16	DifferentialActionModelContactInvDynamics_HyQ \|\|
114			action_type ==
115			DifferentialActionModelTypes::
116	✓✓	15	DifferentialActionModelContactInvDynamicsWithFriction_HyQ \|\|
117			action_type == DifferentialActionModelTypes::
118	✓✓	14	DifferentialActionModelContactInvDynamics_Talos \|\|
119			action_type ==
120			DifferentialActionModelTypes::
121			DifferentialActionModelContactInvDynamicsWithFriction_Talos) {
122			boost::shared_ptr<crocoddyl::DifferentialActionModelContactFwdDynamics> m =
123			boost::static_pointer_cast<
124		16	crocoddyl::DifferentialActionModelContactFwdDynamics>(model);
125	✓✗✓✗	8	m->get_contacts()->changeContactStatus("lf", false);
126
127	✓✗✓✗ ✓✗	8	model->quasiStatic(data, u, x);
128	✓✗✓✗ ✓✗	8	model->calc(data, x, u);
129
130			// Checking that the acceleration is zero as supposed to be in a quasi
131			// static condition
132	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	8	BOOST_CHECK(data->xout.norm() <= 1e-8);
133			}
134			}
135
136		22	void test_partial_derivatives_against_numdiff(
137			DifferentialActionModelTypes::Type action_type) {
138			// create the model
139	✓✗	44	DifferentialActionModelFactory factory;
140			boost::shared_ptr<crocoddyl::DifferentialActionModelAbstract> model =
141	✓✗	44	factory.create(action_type);
142
143			// create the corresponding data object and set the cost to nan
144			boost::shared_ptr<crocoddyl::DifferentialActionDataAbstract> data =
145	✓✗	44	model->createData();
146
147	✓✗	44	crocoddyl::DifferentialActionModelNumDiff model_num_diff(model);
148			boost::shared_ptr<crocoddyl::DifferentialActionDataAbstract> data_num_diff =
149	✓✗	44	model_num_diff.createData();
150
151			// Generating random values for the state and control
152	✓✗	44	Eigen::VectorXd x = model->get_state()->rand();
153	✓✗✓✗	44	const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
154
155			// Computing the action derivatives
156	✓✗✓✗ ✓✗	22	model->calc(data, x, u);
157	✓✗✓✗ ✓✗	22	model->calcDiff(data, x, u);
158	✓✗✓✗ ✓✗	22	model_num_diff.calc(data_num_diff, x, u);
159	✓✗✓✗ ✓✗	22	model_num_diff.calcDiff(data_num_diff, x, u);
160			// Tolerance defined as in
161			// http://www.it.uom.gr/teaching/linearalgebra/NumericalRecipiesInC/c5-7.pdf
162		22	double tol = std::pow(model_num_diff.get_disturbance(), 1. / 3.);
163	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	22	BOOST_CHECK((data->Fx - data_num_diff->Fx).isZero(tol));
164	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	22	BOOST_CHECK((data->Fu - data_num_diff->Fu).isZero(tol));
165	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	22	BOOST_CHECK((data->Lx - data_num_diff->Lx).isZero(tol));
166	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	22	BOOST_CHECK((data->Lu - data_num_diff->Lu).isZero(tol));
167	✗✓	22	if (model_num_diff.get_with_gauss_approx()) {
168			BOOST_CHECK((data->Lxx - data_num_diff->Lxx).isZero(tol));
169			BOOST_CHECK((data->Lxu - data_num_diff->Lxu).isZero(tol));
170			BOOST_CHECK((data->Luu - data_num_diff->Luu).isZero(tol));
171			}
172	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	22	BOOST_CHECK((data->Hx - data_num_diff->Hx).isZero(tol));
173	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	22	BOOST_CHECK((data->Hu - data_num_diff->Hu).isZero(tol));
174	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	22	BOOST_CHECK((data->Gx - data_num_diff->Gx).isZero(tol));
175	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	22	BOOST_CHECK((data->Gu - data_num_diff->Gu).isZero(tol));
176
177			// Computing the action derivatives
178	✓✗	22	x = model->get_state()->rand();
179	✓✗✓✗	22	model->calc(data, x);
180	✓✗✓✗	22	model->calcDiff(data, x);
181	✓✗✓✗	22	model_num_diff.calc(data_num_diff, x);
182	✓✗✓✗	22	model_num_diff.calcDiff(data_num_diff, x);
183	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	22	BOOST_CHECK((data->Lx - data_num_diff->Lx).isZero(tol));
184	✗✓	22	if (model_num_diff.get_with_gauss_approx()) {
185			BOOST_CHECK((data->Lxx - data_num_diff->Lxx).isZero(tol));
186			}
187	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	22	BOOST_CHECK((data->Hx - data_num_diff->Hx).isZero(tol));
188	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	22	BOOST_CHECK((data->Gx - data_num_diff->Gx).isZero(tol));
189		22	}
190
191			//----------------------------------------------------------------------------//
192
193		22	void register_action_model_unit_tests(
194			DifferentialActionModelTypes::Type action_type) {
195	✓✗✓✗	44	boost::test_tools::output_test_stream test_name;
196	✓✗✓✗	22	test_name << "test_" << action_type;
197	✓✗✓✗ ✓✗✓✗	22	std::cout << "Running " << test_name.str() << std::endl;
198	✓✗✓✗ ✓✗✓✗	22	test_suite* ts = BOOST_TEST_SUITE(test_name.str());
199	✓✗✓✗ ✓✗✓✗ ✓✗	22	ts->add(BOOST_TEST_CASE(boost::bind(&test_check_data, action_type)));
200	✓✗✓✗ ✓✗✓✗ ✓✗	22	ts->add(BOOST_TEST_CASE(boost::bind(&test_calc_returns_state, action_type)));
201	✓✗✓✗ ✓✗✓✗ ✓✗	22	ts->add(BOOST_TEST_CASE(boost::bind(&test_calc_returns_a_cost, action_type)));
202	✓✗✓✗ ✓✗✓✗ ✓✗	22	ts->add(BOOST_TEST_CASE(
203			boost::bind(&test_partial_derivatives_against_numdiff, action_type)));
204	✓✗✓✗ ✓✗✓✗ ✓✗	22	ts->add(BOOST_TEST_CASE(boost::bind(&test_quasi_static, action_type)));
205	✓✗✓✗ ✓✗	22	framework::master_test_suite().add(ts);
206		22	}
207
208		1	bool init_function() {
209	✓✓	23	for (size_t i = 0; i < DifferentialActionModelTypes::all.size(); ++i) {
210		22	register_action_model_unit_tests(DifferentialActionModelTypes::all[i]);
211			}
212		1	return true;
213			}
214
215		1	int main(int argc, char** argv) {
216		1	return ::boost::unit_test::unit_test_main(&init_function, argc, argv);
217			}