Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	unittest/test_contact_constraints.cpp	Lines:	51	51	100.0 %
Date:	2024-02-13 11:12:33	Branches:	111	212	52.4 %


///////////////////////////////////////////////////////////////////////////////
// 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_constraint.hpp"
#include "unittest_common.hpp"

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

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

void test_partial_derivatives_against_contact_numdiff(
    ContactConstraintModelTypes::Type constraint_type,
    PinocchioModelTypes::Type model_type,
    ActuationModelTypes::Type actuation_type) {
  // create the model
  const boost::shared_ptr<crocoddyl::DifferentialActionModelAbstract> &model =
      ContactConstraintModelFactory().create(constraint_type, model_type,
                                             actuation_type);

  // create the corresponding data object and set the constraint 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->Gx - data_num_diff->Gx).isZero(tol));
  BOOST_CHECK((data->Gu - data_num_diff->Gu).isZero(tol));
  BOOST_CHECK((data->Hx - data_num_diff->Hx).isZero(tol));
  BOOST_CHECK((data->Hu - data_num_diff->Hu).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->Gx - data_num_diff->Gx).isZero(tol));
  BOOST_CHECK((data->Hx - data_num_diff->Hx).isZero(tol));
}

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

void register_contact_constraint_model_unit_tests(
    ContactConstraintModelTypes::Type constraint_type,
    PinocchioModelTypes::Type model_type,
    ActuationModelTypes::Type actuation_type) {
  boost::test_tools::output_test_stream test_name;
  test_name << "test_" << constraint_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,
                  constraint_type, model_type, actuation_type)));
  framework::master_test_suite().add(ts);
}

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