GCC Code Coverage Report

Directory: ./
File: tests/test_helper.hh
Date: 2025-04-18 04:07:36
Exec Total Coverage
Lines: 131 133 98.5%
Branches: 212 422 50.2%
Line Branch Exec Source
1 #ifndef TEST_HELPER_HH
2 #define TEST_HELPER_HH
3
4 #include <boost/test/included/unit_test.hpp>
5 #include <hpp/bezier-com-traj/common_solve_methods.hh>
6 #include <hpp/bezier-com-traj/solve.hh>
7 #include <hpp/centroidal-dynamics/centroidal_dynamics.hh>
8
9 using bezier_com_traj::Matrix3;
10 using bezier_com_traj::MatrixX3;
11 using bezier_com_traj::MatrixXX;
12 using bezier_com_traj::Vector3;
13 using bezier_com_traj::VectorX;
14
15 #define MASS 50.
16 #define MU 0.5
17 #define LX 0.2172 // contact surface size in x direction
18 #define LY 0.138 // contact surface size in y direction
19 #define KIN_X_MIN -0.7
20 #define KIN_X_MAX 0.7
21 #define KIN_Y_MIN -0.4
22 #define KIN_Y_MAX 0.4
23 #define KIN_Z_MIN 0
24 #define KIN_Z_MAX 0.9
25 #define EPSILON 1e-6
26
27 typedef std::pair<MatrixX3, VectorX> ConstraintsPair;
28
29 2635 std::pair<MatrixX3, MatrixX3> generateKinematicsConstraints() {
30 // generate a simple polytgone : faces aligned along x,y,z axis
31 // size : x [-0.5,0.5] ; y = [-0.3,1] ; z [0,0.8]
32
1/2
✓ Branch 1 taken 2635 times.
✗ Branch 2 not taken.
 2635 MatrixX3 N(6, 3);
33
1/2
✓ Branch 1 taken 2635 times.
✗ Branch 2 not taken.
 2635 MatrixX3 V(6, 3);
34
35
3/6
✓ Branch 1 taken 2635 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2635 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2635 times.
✗ Branch 8 not taken.
 2635 N.block<1, 3>(0, 0) = Vector3(-1, 0, 0);
36
3/6
✓ Branch 1 taken 2635 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2635 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2635 times.
✗ Branch 8 not taken.
 2635 V.block<1, 3>(0, 0) = Vector3(KIN_X_MIN, KIN_Y_MAX, 0);
37
3/6
✓ Branch 1 taken 2635 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2635 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2635 times.
✗ Branch 8 not taken.
 2635 N.block<1, 3>(1, 0) = Vector3(0, 1, 0);
38
3/6
✓ Branch 1 taken 2635 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2635 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2635 times.
✗ Branch 8 not taken.
 2635 V.block<1, 3>(1, 0) = Vector3(KIN_X_MIN, KIN_Y_MAX, 0);
39
3/6
✓ Branch 1 taken 2635 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2635 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2635 times.
✗ Branch 8 not taken.
 2635 N.block<1, 3>(2, 0) = Vector3(1, 0, 0);
40
3/6
✓ Branch 1 taken 2635 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2635 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2635 times.
✗ Branch 8 not taken.
 2635 V.block<1, 3>(2, 0) = Vector3(KIN_X_MAX, KIN_Y_MIN, 0);
41
3/6
✓ Branch 1 taken 2635 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2635 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2635 times.
✗ Branch 8 not taken.
 2635 N.block<1, 3>(3, 0) = Vector3(0, -1, 0);
42
3/6
✓ Branch 1 taken 2635 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2635 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2635 times.
✗ Branch 8 not taken.
 2635 V.block<1, 3>(3, 0) = Vector3(KIN_X_MAX, KIN_Y_MIN, 0);
43
3/6
✓ Branch 1 taken 2635 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2635 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2635 times.
✗ Branch 8 not taken.
 2635 N.block<1, 3>(4, 0) = Vector3(0, 0, -1);
44
3/6
✓ Branch 1 taken 2635 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2635 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2635 times.
✗ Branch 8 not taken.
 2635 V.block<1, 3>(4, 0) = Vector3(0, 0, KIN_Z_MIN);
45
3/6
✓ Branch 1 taken 2635 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2635 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2635 times.
✗ Branch 8 not taken.
 2635 N.block<1, 3>(5, 0) = Vector3(0, 0, 1);
46
3/6
✓ Branch 1 taken 2635 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2635 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2635 times.
✗ Branch 8 not taken.
 2635 V.block<1, 3>(5, 0) = Vector3(0, 0, KIN_Z_MAX);
47
48
1/2
✓ Branch 1 taken 2635 times.
✗ Branch 2 not taken.
 5270 return std::make_pair(N, V);
49 2635 }
50
51 2635 std::pair<MatrixXX, VectorX> generateKinematicsConstraints(
52 Matrix3 endEffRotation, Vector3 endEffTranslation) {
53
1/2
✓ Branch 1 taken 2635 times.
✗ Branch 2 not taken.
 2635 std::pair<MatrixX3, MatrixX3> NV = generateKinematicsConstraints();
54
1/2
✓ Branch 1 taken 2635 times.
✗ Branch 2 not taken.
 2635 MatrixX3 N = NV.first;
55
1/2
✓ Branch 1 taken 2635 times.
✗ Branch 2 not taken.
 2635 MatrixX3 V = NV.second;
56 2635 size_t numFaces = N.rows();
57
1/2
✓ Branch 1 taken 2635 times.
✗ Branch 2 not taken.
 2635 MatrixX3 A(numFaces, 3);
58
1/2
✓ Branch 1 taken 2635 times.
✗ Branch 2 not taken.
 2635 VectorX b(numFaces);
59
2/4
✓ Branch 1 taken 2635 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2635 times.
✗ Branch 5 not taken.
 2635 VectorX n, v;
60
61
2/2
✓ Branch 0 taken 15810 times.
✓ Branch 1 taken 2635 times.
 18445 for (size_t i = 0; i < numFaces; ++i) {
62
4/8
✓ Branch 1 taken 15810 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15810 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 15810 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 15810 times.
✗ Branch 11 not taken.
 15810 n = endEffRotation * (N.block<1, 3>(i, 0).transpose());
63
5/10
✓ Branch 1 taken 15810 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15810 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 15810 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 15810 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 15810 times.
✗ Branch 14 not taken.
 15810 v = endEffRotation * (V.block<1, 3>(i, 0).transpose()) + endEffTranslation;
64
2/4
✓ Branch 1 taken 15810 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15810 times.
✗ Branch 5 not taken.
 15810 A.block<1, 3>(i, 0) = n;
65
2/4
✓ Branch 1 taken 15810 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15810 times.
✗ Branch 5 not taken.
 15810 b[i] = v.dot(n);
66 }
67
68
2/4
✓ Branch 1 taken 2635 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2635 times.
✗ Branch 5 not taken.
 7905 return std::make_pair(A, b);
69 2635 }
70
71 2594 std::pair<MatrixX3, MatrixX3> computeRectangularContacts(MatrixX3 normals,
72 MatrixX3 positions,
73 double size_X,
74 double size_Y) {
75 // TODO : consider normal != z (see code in rbprm :: stability.cc (or add it
76 // as dependency ?)
77
78
6/12
✓ Branch 1 taken 2594 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2594 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 2594 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 2594 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 2594 times.
✗ Branch 20 not taken.
✗ Branch 24 not taken.
✓ Branch 25 taken 2594 times.
 2594 BOOST_CHECK(normals.rows() == positions.rows());
79
1/2
✓ Branch 2 taken 2594 times.
✗ Branch 3 not taken.
 2594 MatrixX3 rec_normals(normals.rows() * 4, 3);
80
1/2
✓ Branch 2 taken 2594 times.
✗ Branch 3 not taken.
 2594 MatrixX3 rec_positions(normals.rows() * 4, 3);
81
82 2594 double lx = size_X / 2.;
83 2594 double ly = size_Y / 2.;
84
1/2
✓ Branch 1 taken 2594 times.
✗ Branch 2 not taken.
 2594 MatrixX3 p(4, 3);
85
12/24
✓ Branch 1 taken 2594 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2594 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2594 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2594 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2594 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 2594 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 2594 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 2594 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 2594 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 2594 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 2594 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 2594 times.
✗ Branch 35 not taken.
 2594 p << lx, ly, 0, lx, -ly, 0, -lx, -ly, 0, -lx, ly, 0;
86
87
2/2
✓ Branch 1 taken 2660 times.
✓ Branch 2 taken 2594 times.
 5254 for (long int ic = 0; ic < normals.rows(); ++ic) {
88
2/2
✓ Branch 0 taken 10640 times.
✓ Branch 1 taken 2660 times.
 13300 for (long int i = 0; i < 4; ++i) {
89
3/6
✓ Branch 1 taken 10640 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10640 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10640 times.
✗ Branch 8 not taken.
 10640 rec_normals.block<1, 3>(ic * 4 + i, 0) = normals.block<1, 3>(ic, 0);
90
1/2
✓ Branch 1 taken 10640 times.
✗ Branch 2 not taken.
 10640 rec_positions.block<1, 3>(ic * 4 + i, 0) =
91
4/8
✓ Branch 1 taken 10640 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10640 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10640 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10640 times.
✗ Branch 11 not taken.
 21280 positions.block<1, 3>(ic, 0) + p.block<1, 3>(i, 0);
92 }
93 }
94
1/2
✓ Branch 1 taken 2594 times.
✗ Branch 2 not taken.
 5188 return std::make_pair(rec_normals, rec_positions);
95 2594 }
96
97 2594 centroidal_dynamics::Equilibrium ComputeContactCone(
98 MatrixX3 normals, MatrixX3 positions,
99 const centroidal_dynamics::EquilibriumAlgorithm algo =
100 centroidal_dynamics::EQUILIBRIUM_ALGORITHM_PP) {
101 centroidal_dynamics::Equilibrium contactCone(
102 "test-quasiStatic", MASS, 4, centroidal_dynamics::SOLVER_LP_QPOASES, true,
103
2/4
✓ Branch 2 taken 2594 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2594 times.
✗ Branch 6 not taken.
 5188 10, false);
104 // centroidal_dynamics::EquilibriumAlgorithm alg =
105 // centroidal_dynamics::EQUILIBRIUM_ALGORITHM_PP;
106
1/2
✓ Branch 1 taken 2594 times.
✗ Branch 2 not taken.
 2594 contactCone.setNewContacts(positions, normals, MU, algo);
107 2594 return contactCone;
108 }
109
110 2505 std::pair<MatrixXX, VectorX> generateStabilityConstraints(
111 centroidal_dynamics::Equilibrium contactPhase,
112 Vector3 acc = Vector3::Zero()) {
113 2505 const Vector3& g = contactPhase.m_gravity;
114
2/4
✓ Branch 1 taken 2505 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2505 times.
✗ Branch 5 not taken.
 2505 const Matrix3 gSkew = bezier_com_traj::skew(g);
115
2/4
✓ Branch 1 taken 2505 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2505 times.
✗ Branch 5 not taken.
 2505 const Matrix3 accSkew = bezier_com_traj::skew(acc);
116 // compute GIWC
117
1/2
✓ Branch 1 taken 2505 times.
✗ Branch 2 not taken.
 2505 centroidal_dynamics::MatrixXX Hrow;
118
1/2
✓ Branch 1 taken 2505 times.
✗ Branch 2 not taken.
 2505 VectorX h;
119
1/2
✓ Branch 1 taken 2505 times.
✗ Branch 2 not taken.
 2505 contactPhase.getPolytopeInequalities(Hrow, h);
120
2/4
✓ Branch 1 taken 2505 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2505 times.
✗ Branch 5 not taken.
 2505 MatrixXX H = -Hrow;
121
2/4
✓ Branch 1 taken 2505 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2505 times.
✗ Branch 5 not taken.
 2505 H.rowwise().normalize();
122 2505 int dimH = (int)(H.rows());
123
2/4
✓ Branch 1 taken 2505 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2505 times.
✗ Branch 5 not taken.
 2505 MatrixXX mH = contactPhase.m_mass * H;
124 // constraints : mH[:,3:6] g^ x <= h + mH[:,0:3]g
125 // A = mH g^
126 // b = h + mHg
127
4/8
✓ Branch 1 taken 2505 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2505 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2505 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2505 times.
✗ Branch 11 not taken.
 2505 MatrixX3 A = mH.block(0, 3, dimH, 3) * (gSkew - accSkew);
128
5/10
✓ Branch 1 taken 2505 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2505 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2505 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2505 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2505 times.
✗ Branch 14 not taken.
 2505 VectorX b = h + mH.block(0, 0, dimH, 3) * (g - acc);
129
2/4
✓ Branch 1 taken 2505 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2505 times.
✗ Branch 5 not taken.
 7515 return std::make_pair(A, b);
130 2505 }
131
132 2501 std::pair<MatrixXX, VectorX> generateStabilityConstraints(
133 MatrixX3 normals, MatrixX3 positions, Vector3 acc = Vector3::Zero(),
134 const centroidal_dynamics::EquilibriumAlgorithm algo =
135 centroidal_dynamics::EQUILIBRIUM_ALGORITHM_PP) {
136 std::pair<MatrixX3, MatrixX3> contacts =
137
3/6
✓ Branch 1 taken 2501 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2501 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2501 times.
✗ Branch 8 not taken.
 5002 computeRectangularContacts(normals, positions, LX, LY);
138 centroidal_dynamics::Equilibrium contactPhase =
139
3/6
✓ Branch 1 taken 2501 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2501 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2501 times.
✗ Branch 8 not taken.
 5002 ComputeContactCone(contacts.first, contacts.second, algo);
140
3/6
✓ Branch 1 taken 2501 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2501 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2501 times.
✗ Branch 8 not taken.
 5002 return generateStabilityConstraints(contactPhase, acc);
141 2501 }
142
143 2505 std::pair<Matrix3, Vector3> computeCost() {
144
2/4
✓ Branch 1 taken 2505 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2505 times.
✗ Branch 5 not taken.
 2505 Matrix3 H = Matrix3::Identity();
145
2/4
✓ Branch 1 taken 2505 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2505 times.
✗ Branch 5 not taken.
 2505 Vector3 g = Vector3::Zero();
146
1/2
✓ Branch 1 taken 2505 times.
✗ Branch 2 not taken.
 5010 return std::make_pair(H, g);
147 }
148
149 2501 std::pair<MatrixX3, VectorX> generateConstraints(MatrixX3 normals,
150 MatrixX3 positions,
151 Matrix3 endEffRotation,
152 Vector3 endEffTranslation) {
153 std::pair<MatrixX3, VectorX> Ab =
154
4/8
✓ Branch 1 taken 2501 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2501 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2501 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2501 times.
✗ Branch 11 not taken.
 2501 generateKinematicsConstraints(endEffRotation, endEffTranslation);
155 std::pair<MatrixX3, VectorX> Cd =
156
6/12
✓ Branch 1 taken 2501 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2501 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2501 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2501 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2501 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 2501 times.
✗ Branch 17 not taken.
 5002 generateStabilityConstraints(normals, positions);
157 2501 size_t numIneq = Ab.first.rows() + Cd.first.rows();
158
1/2
✓ Branch 1 taken 2501 times.
✗ Branch 2 not taken.
 2501 MatrixXX M(numIneq, 3);
159
1/2
✓ Branch 1 taken 2501 times.
✗ Branch 2 not taken.
 2501 VectorX n(numIneq);
160
2/4
✓ Branch 2 taken 2501 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2501 times.
✗ Branch 6 not taken.
 2501 M.block(0, 0, Ab.first.rows(), 3) = Ab.first;
161
2/4
✓ Branch 3 taken 2501 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 2501 times.
✗ Branch 7 not taken.
 2501 M.block(Ab.first.rows(), 0, Cd.first.rows(), 3) = Cd.first;
162
2/4
✓ Branch 2 taken 2501 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2501 times.
✗ Branch 6 not taken.
 2501 n.segment(0, Ab.first.rows()) = Ab.second;
163
2/4
✓ Branch 3 taken 2501 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 2501 times.
✗ Branch 7 not taken.
 2501 n.segment(Ab.first.rows(), Cd.first.rows()) = Cd.second;
164
2/4
✓ Branch 1 taken 2501 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2501 times.
✗ Branch 5 not taken.
 7503 return std::make_pair(M, n);
165 2501 }
166
167 5000 double fRandom(double fMin, double fMax) {
168 5000 double f = (double)std::rand() / RAND_MAX;
169 5000 return fMin + f * (fMax - fMin);
170 }
171
172 62 ConstraintsPair stackConstraints(const ConstraintsPair& Ab,
173 const ConstraintsPair& Cd) {
174 62 size_t numIneq = Ab.first.rows() + Cd.first.rows();
175
1/2
✓ Branch 1 taken 62 times.
✗ Branch 2 not taken.
 62 MatrixX3 M(numIneq, 3);
176
1/2
✓ Branch 1 taken 62 times.
✗ Branch 2 not taken.
 62 VectorX n(numIneq);
177
2/4
✓ Branch 2 taken 62 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 62 times.
✗ Branch 6 not taken.
 62 M.block(0, 0, Ab.first.rows(), 3) = Ab.first;
178
2/4
✓ Branch 3 taken 62 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 62 times.
✗ Branch 7 not taken.
 62 M.block(Ab.first.rows(), 0, Cd.first.rows(), 3) = Cd.first;
179
2/4
✓ Branch 2 taken 62 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 62 times.
✗ Branch 6 not taken.
 62 n.segment(0, Ab.first.rows()) = Ab.second;
180
2/4
✓ Branch 3 taken 62 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 62 times.
✗ Branch 7 not taken.
 62 n.segment(Ab.first.rows(), Cd.first.rows()) = Cd.second;
181
1/2
✓ Branch 1 taken 62 times.
✗ Branch 2 not taken.
 124 return std::make_pair(M, n);
182 62 }
183
184 763 bool verifyKinematicConstraints(const ConstraintsPair& Ab,
185 const Vector3& point) {
186
2/2
✓ Branch 1 taken 7572 times.
✓ Branch 2 taken 763 times.
 8335 for (long int i = 0; i < Ab.second.size(); ++i) {
187
3/6
✓ Branch 2 taken 7572 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7572 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 7572 times.
 7572 if (Ab.first.block<1, 3>(i, 0).dot(point) > Ab.second[i]) {
188 ✗ return false;
189 }
190 }
191 763 return true;
192 }
193
194 763 bool verifyStabilityConstraintsDLP(
195 centroidal_dynamics::Equilibrium contactPhase, Vector3 c, Vector3 /*dc*/,
196 Vector3 ddc) {
197 763 bool success(false);
198 double res;
199
1/2
✓ Branch 1 taken 763 times.
✗ Branch 2 not taken.
 763 centroidal_dynamics::Equilibrium contactPhaseDLP(contactPhase);
200
1/2
✓ Branch 1 taken 763 times.
✗ Branch 2 not taken.
 763 contactPhaseDLP.setAlgorithm(centroidal_dynamics::EQUILIBRIUM_ALGORITHM_DLP);
201 centroidal_dynamics::LP_status status =
202
3/6
✓ Branch 1 taken 763 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 763 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 763 times.
✗ Branch 8 not taken.
 763 contactPhaseDLP.computeEquilibriumRobustness(c, ddc, res);
203
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 763 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 763 success = (status == centroidal_dynamics::LP_STATUS_OPTIMAL ||
204 status == centroidal_dynamics::LP_STATUS_UNBOUNDED);
205
1/2
✓ Branch 0 taken 763 times.
✗ Branch 1 not taken.
 763 if (success) success = res >= -EPSILON;
206
1/8
✗ Branch 0 not taken.
✓ Branch 1 taken 763 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
 763 if (!success) std::cout << "fail level " << res << std::endl;
207 763 return success;
208 763 }
209
210 763 bool verifyStabilityConstraintsPP(centroidal_dynamics::Equilibrium contactPhase,
211 Vector3 c, Vector3 /*dc*/, Vector3 acc) {
212 // compute inequalities :
213 763 const Vector3& g = contactPhase.m_gravity;
214
2/4
✓ Branch 1 taken 763 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 763 times.
✗ Branch 5 not taken.
 763 const Matrix3 gSkew = bezier_com_traj::skew(g);
215
2/4
✓ Branch 1 taken 763 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 763 times.
✗ Branch 5 not taken.
 763 const Matrix3 accSkew = bezier_com_traj::skew(acc);
216 // compute GIWC
217
1/2
✓ Branch 1 taken 763 times.
✗ Branch 2 not taken.
 763 centroidal_dynamics::MatrixXX Hrow;
218
1/2
✓ Branch 1 taken 763 times.
✗ Branch 2 not taken.
 763 VectorX h;
219
1/2
✓ Branch 1 taken 763 times.
✗ Branch 2 not taken.
 763 contactPhase.getPolytopeInequalities(Hrow, h);
220
2/4
✓ Branch 1 taken 763 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 763 times.
✗ Branch 5 not taken.
 763 MatrixXX H = -Hrow;
221
2/4
✓ Branch 1 taken 763 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 763 times.
✗ Branch 5 not taken.
 763 H.rowwise().normalize();
222 763 int dimH = (int)(H.rows());
223
2/4
✓ Branch 1 taken 763 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 763 times.
✗ Branch 5 not taken.
 763 MatrixXX mH = contactPhase.m_mass * H;
224 // constraints : mH[:,3:6] g^ x <= h + mH[:,0:3]g
225 // A = mH g^
226 // b = h + mHg
227
4/8
✓ Branch 1 taken 763 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 763 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 763 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 763 times.
✗ Branch 11 not taken.
 763 MatrixX3 A = mH.block(0, 3, dimH, 3) * (gSkew - accSkew);
228
5/10
✓ Branch 1 taken 763 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 763 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 763 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 763 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 763 times.
✗ Branch 14 not taken.
 763 VectorX b = h + mH.block(0, 0, dimH, 3) * (g - acc);
229
230 // verify inequalities with c :
231
2/2
✓ Branch 1 taken 25360 times.
✓ Branch 2 taken 763 times.
 26123 for (long int i = 0; i < b.size(); ++i) {
232
4/8
✓ Branch 1 taken 25360 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25360 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 25360 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✓ Branch 10 taken 25360 times.
 25360 if (A.block<1, 3>(i, 0).dot(c) - EPSILON > b[i]) {
233 ✗ return false;
234 }
235 }
236 763 return true;
237 763 }
238
239 #endif // TEST_HELPER_HH
240