GCC Code Coverage Report

Directory: ./
File: src/common_solve_methods.cpp
Date: 2025-03-18 04:20:50
Exec Total Coverage
Lines: 69 96 71.9%
Branches: 94 250 37.6%
Line Branch Exec Source
1
2 #include <hpp/bezier-com-traj/common_solve_methods.hh>
3 #include <hpp/bezier-com-traj/solver/eiquadprog-fast.hpp>
4 #include <hpp/bezier-com-traj/solver/glpk-wrapper.hpp>
5 #include <hpp/bezier-com-traj/waypoints/waypoints_definition.hh>
6
7 namespace bezier_com_traj {
8 ✗ std::vector<waypoint6_t> ComputeDiscretizedWaypoints(
9 const std::vector<waypoint6_t>& wps,
10 const std::vector<ndcurves::Bern<double> >& berns, int numSteps) {
11 ✗ double dt = 1. / double(numSteps);
12 ✗ std::vector<waypoint6_t> res;
13 ✗ for (int i = 0; i < numSteps + 1; ++i) {
14 ✗ waypoint6_t w = initwp<waypoint6_t>();
15 ✗ for (int j = 0; j < 5; ++j) {
16 ✗ double b = berns[j](i * dt);
17 ✗ w.first += b * (wps[j].first);
18 ✗ w.second += b * (wps[j].second);
19 }
20 ✗ res.push_back(w);
21 }
22 ✗ return res;
23 }
24
25 1 MatrixXX initMatrixA(const int dimH, const std::vector<waypoint6_t>& wps,
26 const long int extraConstraintSize,
27 const bool useAngMomentum) {
28
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 int dimPb = useAngMomentum ? 6 : 3;
29
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 return MatrixXX::Zero(dimH * wps.size() + extraConstraintSize, dimPb);
30 }
31
32 ✗ MatrixXX initMatrixD(const int colG, const std::vector<waypoint6_t>& wps,
33 const long int extraConstraintSize,
34 const bool useAngMomentum) {
35 ✗ int dimPb = useAngMomentum ? 6 : 3;
36 ✗ return MatrixXX::Zero(6 + extraConstraintSize, wps.size() * colG + dimPb);
37 }
38
39 1 void addKinematic(Ref_matrixXX A, Ref_vectorX b, Cref_matrixX3 Kin,
40 Cref_vectorX kin, const long int otherConstraintIndex) {
41 1 int dimKin = (int)(kin.rows());
42
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 1 if (dimKin == 0) return;
43 ✗ A.block(A.rows() - dimKin - otherConstraintIndex, 0, dimKin, 3) = Kin;
44 ✗ b.segment(A.rows() - dimKin - otherConstraintIndex, dimKin) = kin;
45 }
46
47 ✗ void addAngularMomentum(Ref_matrixXX A, Ref_vectorX b, Cref_matrixX3 Ang,
48 Cref_vectorX ang) {
49 ✗ int dimAng = (int)(ang.rows());
50 ✗ if (dimAng > 0) {
51 ✗ A.block(A.rows() - dimAng, 3, dimAng, 3) = Ang;
52 ✗ b.tail(dimAng) = ang;
53 }
54 }
55
56 817 int removeZeroRows(Ref_matrixXX& A, Ref_vectorX& b) {
57 Eigen::Matrix<bool, Eigen::Dynamic, 1> empty =
58
5/10
✓ Branch 1 taken 817 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 817 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 817 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 817 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 817 times.
✗ Branch 14 not taken.
 817 (A.array() == 0).rowwise().all();
59 817 size_t last = A.rows() - 1;
60
2/2
✓ Branch 0 taken 23950 times.
✓ Branch 1 taken 817 times.
 24767 for (size_t i = 0; i < last + 1;) {
61
3/4
✓ Branch 1 taken 23950 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 630 times.
✓ Branch 4 taken 23320 times.
 23950 if (empty(i)) {
62
3/6
✓ Branch 1 taken 630 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 630 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 630 times.
 630 assert(A.row(i).norm() == 0);
63
2/4
✓ Branch 1 taken 630 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 630 times.
 630 if (b(i) < 0) {
64 // std::cout << "empty row for A while correponding b is negative.
65 // Problem is infeasible" << b(i) << std::endl;
66 ✗ return -1;
67 }
68
3/6
✓ Branch 1 taken 630 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 630 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 630 times.
✗ Branch 8 not taken.
 630 A.row(i).swap(A.row(last));
69
3/6
✓ Branch 1 taken 630 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 630 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 630 times.
✗ Branch 8 not taken.
 630 b.row(i).swap(b.row(last));
70
3/6
✓ Branch 1 taken 630 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 630 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 630 times.
✗ Branch 8 not taken.
 630 empty.segment<1>(i).swap(empty.segment<1>(last));
71 630 --last;
72 } else
73 23320 ++i;
74 }
75 817 return (int)last + 1;
76 817 }
77
78 817 int Normalize(Ref_matrixXX A, Ref_vectorX b) {
79 817 int zeroindex = removeZeroRows(A, b);
80
2/2
✓ Branch 0 taken 790 times.
✓ Branch 1 taken 27 times.
 817 if (zeroindex > 0) {
81
4/8
✓ Branch 2 taken 790 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 790 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 790 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 790 times.
✗ Branch 12 not taken.
 790 Eigen::VectorXd norm = A.block(0, 0, zeroindex, A.cols()).rowwise().norm();
82
3/6
✓ Branch 2 taken 790 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 790 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 790 times.
✗ Branch 9 not taken.
 790 A.block(0, 0, zeroindex, A.cols()).rowwise().normalize();
83
4/8
✓ Branch 1 taken 790 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 790 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 790 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 790 times.
✗ Branch 11 not taken.
 790 b.head(zeroindex) = b.head(zeroindex).cwiseQuotient(norm);
84 790 }
85 817 return zeroindex;
86 }
87
88 1 std::pair<MatrixXX, VectorX> compute6dControlPointInequalities(
89 const ContactData& cData, const std::vector<waypoint6_t>& wps,
90 const std::vector<waypoint6_t>& wpL, const bool useAngMomentum,
91 bool& fail) {
92
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 MatrixXX A;
93
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 VectorX b;
94 // gravity vector
95 1 const point_t& g = cData.contactPhase_->m_gravity;
96 // compute GIWC
97
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 assert(cData.contactPhase_->getAlgorithm() ==
98 centroidal_dynamics::EQUILIBRIUM_ALGORITHM_PP);
99
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 centroidal_dynamics::MatrixXX Hrow;
100
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 VectorX h;
101
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 cData.contactPhase_->getPolytopeInequalities(Hrow, h);
102
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 MatrixXX H = -Hrow;
103
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 H.rowwise().normalize();
104 1 int dimH = (int)(H.rows());
105
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 MatrixXX mH = cData.contactPhase_->m_mass * H;
106 // init and fill Ab matrix
107 1 long int dimKin = cData.kin_.size();
108
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 long int dimAng = useAngMomentum ? (long int)(cData.ang_.size()) : 0;
109
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 A = initMatrixA(dimH, wps, dimKin + dimAng, useAngMomentum);
110
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 b = VectorX::Zero(A.rows());
111
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 point6_t bc = point6_t::Zero();
112
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 bc.head(3) = g; // constant part of Aub, Aubi = mH * (bc - wsi)
113 1 int i = 0;
114 1 std::vector<waypoint6_t>::const_iterator wpLcit = wpL.begin();
115 1 for (std::vector<waypoint6_t>::const_iterator wpcit = wps.begin();
116
2/2
✓ Branch 2 taken 5 times.
✓ Branch 3 taken 1 times.
 6 wpcit != wps.end(); ++wpcit) {
117
3/6
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 5 times.
✗ Branch 9 not taken.
 5 A.block(i * dimH, 0, dimH, 3) = mH * wpcit->first;
118
4/8
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 5 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 5 times.
✗ Branch 12 not taken.
 5 b.segment(i * dimH, dimH) = mH * (bc - wpcit->second);
119
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 5 times.
 5 if (useAngMomentum) {
120 ✗ A.block(i * dimH, 3, dimH, 3) = H * wpLcit->first;
121 ✗ b.segment(i * dimH, dimH) += H * (-wpLcit->second);
122 ✗ ++wpLcit;
123 }
124 5 ++i;
125 }
126
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
 1 addKinematic(A, b, cData.Kin_, cData.kin_, dimAng);
127
1/12
✗ Branch 0 not taken.
✓ Branch 1 taken 1 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.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
 1 if (useAngMomentum) addAngularMomentum(A, b, cData.Kin_, cData.kin_);
128 1 fail = false;
129 // normalization removes 0 value rows, but resizing
130 // must actually be done with matrices and not the references
131 /*int zeroindex = Normalize(A,b);
132 if(zeroindex < 0)
133 fail = true;
134 else
135 {
136 A.conservativeResize(zeroindex, A.cols());
137 b.conservativeResize(zeroindex, 1);
138 fail = false;
139 }*/
140
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 return std::make_pair(A, b);
141 1 }
142
143 ✗ ResultData solve(Cref_matrixXX A, Cref_vectorX ci0, Cref_matrixXX D,
144 Cref_vectorX d, Cref_matrixXX H, Cref_vectorX g,
145 Cref_vectorX initGuess, Cref_vectorX minBounds,
146 Cref_vectorX maxBounds, const solvers::SolverType solver) {
147 return solvers::solve(A, ci0, D, d, H, g, initGuess, minBounds, maxBounds,
148 ✗ solver);
149 }
150
151 2533 ResultData solve(Cref_matrixXX A, Cref_vectorX b, Cref_matrixXX H,
152 Cref_vectorX g, Cref_vectorX initGuess,
153 const solvers::SolverType solver) {
154
2/4
✓ Branch 2 taken 2533 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2533 times.
✗ Branch 6 not taken.
 2533 MatrixXX D = MatrixXX::Zero(0, A.cols());
155
2/4
✓ Branch 1 taken 2533 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2533 times.
✗ Branch 5 not taken.
 2533 VectorX d = VectorX::Zero(0);
156
8/16
✓ Branch 1 taken 2533 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2533 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2533 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2533 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2533 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 2533 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 2533 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 2533 times.
✗ Branch 23 not taken.
 5066 return solvers::solve(A, b, D, d, H, g, initGuess, d, d, solver);
157 2533 }
158
159 2533 ResultData solve(const std::pair<MatrixXX, VectorX>& Ab,
160 const std::pair<MatrixXX, VectorX>& Hg, const VectorX& init,
161 const solvers::SolverType solver) {
162
5/10
✓ Branch 2 taken 2533 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2533 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2533 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 2533 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 2533 times.
✗ Branch 15 not taken.
 2533 return solve(Ab.first, Ab.second, Hg.first, Hg.second, init, solver);
163 }
164
165 38 ResultData solve(const std::pair<MatrixXX, VectorX>& Ab,
166 const std::pair<MatrixXX, VectorX>& Dd,
167 const std::pair<MatrixXX, VectorX>& Hg, Cref_vectorX minBounds,
168 Cref_vectorX maxBounds, const VectorX& init,
169 const solvers::SolverType solver) {
170 38 return solve(Ab.first, Ab.second, Dd.first, Dd.second, Hg.first, Hg.second,
171 38 init, minBounds, maxBounds, solver);
172 }
173
174 } // namespace bezier_com_traj
175