Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	unittest/test_contact_costs.cpp	Lines:	51	55	92.7 %
Date:	2024-02-13 11:12:33	Branches:	93	238	39.1 %


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2021-2023, 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/contact_cost.hpp"
#include "unittest_common.hpp"

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

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

void test_partial_derivatives_against_contact_numdiff(
    ContactCostModelTypes::Type cost_type, PinocchioModelTypes::Type model_type,
    ActivationModelTypes::Type activation_type,
    ActuationModelTypes::Type actuation_type) {
  // create the model
  const boost::shared_ptr<crocoddyl::DifferentialActionModelAbstract>& model =
      ContactCostModelFactory().create(cost_type, model_type, activation_type,
                                       actuation_type);

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

  crocoddyl::DifferentialActionModelNumDiff model_num_diff(model);
  const 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->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));
  }

  // 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));
  }
}

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

void register_contact_cost_model_unit_tests(
    ContactCostModelTypes::Type cost_type, PinocchioModelTypes::Type model_type,
    ActivationModelTypes::Type activation_type,
    ActuationModelTypes::Type actuation_type) {
  boost::test_tools::output_test_stream test_name;
  test_name << "test_" << cost_type << "_" << activation_type << "_"
            << actuation_type << "_" << model_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_partial_derivatives_against_contact_numdiff, cost_type,
                  model_type, activation_type, actuation_type)));
  framework::master_test_suite().add(ts);
}

bool init_function() {
  // Test all the contact cost model. Note that we can do it only with humanoids
  // as it needs to test the contact wrench cone
  for (std::size_t cost_type = 0; cost_type < ContactCostModelTypes::all.size();
       ++cost_type) {
    for (std::size_t activation_type = 0;
         activation_type <
         ActivationModelTypes::ActivationModelQuadraticBarrier;
         ++activation_type) {
      register_contact_cost_model_unit_tests(
          ContactCostModelTypes::all[cost_type], PinocchioModelTypes::Talos,
          ActivationModelTypes::all[activation_type],
          ActuationModelTypes::ActuationModelFloatingBase);
      if (ContactCostModelTypes::all[cost_type] ==
              ContactCostModelTypes::CostModelResidualContactForce ||
          ContactCostModelTypes::all[cost_type] ==
              ContactCostModelTypes::CostModelResidualContactFrictionCone ||
          ContactCostModelTypes::all[cost_type] ==
              ContactCostModelTypes::CostModelResidualContactControlGrav) {
        register_contact_cost_model_unit_tests(
            ContactCostModelTypes::all[cost_type], PinocchioModelTypes::HyQ,
            ActivationModelTypes::all[activation_type],
            ActuationModelTypes::ActuationModelFloatingBase);
      }
    }
  }

  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-2023, 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/contact_cost.hpp"
13			#include "unittest_common.hpp"
14
15			using namespace boost::unit_test;
16			using namespace crocoddyl::unittest;
17
18			//----------------------------------------------------------------------------//
19
20		48	void test_partial_derivatives_against_contact_numdiff(
21			ContactCostModelTypes::Type cost_type, PinocchioModelTypes::Type model_type,
22			ActivationModelTypes::Type activation_type,
23			ActuationModelTypes::Type actuation_type) {
24			// create the model
25			const boost::shared_ptr<crocoddyl::DifferentialActionModelAbstract>& model =
26	✓✗	48	ContactCostModelFactory().create(cost_type, model_type, activation_type,
27	✓✗	96	actuation_type);
28
29			// create the corresponding data object and set the cost to nan
30			const boost::shared_ptr<crocoddyl::DifferentialActionDataAbstract>& data =
31	✓✗	96	model->createData();
32
33	✓✗	96	crocoddyl::DifferentialActionModelNumDiff model_num_diff(model);
34			const boost::shared_ptr<crocoddyl::DifferentialActionDataAbstract>&
35	✓✗	96	data_num_diff = model_num_diff.createData();
36
37			// Generating random values for the state and control
38	✓✗	96	Eigen::VectorXd x = model->get_state()->rand();
39	✓✗✓✗	96	const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
40
41			// Computing the action derivatives
42	✓✗✓✗ ✓✗	48	model->calc(data, x, u);
43	✓✗✓✗ ✓✗	48	model->calcDiff(data, x, u);
44	✓✗✓✗ ✓✗	48	model_num_diff.calc(data_num_diff, x, u);
45	✓✗✓✗ ✓✗	48	model_num_diff.calcDiff(data_num_diff, x, u);
46			// Tolerance defined as in
47			// http://www.it.uom.gr/teaching/linearalgebra/NumericalRecipiesInC/c5-7.pdf
48		48	double tol = std::pow(model_num_diff.get_disturbance(), 1. / 3.);
49	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	48	BOOST_CHECK((data->Lx - data_num_diff->Lx).isZero(tol));
50	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	48	BOOST_CHECK((data->Lu - data_num_diff->Lu).isZero(tol));
51	✗✓	48	if (model_num_diff.get_with_gauss_approx()) {
52			BOOST_CHECK((data->Lxx - data_num_diff->Lxx).isZero(tol));
53			BOOST_CHECK((data->Lxu - data_num_diff->Lxu).isZero(tol));
54			BOOST_CHECK((data->Luu - data_num_diff->Luu).isZero(tol));
55			}
56
57			// Computing the action derivatives
58	✓✗	48	x = model->get_state()->rand();
59	✓✗✓✗	48	model->calc(data, x);
60	✓✗✓✗	48	model->calcDiff(data, x);
61	✓✗✓✗	48	model_num_diff.calc(data_num_diff, x);
62	✓✗✓✗	48	model_num_diff.calcDiff(data_num_diff, x);
63	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	48	BOOST_CHECK((data->Lx - data_num_diff->Lx).isZero(tol));
64	✗✓	48	if (model_num_diff.get_with_gauss_approx()) {
65			BOOST_CHECK((data->Lxx - data_num_diff->Lxx).isZero(tol));
66			}
67		48	}
68
69			//----------------------------------------------------------------------------//
70
71		48	void register_contact_cost_model_unit_tests(
72			ContactCostModelTypes::Type cost_type, PinocchioModelTypes::Type model_type,
73			ActivationModelTypes::Type activation_type,
74			ActuationModelTypes::Type actuation_type) {
75	✓✗✓✗	96	boost::test_tools::output_test_stream test_name;
76	✓✗✓✗ ✓✗✓✗ ✓✗	48	test_name << "test_" << cost_type << "_" << activation_type << "_"
77	✓✗✓✗ ✓✗	48	<< actuation_type << "_" << model_type;
78	✓✗✓✗ ✓✗✓✗	48	std::cout << "Running " << test_name.str() << std::endl;
79	✓✗✓✗ ✓✗✓✗	48	test_suite* ts = BOOST_TEST_SUITE(test_name.str());
80	✓✗✓✗ ✓✗✓✗ ✓✗	48	ts->add(BOOST_TEST_CASE(
81			boost::bind(&test_partial_derivatives_against_contact_numdiff, cost_type,
82			model_type, activation_type, actuation_type)));
83	✓✗✓✗ ✓✗	48	framework::master_test_suite().add(ts);
84		48	}
85
86		1	bool init_function() {
87			// Test all the contact cost model. Note that we can do it only with humanoids
88			// as it needs to test the contact wrench cone
89	✓✓	6	for (std::size_t cost_type = 0; cost_type < ContactCostModelTypes::all.size();
90			++cost_type) {
91		35	for (std::size_t activation_type = 0;
92	✓✓	35	activation_type <
93			ActivationModelTypes::ActivationModelQuadraticBarrier;
94			++activation_type) {
95		30	register_contact_cost_model_unit_tests(
96		30	ContactCostModelTypes::all[cost_type], PinocchioModelTypes::Talos,
97		30	ActivationModelTypes::all[activation_type],
98			ActuationModelTypes::ActuationModelFloatingBase);
99		30	if (ContactCostModelTypes::all[cost_type] ==
100		24	ContactCostModelTypes::CostModelResidualContactForce \|\|
101	✓✓	24	ContactCostModelTypes::all[cost_type] ==
102	✓✓✓✓	54	ContactCostModelTypes::CostModelResidualContactFrictionCone \|\|
103	✓✓	18	ContactCostModelTypes::all[cost_type] ==
104			ContactCostModelTypes::CostModelResidualContactControlGrav) {
105		18	register_contact_cost_model_unit_tests(
106		18	ContactCostModelTypes::all[cost_type], PinocchioModelTypes::HyQ,
107		18	ActivationModelTypes::all[activation_type],
108			ActuationModelTypes::ActuationModelFloatingBase);
109			}
110			}
111			}
112
113		1	return true;
114			}
115
116		1	int main(int argc, char** argv) {
117		1	return ::boost::unit_test::unit_test_main(&init_function, argc, argv);
118			}