GCC Code Coverage Report

Directory: ./
File: src/computeCOMTraj.cpp
Date: 2025-04-18 04:07:36
Exec Total Coverage
Lines: 331 347 95.4%
Branches: 325 636 51.1%
Line Branch Exec Source
1 /*
2 * Copyright 2018, LAAS-CNRS
3 * Author: Pierre Fernbach
4 */
5
6 #include <algorithm>
7 #include <hpp/bezier-com-traj/common_solve_methods.hh>
8 #include <hpp/bezier-com-traj/cost/costfunction_definition.hh>
9 #include <hpp/bezier-com-traj/solve.hh>
10 #include <hpp/bezier-com-traj/solver/solver-abstract.hpp>
11 #include <hpp/bezier-com-traj/waypoints/waypoints_definition.hh>
12 #include <hpp/centroidal-dynamics/centroidal_dynamics.hh>
13 #include <limits>
14
15 static const int dimVarX = 3;
16 static const int numRowsForce = 6;
17
18 namespace bezier_com_traj {
19 typedef std::pair<double, point3_t> coefs_t;
20
21 25 bezier_wp_t::t_point_t computeDiscretizedWwaypoints(const ProblemData& pData,
22 double T,
23 const T_time& timeArray) {
24
1/2
✓ Branch 1 taken 25 times.
✗ Branch 2 not taken.
 25 bezier_wp_t::t_point_t wps = computeWwaypoints(pData, T);
25
1/2
✓ Branch 1 taken 25 times.
✗ Branch 2 not taken.
 25 bezier_wp_t::t_point_t res;
26
1/2
✓ Branch 1 taken 25 times.
✗ Branch 2 not taken.
 25 const int DIM_VAR = (int)dimVar(pData);
27 std::vector<ndcurves::Bern<double> > berns =
28
1/2
✓ Branch 2 taken 25 times.
✗ Branch 3 not taken.
 25 ComputeBersteinPolynoms((int)wps.size() - 1);
29 double t, b;
30
2/2
✓ Branch 4 taken 414 times.
✓ Branch 5 taken 25 times.
 439 for (CIT_time cit = timeArray.begin(); cit != timeArray.end(); ++cit) {
31
1/2
✓ Branch 1 taken 414 times.
✗ Branch 2 not taken.
 414 waypoint_t w = initwp(6, DIM_VAR);
32 414 t = std::min(cit->first / T, 1.);
33
2/2
✓ Branch 1 taken 2720 times.
✓ Branch 2 taken 414 times.
 3134 for (std::size_t j = 0; j < wps.size(); ++j) {
34
1/2
✓ Branch 2 taken 2720 times.
✗ Branch 3 not taken.
 2720 b = berns[j](t);
35
2/4
✓ Branch 2 taken 2720 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2720 times.
✗ Branch 6 not taken.
 2720 w.first += b * (wps[j].first);
36
2/4
✓ Branch 2 taken 2720 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2720 times.
✗ Branch 6 not taken.
 2720 w.second += b * (wps[j].second);
37 }
38
1/2
✓ Branch 1 taken 414 times.
✗ Branch 2 not taken.
 414 res.push_back(w);
39 414 }
40 50 return res;
41 25 }
42
43 ✗ std::pair<MatrixXX, VectorX> dynamicStabilityConstraints_cross(
44 const MatrixXX& mH, const VectorX& h, const Vector3& g, const coefs_t& c,
45 const coefs_t& ddc) {
46 ✗ Matrix3 S_hat;
47 ✗ int dimH = (int)(mH.rows());
48 ✗ MatrixXX A(dimH, 4);
49 ✗ VectorX b(dimH);
50 ✗ S_hat = skew(c.second * ddc.first - ddc.second * c.first + g * c.first);
51 ✗ A.block(0, 0, dimH, 3) =
52 ✗ mH.block(0, 3, dimH, 3) * S_hat + mH.block(0, 0, dimH, 3) * ddc.first;
53 ✗ b = h + mH.block(0, 0, dimH, 3) * (g - ddc.second) +
54 ✗ mH.block(0, 3, dimH, 3) *
55 ✗ (c.second.cross(g) - c.second.cross(ddc.second));
56 ✗ Normalize(A, b);
57 // add 1 for the slack variable :
58 ✗ A.block(0, 3, dimH, 1) = VectorX::Ones(dimH);
59 ✗ return std::make_pair(A, b);
60 }
61
62 817 std::pair<MatrixXX, VectorX> dynamicStabilityConstraints(const MatrixXX& mH,
63 const VectorX& h,
64 const Vector3& g,
65 const waypoint_t& w) {
66 817 int dimH = (int)(mH.rows());
67
1/2
✓ Branch 1 taken 817 times.
✗ Branch 2 not taken.
 817 MatrixXX A(dimH, dimVarX);
68
1/2
✓ Branch 1 taken 817 times.
✗ Branch 2 not taken.
 817 VectorX b(dimH);
69
2/4
✓ Branch 1 taken 817 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 817 times.
✗ Branch 5 not taken.
 817 VectorX g_ = VectorX::Zero(6);
70
2/4
✓ Branch 1 taken 817 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 817 times.
✗ Branch 5 not taken.
 817 g_.head<3>() = g;
71
3/6
✓ 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.
 817 A.block(0, 0, dimH, 3) = mH * w.first;
72
4/8
✓ 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.
 817 b = h + mH * (g_ - w.second);
73
3/6
✓ 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.
 817 Normalize(A, b);
74
1/2
✓ Branch 1 taken 817 times.
✗ Branch 2 not taken.
 1634 return std::make_pair(A, b);
75 817 }
76
77 25 std::vector<int> stepIdPerPhase(
78 const T_time& timeArray) // const int pointsPerPhase)
79 {
80 25 std::vector<int> res;
81 25 int i = 0;
82 25 CIT_time cit = timeArray.begin();
83
2/2
✓ Branch 4 taken 389 times.
✓ Branch 5 taken 25 times.
 414 for (CIT_time cit2 = timeArray.begin() + 1; cit2 != timeArray.end();
84 389 ++cit, ++cit2, ++i) {
85
2/2
✓ Branch 2 taken 48 times.
✓ Branch 3 taken 341 times.
 389 if (cit2->second != cit->second) {
86
1/2
✓ Branch 1 taken 48 times.
✗ Branch 2 not taken.
 48 res.push_back(i);
87 }
88 }
89
1/2
✓ Branch 1 taken 25 times.
✗ Branch 2 not taken.
 25 res.push_back(i);
90 50 return res;
91 }
92
93 25 long int computeNumIneq(const ProblemData& pData, const VectorX& Ts,
94 const std::vector<int>& phaseSwitch) {
95 25 const size_t numPoints = phaseSwitch.back() + 1;
96 25 long int num_stab_ineq = 0;
97 25 long int num_kin_ineq = 0;
98 int numStepForPhase;
99 25 int numStepsCumulated = 0;
100
1/2
✓ Branch 1 taken 25 times.
✗ Branch 2 not taken.
 25 centroidal_dynamics::MatrixXX Hrow;
101
1/2
✓ Branch 1 taken 25 times.
✗ Branch 2 not taken.
 25 VectorX h;
102
2/2
✓ Branch 1 taken 73 times.
✓ Branch 2 taken 25 times.
 98 for (int i = 0; i < Ts.size(); ++i) {
103
1/2
✓ Branch 2 taken 73 times.
✗ Branch 3 not taken.
 73 pData.contacts_[i].contactPhase_->getPolytopeInequalities(Hrow, h);
104 73 numStepForPhase =
105 73 phaseSwitch[i] + 1 - numStepsCumulated; // pointsPerPhase;
106 73 numStepsCumulated = phaseSwitch[i] + 1;
107
2/2
✓ Branch 0 taken 48 times.
✓ Branch 1 taken 25 times.
 73 if (i > 0)
108 48 ++numStepForPhase; // because at the switch point between phases we add
109 // the constraints of both phases.
110 73 num_stab_ineq += Hrow.rows() * numStepForPhase;
111 73 num_kin_ineq += pData.contacts_[i].kin_.rows() * numStepForPhase;
112 }
113 25 long int res = num_stab_ineq + num_kin_ineq;
114
2/2
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 21 times.
 25 if (pData.constraints_.constraintAcceleration_)
115 4 res +=
116 4 2 * 3 * (numPoints); // upper and lower bound on acceleration for each
117 // discretized waypoint (exept the first one)
118 25 return res;
119 25 }
120
121 125 void updateH(const ProblemData& pData, const ContactData& phase, MatrixXX& mH,
122 VectorX& h, int& dimH) {
123
1/2
✓ Branch 1 taken 125 times.
✗ Branch 2 not taken.
 125 VectorX hrow;
124
1/2
✓ Branch 1 taken 125 times.
✗ Branch 2 not taken.
 125 centroidal_dynamics::MatrixXX Hrow;
125 centroidal_dynamics::LP_status status =
126
1/2
✓ Branch 1 taken 125 times.
✗ Branch 2 not taken.
 125 phase.contactPhase_->getPolytopeInequalities(Hrow, hrow);
127
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 125 times.
 125 assert(status == centroidal_dynamics::LP_STATUS_OPTIMAL &&
128 "Error in centroidal dynamics lib while computing inequalities");
129
3/6
✓ Branch 1 taken 125 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 125 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 125 times.
✗ Branch 8 not taken.
 125 mH = -Hrow * phase.contactPhase_->m_mass;
130
2/4
✓ Branch 1 taken 125 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 125 times.
✗ Branch 5 not taken.
 125 mH.rowwise().normalize();
131
1/2
✓ Branch 1 taken 125 times.
✗ Branch 2 not taken.
 125 h = hrow;
132 125 dimH = (int)(mH.rows());
133
1/2
✓ Branch 0 taken 125 times.
✗ Branch 1 not taken.
 125 if (pData.constraints_.reduce_h_ > 0)
134
3/6
✓ Branch 2 taken 125 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 125 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 125 times.
✗ Branch 9 not taken.
 125 h -= VectorX::Ones(h.rows()) * pData.constraints_.reduce_h_;
135 125 }
136
137 817 void assignStabilityConstraintsForTimeStep(MatrixXX& mH, VectorX& h,
138 const waypoint_t& wp_w,
139 const int dimH, long int& id_rows,
140 MatrixXX& A, VectorX& b,
141 const Vector3& g) {
142 std::pair<MatrixXX, VectorX> Ab_stab =
143
1/2
✓ Branch 1 taken 817 times.
✗ Branch 2 not taken.
 817 dynamicStabilityConstraints(mH, h, g, wp_w);
144
2/4
✓ Branch 1 taken 817 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 817 times.
✗ Branch 5 not taken.
 817 A.block(id_rows, 0, dimH, dimVarX) = Ab_stab.first;
145
2/4
✓ Branch 1 taken 817 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 817 times.
✗ Branch 5 not taken.
 817 b.segment(id_rows, dimH) = Ab_stab.second;
146 817 id_rows += dimH;
147 817 }
148
149 // mG is -G
150 321 void assignStabilityConstraintsForTimeStepForce(
151 const waypoint_t& wp_w, const long int rowIdx, long int& id_cols,
152 const centroidal_dynamics::Matrix6X mG, MatrixXX& D, VectorX& d,
153 const double mass, const Vector3& g) {
154
2/4
✓ Branch 2 taken 321 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 321 times.
✗ Branch 6 not taken.
 321 D.block(rowIdx, id_cols, numRowsForce, mG.cols()) = mG;
155 321 id_cols += mG.cols();
156
3/6
✓ Branch 1 taken 321 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 321 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 321 times.
✗ Branch 8 not taken.
 321 D.block(rowIdx, 0, numRowsForce, dimVarX) = mass * wp_w.first;
157
2/4
✓ Branch 1 taken 321 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 321 times.
✗ Branch 5 not taken.
 321 VectorX g_ = VectorX::Zero(6);
158
2/4
✓ Branch 1 taken 321 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 321 times.
✗ Branch 5 not taken.
 321 g_.head<3>() = g;
159
4/8
✓ Branch 1 taken 321 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 321 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 321 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 321 times.
✗ Branch 11 not taken.
 321 d.segment(rowIdx, numRowsForce) = mass * (g_ - wp_w.second);
160 321 }
161
162 48 void switchContactPhase(const ProblemData& pData, MatrixXX& A, VectorX& b,
163 MatrixXX& mH, VectorX& h, const waypoint_t& wp_w,
164 const ContactData& phase, long int& id_rows,
165 int& dimH) {
166 48 updateH(pData, phase, mH, h, dimH);
167 // the current waypoint must have the constraints of both phases. So we add it
168 // again :
169 // TODO : filter for redunbdant constraints ...
170 // add stability constraints :
171 48 assignStabilityConstraintsForTimeStep(mH, h, wp_w, dimH, id_rows, A, b,
172 48 phase.contactPhase_->m_gravity);
173 48 }
174
175 25 long int assignStabilityConstraints(const ProblemData& pData, MatrixXX& A,
176 VectorX& b, const T_time& timeArray,
177 const double t_total,
178 const std::vector<int>& stepIdForPhase) {
179 25 long int id_rows = 0;
180 bezier_wp_t::t_point_t wps_w =
181
1/2
✓ Branch 1 taken 25 times.
✗ Branch 2 not taken.
 25 computeDiscretizedWwaypoints(pData, t_total, timeArray);
182
183 25 std::size_t id_phase = 0;
184 25 const Vector3& g = pData.contacts_[id_phase].contactPhase_->m_gravity;
185
186 // reference to current stability matrix
187
1/2
✓ Branch 1 taken 25 times.
✗ Branch 2 not taken.
 25 MatrixXX mH;
188
1/2
✓ Branch 1 taken 25 times.
✗ Branch 2 not taken.
 25 VectorX h;
189 int dimH;
190
1/2
✓ Branch 2 taken 25 times.
✗ Branch 3 not taken.
 25 updateH(pData, pData.contacts_[id_phase], mH, h, dimH);
191
192 // assign the Stability constraints for each discretized waypoints :
193 // we don't consider the first point, because it's independant of x.
194
2/2
✓ Branch 1 taken 414 times.
✓ Branch 2 taken 25 times.
 439 for (std::size_t id_step = 0; id_step < timeArray.size(); ++id_step) {
195 // add stability constraints :
196
1/2
✓ Branch 2 taken 414 times.
✗ Branch 3 not taken.
 414 assignStabilityConstraintsForTimeStep(mH, h, wps_w[id_step], dimH, id_rows,
197 A, b, g);
198 // check if we are going to switch phases :
199 414 for (std::vector<int>::const_iterator it_switch = stepIdForPhase.begin();
200
2/2
✓ Branch 4 taken 816 times.
✓ Branch 5 taken 414 times.
 1230 it_switch != (stepIdForPhase.end() - 1); ++it_switch) {
201
2/2
✓ Branch 1 taken 48 times.
✓ Branch 2 taken 768 times.
 816 if ((int)id_step == (*it_switch)) {
202 48 id_phase++;
203
1/2
✓ Branch 2 taken 48 times.
✗ Branch 3 not taken.
 48 switchContactPhase(pData, A, b, mH, h, wps_w[id_step],
204 48 pData.contacts_[id_phase], id_rows, dimH);
205 }
206 }
207 }
208 25 return id_rows;
209 25 }
210
211 25 void assignKinematicConstraints(const ProblemData& pData, MatrixXX& A,
212 VectorX& b, const T_time& timeArray,
213 const double t_total,
214 const std::vector<int>& stepIdForPhase,
215 long int& id_rows) {
216 25 std::size_t id_phase = 0;
217 std::vector<coefs_t> wps_c =
218
1/2
✓ Branch 1 taken 25 times.
✗ Branch 2 not taken.
 25 computeDiscretizedWaypoints<point_t>(pData, t_total, timeArray);
219 long int current_size;
220 25 id_phase = 0;
221 // assign the Kinematics constraints for each discretized waypoints :
222 // we don't consider the first point, because it's independant of x.
223
2/2
✓ Branch 1 taken 414 times.
✓ Branch 2 taken 25 times.
 439 for (std::size_t id_step = 0; id_step < timeArray.size(); ++id_step) {
224 // add constraints for wp id_step, on current phase :
225 // add kinematics constraints :
226 // constraint are of the shape A c <= b . But here c(x) = Fx + s so : AFx <=
227 // b - As
228 414 current_size = (int)pData.contacts_[id_phase].kin_.rows();
229
2/2
✓ Branch 0 taken 358 times.
✓ Branch 1 taken 56 times.
 414 if (current_size > 0) {
230
1/2
✓ Branch 1 taken 358 times.
✗ Branch 2 not taken.
 358 A.block(id_rows, 0, current_size, 3) =
231
2/4
✓ Branch 3 taken 358 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 358 times.
✗ Branch 7 not taken.
 716 (pData.contacts_[id_phase].Kin_ * wps_c[id_step].first);
232
1/2
✓ Branch 1 taken 358 times.
✗ Branch 2 not taken.
 358 b.segment(id_rows, current_size) =
233
1/2
✓ Branch 2 taken 358 times.
✗ Branch 3 not taken.
 358 pData.contacts_[id_phase].kin_ -
234
2/4
✓ Branch 3 taken 358 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 358 times.
✗ Branch 7 not taken.
 716 (pData.contacts_[id_phase].Kin_ * wps_c[id_step].second);
235 358 id_rows += current_size;
236 }
237
238 // check if we are going to switch phases :
239
2/2
✓ Branch 1 taken 816 times.
✓ Branch 2 taken 414 times.
 1230 for (std::size_t i = 0; i < (stepIdForPhase.size() - 1); ++i) {
240
2/2
✓ Branch 1 taken 48 times.
✓ Branch 2 taken 768 times.
 816 if (id_step == (std::size_t)stepIdForPhase[i]) {
241 // switch to phase i
242 48 id_phase = i + 1;
243 // the current waypoint must have the constraints of both phases. So we
244 // add it again :
245 // TODO : filter for redunbdant constraints ...
246 48 current_size = pData.contacts_[id_phase].kin_.rows();
247
2/2
✓ Branch 0 taken 44 times.
✓ Branch 1 taken 4 times.
 48 if (current_size > 0) {
248
1/2
✓ Branch 1 taken 44 times.
✗ Branch 2 not taken.
 44 A.block(id_rows, 0, current_size, 3) =
249
2/4
✓ Branch 3 taken 44 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 44 times.
✗ Branch 7 not taken.
 88 (pData.contacts_[id_phase].Kin_ * wps_c[id_step].first);
250
1/2
✓ Branch 1 taken 44 times.
✗ Branch 2 not taken.
 44 b.segment(id_rows, current_size) =
251
1/2
✓ Branch 2 taken 44 times.
✗ Branch 3 not taken.
 44 pData.contacts_[id_phase].kin_ -
252
2/4
✓ Branch 3 taken 44 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 44 times.
✗ Branch 7 not taken.
 88 (pData.contacts_[id_phase].Kin_ * wps_c[id_step].second);
253 44 id_rows += current_size;
254 }
255 }
256 }
257 }
258 25 }
259
260 25 void assignAccelerationConstraints(const ProblemData& pData, MatrixXX& A,
261 VectorX& b, const T_time& timeArray,
262 const double t_total, long int& id_rows) {
263 25 if (pData.constraints_
264
2/2
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 21 times.
 25 .constraintAcceleration_) { // assign the acceleration constraints
265 // for each discretized waypoints :
266 std::vector<coefs_t> wps_ddc =
267 computeDiscretizedAccelerationWaypoints<point_t>(pData, t_total,
268
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 timeArray);
269
3/6
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
 4 Vector3 acc_bounds = Vector3::Ones() * pData.constraints_.maxAcceleration_;
270
2/2
✓ Branch 1 taken 64 times.
✓ Branch 2 taken 4 times.
 68 for (std::size_t id_step = 0; id_step < timeArray.size(); ++id_step) {
271
1/2
✓ Branch 1 taken 64 times.
✗ Branch 2 not taken.
 64 A.block(id_rows, 0, 3, 3) =
272
3/6
✓ Branch 2 taken 64 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 64 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 64 times.
✗ Branch 9 not taken.
 128 Matrix3::Identity() * wps_ddc[id_step].first; // upper
273
3/6
✓ Branch 2 taken 64 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 64 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 64 times.
✗ Branch 9 not taken.
 64 b.segment(id_rows, 3) = acc_bounds - wps_ddc[id_step].second;
274
1/2
✓ Branch 1 taken 64 times.
✗ Branch 2 not taken.
 64 A.block(id_rows + 3, 0, 3, 3) =
275
4/8
✓ Branch 2 taken 64 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 64 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 64 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 64 times.
✗ Branch 12 not taken.
 128 -Matrix3::Identity() * wps_ddc[id_step].first; // lower
276
3/6
✓ Branch 2 taken 64 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 64 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 64 times.
✗ Branch 9 not taken.
 64 b.segment(id_rows + 3, 3) = acc_bounds + wps_ddc[id_step].second;
277 64 id_rows += 6;
278 }
279 4 }
280 25 }
281
282 25 std::pair<MatrixXX, VectorX> computeConstraintsOneStep(
283 const ProblemData& pData, const VectorX& Ts, const double t_total,
284 const T_time& timeArray) {
285 // Compute all the discretized wayPoint
286
1/2
✓ Branch 1 taken 25 times.
✗ Branch 2 not taken.
 25 std::vector<int> stepIdForPhase = stepIdPerPhase(timeArray);
287 // stepIdForPhase[i] is the id of the last step of phase i / first step of
288 // phase i+1 (overlap)
289
290 // init constraints matrix :
291
1/2
✓ Branch 1 taken 25 times.
✗ Branch 2 not taken.
 25 long int num_ineq = computeNumIneq(pData, Ts, stepIdForPhase);
292
2/4
✓ Branch 1 taken 25 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25 times.
✗ Branch 5 not taken.
 25 MatrixXX A = MatrixXX::Zero(num_ineq, dimVarX);
293
1/2
✓ Branch 1 taken 25 times.
✗ Branch 2 not taken.
 25 VectorX b(num_ineq);
294
295 // assign dynamic and kinematic constraints per timestep, including additional
296 // acceleration constraints.
297
1/2
✓ Branch 1 taken 25 times.
✗ Branch 2 not taken.
 25 long int id_rows = assignStabilityConstraints(pData, A, b, timeArray, t_total,
298 25 stepIdForPhase);
299
1/2
✓ Branch 1 taken 25 times.
✗ Branch 2 not taken.
 25 assignKinematicConstraints(pData, A, b, timeArray, t_total, stepIdForPhase,
300 id_rows);
301
1/2
✓ Branch 1 taken 25 times.
✗ Branch 2 not taken.
 25 assignAccelerationConstraints(pData, A, b, timeArray, t_total, id_rows);
302
303
1/2
✓ Branch 1 taken 25 times.
✗ Branch 2 not taken.
 25 assert(id_rows == (A.rows()) &&
304 "The constraints matrices were not fully filled.");
305
1/2
✓ Branch 1 taken 25 times.
✗ Branch 2 not taken.
 50 return std::make_pair(A, b);
306 25 }
307
308 49 void computeFinalAcceleration(ResultDataCOMTraj& res) {
309 49 res.ddc1_ = res.c_of_t_.derivate(res.c_of_t_.max(), 2);
310 49 }
311
312 49 void computeFinalVelocity(ResultDataCOMTraj& res) {
313 49 res.dc1_ = res.c_of_t_.derivate(res.c_of_t_.max(), 1);
314 49 }
315
316 61 std::pair<MatrixXX, VectorX> genCostFunction(const ProblemData& pData,
317 const VectorX& Ts, const double T,
318 const T_time& timeArray,
319 const long int& dim) {
320
3/6
✓ Branch 1 taken 61 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 61 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 61 times.
✗ Branch 8 not taken.
 61 MatrixXX H = MatrixXX::Identity(dim, dim) * 1e-6;
321
2/4
✓ Branch 1 taken 61 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 61 times.
✗ Branch 5 not taken.
 61 VectorX g = VectorX::Zero(dim);
322
1/2
✓ Branch 1 taken 61 times.
✗ Branch 2 not taken.
 61 cost::genCostFunction(pData, Ts, T, timeArray, H, g);
323
1/2
✓ Branch 1 taken 61 times.
✗ Branch 2 not taken.
 122 return std::make_pair(H, g);
324 61 }
325
326 61 ResultDataCOMTraj genTraj(ResultData resQp, const ProblemData& pData,
327 const double T) {
328 61 ResultDataCOMTraj res;
329
2/2
✓ Branch 0 taken 49 times.
✓ Branch 1 taken 12 times.
 61 if (resQp.success_) {
330 49 res.success_ = true;
331
2/4
✓ Branch 1 taken 49 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 49 times.
✗ Branch 5 not taken.
 49 res.x = resQp.x.head<3>();
332 49 res.cost_ = resQp.cost_;
333
1/2
✓ Branch 1 taken 49 times.
✗ Branch 2 not taken.
 49 std::vector<Vector3> pis = computeConstantWaypoints(pData, T);
334 49 res.c_of_t_ = computeBezierCurve<bezier_t, point_t>(
335
3/6
✓ Branch 1 taken 49 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 49 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 49 times.
✗ Branch 8 not taken.
 49 pData.constraints_.flag_, T, pis, res.x);
336
1/2
✓ Branch 1 taken 49 times.
✗ Branch 2 not taken.
 49 computeFinalVelocity(res);
337
1/2
✓ Branch 1 taken 49 times.
✗ Branch 2 not taken.
 49 computeFinalAcceleration(res);
338
2/4
✓ Branch 1 taken 49 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 49 times.
✗ Branch 5 not taken.
 49 res.dL_of_t_ = bezier_t::zero(3, T);
339 49 }
340 61 return res;
341 }
342
343 /**
344 * @brief computeNumIneqContinuous compute the number of inequalitie required by
345 * all the phases
346 * @param pData
347 * @param Ts
348 * @param degree
349 * @param w_degree //FIXME : cannot use 2n+3 because for capturability the
350 * degree doesn't correspond (cf waypoints_c0_dc0_dc1 )
351 * @param useDD whether double description or force formulation is used to
352 * compute dynamic constraints)
353 * @return
354 */
355 36 long int computeNumIneqContinuous(const ProblemData& pData, const VectorX& Ts,
356 const int degree, const int w_degree,
357 const bool useDD) {
358 36 long int num_ineq = 0;
359
1/2
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
 36 centroidal_dynamics::MatrixXX Hrow;
360
1/2
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
 36 VectorX h;
361 36 for (std::vector<ContactData>::const_iterator it = pData.contacts_.begin();
362
2/2
✓ Branch 3 taken 104 times.
✓ Branch 4 taken 36 times.
 140 it != pData.contacts_.end(); ++it) {
363 // kinematics :
364 104 num_ineq += it->kin_.rows() * (degree + 1);
365 // stability :
366
2/2
✓ Branch 0 taken 52 times.
✓ Branch 1 taken 52 times.
 104 if (useDD) {
367
1/2
✓ Branch 2 taken 52 times.
✗ Branch 3 not taken.
 52 it->contactPhase_->getPolytopeInequalities(Hrow, h);
368 52 num_ineq += Hrow.rows() * (w_degree + 1);
369 }
370 }
371 // acceleration constraints : 6 per points
372 36 num_ineq += (degree - 1) * 6 * Ts.size();
373 36 return num_ineq;
374 36 }
375
376 /**
377 * @brief computeNumEqContinuous compute the number of equalities required by
378 * all the phases
379 * @param pData
380 * @param w_degree //FIXME : cannot use 2n+3 because for capturability the
381 * degree doesn't correspond (cf waypoints_c0_dc0_dc1 )
382 * @param useForce whether double description or force formulation is used to
383 * compute dynamic constraints)
384 * @param forceVarDim reference value that stores the total force variable size
385 * @return
386 */
387 36 long int computeNumEqContinuous(const ProblemData& pData, const int w_degree,
388 const bool useForce, long int& forceVarDim) {
389 36 long int numEq = 0;
390
2/2
✓ Branch 0 taken 18 times.
✓ Branch 1 taken 18 times.
 36 if (useForce) {
391 18 long int cSize = 0;
392 int currentDegree; // remove redundant equalities depending on more
393 // constraining problem
394 18 std::vector<ContactData>::const_iterator it2 = pData.contacts_.begin();
395 18 ++it2;
396 18 for (std::vector<ContactData>::const_iterator it = pData.contacts_.begin();
397
2/2
✓ Branch 3 taken 52 times.
✓ Branch 4 taken 18 times.
 70 it != pData.contacts_.end(); ++it, ++it2) {
398 52 currentDegree = w_degree + 1;
399
6/6
✓ Branch 0 taken 34 times.
✓ Branch 1 taken 18 times.
✓ Branch 4 taken 17 times.
✓ Branch 5 taken 17 times.
✓ Branch 6 taken 17 times.
✓ Branch 7 taken 35 times.
 52 if (cSize != 0 && it->contactPhase_->m_G_centr.cols() > cSize)
400 17 currentDegree -= 1;
401 52 cSize = it->contactPhase_->m_G_centr.cols();
402
6/6
✓ Branch 2 taken 34 times.
✓ Branch 3 taken 18 times.
✓ Branch 4 taken 17 times.
✓ Branch 5 taken 17 times.
✓ Branch 6 taken 17 times.
✓ Branch 7 taken 35 times.
 86 if (it2 != pData.contacts_.end() &&
403 34 it2->contactPhase_->m_G_centr.cols() <= cSize)
404 17 currentDegree -= 1;
405 52 forceVarDim += cSize * (currentDegree);
406 52 numEq += (numRowsForce) * (currentDegree);
407 }
408 }
409 36 return numEq;
410 }
411
412 36 std::pair<MatrixXX, VectorX> computeConstraintsContinuous(
413 const ProblemData& pData, const VectorX& Ts,
414 std::pair<MatrixXX, VectorX>& Dd, VectorX& minBounds,
415 VectorX& /*maxBounds*/) {
416 // determine whether to use force or double description
417 // based on equilibrium value
418 36 bool useDD =
419 36 pData.representation_ ==
420 DOUBLE_DESCRIPTION; //!(pData.contacts_.begin()->contactPhase_->getAlgorithm()
421 //!== centroidal_dynamics::EQUILIBRIUM_ALGORITHM_PP);
422
423
1/2
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
 36 double T = Ts.sum();
424
425 // create the curves for c and w with symbolic waypoints (ie. depend on y)
426
1/2
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
 36 bezier_wp_t::t_point_t wps_c = computeConstantWaypointsSymbolic(pData, T);
427
1/2
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
 36 bezier_wp_t::t_point_t wps_w = computeWwaypoints(pData, T);
428
1/2
✓ Branch 3 taken 36 times.
✗ Branch 4 not taken.
 36 bezier_wp_t c(wps_c.begin(), wps_c.end(), 0., T);
429
1/2
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
 36 bezier_wp_t ddc = c.compute_derivate(2);
430
1/2
✓ Branch 3 taken 36 times.
✗ Branch 4 not taken.
 36 bezier_wp_t w(wps_w.begin(), wps_w.end(), 0., T);
431
432 // for each splitted curves : add the constraints for each waypoints
433 72 const long int num_ineq = computeNumIneqContinuous(pData, Ts, (int)c.degree_,
434
1/2
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
 36 (int)w.degree_, useDD);
435 36 long int forceVarDim = 0;
436 const long int num_eq =
437
1/2
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
 36 computeNumEqContinuous(pData, (int)w.degree_, !useDD, forceVarDim);
438 36 long int totalVarDim = dimVarX + forceVarDim;
439 36 long int id_rows = 0;
440 36 long int id_cols = dimVarX; // start after x variable
441 // MatrixXX A = MatrixXX::Zero(num_ineq+forceVarDim,totalVarDim);
442
2/4
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 36 times.
✗ Branch 5 not taken.
 36 MatrixXX A = MatrixXX::Zero(num_ineq, totalVarDim);
443
2/4
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 36 times.
✗ Branch 5 not taken.
 36 VectorX b = VectorX::Zero(num_ineq);
444 36 MatrixXX& D = Dd.first;
445 36 VectorX& d = Dd.second;
446
2/4
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 36 times.
✗ Branch 5 not taken.
 36 D = MatrixXX::Zero(num_eq, totalVarDim);
447
2/4
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 36 times.
✗ Branch 5 not taken.
 36 d = VectorX::Zero(num_eq);
448
449 36 double current_t = 0.;
450
1/2
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
 36 ContactData phase;
451 int size_kin;
452
1/2
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
 36 MatrixXX mH;
453
1/2
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
 36 VectorX h;
454 int dimH;
455
3/6
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 36 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 36 times.
✗ Branch 8 not taken.
 36 Vector3 acc_bounds = Vector3::Ones() * pData.constraints_.maxAcceleration_;
456
457 36 long int cSize = 0;
458 36 long int rowIdx = 0;
459 // for each phases, split the curve and compute the waypoints of the splitted
460 // curves
461
2/2
✓ Branch 1 taken 104 times.
✓ Branch 2 taken 36 times.
 140 for (size_t id_phase = 0; id_phase < (size_t)Ts.size(); ++id_phase) {
462
2/4
✓ Branch 1 taken 104 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 104 times.
✗ Branch 5 not taken.
 104 bezier_wp_t cs = c.extract(current_t, Ts[id_phase] + current_t);
463
2/4
✓ Branch 1 taken 104 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 104 times.
✗ Branch 5 not taken.
 104 bezier_wp_t ddcs = ddc.extract(current_t, Ts[id_phase] + current_t);
464
2/4
✓ Branch 1 taken 104 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 104 times.
✗ Branch 5 not taken.
 104 bezier_wp_t ws = w.extract(current_t, Ts[id_phase] + current_t);
465
1/2
✓ Branch 1 taken 104 times.
✗ Branch 2 not taken.
 104 current_t += Ts[id_phase];
466
467
1/2
✓ Branch 2 taken 104 times.
✗ Branch 3 not taken.
 104 phase = pData.contacts_[id_phase];
468
469 // add kinematics constraints :
470 104 size_kin = (int)phase.kin_.rows();
471 104 for (bezier_wp_t::cit_point_t wpit = cs.waypoints().begin();
472
2/2
✓ Branch 3 taken 560 times.
✓ Branch 4 taken 104 times.
 664 wpit != cs.waypoints().end(); ++wpit) {
473
3/6
✓ Branch 2 taken 560 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 560 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 560 times.
✗ Branch 9 not taken.
 560 A.block(id_rows, 0, size_kin, 3) = (phase.Kin_ * wpit->first);
474
4/8
✓ Branch 2 taken 560 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 560 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 560 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 560 times.
✗ Branch 12 not taken.
 560 b.segment(id_rows, size_kin) = phase.kin_ - (phase.Kin_ * wpit->second);
475 560 id_rows += size_kin;
476 }
477 // add stability constraints
478
2/2
✓ Branch 0 taken 52 times.
✓ Branch 1 taken 52 times.
 104 if (useDD) {
479
1/2
✓ Branch 1 taken 52 times.
✗ Branch 2 not taken.
 52 updateH(pData, phase, mH, h, dimH);
480 52 for (bezier_wp_t::cit_point_t wpit = ws.waypoints().begin();
481
2/2
✓ Branch 4 taken 355 times.
✓ Branch 5 taken 52 times.
 407 wpit != ws.waypoints().end(); ++wpit)
482
1/2
✓ Branch 1 taken 355 times.
✗ Branch 2 not taken.
 355 assignStabilityConstraintsForTimeStep(mH, h, *wpit, dimH, id_rows, A, b,
483 355 phase.contactPhase_->m_gravity);
484 } else {
485 52 bezier_wp_t::cit_point_t start = ws.waypoints().begin();
486 52 bezier_wp_t::cit_point_t stop = ws.waypoints().end();
487
6/6
✓ Branch 1 taken 34 times.
✓ Branch 2 taken 18 times.
✓ Branch 3 taken 17 times.
✓ Branch 4 taken 17 times.
✓ Branch 5 taken 17 times.
✓ Branch 6 taken 35 times.
 86 if (id_phase + 1 < (size_t)Ts.size() &&
488 34 pData.contacts_[id_phase + 1].contactPhase_->m_G_centr.cols() <=
489 34 phase.contactPhase_->m_G_centr.cols())
490 17 --stop;
491
6/6
✓ Branch 0 taken 34 times.
✓ Branch 1 taken 18 times.
✓ Branch 3 taken 17 times.
✓ Branch 4 taken 17 times.
✓ Branch 5 taken 17 times.
✓ Branch 6 taken 35 times.
 52 if (cSize > 0 && cSize < phase.contactPhase_->m_G_centr.cols()) ++start;
492 52 cSize = phase.contactPhase_->m_G_centr.cols();
493
2/2
✓ Branch 1 taken 321 times.
✓ Branch 2 taken 52 times.
 373 for (bezier_wp_t::cit_point_t wpit = start; wpit != stop;
494 321 ++wpit, rowIdx += 6)
495
1/2
✓ Branch 1 taken 321 times.
✗ Branch 2 not taken.
 321 assignStabilityConstraintsForTimeStepForce(
496 321 *wpit, rowIdx, id_cols, phase.contactPhase_->m_G_centr, D, d,
497
1/2
✓ Branch 1 taken 321 times.
✗ Branch 2 not taken.
 321 phase.contactPhase_->m_mass, phase.contactPhase_->m_gravity);
498 }
499 // add acceleration constraints :
500 104 for (bezier_wp_t::cit_point_t wpit = ddcs.waypoints().begin();
501
2/2
✓ Branch 3 taken 352 times.
✓ Branch 4 taken 104 times.
 456 wpit != ddcs.waypoints().end(); ++wpit) {
502
2/4
✓ Branch 2 taken 352 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 352 times.
✗ Branch 6 not taken.
 352 A.block(id_rows, 0, 3, 3) = wpit->first; // upper
503
3/6
✓ Branch 2 taken 352 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 352 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 352 times.
✗ Branch 9 not taken.
 352 b.segment(id_rows, 3) = acc_bounds - wpit->second;
504
3/6
✓ Branch 2 taken 352 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 352 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 352 times.
✗ Branch 9 not taken.
 352 A.block(id_rows + 3, 0, 3, 3) = -wpit->first; // lower
505
3/6
✓ Branch 2 taken 352 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 352 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 352 times.
✗ Branch 9 not taken.
 352 b.segment(id_rows + 3, 3) = acc_bounds + wpit->second;
506 352 id_rows += 6;
507 }
508 104 }
509
2/2
✓ Branch 0 taken 18 times.
✓ Branch 1 taken 18 times.
 36 if (!useDD) {
510 // add positive constraints on forces
511 // A.block(id_rows,3,forceVarDim,forceVarDim)=MatrixXX::Identity(forceVarDim,forceVarDim)*(-1);
512 // id_rows += forceVarDim;
513
1/2
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
 18 minBounds = VectorX::Zero(
514
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 18 A.cols()); // * (-std::numeric_limits<double>::infinity());
515
4/8
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 18 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 18 times.
✗ Branch 11 not taken.
 18 minBounds.head<3>() = VectorX::Ones(3) * (solvers::UNBOUNDED_DOWN);
516
3/6
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 18 times.
✗ Branch 8 not taken.
 18 minBounds.tail(forceVarDim) = VectorX::Zero(forceVarDim);
517 }
518
1/2
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
 36 assert(id_rows == (A.rows()) &&
519 "The inequality constraints matrices were not fully filled.");
520
1/2
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
 36 assert(id_rows == (b.rows()) &&
521 "The inequality constraints matrices were not fully filled.");
522
1/2
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
 36 assert(id_cols == (D.cols()) &&
523 "The equality constraints matrices were not fully filled.");
524
1/2
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
 36 assert(rowIdx == (d.rows()) &&
525 "The equality constraints matrices were not fully filled.");
526 // int out = Normalize(D,d);
527
1/2
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
 72 return std::make_pair(A, b);
528 36 }
529
530 23 ResultDataCOMTraj computeCOMTrajFixedSize(const ProblemData& pData,
531 const VectorX& Ts,
532 const unsigned int pointsPerPhase) {
533
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 23 times.
 23 assert(pData.representation_ == DOUBLE_DESCRIPTION);
534
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 23 times.
 23 if (Ts.size() != (int)pData.contacts_.size())
535 ✗ throw std::runtime_error(
536 "Time phase vector has different size than the number of contact "
537 ✗ "phases");
538
1/2
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
 23 double T = Ts.sum();
539
1/2
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
 23 T_time timeArray = computeDiscretizedTimeFixed(Ts, pointsPerPhase);
540 std::pair<MatrixXX, VectorX> Ab =
541
1/2
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
 23 computeConstraintsOneStep(pData, Ts, T, timeArray);
542 std::pair<MatrixXX, VectorX> Hg =
543
1/2
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
 23 genCostFunction(pData, Ts, T, timeArray, dimVarX);
544
2/4
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23 times.
✗ Branch 5 not taken.
 23 VectorX x = VectorX::Zero(dimVarX);
545
1/2
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
 23 ResultData resQp = solve(Ab, Hg, x);
546 #if PRINT_QHULL_INEQ
547 if (resQp.success_) printQHullFile(Ab, resQp.x, "bezier_wp.txt");
548 #endif
549
2/4
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23 times.
✗ Branch 5 not taken.
 46 return genTraj(resQp, pData, T);
550 23 }
551
552 39 ResultDataCOMTraj computeCOMTraj(const ProblemData& pData, const VectorX& Ts,
553 const double timeStep,
554 const solvers::SolverType solver) {
555
2/2
✓ Branch 2 taken 1 times.
✓ Branch 3 taken 38 times.
 39 if (Ts.size() != (int)pData.contacts_.size())
556
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 throw std::runtime_error(
557 "Time phase vector has different size than the number of contact "
558 2 "phases");
559
1/2
✓ Branch 1 taken 38 times.
✗ Branch 2 not taken.
 38 double T = Ts.sum();
560 38 T_time timeArray;
561
1/2
✓ Branch 1 taken 38 times.
✗ Branch 2 not taken.
 38 std::pair<MatrixXX, VectorX> Ab;
562
1/2
✓ Branch 1 taken 38 times.
✗ Branch 2 not taken.
 38 std::pair<MatrixXX, VectorX> Dd;
563
2/4
✓ Branch 1 taken 38 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 38 times.
✗ Branch 5 not taken.
 38 VectorX minBounds, maxBounds;
564
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 36 times.
 38 if (timeStep > 0) { // discretized
565
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 2 times.
 2 assert(pData.representation_ == DOUBLE_DESCRIPTION);
566
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 timeArray = computeDiscretizedTime(Ts, timeStep);
567
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 Ab = computeConstraintsOneStep(pData, Ts, T, timeArray);
568
4/8
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 2 times.
✗ Branch 12 not taken.
 2 Dd = std::make_pair(MatrixXX::Zero(0, Ab.first.cols()), VectorX::Zero(0));
569
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 minBounds = VectorX::Zero(0);
570
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 maxBounds = VectorX::Zero(0);
571 } else { // continuous
572
1/2
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
 36 Ab = computeConstraintsContinuous(pData, Ts, Dd, minBounds, maxBounds);
573
1/2
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
 72 timeArray = computeDiscretizedTimeFixed(
574 36 Ts, 7); // FIXME : hardcoded value for discretization for cost function
575 // in case of continuous formulation for the constraints
576 }
577 std::pair<MatrixXX, VectorX> Hg =
578
1/2
✓ Branch 2 taken 38 times.
✗ Branch 3 not taken.
 38 genCostFunction(pData, Ts, T, timeArray, Ab.first.cols());
579
2/4
✓ Branch 2 taken 38 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 38 times.
✗ Branch 6 not taken.
 38 VectorX x = VectorX::Zero(Ab.first.cols());
580
3/6
✓ Branch 1 taken 38 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 38 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 38 times.
✗ Branch 8 not taken.
 76 ResultData resQp = solve(Ab, Dd, Hg, minBounds, maxBounds, x, solver);
581 #if PRINT_QHULL_INEQ
582 if (resQp.success_) printQHullFile(Ab, resQp.x, "bezier_wp.txt");
583 #endif
584
2/4
✓ Branch 1 taken 38 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 38 times.
✗ Branch 5 not taken.
 76 return genTraj(resQp, pData, T);
585 38 }
586
587 } // namespace bezier_com_traj
588