GCC Code Coverage Report

Directory: ./
File: test/test_static_equilibrium.cpp
Date: 2025-03-17 04:04:52
Exec Total Coverage
Lines: 193 292 66.1%
Branches: 328 1092 30.0%
Line Branch Exec Source
1 /*
2 * Copyright 2015, LAAS-CNRS
3 * Author: Andrea Del Prete
4 */
5
6 #include <hpp/centroidal-dynamics/centroidal_dynamics.hh>
7 #include <hpp/centroidal-dynamics/logger.hh>
8 #include <hpp/centroidal-dynamics/stop-watch.hh>
9 #include <iostream>
10 #include <vector>
11
12 using namespace centroidal_dynamics;
13 using namespace Eigen;
14 using namespace std;
15
16 #define PERF_PP "Polytope Projection"
17 #define PERF_LP_PREPARATION "Computation of GIWC generators"
18 #define PERF_LP_COIN "Compute Equilibrium Robustness with LP coin"
19 #define PERF_LP_OASES "Compute Equilibrium Robustness with LP oases"
20 #define PERF_LP2_COIN "Compute Equilibrium Robustness with LP2 coin"
21 #define PERF_LP2_OASES "Compute Equilibrium Robustness with LP2 oases"
22 #define PERF_DLP_COIN "Compute Equilibrium Robustness with DLP coin"
23 #define PERF_DLP_OASES "Compute Equilibrium Robustness with DLP oases"
24
25 #define EPS 1e-3 // required precision
26
27 /** Check the coherence between the method
28 * Equilibrium::computeEquilibriumRobustness and the method
29 * Equilibrium::checkRobustEquilibrium.
30 * @param solver_1 Solver used to test computeEquilibriumRobustness.
31 * @param solver_2 Solver used to test checkRobustEquilibrium.
32 * @param comPositions List of 2d com positions on which to perform the tests.
33 * @param PERF_STRING_1 String to use for logging the computation times of
34 * solver_1
35 * @param PERF_STRING_2 String to use for logging the computation times of
36 * solver_2
37 * @param verb Verbosity level, 0 print nothing, 1 print summary, 2 print
38 * everything
39 */
40 30 int test_computeEquilibriumRobustness_vs_checkEquilibrium(
41 Equilibrium* solver_1, Equilibrium* solver_2, Cref_matrixXX comPositions,
42 const string& PERF_STRING_1 = "", const string& PERF_STRING_2 = "",
43 int verb = 0) {
44 30 int error_counter = 0;
45 double rob;
46 LP_status status;
47 bool equilibrium;
48
2/2
✓ Branch 1 taken 3000 times.
✓ Branch 2 taken 30 times.
 3030 for (unsigned int i = 0; i < comPositions.rows(); i++) {
49
4/8
✓ Branch 1 taken 3000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3000 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3000 times.
✗ Branch 11 not taken.
 3000 if (!PERF_STRING_1.empty()) getProfiler().start(PERF_STRING_1);
50
2/4
✓ Branch 1 taken 3000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3000 times.
✗ Branch 5 not taken.
 3000 status = solver_1->computeEquilibriumRobustness(
51
2/4
✓ Branch 1 taken 3000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3000 times.
✗ Branch 5 not taken.
 3000 comPositions.row(i).transpose(), rob);
52
4/8
✓ Branch 1 taken 3000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3000 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3000 times.
✗ Branch 11 not taken.
 3000 if (!PERF_STRING_1.empty()) getProfiler().stop(PERF_STRING_1);
53
54
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 3000 times.
 3000 if (status != LP_STATUS_OPTIMAL) {
55 ✗ if (verb > 1)
56 ✗ SEND_ERROR_MSG(solver_1->getName() +
57 " failed to compute robustness of com position " +
58 toString(comPositions.row(i)));
59 ✗ error_counter++;
60 ✗ continue;
61 }
62
63
1/8
✗ Branch 1 not taken.
✓ Branch 2 taken 3000 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
 3000 if (!PERF_STRING_2.empty()) getProfiler().start(PERF_STRING_2);
64
4/8
✓ Branch 1 taken 3000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3000 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3000 times.
✗ Branch 11 not taken.
 3000 status = solver_2->checkRobustEquilibrium(comPositions.row(i).transpose(),
65 equilibrium);
66
1/8
✗ Branch 1 not taken.
✓ Branch 2 taken 3000 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
 3000 if (!PERF_STRING_2.empty()) getProfiler().stop(PERF_STRING_2);
67
68
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 3000 times.
 3000 if (status != LP_STATUS_OPTIMAL) {
69 ✗ if (verb > 1)
70 ✗ SEND_ERROR_MSG(solver_2->getName() +
71 " failed to check equilibrium of com position " +
72 toString(comPositions.row(i)));
73 ✗ error_counter++;
74 ✗ continue;
75 }
76
77
3/4
✓ Branch 0 taken 231 times.
✓ Branch 1 taken 2769 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 231 times.
 3000 if (equilibrium == true && rob < 0.0) {
78 ✗ if (verb > 1)
79 ✗ SEND_ERROR_MSG(
80 solver_2->getName() + " says com is in equilibrium while " +
81 solver_1->getName() + " computed a negative robustness measure " +
82 toString(rob));
83 ✗ error_counter++;
84
3/4
✓ Branch 0 taken 2769 times.
✓ Branch 1 taken 231 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 2769 times.
 3000 } else if (equilibrium == false && rob > 0.0) {
85 ✗ if (verb > 1)
86 ✗ SEND_ERROR_MSG(
87 solver_2->getName() + " says com is not in equilibrium while " +
88 solver_1->getName() + " computed a positive robustness measure " +
89 toString(rob));
90 ✗ error_counter++;
91 }
92 }
93
94
1/2
✓ Branch 0 taken 30 times.
✗ Branch 1 not taken.
 30 if (verb > 0)
95
1/2
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
 30 cout << "Test test_computeEquilibriumRobustness_vs_checkEquilibrium " +
96
6/12
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 30 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 30 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 30 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 30 times.
✗ Branch 17 not taken.
 90 solver_1->getName() + " VS " + solver_2->getName() + ": " +
97
3/6
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 30 times.
✗ Branch 8 not taken.
 90 toString(error_counter) + " error(s).\n";
98 30 return error_counter;
99 }
100
101 /** Test two different solvers on the method
102 * Equilibrium::computeEquilibriumRobustness.
103 * @param solver_1 First solver to test.
104 * @param solver_2 Second solver to test.
105 * @param comPositions List of 2d com positions on which to perform the tests.
106 * @param PERF_STRING_1 String to use for logging the computation times of
107 * solver_1
108 * @param PERF_STRING_2 String to use for logging the computation times of
109 * solver_2
110 * @param verb Verbosity level, 0 print nothing, 1 print summary, 2 print
111 * everything
112 */
113 20 int test_computeEquilibriumRobustness(
114 Equilibrium* solver_1, Equilibrium* solver_2, Cref_matrixXX comPositions,
115 const string& PERF_STRING_1, const string& PERF_STRING_2, int verb = 0) {
116 20 int error_counter = 0;
117 double rob_1, rob_2;
118 LP_status status;
119
2/2
✓ Branch 1 taken 2000 times.
✓ Branch 2 taken 20 times.
 2020 for (unsigned int i = 0; i < comPositions.rows(); i++) {
120
3/6
✓ Branch 1 taken 2000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2000 times.
✗ Branch 8 not taken.
 2000 getProfiler().start(PERF_STRING_1);
121
2/4
✓ Branch 1 taken 2000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2000 times.
✗ Branch 5 not taken.
 2000 status = solver_1->computeEquilibriumRobustness(
122
2/4
✓ Branch 1 taken 2000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2000 times.
✗ Branch 5 not taken.
 2000 comPositions.row(i).transpose(), rob_1);
123
3/6
✓ Branch 1 taken 2000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2000 times.
✗ Branch 8 not taken.
 2000 getProfiler().stop(PERF_STRING_1);
124
125
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 2000 times.
 2000 if (status != LP_STATUS_OPTIMAL) {
126 ✗ if (verb > 1)
127 ✗ SEND_ERROR_MSG(solver_1->getName() +
128 " failed to compute robustness of com position " +
129 toString(comPositions.row(i)));
130 ✗ error_counter++;
131 ✗ continue;
132 }
133
134
3/6
✓ Branch 1 taken 2000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2000 times.
✗ Branch 8 not taken.
 2000 getProfiler().start(PERF_STRING_2);
135
2/4
✓ Branch 1 taken 2000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2000 times.
✗ Branch 5 not taken.
 2000 status = solver_2->computeEquilibriumRobustness(
136
2/4
✓ Branch 1 taken 2000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2000 times.
✗ Branch 5 not taken.
 2000 comPositions.row(i).transpose(), rob_2);
137
3/6
✓ Branch 1 taken 2000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2000 times.
✗ Branch 8 not taken.
 2000 getProfiler().stop(PERF_STRING_2);
138
139
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 2000 times.
 2000 if (status != LP_STATUS_OPTIMAL) {
140 ✗ if (verb > 1)
141 ✗ SEND_ERROR_MSG(solver_2->getName() +
142 " failed to compute robustness of com position " +
143 toString(comPositions.row(i)));
144 ✗ error_counter++;
145 ✗ continue;
146 }
147
148
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 2000 times.
 2000 if (fabs(rob_1 - rob_2) > EPS) {
149 ✗ if (verb > 1)
150 ✗ SEND_ERROR_MSG(solver_1->getName() + " and " + solver_2->getName() +
151 " returned different results: " + toString(rob_1) +
152 " VS " + toString(rob_2));
153 ✗ error_counter++;
154 }
155 }
156
157
1/2
✓ Branch 0 taken 20 times.
✗ Branch 1 not taken.
 20 if (verb > 0)
158
3/6
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
 40 cout << "Test computeEquilibriumRobustness " + solver_1->getName() +
159
6/12
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 20 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 20 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 20 times.
✗ Branch 17 not taken.
 80 " VS " + solver_2->getName() + ": " + toString(error_counter) +
160
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 " error(s).\n";
161 20 return error_counter;
162 }
163
164 /** Test method Equilibrium::findExtremumOverLine. The test works in this way:
165 * first it calls the method findExtremumOverLine of the solver to test to find
166 * the extremum over a random line with a specified robustness. Then it checks
167 * that the point found really has the specified robustness by using the
168 * ground-truth solver.
169 * @param solver_to_test Solver to test.
170 * @param solver_ground_truth Second solver to use as ground truth.
171 * @param a0 A 2d com position that allows for static equilibrium.
172 * @param N_TESTS Number of tests to perform.
173 * @param e_max Maximum value for the desired robustness.
174 * @param PERF_STRING_TEST String to use for logging the computation times of
175 * solver_to_test
176 * @param PERF_STRING_GROUND_TRUTH String to use for logging the computation
177 * times of solver_ground_truth
178 * @param verb Verbosity level, 0 print nothing, 1 print summary, 2 print
179 * everything
180 */
181 10 int test_findExtremumOverLine(Equilibrium* solver_to_test,
182 Equilibrium* solver_ground_truth, Cref_vector3 a0,
183 unsigned int N_TESTS, double e_max,
184 const string& PERF_STRING_TEST,
185 const string& PERF_STRING_GROUND_TRUTH,
186 int verb = 0) {
187 10 int error_counter = 0;
188
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
 10 centroidal_dynamics::Vector3 a, com;
189 LP_status status;
190 double desired_robustness, robustness;
191
2/2
✓ Branch 0 taken 1000 times.
✓ Branch 1 taken 10 times.
 1010 for (unsigned int i = 0; i < N_TESTS; i++) {
192
6/12
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1000 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1000 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1000 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1000 times.
✗ Branch 17 not taken.
 2000 uniform3(-1.0 * centroidal_dynamics::Vector3::Ones(),
193
1/2
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
 1000 centroidal_dynamics::Vector3::Ones(), a);
194
2/2
✓ Branch 0 taken 400 times.
✓ Branch 1 taken 600 times.
 1000 if (e_max >= 0.0)
195 400 desired_robustness = (rand() / value_type(RAND_MAX)) * e_max;
196 else
197 600 desired_robustness = e_max - EPS;
198
199
3/6
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1000 times.
✗ Branch 8 not taken.
 1000 getProfiler().start(PERF_STRING_TEST);
200 status =
201
3/6
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1000 times.
✗ Branch 8 not taken.
 1000 solver_to_test->findExtremumOverLine(a, a0, desired_robustness, com);
202
3/6
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1000 times.
✗ Branch 8 not taken.
 1000 getProfiler().stop(PERF_STRING_TEST);
203
204
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1000 times.
 1000 if (status != LP_STATUS_OPTIMAL) {
205 status =
206 ✗ solver_ground_truth->computeEquilibriumRobustness(a0, robustness);
207 ✗ if (status != LP_STATUS_OPTIMAL) {
208 ✗ error_counter++;
209 ✗ if (verb > 1)
210 ✗ SEND_ERROR_MSG(
211 solver_ground_truth->getName() +
212 " failed to compute equilibrium robustness of com position " +
213 toString(a0.transpose()));
214 ✗ } else if (robustness > desired_robustness) {
215 ✗ error_counter++;
216 ✗ if (verb > 1)
217 ✗ SEND_ERROR_MSG(solver_to_test->getName() +
218 " failed to find extremum over line starting from " +
219 toString(a0.transpose()) + " with robustness " +
220 toString(desired_robustness) + " while " +
221 solver_ground_truth->getName() +
222 " says this position has robustness " +
223 toString(robustness));
224 }
225 ✗ continue;
226 }
227
228
3/6
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1000 times.
✗ Branch 8 not taken.
 1000 getProfiler().start(PERF_STRING_GROUND_TRUTH);
229
2/4
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1000 times.
✗ Branch 5 not taken.
 1000 status = solver_ground_truth->computeEquilibriumRobustness(com, robustness);
230
3/6
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1000 times.
✗ Branch 8 not taken.
 1000 getProfiler().stop(PERF_STRING_GROUND_TRUTH);
231
232
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1000 times.
 1000 if (status != LP_STATUS_OPTIMAL) {
233 ✗ error_counter++;
234 ✗ if (verb > 1)
235 ✗ SEND_ERROR_MSG(
236 solver_ground_truth->getName() +
237 " failed to compute equilibrium robustness of com posiiton " +
238 toString(com.transpose()));
239
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1000 times.
 1000 } else if (fabs(robustness - desired_robustness) > EPS) {
240 ✗ if (verb > 1)
241 ✗ SEND_ERROR_MSG(
242 solver_to_test->getName() +
243 " found this extremum: " + toString(com.transpose()) +
244 " over the line starting at " + toString(a0.transpose()) +
245 " in direction " + toString(a.transpose()) +
246 " which should have robustness " + toString(desired_robustness) +
247 " but actually has robustness " + toString(robustness));
248 ✗ error_counter++;
249 }
250 }
251
252
1/2
✓ Branch 0 taken 10 times.
✗ Branch 1 not taken.
 10 if (verb > 0)
253
4/8
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
 20 cout << "Test findExtremumOverLine " + solver_to_test->getName() + " VS " +
254
3/6
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
 40 solver_ground_truth->getName() + ": " +
255
3/6
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
 30 toString(error_counter) + " error(s).\n";
256 10 return error_counter;
257 }
258
259 /** Draw a grid on the screen using the robustness computed with the method
260 * Equilibrium::computeEquilibriumRobustness.
261 * @param solver The solver to use for computing the equilibrium robustness.
262 * @param comPositions Grid of CoM positions in the form of an Nx2 matrix.
263 */
264 ✗ void drawRobustnessGrid(int N_CONTACTS, int GRID_SIZE, Equilibrium* solver,
265 Cref_matrixXX comPositions, Cref_matrixXX p) {
266 ✗ MatrixXi contactPointCoord(4 * N_CONTACTS, 2);
267 centroidal_dynamics::VectorX minDistContactPoint =
268 ✗ 1e10 * centroidal_dynamics::VectorX::Ones(4 * N_CONTACTS);
269
270 // create grid of com positions to test
271 ✗ for (int i = 0; i < GRID_SIZE; i++) {
272 ✗ for (int j = 0; j < GRID_SIZE; j++) {
273 // look for contact point positions on grid
274 ✗ for (long k = 0; k < 4 * N_CONTACTS; k++) {
275 ✗ double dist = (p.block<1, 2>(k, 0) -
276 ✗ comPositions.block<1, 2>(i * GRID_SIZE + j, 0))
277 ✗ .norm();
278 ✗ if (dist < minDistContactPoint(k)) {
279 ✗ minDistContactPoint(k) = dist;
280 ✗ contactPointCoord(k, 0) = i;
281 ✗ contactPointCoord(k, 1) = j;
282 }
283 }
284 }
285 }
286
287 ✗ cout << "\nContact point positions\n";
288 bool contactPointDrawn;
289 ✗ for (int i = 0; i < GRID_SIZE; i++) {
290 ✗ for (int j = 0; j < GRID_SIZE; j++) {
291 ✗ contactPointDrawn = false;
292 ✗ for (long k = 0; k < 4 * N_CONTACTS; k++) {
293 ✗ if (contactPointCoord(k, 0) == i && contactPointCoord(k, 1) == j) {
294 ✗ cout << "X ";
295 ✗ contactPointDrawn = true;
296 ✗ break;
297 }
298 }
299 ✗ if (contactPointDrawn == false) cout << "- ";
300 }
301 ✗ printf("\n");
302 }
303
304 ✗ cout << "\nRobustness grid computed with solver " << solver->getName()
305 ✗ << endl;
306 ✗ int grid_size = (int)sqrt(comPositions.rows());
307 double rob;
308 LP_status status;
309 ✗ for (int i = 0; i < comPositions.rows(); i++) {
310 ✗ status = solver->computeEquilibriumRobustness(
311 ✗ comPositions.row(i).transpose(), rob);
312 ✗ if (status != LP_STATUS_OPTIMAL) {
313 ✗ SEND_ERROR_MSG(
314 "Faild to compute equilibrium robustness of com position " +
315 toString(comPositions.row(i)) + ", error code " + toString(status));
316 ✗ rob = -1.0;
317 }
318
319 ✗ if (rob >= 0.0) {
320 ✗ if (rob > 9.0) rob = 9.0;
321 ✗ printf("%d ", (int)rob);
322 } else
323 ✗ printf("- ");
324 ✗ if ((i + 1) % grid_size == 0) printf("\n");
325 }
326 }
327
328 10 void generateContacts(unsigned int N_CONTACTS, double MIN_CONTACT_DISTANCE,
329 double LX, double LY,
330 RVector3& CONTACT_POINT_LOWER_BOUNDS,
331 RVector3& CONTACT_POINT_UPPER_BOUNDS,
332 RVector3& RPY_LOWER_BOUNDS, RVector3& RPY_UPPER_BOUNDS,
333 centroidal_dynamics::MatrixX3& p,
334 centroidal_dynamics::MatrixX3& N) {
335
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
 10 MatrixXX contact_pos = MatrixXX::Zero(N_CONTACTS, 3);
336
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
 10 MatrixXX contact_rpy = MatrixXX::Zero(N_CONTACTS, 3);
337
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 p.setZero(4 * N_CONTACTS, 3); // contact points
338
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 N.setZero(4 * N_CONTACTS, 3); // contact normals
339
340 // Generate contact positions and orientations
341 bool collision;
342
2/2
✓ Branch 0 taken 20 times.
✓ Branch 1 taken 10 times.
 30 for (unsigned int i = 0; i < N_CONTACTS; i++) {
343 while (true) // generate contact position
344 {
345
4/8
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 20 times.
✗ Branch 11 not taken.
 20 uniform(CONTACT_POINT_LOWER_BOUNDS, CONTACT_POINT_UPPER_BOUNDS,
346
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 contact_pos.row(i));
347
2/2
✓ Branch 0 taken 10 times.
✓ Branch 1 taken 10 times.
 20 if (i == 0) break;
348 10 collision = false;
349
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 10 times.
 10 for (unsigned int j = 0; j < i - 1; j++)
350 ✗ if ((contact_pos.row(i) - contact_pos.row(j)).norm() <
351 MIN_CONTACT_DISTANCE)
352 ✗ collision = true;
353
1/2
✓ Branch 0 taken 10 times.
✗ Branch 1 not taken.
 10 if (collision == false) break;
354 }
355
356 // generate contact orientation
357
5/10
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 20 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 20 times.
✗ Branch 14 not taken.
 20 uniform(RPY_LOWER_BOUNDS, RPY_UPPER_BOUNDS, contact_rpy.row(i));
358
7/14
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 20 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 20 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 20 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 20 times.
✗ Branch 20 not taken.
 60 generate_rectangle_contacts(LX, LY, contact_pos.row(i).transpose(),
359
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 20 contact_rpy.row(i).transpose(),
360
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 60 p.middleRows<4>(i * 4), N.middleRows<4>(i * 4));
361 // printf("Contact surface %d position (%.3f,%.3f,%.3f) ", i,
362 // contact_pos(i,0), contact_pos(i,1), contact_pos(i,2));
363 // printf("Orientation (%.3f,%.3f,%.3f)\n", contact_rpy(i,0),
364 // contact_rpy(i,1), contact_rpy(i,2));
365 }
366
367 // for(int i=0; i<p.rows(); i++)
368 // {
369 // printf("Contact point %d position (%.3f,%.3f,%.3f) ", i, p(i,0), p(i,1),
370 // p(i,2)); printf("Normal (%.3f,%.3f,%.3f)\n", N(i,0), N(i,1), N(i,2));
371 // }
372 10 }
373
374 1 void testWithLoadedData() {
375
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 cout << "*** TEST WITH LOADED DATA ***\n";
376
377 1 double mass = 55.8836;
378 1 double mu = 0.5; // friction coefficient
379 1 unsigned int generatorsPerContact = 4;
380
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 string file_path = "../test_data/";
381 1 double expected_robustness = 17.1222;
382
383 1 const int N_SOLVERS = 3;
384
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
 5 string solverNames[] = {"LP oases", "LP2 oases", "DLP oases"};
385 1 EquilibriumAlgorithm algorithms[] = {EQUILIBRIUM_ALGORITHM_LP,
386 EQUILIBRIUM_ALGORITHM_LP2,
387 EQUILIBRIUM_ALGORITHM_DLP};
388
389
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 MatrixXX contactPoints, contactNormals;
390
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 centroidal_dynamics::Vector3 com;
391
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 1 times.
 1 if (!readMatrixFromFile(file_path + "positions.dat", contactPoints)) {
392 ✗ SEND_ERROR_MSG("Impossible to read positions from file");
393 ✗ return;
394 }
395
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 1 times.
 1 if (!readMatrixFromFile(file_path + "normals.dat", contactNormals)) {
396 ✗ SEND_ERROR_MSG("Impossible to read normals from file");
397 ✗ return;
398 }
399
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 1 times.
 1 if (!readMatrixFromFile(file_path + "com.dat", com)) {
400 ✗ SEND_ERROR_MSG("Impossible to read com from file");
401 ✗ return;
402 }
403
404 // this is a test to ensure that a matrixXX can be cast into a MatrixX3
405
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 const centroidal_dynamics::MatrixX3& cp = contactPoints;
406
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 const centroidal_dynamics::MatrixX3& cn = contactNormals;
407 Equilibrium* solvers[N_SOLVERS];
408 double robustness[N_SOLVERS];
409
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 1 times.
 4 for (int s = 0; s < N_SOLVERS; s++) {
410 6 solvers[s] = new Equilibrium(solverNames[s], mass, generatorsPerContact,
411
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 3 SOLVER_LP_QPOASES);
412
413
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 3 times.
 3 if (!solvers[s]->setNewContacts(cp, cn, mu, algorithms[s])) {
414 ✗ SEND_ERROR_MSG("Error while setting new contacts for solver " +
415 solvers[s]->getName());
416 ✗ continue;
417 }
418 LP_status status =
419
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 3 solvers[s]->computeEquilibriumRobustness(com, robustness[s]);
420
1/2
✓ Branch 0 taken 3 times.
✗ Branch 1 not taken.
 3 if (status == LP_STATUS_OPTIMAL) {
421
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 3 times.
 3 if (fabs(expected_robustness - robustness[s]) > EPS)
422 ✗ cout << "[ERROR] Solver " << solvers[s]->getName()
423 ✗ << " computed robustness " << robustness[s] << " rather than "
424 ✗ << expected_robustness << endl;
425 } else
426 ✗ SEND_ERROR_MSG("Solver " + solvers[s]->getName() +
427 " failed to compute robustness, error code " +
428 toString(status));
429 }
430
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 cout << "*** END TEST WITH LOADED DATA ***\n\n";
431
5/10
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 3 times.
✓ Branch 9 taken 1 times.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
 6 }
432
433 1 int main() {
434
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 testWithLoadedData();
435
436
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 cout << "*** TEST WITH RANDOMLY GENERATED DATA ***\n";
437 1 unsigned int seed = (unsigned int)(time(NULL));
438 // seed = 1446555515;
439 1 srand(seed);
440
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 cout << "Initialize random number generator with seed " << seed
441
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 << " (in case you wanna repeat the same test later)\n";
442
443
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 RVector3 CONTACT_POINT_LOWER_BOUNDS, CONTACT_POINT_UPPER_BOUNDS;
444
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 RVector3 RPY_LOWER_BOUNDS, RPY_UPPER_BOUNDS;
445
446 /************************************** USER PARAMETERS
447 * *******************************/
448 1 unsigned int N_TESTS = 10;
449 1 double mass = 55.0;
450 1 double mu = 0.3; // friction coefficient
451 1 unsigned int generatorsPerContact = 4;
452 1 unsigned int N_CONTACTS = 2;
453 1 double MIN_CONTACT_DISTANCE = 0.3;
454 1 double LX = 0.5 * 0.2172; // half contact surface size in x direction
455 1 double LY = 0.5 * 0.138; // half contact surface size in y direction
456
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 CONTACT_POINT_LOWER_BOUNDS << 0.0, 0.0, 0.0;
457
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 CONTACT_POINT_UPPER_BOUNDS << 0.5, 0.5, 0.5;
458 1 double gamma = atan(mu); // half friction cone angle
459
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 RPY_LOWER_BOUNDS << -2 * gamma, -2 * gamma, -M_PI;
460
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 RPY_UPPER_BOUNDS << +2 * gamma, +2 * gamma, +M_PI;
461 1 double X_MARG = 0.07;
462 1 double Y_MARG = 0.07;
463 1 const unsigned int GRID_SIZE = 10;
464 1 bool DRAW_CONTACT_POINTS = false;
465 /************************************ END USER PARAMETERS
466 * *****************************/
467
468 #ifdef CLP_FOUND
469 const int N_SOLVERS = 6;
470 string solverNames[] = {"LP oases", "LP2 oases", "DLP oases",
471 "LP coin", "LP2 coin", "DLP coin"};
472 EquilibriumAlgorithm algorithms[] = {
473 EQUILIBRIUM_ALGORITHM_LP, EQUILIBRIUM_ALGORITHM_LP2,
474 EQUILIBRIUM_ALGORITHM_DLP, EQUILIBRIUM_ALGORITHM_LP,
475 EQUILIBRIUM_ALGORITHM_LP2, EQUILIBRIUM_ALGORITHM_DLP};
476 SolverLP lp_solver_types[] = {SOLVER_LP_QPOASES, SOLVER_LP_QPOASES,
477 SOLVER_LP_QPOASES, SOLVER_LP_CLP,
478 SOLVER_LP_CLP, SOLVER_LP_CLP};
479 #else
480 1 const int N_SOLVERS = 3;
481
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
 5 string solverNames[] = {"LP oases", "LP2 oases", "DLP oases"};
482 1 EquilibriumAlgorithm algorithms[] = {EQUILIBRIUM_ALGORITHM_LP,
483 EQUILIBRIUM_ALGORITHM_LP2,
484 EQUILIBRIUM_ALGORITHM_DLP};
485 1 SolverLP lp_solver_types[] = {SOLVER_LP_QPOASES, SOLVER_LP_QPOASES,
486 SOLVER_LP_QPOASES};
487 #endif
488
489
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 cout << "Number of contacts: " << N_CONTACTS << endl;
490
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 cout << "Number of generators per contact: " << generatorsPerContact << endl;
491
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 cout << "Gonna test equilibrium on a 2d grid of " << GRID_SIZE << "X"
492
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 << GRID_SIZE << " points " << endl;
493
494 Equilibrium* solver_PP =
495
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 new Equilibrium("PP", mass, generatorsPerContact, SOLVER_LP_QPOASES);
496 Equilibrium* solvers[N_SOLVERS];
497
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 1 times.
 4 for (int s = 0; s < N_SOLVERS; s++)
498 3 solvers[s] = new Equilibrium(solverNames[s], mass, generatorsPerContact,
499
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 3 lp_solver_types[s]);
500
501
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 centroidal_dynamics::MatrixX3 p, N;
502
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 RVector2 com_LB, com_UB;
503
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 centroidal_dynamics::VectorX x_range(GRID_SIZE), y_range(GRID_SIZE);
504
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 MatrixXX comPositions(GRID_SIZE * GRID_SIZE, 3);
505
2/2
✓ Branch 0 taken 10 times.
✓ Branch 1 taken 1 times.
 11 for (unsigned int n_test = 0; n_test < N_TESTS; n_test++) {
506
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 generateContacts(N_CONTACTS, MIN_CONTACT_DISTANCE, LX, LY,
507 CONTACT_POINT_LOWER_BOUNDS, CONTACT_POINT_UPPER_BOUNDS,
508 RPY_LOWER_BOUNDS, RPY_UPPER_BOUNDS, p, N);
509
510
2/2
✓ Branch 0 taken 30 times.
✓ Branch 1 taken 10 times.
 40 for (int s = 0; s < N_SOLVERS; s++) {
511
3/6
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 30 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 30 times.
✗ Branch 9 not taken.
 30 getProfiler().start(PERF_LP_PREPARATION);
512
2/4
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 30 times.
 30 if (!solvers[s]->setNewContacts(p, N, mu, algorithms[s])) {
513 ✗ SEND_ERROR_MSG("Error while setting new contacts for solver " +
514 solvers[s]->getName());
515 ✗ return -1;
516 }
517
3/6
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 30 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 30 times.
✗ Branch 9 not taken.
 30 getProfiler().stop(PERF_LP_PREPARATION);
518 }
519
3/6
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 10 times.
✗ Branch 9 not taken.
 10 getProfiler().start(PERF_PP);
520
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 10 times.
 10 if (!solver_PP->setNewContacts(p, N, mu, EQUILIBRIUM_ALGORITHM_PP)) {
521 ✗ SEND_ERROR_MSG("Error while setting new contacts for solver " +
522 solver_PP->getName());
523 ✗ return -1;
524 }
525
3/6
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 10 times.
✗ Branch 9 not taken.
 10 getProfiler().stop(PERF_PP);
526
527 // compute upper and lower bounds of com positions to test
528
3/6
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
 10 com_LB(0) = p.col(0).minCoeff() - X_MARG;
529
3/6
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
 10 com_UB(0) = p.col(0).maxCoeff() + X_MARG;
530
3/6
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
 10 com_LB(1) = p.col(1).minCoeff() - Y_MARG;
531
3/6
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
 10 com_UB(1) = p.col(1).maxCoeff() + Y_MARG;
532
533 // create grid of com positions to test
534
535
3/6
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
 10 x_range.setLinSpaced(GRID_SIZE, com_LB(0), com_UB(0));
536
3/6
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
 10 y_range.setLinSpaced(GRID_SIZE, com_LB(1), com_UB(1));
537 // cout<<"ranging from "<<com_LB<<" to "<<com_UB<<endl;
538
2/2
✓ Branch 0 taken 100 times.
✓ Branch 1 taken 10 times.
 110 for (unsigned int i = 0; i < GRID_SIZE; i++) {
539
2/2
✓ Branch 0 taken 1000 times.
✓ Branch 1 taken 100 times.
 1100 for (unsigned int j = 0; j < GRID_SIZE; j++) {
540
2/4
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1000 times.
✗ Branch 5 not taken.
 1000 comPositions(i * GRID_SIZE + j, 1) = y_range(GRID_SIZE - 1 - i);
541
2/4
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1000 times.
✗ Branch 5 not taken.
 1000 comPositions(i * GRID_SIZE + j, 0) = x_range(j);
542
1/2
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
 1000 comPositions(i * GRID_SIZE + j, 2) = 0.0;
543 }
544 }
545
546
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 10 times.
 10 if (DRAW_CONTACT_POINTS)
547 ✗ drawRobustnessGrid(N_CONTACTS, GRID_SIZE, solvers[0], comPositions, p);
548
549
1/2
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
 10 string test_name = "Compute equilibrium robustness ";
550
2/2
✓ Branch 0 taken 20 times.
✓ Branch 1 taken 10 times.
 30 for (int s = 1; s < N_SOLVERS; s++) {
551
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 20 test_computeEquilibriumRobustness(solvers[0], solvers[s], comPositions,
552
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 test_name + solvers[0]->getName(),
553
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 test_name + solvers[s]->getName(), 1);
554 }
555
556
2/2
✓ Branch 0 taken 30 times.
✓ Branch 1 taken 10 times.
 40 for (int s = 0; s < N_SOLVERS; s++) {
557
3/6
✓ Branch 2 taken 30 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 30 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 30 times.
✗ Branch 9 not taken.
 90 test_computeEquilibriumRobustness_vs_checkEquilibrium(
558 solvers[s], solver_PP, comPositions,
559
2/4
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
 60 test_name + solvers[s]->getName(), "", 1);
560 }
561
562 10 const int N_TESTS_EXTREMUM = 100;
563
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
 10 centroidal_dynamics::Vector3 a0 = centroidal_dynamics::Vector3::Zero();
564
4/8
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
 10 a0.head<2>() = 0.5 * (com_LB + com_UB);
565 double e_max;
566
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
 10 LP_status status = solvers[0]->computeEquilibriumRobustness(a0, e_max);
567
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 10 times.
 10 if (status != LP_STATUS_OPTIMAL)
568 ✗ SEND_ERROR_MSG(solvers[0]->getName() +
569 " failed to compute robustness of com position " +
570 toString(a0.transpose()) +
571 ", error code: " + toString(status));
572 else {
573
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 test_name = "EXTREMUM OVER LINE ";
574
1/2
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
 10 string test_name2 = "Compute equilibrium robustness ";
575
2/2
✓ Branch 0 taken 20 times.
✓ Branch 1 taken 10 times.
 30 for (int s = 1; s < N_SOLVERS; s++) {
576
2/2
✓ Branch 1 taken 10 times.
✓ Branch 2 taken 10 times.
 20 if (solvers[s]->getAlgorithm() != EQUILIBRIUM_ALGORITHM_LP2)
577
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
 10 test_findExtremumOverLine(solvers[s], solvers[0], a0,
578 N_TESTS_EXTREMUM, e_max,
579
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
 20 test_name + solvers[s]->getName(),
580
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
 20 test_name2 + solvers[0]->getName(), 1);
581 }
582 10 }
583 10 }
584
585
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 getProfiler().report_all();
586
587
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 cout << "*** END TEST WITH RANDOMLY GENERATED DATA ***\n";
588
589 1 return 0;
590
2/4
✓ Branch 5 taken 3 times.
✓ Branch 6 taken 1 times.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
 5 }
591