Directory: | ./ |
---|---|
File: | src/config-projector.cc |
Date: | 2024-08-10 11:29:48 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 87 | 197 | 44.2% |
Branches: | 39 | 253 | 15.4% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // | ||
2 | // Copyright (c) 2014 CNRS | ||
3 | // Authors: Florent Lamiraux | ||
4 | // | ||
5 | |||
6 | // Redistribution and use in source and binary forms, with or without | ||
7 | // modification, are permitted provided that the following conditions are | ||
8 | // met: | ||
9 | // | ||
10 | // 1. Redistributions of source code must retain the above copyright | ||
11 | // notice, this list of conditions and the following disclaimer. | ||
12 | // | ||
13 | // 2. Redistributions in binary form must reproduce the above copyright | ||
14 | // notice, this list of conditions and the following disclaimer in the | ||
15 | // documentation and/or other materials provided with the distribution. | ||
16 | // | ||
17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS | ||
18 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT | ||
19 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR | ||
20 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT | ||
21 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, | ||
22 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT | ||
23 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, | ||
24 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY | ||
25 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT | ||
26 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE | ||
27 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH | ||
28 | // DAMAGE. | ||
29 | |||
30 | #include <boost/serialization/weak_ptr.hpp> | ||
31 | #include <hpp/constraints/differentiable-function.hh> | ||
32 | #include <hpp/constraints/solver/by-substitution.hh> | ||
33 | #include <hpp/core/config-projector.hh> | ||
34 | #include <hpp/pinocchio/configuration.hh> | ||
35 | #include <hpp/pinocchio/device.hh> | ||
36 | #include <hpp/pinocchio/extra-config-space.hh> | ||
37 | #include <hpp/pinocchio/joint-collection.hh> | ||
38 | #include <hpp/pinocchio/joint.hh> | ||
39 | #include <hpp/pinocchio/serialization.hh> | ||
40 | #include <hpp/util/debug.hh> | ||
41 | #include <hpp/util/serialization.hh> | ||
42 | #include <hpp/util/timer.hh> | ||
43 | #include <limits> | ||
44 | #include <pinocchio/multibody/model.hpp> | ||
45 | |||
46 | namespace hpp { | ||
47 | namespace core { | ||
48 | using constraints::solver::BySubstitution; | ||
49 | typedef constraints::solver::lineSearch::Backtracking Backtracking_t; | ||
50 | typedef constraints::solver::lineSearch::ErrorNormBased ErrorNormBased_t; | ||
51 | typedef constraints::solver::lineSearch::FixedSequence FixedSequence_t; | ||
52 | typedef constraints::solver::lineSearch::Constant Constant_t; | ||
53 | |||
54 | ConfigProjector::LineSearchType ConfigProjector::defaultLineSearch_ = | ||
55 | ConfigProjector::FixedSequence; | ||
56 | |||
57 | ✗ | void ConfigProjector::defaultLineSearch(LineSearchType ls) { | |
58 | ✗ | defaultLineSearch_ = ls; | |
59 | } | ||
60 | |||
61 | HPP_DEFINE_REASON_FAILURE(REASON_MAX_ITER, "Max Iterations reached"); | ||
62 | HPP_DEFINE_REASON_FAILURE(REASON_ERROR_INCREASED, "Error increased"); | ||
63 | HPP_DEFINE_REASON_FAILURE(REASON_INFEASIBLE, "Problem infeasible"); | ||
64 | |||
65 | 81 | ConfigProjectorPtr_t ConfigProjector::create(const DevicePtr_t& robot, | |
66 | const std::string& name, | ||
67 | value_type errorThreshold, | ||
68 | size_type maxIterations) { | ||
69 | ConfigProjector* ptr = | ||
70 |
1/2✓ Branch 2 taken 81 times.
✗ Branch 3 not taken.
|
81 | new ConfigProjector(robot, name, errorThreshold, maxIterations); |
71 | 81 | ConfigProjectorPtr_t shPtr(ptr); | |
72 | 81 | ptr->init(shPtr); | |
73 | 81 | return shPtr; | |
74 | } | ||
75 | |||
76 | 3705932 | ConfigProjectorPtr_t ConfigProjector::createCopy( | |
77 | const ConfigProjectorPtr_t cp) { | ||
78 |
1/2✓ Branch 3 taken 3705932 times.
✗ Branch 4 not taken.
|
3705932 | ConfigProjector* ptr = new ConfigProjector(*cp); |
79 | 3705932 | ConfigProjectorPtr_t shPtr(ptr); | |
80 | 3705932 | ptr->init(shPtr); | |
81 | 3705932 | return shPtr; | |
82 | } | ||
83 | |||
84 | 81 | ConfigProjector::ConfigProjector(const DevicePtr_t& robot, | |
85 | const std::string& name, | ||
86 | value_type _errorThreshold, | ||
87 | 81 | size_type _maxIterations) | |
88 | : Constraint(name), | ||
89 | 81 | robot_(robot), | |
90 | 81 | lineSearchType_(defaultLineSearch_), | |
91 |
3/6✓ Branch 4 taken 81 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 81 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 81 times.
✗ Branch 11 not taken.
|
81 | solver_(new BySubstitution(robot->configSpace()->vectorSpacesMerged())), |
92 | 81 | weak_(), | |
93 |
3/6✓ Branch 3 taken 81 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 81 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 81 times.
✗ Branch 11 not taken.
|
162 | statistics_("ConfigProjector " + name) { |
94 |
1/2✓ Branch 1 taken 81 times.
✗ Branch 2 not taken.
|
81 | errorThreshold(_errorThreshold); |
95 |
1/2✓ Branch 1 taken 81 times.
✗ Branch 2 not taken.
|
81 | maxIterations(_maxIterations); |
96 |
1/2✓ Branch 1 taken 81 times.
✗ Branch 2 not taken.
|
81 | lastIsOptional(false); |
97 | 81 | solver_->saturation( | |
98 |
1/2✓ Branch 1 taken 81 times.
✗ Branch 2 not taken.
|
162 | make_shared<constraints::solver::saturation::Device>(robot_)); |
99 | 81 | } | |
100 | |||
101 | 3705932 | ConfigProjector::ConfigProjector(const ConfigProjector& cp) | |
102 | : Constraint(cp), | ||
103 | 3705932 | robot_(cp.robot_), | |
104 | 3705932 | lineSearchType_(cp.lineSearchType_), | |
105 |
2/4✓ Branch 1 taken 3705932 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3705932 times.
✗ Branch 5 not taken.
|
3705932 | solver_(new BySubstitution(*cp.solver_)), |
106 | 3705932 | weak_(), | |
107 |
1/2✓ Branch 3 taken 3705932 times.
✗ Branch 4 not taken.
|
7411864 | statistics_(cp.statistics_) {} |
108 | |||
109 |
1/2✓ Branch 0 taken 3705884 times.
✗ Branch 1 not taken.
|
14823536 | ConfigProjector::~ConfigProjector() { delete solver_; } |
110 | |||
111 | 3705932 | ConstraintPtr_t ConfigProjector::copy() const { | |
112 |
1/2✓ Branch 2 taken 3705932 times.
✗ Branch 3 not taken.
|
3705932 | return createCopy(weak_.lock()); |
113 | } | ||
114 | |||
115 | ✗ | bool ConfigProjector::contains( | |
116 | const constraints::ImplicitPtr_t& numericalConstraint) const { | ||
117 | ✗ | return solver_->contains(numericalConstraint); | |
118 | } | ||
119 | |||
120 | 15 | bool ConfigProjector::add(const constraints::ImplicitPtr_t& nm, | |
121 | const std::size_t priority) { | ||
122 | 15 | return solver_->add(nm, priority); | |
123 | } | ||
124 | |||
125 | ✗ | void ConfigProjector::computeValueAndJacobian(ConfigurationIn_t configuration, | |
126 | vectorOut_t value, | ||
127 | matrixOut_t reducedJacobian) { | ||
128 | ✗ | Configuration_t q(configuration); | |
129 | // q_{out} = f (q_{in}) | ||
130 | ✗ | solver_->explicitConstraintSet().solve(q); | |
131 | ✗ | solver_->computeValue<true>(q); | |
132 | ✗ | solver_->updateJacobian(q); // includes the jacobian of the explicit system | |
133 | ✗ | solver_->getValue(value); | |
134 | ✗ | solver_->getReducedJacobian(reducedJacobian); | |
135 | } | ||
136 | |||
137 | /// Convert vector of non locked degrees of freedom to vector of | ||
138 | /// all degrees of freedom | ||
139 | ✗ | void ConfigProjector::uncompressVector(vectorIn_t small, | |
140 | vectorOut_t normal) const { | ||
141 | ✗ | solver_->explicitConstraintSet().notOutDers().transpose().lview(normal) = | |
142 | ✗ | small; | |
143 | } | ||
144 | |||
145 | ✗ | void ConfigProjector::compressVector(vectorIn_t normal, | |
146 | vectorOut_t small) const { | ||
147 | small = | ||
148 | ✗ | solver_->explicitConstraintSet().notOutDers().transpose().rview(normal); | |
149 | } | ||
150 | |||
151 | ✗ | void ConfigProjector::compressMatrix(matrixIn_t normal, matrixOut_t small, | |
152 | bool rows) const { | ||
153 | ✗ | if (rows) { | |
154 | typedef Eigen::MatrixBlockView<matrixIn_t, Eigen::Dynamic, Eigen::Dynamic, | ||
155 | false, false> | ||
156 | View; | ||
157 | const Eigen::ColBlockIndices& cols = | ||
158 | ✗ | solver_->explicitConstraintSet().notOutDers(); | |
159 | ✗ | small = View(normal, cols.nbIndices(), cols.indices(), cols.nbIndices(), | |
160 | ✗ | cols.indices()); | |
161 | } else { | ||
162 | ✗ | small = solver_->explicitConstraintSet().notOutDers().rview(normal); | |
163 | } | ||
164 | } | ||
165 | |||
166 | ✗ | void ConfigProjector::uncompressMatrix(matrixIn_t small, matrixOut_t normal, | |
167 | bool rows) const { | ||
168 | ✗ | if (rows) { | |
169 | typedef Eigen::MatrixBlockView<matrixOut_t, Eigen::Dynamic, Eigen::Dynamic, | ||
170 | false, false> | ||
171 | View; | ||
172 | const Eigen::ColBlockIndices& cols = | ||
173 | ✗ | solver_->explicitConstraintSet().notOutDers(); | |
174 | ✗ | View(normal, cols.nbIndices(), cols.indices(), cols.nbIndices(), | |
175 | ✗ | cols.indices()) = small; | |
176 | } else { | ||
177 | ✗ | solver_->explicitConstraintSet().notOutDers().lview(normal) = small; | |
178 | } | ||
179 | } | ||
180 | |||
181 | 1861320 | bool ConfigProjector::impl_compute(ConfigurationOut_t configuration) { | |
182 | // If configuration satisfies the constraint, do not modify it | ||
183 |
3/4✓ Branch 2 taken 1861320 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1858108 times.
✓ Branch 6 taken 3212 times.
|
1861320 | if (isSatisfied(configuration)) return true; |
184 | BySubstitution::Status status = | ||
185 |
1/2✓ Branch 2 taken 3212 times.
✗ Branch 3 not taken.
|
3212 | (BySubstitution::Status)solverSolve(configuration); |
186 |
2/5✗ Branch 0 not taken.
✗ Branch 1 not taken.
✓ Branch 2 taken 36 times.
✓ Branch 3 taken 3176 times.
✗ Branch 4 not taken.
|
3212 | switch (status) { |
187 | ✗ | case BySubstitution::ERROR_INCREASED: | |
188 | ✗ | statistics_.addFailure(REASON_ERROR_INCREASED); | |
189 | ✗ | return false; | |
190 | break; | ||
191 | ✗ | case BySubstitution::MAX_ITERATION_REACHED: | |
192 | ✗ | statistics_.addFailure(REASON_MAX_ITER); | |
193 | ✗ | return false; | |
194 | break; | ||
195 | 36 | case BySubstitution::INFEASIBLE: | |
196 | 36 | statistics_.addFailure(REASON_INFEASIBLE); | |
197 | 36 | return false; | |
198 | break; | ||
199 | 3176 | case BySubstitution::SUCCESS: | |
200 | 3176 | statistics_.addSuccess(); | |
201 | 3176 | return true; | |
202 | break; | ||
203 | } | ||
204 | ✗ | return false; | |
205 | } | ||
206 | |||
207 | ✗ | bool ConfigProjector::optimize(ConfigurationOut_t configuration, | |
208 | std::size_t maxIter) { | ||
209 | ✗ | if (!lastIsOptional()) return true; | |
210 | ✗ | if (!isSatisfied(configuration)) return false; | |
211 | ✗ | const size_type maxIterSave = maxIterations(); | |
212 | ✗ | if (maxIter != 0) maxIterations(maxIter); | |
213 | hppDout(info, "before optimization: " << configuration.transpose()); | ||
214 | ✗ | BySubstitution::Status status = BySubstitution::MAX_ITERATION_REACHED; | |
215 | ✗ | switch (lineSearchType_) { | |
216 | ✗ | case Backtracking: { | |
217 | ✗ | Backtracking_t ls; | |
218 | ✗ | status = solver_->solve(configuration, true, ls); | |
219 | ✗ | break; | |
220 | } | ||
221 | ✗ | case ErrorNormBased: { | |
222 | ✗ | ErrorNormBased_t ls; | |
223 | ✗ | status = solver_->solve(configuration, true, ls); | |
224 | ✗ | break; | |
225 | } | ||
226 | ✗ | case FixedSequence: { | |
227 | ✗ | FixedSequence_t ls; | |
228 | ✗ | status = solver_->solve(configuration, true, ls); | |
229 | ✗ | break; | |
230 | } | ||
231 | ✗ | case Constant: { | |
232 | Constant_t ls; | ||
233 | ✗ | status = solver_->solve(configuration, true, ls); | |
234 | ✗ | break; | |
235 | } | ||
236 | } | ||
237 | ✗ | maxIterations(maxIterSave); | |
238 | hppDout(info, "After optimization: " << configuration.transpose()); | ||
239 | ✗ | if (status == BySubstitution::SUCCESS) | |
240 | ✗ | return true; | |
241 | ✗ | else if (status == BySubstitution::INFEASIBLE && isSatisfied(configuration)) | |
242 | ✗ | return true; | |
243 | else { | ||
244 | hppDout(info, "Optimization failed."); | ||
245 | ✗ | return false; | |
246 | } | ||
247 | } | ||
248 | |||
249 | 4631404 | void ConfigProjector::projectVectorOnKernel(ConfigurationIn_t from, | |
250 | vectorIn_t velocity, | ||
251 | vectorOut_t result) { | ||
252 |
3/6✓ Branch 2 taken 4631404 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4631404 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 4631404 times.
✗ Branch 9 not taken.
|
4631404 | solver_->projectVectorOnKernel(from, velocity, result); |
253 | 4631404 | } | |
254 | |||
255 | ✗ | void ConfigProjector::projectOnKernel(ConfigurationIn_t from, | |
256 | ConfigurationIn_t to, | ||
257 | ConfigurationOut_t result) { | ||
258 | ✗ | solver_->projectOnKernel(from, to, result); | |
259 | } | ||
260 | |||
261 | ✗ | std::ostream& ConfigProjector::print(std::ostream& os) const { | |
262 | ✗ | return os << "Config projector: " << name() << ", contains " << *solver_ | |
263 | ✗ | << decindent; | |
264 | } | ||
265 | |||
266 | 9281190 | bool ConfigProjector::isSatisfied(ConfigurationIn_t config) { | |
267 |
1/2✓ Branch 2 taken 9281190 times.
✗ Branch 3 not taken.
|
9281190 | return solver_->isSatisfied(config); |
268 | } | ||
269 | |||
270 | ✗ | bool ConfigProjector::isSatisfied(ConfigurationIn_t config, | |
271 | value_type errorThreshold) { | ||
272 | ✗ | return solver_->isSatisfied(config, errorThreshold); | |
273 | } | ||
274 | |||
275 | 56 | bool ConfigProjector::isSatisfied(ConfigurationIn_t config, vector_t& error) { | |
276 | 112 | error.resize(solver_->dimension() + | |
277 | 56 | solver_->explicitConstraintSet().errorSize()); | |
278 |
2/4✓ Branch 2 taken 56 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 56 times.
✗ Branch 6 not taken.
|
56 | return solver_->isSatisfied(config, error); |
279 | } | ||
280 | |||
281 | 5 | const NumericalConstraints_t& ConfigProjector::numericalConstraints() const { | |
282 | 5 | return solver_->numericalConstraints(); | |
283 | } | ||
284 | |||
285 | 64 | vector_t ConfigProjector::rightHandSideFromConfig(ConfigurationIn_t config) { | |
286 |
1/2✓ Branch 2 taken 64 times.
✗ Branch 3 not taken.
|
64 | return solver_->rightHandSideFromConfig(config); |
287 | } | ||
288 | |||
289 | ✗ | void ConfigProjector::rightHandSideFromConfig( | |
290 | const constraints::ImplicitPtr_t& nm, ConfigurationIn_t config) { | ||
291 | ✗ | if (!solver_->rightHandSideFromConfig(nm, config)) { | |
292 | ✗ | std::ostringstream os; | |
293 | ✗ | os << "Function \"" << nm->function().name() | |
294 | ✗ | << "\" was not found in the solver. Solver contains ("; | |
295 | ✗ | for (auto constraint : solver_->constraints()) { | |
296 | ✗ | os << "\"" << constraint->function().name() << "\","; | |
297 | } | ||
298 | ✗ | os << ")."; | |
299 | ✗ | throw std::runtime_error(os.str().c_str()); | |
300 | } | ||
301 | } | ||
302 | |||
303 | ✗ | void ConfigProjector::rightHandSide(const vector_t& small) { | |
304 | ✗ | solver_->rightHandSide(small); | |
305 | } | ||
306 | |||
307 | ✗ | void ConfigProjector::rightHandSide(const constraints::ImplicitPtr_t& nm, | |
308 | vectorIn_t rhs) { | ||
309 | ✗ | if (!solver_->rightHandSide(nm, rhs)) { | |
310 | ✗ | std::ostringstream os; | |
311 | ✗ | os << "Function \"" << nm->function().name() | |
312 | ✗ | << "\" was not found in the solver. Solver contains ("; | |
313 | ✗ | for (auto constraint : solver_->constraints()) { | |
314 | ✗ | os << "\"" << constraint->function().name() << "\","; | |
315 | } | ||
316 | ✗ | os << ")."; | |
317 | } | ||
318 | } | ||
319 | |||
320 | ✗ | vector_t ConfigProjector::rightHandSide() const { | |
321 | ✗ | return solver_->rightHandSide(); | |
322 | } | ||
323 | |||
324 | 9263500 | void ConfigProjector::rightHandSideAt(const value_type& s) { | |
325 | 9263500 | solver_->rightHandSideAt(s); | |
326 | 9263500 | } | |
327 | |||
328 | inline bool ConfigProjector::solverOneStep(ConfigurationOut_t config) const { | ||
329 | switch (lineSearchType_) { | ||
330 | case Backtracking: { | ||
331 | Backtracking_t ls; | ||
332 | return solver_->oneStep(config, ls); | ||
333 | } | ||
334 | case ErrorNormBased: { | ||
335 | ErrorNormBased_t ls; | ||
336 | return solver_->oneStep(config, ls); | ||
337 | } | ||
338 | case FixedSequence: { | ||
339 | FixedSequence_t ls; | ||
340 | return solver_->oneStep(config, ls); | ||
341 | } | ||
342 | case Constant: { | ||
343 | Constant_t ls; | ||
344 | return solver_->oneStep(config, ls); | ||
345 | } | ||
346 | } | ||
347 | return false; | ||
348 | } | ||
349 | |||
350 | 3212 | inline int ConfigProjector::solverSolve(ConfigurationOut_t config) const { | |
351 |
2/5✓ Branch 0 taken 2350 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 862 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
|
3212 | switch (lineSearchType_) { |
352 | 2350 | case Backtracking: { | |
353 |
1/2✓ Branch 1 taken 2350 times.
✗ Branch 2 not taken.
|
2350 | Backtracking_t ls; |
354 |
3/6✓ Branch 1 taken 2350 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2350 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2350 times.
✗ Branch 8 not taken.
|
2350 | return solver_->solve(config, ls); |
355 | 2350 | } | |
356 | ✗ | case ErrorNormBased: { | |
357 | ✗ | ErrorNormBased_t ls; | |
358 | ✗ | return solver_->solve(config, ls); | |
359 | } | ||
360 | 862 | case FixedSequence: { | |
361 |
1/2✓ Branch 1 taken 862 times.
✗ Branch 2 not taken.
|
862 | FixedSequence_t ls; |
362 |
2/4✓ Branch 1 taken 862 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 862 times.
✗ Branch 5 not taken.
|
862 | return solver_->solve(config, ls); |
363 | } | ||
364 | ✗ | case Constant: { | |
365 | Constant_t ls; | ||
366 | ✗ | return solver_->solve(config, ls); | |
367 | } | ||
368 | } | ||
369 | ✗ | throw std::runtime_error("Unknow line search type"); | |
370 | return BySubstitution::MAX_ITERATION_REACHED; | ||
371 | } | ||
372 | |||
373 | 81 | void ConfigProjector::lastIsOptional(bool optional) { | |
374 | 81 | solver_->lastIsOptional(optional); | |
375 | 81 | } | |
376 | |||
377 | ✗ | bool ConfigProjector::lastIsOptional() const { | |
378 | ✗ | return solver_->lastIsOptional(); | |
379 | } | ||
380 | |||
381 | ✗ | size_type ConfigProjector::numberFreeVariables() const { | |
382 | ✗ | return solver_->numberFreeVariables(); | |
383 | } | ||
384 | |||
385 | 192 | size_type ConfigProjector::dimension() const { | |
386 | 192 | return solver_->reducedDimension(); | |
387 | } | ||
388 | |||
389 | 81 | void ConfigProjector::maxIterations(size_type iterations) { | |
390 | 81 | solver_->maxIterations(iterations); | |
391 | 81 | } | |
392 | |||
393 | 276 | size_type ConfigProjector::maxIterations() const { | |
394 | 276 | return solver_->maxIterations(); | |
395 | } | ||
396 | |||
397 | 81 | void ConfigProjector::errorThreshold(const value_type& threshold) { | |
398 | 81 | solver_->errorThreshold(threshold); | |
399 | 81 | solver_->inequalityThreshold(threshold); | |
400 | 81 | } | |
401 | |||
402 | 64 | value_type ConfigProjector::errorThreshold() const { | |
403 | 64 | return solver_->errorThreshold(); | |
404 | } | ||
405 | |||
406 | ✗ | value_type ConfigProjector::residualError() const { | |
407 | ✗ | return solver_->residualError(); | |
408 | } | ||
409 | |||
410 | 2358 | const value_type& ConfigProjector::sigma() const { return solver_->sigma(); } | |
411 | |||
412 | template <class Archive> | ||
413 | ✗ | void ConfigProjector::serialize(Archive& ar, const unsigned int version) { | |
414 | using namespace boost::serialization; | ||
415 | (void)version; | ||
416 | ✗ | ar& make_nvp("base", base_object<Constraint>(*this)); | |
417 | ✗ | ar& BOOST_SERIALIZATION_NVP(robot_); | |
418 | ✗ | ar& BOOST_SERIALIZATION_NVP(lineSearchType_); | |
419 | ✗ | ar& BOOST_SERIALIZATION_NVP(solver_); | |
420 | ✗ | ar& BOOST_SERIALIZATION_NVP(weak_); | |
421 | if (!Archive::is_saving::value) | ||
422 | ✗ | statistics_.name_ = "ConfigProjector " + name(); | |
423 | } | ||
424 | |||
425 | HPP_SERIALIZATION_IMPLEMENT(ConfigProjector); | ||
426 | } // namespace core | ||
427 | } // namespace hpp | ||
428 | |||
429 | 18 | BOOST_CLASS_EXPORT(hpp::core::ConfigProjector) | |
430 |