Directory: | ./ |
---|---|
File: | tests/test_helper.hh |
Date: | 2025-03-18 04:20:50 |
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 |