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 |