Directory: | ./ |
---|---|
File: | src/computeCOMTraj.cpp |
Date: | 2025-03-18 04:20:50 |
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 |