GCC Code Coverage Report

Directory:	./
File:	unittest/test_cost_sum.cpp
Date:	2025-01-16 08:47:40

	Exec	Total	Coverage
Lines:	298	298	100.0%
Branches:	627	1230	51.0%

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2019-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 "crocoddyl/core/actions/lqr.hpp"
13			#include "crocoddyl/multibody/data/multibody.hpp"
14			#include "factory/cost.hpp"
15			#include "unittest_common.hpp"
16
17			using namespace boost::unit_test;
18			using namespace crocoddyl::unittest;
19
20			//----------------------------------------------------------------------------//
21
22		3	void test_constructor(StateModelTypes::Type state_type) {
23			// Setup the test
24	1/2 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken.	3	StateModelFactory state_factory;
25	2/4 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 3 times. ✗ Branch 5 not taken.	3	crocoddyl::CostModelSum model(state_factory.create(state_type));
26
27			// Run the print function
28	1/2 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken.	3	std::ostringstream tmp;
29	1/2 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken.	3	tmp << model;
30
31			// Test the initial size of the map
32	6/12 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 3 times. ✗ Branch 12 not taken. ✓ Branch 16 taken 3 times. ✗ Branch 17 not taken. ✓ Branch 19 taken 3 times. ✗ Branch 20 not taken. ✗ Branch 24 not taken. ✓ Branch 25 taken 3 times.	3	BOOST_CHECK(model.get_costs().size() == 0);
33		3	}
34
35		3	void test_addCost(StateModelTypes::Type state_type) {
36			// Setup the test
37	1/2 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken.	3	StateModelFactory state_factory;
38	2/4 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 3 times. ✗ Branch 5 not taken.	3	crocoddyl::CostModelSum model(state_factory.create(state_type));
39
40			// add an active cost
41			boost::shared_ptr<crocoddyl::CostModelAbstract> rand_cost_1 =
42	1/2 ✓ Branch 2 taken 3 times. ✗ Branch 3 not taken.	3	create_random_cost(state_type);
43	2/4 ✓ Branch 3 taken 3 times. ✗ Branch 4 not taken. ✓ Branch 6 taken 3 times. ✗ Branch 7 not taken.	3	model.addCost("random_cost_1", rand_cost_1, 1.);
44	6/12 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 3 times. ✗ Branch 12 not taken. ✓ Branch 19 taken 3 times. ✗ Branch 20 not taken. ✓ Branch 22 taken 3 times. ✗ Branch 23 not taken. ✗ Branch 27 not taken. ✓ Branch 28 taken 3 times.	3	BOOST_CHECK(model.get_nr() == rand_cost_1->get_activation()->get_nr());
45	6/12 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 3 times. ✗ Branch 12 not taken. ✓ Branch 19 taken 3 times. ✗ Branch 20 not taken. ✓ Branch 22 taken 3 times. ✗ Branch 23 not taken. ✗ Branch 27 not taken. ✓ Branch 28 taken 3 times.	3	BOOST_CHECK(model.get_nr_total() == rand_cost_1->get_activation()->get_nr());
46
47			// add an inactive cost
48			boost::shared_ptr<crocoddyl::CostModelAbstract> rand_cost_2 =
49	1/2 ✓ Branch 2 taken 3 times. ✗ Branch 3 not taken.	3	create_random_cost(state_type);
50	2/4 ✓ Branch 3 taken 3 times. ✗ Branch 4 not taken. ✓ Branch 6 taken 3 times. ✗ Branch 7 not taken.	3	model.addCost("random_cost_2", rand_cost_2, 1., false);
51	6/12 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 3 times. ✗ Branch 12 not taken. ✓ Branch 19 taken 3 times. ✗ Branch 20 not taken. ✓ Branch 22 taken 3 times. ✗ Branch 23 not taken. ✗ Branch 27 not taken. ✓ Branch 28 taken 3 times.	3	BOOST_CHECK(model.get_nr() == rand_cost_1->get_activation()->get_nr());
52	6/12 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 3 times. ✗ Branch 12 not taken. ✓ Branch 23 taken 3 times. ✗ Branch 24 not taken. ✓ Branch 26 taken 3 times. ✗ Branch 27 not taken. ✗ Branch 31 not taken. ✓ Branch 32 taken 3 times.	3	BOOST_CHECK(model.get_nr_total() ==
53			rand_cost_1->get_activation()->get_nr() +
54			rand_cost_2->get_activation()->get_nr());
55
56			// change the random cost 2 status
57	2/4 ✓ Branch 2 taken 3 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken.	3	model.changeCostStatus("random_cost_2", true);
58	6/12 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 3 times. ✗ Branch 12 not taken. ✓ Branch 23 taken 3 times. ✗ Branch 24 not taken. ✓ Branch 26 taken 3 times. ✗ Branch 27 not taken. ✗ Branch 31 not taken. ✓ Branch 32 taken 3 times.	3	BOOST_CHECK(model.get_nr() == rand_cost_1->get_activation()->get_nr() +
59			rand_cost_2->get_activation()->get_nr());
60	6/12 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 3 times. ✗ Branch 12 not taken. ✓ Branch 23 taken 3 times. ✗ Branch 24 not taken. ✓ Branch 26 taken 3 times. ✗ Branch 27 not taken. ✗ Branch 31 not taken. ✓ Branch 32 taken 3 times.	3	BOOST_CHECK(model.get_nr_total() ==
61			rand_cost_1->get_activation()->get_nr() +
62			rand_cost_2->get_activation()->get_nr());
63
64			// change the random cost 1 status
65	2/4 ✓ Branch 2 taken 3 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken.	3	model.changeCostStatus("random_cost_1", false);
66	6/12 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 3 times. ✗ Branch 12 not taken. ✓ Branch 19 taken 3 times. ✗ Branch 20 not taken. ✓ Branch 22 taken 3 times. ✗ Branch 23 not taken. ✗ Branch 27 not taken. ✓ Branch 28 taken 3 times.	3	BOOST_CHECK(model.get_nr() == rand_cost_2->get_activation()->get_nr());
67	6/12 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 3 times. ✗ Branch 12 not taken. ✓ Branch 23 taken 3 times. ✗ Branch 24 not taken. ✓ Branch 26 taken 3 times. ✗ Branch 27 not taken. ✗ Branch 31 not taken. ✓ Branch 32 taken 3 times.	3	BOOST_CHECK(model.get_nr_total() ==
68			rand_cost_1->get_activation()->get_nr() +
69			rand_cost_2->get_activation()->get_nr());
70		3	}
71
72		3	void test_addCost_error_message(StateModelTypes::Type state_type) {
73			// Setup the test
74	1/2 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken.	3	StateModelFactory state_factory;
75	2/4 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 3 times. ✗ Branch 5 not taken.	3	crocoddyl::CostModelSum model(state_factory.create(state_type));
76
77			// create an cost object
78			boost::shared_ptr<crocoddyl::CostModelAbstract> rand_cost =
79	1/2 ✓ Branch 2 taken 3 times. ✗ Branch 3 not taken.	3	create_random_cost(state_type);
80
81			// add twice the same cost object to the container
82	2/4 ✓ Branch 3 taken 3 times. ✗ Branch 4 not taken. ✓ Branch 6 taken 3 times. ✗ Branch 7 not taken.	3	model.addCost("random_cost", rand_cost, 1.);
83
84			// test error message when we add a duplicate cost
85	1/2 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken.	3	CaptureIOStream capture_ios;
86	1/2 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken.	3	capture_ios.beginCapture();
87	2/4 ✓ Branch 3 taken 3 times. ✗ Branch 4 not taken. ✓ Branch 6 taken 3 times. ✗ Branch 7 not taken.	3	model.addCost("random_cost", rand_cost, 1.);
88	1/2 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken.	3	capture_ios.endCapture();
89	1/2 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken.	3	std::stringstream expected_buffer;
90			expected_buffer << "Warning: we couldn't add the random_cost cost item, it "
91	1/2 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken.	3	"already existed."
92	1/2 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken.	3	<< std::endl;
93	8/16 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 3 times. ✗ Branch 12 not taken. ✓ Branch 14 taken 3 times. ✗ Branch 15 not taken. ✓ Branch 17 taken 3 times. ✗ Branch 18 not taken. ✓ Branch 21 taken 3 times. ✗ Branch 22 not taken. ✓ Branch 24 taken 3 times. ✗ Branch 25 not taken. ✗ Branch 31 not taken. ✓ Branch 32 taken 3 times.	3	BOOST_CHECK(capture_ios.str() == expected_buffer.str());
94
95			// test error message when we change the cost status of an inexistent cost
96	1/2 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken.	3	capture_ios.beginCapture();
97	2/4 ✓ Branch 2 taken 3 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken.	3	model.changeCostStatus("no_exist_cost", true);
98	1/2 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken.	3	capture_ios.endCapture();
99	1/2 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken.	3	expected_buffer.clear();
100			expected_buffer << "Warning: we couldn't change the status of the "
101	1/2 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken.	3	"no_exist_cost cost item, it doesn't exist."
102	1/2 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken.	3	<< std::endl;
103	8/16 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 3 times. ✗ Branch 12 not taken. ✓ Branch 14 taken 3 times. ✗ Branch 15 not taken. ✓ Branch 17 taken 3 times. ✗ Branch 18 not taken. ✓ Branch 21 taken 3 times. ✗ Branch 22 not taken. ✓ Branch 24 taken 3 times. ✗ Branch 25 not taken. ✗ Branch 31 not taken. ✓ Branch 32 taken 3 times.	3	BOOST_CHECK(capture_ios.str() == expected_buffer.str());
104		3	}
105
106		3	void test_removeCost(StateModelTypes::Type state_type) {
107			// Setup the test
108	1/2 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken.	3	StateModelFactory state_factory;
109	2/4 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 3 times. ✗ Branch 5 not taken.	3	crocoddyl::CostModelSum model(state_factory.create(state_type));
110
111			// add an active cost
112			boost::shared_ptr<crocoddyl::CostModelAbstract> rand_cost =
113	1/2 ✓ Branch 2 taken 3 times. ✗ Branch 3 not taken.	3	create_random_cost(state_type);
114	2/4 ✓ Branch 3 taken 3 times. ✗ Branch 4 not taken. ✓ Branch 6 taken 3 times. ✗ Branch 7 not taken.	3	model.addCost("random_cost", rand_cost, 1.);
115	6/12 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 3 times. ✗ Branch 12 not taken. ✓ Branch 19 taken 3 times. ✗ Branch 20 not taken. ✓ Branch 22 taken 3 times. ✗ Branch 23 not taken. ✗ Branch 27 not taken. ✓ Branch 28 taken 3 times.	3	BOOST_CHECK(model.get_nr() == rand_cost->get_activation()->get_nr());
116
117			// remove the cost
118	2/4 ✓ Branch 2 taken 3 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken.	3	model.removeCost("random_cost");
119	6/12 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 3 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 3 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 3 times. ✗ Branch 19 not taken. ✗ Branch 23 not taken. ✓ Branch 24 taken 3 times.	3	BOOST_CHECK(model.get_nr() == 0);
120	6/12 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 3 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 3 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 3 times. ✗ Branch 19 not taken. ✗ Branch 23 not taken. ✓ Branch 24 taken 3 times.	3	BOOST_CHECK(model.get_nr_total() == 0);
121		3	}
122
123		3	void test_removeCost_error_message(StateModelTypes::Type state_type) {
124			// Setup the test
125	1/2 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken.	3	StateModelFactory state_factory;
126	2/4 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 3 times. ✗ Branch 5 not taken.	3	crocoddyl::CostModelSum model(state_factory.create(state_type));
127
128			// remove a none existing cost form the container, we expect a cout message
129			// here
130	1/2 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken.	3	CaptureIOStream capture_ios;
131	1/2 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken.	3	capture_ios.beginCapture();
132	2/4 ✓ Branch 2 taken 3 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken.	3	model.removeCost("random_cost");
133	1/2 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken.	3	capture_ios.endCapture();
134
135			// Test that the error message is sent.
136	1/2 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken.	3	std::stringstream expected_buffer;
137			expected_buffer << "Warning: we couldn't remove the random_cost cost item, "
138	1/2 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken.	3	"it doesn't exist."
139	1/2 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken.	3	<< std::endl;
140	8/16 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 3 times. ✗ Branch 12 not taken. ✓ Branch 14 taken 3 times. ✗ Branch 15 not taken. ✓ Branch 17 taken 3 times. ✗ Branch 18 not taken. ✓ Branch 21 taken 3 times. ✗ Branch 22 not taken. ✓ Branch 24 taken 3 times. ✗ Branch 25 not taken. ✗ Branch 31 not taken. ✓ Branch 32 taken 3 times.	3	BOOST_CHECK(capture_ios.str() == expected_buffer.str());
141		3	}
142
143		3	void test_calc(StateModelTypes::Type state_type) {
144			// setup the test
145	1/2 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken.	3	StateModelFactory state_factory;
146	2/4 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 3 times. ✗ Branch 5 not taken.	3	crocoddyl::CostModelSum model(state_factory.create(state_type));
147			// create the corresponding data object
148			const boost::shared_ptr<crocoddyl::StateMultibody>& state =
149		3	boost::static_pointer_cast<crocoddyl::StateMultibody>(model.get_state());
150		3	pinocchio::Model& pinocchio_model = *state->get_pinocchio().get();
151	1/2 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken.	3	pinocchio::Data pinocchio_data(pinocchio_model);
152	1/2 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken.	3	crocoddyl::DataCollectorMultibody shared_data(&pinocchio_data);
153
154			// create and add some cost objects
155		3	std::vector<boost::shared_ptr<crocoddyl::CostModelAbstract> > models;
156		3	std::vector<boost::shared_ptr<crocoddyl::CostDataAbstract> > datas;
157	2/2 ✓ Branch 0 taken 15 times. ✓ Branch 1 taken 3 times.	18	for (std::size_t i = 0; i < 5; ++i) {
158	1/2 ✓ Branch 1 taken 15 times. ✗ Branch 2 not taken.	15	std::ostringstream os;
159	2/4 ✓ Branch 1 taken 15 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 15 times. ✗ Branch 5 not taken.	15	os << "random_cost_" << i;
160			const boost::shared_ptr<crocoddyl::CostModelAbstract>& m =
161	1/2 ✓ Branch 2 taken 15 times. ✗ Branch 3 not taken.	15	create_random_cost(state_type);
162	2/4 ✓ Branch 2 taken 15 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 15 times. ✗ Branch 6 not taken.	15	model.addCost(os.str(), m, 1.);
163	1/2 ✓ Branch 1 taken 15 times. ✗ Branch 2 not taken.	15	models.push_back(m);
164	2/4 ✓ Branch 2 taken 15 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 15 times. ✗ Branch 6 not taken.	15	datas.push_back(m->createData(&shared_data));
165		15	}
166
167			// create the data of the cost sum
168			const boost::shared_ptr<crocoddyl::CostDataSum>& data =
169	1/2 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken.	3	model.createData(&shared_data);
170
171			// compute the cost sum data for the case when all costs are defined as active
172	1/2 ✓ Branch 2 taken 3 times. ✗ Branch 3 not taken.	3	const Eigen::VectorXd x1 = state->rand();
173	2/4 ✓ Branch 2 taken 3 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken.	3	const Eigen::VectorXd u1 = Eigen::VectorXd::Random(model.get_nu());
174	2/4 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 3 times. ✗ Branch 5 not taken.	3	crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data,
175			x1);
176	3/6 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 3 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 3 times. ✗ Branch 8 not taken.	3	model.calc(data, x1, u1);
177
178			// check that the cost has been filled
179	6/12 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 3 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 3 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 3 times. ✗ Branch 19 not taken. ✗ Branch 23 not taken. ✓ Branch 24 taken 3 times.	3	BOOST_CHECK(data->cost > 0.);
180
181			// check the cost against single cost computations
182		3	double cost = 0;
183	2/2 ✓ Branch 0 taken 15 times. ✓ Branch 1 taken 3 times.	18	for (std::size_t i = 0; i < 5; ++i) {
184	3/6 ✓ Branch 3 taken 15 times. ✗ Branch 4 not taken. ✓ Branch 6 taken 15 times. ✗ Branch 7 not taken. ✓ Branch 10 taken 15 times. ✗ Branch 11 not taken.	15	models[i]->calc(datas[i], x1, u1);
185		15	cost += datas[i]->cost;
186			}
187	6/12 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 3 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 3 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 3 times. ✗ Branch 19 not taken. ✗ Branch 23 not taken. ✓ Branch 24 taken 3 times.	3	BOOST_CHECK(data->cost == cost);
188
189			// compute the cost sum data for the case when the first three costs are
190			// defined as active
191	2/4 ✓ Branch 2 taken 3 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken.	3	model.changeCostStatus("random_cost_3", false);
192	2/4 ✓ Branch 2 taken 3 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken.	3	model.changeCostStatus("random_cost_4", false);
193	1/2 ✓ Branch 2 taken 3 times. ✗ Branch 3 not taken.	3	const Eigen::VectorXd x2 = state->rand();
194	2/4 ✓ Branch 2 taken 3 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken.	3	const Eigen::VectorXd u2 = Eigen::VectorXd::Random(model.get_nu());
195	2/4 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 3 times. ✗ Branch 5 not taken.	3	crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data,
196			x1);
197	3/6 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 3 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 3 times. ✗ Branch 8 not taken.	3	model.calc(data, x2, u2);
198		3	cost = 0;
199	2/2 ✓ Branch 0 taken 9 times. ✓ Branch 1 taken 3 times.	12	for (std::size_t i = 0; i < 3;
200			++i) { // we need to update data because this costs are active
201	3/6 ✓ Branch 3 taken 9 times. ✗ Branch 4 not taken. ✓ Branch 6 taken 9 times. ✗ Branch 7 not taken. ✓ Branch 10 taken 9 times. ✗ Branch 11 not taken.	9	models[i]->calc(datas[i], x2, u2);
202		9	cost += datas[i]->cost;
203			}
204	6/12 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 3 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 3 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 3 times. ✗ Branch 19 not taken. ✗ Branch 23 not taken. ✓ Branch 24 taken 3 times.	3	BOOST_CHECK(data->cost == cost);
205		3	}
206
207		3	void test_calcDiff(StateModelTypes::Type state_type) {
208			// setup the test
209	1/2 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken.	3	StateModelFactory state_factory;
210	2/4 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 3 times. ✗ Branch 5 not taken.	3	crocoddyl::CostModelSum model(state_factory.create(state_type));
211			// create the corresponding data object
212			const boost::shared_ptr<crocoddyl::StateMultibody>& state =
213		3	boost::static_pointer_cast<crocoddyl::StateMultibody>(model.get_state());
214		3	pinocchio::Model& pinocchio_model = *state->get_pinocchio().get();
215	1/2 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken.	3	pinocchio::Data pinocchio_data(pinocchio_model);
216	1/2 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken.	3	crocoddyl::DataCollectorMultibody shared_data(&pinocchio_data);
217
218			// create and add some cost objects
219		3	std::vector<boost::shared_ptr<crocoddyl::CostModelAbstract> > models;
220		3	std::vector<boost::shared_ptr<crocoddyl::CostDataAbstract> > datas;
221	2/2 ✓ Branch 0 taken 15 times. ✓ Branch 1 taken 3 times.	18	for (std::size_t i = 0; i < 5; ++i) {
222	1/2 ✓ Branch 1 taken 15 times. ✗ Branch 2 not taken.	15	std::ostringstream os;
223	2/4 ✓ Branch 1 taken 15 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 15 times. ✗ Branch 5 not taken.	15	os << "random_cost_" << i;
224			const boost::shared_ptr<crocoddyl::CostModelAbstract>& m =
225	1/2 ✓ Branch 2 taken 15 times. ✗ Branch 3 not taken.	15	create_random_cost(state_type);
226	2/4 ✓ Branch 2 taken 15 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 15 times. ✗ Branch 6 not taken.	15	model.addCost(os.str(), m, 1.);
227	1/2 ✓ Branch 1 taken 15 times. ✗ Branch 2 not taken.	15	models.push_back(m);
228	2/4 ✓ Branch 2 taken 15 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 15 times. ✗ Branch 6 not taken.	15	datas.push_back(m->createData(&shared_data));
229		15	}
230
231			// create the data of the cost sum
232			const boost::shared_ptr<crocoddyl::CostDataSum>& data =
233	1/2 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken.	3	model.createData(&shared_data);
234
235			// compute the cost sum data for the case when all costs are defined as active
236	1/2 ✓ Branch 2 taken 3 times. ✗ Branch 3 not taken.	3	Eigen::VectorXd x1 = state->rand();
237	2/4 ✓ Branch 2 taken 3 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken.	3	const Eigen::VectorXd u1 = Eigen::VectorXd::Random(model.get_nu());
238	2/4 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 3 times. ✗ Branch 5 not taken.	3	crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data,
239			x1);
240	3/6 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 3 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 3 times. ✗ Branch 8 not taken.	3	model.calc(data, x1, u1);
241	3/6 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 3 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 3 times. ✗ Branch 8 not taken.	3	model.calcDiff(data, x1, u1);
242
243			// check that the cost has been filled
244	6/12 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 3 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 3 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 3 times. ✗ Branch 19 not taken. ✗ Branch 23 not taken. ✓ Branch 24 taken 3 times.	3	BOOST_CHECK(data->cost > 0.);
245
246			// check the cost against single cost computations
247		3	double cost = 0;
248	2/4 ✓ Branch 3 taken 3 times. ✗ Branch 4 not taken. ✓ Branch 6 taken 3 times. ✗ Branch 7 not taken.	3	Eigen::VectorXd Lx = Eigen::VectorXd::Zero(state->get_ndx());
249	2/4 ✓ Branch 2 taken 3 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken.	3	Eigen::VectorXd Lu = Eigen::VectorXd::Zero(model.get_nu());
250			Eigen::MatrixXd Lxx =
251	2/4 ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken. ✓ Branch 8 taken 3 times. ✗ Branch 9 not taken.	3	Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx());
252	2/4 ✓ Branch 4 taken 3 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 3 times. ✗ Branch 8 not taken.	3	Eigen::MatrixXd Lxu = Eigen::MatrixXd::Zero(state->get_ndx(), model.get_nu());
253	2/4 ✓ Branch 3 taken 3 times. ✗ Branch 4 not taken. ✓ Branch 6 taken 3 times. ✗ Branch 7 not taken.	3	Eigen::MatrixXd Luu = Eigen::MatrixXd::Zero(model.get_nu(), model.get_nu());
254	2/2 ✓ Branch 0 taken 15 times. ✓ Branch 1 taken 3 times.	18	for (std::size_t i = 0; i < 5; ++i) {
255	3/6 ✓ Branch 3 taken 15 times. ✗ Branch 4 not taken. ✓ Branch 6 taken 15 times. ✗ Branch 7 not taken. ✓ Branch 10 taken 15 times. ✗ Branch 11 not taken.	15	models[i]->calc(datas[i], x1, u1);
256	3/6 ✓ Branch 3 taken 15 times. ✗ Branch 4 not taken. ✓ Branch 6 taken 15 times. ✗ Branch 7 not taken. ✓ Branch 10 taken 15 times. ✗ Branch 11 not taken.	15	models[i]->calcDiff(datas[i], x1, u1);
257		15	cost += datas[i]->cost;
258	1/2 ✓ Branch 3 taken 15 times. ✗ Branch 4 not taken.	15	Lx += datas[i]->Lx;
259	1/2 ✓ Branch 3 taken 15 times. ✗ Branch 4 not taken.	15	Lu += datas[i]->Lu;
260	1/2 ✓ Branch 3 taken 15 times. ✗ Branch 4 not taken.	15	Lxx += datas[i]->Lxx;
261	1/2 ✓ Branch 3 taken 15 times. ✗ Branch 4 not taken.	15	Lxu += datas[i]->Lxu;
262	1/2 ✓ Branch 3 taken 15 times. ✗ Branch 4 not taken.	15	Luu += datas[i]->Luu;
263			}
264	6/12 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 3 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 3 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 3 times. ✗ Branch 19 not taken. ✗ Branch 23 not taken. ✓ Branch 24 taken 3 times.	3	BOOST_CHECK(data->cost == cost);
265	7/14 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 3 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 3 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 3 times. ✗ Branch 19 not taken. ✓ Branch 21 taken 3 times. ✗ Branch 22 not taken. ✗ Branch 26 not taken. ✓ Branch 27 taken 3 times.	3	BOOST_CHECK(data->Lx == Lx);
266	7/14 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 3 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 3 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 3 times. ✗ Branch 19 not taken. ✓ Branch 21 taken 3 times. ✗ Branch 22 not taken. ✗ Branch 26 not taken. ✓ Branch 27 taken 3 times.	3	BOOST_CHECK(data->Lu == Lu);
267	7/14 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 3 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 3 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 3 times. ✗ Branch 19 not taken. ✓ Branch 21 taken 3 times. ✗ Branch 22 not taken. ✗ Branch 26 not taken. ✓ Branch 27 taken 3 times.	3	BOOST_CHECK(data->Lxx == Lxx);
268	7/14 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 3 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 3 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 3 times. ✗ Branch 19 not taken. ✓ Branch 21 taken 3 times. ✗ Branch 22 not taken. ✗ Branch 26 not taken. ✓ Branch 27 taken 3 times.	3	BOOST_CHECK(data->Lxu == Lxu);
269	7/14 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 3 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 3 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 3 times. ✗ Branch 19 not taken. ✓ Branch 21 taken 3 times. ✗ Branch 22 not taken. ✗ Branch 26 not taken. ✓ Branch 27 taken 3 times.	3	BOOST_CHECK(data->Luu == Luu);
270
271	1/2 ✓ Branch 2 taken 3 times. ✗ Branch 3 not taken.	3	x1 = state->rand();
272	2/4 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 3 times. ✗ Branch 5 not taken.	3	crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data,
273			x1);
274	2/4 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 3 times. ✗ Branch 5 not taken.	3	model.calc(data, x1);
275	2/4 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 3 times. ✗ Branch 5 not taken.	3	model.calcDiff(data, x1);
276		3	cost = 0.;
277	1/2 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken.	3	Lx.setZero();
278	1/2 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken.	3	Lxx.setZero();
279	2/2 ✓ Branch 0 taken 15 times. ✓ Branch 1 taken 3 times.	18	for (std::size_t i = 0; i < 5; ++i) {
280	2/4 ✓ Branch 3 taken 15 times. ✗ Branch 4 not taken. ✓ Branch 7 taken 15 times. ✗ Branch 8 not taken.	15	models[i]->calc(datas[i], x1);
281	2/4 ✓ Branch 3 taken 15 times. ✗ Branch 4 not taken. ✓ Branch 7 taken 15 times. ✗ Branch 8 not taken.	15	models[i]->calcDiff(datas[i], x1);
282		15	cost += datas[i]->cost;
283	1/2 ✓ Branch 3 taken 15 times. ✗ Branch 4 not taken.	15	Lx += datas[i]->Lx;
284	1/2 ✓ Branch 3 taken 15 times. ✗ Branch 4 not taken.	15	Lxx += datas[i]->Lxx;
285			}
286	6/12 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 3 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 3 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 3 times. ✗ Branch 19 not taken. ✗ Branch 23 not taken. ✓ Branch 24 taken 3 times.	3	BOOST_CHECK(data->cost == cost);
287	7/14 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 3 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 3 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 3 times. ✗ Branch 19 not taken. ✓ Branch 21 taken 3 times. ✗ Branch 22 not taken. ✗ Branch 26 not taken. ✓ Branch 27 taken 3 times.	3	BOOST_CHECK(data->Lx == Lx);
288	7/14 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 3 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 3 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 3 times. ✗ Branch 19 not taken. ✓ Branch 21 taken 3 times. ✗ Branch 22 not taken. ✗ Branch 26 not taken. ✓ Branch 27 taken 3 times.	3	BOOST_CHECK(data->Lxx == Lxx);
289
290			// compute the cost sum data for the case when the first three costs are
291			// defined as active
292	2/4 ✓ Branch 2 taken 3 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken.	3	model.changeCostStatus("random_cost_3", false);
293	2/4 ✓ Branch 2 taken 3 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken.	3	model.changeCostStatus("random_cost_4", false);
294	1/2 ✓ Branch 2 taken 3 times. ✗ Branch 3 not taken.	3	Eigen::VectorXd x2 = state->rand();
295	2/4 ✓ Branch 2 taken 3 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken.	3	const Eigen::VectorXd u2 = Eigen::VectorXd::Random(model.get_nu());
296	2/4 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 3 times. ✗ Branch 5 not taken.	3	crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data,
297			x2);
298	3/6 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 3 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 3 times. ✗ Branch 8 not taken.	3	model.calc(data, x2, u2);
299	3/6 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 3 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 3 times. ✗ Branch 8 not taken.	3	model.calcDiff(data, x2, u2);
300		3	cost = 0;
301	1/2 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken.	3	Lx.setZero();
302	1/2 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken.	3	Lu.setZero();
303	1/2 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken.	3	Lxx.setZero();
304	1/2 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken.	3	Lxu.setZero();
305	1/2 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken.	3	Luu.setZero();
306	2/2 ✓ Branch 0 taken 9 times. ✓ Branch 1 taken 3 times.	12	for (std::size_t i = 0; i < 3;
307			++i) { // we need to update data because this costs are active
308	3/6 ✓ Branch 3 taken 9 times. ✗ Branch 4 not taken. ✓ Branch 6 taken 9 times. ✗ Branch 7 not taken. ✓ Branch 10 taken 9 times. ✗ Branch 11 not taken.	9	models[i]->calc(datas[i], x2, u2);
309	3/6 ✓ Branch 3 taken 9 times. ✗ Branch 4 not taken. ✓ Branch 6 taken 9 times. ✗ Branch 7 not taken. ✓ Branch 10 taken 9 times. ✗ Branch 11 not taken.	9	models[i]->calcDiff(datas[i], x2, u2);
310		9	cost += datas[i]->cost;
311	1/2 ✓ Branch 3 taken 9 times. ✗ Branch 4 not taken.	9	Lx += datas[i]->Lx;
312	1/2 ✓ Branch 3 taken 9 times. ✗ Branch 4 not taken.	9	Lu += datas[i]->Lu;
313	1/2 ✓ Branch 3 taken 9 times. ✗ Branch 4 not taken.	9	Lxx += datas[i]->Lxx;
314	1/2 ✓ Branch 3 taken 9 times. ✗ Branch 4 not taken.	9	Lxu += datas[i]->Lxu;
315	1/2 ✓ Branch 3 taken 9 times. ✗ Branch 4 not taken.	9	Luu += datas[i]->Luu;
316			}
317	6/12 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 3 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 3 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 3 times. ✗ Branch 19 not taken. ✗ Branch 23 not taken. ✓ Branch 24 taken 3 times.	3	BOOST_CHECK(data->cost == cost);
318	7/14 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 3 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 3 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 3 times. ✗ Branch 19 not taken. ✓ Branch 21 taken 3 times. ✗ Branch 22 not taken. ✗ Branch 26 not taken. ✓ Branch 27 taken 3 times.	3	BOOST_CHECK(data->Lx == Lx);
319	7/14 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 3 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 3 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 3 times. ✗ Branch 19 not taken. ✓ Branch 21 taken 3 times. ✗ Branch 22 not taken. ✗ Branch 26 not taken. ✓ Branch 27 taken 3 times.	3	BOOST_CHECK(data->Lu == Lu);
320	7/14 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 3 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 3 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 3 times. ✗ Branch 19 not taken. ✓ Branch 21 taken 3 times. ✗ Branch 22 not taken. ✗ Branch 26 not taken. ✓ Branch 27 taken 3 times.	3	BOOST_CHECK(data->Lxx == Lxx);
321	7/14 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 3 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 3 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 3 times. ✗ Branch 19 not taken. ✓ Branch 21 taken 3 times. ✗ Branch 22 not taken. ✗ Branch 26 not taken. ✓ Branch 27 taken 3 times.	3	BOOST_CHECK(data->Lxu == Lxu);
322	7/14 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 3 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 3 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 3 times. ✗ Branch 19 not taken. ✓ Branch 21 taken 3 times. ✗ Branch 22 not taken. ✗ Branch 26 not taken. ✓ Branch 27 taken 3 times.	3	BOOST_CHECK(data->Luu == Luu);
323
324	1/2 ✓ Branch 2 taken 3 times. ✗ Branch 3 not taken.	3	x2 = state->rand();
325	2/4 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 3 times. ✗ Branch 5 not taken.	3	crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data,
326			x2);
327	2/4 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 3 times. ✗ Branch 5 not taken.	3	model.calc(data, x2);
328	2/4 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 3 times. ✗ Branch 5 not taken.	3	model.calcDiff(data, x2);
329		3	cost = 0.;
330	1/2 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken.	3	Lx.setZero();
331	1/2 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken.	3	Lxx.setZero();
332	2/2 ✓ Branch 0 taken 9 times. ✓ Branch 1 taken 3 times.	12	for (std::size_t i = 0; i < 3; ++i) {
333	2/4 ✓ Branch 3 taken 9 times. ✗ Branch 4 not taken. ✓ Branch 7 taken 9 times. ✗ Branch 8 not taken.	9	models[i]->calc(datas[i], x2);
334	2/4 ✓ Branch 3 taken 9 times. ✗ Branch 4 not taken. ✓ Branch 7 taken 9 times. ✗ Branch 8 not taken.	9	models[i]->calcDiff(datas[i], x2);
335		9	cost += datas[i]->cost;
336	1/2 ✓ Branch 3 taken 9 times. ✗ Branch 4 not taken.	9	Lx += datas[i]->Lx;
337	1/2 ✓ Branch 3 taken 9 times. ✗ Branch 4 not taken.	9	Lxx += datas[i]->Lxx;
338			}
339	6/12 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 3 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 3 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 3 times. ✗ Branch 19 not taken. ✗ Branch 23 not taken. ✓ Branch 24 taken 3 times.	3	BOOST_CHECK(data->cost == cost);
340	7/14 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 3 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 3 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 3 times. ✗ Branch 19 not taken. ✓ Branch 21 taken 3 times. ✗ Branch 22 not taken. ✗ Branch 26 not taken. ✓ Branch 27 taken 3 times.	3	BOOST_CHECK(data->Lx == Lx);
341	7/14 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 3 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 3 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 3 times. ✗ Branch 19 not taken. ✓ Branch 21 taken 3 times. ✗ Branch 22 not taken. ✗ Branch 26 not taken. ✓ Branch 27 taken 3 times.	3	BOOST_CHECK(data->Lxx == Lxx);
342		3	}
343
344		3	void test_get_costs(StateModelTypes::Type state_type) {
345			// setup the test
346	1/2 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken.	3	StateModelFactory state_factory;
347	2/4 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 3 times. ✗ Branch 5 not taken.	3	crocoddyl::CostModelSum model(state_factory.create(state_type));
348			// create the corresponding data object
349			const boost::shared_ptr<crocoddyl::StateMultibody>& state =
350		3	boost::static_pointer_cast<crocoddyl::StateMultibody>(model.get_state());
351	1/2 ✓ Branch 4 taken 3 times. ✗ Branch 5 not taken.	3	pinocchio::Data pinocchio_data(*state->get_pinocchio().get());
352
353			// create and add some contact objects
354	2/2 ✓ Branch 0 taken 15 times. ✓ Branch 1 taken 3 times.	18	for (unsigned i = 0; i < 5; ++i) {
355	1/2 ✓ Branch 1 taken 15 times. ✗ Branch 2 not taken.	15	std::ostringstream os;
356	2/4 ✓ Branch 1 taken 15 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 15 times. ✗ Branch 5 not taken.	15	os << "random_cost_" << i;
357	3/6 ✓ Branch 2 taken 15 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 15 times. ✗ Branch 6 not taken. ✓ Branch 8 taken 15 times. ✗ Branch 9 not taken.	15	model.addCost(os.str(), create_random_cost(state_type), 1.);
358		15	}
359
360			// get the contacts
361		3	const crocoddyl::CostModelSum::CostModelContainer& costs = model.get_costs();
362
363			// test
364		3	crocoddyl::CostModelSum::CostModelContainer::const_iterator it_m, end_m;
365			unsigned i;
366	2/2 ✓ Branch 3 taken 15 times. ✓ Branch 4 taken 3 times.	18	for (i = 0, it_m = costs.begin(), end_m = costs.end(); it_m != end_m;
367		15	++it_m, ++i) {
368	1/2 ✓ Branch 1 taken 15 times. ✗ Branch 2 not taken.	15	std::ostringstream os;
369	2/4 ✓ Branch 1 taken 15 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 15 times. ✗ Branch 5 not taken.	15	os << "random_cost_" << i;
370	7/14 ✓ Branch 1 taken 15 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 15 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 15 times. ✗ Branch 12 not taken. ✓ Branch 14 taken 15 times. ✗ Branch 15 not taken. ✓ Branch 19 taken 15 times. ✗ Branch 20 not taken. ✓ Branch 22 taken 15 times. ✗ Branch 23 not taken. ✗ Branch 28 not taken. ✓ Branch 29 taken 15 times.	15	BOOST_CHECK(it_m->first == os.str());
371		15	}
372		3	}
373
374		3	void test_get_nr(StateModelTypes::Type state_type) {
375			// Setup the test
376	1/2 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken.	3	StateModelFactory state_factory;
377	2/4 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 3 times. ✗ Branch 5 not taken.	3	crocoddyl::CostModelSum model(state_factory.create(state_type));
378
379			// create the corresponding data object
380			const boost::shared_ptr<crocoddyl::StateMultibody>& state =
381		3	boost::static_pointer_cast<crocoddyl::StateMultibody>(model.get_state());
382	1/2 ✓ Branch 4 taken 3 times. ✗ Branch 5 not taken.	3	pinocchio::Data pinocchio_data(*state->get_pinocchio().get());
383
384			// create and add some contact objects
385	2/2 ✓ Branch 0 taken 15 times. ✓ Branch 1 taken 3 times.	18	for (unsigned i = 0; i < 5; ++i) {
386	1/2 ✓ Branch 1 taken 15 times. ✗ Branch 2 not taken.	15	std::ostringstream os;
387	2/4 ✓ Branch 1 taken 15 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 15 times. ✗ Branch 5 not taken.	15	os << "random_cost_" << i;
388	3/6 ✓ Branch 2 taken 15 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 15 times. ✗ Branch 6 not taken. ✓ Branch 8 taken 15 times. ✗ Branch 9 not taken.	15	model.addCost(os.str(), create_random_cost(state_type), 1.);
389		15	}
390
391			// compute ni
392		3	std::size_t nr = 0;
393		3	crocoddyl::CostModelSum::CostModelContainer::const_iterator it_m, end_m;
394		3	for (it_m = model.get_costs().begin(), end_m = model.get_costs().end();
395	2/2 ✓ Branch 1 taken 15 times. ✓ Branch 2 taken 3 times.	18	it_m != end_m; ++it_m) {
396		15	nr += it_m->second->cost->get_activation()->get_nr();
397			}
398
399	6/12 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 3 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 3 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 3 times. ✗ Branch 19 not taken. ✗ Branch 23 not taken. ✓ Branch 24 taken 3 times.	3	BOOST_CHECK(nr == model.get_nr());
400		3	}
401
402		3	void test_shareMemory(StateModelTypes::Type state_type) {
403			// setup the test
404	1/2 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken.	3	StateModelFactory state_factory;
405			const boost::shared_ptr<crocoddyl::StateAbstract> state =
406	1/2 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken.	3	state_factory.create(state_type);
407	1/2 ✓ Branch 2 taken 3 times. ✗ Branch 3 not taken.	3	crocoddyl::CostModelSum cost_model(state);
408		3	crocoddyl::DataCollectorAbstract shared_data;
409			const boost::shared_ptr<crocoddyl::CostDataSum>& cost_data =
410	1/2 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken.	3	cost_model.createData(&shared_data);
411
412		3	const std::size_t ndx = state->get_ndx();
413		3	const std::size_t nu = cost_model.get_nu();
414	1/2 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken.	3	crocoddyl::ActionModelLQR action_model(ndx, nu);
415			const boost::shared_ptr<crocoddyl::ActionDataAbstract>& action_data =
416	1/2 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken.	3	action_model.createData();
417
418	1/2 ✓ Branch 3 taken 3 times. ✗ Branch 4 not taken.	3	cost_data->shareMemory(action_data.get());
419	2/4 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken.	3	cost_data->Lx = Eigen::VectorXd::Random(ndx);
420	2/4 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken.	3	cost_data->Lu = Eigen::VectorXd::Random(nu);
421	2/4 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken.	3	cost_data->Lxx = Eigen::MatrixXd::Random(ndx, ndx);
422	2/4 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken.	3	cost_data->Luu = Eigen::MatrixXd::Random(nu, nu);
423	2/4 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken.	3	cost_data->Lxu = Eigen::MatrixXd::Random(ndx, nu);
424
425			// check that the data has been shared
426	7/14 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 3 times. ✗ Branch 12 not taken. ✓ Branch 16 taken 3 times. ✗ Branch 17 not taken. ✓ Branch 19 taken 3 times. ✗ Branch 20 not taken. ✓ Branch 22 taken 3 times. ✗ Branch 23 not taken. ✗ Branch 27 not taken. ✓ Branch 28 taken 3 times.	3	BOOST_CHECK(action_data->Lx.isApprox(cost_data->Lx, 1e-9));
427	7/14 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 3 times. ✗ Branch 12 not taken. ✓ Branch 16 taken 3 times. ✗ Branch 17 not taken. ✓ Branch 19 taken 3 times. ✗ Branch 20 not taken. ✓ Branch 22 taken 3 times. ✗ Branch 23 not taken. ✗ Branch 27 not taken. ✓ Branch 28 taken 3 times.	3	BOOST_CHECK(action_data->Lu.isApprox(cost_data->Lu, 1e-9));
428	7/14 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 3 times. ✗ Branch 12 not taken. ✓ Branch 16 taken 3 times. ✗ Branch 17 not taken. ✓ Branch 19 taken 3 times. ✗ Branch 20 not taken. ✓ Branch 22 taken 3 times. ✗ Branch 23 not taken. ✗ Branch 27 not taken. ✓ Branch 28 taken 3 times.	3	BOOST_CHECK(action_data->Lxx.isApprox(cost_data->Lxx, 1e-9));
429	7/14 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 3 times. ✗ Branch 12 not taken. ✓ Branch 16 taken 3 times. ✗ Branch 17 not taken. ✓ Branch 19 taken 3 times. ✗ Branch 20 not taken. ✓ Branch 22 taken 3 times. ✗ Branch 23 not taken. ✗ Branch 27 not taken. ✓ Branch 28 taken 3 times.	3	BOOST_CHECK(action_data->Luu.isApprox(cost_data->Luu, 1e-9));
430	7/14 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 3 times. ✗ Branch 12 not taken. ✓ Branch 16 taken 3 times. ✗ Branch 17 not taken. ✓ Branch 19 taken 3 times. ✗ Branch 20 not taken. ✓ Branch 22 taken 3 times. ✗ Branch 23 not taken. ✗ Branch 27 not taken. ✓ Branch 28 taken 3 times.	3	BOOST_CHECK(action_data->Lxu.isApprox(cost_data->Lxu, 1e-9));
431		3	}
432
433			//----------------------------------------------------------------------------//
434
435		3	void register_unit_tests(StateModelTypes::Type state_type) {
436	2/4 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 3 times. ✗ Branch 5 not taken.	3	boost::test_tools::output_test_stream test_name;
437			test_name << "test_CostModelSum"
438	3/6 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 3 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 3 times. ✗ Branch 8 not taken.	3	<< "_" << state_type;
439	4/8 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 3 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 3 times. ✗ Branch 8 not taken. ✓ Branch 10 taken 3 times. ✗ Branch 11 not taken.	3	std::cout << "Running " << test_name.str() << std::endl;
440	4/8 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken. ✓ Branch 8 taken 3 times. ✗ Branch 9 not taken. ✓ Branch 11 taken 3 times. ✗ Branch 12 not taken.	3	test_suite* ts = BOOST_TEST_SUITE(test_name.str());
441	5/10 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken. ✓ Branch 8 taken 3 times. ✗ Branch 9 not taken. ✓ Branch 11 taken 3 times. ✗ Branch 12 not taken. ✓ Branch 14 taken 3 times. ✗ Branch 15 not taken.	3	ts->add(BOOST_TEST_CASE(boost::bind(&test_constructor, state_type)));
442	5/10 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken. ✓ Branch 8 taken 3 times. ✗ Branch 9 not taken. ✓ Branch 11 taken 3 times. ✗ Branch 12 not taken. ✓ Branch 14 taken 3 times. ✗ Branch 15 not taken.	3	ts->add(BOOST_TEST_CASE(boost::bind(&test_addCost, state_type)));
443		6	ts->add(
444	5/10 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken. ✓ Branch 8 taken 3 times. ✗ Branch 9 not taken. ✓ Branch 11 taken 3 times. ✗ Branch 12 not taken. ✓ Branch 14 taken 3 times. ✗ Branch 15 not taken.	3	BOOST_TEST_CASE(boost::bind(&test_addCost_error_message, state_type)));
445	5/10 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken. ✓ Branch 8 taken 3 times. ✗ Branch 9 not taken. ✓ Branch 11 taken 3 times. ✗ Branch 12 not taken. ✓ Branch 14 taken 3 times. ✗ Branch 15 not taken.	3	ts->add(BOOST_TEST_CASE(boost::bind(&test_removeCost, state_type)));
446		6	ts->add(
447	5/10 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken. ✓ Branch 8 taken 3 times. ✗ Branch 9 not taken. ✓ Branch 11 taken 3 times. ✗ Branch 12 not taken. ✓ Branch 14 taken 3 times. ✗ Branch 15 not taken.	3	BOOST_TEST_CASE(boost::bind(&test_removeCost_error_message, state_type)));
448	5/10 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken. ✓ Branch 8 taken 3 times. ✗ Branch 9 not taken. ✓ Branch 11 taken 3 times. ✗ Branch 12 not taken. ✓ Branch 14 taken 3 times. ✗ Branch 15 not taken.	3	ts->add(BOOST_TEST_CASE(boost::bind(&test_calc, state_type)));
449	5/10 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken. ✓ Branch 8 taken 3 times. ✗ Branch 9 not taken. ✓ Branch 11 taken 3 times. ✗ Branch 12 not taken. ✓ Branch 14 taken 3 times. ✗ Branch 15 not taken.	3	ts->add(BOOST_TEST_CASE(boost::bind(&test_calcDiff, state_type)));
450	5/10 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken. ✓ Branch 8 taken 3 times. ✗ Branch 9 not taken. ✓ Branch 11 taken 3 times. ✗ Branch 12 not taken. ✓ Branch 14 taken 3 times. ✗ Branch 15 not taken.	3	ts->add(BOOST_TEST_CASE(boost::bind(&test_get_costs, state_type)));
451	5/10 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken. ✓ Branch 8 taken 3 times. ✗ Branch 9 not taken. ✓ Branch 11 taken 3 times. ✗ Branch 12 not taken. ✓ Branch 14 taken 3 times. ✗ Branch 15 not taken.	3	ts->add(BOOST_TEST_CASE(boost::bind(&test_get_nr, state_type)));
452	5/10 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 3 times. ✗ Branch 6 not taken. ✓ Branch 8 taken 3 times. ✗ Branch 9 not taken. ✓ Branch 11 taken 3 times. ✗ Branch 12 not taken. ✓ Branch 14 taken 3 times. ✗ Branch 15 not taken.	3	ts->add(BOOST_TEST_CASE(boost::bind(&test_shareMemory, state_type)));
453	3/6 ✓ Branch 1 taken 3 times. ✗ Branch 2 not taken. ✓ Branch 3 taken 3 times. ✗ Branch 4 not taken. ✓ Branch 6 taken 3 times. ✗ Branch 7 not taken.	3	framework::master_test_suite().add(ts);
454		3	}
455
456		1	bool init_function() {
457		1	register_unit_tests(StateModelTypes::StateMultibody_TalosArm);
458		1	register_unit_tests(StateModelTypes::StateMultibody_HyQ);
459		1	register_unit_tests(StateModelTypes::StateMultibody_Talos);
460		1	return true;
461			}
462
463		1	int main(int argc, char** argv) {
464		1	return ::boost::unit_test::unit_test_main(&init_function, argc, argv);
465			}
466

1

///////////////////////////////////////////////////////////////////////////////

2

// BSD 3-Clause License

3

//

4

5

// Copyright note valid unless otherwise stated in individual files.

6

7

///////////////////////////////////////////////////////////////////////////////

8

9

#define BOOST_TEST_NO_MAIN

10

#define BOOST_TEST_ALTERNATIVE_INIT_API

11

12

#include "crocoddyl/core/actions/lqr.hpp"

13

#include "crocoddyl/multibody/data/multibody.hpp"

14

#include "factory/cost.hpp"

15

#include "unittest_common.hpp"

16

17

using namespace boost::unit_test;

18

using namespace crocoddyl::unittest;

19

20

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

21

22

3

void test_constructor(StateModelTypes::Type state_type) {

23

// Setup the test

24

1/2

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

3

StateModelFactory state_factory;

25

2/4

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 4 taken 3 times.

✗ Branch 5 not taken.

3

crocoddyl::CostModelSum model(state_factory.create(state_type));

26

27

// Run the print function

28

1/2

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

3

std::ostringstream tmp;

29

1/2

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

3

tmp << model;

30

31

// Test the initial size of the map

32

6/12

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

✓ Branch 11 taken 3 times.

✗ Branch 12 not taken.

✓ Branch 16 taken 3 times.

✗ Branch 17 not taken.

✓ Branch 19 taken 3 times.

✗ Branch 20 not taken.

✗ Branch 24 not taken.

✓ Branch 25 taken 3 times.

3

BOOST_CHECK(model.get_costs().size() == 0);

33

3

}

34

35

3

void test_addCost(StateModelTypes::Type state_type) {

36

// Setup the test

37

1/2

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

3

StateModelFactory state_factory;

38

2/4

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 4 taken 3 times.

✗ Branch 5 not taken.

3

crocoddyl::CostModelSum model(state_factory.create(state_type));

39

40

// add an active cost

41

boost::shared_ptr<crocoddyl::CostModelAbstract> rand_cost_1 =

42

1/2

✓ Branch 2 taken 3 times.

✗ Branch 3 not taken.

3

create_random_cost(state_type);

43

2/4

✓ Branch 3 taken 3 times.

✗ Branch 4 not taken.

✓ Branch 6 taken 3 times.

✗ Branch 7 not taken.

3

model.addCost("random_cost_1", rand_cost_1, 1.);

44

6/12

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

✓ Branch 11 taken 3 times.

✗ Branch 12 not taken.

✓ Branch 19 taken 3 times.

✗ Branch 20 not taken.

✓ Branch 22 taken 3 times.

✗ Branch 23 not taken.

✗ Branch 27 not taken.

✓ Branch 28 taken 3 times.

3

BOOST_CHECK(model.get_nr() == rand_cost_1->get_activation()->get_nr());

45

6/12

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

✓ Branch 11 taken 3 times.

✗ Branch 12 not taken.

✓ Branch 19 taken 3 times.

✗ Branch 20 not taken.

✓ Branch 22 taken 3 times.

✗ Branch 23 not taken.

✗ Branch 27 not taken.

✓ Branch 28 taken 3 times.

3

BOOST_CHECK(model.get_nr_total() == rand_cost_1->get_activation()->get_nr());

46

47

// add an inactive cost

48

boost::shared_ptr<crocoddyl::CostModelAbstract> rand_cost_2 =

49

1/2

✓ Branch 2 taken 3 times.

✗ Branch 3 not taken.

3

create_random_cost(state_type);

50

2/4

✓ Branch 3 taken 3 times.

✗ Branch 4 not taken.

✓ Branch 6 taken 3 times.

✗ Branch 7 not taken.

3

model.addCost("random_cost_2", rand_cost_2, 1., false);

51

6/12

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

✓ Branch 11 taken 3 times.

✗ Branch 12 not taken.

✓ Branch 19 taken 3 times.

✗ Branch 20 not taken.

✓ Branch 22 taken 3 times.

✗ Branch 23 not taken.

✗ Branch 27 not taken.

✓ Branch 28 taken 3 times.

3

BOOST_CHECK(model.get_nr() == rand_cost_1->get_activation()->get_nr());

52

6/12

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

✓ Branch 11 taken 3 times.

✗ Branch 12 not taken.

✓ Branch 23 taken 3 times.

✗ Branch 24 not taken.

✓ Branch 26 taken 3 times.

✗ Branch 27 not taken.

✗ Branch 31 not taken.

✓ Branch 32 taken 3 times.

3

BOOST_CHECK(model.get_nr_total() ==

53

rand_cost_1->get_activation()->get_nr() +

54

rand_cost_2->get_activation()->get_nr());

55

56

// change the random cost 2 status

57

2/4

✓ Branch 2 taken 3 times.

✗ Branch 3 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

3

model.changeCostStatus("random_cost_2", true);

58

6/12

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

✓ Branch 11 taken 3 times.

✗ Branch 12 not taken.

✓ Branch 23 taken 3 times.

✗ Branch 24 not taken.

✓ Branch 26 taken 3 times.

✗ Branch 27 not taken.

✗ Branch 31 not taken.

✓ Branch 32 taken 3 times.

3

BOOST_CHECK(model.get_nr() == rand_cost_1->get_activation()->get_nr() +

59

rand_cost_2->get_activation()->get_nr());

60

6/12

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

✓ Branch 11 taken 3 times.

✗ Branch 12 not taken.

✓ Branch 23 taken 3 times.

✗ Branch 24 not taken.

✓ Branch 26 taken 3 times.

✗ Branch 27 not taken.

✗ Branch 31 not taken.

✓ Branch 32 taken 3 times.

3

BOOST_CHECK(model.get_nr_total() ==

61

rand_cost_1->get_activation()->get_nr() +

62

rand_cost_2->get_activation()->get_nr());

63

64

// change the random cost 1 status

65

2/4

✓ Branch 2 taken 3 times.

✗ Branch 3 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

3

model.changeCostStatus("random_cost_1", false);

66

6/12

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

✓ Branch 11 taken 3 times.

✗ Branch 12 not taken.

✓ Branch 19 taken 3 times.

✗ Branch 20 not taken.

✓ Branch 22 taken 3 times.

✗ Branch 23 not taken.

✗ Branch 27 not taken.

✓ Branch 28 taken 3 times.

3

BOOST_CHECK(model.get_nr() == rand_cost_2->get_activation()->get_nr());

67

6/12

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

✓ Branch 11 taken 3 times.

✗ Branch 12 not taken.

✓ Branch 23 taken 3 times.

✗ Branch 24 not taken.

✓ Branch 26 taken 3 times.

✗ Branch 27 not taken.

✗ Branch 31 not taken.

✓ Branch 32 taken 3 times.

3

BOOST_CHECK(model.get_nr_total() ==

68

rand_cost_1->get_activation()->get_nr() +

69

rand_cost_2->get_activation()->get_nr());

70

3

}

71

72

3

void test_addCost_error_message(StateModelTypes::Type state_type) {

73

// Setup the test

74

1/2

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

3

StateModelFactory state_factory;

75

2/4

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 4 taken 3 times.

✗ Branch 5 not taken.

3

crocoddyl::CostModelSum model(state_factory.create(state_type));

76

77

// create an cost object

78

boost::shared_ptr<crocoddyl::CostModelAbstract> rand_cost =

79

1/2

✓ Branch 2 taken 3 times.

✗ Branch 3 not taken.

3

create_random_cost(state_type);

80

81

// add twice the same cost object to the container

82

2/4

✓ Branch 3 taken 3 times.

✗ Branch 4 not taken.

✓ Branch 6 taken 3 times.

✗ Branch 7 not taken.

3

model.addCost("random_cost", rand_cost, 1.);

83

84

// test error message when we add a duplicate cost

85

1/2

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

3

CaptureIOStream capture_ios;

86

1/2

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

3

capture_ios.beginCapture();

87

2/4

✓ Branch 3 taken 3 times.

✗ Branch 4 not taken.

✓ Branch 6 taken 3 times.

✗ Branch 7 not taken.

3

model.addCost("random_cost", rand_cost, 1.);

88

1/2

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

3

capture_ios.endCapture();

89

1/2

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

3

std::stringstream expected_buffer;

90

expected_buffer << "Warning: we couldn't add the random_cost cost item, it "

91

1/2

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

3

"already existed."

92

1/2

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

3

<< std::endl;

93

8/16

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

✓ Branch 11 taken 3 times.

✗ Branch 12 not taken.

✓ Branch 14 taken 3 times.

✗ Branch 15 not taken.

✓ Branch 17 taken 3 times.

✗ Branch 18 not taken.

✓ Branch 21 taken 3 times.

✗ Branch 22 not taken.

✓ Branch 24 taken 3 times.

✗ Branch 25 not taken.

✗ Branch 31 not taken.

✓ Branch 32 taken 3 times.

3

BOOST_CHECK(capture_ios.str() == expected_buffer.str());

94

95

// test error message when we change the cost status of an inexistent cost

96

1/2

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

3

capture_ios.beginCapture();

97

2/4

✓ Branch 2 taken 3 times.

✗ Branch 3 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

3

model.changeCostStatus("no_exist_cost", true);

98

1/2

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

3

capture_ios.endCapture();

99

1/2

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

3

expected_buffer.clear();

100

expected_buffer << "Warning: we couldn't change the status of the "

101

1/2

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

3

"no_exist_cost cost item, it doesn't exist."

102

1/2

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

3

<< std::endl;

103

8/16

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

✓ Branch 11 taken 3 times.

✗ Branch 12 not taken.

✓ Branch 14 taken 3 times.

✗ Branch 15 not taken.

✓ Branch 17 taken 3 times.

✗ Branch 18 not taken.

✓ Branch 21 taken 3 times.

✗ Branch 22 not taken.

✓ Branch 24 taken 3 times.

✗ Branch 25 not taken.

✗ Branch 31 not taken.

✓ Branch 32 taken 3 times.

3

BOOST_CHECK(capture_ios.str() == expected_buffer.str());

104

3

}

105

106

3

void test_removeCost(StateModelTypes::Type state_type) {

107

// Setup the test

108

1/2

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

3

StateModelFactory state_factory;

109

2/4

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 4 taken 3 times.

✗ Branch 5 not taken.

3

crocoddyl::CostModelSum model(state_factory.create(state_type));

110

111

// add an active cost

112

boost::shared_ptr<crocoddyl::CostModelAbstract> rand_cost =

113

1/2

✓ Branch 2 taken 3 times.

✗ Branch 3 not taken.

3

create_random_cost(state_type);

114

2/4

✓ Branch 3 taken 3 times.

✗ Branch 4 not taken.

✓ Branch 6 taken 3 times.

✗ Branch 7 not taken.

3

model.addCost("random_cost", rand_cost, 1.);

115

6/12

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

✓ Branch 11 taken 3 times.

✗ Branch 12 not taken.

✓ Branch 19 taken 3 times.

✗ Branch 20 not taken.

✓ Branch 22 taken 3 times.

✗ Branch 23 not taken.

✗ Branch 27 not taken.

✓ Branch 28 taken 3 times.

3

BOOST_CHECK(model.get_nr() == rand_cost->get_activation()->get_nr());

116

117

// remove the cost

118

2/4

✓ Branch 2 taken 3 times.

✗ Branch 3 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

3

model.removeCost("random_cost");

119

6/12

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

✓ Branch 11 taken 3 times.

✗ Branch 12 not taken.

✓ Branch 15 taken 3 times.

✗ Branch 16 not taken.

✓ Branch 18 taken 3 times.

✗ Branch 19 not taken.

✗ Branch 23 not taken.

✓ Branch 24 taken 3 times.

3

BOOST_CHECK(model.get_nr() == 0);

120

6/12

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

✓ Branch 11 taken 3 times.

✗ Branch 12 not taken.

✓ Branch 15 taken 3 times.

✗ Branch 16 not taken.

✓ Branch 18 taken 3 times.

✗ Branch 19 not taken.

✗ Branch 23 not taken.

✓ Branch 24 taken 3 times.

3

BOOST_CHECK(model.get_nr_total() == 0);

121

3

}

122

123

3

void test_removeCost_error_message(StateModelTypes::Type state_type) {

124

// Setup the test

125

1/2

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

3

StateModelFactory state_factory;

126

2/4

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 4 taken 3 times.

✗ Branch 5 not taken.

3

crocoddyl::CostModelSum model(state_factory.create(state_type));

127

128

// remove a none existing cost form the container, we expect a cout message

129

// here

130

1/2

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

3

CaptureIOStream capture_ios;

131

1/2

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

3

capture_ios.beginCapture();

132

2/4

✓ Branch 2 taken 3 times.

✗ Branch 3 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

3

model.removeCost("random_cost");

133

1/2

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

3

capture_ios.endCapture();

134

135

// Test that the error message is sent.

136

1/2

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

3

std::stringstream expected_buffer;

137

expected_buffer << "Warning: we couldn't remove the random_cost cost item, "

138

1/2

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

3

"it doesn't exist."

139

1/2

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

3

<< std::endl;

140

8/16

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

✓ Branch 11 taken 3 times.

✗ Branch 12 not taken.

✓ Branch 14 taken 3 times.

✗ Branch 15 not taken.

✓ Branch 17 taken 3 times.

✗ Branch 18 not taken.

✓ Branch 21 taken 3 times.

✗ Branch 22 not taken.

✓ Branch 24 taken 3 times.

✗ Branch 25 not taken.

✗ Branch 31 not taken.

✓ Branch 32 taken 3 times.

3

BOOST_CHECK(capture_ios.str() == expected_buffer.str());

141

3

}

142

143

3

void test_calc(StateModelTypes::Type state_type) {

144

// setup the test

145

1/2

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

3

StateModelFactory state_factory;

146

2/4

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 4 taken 3 times.

✗ Branch 5 not taken.

3

crocoddyl::CostModelSum model(state_factory.create(state_type));

147

// create the corresponding data object

148

const boost::shared_ptr<crocoddyl::StateMultibody>& state =

149

3

boost::static_pointer_cast<crocoddyl::StateMultibody>(model.get_state());

150

3

pinocchio::Model& pinocchio_model = *state->get_pinocchio().get();

151

1/2

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

3

pinocchio::Data pinocchio_data(pinocchio_model);

152

1/2

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

3

crocoddyl::DataCollectorMultibody shared_data(&pinocchio_data);

153

154

// create and add some cost objects

155

3

std::vector<boost::shared_ptr<crocoddyl::CostModelAbstract> > models;

156

3

std::vector<boost::shared_ptr<crocoddyl::CostDataAbstract> > datas;

157

2/2

✓ Branch 0 taken 15 times.

✓ Branch 1 taken 3 times.

18

for (std::size_t i = 0; i < 5; ++i) {

158

1/2

✓ Branch 1 taken 15 times.

✗ Branch 2 not taken.

15

std::ostringstream os;

159

2/4

✓ Branch 1 taken 15 times.

✗ Branch 2 not taken.

✓ Branch 4 taken 15 times.

✗ Branch 5 not taken.

15

os << "random_cost_" << i;

160

const boost::shared_ptr<crocoddyl::CostModelAbstract>& m =

161

1/2

✓ Branch 2 taken 15 times.

✗ Branch 3 not taken.

15

create_random_cost(state_type);

162

2/4

✓ Branch 2 taken 15 times.

✗ Branch 3 not taken.

✓ Branch 5 taken 15 times.

✗ Branch 6 not taken.

15

model.addCost(os.str(), m, 1.);

163

1/2

✓ Branch 1 taken 15 times.

✗ Branch 2 not taken.

15

models.push_back(m);

164

2/4

✓ Branch 2 taken 15 times.

✗ Branch 3 not taken.

✓ Branch 5 taken 15 times.

✗ Branch 6 not taken.

15

datas.push_back(m->createData(&shared_data));

165

15

}

166

167

// create the data of the cost sum

168

const boost::shared_ptr<crocoddyl::CostDataSum>& data =

169

1/2

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

3

model.createData(&shared_data);

170

171

// compute the cost sum data for the case when all costs are defined as active

172

1/2

✓ Branch 2 taken 3 times.

✗ Branch 3 not taken.

3

const Eigen::VectorXd x1 = state->rand();

173

2/4

✓ Branch 2 taken 3 times.

✗ Branch 3 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

3

const Eigen::VectorXd u1 = Eigen::VectorXd::Random(model.get_nu());

174

2/4

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 4 taken 3 times.

✗ Branch 5 not taken.

3

crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data,

175

x1);

176

3/6

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 4 taken 3 times.

✗ Branch 5 not taken.

✓ Branch 7 taken 3 times.

✗ Branch 8 not taken.

3

model.calc(data, x1, u1);

177

178

// check that the cost has been filled

179

6/12

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

✓ Branch 11 taken 3 times.

✗ Branch 12 not taken.

✓ Branch 15 taken 3 times.

✗ Branch 16 not taken.

✓ Branch 18 taken 3 times.

✗ Branch 19 not taken.

✗ Branch 23 not taken.

✓ Branch 24 taken 3 times.

3

BOOST_CHECK(data->cost > 0.);

180

181

// check the cost against single cost computations

182

3

double cost = 0;

183

2/2

✓ Branch 0 taken 15 times.

✓ Branch 1 taken 3 times.

18

for (std::size_t i = 0; i < 5; ++i) {

184

3/6

✓ Branch 3 taken 15 times.

✗ Branch 4 not taken.

✓ Branch 6 taken 15 times.

✗ Branch 7 not taken.

✓ Branch 10 taken 15 times.

✗ Branch 11 not taken.

15

models[i]->calc(datas[i], x1, u1);

185

15

cost += datas[i]->cost;

186

}

187

6/12

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

✓ Branch 11 taken 3 times.

✗ Branch 12 not taken.

✓ Branch 15 taken 3 times.

✗ Branch 16 not taken.

✓ Branch 18 taken 3 times.

✗ Branch 19 not taken.

✗ Branch 23 not taken.

✓ Branch 24 taken 3 times.

3

BOOST_CHECK(data->cost == cost);

188

189

// compute the cost sum data for the case when the first three costs are

190

// defined as active

191

2/4

✓ Branch 2 taken 3 times.

✗ Branch 3 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

3

model.changeCostStatus("random_cost_3", false);

192

2/4

✓ Branch 2 taken 3 times.

✗ Branch 3 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

3

model.changeCostStatus("random_cost_4", false);

193

1/2

✓ Branch 2 taken 3 times.

✗ Branch 3 not taken.

3

const Eigen::VectorXd x2 = state->rand();

194

2/4

✓ Branch 2 taken 3 times.

✗ Branch 3 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

3

const Eigen::VectorXd u2 = Eigen::VectorXd::Random(model.get_nu());

195

2/4

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 4 taken 3 times.

✗ Branch 5 not taken.

3

crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data,

196

x1);

197

3/6

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 4 taken 3 times.

✗ Branch 5 not taken.

✓ Branch 7 taken 3 times.

✗ Branch 8 not taken.

3

model.calc(data, x2, u2);

198

3

cost = 0;

199

2/2

✓ Branch 0 taken 9 times.

✓ Branch 1 taken 3 times.

12

for (std::size_t i = 0; i < 3;

200

++i) { // we need to update data because this costs are active

201

3/6

✓ Branch 3 taken 9 times.

✗ Branch 4 not taken.

✓ Branch 6 taken 9 times.

✗ Branch 7 not taken.

✓ Branch 10 taken 9 times.

✗ Branch 11 not taken.

9

models[i]->calc(datas[i], x2, u2);

202

9

cost += datas[i]->cost;

203

}

204

6/12

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

✓ Branch 11 taken 3 times.

✗ Branch 12 not taken.

✓ Branch 15 taken 3 times.

✗ Branch 16 not taken.

✓ Branch 18 taken 3 times.

✗ Branch 19 not taken.

✗ Branch 23 not taken.

✓ Branch 24 taken 3 times.

3

BOOST_CHECK(data->cost == cost);

205

3

}

206

207

3

void test_calcDiff(StateModelTypes::Type state_type) {

208

// setup the test

209

1/2

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

3

StateModelFactory state_factory;

210

2/4

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 4 taken 3 times.

✗ Branch 5 not taken.

3

crocoddyl::CostModelSum model(state_factory.create(state_type));

211

// create the corresponding data object

212

const boost::shared_ptr<crocoddyl::StateMultibody>& state =

213

3

boost::static_pointer_cast<crocoddyl::StateMultibody>(model.get_state());

214

3

pinocchio::Model& pinocchio_model = *state->get_pinocchio().get();

215

1/2

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

3

pinocchio::Data pinocchio_data(pinocchio_model);

216

1/2

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

3

crocoddyl::DataCollectorMultibody shared_data(&pinocchio_data);

217

218

// create and add some cost objects

219

3

std::vector<boost::shared_ptr<crocoddyl::CostModelAbstract> > models;

220

3

std::vector<boost::shared_ptr<crocoddyl::CostDataAbstract> > datas;

221

2/2

✓ Branch 0 taken 15 times.

✓ Branch 1 taken 3 times.

18

for (std::size_t i = 0; i < 5; ++i) {

222

1/2

✓ Branch 1 taken 15 times.

✗ Branch 2 not taken.

15

std::ostringstream os;

223

2/4

✓ Branch 1 taken 15 times.

✗ Branch 2 not taken.

✓ Branch 4 taken 15 times.

✗ Branch 5 not taken.

15

os << "random_cost_" << i;

224

const boost::shared_ptr<crocoddyl::CostModelAbstract>& m =

225

1/2

✓ Branch 2 taken 15 times.

✗ Branch 3 not taken.

15

create_random_cost(state_type);

226

2/4

✓ Branch 2 taken 15 times.

✗ Branch 3 not taken.

✓ Branch 5 taken 15 times.

✗ Branch 6 not taken.

15

model.addCost(os.str(), m, 1.);

227

1/2

✓ Branch 1 taken 15 times.

✗ Branch 2 not taken.

15

models.push_back(m);

228

2/4

✓ Branch 2 taken 15 times.

✗ Branch 3 not taken.

✓ Branch 5 taken 15 times.

✗ Branch 6 not taken.

15

datas.push_back(m->createData(&shared_data));

229

15

}

230

231

// create the data of the cost sum

232

const boost::shared_ptr<crocoddyl::CostDataSum>& data =

233

1/2

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

3

model.createData(&shared_data);

234

235

// compute the cost sum data for the case when all costs are defined as active

236

1/2

✓ Branch 2 taken 3 times.

✗ Branch 3 not taken.

3

Eigen::VectorXd x1 = state->rand();

237

2/4

✓ Branch 2 taken 3 times.

✗ Branch 3 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

3

const Eigen::VectorXd u1 = Eigen::VectorXd::Random(model.get_nu());

238

2/4

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 4 taken 3 times.

✗ Branch 5 not taken.

3

crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data,

239

x1);

240

3/6

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 4 taken 3 times.

✗ Branch 5 not taken.

✓ Branch 7 taken 3 times.

✗ Branch 8 not taken.

3

model.calc(data, x1, u1);

241

3/6

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 4 taken 3 times.

✗ Branch 5 not taken.

✓ Branch 7 taken 3 times.

✗ Branch 8 not taken.

3

model.calcDiff(data, x1, u1);

242

243

// check that the cost has been filled

244

6/12

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

✓ Branch 11 taken 3 times.

✗ Branch 12 not taken.

✓ Branch 15 taken 3 times.

✗ Branch 16 not taken.

✓ Branch 18 taken 3 times.

✗ Branch 19 not taken.

✗ Branch 23 not taken.

✓ Branch 24 taken 3 times.

3

BOOST_CHECK(data->cost > 0.);

245

246

// check the cost against single cost computations

247

3

double cost = 0;

248

2/4

✓ Branch 3 taken 3 times.

✗ Branch 4 not taken.

✓ Branch 6 taken 3 times.

✗ Branch 7 not taken.

3

Eigen::VectorXd Lx = Eigen::VectorXd::Zero(state->get_ndx());

249

2/4

✓ Branch 2 taken 3 times.

✗ Branch 3 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

3

Eigen::VectorXd Lu = Eigen::VectorXd::Zero(model.get_nu());

250

Eigen::MatrixXd Lxx =

251

2/4

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

✓ Branch 8 taken 3 times.

✗ Branch 9 not taken.

3

Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx());

252

2/4

✓ Branch 4 taken 3 times.

✗ Branch 5 not taken.

✓ Branch 7 taken 3 times.

✗ Branch 8 not taken.

3

Eigen::MatrixXd Lxu = Eigen::MatrixXd::Zero(state->get_ndx(), model.get_nu());

253

2/4

✓ Branch 3 taken 3 times.

✗ Branch 4 not taken.

✓ Branch 6 taken 3 times.

✗ Branch 7 not taken.

3

Eigen::MatrixXd Luu = Eigen::MatrixXd::Zero(model.get_nu(), model.get_nu());

254

2/2

✓ Branch 0 taken 15 times.

✓ Branch 1 taken 3 times.

18

for (std::size_t i = 0; i < 5; ++i) {

255

3/6

✓ Branch 3 taken 15 times.

✗ Branch 4 not taken.

✓ Branch 6 taken 15 times.

✗ Branch 7 not taken.

✓ Branch 10 taken 15 times.

✗ Branch 11 not taken.

15

models[i]->calc(datas[i], x1, u1);

256

3/6

✓ Branch 3 taken 15 times.

✗ Branch 4 not taken.

✓ Branch 6 taken 15 times.

✗ Branch 7 not taken.

✓ Branch 10 taken 15 times.

✗ Branch 11 not taken.

15

models[i]->calcDiff(datas[i], x1, u1);

257

15

cost += datas[i]->cost;

258

1/2

✓ Branch 3 taken 15 times.

✗ Branch 4 not taken.

15

Lx += datas[i]->Lx;

259

1/2

✓ Branch 3 taken 15 times.

✗ Branch 4 not taken.

15

Lu += datas[i]->Lu;

260

1/2

✓ Branch 3 taken 15 times.

✗ Branch 4 not taken.

15

Lxx += datas[i]->Lxx;

261

1/2

✓ Branch 3 taken 15 times.

✗ Branch 4 not taken.

15

Lxu += datas[i]->Lxu;

262

1/2

✓ Branch 3 taken 15 times.

✗ Branch 4 not taken.

15

Luu += datas[i]->Luu;

263

}

264

6/12

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

✓ Branch 11 taken 3 times.

✗ Branch 12 not taken.

✓ Branch 15 taken 3 times.

✗ Branch 16 not taken.

✓ Branch 18 taken 3 times.

✗ Branch 19 not taken.

✗ Branch 23 not taken.

✓ Branch 24 taken 3 times.

3

BOOST_CHECK(data->cost == cost);

265

7/14

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

✓ Branch 11 taken 3 times.

✗ Branch 12 not taken.

✓ Branch 15 taken 3 times.

✗ Branch 16 not taken.

✓ Branch 18 taken 3 times.

✗ Branch 19 not taken.

✓ Branch 21 taken 3 times.

✗ Branch 22 not taken.

✗ Branch 26 not taken.

✓ Branch 27 taken 3 times.

3

BOOST_CHECK(data->Lx == Lx);

266

7/14

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

✓ Branch 11 taken 3 times.

✗ Branch 12 not taken.

✓ Branch 15 taken 3 times.

✗ Branch 16 not taken.

✓ Branch 18 taken 3 times.

✗ Branch 19 not taken.

✓ Branch 21 taken 3 times.

✗ Branch 22 not taken.

✗ Branch 26 not taken.

✓ Branch 27 taken 3 times.

3

BOOST_CHECK(data->Lu == Lu);

267

7/14

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

✓ Branch 11 taken 3 times.

✗ Branch 12 not taken.

✓ Branch 15 taken 3 times.

✗ Branch 16 not taken.

✓ Branch 18 taken 3 times.

✗ Branch 19 not taken.

✓ Branch 21 taken 3 times.

✗ Branch 22 not taken.

✗ Branch 26 not taken.

✓ Branch 27 taken 3 times.

3

BOOST_CHECK(data->Lxx == Lxx);

268

7/14

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

✓ Branch 11 taken 3 times.

✗ Branch 12 not taken.

✓ Branch 15 taken 3 times.

✗ Branch 16 not taken.

✓ Branch 18 taken 3 times.

✗ Branch 19 not taken.

✓ Branch 21 taken 3 times.

✗ Branch 22 not taken.

✗ Branch 26 not taken.

✓ Branch 27 taken 3 times.

3

BOOST_CHECK(data->Lxu == Lxu);

269

7/14

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

✓ Branch 11 taken 3 times.

✗ Branch 12 not taken.

✓ Branch 15 taken 3 times.

✗ Branch 16 not taken.

✓ Branch 18 taken 3 times.

✗ Branch 19 not taken.

✓ Branch 21 taken 3 times.

✗ Branch 22 not taken.

✗ Branch 26 not taken.

✓ Branch 27 taken 3 times.

3

BOOST_CHECK(data->Luu == Luu);

270

271

1/2

✓ Branch 2 taken 3 times.

✗ Branch 3 not taken.

3

x1 = state->rand();

272

2/4

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 4 taken 3 times.

✗ Branch 5 not taken.

3

crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data,

273

x1);

274

2/4

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 4 taken 3 times.

✗ Branch 5 not taken.

3

model.calc(data, x1);

275

2/4

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 4 taken 3 times.

✗ Branch 5 not taken.

3

model.calcDiff(data, x1);

276

3

cost = 0.;

277

1/2

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

3

Lx.setZero();

278

1/2

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

3

Lxx.setZero();

279

2/2

✓ Branch 0 taken 15 times.

✓ Branch 1 taken 3 times.

18

for (std::size_t i = 0; i < 5; ++i) {

280

2/4

✓ Branch 3 taken 15 times.

✗ Branch 4 not taken.

✓ Branch 7 taken 15 times.

✗ Branch 8 not taken.

15

models[i]->calc(datas[i], x1);

281

2/4

✓ Branch 3 taken 15 times.

✗ Branch 4 not taken.

✓ Branch 7 taken 15 times.

✗ Branch 8 not taken.

15

models[i]->calcDiff(datas[i], x1);

282

15

cost += datas[i]->cost;

283

1/2

✓ Branch 3 taken 15 times.

✗ Branch 4 not taken.

15

Lx += datas[i]->Lx;

284

1/2

✓ Branch 3 taken 15 times.

✗ Branch 4 not taken.

15

Lxx += datas[i]->Lxx;

285

}

286

6/12

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

✓ Branch 11 taken 3 times.

✗ Branch 12 not taken.

✓ Branch 15 taken 3 times.

✗ Branch 16 not taken.

✓ Branch 18 taken 3 times.

✗ Branch 19 not taken.

✗ Branch 23 not taken.

✓ Branch 24 taken 3 times.

3

BOOST_CHECK(data->cost == cost);

287

7/14

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

✓ Branch 11 taken 3 times.

✗ Branch 12 not taken.

✓ Branch 15 taken 3 times.

✗ Branch 16 not taken.

✓ Branch 18 taken 3 times.

✗ Branch 19 not taken.

✓ Branch 21 taken 3 times.

✗ Branch 22 not taken.

✗ Branch 26 not taken.

✓ Branch 27 taken 3 times.

3

BOOST_CHECK(data->Lx == Lx);

288

7/14

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

✓ Branch 11 taken 3 times.

✗ Branch 12 not taken.

✓ Branch 15 taken 3 times.

✗ Branch 16 not taken.

✓ Branch 18 taken 3 times.

✗ Branch 19 not taken.

✓ Branch 21 taken 3 times.

✗ Branch 22 not taken.

✗ Branch 26 not taken.

✓ Branch 27 taken 3 times.

3

BOOST_CHECK(data->Lxx == Lxx);

289

290

// compute the cost sum data for the case when the first three costs are

291

// defined as active

292

2/4

✓ Branch 2 taken 3 times.

✗ Branch 3 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

3

model.changeCostStatus("random_cost_3", false);

293

2/4

✓ Branch 2 taken 3 times.

✗ Branch 3 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

3

model.changeCostStatus("random_cost_4", false);

294

1/2

✓ Branch 2 taken 3 times.

✗ Branch 3 not taken.

3

Eigen::VectorXd x2 = state->rand();

295

2/4

✓ Branch 2 taken 3 times.

✗ Branch 3 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

3

const Eigen::VectorXd u2 = Eigen::VectorXd::Random(model.get_nu());

296

2/4

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 4 taken 3 times.

✗ Branch 5 not taken.

3

crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data,

297

x2);

298

3/6

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 4 taken 3 times.

✗ Branch 5 not taken.

✓ Branch 7 taken 3 times.

✗ Branch 8 not taken.

3

model.calc(data, x2, u2);

299

3/6

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 4 taken 3 times.

✗ Branch 5 not taken.

✓ Branch 7 taken 3 times.

✗ Branch 8 not taken.

3

model.calcDiff(data, x2, u2);

300

3

cost = 0;

301

1/2

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

3

Lx.setZero();

302

1/2

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

3

Lu.setZero();

303

1/2

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

3

Lxx.setZero();

304

1/2

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

3

Lxu.setZero();

305

1/2

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

3

Luu.setZero();

306

2/2

✓ Branch 0 taken 9 times.

✓ Branch 1 taken 3 times.

12

for (std::size_t i = 0; i < 3;

307

++i) { // we need to update data because this costs are active

308

3/6

✓ Branch 3 taken 9 times.

✗ Branch 4 not taken.

✓ Branch 6 taken 9 times.

✗ Branch 7 not taken.

✓ Branch 10 taken 9 times.

✗ Branch 11 not taken.

9

models[i]->calc(datas[i], x2, u2);

309

3/6

✓ Branch 3 taken 9 times.

✗ Branch 4 not taken.

✓ Branch 6 taken 9 times.

✗ Branch 7 not taken.

✓ Branch 10 taken 9 times.

✗ Branch 11 not taken.

9

models[i]->calcDiff(datas[i], x2, u2);

310

9

cost += datas[i]->cost;

311

1/2

✓ Branch 3 taken 9 times.

✗ Branch 4 not taken.

9

Lx += datas[i]->Lx;

312

1/2

✓ Branch 3 taken 9 times.

✗ Branch 4 not taken.

9

Lu += datas[i]->Lu;

313

1/2

✓ Branch 3 taken 9 times.

✗ Branch 4 not taken.

9

Lxx += datas[i]->Lxx;

314

1/2

✓ Branch 3 taken 9 times.

✗ Branch 4 not taken.

9

Lxu += datas[i]->Lxu;

315

1/2

✓ Branch 3 taken 9 times.

✗ Branch 4 not taken.

9

Luu += datas[i]->Luu;

316

}

317

6/12

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

✓ Branch 11 taken 3 times.

✗ Branch 12 not taken.

✓ Branch 15 taken 3 times.

✗ Branch 16 not taken.

✓ Branch 18 taken 3 times.

✗ Branch 19 not taken.

✗ Branch 23 not taken.

✓ Branch 24 taken 3 times.

3

BOOST_CHECK(data->cost == cost);

318

7/14

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

✓ Branch 11 taken 3 times.

✗ Branch 12 not taken.

✓ Branch 15 taken 3 times.

✗ Branch 16 not taken.

✓ Branch 18 taken 3 times.

✗ Branch 19 not taken.

✓ Branch 21 taken 3 times.

✗ Branch 22 not taken.

✗ Branch 26 not taken.

✓ Branch 27 taken 3 times.

3

BOOST_CHECK(data->Lx == Lx);

319

7/14

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

✓ Branch 11 taken 3 times.

✗ Branch 12 not taken.

✓ Branch 15 taken 3 times.

✗ Branch 16 not taken.

✓ Branch 18 taken 3 times.

✗ Branch 19 not taken.

✓ Branch 21 taken 3 times.

✗ Branch 22 not taken.

✗ Branch 26 not taken.

✓ Branch 27 taken 3 times.

3

BOOST_CHECK(data->Lu == Lu);

320

7/14

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

✓ Branch 11 taken 3 times.

✗ Branch 12 not taken.

✓ Branch 15 taken 3 times.

✗ Branch 16 not taken.

✓ Branch 18 taken 3 times.

✗ Branch 19 not taken.

✓ Branch 21 taken 3 times.

✗ Branch 22 not taken.

✗ Branch 26 not taken.

✓ Branch 27 taken 3 times.

3

BOOST_CHECK(data->Lxx == Lxx);

321

7/14

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

✓ Branch 11 taken 3 times.

✗ Branch 12 not taken.

✓ Branch 15 taken 3 times.

✗ Branch 16 not taken.

✓ Branch 18 taken 3 times.

✗ Branch 19 not taken.

✓ Branch 21 taken 3 times.

✗ Branch 22 not taken.

✗ Branch 26 not taken.

✓ Branch 27 taken 3 times.

3

BOOST_CHECK(data->Lxu == Lxu);

322

7/14

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

✓ Branch 11 taken 3 times.

✗ Branch 12 not taken.

✓ Branch 15 taken 3 times.

✗ Branch 16 not taken.

✓ Branch 18 taken 3 times.

✗ Branch 19 not taken.

✓ Branch 21 taken 3 times.

✗ Branch 22 not taken.

✗ Branch 26 not taken.

✓ Branch 27 taken 3 times.

3

BOOST_CHECK(data->Luu == Luu);

323

324

1/2

✓ Branch 2 taken 3 times.

✗ Branch 3 not taken.

3

x2 = state->rand();

325

2/4

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 4 taken 3 times.

✗ Branch 5 not taken.

3

crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data,

326

x2);

327

2/4

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 4 taken 3 times.

✗ Branch 5 not taken.

3

model.calc(data, x2);

328

2/4

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 4 taken 3 times.

✗ Branch 5 not taken.

3

model.calcDiff(data, x2);

329

3

cost = 0.;

330

1/2

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

3

Lx.setZero();

331

1/2

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

3

Lxx.setZero();

332

2/2

✓ Branch 0 taken 9 times.

✓ Branch 1 taken 3 times.

12

for (std::size_t i = 0; i < 3; ++i) {

333

2/4

✓ Branch 3 taken 9 times.

✗ Branch 4 not taken.

✓ Branch 7 taken 9 times.

✗ Branch 8 not taken.

9

models[i]->calc(datas[i], x2);

334

2/4

✓ Branch 3 taken 9 times.

✗ Branch 4 not taken.

✓ Branch 7 taken 9 times.

✗ Branch 8 not taken.

9

models[i]->calcDiff(datas[i], x2);

335

9

cost += datas[i]->cost;

336

1/2

✓ Branch 3 taken 9 times.

✗ Branch 4 not taken.

9

Lx += datas[i]->Lx;

337

1/2

✓ Branch 3 taken 9 times.

✗ Branch 4 not taken.

9

Lxx += datas[i]->Lxx;

338

}

339

6/12

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

✓ Branch 11 taken 3 times.

✗ Branch 12 not taken.

✓ Branch 15 taken 3 times.

✗ Branch 16 not taken.

✓ Branch 18 taken 3 times.

✗ Branch 19 not taken.

✗ Branch 23 not taken.

✓ Branch 24 taken 3 times.

3

BOOST_CHECK(data->cost == cost);

340

7/14

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

✓ Branch 11 taken 3 times.

✗ Branch 12 not taken.

✓ Branch 15 taken 3 times.

✗ Branch 16 not taken.

✓ Branch 18 taken 3 times.

✗ Branch 19 not taken.

✓ Branch 21 taken 3 times.

✗ Branch 22 not taken.

✗ Branch 26 not taken.

✓ Branch 27 taken 3 times.

3

BOOST_CHECK(data->Lx == Lx);

341

7/14

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

✓ Branch 11 taken 3 times.

✗ Branch 12 not taken.

✓ Branch 15 taken 3 times.

✗ Branch 16 not taken.

✓ Branch 18 taken 3 times.

✗ Branch 19 not taken.

✓ Branch 21 taken 3 times.

✗ Branch 22 not taken.

✗ Branch 26 not taken.

✓ Branch 27 taken 3 times.

3

BOOST_CHECK(data->Lxx == Lxx);

342

3

}

343

344

3

void test_get_costs(StateModelTypes::Type state_type) {

345

// setup the test

346

1/2

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

3

StateModelFactory state_factory;

347

2/4

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 4 taken 3 times.

✗ Branch 5 not taken.

3

crocoddyl::CostModelSum model(state_factory.create(state_type));

348

// create the corresponding data object

349

const boost::shared_ptr<crocoddyl::StateMultibody>& state =

350

3

boost::static_pointer_cast<crocoddyl::StateMultibody>(model.get_state());

351

1/2

✓ Branch 4 taken 3 times.

✗ Branch 5 not taken.

3

pinocchio::Data pinocchio_data(*state->get_pinocchio().get());

352

353

// create and add some contact objects

354

2/2

✓ Branch 0 taken 15 times.

✓ Branch 1 taken 3 times.

18

for (unsigned i = 0; i < 5; ++i) {

355

1/2

✓ Branch 1 taken 15 times.

✗ Branch 2 not taken.

15

std::ostringstream os;

356

2/4

✓ Branch 1 taken 15 times.

✗ Branch 2 not taken.

✓ Branch 4 taken 15 times.

✗ Branch 5 not taken.

15

os << "random_cost_" << i;

357

3/6

✓ Branch 2 taken 15 times.

✗ Branch 3 not taken.

✓ Branch 5 taken 15 times.

✗ Branch 6 not taken.

✓ Branch 8 taken 15 times.

✗ Branch 9 not taken.

15

model.addCost(os.str(), create_random_cost(state_type), 1.);

358

15

}

359

360

// get the contacts

361

3

const crocoddyl::CostModelSum::CostModelContainer& costs = model.get_costs();

362

363

// test

364

3

crocoddyl::CostModelSum::CostModelContainer::const_iterator it_m, end_m;

365

unsigned i;

366

2/2

✓ Branch 3 taken 15 times.

✓ Branch 4 taken 3 times.

18

for (i = 0, it_m = costs.begin(), end_m = costs.end(); it_m != end_m;

367

15

++it_m, ++i) {

368

1/2

✓ Branch 1 taken 15 times.

✗ Branch 2 not taken.

15

std::ostringstream os;

369

2/4

✓ Branch 1 taken 15 times.

✗ Branch 2 not taken.

✓ Branch 4 taken 15 times.

✗ Branch 5 not taken.

15

os << "random_cost_" << i;

370

7/14

✓ Branch 1 taken 15 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 15 times.

✗ Branch 6 not taken.

✓ Branch 11 taken 15 times.

✗ Branch 12 not taken.

✓ Branch 14 taken 15 times.

✗ Branch 15 not taken.

✓ Branch 19 taken 15 times.

✗ Branch 20 not taken.

✓ Branch 22 taken 15 times.

✗ Branch 23 not taken.

✗ Branch 28 not taken.

✓ Branch 29 taken 15 times.

15

BOOST_CHECK(it_m->first == os.str());

371

15

}

372

3

}

373

374

3

void test_get_nr(StateModelTypes::Type state_type) {

375

// Setup the test

376

1/2

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

3

StateModelFactory state_factory;

377

2/4

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 4 taken 3 times.

✗ Branch 5 not taken.

3

crocoddyl::CostModelSum model(state_factory.create(state_type));

378

379

// create the corresponding data object

380

const boost::shared_ptr<crocoddyl::StateMultibody>& state =

381

3

boost::static_pointer_cast<crocoddyl::StateMultibody>(model.get_state());

382

1/2

✓ Branch 4 taken 3 times.

✗ Branch 5 not taken.

3

pinocchio::Data pinocchio_data(*state->get_pinocchio().get());

383

384

// create and add some contact objects

385

2/2

✓ Branch 0 taken 15 times.

✓ Branch 1 taken 3 times.

18

for (unsigned i = 0; i < 5; ++i) {

386

1/2

✓ Branch 1 taken 15 times.

✗ Branch 2 not taken.

15

std::ostringstream os;

387

2/4

✓ Branch 1 taken 15 times.

✗ Branch 2 not taken.

✓ Branch 4 taken 15 times.

✗ Branch 5 not taken.

15

os << "random_cost_" << i;

388

3/6

✓ Branch 2 taken 15 times.

✗ Branch 3 not taken.

✓ Branch 5 taken 15 times.

✗ Branch 6 not taken.

✓ Branch 8 taken 15 times.

✗ Branch 9 not taken.

15

model.addCost(os.str(), create_random_cost(state_type), 1.);

389

15

}

390

391

// compute ni

392

3

std::size_t nr = 0;

393

3

crocoddyl::CostModelSum::CostModelContainer::const_iterator it_m, end_m;

394

3

for (it_m = model.get_costs().begin(), end_m = model.get_costs().end();

395

2/2

✓ Branch 1 taken 15 times.

✓ Branch 2 taken 3 times.

18

it_m != end_m; ++it_m) {

396

15

nr += it_m->second->cost->get_activation()->get_nr();

397

}

398

399

6/12

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

✓ Branch 11 taken 3 times.

✗ Branch 12 not taken.

✓ Branch 15 taken 3 times.

✗ Branch 16 not taken.

✓ Branch 18 taken 3 times.

✗ Branch 19 not taken.

✗ Branch 23 not taken.

✓ Branch 24 taken 3 times.

3

BOOST_CHECK(nr == model.get_nr());

400

3

}

401

402

3

void test_shareMemory(StateModelTypes::Type state_type) {

403

// setup the test

404

1/2

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

3

StateModelFactory state_factory;

405

const boost::shared_ptr<crocoddyl::StateAbstract> state =

406

1/2

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

3

state_factory.create(state_type);

407

1/2

✓ Branch 2 taken 3 times.

✗ Branch 3 not taken.

3

crocoddyl::CostModelSum cost_model(state);

408

3

crocoddyl::DataCollectorAbstract shared_data;

409

const boost::shared_ptr<crocoddyl::CostDataSum>& cost_data =

410

1/2

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

3

cost_model.createData(&shared_data);

411

412

3

const std::size_t ndx = state->get_ndx();

413

3

const std::size_t nu = cost_model.get_nu();

414

1/2

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

3

crocoddyl::ActionModelLQR action_model(ndx, nu);

415

const boost::shared_ptr<crocoddyl::ActionDataAbstract>& action_data =

416

1/2

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

3

action_model.createData();

417

418

1/2

✓ Branch 3 taken 3 times.

✗ Branch 4 not taken.

3

cost_data->shareMemory(action_data.get());

419

2/4

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

3

cost_data->Lx = Eigen::VectorXd::Random(ndx);

420

2/4

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

3

cost_data->Lu = Eigen::VectorXd::Random(nu);

421

2/4

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

3

cost_data->Lxx = Eigen::MatrixXd::Random(ndx, ndx);

422

2/4

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

3

cost_data->Luu = Eigen::MatrixXd::Random(nu, nu);

423

2/4

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

3

cost_data->Lxu = Eigen::MatrixXd::Random(ndx, nu);

424

425

// check that the data has been shared

426

7/14

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

✓ Branch 11 taken 3 times.

✗ Branch 12 not taken.

✓ Branch 16 taken 3 times.

✗ Branch 17 not taken.

✓ Branch 19 taken 3 times.

✗ Branch 20 not taken.

✓ Branch 22 taken 3 times.

✗ Branch 23 not taken.

✗ Branch 27 not taken.

✓ Branch 28 taken 3 times.

3

BOOST_CHECK(action_data->Lx.isApprox(cost_data->Lx, 1e-9));

427

7/14

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

✓ Branch 11 taken 3 times.

✗ Branch 12 not taken.

✓ Branch 16 taken 3 times.

✗ Branch 17 not taken.

✓ Branch 19 taken 3 times.

✗ Branch 20 not taken.

✓ Branch 22 taken 3 times.

✗ Branch 23 not taken.

✗ Branch 27 not taken.

✓ Branch 28 taken 3 times.

3

BOOST_CHECK(action_data->Lu.isApprox(cost_data->Lu, 1e-9));

428

7/14

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

✓ Branch 11 taken 3 times.

✗ Branch 12 not taken.

✓ Branch 16 taken 3 times.

✗ Branch 17 not taken.

✓ Branch 19 taken 3 times.

✗ Branch 20 not taken.

✓ Branch 22 taken 3 times.

✗ Branch 23 not taken.

✗ Branch 27 not taken.

✓ Branch 28 taken 3 times.

3

BOOST_CHECK(action_data->Lxx.isApprox(cost_data->Lxx, 1e-9));

429

7/14

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

✓ Branch 11 taken 3 times.

✗ Branch 12 not taken.

✓ Branch 16 taken 3 times.

✗ Branch 17 not taken.

✓ Branch 19 taken 3 times.

✗ Branch 20 not taken.

✓ Branch 22 taken 3 times.

✗ Branch 23 not taken.

✗ Branch 27 not taken.

✓ Branch 28 taken 3 times.

3

BOOST_CHECK(action_data->Luu.isApprox(cost_data->Luu, 1e-9));

430

7/14

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

✓ Branch 11 taken 3 times.

✗ Branch 12 not taken.

✓ Branch 16 taken 3 times.

✗ Branch 17 not taken.

✓ Branch 19 taken 3 times.

✗ Branch 20 not taken.

✓ Branch 22 taken 3 times.

✗ Branch 23 not taken.

✗ Branch 27 not taken.

✓ Branch 28 taken 3 times.

3

BOOST_CHECK(action_data->Lxu.isApprox(cost_data->Lxu, 1e-9));

431

3

}

432

433

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

434

435

3

void register_unit_tests(StateModelTypes::Type state_type) {

436

2/4

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 4 taken 3 times.

✗ Branch 5 not taken.

3

boost::test_tools::output_test_stream test_name;

437

test_name << "test_CostModelSum"

438

3/6

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 4 taken 3 times.

✗ Branch 5 not taken.

✓ Branch 7 taken 3 times.

✗ Branch 8 not taken.

3

<< "_" << state_type;

439

4/8

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 4 taken 3 times.

✗ Branch 5 not taken.

✓ Branch 7 taken 3 times.

✗ Branch 8 not taken.

✓ Branch 10 taken 3 times.

✗ Branch 11 not taken.

3

std::cout << "Running " << test_name.str() << std::endl;

440

4/8

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

✓ Branch 8 taken 3 times.

✗ Branch 9 not taken.

✓ Branch 11 taken 3 times.

✗ Branch 12 not taken.

3

test_suite* ts = BOOST_TEST_SUITE(test_name.str());

441

5/10

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

✓ Branch 8 taken 3 times.

✗ Branch 9 not taken.

✓ Branch 11 taken 3 times.

✗ Branch 12 not taken.

✓ Branch 14 taken 3 times.

✗ Branch 15 not taken.

3

ts->add(BOOST_TEST_CASE(boost::bind(&test_constructor, state_type)));

442

5/10

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

✓ Branch 8 taken 3 times.

✗ Branch 9 not taken.

✓ Branch 11 taken 3 times.

✗ Branch 12 not taken.

✓ Branch 14 taken 3 times.

✗ Branch 15 not taken.

3

ts->add(BOOST_TEST_CASE(boost::bind(&test_addCost, state_type)));

443

6

ts->add(

444

5/10

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

✓ Branch 8 taken 3 times.

✗ Branch 9 not taken.

✓ Branch 11 taken 3 times.

✗ Branch 12 not taken.

✓ Branch 14 taken 3 times.

✗ Branch 15 not taken.

3

BOOST_TEST_CASE(boost::bind(&test_addCost_error_message, state_type)));

445

5/10

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

✓ Branch 8 taken 3 times.

✗ Branch 9 not taken.

✓ Branch 11 taken 3 times.

✗ Branch 12 not taken.

✓ Branch 14 taken 3 times.

✗ Branch 15 not taken.

3

ts->add(BOOST_TEST_CASE(boost::bind(&test_removeCost, state_type)));

446

6

ts->add(

447

5/10

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

✓ Branch 8 taken 3 times.

✗ Branch 9 not taken.

✓ Branch 11 taken 3 times.

✗ Branch 12 not taken.

✓ Branch 14 taken 3 times.

✗ Branch 15 not taken.

3

BOOST_TEST_CASE(boost::bind(&test_removeCost_error_message, state_type)));

448

5/10

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

✓ Branch 8 taken 3 times.

✗ Branch 9 not taken.

✓ Branch 11 taken 3 times.

✗ Branch 12 not taken.

✓ Branch 14 taken 3 times.

✗ Branch 15 not taken.

3

ts->add(BOOST_TEST_CASE(boost::bind(&test_calc, state_type)));

449

5/10

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

✓ Branch 8 taken 3 times.

✗ Branch 9 not taken.

✓ Branch 11 taken 3 times.

✗ Branch 12 not taken.

✓ Branch 14 taken 3 times.

✗ Branch 15 not taken.

3

ts->add(BOOST_TEST_CASE(boost::bind(&test_calcDiff, state_type)));

450

5/10

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

✓ Branch 8 taken 3 times.

✗ Branch 9 not taken.

✓ Branch 11 taken 3 times.

✗ Branch 12 not taken.

✓ Branch 14 taken 3 times.

✗ Branch 15 not taken.

3

ts->add(BOOST_TEST_CASE(boost::bind(&test_get_costs, state_type)));

451

5/10

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

✓ Branch 8 taken 3 times.

✗ Branch 9 not taken.

✓ Branch 11 taken 3 times.

✗ Branch 12 not taken.

✓ Branch 14 taken 3 times.

✗ Branch 15 not taken.

3

ts->add(BOOST_TEST_CASE(boost::bind(&test_get_nr, state_type)));

452

5/10

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 3 times.

✗ Branch 6 not taken.

✓ Branch 8 taken 3 times.

✗ Branch 9 not taken.

✓ Branch 11 taken 3 times.

✗ Branch 12 not taken.

✓ Branch 14 taken 3 times.

✗ Branch 15 not taken.

3

ts->add(BOOST_TEST_CASE(boost::bind(&test_shareMemory, state_type)));

453

3/6

✓ Branch 1 taken 3 times.

✗ Branch 2 not taken.

✓ Branch 3 taken 3 times.

✗ Branch 4 not taken.

✓ Branch 6 taken 3 times.

✗ Branch 7 not taken.

3

framework::master_test_suite().add(ts);

454

3

}

455

456

1

bool init_function() {

457

1

register_unit_tests(StateModelTypes::StateMultibody_TalosArm);

458

1

register_unit_tests(StateModelTypes::StateMultibody_HyQ);

459

1

register_unit_tests(StateModelTypes::StateMultibody_Talos);

460

1

return true;

461

}

462

463

1

int main(int argc, char** argv) {

464

1

return ::boost::unit_test::unit_test_main(&init_function, argc, argv);

465

}

466