| 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 |
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 11 not taken.
✓ Branch 12 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 |
2/4✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 11 times.
✗ Branch 5 not taken.
|
11 | m_HD = m_H * m_D; |
| 212 |
2/4✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 11 times.
✗ Branch 5 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 |
2/4✓ Branch 1 taken 8014 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8014 times.
✗ Branch 5 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 |
6/12✓ Branch 1 taken 8015 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8015 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8015 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 8015 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 8015 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 8015 times.
✗ Branch 17 not taken.
|
8015 | m_D.block<3, 3>(3, 0) = crossMatrix(-m_mass * (m_gravity - acc)); |
| 230 |
4/8✓ Branch 1 taken 8015 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8015 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8015 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 8015 times.
✗ Branch 11 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 |
2/4✓ Branch 1 taken 3001 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3001 times.
✗ Branch 5 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 102205 times.
✓ Branch 2 taken 190 times.
|
102395 | for (long i = 0; i < res.size(); i++) |
| 386 |
3/4✓ Branch 1 taken 102205 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 2811 times.
✓ Branch 4 taken 99394 times.
|
102205 | if (res(i) > 0.0) { |
| 387 | 2811 | equilibrium = false; | |
| 388 | 2811 | return LP_STATUS_OPTIMAL; | |
| 389 | } | ||
| 390 | |||
| 391 | 190 | equilibrium = true; | |
| 392 | 190 | 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 2406 times.
✓ Branch 1 taken 11 times.
|
2417 | for (long elem = 1; elem <= (long)(b_A->linset[0]); ++elem) { |
| 598 |
2/6✓ Branch 1 taken 2406 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 2406 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
|
2406 | 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 2406 times.
✓ Branch 1 taken 11 times.
|
2417 | for (int i = 0; i < rowsize; ++i) { |
| 604 |
1/2✓ Branch 1 taken 2406 times.
✗ Branch 2 not taken.
|
2406 | m_h(i) = (value_type)(*(b_A->matrix[i][0])); |
| 605 |
2/2✓ Branch 0 taken 14436 times.
✓ Branch 1 taken 2406 times.
|
16842 | for (int j = 1; j < b_A->colsize; ++j) |
| 606 |
1/2✓ Branch 1 taken 14436 times.
✗ Branch 2 not taken.
|
14436 | 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 |