GCC Code Coverage Report

Directory: ./
File: unittest/test_cost_sum.cpp
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 300 300 100.0%
Branches: 696 1368 50.9%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2025, 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/core/utils/conversions.hpp"
14 #include "crocoddyl/multibody/data/multibody.hpp"
15 #include "factory/cost.hpp"
16 #include "unittest_common.hpp"
17
18 using namespace boost::unit_test;
19 using namespace crocoddyl::unittest;
20
21 //----------------------------------------------------------------------------//
22
23 3 void test_constructor(StateModelTypes::Type state_type) {
24 // Setup the test
25
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 StateModelFactory state_factory;
26
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));
27
28 // Run the print function
29
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 std::ostringstream tmp;
30
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 tmp << model;
31
32 // Test the initial size of the map
33
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 14 taken 3 times.
✗ Branch 15 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(model.get_costs().size() == 0);
34
35 // Checking that casted computation is the same
36 #ifdef NDEBUG // Run only in release mode
37 crocoddyl::CostModelSumTpl<float> casted_model = model.cast<float>();
38 BOOST_CHECK(casted_model.get_costs().size() == 0);
39 #endif
40 3 }
41
42 3 void test_addCost(StateModelTypes::Type state_type) {
43 // Setup the test
44
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 StateModelFactory state_factory;
45
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));
46
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 crocoddyl::CostModelSumTpl<float> casted_model = model.cast<float>();
47
48 // add an active cost
49 std::shared_ptr<crocoddyl::CostModelAbstract> rand_cost_1 =
50
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 create_random_cost(state_type);
51
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.);
52
9/18
✓ 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 18 taken 3 times.
✗ Branch 19 not taken.
✓ Branch 22 taken 3 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 3 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 3 times.
✗ Branch 29 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 3 times.
 3 BOOST_CHECK(model.get_nr() == rand_cost_1->get_activation()->get_nr());
53
9/18
✓ 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 18 taken 3 times.
✗ Branch 19 not taken.
✓ Branch 22 taken 3 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 3 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 3 times.
✗ Branch 29 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 3 times.
 3 BOOST_CHECK(model.get_nr_total() == rand_cost_1->get_activation()->get_nr());
54
55 // add an inactive cost
56 std::shared_ptr<crocoddyl::CostModelAbstract> rand_cost_2 =
57
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 create_random_cost(state_type);
58
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);
59
9/18
✓ 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 18 taken 3 times.
✗ Branch 19 not taken.
✓ Branch 22 taken 3 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 3 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 3 times.
✗ Branch 29 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 3 times.
 3 BOOST_CHECK(model.get_nr() == rand_cost_1->get_activation()->get_nr());
60
11/22
✓ 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 18 taken 3 times.
✗ Branch 19 not taken.
✓ Branch 22 taken 3 times.
✗ Branch 23 not taken.
✓ Branch 26 taken 3 times.
✗ Branch 27 not taken.
✓ Branch 30 taken 3 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 3 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 3 times.
✗ Branch 37 not taken.
✗ Branch 41 not taken.
✓ Branch 42 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 2 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_2", true);
66
11/22
✓ 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 18 taken 3 times.
✗ Branch 19 not taken.
✓ Branch 22 taken 3 times.
✗ Branch 23 not taken.
✓ Branch 26 taken 3 times.
✗ Branch 27 not taken.
✓ Branch 30 taken 3 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 3 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 3 times.
✗ Branch 37 not taken.
✗ Branch 41 not taken.
✓ Branch 42 taken 3 times.
 3 BOOST_CHECK(model.get_nr() == rand_cost_1->get_activation()->get_nr() +
67 rand_cost_2->get_activation()->get_nr());
68
11/22
✓ 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 18 taken 3 times.
✗ Branch 19 not taken.
✓ Branch 22 taken 3 times.
✗ Branch 23 not taken.
✓ Branch 26 taken 3 times.
✗ Branch 27 not taken.
✓ Branch 30 taken 3 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 3 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 3 times.
✗ Branch 37 not taken.
✗ Branch 41 not taken.
✓ Branch 42 taken 3 times.
 3 BOOST_CHECK(model.get_nr_total() ==
69 rand_cost_1->get_activation()->get_nr() +
70 rand_cost_2->get_activation()->get_nr());
71
72 // change the random cost 1 status
73
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);
74
9/18
✓ 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 18 taken 3 times.
✗ Branch 19 not taken.
✓ Branch 22 taken 3 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 3 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 3 times.
✗ Branch 29 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 3 times.
 3 BOOST_CHECK(model.get_nr() == rand_cost_2->get_activation()->get_nr());
75
11/22
✓ 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 18 taken 3 times.
✗ Branch 19 not taken.
✓ Branch 22 taken 3 times.
✗ Branch 23 not taken.
✓ Branch 26 taken 3 times.
✗ Branch 27 not taken.
✓ Branch 30 taken 3 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 3 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 3 times.
✗ Branch 37 not taken.
✗ Branch 41 not taken.
✓ Branch 42 taken 3 times.
 3 BOOST_CHECK(model.get_nr_total() ==
76 rand_cost_1->get_activation()->get_nr() +
77 rand_cost_2->get_activation()->get_nr());
78
79 // Checking that casted computation is the same
80 #ifdef NDEBUG // Run only in release mode
81 std::shared_ptr<crocoddyl::CostModelAbstractTpl<float>> casted_rand_cost_1 =
82 rand_cost_1->cast<float>();
83 casted_model.addCost("random_cost_1", casted_rand_cost_1, 1.f);
84 BOOST_CHECK(casted_model.get_nr() ==
85 casted_rand_cost_1->get_activation()->get_nr());
86 BOOST_CHECK(casted_model.get_nr_total() ==
87 casted_rand_cost_1->get_activation()->get_nr());
88 std::shared_ptr<crocoddyl::CostModelAbstractTpl<float>> casted_rand_cost_2 =
89 rand_cost_2->cast<float>();
90 casted_model.addCost("random_cost_2", casted_rand_cost_2, 1.f, false);
91 BOOST_CHECK(casted_model.get_nr() ==
92 casted_rand_cost_1->get_activation()->get_nr());
93 BOOST_CHECK(casted_model.get_nr_total() ==
94 casted_rand_cost_1->get_activation()->get_nr() +
95 casted_rand_cost_2->get_activation()->get_nr());
96 casted_model.changeCostStatus("random_cost_2", true);
97 BOOST_CHECK(casted_model.get_nr() ==
98 casted_rand_cost_1->get_activation()->get_nr() +
99 casted_rand_cost_2->get_activation()->get_nr());
100 BOOST_CHECK(casted_model.get_nr_total() ==
101 casted_rand_cost_1->get_activation()->get_nr() +
102 casted_rand_cost_2->get_activation()->get_nr());
103 casted_model.changeCostStatus("random_cost_1", false);
104 BOOST_CHECK(casted_model.get_nr() ==
105 casted_rand_cost_2->get_activation()->get_nr());
106 BOOST_CHECK(casted_model.get_nr_total() ==
107 casted_rand_cost_1->get_activation()->get_nr() +
108 casted_rand_cost_2->get_activation()->get_nr());
109 #endif
110 3 }
111
112 3 void test_addCost_error_message(StateModelTypes::Type state_type) {
113 // Setup the test
114
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 StateModelFactory state_factory;
115
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));
116
117 // create an cost object
118 std::shared_ptr<crocoddyl::CostModelAbstract> rand_cost =
119
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 create_random_cost(state_type);
120
121 // add twice the same cost object to the container
122
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.);
123
124 // test error message when we add a duplicate cost
125
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 CaptureIOStream capture_ios;
126
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 capture_ios.beginCapture();
127
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.);
128
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 capture_ios.endCapture();
129
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 std::stringstream expected_buffer;
130 expected_buffer << "Warning: we couldn't add the random_cost cost item, it "
131
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 "already existed."
132
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 << std::endl;
133
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());
134
135 // test error message when we change the cost status of an inexistent cost
136
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 capture_ios.beginCapture();
137
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);
138
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 capture_ios.endCapture();
139
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 expected_buffer.clear();
140 expected_buffer << "Warning: we couldn't change the status of the "
141
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 "no_exist_cost cost item, it doesn't exist."
142
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 << std::endl;
143
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());
144 3 }
145
146 3 void test_removeCost(StateModelTypes::Type state_type) {
147 // Setup the test
148
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 StateModelFactory state_factory;
149
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));
150
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 crocoddyl::CostModelSumTpl<float> casted_model = model.cast<float>();
151
152 // add an active cost
153 std::shared_ptr<crocoddyl::CostModelAbstract> rand_cost =
154
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 create_random_cost(state_type);
155
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.);
156
9/18
✓ 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 18 taken 3 times.
✗ Branch 19 not taken.
✓ Branch 22 taken 3 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 3 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 3 times.
✗ Branch 29 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 3 times.
 3 BOOST_CHECK(model.get_nr() == rand_cost->get_activation()->get_nr());
157
158 // remove the cost
159
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");
160
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 14 taken 3 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 3 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 3 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 3 times.
 3 BOOST_CHECK(model.get_nr() == 0);
161
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 14 taken 3 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 3 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 3 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 3 times.
 3 BOOST_CHECK(model.get_nr_total() == 0);
162
163 // Checking that casted computation is the same
164 #ifdef NDEBUG // Run only in release mode
165 std::shared_ptr<crocoddyl::CostModelAbstractTpl<float>> casted_rand_cost =
166 rand_cost->cast<float>();
167 casted_model.addCost("random_cost", casted_rand_cost, 1.f);
168 BOOST_CHECK(casted_model.get_nr() ==
169 casted_rand_cost->get_activation()->get_nr());
170 casted_model.removeCost("random_cost");
171 BOOST_CHECK(casted_model.get_nr() == 0);
172 BOOST_CHECK(casted_model.get_nr_total() == 0);
173 #endif
174 3 }
175
176 3 void test_removeCost_error_message(StateModelTypes::Type state_type) {
177 // Setup the test
178
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 StateModelFactory state_factory;
179
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));
180
181 // remove a none existing cost form the container, we expect a cout message
182 // here
183
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 CaptureIOStream capture_ios;
184
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 capture_ios.beginCapture();
185
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");
186
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 capture_ios.endCapture();
187
188 // Test that the error message is sent.
189
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 std::stringstream expected_buffer;
190 expected_buffer << "Warning: we couldn't remove the random_cost cost item, "
191
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 "it doesn't exist."
192
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 << std::endl;
193
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());
194 3 }
195
196 3 void test_calc(StateModelTypes::Type state_type) {
197 // setup the test
198
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 StateModelFactory state_factory;
199
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));
200 // create the corresponding data object
201 const std::shared_ptr<crocoddyl::StateMultibody>& state =
202
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 std::static_pointer_cast<crocoddyl::StateMultibody>(model.get_state());
203
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 pinocchio::Model& pinocchio_model = *state->get_pinocchio().get();
204
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 pinocchio::Data pinocchio_data(pinocchio_model);
205
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 crocoddyl::DataCollectorMultibody shared_data(&pinocchio_data);
206
207 // create and add some cost objects
208 3 std::vector<std::shared_ptr<crocoddyl::CostModelAbstract>> models;
209 3 std::vector<std::shared_ptr<crocoddyl::CostDataAbstract>> datas;
210
2/2
✓ Branch 0 taken 15 times.
✓ Branch 1 taken 3 times.
 18 for (std::size_t i = 0; i < 5; ++i) {
211
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 15 std::ostringstream os;
212
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;
213 const std::shared_ptr<crocoddyl::CostModelAbstract>& m =
214
1/2
✓ Branch 2 taken 15 times.
✗ Branch 3 not taken.
 15 create_random_cost(state_type);
215
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.);
216
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 15 models.push_back(m);
217
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));
218 15 }
219
220 // create the data of the cost sum
221 const std::shared_ptr<crocoddyl::CostDataSum>& data =
222
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 model.createData(&shared_data);
223
224 // compute the cost sum data for the case when all costs are defined as active
225
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 const Eigen::VectorXd x1 = state->rand();
226
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 const Eigen::VectorXd u1 = Eigen::VectorXd::Random(model.get_nu());
227
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,
228 x1);
229
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);
230
231 // check that the cost has been filled
232
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.);
233
234 // check the cost against single cost computations
235 3 double cost = 0;
236
2/2
✓ Branch 0 taken 15 times.
✓ Branch 1 taken 3 times.
 18 for (std::size_t i = 0; i < 5; ++i) {
237
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);
238 15 cost += datas[i]->cost;
239 }
240
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);
241
242 // compute the cost sum data for the case when the first three costs are
243 // defined as active
244
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);
245
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);
246
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 const Eigen::VectorXd x2 = state->rand();
247
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 const Eigen::VectorXd u2 = Eigen::VectorXd::Random(model.get_nu());
248
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,
249 x1);
250
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);
251 3 cost = 0;
252
2/2
✓ Branch 0 taken 9 times.
✓ Branch 1 taken 3 times.
 12 for (std::size_t i = 0; i < 3;
253 ++i) { // we need to update data because this costs are active
254
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);
255 9 cost += datas[i]->cost;
256 }
257
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);
258
259 // Checking that casted computation is the same
260 #ifdef NDEBUG // Run only in release mode
261 model.changeCostStatus("random_cost_3", true);
262 model.changeCostStatus("random_cost_4", true);
263 crocoddyl::CostModelSumTpl<float> casted_model = model.cast<float>();
264 const std::shared_ptr<crocoddyl::StateMultibodyTpl<float>>& casted_state =
265 std::static_pointer_cast<crocoddyl::StateMultibodyTpl<float>>(
266 casted_model.get_state());
267 pinocchio::ModelTpl<float>& casted_pinocchio_model =
268 *casted_state->get_pinocchio().get();
269 pinocchio::DataTpl<float> casted_pinocchio_data(casted_pinocchio_model);
270 crocoddyl::DataCollectorMultibodyTpl<float> casted_shared_data(
271 &casted_pinocchio_data);
272 std::vector<std::shared_ptr<crocoddyl::CostModelAbstractTpl<float>>>
273 casted_models;
274 std::vector<std::shared_ptr<crocoddyl::CostDataAbstractTpl<float>>>
275 casted_datas;
276 for (std::size_t i = 0; i < 5; ++i) {
277 casted_models.push_back(models[i]->cast<float>());
278 casted_datas.push_back(casted_models[i]->createData(&casted_shared_data));
279 }
280 const std::shared_ptr<crocoddyl::CostDataSumTpl<float>>& casted_data =
281 casted_model.createData(&casted_shared_data);
282 const Eigen::VectorXf x1_f = x1.cast<float>();
283 const Eigen::VectorXf u1_f = u1.cast<float>();
284 crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data,
285 x1);
286 crocoddyl::unittest::updateAllPinocchio(&casted_pinocchio_model,
287 &casted_pinocchio_data, x1_f);
288 model.calc(data, x1, u1);
289 casted_model.calc(casted_data, x1_f, u1_f);
290 BOOST_CHECK(casted_data->cost > 0.f);
291 float tol_f = std::sqrt(2.0f * std::numeric_limits<float>::epsilon());
292 BOOST_CHECK(std::abs(float(data->cost) - casted_data->cost) <= tol_f);
293 float cost_f = 0.f;
294 for (std::size_t i = 0; i < 5; ++i) {
295 casted_models[i]->calc(casted_datas[i], x1_f, u1_f);
296 cost_f += casted_datas[i]->cost;
297 }
298 BOOST_CHECK(casted_data->cost == cost_f);
299 #endif
300 3 }
301
302 3 void test_calcDiff(StateModelTypes::Type state_type) {
303 // setup the test
304
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 StateModelFactory state_factory;
305
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));
306 // create the corresponding data object
307 const std::shared_ptr<crocoddyl::StateMultibody>& state =
308
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 std::static_pointer_cast<crocoddyl::StateMultibody>(model.get_state());
309
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 pinocchio::Model& pinocchio_model = *state->get_pinocchio().get();
310
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 pinocchio::Data pinocchio_data(pinocchio_model);
311
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 crocoddyl::DataCollectorMultibody shared_data(&pinocchio_data);
312
313 // create and add some cost objects
314 3 std::vector<std::shared_ptr<crocoddyl::CostModelAbstract>> models;
315 3 std::vector<std::shared_ptr<crocoddyl::CostDataAbstract>> datas;
316
2/2
✓ Branch 0 taken 15 times.
✓ Branch 1 taken 3 times.
 18 for (std::size_t i = 0; i < 5; ++i) {
317
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 15 std::ostringstream os;
318
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;
319 const std::shared_ptr<crocoddyl::CostModelAbstract>& m =
320
1/2
✓ Branch 2 taken 15 times.
✗ Branch 3 not taken.
 15 create_random_cost(state_type);
321
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.);
322
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 15 models.push_back(m);
323
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));
324 15 }
325
326 // create the data of the cost sum
327 const std::shared_ptr<crocoddyl::CostDataSum>& data =
328
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 model.createData(&shared_data);
329
330 // compute the cost sum data for the case when all costs are defined as active
331
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 Eigen::VectorXd x1 = state->rand();
332
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 const Eigen::VectorXd u1 = Eigen::VectorXd::Random(model.get_nu());
333
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,
334 x1);
335
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);
336
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);
337
338 // check that the cost has been filled
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 > 0.);
340
341 // check the cost against single cost computations
342 3 double cost = 0;
343
3/6
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
 3 Eigen::VectorXd Lx = Eigen::VectorXd::Zero(state->get_ndx());
344
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 Eigen::VectorXd Lu = Eigen::VectorXd::Zero(model.get_nu());
345 Eigen::MatrixXd Lxx =
346
4/8
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 3 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 3 times.
✗ Branch 13 not taken.
 3 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx());
347
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 Eigen::MatrixXd Lxu = Eigen::MatrixXd::Zero(state->get_ndx(), model.get_nu());
348
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 Eigen::MatrixXd Luu = Eigen::MatrixXd::Zero(model.get_nu(), model.get_nu());
349
2/2
✓ Branch 0 taken 15 times.
✓ Branch 1 taken 3 times.
 18 for (std::size_t i = 0; i < 5; ++i) {
350
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);
351
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);
352 15 cost += datas[i]->cost;
353
1/2
✓ Branch 3 taken 15 times.
✗ Branch 4 not taken.
 15 Lx += datas[i]->Lx;
354
1/2
✓ Branch 3 taken 15 times.
✗ Branch 4 not taken.
 15 Lu += datas[i]->Lu;
355
1/2
✓ Branch 3 taken 15 times.
✗ Branch 4 not taken.
 15 Lxx += datas[i]->Lxx;
356
1/2
✓ Branch 3 taken 15 times.
✗ Branch 4 not taken.
 15 Lxu += datas[i]->Lxu;
357
1/2
✓ Branch 3 taken 15 times.
✗ Branch 4 not taken.
 15 Luu += datas[i]->Luu;
358 }
359
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);
360
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);
361
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);
362
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);
363
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);
364
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);
365
366
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 x1 = state->rand();
367
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,
368 x1);
369
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);
370
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);
371 3 cost = 0.;
372
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 Lx.setZero();
373
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 Lxx.setZero();
374
2/2
✓ Branch 0 taken 15 times.
✓ Branch 1 taken 3 times.
 18 for (std::size_t i = 0; i < 5; ++i) {
375
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);
376
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);
377 15 cost += datas[i]->cost;
378
1/2
✓ Branch 3 taken 15 times.
✗ Branch 4 not taken.
 15 Lx += datas[i]->Lx;
379
1/2
✓ Branch 3 taken 15 times.
✗ Branch 4 not taken.
 15 Lxx += datas[i]->Lxx;
380 }
381
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);
382
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);
383
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);
384
385 // compute the cost sum data for the case when the first three costs are
386 // defined as active
387
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);
388
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);
389
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 Eigen::VectorXd x2 = state->rand();
390
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 const Eigen::VectorXd u2 = Eigen::VectorXd::Random(model.get_nu());
391
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,
392 x2);
393
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);
394
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);
395 3 cost = 0;
396
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 Lx.setZero();
397
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 Lu.setZero();
398
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 Lxx.setZero();
399
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 Lxu.setZero();
400
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 Luu.setZero();
401
2/2
✓ Branch 0 taken 9 times.
✓ Branch 1 taken 3 times.
 12 for (std::size_t i = 0; i < 3;
402 ++i) { // we need to update data because this costs are active
403
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);
404
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);
405 9 cost += datas[i]->cost;
406
1/2
✓ Branch 3 taken 9 times.
✗ Branch 4 not taken.
 9 Lx += datas[i]->Lx;
407
1/2
✓ Branch 3 taken 9 times.
✗ Branch 4 not taken.
 9 Lu += datas[i]->Lu;
408
1/2
✓ Branch 3 taken 9 times.
✗ Branch 4 not taken.
 9 Lxx += datas[i]->Lxx;
409
1/2
✓ Branch 3 taken 9 times.
✗ Branch 4 not taken.
 9 Lxu += datas[i]->Lxu;
410
1/2
✓ Branch 3 taken 9 times.
✗ Branch 4 not taken.
 9 Luu += datas[i]->Luu;
411 }
412
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);
413
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);
414
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);
415
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);
416
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);
417
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);
418
419
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 x2 = state->rand();
420
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,
421 x2);
422
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);
423
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);
424 3 cost = 0.;
425
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 Lx.setZero();
426
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 Lxx.setZero();
427
2/2
✓ Branch 0 taken 9 times.
✓ Branch 1 taken 3 times.
 12 for (std::size_t i = 0; i < 3; ++i) {
428
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);
429
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);
430 9 cost += datas[i]->cost;
431
1/2
✓ Branch 3 taken 9 times.
✗ Branch 4 not taken.
 9 Lx += datas[i]->Lx;
432
1/2
✓ Branch 3 taken 9 times.
✗ Branch 4 not taken.
 9 Lxx += datas[i]->Lxx;
433 }
434
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);
435
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);
436
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);
437
438 // Checking that casted computation is the same
439 #ifdef NDEBUG // Run only in release mode
440 model.changeCostStatus("random_cost_3", true);
441 model.changeCostStatus("random_cost_4", true);
442 crocoddyl::CostModelSumTpl<float> casted_model = model.cast<float>();
443 const std::shared_ptr<crocoddyl::StateMultibodyTpl<float>>& casted_state =
444 std::static_pointer_cast<crocoddyl::StateMultibodyTpl<float>>(
445 casted_model.get_state());
446 pinocchio::ModelTpl<float>& casted_pinocchio_model =
447 *casted_state->get_pinocchio().get();
448 pinocchio::DataTpl<float> casted_pinocchio_data(casted_pinocchio_model);
449 crocoddyl::DataCollectorMultibodyTpl<float> casted_shared_data(
450 &casted_pinocchio_data);
451 std::vector<std::shared_ptr<crocoddyl::CostModelAbstractTpl<float>>>
452 casted_models;
453 std::vector<std::shared_ptr<crocoddyl::CostDataAbstractTpl<float>>>
454 casted_datas;
455 for (std::size_t i = 0; i < 5; ++i) {
456 casted_models.push_back(models[i]->cast<float>());
457 casted_datas.push_back(casted_models[i]->createData(&casted_shared_data));
458 }
459 const std::shared_ptr<crocoddyl::CostDataSumTpl<float>>& casted_data =
460 casted_model.createData(&casted_shared_data);
461 const Eigen::VectorXf x1_f = x1.cast<float>();
462 const Eigen::VectorXf u1_f = u1.cast<float>();
463 crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data,
464 x1);
465 crocoddyl::unittest::updateAllPinocchio(&casted_pinocchio_model,
466 &casted_pinocchio_data, x1_f);
467 model.calc(data, x1, u1);
468 model.calcDiff(data, x1, u1);
469 casted_model.calc(casted_data, x1_f, u1_f);
470 casted_model.calcDiff(casted_data, x1_f, u1_f);
471 Lx.setZero();
472 Lu.setZero();
473 Lxx.setZero();
474 Lxu.setZero();
475 Luu.setZero();
476 float cost_f = 0.f;
477 Eigen::VectorXf Lx_f = Lx.cast<float>();
478 Eigen::VectorXf Lu_f = Lu.cast<float>();
479 Eigen::MatrixXf Lxx_f = Lxx.cast<float>();
480 Eigen::MatrixXf Lxu_f = Lxu.cast<float>();
481 Eigen::MatrixXf Luu_f = Luu.cast<float>();
482 for (std::size_t i = 0; i < 5;
483 ++i) { // we need to update data because this costs are active
484 casted_models[i]->calc(casted_datas[i], x1_f, u1_f);
485 casted_models[i]->calcDiff(casted_datas[i], x1_f, u1_f);
486 cost_f += casted_datas[i]->cost;
487 Lx_f += casted_datas[i]->Lx;
488 Lu_f += casted_datas[i]->Lu;
489 Lxx_f += casted_datas[i]->Lxx;
490 Lxu_f += casted_datas[i]->Lxu;
491 Luu_f += casted_datas[i]->Luu;
492 }
493 BOOST_CHECK(casted_data->cost == cost_f);
494 BOOST_CHECK(casted_data->Lx == Lx_f);
495 BOOST_CHECK(casted_data->Lu == Lu_f);
496 BOOST_CHECK(casted_data->Lxx == Lxx_f);
497 BOOST_CHECK(casted_data->Lxu == Lxu_f);
498 BOOST_CHECK(casted_data->Luu == Luu_f);
499 #endif
500 3 }
501
502 3 void test_get_costs(StateModelTypes::Type state_type) {
503 // setup the test
504
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 StateModelFactory state_factory;
505
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));
506 // create the corresponding data object
507 const std::shared_ptr<crocoddyl::StateMultibody>& state =
508
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 std::static_pointer_cast<crocoddyl::StateMultibody>(model.get_state());
509
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
 3 pinocchio::Data pinocchio_data(*state->get_pinocchio().get());
510
511 // create and add some contact objects
512
2/2
✓ Branch 0 taken 15 times.
✓ Branch 1 taken 3 times.
 18 for (unsigned i = 0; i < 5; ++i) {
513
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 15 std::ostringstream os;
514
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;
515
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.);
516 15 }
517
518 // get the contacts
519
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 const crocoddyl::CostModelSum::CostModelContainer& costs = model.get_costs();
520
521 // test
522 3 crocoddyl::CostModelSum::CostModelContainer::const_iterator it_m, end_m;
523 unsigned i;
524
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;
525 15 ++it_m, ++i) {
526
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 15 std::ostringstream os;
527
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;
528
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());
529 15 }
530 3 }
531
532 3 void test_get_nr(StateModelTypes::Type state_type) {
533 // Setup the test
534
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 StateModelFactory state_factory;
535
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));
536
537 // create the corresponding data object
538 const std::shared_ptr<crocoddyl::StateMultibody>& state =
539
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 std::static_pointer_cast<crocoddyl::StateMultibody>(model.get_state());
540
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
 3 pinocchio::Data pinocchio_data(*state->get_pinocchio().get());
541
542 // create and add some contact objects
543
2/2
✓ Branch 0 taken 15 times.
✓ Branch 1 taken 3 times.
 18 for (unsigned i = 0; i < 5; ++i) {
544
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 15 std::ostringstream os;
545
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;
546
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.);
547 15 }
548
549 // compute ni
550 3 std::size_t nr = 0;
551 3 crocoddyl::CostModelSum::CostModelContainer::const_iterator it_m, end_m;
552
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 3 for (it_m = model.get_costs().begin(), end_m = model.get_costs().end();
553
2/2
✓ Branch 1 taken 15 times.
✓ Branch 2 taken 3 times.
 18 it_m != end_m; ++it_m) {
554
2/4
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 15 times.
✗ Branch 9 not taken.
 15 nr += it_m->second->cost->get_activation()->get_nr();
555 }
556
557
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 14 taken 3 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 3 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 3 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 3 times.
 3 BOOST_CHECK(nr == model.get_nr());
558 3 }
559
560 3 void test_shareMemory(StateModelTypes::Type state_type) {
561 // setup the test
562
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 StateModelFactory state_factory;
563 const std::shared_ptr<crocoddyl::StateAbstract> state =
564
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 state_factory.create(state_type);
565
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 crocoddyl::CostModelSum cost_model(state);
566
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 crocoddyl::DataCollectorAbstract shared_data;
567 const std::shared_ptr<crocoddyl::CostDataSum>& cost_data =
568
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 cost_model.createData(&shared_data);
569
570
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 const std::size_t ndx = state->get_ndx();
571
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 const std::size_t nu = cost_model.get_nu();
572
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 crocoddyl::ActionModelLQR action_model(ndx, nu);
573 const std::shared_ptr<crocoddyl::ActionDataAbstract>& action_data =
574
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 action_model.createData();
575
576
1/2
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
 3 cost_data->shareMemory(action_data.get());
577
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);
578
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);
579
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);
580
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);
581
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);
582
583 // check that the data has been shared
584
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));
585
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));
586
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));
587
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));
588
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));
589 3 }
590
591 //----------------------------------------------------------------------------//
592
593 3 void register_unit_tests(StateModelTypes::Type state_type) {
594
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;
595 test_name << "test_CostModelSum"
596
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;
597
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;
598
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());
599
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)));
600
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)));
601 6 ts->add(
602
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)));
603
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)));
604 6 ts->add(
605
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)));
606
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)));
607
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)));
608
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)));
609
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)));
610
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)));
611
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);
612 3 }
613
614 1 bool init_function() {
615 1 register_unit_tests(StateModelTypes::StateMultibody_TalosArm);
616 1 register_unit_tests(StateModelTypes::StateMultibody_HyQ);
617 1 register_unit_tests(StateModelTypes::StateMultibody_Talos);
618 1 return true;
619 }
620
621 1 int main(int argc, char** argv) {
622 1 return ::boost::unit_test::unit_test_main(&init_function, argc, argv);
623 }
624