GCC Code Coverage Report

Directory: ./
File: src/centroidal_dynamics.cpp
Date: 2025-03-17 04:04:52
Exec Total Coverage
Lines: 225 372 60.5%
Branches: 336 1122 29.9%
Line Branch Exec Source
1 /*
2 * Copyright 2015, LAAS-CNRS
3 * Author: Andrea Del Prete
4 */
5
6 #include <ctime>
7 #include <hpp/centroidal-dynamics/centroidal_dynamics.hh>
8 #include <hpp/centroidal-dynamics/logger.hh>
9 #include <hpp/centroidal-dynamics/stop-watch.hh>
10 #include <iostream>
11 #include <vector>
12
13 using namespace std;
14
15 namespace centroidal_dynamics {
16
17 bool Equilibrium::m_is_cdd_initialized = false;
18
19 ✗ Equilibrium::Equilibrium(const Equilibrium& other)
20 ✗ : m_mass(other.m_mass),
21 ✗ m_gravity(other.m_gravity),
22 ✗ m_G_centr(other.m_G_centr),
23 ✗ m_name(other.m_name),
24 ✗ m_algorithm(other.m_algorithm),
25 ✗ m_solver_type(other.m_solver_type),
26 ✗ m_solver(Solver_LP_abstract::getNewSolver(other.m_solver_type)),
27 ✗ m_generatorsPerContact(other.m_generatorsPerContact),
28 ✗ m_H(other.m_H),
29 ✗ m_h(other.m_h),
30 ✗ m_is_cdd_stable(other.m_is_cdd_stable),
31 ✗ max_num_cdd_trials(other.max_num_cdd_trials),
32 ✗ canonicalize_cdd_matrix(other.canonicalize_cdd_matrix),
33 ✗ m_HD(other.m_HD),
34 ✗ m_Hd(other.m_Hd),
35 ✗ m_D(other.m_D),
36 ✗ m_d(other.m_d),
37 ✗ m_b0_to_emax_coefficient(other.m_b0_to_emax_coefficient) {
38 ✗ m_solver->setUseWarmStart(other.m_solver->getUseWarmStart());
39 }
40
41 13 Equilibrium::Equilibrium(const string& name, const double mass,
42 const unsigned int generatorsPerContact,
43 const SolverLP solver_type, const bool useWarmStart,
44 const unsigned int max_num_cdd_trials,
45 13 const bool canonicalize_cdd_matrix)
46 13 : m_mass(mass),
47
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 13 m_gravity(0., 0., -9.81),
48 13 m_is_cdd_stable(true),
49 13 max_num_cdd_trials(max_num_cdd_trials),
50
6/12
✓ Branch 3 taken 13 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 13 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 13 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 13 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 13 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 13 times.
✗ Branch 19 not taken.
 13 canonicalize_cdd_matrix(canonicalize_cdd_matrix) {
51
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 11 times.
 13 if (!m_is_cdd_initialized) {
52
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 init_cdd_library();
53 2 m_is_cdd_initialized = true;
54 // srand ( (unsigned int) (time(NULL)) );
55 }
56
57
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 13 m_name = name;
58 13 m_solver_type = solver_type;
59
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 13 m_solver = Solver_LP_abstract::getNewSolver(solver_type);
60
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 13 m_solver->setUseWarmStart(useWarmStart);
61
62 13 m_generatorsPerContact = generatorsPerContact;
63
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 13 times.
 13 if (generatorsPerContact < 3) {
64 ✗ SEND_WARNING_MSG(
65 "Algorithm cannot work with less than 3 generators per contact!");
66 ✗ m_generatorsPerContact = 3;
67 }
68
69 /*m_gravity.setZero();
70 m_gravity(2) = -9.81;*/
71
72
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 13 m_d.setZero();
73
3/6
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13 times.
✗ Branch 8 not taken.
 13 m_d.head<3>() = m_mass * m_gravity;
74
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 13 m_D.setZero();
75
5/10
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 13 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 13 times.
✗ Branch 14 not taken.
 13 m_D.block<3, 3>(3, 0) = crossMatrix(-m_mass * m_gravity);
76 13 }
77
78
1/2
✓ Branch 0 taken 6 times.
✗ Branch 1 not taken.
 6 Equilibrium::~Equilibrium() { delete m_solver; }
79
80 45 bool Equilibrium::computeGenerators(Cref_matrixX3 contactPoints,
81 Cref_matrixX3 contactNormals,
82 double frictionCoefficient,
83 const bool perturbate) {
84 45 long int c = contactPoints.rows();
85 45 unsigned int& cg = m_generatorsPerContact;
86 45 double theta, delta_theta = 2 * M_PI / cg;
87 45 const value_type pi_4 = value_type(M_PI_4);
88 // perturbation for libcdd
89 45 const value_type epsilon = 2 * 1e-3;
90
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 45 times.
 45 if (perturbate)
91 ✗ frictionCoefficient =
92 ✗ frictionCoefficient + (rand() / value_type(RAND_MAX)) * epsilon;
93 // Tangent directions
94
3/6
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 45 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 45 times.
✗ Branch 8 not taken.
 45 Vector3 T1, T2, N;
95 // contact point
96
1/2
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
 45 Vector3 P;
97 // Matrix mapping a 3d contact force to gravito-inertial wrench (6 X 3)
98
1/2
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
 45 Matrix63 A;
99
4/8
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 45 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 45 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 45 times.
✗ Branch 11 not taken.
 45 A.topRows<3>() = -Matrix3::Identity();
100
1/2
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
 45 Matrix3X G(3, cg);
101
2/2
✓ Branch 0 taken 352 times.
✓ Branch 1 taken 45 times.
 397 for (long int i = 0; i < c; i++) {
102 // check that contact normals have norm 1
103
3/6
✓ Branch 1 taken 352 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 352 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 352 times.
 352 if (fabs(contactNormals.row(i).norm() - 1.0) > 1e-4) {
104 ✗ SEND_ERROR_MSG("Contact normals should have norm 1, this has norm %f" +
105 toString(contactNormals.row(i).norm()));
106 ✗ return false;
107 }
108 // compute tangent directions
109
2/4
✓ Branch 1 taken 352 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 352 times.
✗ Branch 5 not taken.
 352 N = contactNormals.row(i);
110
2/4
✓ Branch 1 taken 352 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 352 times.
✗ Branch 5 not taken.
 352 P = contactPoints.row(i);
111
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 352 times.
 352 if (perturbate) {
112 ✗ for (int k = 0; k < 3; ++k) {
113 ✗ N(k) += (rand() / value_type(RAND_MAX)) * epsilon;
114 ✗ P(k) += (rand() / value_type(RAND_MAX)) * epsilon;
115 }
116 ✗ N.normalize();
117 }
118
2/4
✓ Branch 1 taken 352 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 352 times.
✗ Branch 5 not taken.
 352 T1 = N.cross(Vector3::UnitY());
119
2/8
✓ Branch 1 taken 352 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 352 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
 352 if (T1.norm() < 1e-5) T1 = N.cross(Vector3::UnitX());
120
3/6
✓ Branch 1 taken 352 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 352 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 352 times.
✗ Branch 8 not taken.
 352 T2 = N.transpose().cross(T1);
121
1/2
✓ Branch 1 taken 352 times.
✗ Branch 2 not taken.
 352 T1.normalize();
122
1/2
✓ Branch 1 taken 352 times.
✗ Branch 2 not taken.
 352 T2.normalize();
123
124 // compute matrix mapping contact forces to gravito-inertial wrench
125
5/10
✓ Branch 1 taken 352 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 352 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 352 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 352 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 352 times.
✗ Branch 14 not taken.
 352 A.bottomRows<3>() = crossMatrix(-1.0 * P);
126
127 // compute generators
128 352 theta = pi_4;
129
2/2
✓ Branch 0 taken 1408 times.
✓ Branch 1 taken 352 times.
 1760 for (unsigned int j = 0; j < cg; j++) {
130
3/6
✓ Branch 1 taken 1408 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1408 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1408 times.
✗ Branch 8 not taken.
 2816 G.col(j) = frictionCoefficient * sin(theta) * T1 +
131
2/4
✓ Branch 1 taken 1408 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1408 times.
✗ Branch 5 not taken.
 2816 frictionCoefficient * cos(theta) * T2 +
132
3/6
✓ Branch 1 taken 1408 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1408 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1408 times.
✗ Branch 8 not taken.
 2816 contactNormals.row(i).transpose();
133
2/4
✓ Branch 1 taken 1408 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1408 times.
✗ Branch 5 not taken.
 1408 G.col(j).normalize();
134 // SEND_DEBUG_MSG("Contact "+toString(i)+" generator "+toString(j)+"
135 // = "+toString(G.col(j).transpose()));
136 1408 theta += delta_theta;
137 }
138
139 // project generators in 6d centroidal space
140
3/6
✓ Branch 1 taken 352 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 352 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 352 times.
✗ Branch 8 not taken.
 352 m_G_centr.block(0, cg * i, 6, cg) = A * G;
141 }
142
143 // Compute the coefficient to convert b0 to e_max
144
2/4
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 45 times.
✗ Branch 5 not taken.
 45 Vector3 f0 = Vector3::Zero();
145
2/2
✓ Branch 0 taken 180 times.
✓ Branch 1 taken 45 times.
 225 for (unsigned int j = 0; j < cg; j++)
146
2/4
✓ Branch 1 taken 180 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 180 times.
✗ Branch 5 not taken.
 180 f0 += G.col(j); // sum of the contact generators
147 // Compute the distance between the friction cone boundaries and
148 // the sum of the contact generators, which is e_max when b0=1.
149 // When b0!=1 we just multiply b0 times this value.
150 // This value depends only on the number of generators and the friction
151 // coefficient
152
3/6
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 45 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 45 times.
✗ Branch 8 not taken.
 45 m_b0_to_emax_coefficient = (f0.cross(G.col(0))).norm();
153 45 return true;
154 45 }
155
156 ✗ void Equilibrium::setAlgorithm(EquilibriumAlgorithm algorithm) {
157 ✗ if (algorithm == EQUILIBRIUM_ALGORITHM_PP && m_G_centr.rows() > 0)
158 ✗ SEND_DEBUG_MSG(
159 "Cannot set algorithm to PP after setting contacts, you should call "
160 "again setNewContact with PP algorithm");
161 else
162 ✗ m_algorithm = algorithm;
163 }
164
165 2 bool Equilibrium::setNewContacts(const MatrixX3ColMajor& contactPoints,
166 const MatrixX3ColMajor& contactNormals,
167 const double frictionCoefficient,
168 const EquilibriumAlgorithm alg) {
169
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 MatrixX3 _contactPoints = contactPoints;
170
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 MatrixX3 _contactNormals = contactNormals;
171
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 return setNewContacts(_contactPoints, _contactNormals, frictionCoefficient,
172 4 alg);
173 2 }
174
175 45 bool Equilibrium::setNewContacts(const MatrixX3& contactPoints,
176 const MatrixX3& contactNormals,
177 const double frictionCoefficient,
178 const EquilibriumAlgorithm alg) {
179
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 45 times.
 45 assert(contactPoints.rows() == contactNormals.rows());
180
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 45 times.
 45 if (alg == EQUILIBRIUM_ALGORITHM_IP) {
181 ✗ SEND_ERROR_MSG("Algorithm IP not implemented yet");
182 ✗ return false;
183 }
184
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 45 times.
 45 if (alg == EQUILIBRIUM_ALGORITHM_DIP) {
185 ✗ SEND_ERROR_MSG("Algorithm DIP not implemented yet");
186 ✗ return false;
187 }
188
189 45 m_algorithm = alg;
190
191 // Lists of contact generators (3 X generatorsPerContact)
192 45 m_G_centr.resize(6, contactPoints.rows() * m_generatorsPerContact);
193
194
3/6
✓ Branch 2 taken 45 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 45 times.
✗ Branch 6 not taken.
✗ Branch 9 not taken.
✓ Branch 10 taken 45 times.
 45 if (!computeGenerators(contactPoints, contactNormals, frictionCoefficient,
195 false)) {
196 ✗ return false;
197 }
198
199
2/2
✓ Branch 0 taken 11 times.
✓ Branch 1 taken 34 times.
 45 if (m_algorithm == EQUILIBRIUM_ALGORITHM_PP) {
200 11 unsigned int attempts = max_num_cdd_trials;
201
5/14
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 11 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 11 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✓ Branch 10 taken 11 times.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✓ Branch 14 taken 11 times.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
 11 while (!computePolytopeProjection(m_G_centr) && attempts > 0) {
202 ✗ computeGenerators(contactPoints, contactNormals, frictionCoefficient,
203 true);
204 ✗ attempts--;
205 }
206 // resetting random because obviously libcdd always resets to 0
207 11 srand((unsigned int)time(NULL));
208
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 11 times.
 11 if (!m_is_cdd_stable) {
209 ✗ return false;
210 }
211
1/2
✓ Branch 2 taken 11 times.
✗ Branch 3 not taken.
 11 m_HD = m_H * m_D;
212
1/2
✓ Branch 2 taken 11 times.
✗ Branch 3 not taken.
 11 m_Hd = m_H * m_d;
213 }
214
215 45 return true;
216 }
217
218 static const Vector3 zero_acc = Vector3::Zero();
219
220 8014 LP_status Equilibrium::computeEquilibriumRobustness(Cref_vector3 com,
221 double& robustness) {
222
1/2
✓ Branch 2 taken 8014 times.
✗ Branch 3 not taken.
 8014 return computeEquilibriumRobustness(com, zero_acc, robustness);
223 }
224
225 8015 LP_status Equilibrium::computeEquilibriumRobustness(Cref_vector3 com,
226 Cref_vector3 acc,
227 double& robustness) {
228 // Take the acceleration in account in D and d :
229
5/10
✓ Branch 2 taken 8015 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 8015 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 8015 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 8015 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 8015 times.
✗ Branch 15 not taken.
 8015 m_D.block<3, 3>(3, 0) = crossMatrix(-m_mass * (m_gravity - acc));
230
3/6
✓ Branch 2 taken 8015 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 8015 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 8015 times.
✗ Branch 9 not taken.
 8015 m_d.head<3>() = m_mass * (m_gravity - acc);
231
232 const long m =
233 8015 m_G_centr.cols(); // number of gravito-inertial wrench generators
234
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 8015 times.
 8015 if (m == 0) return LP_STATUS_INFEASIBLE;
235
236
2/2
✓ Branch 0 taken 4013 times.
✓ Branch 1 taken 4002 times.
 8015 if (m_algorithm == EQUILIBRIUM_ALGORITHM_LP) {
237 /* Compute the robustness measure of the equilibrium of a specified CoM
238 position
239 * by solving the following LP:
240 find b, b0
241 minimize -b0
242 subject to D c + d <= G b <= D c + d
243 0 <= b - b0 <= Inf
244 where
245 b are the coefficient of the contact force generators (f = V
246 b) b0 is the robustness measure c is the CoM position G is
247 the matrix whose columns are the gravito-inertial wrench generators
248 */
249
1/2
✓ Branch 1 taken 4013 times.
✗ Branch 2 not taken.
 4013 VectorX b_b0(m + 1);
250
2/4
✓ Branch 1 taken 4013 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4013 times.
✗ Branch 5 not taken.
 4013 VectorX c = VectorX::Zero(m + 1);
251
1/2
✓ Branch 1 taken 4013 times.
✗ Branch 2 not taken.
 4013 c(m) = -1.0;
252
4/8
✓ Branch 1 taken 4013 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4013 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4013 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4013 times.
✗ Branch 11 not taken.
 4013 VectorX lb = -VectorX::Ones(m + 1) * 1e5;
253
3/6
✓ Branch 1 taken 4013 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4013 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4013 times.
✗ Branch 8 not taken.
 4013 VectorX ub = VectorX::Ones(m + 1) * 1e10;
254
2/4
✓ Branch 1 taken 4013 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4013 times.
✗ Branch 5 not taken.
 4013 VectorX Alb = VectorX::Zero(6 + m);
255
3/6
✓ Branch 1 taken 4013 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4013 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4013 times.
✗ Branch 8 not taken.
 4013 VectorX Aub = VectorX::Ones(6 + m) * 1e100;
256
2/4
✓ Branch 1 taken 4013 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4013 times.
✗ Branch 5 not taken.
 4013 MatrixXX A = MatrixXX::Zero(6 + m, m + 1);
257
4/8
✓ Branch 1 taken 4013 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4013 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4013 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4013 times.
✗ Branch 11 not taken.
 4013 Alb.head<6>() = m_D * com + m_d;
258
3/6
✓ Branch 1 taken 4013 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4013 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4013 times.
✗ Branch 8 not taken.
 4013 Aub.head<6>() = Alb.head<6>();
259
2/4
✓ Branch 1 taken 4013 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4013 times.
✗ Branch 5 not taken.
 4013 A.topLeftCorner(6, m) = m_G_centr;
260
3/6
✓ Branch 1 taken 4013 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4013 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4013 times.
✗ Branch 8 not taken.
 4013 A.bottomLeftCorner(m, m) = MatrixXX::Identity(m, m);
261
4/8
✓ Branch 1 taken 4013 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4013 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4013 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4013 times.
✗ Branch 11 not taken.
 4013 A.bottomRightCorner(m, 1) = -VectorX::Ones(m);
262
263
8/16
✓ Branch 1 taken 4013 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4013 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4013 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4013 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 4013 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 4013 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 4013 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 4013 times.
✗ Branch 23 not taken.
 4013 LP_status lpStatus = m_solver->solve(c, lb, ub, A, Alb, Aub, b_b0);
264
1/2
✓ Branch 0 taken 4013 times.
✗ Branch 1 not taken.
 4013 if (lpStatus == LP_STATUS_OPTIMAL) {
265
2/4
✓ Branch 1 taken 4013 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4013 times.
✗ Branch 5 not taken.
 4013 robustness = convert_b0_to_emax(-1.0 * m_solver->getObjectiveValue());
266 4013 return lpStatus;
267 }
268
269 ✗ SEND_DEBUG_MSG("Primal LP problem could not be solved: " +
270 toString(lpStatus));
271 ✗ return lpStatus;
272 4013 }
273
274
2/2
✓ Branch 0 taken 2001 times.
✓ Branch 1 taken 2001 times.
 4002 if (m_algorithm == EQUILIBRIUM_ALGORITHM_LP2) {
275 /* Compute the robustness measure of the equilibrium of a specified CoM
276 position
277 * by solving the following LP:
278 find b, b0
279 minimize -b0
280 subject to D c + d <= G (b + 1*b0) <= D c + d
281 0 <= b <= Inf
282 where
283 b are the (delta) coefficient of the contact force
284 generators (f = G (b+b0)) b0 is the robustness measure c is
285 the CoM position G is the matrix whose columns are the
286 gravito-inertial wrench generators
287 */
288
1/2
✓ Branch 1 taken 2001 times.
✗ Branch 2 not taken.
 2001 VectorX b_b0(m + 1);
289
2/4
✓ Branch 1 taken 2001 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2001 times.
✗ Branch 5 not taken.
 2001 VectorX c = VectorX::Zero(m + 1);
290
1/2
✓ Branch 1 taken 2001 times.
✗ Branch 2 not taken.
 2001 c(m) = -1.0;
291
2/4
✓ Branch 1 taken 2001 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2001 times.
✗ Branch 5 not taken.
 2001 VectorX lb = VectorX::Zero(m + 1);
292
1/2
✓ Branch 1 taken 2001 times.
✗ Branch 2 not taken.
 2001 lb(m) = -1e10;
293
3/6
✓ Branch 1 taken 2001 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2001 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2001 times.
✗ Branch 8 not taken.
 2001 VectorX ub = VectorX::Ones(m + 1) * 1e10;
294
2/4
✓ Branch 1 taken 2001 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2001 times.
✗ Branch 5 not taken.
 2001 MatrixXX A = MatrixXX::Zero(6, m + 1);
295
3/6
✓ Branch 1 taken 2001 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2001 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2001 times.
✗ Branch 8 not taken.
 2001 Vector6 Alb = m_D * com + m_d;
296
1/2
✓ Branch 1 taken 2001 times.
✗ Branch 2 not taken.
 2001 Vector6 Aub = Alb;
297
2/4
✓ Branch 1 taken 2001 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2001 times.
✗ Branch 5 not taken.
 2001 A.leftCols(m) = m_G_centr;
298
4/8
✓ Branch 1 taken 2001 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2001 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2001 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2001 times.
✗ Branch 11 not taken.
 2001 A.rightCols(1) = m_G_centr * VectorX::Ones(m);
299
300
8/16
✓ Branch 1 taken 2001 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2001 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2001 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2001 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2001 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 2001 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 2001 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 2001 times.
✗ Branch 23 not taken.
 2001 LP_status lpStatus_primal = m_solver->solve(c, lb, ub, A, Alb, Aub, b_b0);
301
1/2
✓ Branch 0 taken 2001 times.
✗ Branch 1 not taken.
 2001 if (lpStatus_primal == LP_STATUS_OPTIMAL) {
302
2/4
✓ Branch 1 taken 2001 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2001 times.
✗ Branch 5 not taken.
 2001 robustness = convert_b0_to_emax(-1.0 * m_solver->getObjectiveValue());
303 2001 return lpStatus_primal;
304 }
305
306 ✗ SEND_DEBUG_MSG("Primal LP problem could not be solved: " +
307 toString(lpStatus_primal));
308 ✗ return lpStatus_primal;
309 2001 }
310
311
1/2
✓ Branch 0 taken 2001 times.
✗ Branch 1 not taken.
 2001 if (m_algorithm == EQUILIBRIUM_ALGORITHM_DLP) {
312 /*Compute the robustness measure of the equilibrium of a specified CoM
313 position by solving the following dual LP: find v minimize
314 (d+D*com)' v subject to G' v >= 0 1' G' v = 1 where
315 -(d+D c)' v is the robustness measure
316 c is the CoM position
317 G is the matrix whose columns are the gravito-inertial
318 wrench generators
319 */
320
1/2
✓ Branch 1 taken 2001 times.
✗ Branch 2 not taken.
 2001 Vector6 v;
321
3/6
✓ Branch 1 taken 2001 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2001 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2001 times.
✗ Branch 8 not taken.
 2001 Vector6 c = m_D * com + m_d;
322
3/6
✓ Branch 1 taken 2001 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2001 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2001 times.
✗ Branch 8 not taken.
 2001 Vector6 lb = Vector6::Ones() * -1e100;
323
3/6
✓ Branch 1 taken 2001 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2001 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2001 times.
✗ Branch 8 not taken.
 2001 Vector6 ub = Vector6::Ones() * 1e100;
324
2/4
✓ Branch 1 taken 2001 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2001 times.
✗ Branch 5 not taken.
 2001 VectorX Alb = VectorX::Zero(m + 1);
325
1/2
✓ Branch 1 taken 2001 times.
✗ Branch 2 not taken.
 2001 Alb(m) = 1.0;
326
3/6
✓ Branch 1 taken 2001 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2001 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2001 times.
✗ Branch 8 not taken.
 2001 VectorX Aub = VectorX::Ones(m + 1) * 1e100;
327
1/2
✓ Branch 1 taken 2001 times.
✗ Branch 2 not taken.
 2001 Aub(m) = 1.0;
328
1/2
✓ Branch 1 taken 2001 times.
✗ Branch 2 not taken.
 2001 MatrixX6 A(m + 1, 6);
329
3/6
✓ Branch 1 taken 2001 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2001 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2001 times.
✗ Branch 8 not taken.
 2001 A.topRows(m) = m_G_centr.transpose();
330
5/10
✓ Branch 1 taken 2001 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2001 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2001 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2001 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2001 times.
✗ Branch 14 not taken.
 2001 A.bottomRows<1>() = (m_G_centr * VectorX::Ones(m)).transpose();
331
332
8/16
✓ Branch 1 taken 2001 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2001 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2001 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2001 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2001 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 2001 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 2001 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 2001 times.
✗ Branch 23 not taken.
 2001 LP_status lpStatus_dual = m_solver->solve(c, lb, ub, A, Alb, Aub, v);
333
1/2
✓ Branch 0 taken 2001 times.
✗ Branch 1 not taken.
 2001 if (lpStatus_dual == LP_STATUS_OPTIMAL) {
334
2/4
✓ Branch 1 taken 2001 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2001 times.
✗ Branch 5 not taken.
 2001 robustness = convert_b0_to_emax(m_solver->getObjectiveValue());
335 2001 return lpStatus_dual;
336 }
337 ✗ SEND_DEBUG_MSG("Dual LP problem for com position " +
338 toString(com.transpose()) +
339 " could not be solved: " + toString(lpStatus_dual));
340
341 // switch UNFEASIBLE and UNBOUNDED flags because we are solving dual problem
342 ✗ if (lpStatus_dual == LP_STATUS_INFEASIBLE)
343 ✗ lpStatus_dual = LP_STATUS_UNBOUNDED;
344 ✗ else if (lpStatus_dual == LP_STATUS_UNBOUNDED)
345 ✗ lpStatus_dual = LP_STATUS_INFEASIBLE;
346
347 ✗ return lpStatus_dual;
348 2001 }
349
350 ✗ SEND_ERROR_MSG(
351 "computeEquilibriumRobustness is not implemented for the specified "
352 "algorithm");
353 ✗ return LP_STATUS_ERROR;
354 }
355
356 3001 LP_status Equilibrium::checkRobustEquilibrium(Cref_vector3 com,
357 bool& equilibrium, double e_max) {
358
1/2
✓ Branch 2 taken 3001 times.
✗ Branch 3 not taken.
 3001 return checkRobustEquilibrium(com, zero_acc, equilibrium, e_max);
359 }
360
361 3001 LP_status Equilibrium::checkRobustEquilibrium(Cref_vector3 com,
362 Cref_vector3 acc,
363 bool& equilibrium, double e_max) {
364 // Take the acceleration in account in D and d :
365
6/12
✓ Branch 1 taken 3001 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3001 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3001 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3001 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 3001 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 3001 times.
✗ Branch 17 not taken.
 3001 m_D.block<3, 3>(3, 0) = crossMatrix(-m_mass * (m_gravity - acc));
366
4/8
✓ Branch 1 taken 3001 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3001 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3001 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3001 times.
✗ Branch 11 not taken.
 3001 m_d.head<3>() = m_mass * (m_gravity - acc);
367
2/4
✓ Branch 1 taken 3001 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3001 times.
✗ Branch 5 not taken.
 3001 m_HD = m_H * m_D;
368
2/4
✓ Branch 1 taken 3001 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3001 times.
✗ Branch 5 not taken.
 3001 m_Hd = m_H * m_d;
369
370
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 3001 times.
 3001 if (m_G_centr.cols() == 0) {
371 ✗ equilibrium = false;
372 ✗ return LP_STATUS_OPTIMAL;
373 }
374
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 3001 times.
 3001 if (e_max != 0.0) {
375 ✗ SEND_ERROR_MSG("checkRobustEquilibrium with e_max!=0 not implemented yet");
376 ✗ return LP_STATUS_ERROR;
377 }
378
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 3001 times.
 3001 if (m_algorithm != EQUILIBRIUM_ALGORITHM_PP) {
379 ✗ SEND_ERROR_MSG(
380 "checkRobustEquilibrium is only implemented for the PP algorithm");
381 ✗ return LP_STATUS_ERROR;
382 }
383
384
3/6
✓ Branch 1 taken 3001 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3001 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3001 times.
✗ Branch 8 not taken.
 3001 VectorX res = m_HD * com + m_Hd;
385
2/2
✓ Branch 1 taken 116977 times.
✓ Branch 2 taken 232 times.
 117209 for (long i = 0; i < res.size(); i++)
386
3/4
✓ Branch 1 taken 116977 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 2769 times.
✓ Branch 4 taken 114208 times.
 116977 if (res(i) > 0.0) {
387 2769 equilibrium = false;
388 2769 return LP_STATUS_OPTIMAL;
389 }
390
391 232 equilibrium = true;
392 232 return LP_STATUS_OPTIMAL;
393 3001 }
394
395 1 LP_status Equilibrium::getPolytopeInequalities(MatrixXX& H, VectorX& h) const {
396
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 if (m_algorithm != EQUILIBRIUM_ALGORITHM_PP) {
397 ✗ SEND_ERROR_MSG(
398 "getPolytopeInequalities is only implemented for the PP algorithm");
399 ✗ return LP_STATUS_ERROR;
400 }
401
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 if (!m_is_cdd_stable) {
402 ✗ SEND_ERROR_MSG("numerical instability in cddlib");
403 ✗ return LP_STATUS_ERROR;
404 }
405
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 if (m_G_centr.cols() == 0) {
406 ✗ return LP_STATUS_INFEASIBLE;
407 }
408 1 H = m_H;
409 1 h = m_h;
410 1 return LP_STATUS_OPTIMAL;
411 }
412
413 1000 LP_status Equilibrium::findExtremumOverLine(Cref_vector3 a, Cref_vector3 a0,
414 double e_max, Ref_vector3 com) {
415 const long m =
416 1000 m_G_centr.cols(); // number of gravito-inertial wrench generators
417
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1000 times.
 1000 if (m_G_centr.cols() == 0) return LP_STATUS_INFEASIBLE;
418
419
1/2
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
 1000 double b0 = convert_emax_to_b0(e_max);
420
421
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1000 times.
 1000 if (m_algorithm == EQUILIBRIUM_ALGORITHM_LP) {
422 /* Compute the extremum CoM position over the line a*p + a0 that is in
423 robust equilibrium
424 * by solving the following LP:
425 find b, p
426 minimize -p
427 subject to D (a p + a0) + d <= G (b + b0) <= D (a p + a0) + d
428 0 <= b <= Inf
429 where
430 b0+b are the coefficient of the contact force generators (f = G
431 (b0+b)) b0 is the robustness measure p is the line parameter
432 G is the matrix whose columns are the gravito-inertial wrench
433 generators
434 */
435 ✗ VectorX b_p(m + 1);
436 ✗ VectorX c = VectorX::Zero(m + 1);
437 ✗ c(m) = -1.0;
438 ✗ VectorX lb = -VectorX::Zero(m + 1);
439 ✗ lb(m) = -1e5;
440 ✗ VectorX ub = VectorX::Ones(m + 1) * 1e10;
441 ✗ Vector6 Alb = m_D * a0 + m_d - m_G_centr * VectorX::Ones(m) * b0;
442 ✗ Vector6 Aub = Alb;
443 ✗ Matrix6X A = Matrix6X::Zero(6, m + 1);
444 ✗ A.leftCols(m) = m_G_centr;
445 ✗ A.rightCols(1) = -m_D * a;
446
447 ✗ LP_status lpStatus_primal = m_solver->solve(c, lb, ub, A, Alb, Aub, b_p);
448 ✗ if (lpStatus_primal == LP_STATUS_OPTIMAL) {
449 ✗ com = a0 + a * b_p(m);
450
451 // #define WRITE_LPS_TO_FILE
452 #ifdef WRITE_LPS_TO_FILE
453 string date_time = getDateAndTimeAsString();
454 string filename = "LP_findExtremumOverLine" + date_time + ".dat";
455 if (!m_solver->writeLpToFile(filename.c_str(), c, lb, ub, A, Alb, Aub))
456 SEND_ERROR_MSG("Error while writing LP to file " + filename);
457 filename = "LP_findExtremumOverLine" + date_time + "_solution.dat";
458 if (!writeMatrixToFile(filename.c_str(), b_p))
459 SEND_ERROR_MSG("Error while writing LP solution to file " + filename);
460 #endif
461
462 ✗ return lpStatus_primal;
463 }
464
465 ✗ com = a0;
466 ✗ SEND_DEBUG_MSG(
467 "Primal LP problem could not be solved suggesting that no equilibrium "
468 "position with robustness " +
469 toString(e_max) + " exists over the line starting from " +
470 toString(a0.transpose()) + " in direction " + toString(a.transpose()) +
471 ", solver error code: " + toString(lpStatus_primal));
472 ✗ return lpStatus_primal;
473 }
474
475
1/2
✓ Branch 0 taken 1000 times.
✗ Branch 1 not taken.
 1000 if (m_algorithm == EQUILIBRIUM_ALGORITHM_DLP) {
476 /* Compute the extremum CoM position over the line a*x + a0 that is in
477 robust equilibrium
478 * by solving the following dual LP:
479 find v
480 minimize (D a0 + d -G b0)' v
481 subject to 0 <= G' v <= Inf
482 -1 <= a' D' v <= -1
483 where
484 G is the matrix whose columns are the gravito-inertial wrench
485 generators
486 */
487
1/2
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
 1000 Vector6 v;
488
7/14
✓ 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.
✓ Branch 19 taken 1000 times.
✗ Branch 20 not taken.
 1000 Vector6 c = m_D * a0 + m_d - m_G_centr * VectorX::Ones(m) * b0;
489
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 Vector6 lb = Vector6::Ones() * -1e10;
490
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 Vector6 ub = Vector6::Ones() * 1e10;
491
2/4
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1000 times.
✗ Branch 5 not taken.
 1000 VectorX Alb = VectorX::Zero(m + 1);
492
1/2
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
 1000 Alb(m) = -1.0;
493
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 VectorX Aub = VectorX::Ones(m + 1) * 1e10;
494
1/2
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
 1000 Aub(m) = -1.0;
495
1/2
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
 1000 MatrixX6 A(m + 1, 6);
496
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 A.topRows(m) = m_G_centr.transpose();
497
4/8
✓ 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.
 1000 A.bottomRows<1>() = (m_D * a).transpose();
498
499
8/16
✓ 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.
✓ Branch 19 taken 1000 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1000 times.
✗ Branch 23 not taken.
 1000 LP_status lpStatus_dual = m_solver->solve(c, lb, ub, A, Alb, Aub, v);
500
1/2
✓ Branch 0 taken 1000 times.
✗ Branch 1 not taken.
 1000 if (lpStatus_dual == LP_STATUS_OPTIMAL) {
501
1/2
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
 1000 double p = m_solver->getObjectiveValue();
502
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 com = a0 + a * p;
503
504 #ifdef WRITE_LPS_TO_FILE
505 string date_time = getDateAndTimeAsString();
506 string filename = "DLP_findExtremumOverLine" + date_time + ".dat";
507 if (!m_solver->writeLpToFile(filename.c_str(), c, lb, ub, A, Alb, Aub))
508 SEND_ERROR_MSG("Error while writing LP to file " + filename);
509 filename = "DLP_findExtremumOverLine" + date_time + "_solution.dat";
510 if (!writeMatrixToFile(filename.c_str(), v))
511 SEND_ERROR_MSG("Error while writing LP solution to file " + filename);
512 #endif
513
514 // since QP oases cannot detect unboundedness we check here whether the
515 // objective value is a large negative value
516
2/4
✓ Branch 0 taken 1000 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 1000 times.
 1000 if (m_solver_type == SOLVER_LP_QPOASES && p < -1e7) {
517 ✗ SEND_DEBUG_MSG("Dual LP problem with robustness " + toString(e_max) +
518 " over the line starting from " +
519 toString(a0.transpose()) + " in direction " +
520 toString(a.transpose()) +
521 " has large negative objective value: " + toString(p) +
522 " suggesting it is probably unbounded.");
523 ✗ lpStatus_dual = LP_STATUS_UNBOUNDED;
524 }
525
526 1000 return lpStatus_dual;
527 }
528
529 ✗ com = a0;
530 ✗ SEND_DEBUG_MSG(
531 "Dual LP problem could not be solved suggesting that no equilibrium "
532 "position with robustness " +
533 toString(e_max) + " exists over the line starting from " +
534 toString(a0.transpose()) + " in direction " + toString(a.transpose()) +
535 ", solver error code: " + toString(lpStatus_dual));
536
537 // switch UNFEASIBLE and UNBOUNDED flags because we are solving dual problem
538 ✗ if (lpStatus_dual == LP_STATUS_INFEASIBLE)
539 ✗ lpStatus_dual = LP_STATUS_UNBOUNDED;
540 ✗ else if (lpStatus_dual == LP_STATUS_UNBOUNDED)
541 ✗ lpStatus_dual = LP_STATUS_INFEASIBLE;
542
543 ✗ return lpStatus_dual;
544 1000 }
545
546 ✗ SEND_ERROR_MSG(
547 "findExtremumOverLine is not implemented for the specified algorithm");
548 ✗ return LP_STATUS_ERROR;
549 }
550
551 ✗ LP_status Equilibrium::findExtremumInDirection(Cref_vector3 /*direction*/,
552 Ref_vector3 /*com*/,
553 double /*e_max*/) {
554 ✗ if (m_G_centr.cols() == 0) return LP_STATUS_INFEASIBLE;
555 ✗ SEND_ERROR_MSG("findExtremumInDirection not implemented yet");
556 ✗ return LP_STATUS_ERROR;
557 }
558 11 bool Equilibrium::computePolytopeProjection(Cref_matrix6X v) {
559 // todo: for the moment using ad hoc upper bound = 500 N
560 11 int n = (int)v.rows();
561 11 int m = (int)v.cols();
562
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 11 times.
 11 if (n > m) {
563 ✗ SEND_ERROR_MSG("V has more lines that columns, this case is not handled!");
564 ✗ return false;
565 }
566 // getProfiler().start("eigen_to_cdd");
567 dd_MatrixPtr V =
568
3/6
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 11 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 11 times.
✗ Branch 8 not taken.
 11 cone_span_eigen_to_cdd(v.transpose(), canonicalize_cdd_matrix);
569 // getProfiler().stop("eigen_to_cdd");
570
571 11 dd_ErrorType error = dd_NoError;
572 11 m_is_cdd_stable = true;
573
574 // getProfiler().start("dd_DDMatrix2Poly");
575
1/2
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
 11 dd_PolyhedraPtr H_ = dd_DDMatrix2Poly(V, &error);
576 // getProfiler().stop("dd_DDMatrix2Poly");
577
578
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 11 times.
 11 if (error != dd_NoError) {
579 ✗ SEND_ERROR_MSG("numerical instability in cddlib. ill formed polytope");
580 ✗ m_is_cdd_stable = false;
581 ✗ return false;
582 }
583
584 // getProfiler().start("cdd to eigen");
585
1/2
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
 11 dd_MatrixPtr b_A = dd_CopyInequalities(H_);
586
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 10 times.
 11 if (canonicalize_cdd_matrix) {
587 1 dd_ErrorType error = dd_NoError;
588 dd_rowset redset, impl_linset;
589 dd_rowindex newpos;
590
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 dd_MatrixCanonicalize(&b_A, &impl_linset, &redset, &newpos, &error);
591
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 set_free(redset);
592
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 set_free(impl_linset);
593 1 free(newpos);
594 }
595 // get equalities and add them as complementary inequality constraints
596 11 std::vector<long> eq_rows;
597
2/2
✓ Branch 0 taken 2361 times.
✓ Branch 1 taken 11 times.
 2372 for (long elem = 1; elem <= (long)(b_A->linset[0]); ++elem) {
598
2/6
✓ Branch 1 taken 2361 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 2361 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 2361 if (set_member(elem, b_A->linset)) eq_rows.push_back(elem);
599 }
600 11 int rowsize = (int)b_A->rowsize;
601
1/2
✓ Branch 2 taken 11 times.
✗ Branch 3 not taken.
 11 m_H.resize(rowsize + eq_rows.size(), (int)b_A->colsize - 1);
602
1/2
✓ Branch 2 taken 11 times.
✗ Branch 3 not taken.
 11 m_h.resize(rowsize + eq_rows.size());
603
2/2
✓ Branch 0 taken 2361 times.
✓ Branch 1 taken 11 times.
 2372 for (int i = 0; i < rowsize; ++i) {
604
1/2
✓ Branch 1 taken 2361 times.
✗ Branch 2 not taken.
 2361 m_h(i) = (value_type)(*(b_A->matrix[i][0]));
605
2/2
✓ Branch 0 taken 14166 times.
✓ Branch 1 taken 2361 times.
 16527 for (int j = 1; j < b_A->colsize; ++j)
606
1/2
✓ Branch 1 taken 14166 times.
✗ Branch 2 not taken.
 14166 m_H(i, j - 1) = -(value_type)(*(b_A->matrix[i][j]));
607 }
608 11 int i = 0;
609 11 for (std::vector<long int>::const_iterator cit = eq_rows.begin();
610
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 11 times.
 11 cit != eq_rows.end(); ++cit, ++i) {
611 ✗ m_h(rowsize + i) = -m_h((int)(*cit));
612 ✗ m_H(rowsize + i) = -m_H((int)(*cit));
613 }
614 // getProfiler().stop("cdd to eigen");
615
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 11 times.
 11 if (m_h.rows() < n) {
616 ✗ SEND_ERROR_MSG("numerical instability in cddlib. ill formed polytope");
617 ✗ m_is_cdd_stable = false;
618 ✗ return false;
619 }
620
621 11 return true;
622 11 }
623
624 8015 double Equilibrium::convert_b0_to_emax(double b0) {
625 8015 return (b0 * m_b0_to_emax_coefficient);
626 }
627
628 1000 double Equilibrium::convert_emax_to_b0(double emax) {
629 1000 return (emax / m_b0_to_emax_coefficient);
630 }
631
632 ✗ LP_status Equilibrium::findMaximumAcceleration(Cref_matrix63 H, Cref_vector6 h,
633 Cref_vector3 v, double& alpha0) {
634 ✗ int m = (int)m_G_centr.cols(); // 4* number of contacts
635 ✗ VectorX b_a0(m);
636 ✗ VectorX c = VectorX::Zero(m);
637 ✗ MatrixXX A = MatrixXX::Zero(6, m);
638 ✗ A.topLeftCorner(6, m) = -m_G_centr;
639 ✗ A.topRightCorner(6, 1) = H * v;
640 ✗ c(m - 1) = -1.0; // because we search max alpha0, and c = [ B alpha ]^t
641 ✗ VectorX lb = VectorX::Zero(m);
642 ✗ VectorX ub = VectorX::Ones(m) * 1e10; // Inf
643 ✗ VectorX Alb = -h;
644 ✗ VectorX Aub = -h;
645
646 ✗ int iter = 0;
647 LP_status lpStatus;
648 do {
649 ✗ lpStatus = m_solver->solve(c, lb, ub, A, Alb, Aub, b_a0);
650 ✗ iter++;
651 ✗ } while (lpStatus == LP_STATUS_ERROR && iter < 3);
652
653 ✗ if (lpStatus == LP_STATUS_UNBOUNDED) {
654 // SEND_DEBUG_MSG("Primal LP problem is unbounded : "+toString(lpStatus));
655 ✗ alpha0 = std::numeric_limits<double>::infinity();
656 ✗ return lpStatus;
657 }
658 ✗ if (lpStatus == LP_STATUS_OPTIMAL) {
659 ✗ alpha0 = -1.0 * m_solver->getObjectiveValue();
660 ✗ return lpStatus;
661 }
662 ✗ alpha0 = 0.0;
663 // SEND_DEBUG_MSG("Primal LP problem could not be solved:
664 // "+toString(lpStatus));
665 ✗ return lpStatus;
666 }
667
668 ✗ bool Equilibrium::checkAdmissibleAcceleration(Cref_matrix63 H, Cref_vector6 h,
669 Cref_vector3 a) {
670 ✗ int m = (int)m_G_centr.cols(); // number of contact * generator per contacts
671 ✗ VectorX b(m);
672 ✗ VectorX c = VectorX::Zero(m);
673 ✗ VectorX lb = VectorX::Zero(m);
674 ✗ VectorX ub = VectorX::Ones(m) * 1e10; // Inf
675 ✗ VectorX Alb = H * a + h;
676 ✗ VectorX Aub = H * a + h;
677 ✗ int iter = 0;
678 LP_status lpStatus;
679 do {
680 ✗ lpStatus = m_solver->solve(c, lb, ub, m_G_centr, Alb, Aub, b);
681 ✗ iter++;
682 ✗ } while (lpStatus == LP_STATUS_ERROR && iter < 3);
683
684 ✗ if (lpStatus == LP_STATUS_OPTIMAL || lpStatus == LP_STATUS_UNBOUNDED) {
685 ✗ return true;
686 } else {
687 // SEND_DEBUG_MSG("Primal LP problem could not be solved:
688 // "+toString(lpStatus));
689 ✗ return false;
690 }
691 }
692
693 } // end namespace centroidal_dynamics
694