Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	unittest/test_impulses.cpp	Lines:	89	89	100.0 %
Date:	2024-02-13 11:12:33	Branches:	225	440	51.1 %


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2019-2022, LAAS-CNRS, New York University, Max Planck
// Gesellschaft,
//                          INRIA, 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 <boost/make_shared.hpp>
#include <boost/shared_ptr.hpp>
#include <pinocchio/algorithm/frames.hpp>
#include <pinocchio/algorithm/kinematics-derivatives.hpp>

#include "crocoddyl/multibody/impulses/impulse-3d.hpp"
#include "crocoddyl/multibody/impulses/impulse-6d.hpp"
#include "factory/impulse.hpp"
#include "unittest_common.hpp"

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

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

void test_construct_data(ImpulseModelTypes::Type impulse_type,
                         PinocchioModelTypes::Type model_type) {
  // create the model
  ImpulseModelFactory factory;
  boost::shared_ptr<crocoddyl::ImpulseModelAbstract> model =
      factory.create(impulse_type, model_type);

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

  // create the corresponding data object
  pinocchio::Data pinocchio_data(*model->get_state()->get_pinocchio().get());
  boost::shared_ptr<crocoddyl::ImpulseDataAbstract> data =
      model->createData(&pinocchio_data);
}

void test_calc_fetch_jacobians(ImpulseModelTypes::Type impulse_type,
                               PinocchioModelTypes::Type model_type) {
  // create the model
  ImpulseModelFactory factory;
  boost::shared_ptr<crocoddyl::ImpulseModelAbstract> model =
      factory.create(impulse_type, model_type);

  // create the corresponding data object
  const boost::shared_ptr<pinocchio::Model>& pinocchio_model =
      model->get_state()->get_pinocchio();
  pinocchio::Data pinocchio_data(*pinocchio_model.get());
  boost::shared_ptr<crocoddyl::ImpulseDataAbstract> data =
      model->createData(&pinocchio_data);

  // Compute the jacobian and check that the impulse model fetch it.
  Eigen::VectorXd x = model->get_state()->rand();
  crocoddyl::unittest::updateAllPinocchio(pinocchio_model.get(),
                                          &pinocchio_data, x);

  // Getting the jacobian from the model
  Eigen::VectorXd dx;
  model->calc(data, dx);

  // Check that only the Jacobian has been filled
  BOOST_CHECK(!data->Jc.isZero());
  BOOST_CHECK(data->dv0_dq.isZero());
  BOOST_CHECK(data->f.toVector().isZero());
  BOOST_CHECK(data->df_dx.isZero());
}

void test_calc_diff_fetch_derivatives(ImpulseModelTypes::Type impulse_type,
                                      PinocchioModelTypes::Type model_type) {
  // create the model
  ImpulseModelFactory factory;
  boost::shared_ptr<crocoddyl::ImpulseModelAbstract> model =
      factory.create(impulse_type, model_type);

  // create the corresponding data object
  const boost::shared_ptr<pinocchio::Model>& pinocchio_model =
      model->get_state()->get_pinocchio();
  pinocchio::Data pinocchio_data(*pinocchio_model.get());
  boost::shared_ptr<crocoddyl::ImpulseDataAbstract> data =
      model->createData(&pinocchio_data);

  // Compute the jacobian and check that the impulse model fetch it.
  Eigen::VectorXd x = model->get_state()->rand();
  crocoddyl::unittest::updateAllPinocchio(pinocchio_model.get(),
                                          &pinocchio_data, x);

  // Getting the jacobian from the model
  Eigen::VectorXd dx;
  model->calc(data, dx);
  model->calcDiff(data, dx);

  // Check that nothing has been computed and that all value are initialized to
  // 0
  BOOST_CHECK(!data->Jc.isZero());
  if (model_type == PinocchioModelTypes::Hector &&
      (impulse_type == ImpulseModelTypes::ImpulseModel3D_LOCAL ||
       impulse_type ==
           ImpulseModelTypes::ImpulseModel6D_LOCAL)) {  // this is due to Hector
                                                        // is a single rigid
                                                        // body system.
    BOOST_CHECK(data->dv0_dq.isZero());
  } else {
    BOOST_CHECK(!data->dv0_dq.isZero());
  }
  BOOST_CHECK(data->f.toVector().isZero());
  BOOST_CHECK(data->df_dx.isZero());
}

void test_update_force(ImpulseModelTypes::Type impulse_type,
                       PinocchioModelTypes::Type model_type) {
  // create the model
  ImpulseModelFactory factory;
  boost::shared_ptr<crocoddyl::ImpulseModelAbstract> model =
      factory.create(impulse_type, model_type);

  // create the corresponding data object
  const boost::shared_ptr<pinocchio::Model>& pinocchio_model =
      model->get_state()->get_pinocchio();
  pinocchio::Data pinocchio_data(*pinocchio_model.get());
  boost::shared_ptr<crocoddyl::ImpulseDataAbstract> data =
      model->createData(&pinocchio_data);

  // Create a random force and update it
  Eigen::VectorXd f = Eigen::VectorXd::Random(data->Jc.rows());
  model->updateForce(data, f);
  boost::shared_ptr<crocoddyl::ImpulseModel3D> m =
      boost::static_pointer_cast<crocoddyl::ImpulseModel3D>(model);

  // Check that nothing has been computed and that all value are initialized to
  // 0
  BOOST_CHECK(data->Jc.isZero());
  BOOST_CHECK(data->dv0_dq.isZero());
  BOOST_CHECK(!data->f.toVector().isZero());
  BOOST_CHECK(data->df_dx.isZero());
}

void test_update_force_diff(ImpulseModelTypes::Type impulse_type,
                            PinocchioModelTypes::Type model_type) {
  // create the model
  ImpulseModelFactory factory;
  boost::shared_ptr<crocoddyl::ImpulseModelAbstract> model =
      factory.create(impulse_type, model_type);

  // create the corresponding data object
  const boost::shared_ptr<pinocchio::Model>& pinocchio_model =
      model->get_state()->get_pinocchio();
  pinocchio::Data pinocchio_data(*pinocchio_model.get());
  boost::shared_ptr<crocoddyl::ImpulseDataAbstract> data =
      model->createData(&pinocchio_data);

  // Create a random force and update it
  Eigen::MatrixXd df_dx =
      Eigen::MatrixXd::Random(data->df_dx.rows(), data->df_dx.cols());
  model->updateForceDiff(data, df_dx);

  // Check that nothing has been computed and that all value are initialized to
  // 0
  BOOST_CHECK(data->Jc.isZero());
  BOOST_CHECK(data->dv0_dq.isZero());
  BOOST_CHECK(data->f.toVector().isZero());
  BOOST_CHECK(!data->df_dx.isZero());
}

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

void register_impulse_model_unit_tests(ImpulseModelTypes::Type impulse_type,
                                       PinocchioModelTypes::Type model_type) {
  boost::test_tools::output_test_stream test_name;
  test_name << "test_" << impulse_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_construct_data, impulse_type, model_type)));
  ts->add(BOOST_TEST_CASE(
      boost::bind(&test_calc_fetch_jacobians, impulse_type, model_type)));
  ts->add(BOOST_TEST_CASE(boost::bind(&test_calc_diff_fetch_derivatives,
                                      impulse_type, model_type)));
  ts->add(BOOST_TEST_CASE(
      boost::bind(&test_update_force, impulse_type, model_type)));
  ts->add(BOOST_TEST_CASE(
      boost::bind(&test_update_force_diff, impulse_type, model_type)));
  framework::master_test_suite().add(ts);
}

bool init_function() {
  for (size_t impulse_type = 0; impulse_type < ImpulseModelTypes::all.size();
       ++impulse_type) {
    for (size_t model_type = 0; model_type < PinocchioModelTypes::all.size();
         ++model_type) {
      register_impulse_model_unit_tests(ImpulseModelTypes::all[impulse_type],
                                        PinocchioModelTypes::all[model_type]);
    }
  }
  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-2022, LAAS-CNRS, New York University, Max Planck
5			// Gesellschaft,
6			// INRIA, University of Edinburgh, Heriot-Watt
7			// University
8			// Copyright note valid unless otherwise stated in individual files.
9			// All rights reserved.
10			///////////////////////////////////////////////////////////////////////////////
11
12			#define BOOST_TEST_NO_MAIN
13			#define BOOST_TEST_ALTERNATIVE_INIT_API
14
15			#include <boost/make_shared.hpp>
16			#include <boost/shared_ptr.hpp>
17			#include <pinocchio/algorithm/frames.hpp>
18			#include <pinocchio/algorithm/kinematics-derivatives.hpp>
19
20			#include "crocoddyl/multibody/impulses/impulse-3d.hpp"
21			#include "crocoddyl/multibody/impulses/impulse-6d.hpp"
22			#include "factory/impulse.hpp"
23			#include "unittest_common.hpp"
24
25			using namespace boost::unit_test;
26			using namespace crocoddyl::unittest;
27
28			//----------------------------------------------------------------------------//
29
30		30	void test_construct_data(ImpulseModelTypes::Type impulse_type,
31			PinocchioModelTypes::Type model_type) {
32			// create the model
33	✓✗	60	ImpulseModelFactory factory;
34			boost::shared_ptr<crocoddyl::ImpulseModelAbstract> model =
35	✓✗✓✗	90	factory.create(impulse_type, model_type);
36
37			// Run the print function
38	✓✗	60	std::ostringstream tmp;
39	✓✗	30	tmp << *model;
40
41			// create the corresponding data object
42	✓✗	60	pinocchio::Data pinocchio_data(*model->get_state()->get_pinocchio().get());
43			boost::shared_ptr<crocoddyl::ImpulseDataAbstract> data =
44	✓✗	30	model->createData(&pinocchio_data);
45		30	}
46
47		30	void test_calc_fetch_jacobians(ImpulseModelTypes::Type impulse_type,
48			PinocchioModelTypes::Type model_type) {
49			// create the model
50	✓✗	60	ImpulseModelFactory factory;
51			boost::shared_ptr<crocoddyl::ImpulseModelAbstract> model =
52	✓✗✓✗	90	factory.create(impulse_type, model_type);
53
54			// create the corresponding data object
55			const boost::shared_ptr<pinocchio::Model>& pinocchio_model =
56		30	model->get_state()->get_pinocchio();
57	✓✗	60	pinocchio::Data pinocchio_data(*pinocchio_model.get());
58			boost::shared_ptr<crocoddyl::ImpulseDataAbstract> data =
59	✓✗	60	model->createData(&pinocchio_data);
60
61			// Compute the jacobian and check that the impulse model fetch it.
62	✓✗	60	Eigen::VectorXd x = model->get_state()->rand();
63	✓✗✓✗	30	crocoddyl::unittest::updateAllPinocchio(pinocchio_model.get(),
64			&pinocchio_data, x);
65
66			// Getting the jacobian from the model
67	✓✗	60	Eigen::VectorXd dx;
68	✓✗✓✗	30	model->calc(data, dx);
69
70			// Check that only the Jacobian has been filled
71	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	30	BOOST_CHECK(!data->Jc.isZero());
72	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	30	BOOST_CHECK(data->dv0_dq.isZero());
73	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	30	BOOST_CHECK(data->f.toVector().isZero());
74	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	30	BOOST_CHECK(data->df_dx.isZero());
75		30	}
76
77		30	void test_calc_diff_fetch_derivatives(ImpulseModelTypes::Type impulse_type,
78			PinocchioModelTypes::Type model_type) {
79			// create the model
80	✓✗	60	ImpulseModelFactory factory;
81			boost::shared_ptr<crocoddyl::ImpulseModelAbstract> model =
82	✓✗✓✗	90	factory.create(impulse_type, model_type);
83
84			// create the corresponding data object
85			const boost::shared_ptr<pinocchio::Model>& pinocchio_model =
86		30	model->get_state()->get_pinocchio();
87	✓✗	60	pinocchio::Data pinocchio_data(*pinocchio_model.get());
88			boost::shared_ptr<crocoddyl::ImpulseDataAbstract> data =
89	✓✗	60	model->createData(&pinocchio_data);
90
91			// Compute the jacobian and check that the impulse model fetch it.
92	✓✗	60	Eigen::VectorXd x = model->get_state()->rand();
93	✓✗✓✗	30	crocoddyl::unittest::updateAllPinocchio(pinocchio_model.get(),
94			&pinocchio_data, x);
95
96			// Getting the jacobian from the model
97	✓✗	60	Eigen::VectorXd dx;
98	✓✗✓✗	30	model->calc(data, dx);
99	✓✗✓✗	30	model->calcDiff(data, dx);
100
101			// Check that nothing has been computed and that all value are initialized to
102			// 0
103	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	30	BOOST_CHECK(!data->Jc.isZero());
104	✓✓✓✓	30	if (model_type == PinocchioModelTypes::Hector &&
105	✓✓	5	(impulse_type == ImpulseModelTypes::ImpulseModel3D_LOCAL \|\|
106			impulse_type ==
107			ImpulseModelTypes::ImpulseModel6D_LOCAL)) { // this is due to Hector
108			// is a single rigid
109			// body system.
110	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(data->dv0_dq.isZero());
111			} else {
112	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	28	BOOST_CHECK(!data->dv0_dq.isZero());
113			}
114	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	30	BOOST_CHECK(data->f.toVector().isZero());
115	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	30	BOOST_CHECK(data->df_dx.isZero());
116		30	}
117
118		30	void test_update_force(ImpulseModelTypes::Type impulse_type,
119			PinocchioModelTypes::Type model_type) {
120			// create the model
121	✓✗	60	ImpulseModelFactory factory;
122			boost::shared_ptr<crocoddyl::ImpulseModelAbstract> model =
123	✓✗✓✗	90	factory.create(impulse_type, model_type);
124
125			// create the corresponding data object
126			const boost::shared_ptr<pinocchio::Model>& pinocchio_model =
127		30	model->get_state()->get_pinocchio();
128	✓✗	60	pinocchio::Data pinocchio_data(*pinocchio_model.get());
129			boost::shared_ptr<crocoddyl::ImpulseDataAbstract> data =
130	✓✗	60	model->createData(&pinocchio_data);
131
132			// Create a random force and update it
133	✓✗✓✗ ✓✗	60	Eigen::VectorXd f = Eigen::VectorXd::Random(data->Jc.rows());
134	✓✗	30	model->updateForce(data, f);
135			boost::shared_ptr<crocoddyl::ImpulseModel3D> m =
136		60	boost::static_pointer_cast<crocoddyl::ImpulseModel3D>(model);
137
138			// Check that nothing has been computed and that all value are initialized to
139			// 0
140	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	30	BOOST_CHECK(data->Jc.isZero());
141	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	30	BOOST_CHECK(data->dv0_dq.isZero());
142	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	30	BOOST_CHECK(!data->f.toVector().isZero());
143	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	30	BOOST_CHECK(data->df_dx.isZero());
144		30	}
145
146		30	void test_update_force_diff(ImpulseModelTypes::Type impulse_type,
147			PinocchioModelTypes::Type model_type) {
148			// create the model
149	✓✗	60	ImpulseModelFactory factory;
150			boost::shared_ptr<crocoddyl::ImpulseModelAbstract> model =
151	✓✗✓✗	90	factory.create(impulse_type, model_type);
152
153			// create the corresponding data object
154			const boost::shared_ptr<pinocchio::Model>& pinocchio_model =
155		30	model->get_state()->get_pinocchio();
156	✓✗	60	pinocchio::Data pinocchio_data(*pinocchio_model.get());
157			boost::shared_ptr<crocoddyl::ImpulseDataAbstract> data =
158	✓✗	60	model->createData(&pinocchio_data);
159
160			// Create a random force and update it
161			Eigen::MatrixXd df_dx =
162	✓✗✓✗ ✓✗✓✗	60	Eigen::MatrixXd::Random(data->df_dx.rows(), data->df_dx.cols());
163	✓✗	30	model->updateForceDiff(data, df_dx);
164
165			// Check that nothing has been computed and that all value are initialized to
166			// 0
167	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	30	BOOST_CHECK(data->Jc.isZero());
168	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	30	BOOST_CHECK(data->dv0_dq.isZero());
169	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	30	BOOST_CHECK(data->f.toVector().isZero());
170	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	30	BOOST_CHECK(!data->df_dx.isZero());
171		30	}
172
173			//----------------------------------------------------------------------------//
174
175		30	void register_impulse_model_unit_tests(ImpulseModelTypes::Type impulse_type,
176			PinocchioModelTypes::Type model_type) {
177	✓✗✓✗	60	boost::test_tools::output_test_stream test_name;
178	✓✗✓✗ ✓✗✓✗	30	test_name << "test_" << impulse_type << "_" << model_type;
179	✓✗✓✗ ✓✗✓✗	30	std::cout << "Running " << test_name.str() << std::endl;
180	✓✗✓✗ ✓✗✓✗	30	test_suite* ts = BOOST_TEST_SUITE(test_name.str());
181	✓✗✓✗ ✓✗✓✗ ✓✗	30	ts->add(BOOST_TEST_CASE(
182			boost::bind(&test_construct_data, impulse_type, model_type)));
183	✓✗✓✗ ✓✗✓✗ ✓✗	30	ts->add(BOOST_TEST_CASE(
184			boost::bind(&test_calc_fetch_jacobians, impulse_type, model_type)));
185	✓✗✓✗ ✓✗✓✗ ✓✗	30	ts->add(BOOST_TEST_CASE(boost::bind(&test_calc_diff_fetch_derivatives,
186			impulse_type, model_type)));
187	✓✗✓✗ ✓✗✓✗ ✓✗	30	ts->add(BOOST_TEST_CASE(
188			boost::bind(&test_update_force, impulse_type, model_type)));
189	✓✗✓✗ ✓✗✓✗ ✓✗	30	ts->add(BOOST_TEST_CASE(
190			boost::bind(&test_update_force_diff, impulse_type, model_type)));
191	✓✗✓✗ ✓✗	30	framework::master_test_suite().add(ts);
192		30	}
193
194		1	bool init_function() {
195	✓✓	7	for (size_t impulse_type = 0; impulse_type < ImpulseModelTypes::all.size();
196			++impulse_type) {
197	✓✓	36	for (size_t model_type = 0; model_type < PinocchioModelTypes::all.size();
198			++model_type) {
199		30	register_impulse_model_unit_tests(ImpulseModelTypes::all[impulse_type],
200		30	PinocchioModelTypes::all[model_type]);
201			}
202			}
203		1	return true;
204			}
205
206		1	int main(int argc, char** argv) {
207		1	return ::boost::unit_test::unit_test_main(&init_function, argc, argv);
208			}