GCC Code Coverage Report

Directory: ./
File: src/path-optimization/spline-gradient-based.cc
Date: 2024-08-10 11:29:48
Exec Total Coverage
Lines: 216 311 69.5%
Branches: 245 662 37.0%
Line Branch Exec Source
1 // Copyright (c) 2017, Joseph Mirabel
2 // Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
3 //
4
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are
7 // met:
8 //
9 // 1. Redistributions of source code must retain the above copyright
10 // notice, this list of conditions and the following disclaimer.
11 //
12 // 2. Redistributions in binary form must reproduce the above copyright
13 // notice, this list of conditions and the following disclaimer in the
14 // documentation and/or other materials provided with the distribution.
15 //
16 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
17 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
18 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
19 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
20 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
21 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
22 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
23 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
24 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
26 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
27 // DAMAGE.
28
29 #include <hpp/constraints/svd.hh>
30 #include <hpp/core/collision-path-validation-report.hh>
31 #include <hpp/core/config-projector.hh>
32 #include <hpp/core/path-optimization/quadratic-program.hh>
33 #include <hpp/core/path-optimization/spline-gradient-based.hh>
34 #include <hpp/core/problem.hh>
35 #include <hpp/pinocchio/device.hh>
36 #include <hpp/util/exception-factory.hh>
37 #include <hpp/util/timer.hh>
38 #include <path-optimization/spline-gradient-based/collision-constraint.hh>
39 #include <path-optimization/spline-gradient-based/cost.hh>
40
41 namespace hpp {
42 namespace core {
43 using constraints::ExplicitConstraintSet;
44 using constraints::solver::BySubstitution;
45 using pinocchio::Device;
46
47 namespace pathOptimization {
48 typedef Eigen::Matrix<value_type, Eigen::Dynamic, Eigen::Dynamic,
49 Eigen::RowMajor>
50 RowMajorMatrix_t;
51 typedef Eigen::Map<const vector_t> ConstVectorMap_t;
52 typedef Eigen::Map<vector_t> VectorMap_t;
53
54 typedef Eigen::BlockIndex BlockIndex;
55
56 HPP_DEFINE_TIMECOUNTER(SGB_findNewConstraint);
57
58 template <int NbRows>
59 VectorMap_t reshape(Eigen::Matrix<value_type, NbRows, Eigen::Dynamic,
60 Eigen::RowMajor>& parameters) {
61 return VectorMap_t(parameters.data(), parameters.size());
62 }
63
64 template <int NbRows>
65 ConstVectorMap_t reshape(const Eigen::Matrix<value_type, NbRows, Eigen::Dynamic,
66 Eigen::RowMajor>& parameters) {
67 return ConstVectorMap_t(parameters.data(), parameters.size());
68 }
69
70 template <int _PB, int _SO>
71 30 SplineGradientBased<_PB, _SO>::SplineGradientBased(
72 const ProblemConstPtr_t& problem)
73 30 : Base(problem), checkOptimum_(false) {}
74
75 // ----------- Convenience class -------------------------------------- //
76
77 /// TODO Two options:
78 /// - Split this class into two classes:
79 /// - Move generic part outside of this class
80 /// - Keep linearization
81 /// - Move all the code outside
82 template <int _PB, int _SO>
83 struct SplineGradientBased<_PB, _SO>::CollisionFunctions {
84 6 void addConstraint(const typename CollisionFunction<SplinePtr_t>::Ptr_t& f,
85 const std::size_t& idx, const size_type& row,
86 const value_type& r) {
87
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 3 times.
 6 assert(f->outputSize() == 1);
88 6 functions.push_back(f);
89 6 splineIds.push_back(idx);
90 6 rows.push_back(row);
91 6 ratios.push_back(r);
92 }
93
94 ✗ void removeLastConstraint(const std::size_t& n, LinearConstraint& lc) {
95 ✗ assert(functions.size() >= n && std::size_t(lc.J.rows()) >= n);
96
97 ✗ const std::size_t nSize = functions.size() - n;
98 ✗ functions.resize(nSize);
99 ✗ splineIds.resize(nSize);
100 ✗ rows.resize(nSize);
101 ✗ ratios.resize(nSize);
102
103 ✗ lc.J.conservativeResize(lc.J.rows() - n, lc.J.cols());
104 ✗ lc.b.conservativeResize(lc.b.rows() - n, lc.b.cols());
105 }
106
107 // Compute linearization
108 // b = f(S(t))
109 // J = Jf(S(p, t)) * dS/dp
110 // f(S(t)) = b -> J * P = b
111 6 void linearize(const SplinePtr_t& spline, const SplineOptimizationData& sod,
112 const std::size_t& fIdx, LinearConstraint& lc) {
113 6 const typename CollisionFunction<SplinePtr_t>::Ptr_t& f = functions[fIdx];
114
115 6 const size_type row = rows[fIdx], nbRows = 1,
116 6 rDof = f->inputDerivativeSize();
117
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 6 const value_type t = spline->length() * ratios[fIdx];
118
119
1/2
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
 6 q.resize(f->inputSize());
120
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 6 spline->eval(q, t);
121
122 // Evaluate explicit functions
123
1/6
✗ Branch 1 not taken.
✓ Branch 2 taken 3 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
 6 if (sod.es) sod.es->solve(q);
124
125
1/2
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
 6 LiegroupElement v(f->outputSpace());
126
3/6
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
 6 f->value(v, q);
127
128
1/2
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 6 J.resize(f->outputSize(), f->inputDerivativeSize());
129
3/6
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
 6 f->jacobian(J, q);
130
131 // Apply chain rule if necessary
132
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 3 times.
 6 if (sod.es) {
133 ✗ Js.resize(sod.es->nv(), sod.es->nv());
134 ✗ sod.es->jacobian(Js, q);
135
136 ✗ sod.es->notOutDers().lview(J) =
137 ✗ sod.es->notOutDers().lview(J).eval() +
138 ✗ sod.es->outDers().transpose().rview(J).eval() *
139 ✗ sod.es->jacobianNotOutToOut(Js).eval();
140 ✗ sod.es->outDers().transpose().lview(J).setZero();
141 }
142
143
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 6 Spline::timeFreeBasisFunctionDerivative(0, ratios[fIdx],
144 6 paramDerivativeCoeff);
145
146 6 const size_type col = splineIds[fIdx] * Spline::NbCoeffs * rDof;
147
2/2
✓ Branch 0 taken 12 times.
✓ Branch 1 taken 3 times.
 30 for (size_type i = 0; i < Spline::NbCoeffs; ++i)
148
3/6
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12 times.
✗ Branch 8 not taken.
 24 lc.J.block(row, col + i * rDof, nbRows, rDof).noalias() =
149
2/4
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
 24 paramDerivativeCoeff(i) * J;
150
151
4/8
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
 12 lc.b.segment(row, nbRows) =
152
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 6 lc.J.block(row, col, nbRows, Spline::NbCoeffs * rDof) *
153 spline->rowParameters();
154 }
155
156 ✗ void linearize(const Splines_t& splines, const SplineOptimizationDatas_t& ss,
157 LinearConstraint& lc) {
158 ✗ for (std::size_t i = 0; i < functions.size(); ++i)
159 ✗ linearize(splines[splineIds[i]], ss[i], i, lc);
160 }
161
162 std::vector<typename CollisionFunction<SplinePtr_t>::Ptr_t> functions;
163 std::vector<std::size_t> splineIds;
164 std::vector<size_type> rows;
165 std::vector<value_type> ratios;
166
167 mutable Configuration_t q;
168 mutable matrix_t J, Js;
169 mutable typename Spline::BasisFunctionVector_t paramDerivativeCoeff;
170 };
171
172 // ----------- Resolution steps --------------------------------------- //
173
174 template <int _PB, int _SO>
175 typename SplineGradientBased<_PB, _SO>::Ptr_t
176 30 SplineGradientBased<_PB, _SO>::create(const ProblemConstPtr_t& problem) {
177
1/2
✓ Branch 2 taken 15 times.
✗ Branch 3 not taken.
 30 SplineGradientBased* ptr = new SplineGradientBased(problem);
178 30 Ptr_t shPtr(ptr);
179 30 return shPtr;
180 }
181
182 template <int _PB, int _SO>
183 30 void SplineGradientBased<_PB, _SO>::addProblemConstraints(
184 const PathVectorPtr_t& init, const Splines_t& splines, LinearConstraint& lc,
185 SplineOptimizationDatas_t& ss) const {
186
2/4
✓ Branch 3 taken 15 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 15 times.
✗ Branch 8 not taken.
 30 assert(init->numberPaths() == splines.size() && ss.size() == splines.size());
187
2/2
✓ Branch 1 taken 51 times.
✓ Branch 2 taken 15 times.
 132 for (std::size_t i = 0; i < splines.size(); ++i) {
188
2/4
✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 51 times.
✗ Branch 8 not taken.
 102 addProblemConstraintOnPath(init->pathAtRank(i), i, splines[i], lc, ss[i]);
189 }
190 30 }
191
192 template <int _PB, int _SO>
193 102 void SplineGradientBased<_PB, _SO>::addProblemConstraintOnPath(
194 const PathPtr_t& path, const size_type& idxSpline,
195 const SplinePtr_t& spline, LinearConstraint& lc,
196 SplineOptimizationData& sod) const {
197 102 ConstraintSetPtr_t cs = path->constraints();
198
2/2
✓ Branch 1 taken 24 times.
✓ Branch 2 taken 27 times.
 102 if (cs) {
199
1/2
✓ Branch 2 taken 24 times.
✗ Branch 3 not taken.
 48 ConfigProjectorPtr_t cp = cs->configProjector();
200
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 24 times.
 48 if (cp) {
201 ✗ const BySubstitution& hs = cp->solver();
202 ✗ const ExplicitConstraintSet& es = hs.explicitConstraintSet();
203
204 // Get the active parameter row selection.
205 ✗ value_type guessThreshold =
206 problem()
207 ✗ ->getParameter("SplineGradientBased/guessThreshold")
208 ✗ .floatValue();
209 ✗ Eigen::RowBlockIndices select =
210 computeActiveParameters(path, hs, guessThreshold);
211
212 ✗ const size_type rDof = robot_->numberDof(),
213 ✗ col = idxSpline * Spline::NbCoeffs * rDof,
214 ✗ row = lc.J.rows(), nOutVar = select.nbIndices();
215
216 ✗ sod.set = cs;
217 ✗ sod.es.reset(new ExplicitConstraintSet(es));
218 ✗ sod.activeParameters = RowBlockIndices(BlockIndex::difference(
219 ✗ BlockIndex::segment_t(0, rDof), select.indices()));
220 hppDout(info,
221 "Path " << idxSpline << ": do not change this dof " << select);
222 hppDout(info,
223 "Path " << idxSpline << ": active dofs " << sod.activeParameters);
224
225 // Add nOutVar constraint per coefficient.
226 ✗ lc.addRows(Spline::NbCoeffs * nOutVar);
227 ✗ matrix_t I = select.rview(matrix_t::Identity(rDof, rDof));
228 ✗ for (size_type k = 0; k < Spline::NbCoeffs; ++k) {
229 ✗ lc.J.block(row + k * nOutVar, col + k * rDof, nOutVar, rDof) = I;
230 ✗ lc.b.segment(row + k * nOutVar, nOutVar) =
231 ✗ I * spline->parameters().row(k).transpose();
232 }
233
234 ✗ assert((lc.J.block(row, col, Spline::NbCoeffs * nOutVar,
235 rDof * Spline::NbCoeffs) *
236 spline->rowParameters())
237 .isApprox(lc.b.segment(row, Spline::NbCoeffs * nOutVar)));
238 }
239 }
240 102 }
241
242 template <int _PB, int _SO>
243 ✗ Eigen::RowBlockIndices SplineGradientBased<_PB, _SO>::computeActiveParameters(
244 const PathPtr_t& path, const BySubstitution& hs, const value_type& guessThr,
245 const bool& useExplicitInput) const {
246 ✗ const ExplicitConstraintSet& es = hs.explicitConstraintSet();
247
248 ✗ BlockIndex::segments_t implicitBI, explicitBI;
249
250 // Handle implicit part
251 ✗ if (hs.reducedDimension() > 0) {
252 ✗ implicitBI = hs.implicitDof();
253
254 hppDout(info, "Solver " << hs << '\n'
255 << Eigen::RowBlockIndices(implicitBI));
256
257 // in the case of PR2 passing a box from right to left hand,
258 // the double grasp is a loop closure so the DoF of the base are
259 // not active (one can see this in the Jacobian).
260 // They should be left unconstrained.
261 // TODO I do not see any good way of guessing this since it is
262 // the DoF of the base are not active only on the submanifold
263 // satisfying the constraint. It has to be dealt with in
264 // hpp-manipulation.
265
266 // If requested, check if the jacobian has columns of zeros.
267 ✗ BlockIndex::segments_t passive;
268 ✗ if (guessThr >= 0) {
269 ✗ matrix_t J(hs.reducedDimension(), hs.freeVariables().nbIndices());
270 ✗ hs.computeValue<true>(path->initial());
271 ✗ hs.updateJacobian(path->initial());
272 ✗ hs.getReducedJacobian(J);
273 ✗ size_type j = 0, k = 0;
274 ✗ for (size_type r = 0; r < J.cols(); ++r) {
275 ✗ if (J.col(r).isZero(guessThr)) {
276 ✗ size_type idof = es.notOutDers().indices()[j].first + k;
277 ✗ passive.push_back(BlockIndex::segment_t(idof, 1));
278 hppDout(info,
279 "Deactivated dof (thr=" << guessThr << ") " << idof
280 << ". J = " << J.col(r).transpose());
281 }
282 ✗ k++;
283 ✗ if (k >= hs.freeVariables().indices()[j].second) {
284 ✗ j++;
285 ✗ k = 0;
286 }
287 }
288 ✗ BlockIndex::sort(passive);
289 ✗ BlockIndex::shrink(passive);
290 hppDout(info, "Deactivated dof (thr=" << guessThr << ") "
291 << Eigen::ColBlockIndices(passive)
292 << "J = " << J);
293 ✗ implicitBI = BlockIndex::difference(implicitBI, passive);
294 }
295 ✗ } else if (useExplicitInput) {
296 ✗ Eigen::ColBlockIndices esadp = es.activeDerivativeParameters();
297 ✗ implicitBI = esadp.indices();
298 }
299
300 // Handle explicit part
301 ✗ explicitBI = es.outDers().indices();
302
303 // Add both
304 ✗ implicitBI.insert(implicitBI.end(), explicitBI.begin(), explicitBI.end());
305 ✗ Eigen::RowBlockIndices rbi(implicitBI);
306 ✗ rbi.updateIndices<true, true, true>();
307 ✗ return rbi;
308 }
309
310 template <int _PB, int _SO>
311 6 void SplineGradientBased<_PB, _SO>::addCollisionConstraint(
312 const std::size_t idxSpline, const SplinePtr_t& spline,
313 const SplinePtr_t& nextSpline, const SplineOptimizationData& sod,
314 const PathValidationReportPtr_t& report, LinearConstraint& collision,
315 CollisionFunctions& functions) const {
316 hppDout(info, "Collision on spline "
317 << idxSpline << " at ratio (in [0,1]) = "
318 << report->parameter / nextSpline->length());
319 6 typename CollisionFunction<SplinePtr_t>::Ptr_t cc(
320
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 6 CollisionFunction<SplinePtr_t>::create(robot_, spline, nextSpline,
321 report));
322
323
1/2
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
 6 collision.addRows(cc->outputSize());
324
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 6 functions.addConstraint(cc, idxSpline, collision.J.rows() - 1,
325
1/2
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
 6 report->parameter / nextSpline->length());
326
327
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 6 functions.linearize(spline, sod, functions.functions.size() - 1, collision);
328 }
329
330 template <int _PB, int _SO>
331 6 bool SplineGradientBased<_PB, _SO>::findNewConstraint(
332 LinearConstraint& constraint, LinearConstraint& collision,
333 LinearConstraint& collisionReduced, CollisionFunctions& functions,
334 const std::size_t iF, const SplinePtr_t& spline,
335 const SplineOptimizationData& sod) const {
336 HPP_SCOPE_TIMECOUNTER(SGB_findNewConstraint);
337 6 bool solved = false;
338
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 6 Configuration_t q(robot_->configSize());
339 6 typename CollisionFunction<SplinePtr_t>::Ptr_t function(
340 6 functions.functions[iF]);
341
342
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 6 solved = constraint.reduceConstraint(collision, collisionReduced);
343
344 6 size_type i = 5;
345
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 3 times.
 6 while (not solved) {
346 ✗ if (i == 0) {
347 ✗ functions.removeLastConstraint(1, collision);
348 hppDout(warning,
349 "Could not find a suitable collision constraint. Removing it.");
350 ✗ return false;
351 }
352 hppDout(info,
353 "Looking for collision which does not make the constraint rank "
354 "deficient.");
355 // interpolate at alpha
356 ✗ pinocchio::interpolate<pinocchio::RnxSOnLieGroupMap>(
357 ✗ robot_, function->qFree_, function->qColl_, 0.5, q);
358 hppDout(info, "New q: " << q.transpose());
359 // update the constraint
360 ✗ function->updateConstraint(q);
361 ✗ functions.linearize(spline, sod, iF, collision);
362 // check the rank
363 ✗ solved = constraint.reduceConstraint(collision, collisionReduced, true);
364 ✗ --i;
365 }
366 6 return true;
367 }
368
369 // ----------- Optimize ----------------------------------------------- //
370
371 template <int _PB, int _SO>
372 30 PathVectorPtr_t SplineGradientBased<_PB, _SO>::optimize(
373 const PathVectorPtr_t& path) {
374
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 30 this->monitorExecution();
375
376 // Get some parameters
377 value_type alphaInit =
378
3/6
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 15 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 15 times.
✗ Branch 11 not taken.
 30 problem()->getParameter("SplineGradientBased/alphaInit").floatValue();
379 bool alwaysStopAtFirst =
380 problem()
381
2/4
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 15 times.
✗ Branch 8 not taken.
 60 ->getParameter("SplineGradientBased/alwaysStopAtFirst")
382
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 30 .boolValue();
383 size_type costOrder =
384
3/6
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 15 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 15 times.
✗ Branch 11 not taken.
 30 problem()->getParameter("SplineGradientBased/costOrder").intValue();
385 bool usePathLengthAsWeights =
386 problem()
387
2/4
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 15 times.
✗ Branch 8 not taken.
 60 ->getParameter("SplineGradientBased/usePathLengthAsWeights")
388
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 30 .boolValue();
389 bool reorderIntervals =
390 problem()
391
2/4
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 15 times.
✗ Branch 8 not taken.
 60 ->getParameter("SplineGradientBased/reorderIntervals")
392
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 30 .boolValue();
393 bool linearizeAtEachStep =
394 problem()
395
2/4
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 15 times.
✗ Branch 8 not taken.
 60 ->getParameter("SplineGradientBased/linearizeAtEachStep")
396
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 30 .boolValue();
397 bool checkJointBound =
398 problem()
399
2/4
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 15 times.
✗ Branch 8 not taken.
 60 ->getParameter("SplineGradientBased/checkJointBound")
400
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 30 .boolValue();
401 bool returnOptimum =
402
3/6
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 15 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 15 times.
✗ Branch 11 not taken.
 30 problem()->getParameter("SplineGradientBased/returnOptimum").boolValue();
403 value_type costThreshold =
404
3/6
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 15 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 15 times.
✗ Branch 11 not taken.
 30 problem()->getParameter("SplineGradientBased/costThreshold").floatValue();
405 bool useProxqp =
406
3/6
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 15 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 15 times.
✗ Branch 11 not taken.
 30 problem()->getParameter("SplineGradientBased/useProxqp").boolValue();
407 value_type eps_abs(
408
3/6
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 15 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 15 times.
✗ Branch 11 not taken.
 30 problem()->getParameter("SplineGradientBased/QPAccuracy").floatValue());
409
2/4
✓ Branch 2 taken 15 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 15 times.
 30 if (path->length() == 0) return path;
410
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 30 PathVectorPtr_t input = Base::cleanInput(path);
411
412
1/2
✓ Branch 2 taken 15 times.
✗ Branch 3 not taken.
 30 const size_type rDof = robot_->numberDof();
413
414 // 1
415 // Replace each path of the vector by a spline with 0 derivatives at
416 // start and end.
417 30 Splines_t splines;
418
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 30 this->appendEquivalentSpline(input, splines);
419 30 const size_type nParameters = splines.size() * Spline::NbCoeffs;
420
421 // Initialize one path validation method for each spline.
422 // Path validation methods are retrieve in the transition of the
423 // constraint graph that produced the initial part of the path.
424
1/2
✓ Branch 3 taken 15 times.
✗ Branch 4 not taken.
 30 std::vector<std::size_t> collisionReordering(splines.size());
425
2/2
✓ Branch 2 taken 51 times.
✓ Branch 3 taken 15 times.
 132 for (std::size_t i = 0; i < splines.size(); ++i) collisionReordering[i] = i;
426
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 30 this->initializePathValidation(splines);
427
428 // 2
429 enum { MaxContinuityOrder = int((SplineOrder - 1) / 2) };
430 30 const size_type orderContinuity = MaxContinuityOrder;
431
432
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 30 LinearConstraint constraint(nParameters * rDof, 0);
433
1/2
✓ Branch 3 taken 15 times.
✗ Branch 4 not taken.
 60 SplineOptimizationDatas_t solvers(splines.size(),
434
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 60 SplineOptimizationData(rDof));
435
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 30 addProblemConstraints(input, splines, constraint, solvers);
436
437
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 30 this->addContinuityConstraints(splines, orderContinuity, solvers, constraint);
438
439 // 3
440
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 30 LinearConstraint collision(nParameters * rDof, 0);
441
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 30 CollisionFunctions collisionFunctions;
442
443 // 4
444
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 30 L2NormSquaredOfDerivative<Spline> cost(splines, rDof, rDof, costOrder);
445
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 15 times.
 30 if (usePathLengthAsWeights) {
446 ✗ cost.computeLambdasFromSplineLength(splines);
447 }
448
449 // 5
450 //
451 // true = check that the constraint is feasible.
452 // true = throws if the constraint is infeasible.
453
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 30 constraint.decompose(true, true);
454
455
1/2
✓ Branch 2 taken 15 times.
✗ Branch 3 not taken.
 30 LinearConstraint collisionReduced(constraint.PK.rows(), 0);
456
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 30 constraint.reduceConstraint(collision, collisionReduced);
457
458
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 30 LinearConstraint boundConstraint(nParameters * rDof, 0);
459
1/2
✓ Branch 0 taken 15 times.
✗ Branch 1 not taken.
 30 if (checkJointBound) {
460
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 30 this->jointBoundConstraint(splines, boundConstraint);
461
2/4
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 15 times.
 30 if (!this->validateBounds(splines, boundConstraint).empty())
462 ✗ throw std::invalid_argument("Input path does not satisfy joint bounds");
463 }
464
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 30 LinearConstraint boundConstraintReduced(0, 0);
465
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 30 constraint.reduceConstraint(boundConstraint, boundConstraintReduced, false);
466
467 // 6
468 30 bool noCollision = true, stopAtFirst = alwaysStopAtFirst;
469 30 bool minimumReached = false;
470
471 30 bool computeOptimum = true,
472
3/4
✓ Branch 0 taken 15 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 6 times.
✓ Branch 3 taken 9 times.
 30 computeInterpolatedSpline = !(checkOptimum_ || returnOptimum);
473
474
3/4
✓ Branch 0 taken 15 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 6 times.
✓ Branch 3 taken 9 times.
 30 value_type alpha = (checkOptimum_ || returnOptimum ? 1 : alphaInit);
475
476 30 Splines_t alphaSplines, collSplines;
477 30 Splines_t* currentSplines(0x0);
478
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 30 Base::copy(splines, alphaSplines);
479
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 30 Base::copy(splines, collSplines);
480 30 Reports_t reports;
481
482
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 30 QuadraticProgram QP(cost.inputDerivativeSize_, useProxqp);
483 30 QP.accuracy(eps_abs);
484 30 value_type optimalCost, costLowerBound = 0;
485
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 30 cost.value(optimalCost, splines);
486 hppDout(info, "Initial cost is " << optimalCost);
487
2/4
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
 30 cost.hessian(QP.H, splines);
488 #ifndef NDEBUG
489
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 30 checkHessian(cost, QP.H, splines);
490 #endif // NDEBUG
491
492
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 30 QuadraticProgram QPc(QP, constraint, useProxqp);
493 30 QPc.accuracy(eps_abs);
494
495
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 15 times.
 30 if (QPc.H.rows() == 0)
496 // There are no variables left for optimization.
497 ✗ return this->buildPathVector(splines);
498
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 30 QPc.computeLLT();
499
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 30 QPc.solve(collisionReduced, boundConstraintReduced);
500
501
8/10
✓ Branch 0 taken 33 times.
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 18 times.
✓ Branch 3 taken 15 times.
✓ Branch 5 taken 21 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 21 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 21 times.
✓ Branch 10 taken 15 times.
 72 while (!(noCollision && minimumReached) && !this->shouldStop()) {
502 // 6.1
503
2/2
✓ Branch 0 taken 18 times.
✓ Branch 1 taken 3 times.
 42 if (computeOptimum) {
504 // 6.2
505
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 36 constraint.computeSolution(QPc.xStar);
506
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 36 Base::updateSplines(collSplines, constraint.xSol);
507
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 36 cost.value(costLowerBound, collSplines);
508 hppDout(info, "Cost interval: [" << costLowerBound << ", " << optimalCost
509 << "]");
510 36 currentSplines = &collSplines;
511 36 minimumReached = true;
512 36 computeOptimum = false;
513 }
514
2/2
✓ Branch 0 taken 9 times.
✓ Branch 1 taken 12 times.
 42 if (computeInterpolatedSpline) {
515
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 18 Base::interpolate(splines, collSplines, alpha, alphaSplines);
516 18 currentSplines = &alphaSplines;
517 18 minimumReached = false;
518 18 computeInterpolatedSpline = false;
519 }
520
521 // 6.3.2 Check for collision
522
2/2
✓ Branch 0 taken 12 times.
✓ Branch 1 taken 9 times.
 42 if (!returnOptimum) {
523
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 24 reports = this->validatePath(*currentSplines, collisionReordering,
524 stopAtFirst, reorderIntervals);
525 24 noCollision = reports.empty();
526 } else {
527 18 minimumReached = true;
528 18 noCollision = true;
529 }
530
2/2
✓ Branch 0 taken 18 times.
✓ Branch 1 taken 3 times.
 42 if (noCollision) {
531
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 36 cost.value(optimalCost, *currentSplines);
532 hppDout(info, "Cost interval: [" << costLowerBound << ", " << optimalCost
533 << "]");
534 // Update the spline
535
2/2
✓ Branch 1 taken 63 times.
✓ Branch 2 taken 18 times.
 162 for (std::size_t i = 0; i < splines.size(); ++i)
536
3/6
✓ Branch 5 taken 63 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 63 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 63 times.
✗ Branch 12 not taken.
 126 splines[i]->rowParameters((*currentSplines)[i]->rowParameters());
537
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 18 times.
 36 if (linearizeAtEachStep) {
538 ✗ collisionFunctions.linearize(splines, solvers, collision);
539 ✗ constraint.reduceConstraint(collision, collisionReduced);
540 ✗ QPc.solve(collisionReduced, boundConstraintReduced);
541 hppDout(info, "linearized");
542 ✗ computeOptimum = true;
543 }
544 hppDout(info, "Improved path with alpha = " << alpha);
545
546 36 computeInterpolatedSpline = true;
547
4/4
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 12 times.
✓ Branch 3 taken 3 times.
✓ Branch 4 taken 15 times.
 48 if (!minimumReached && std::abs(optimalCost - costLowerBound) <
548
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 3 times.
 12 costThreshold * costLowerBound) {
549 hppDout(info, "Stopping because cost interval is small.");
550 6 minimumReached = true;
551 }
552 } else {
553
1/2
✓ Branch 0 taken 3 times.
✗ Branch 1 not taken.
 6 if (alpha != 1.) {
554
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 3 times.
 6 if (QPc.H.rows() <= collisionReduced.rank) {
555 hppDout(info, "No more constraints can be added."
556 << QP.H.rows() << " variables for "
557 << collisionReduced.rank
558 << " independant constraints.");
559 ✗ break;
560 }
561
562 6 bool ok = false;
563
2/2
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 3 times.
 12 for (std::size_t i = 0; i < reports.size(); ++i) {
564
1/2
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 6 addCollisionConstraint(reports[i].second, splines[reports[i].second],
565 6 (*currentSplines)[reports[i].second],
566 6 solvers[reports[i].second], reports[i].first,
567 collision, collisionFunctions);
568
569
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 6 ok |= findNewConstraint(
570 constraint, collision, collisionReduced, collisionFunctions,
571 6 collisionFunctions.functions.size() - 1,
572 6 splines[reports[i].second], solvers[reports[i].second]);
573
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 3 times.
 6 if (!ok) break;
574
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 3 times.
 6 if (QPc.H.rows() <= collisionReduced.rank) break;
575 }
576
577
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 3 times.
 6 if (!ok) {
578 hppDout(info,
579 "The collision constraint would be rank deficient. Removing "
580 "added constraint.");
581 ✗ if (alpha < alphaInit / (1 << 2)) {
582 hppDout(info, "Interruption because alpha became too small.");
583 ✗ break;
584 }
585 ✗ alpha *= 0.5;
586 ✗ stopAtFirst = alwaysStopAtFirst;
587
588 ✗ computeInterpolatedSpline = true;
589 } else {
590
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 6 QPc.solve(collisionReduced, boundConstraintReduced);
591 hppDout(info, "Added " << reports.size()
592 << " constraints. "
593 "Constraints size "
594 << collision.J.rows()
595 << "(rank=" << collisionReduced.rank
596 << ", ass=" << QPc.activeSetSize << ") / "
597 << QPc.H.cols());
598
599 // When adding a new constraint, try first minimum under this
600 // constraint. If this latter minimum is in collision,
601 // re-initialize alpha to alphaInit.
602 6 alpha = 1.;
603 6 stopAtFirst = true;
604 6 computeOptimum = true;
605 }
606 } else {
607 ✗ alpha = alphaInit;
608 ✗ stopAtFirst = alwaysStopAtFirst;
609 ✗ computeInterpolatedSpline = true;
610 }
611 }
612 42 this->endIteration();
613 }
614
615 // 7
616 HPP_DISPLAY_TIMECOUNTER(SGB_findNewConstraint);
617
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 30 return this->buildPathVector(splines);
618 30 }
619
620 // ----------- Convenience functions ---------------------------------- //
621
622 template <int _PB, int _SO>
623 template <typename Cost_t>
624 30 bool SplineGradientBased<_PB, _SO>::checkHessian(
625 const Cost_t& cost, const matrix_t& H, const Splines_t& splines) const {
626 value_type expected;
627
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 30 cost.value(expected, splines);
628
629
1/2
✓ Branch 2 taken 15 times.
✗ Branch 3 not taken.
 30 vector_t P(H.rows());
630
631
1/2
✓ Branch 2 taken 15 times.
✗ Branch 3 not taken.
 30 const size_type size = robot_->numberDof() * Spline::NbCoeffs;
632
2/2
✓ Branch 1 taken 51 times.
✓ Branch 2 taken 15 times.
 132 for (std::size_t i = 0; i < splines.size(); ++i)
633
3/6
✓ Branch 3 taken 51 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 51 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 51 times.
✗ Branch 10 not taken.
 102 P.segment(i * size, size) = splines[i]->rowParameters();
634
5/10
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 15 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 15 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 15 times.
✗ Branch 14 not taken.
 30 value_type result = 0.5 * P.transpose() * H * P;
635
636 60 bool ret = std::fabs(expected - result) <
637 30 Eigen::NumTraits<value_type>::dummy_precision();
638 30 if (!ret) {
639 hppDout(error, "Hessian of the cost is not correct: " << expected << " - "
640 << result << " = "
641 << expected - result);
642 }
643 30 return ret;
644 30 }
645
646 // ----------- Instanciate -------------------------------------------- //
647
648 // template class SplineGradientBased<path::CanonicalPolynomeBasis, 1>; //
649 // equivalent to StraightPath template class
650 // SplineGradientBased<path::CanonicalPolynomeBasis, 2>; template class
651 // SplineGradientBased<path::CanonicalPolynomeBasis, 3>;
652 template class SplineGradientBased<path::BernsteinBasis,
653 1>; // equivalent to StraightPath
654 // template class SplineGradientBased<path::BernsteinBasis, 2>;
655 template class SplineGradientBased<path::BernsteinBasis, 3>;
656 template class SplineGradientBased<path::BernsteinBasis, 5>;
657 template class SplineGradientBased<path::BernsteinBasis, 7>;
658
659 // ----------- Declare parameters ------------------------------------- //
660
661 2 HPP_START_PARAMETER_DECLARATION(SplineGradientBased)
662
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 Problem::declareParameter(
663
3/6
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
 4 ParameterDescription(Parameter::FLOAT, "SplineGradientBased/alphaInit",
664 "In [0,1]. The initial value used when interpolating "
665 "between non-colliding current solution and"
666 " the optimal colliding trajector.",
667
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 Parameter(0.2)));
668
4/8
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 2 times.
✗ Branch 13 not taken.
 2 Problem::declareParameter(ParameterDescription(
669 Parameter::BOOL, "SplineGradientBased/alwaysStopAtFirst",
670 "If true, consider only one (not all) collision constraint at each "
671 "iteration.",
672
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 Parameter(true)));
673
4/8
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 2 times.
✗ Branch 13 not taken.
 2 Problem::declareParameter(ParameterDescription(
674 Parameter::INT, "SplineGradientBased/costOrder",
675 "The order of the derivative used for the optimized cost function. This is "
676 "most likely 1, 2 or 3",
677
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 Parameter((size_type)1)));
678
4/8
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 2 times.
✗ Branch 13 not taken.
 2 Problem::declareParameter(ParameterDescription(
679 Parameter::BOOL, "SplineGradientBased/usePathLengthAsWeights",
680 "If true, the initial path length are used to weight the splines.",
681
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 Parameter(false)));
682
4/8
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 2 times.
✗ Branch 13 not taken.
 2 Problem::declareParameter(ParameterDescription(
683 Parameter::BOOL, "SplineGradientBased/reorderIntervals",
684 "If true, interval in collision are checked first at next iteration.",
685
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 Parameter(false)));
686
4/8
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 2 times.
✗ Branch 13 not taken.
 2 Problem::declareParameter(ParameterDescription(
687 Parameter::BOOL, "SplineGradientBased/linearizeAtEachStep",
688 "If true, collision constraint will be re-linearized at each iteration.",
689
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 Parameter(false)));
690
4/8
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 2 times.
✗ Branch 13 not taken.
 2 Problem::declareParameter(ParameterDescription(
691 Parameter::BOOL, "SplineGradientBased/checkJointBound",
692
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 "If true, joint bounds are enforced.", Parameter(true)));
693
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 Problem::declareParameter(
694
3/6
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
 4 ParameterDescription(Parameter::BOOL, "SplineGradientBased/returnOptimum",
695 "(for debugging purpose) If true, returns the optimum "
696 "regardless of collision.",
697
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 Parameter(false)));
698
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 Problem::declareParameter(
699
3/6
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
 4 ParameterDescription(Parameter::FLOAT, "SplineGradientBased/costThreshold",
700 "Stop optimizing if the cost improves less than this "
701 "threshold between two iterations.",
702
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 Parameter(0.01)));
703
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 Problem::declareParameter(
704
3/6
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
 4 ParameterDescription(Parameter::FLOAT, "SplineGradientBased/guessThreshold",
705 "Threshold used to check whether the Jacobian "
706 "contains rows of zeros, in which case the "
707 "corresponding DoF is considered passive.",
708
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 Parameter(-1.)));
709
4/8
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 2 times.
✗ Branch 13 not taken.
 2 Problem::declareParameter(ParameterDescription(
710 Parameter::BOOL, "SplineGradientBased/useProxqp",
711 "Use proxqp QP solver instead of eiquadprog_2011. Temporary parameter "
712 "that will be removed soon.",
713
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 Parameter(true)));
714
4/8
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 2 times.
✗ Branch 13 not taken.
 2 Problem::declareParameter(ParameterDescription(
715 Parameter::FLOAT, "SplineGradientBased/QPAccuracy",
716
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 "Accuracy of QP solver (only used by proxqp.", Parameter(1e-4)));
717 2 HPP_END_PARAMETER_DECLARATION(SplineGradientBased)
718 } // namespace pathOptimization
719 } // namespace core
720 } // namespace hpp
721