GCC Code Coverage Report

Directory: ./
File: src/config-projector.cc
Date: 2025-03-10 11:18:21
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 1861342 bool ConfigProjector::impl_compute(ConfigurationOut_t configuration) {
182 // If configuration satisfies the constraint, do not modify it
183
3/4
✓ Branch 2 taken 1861342 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1858130 times.
✓ Branch 6 taken 3212 times.
 1861342 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 9281214 bool ConfigProjector::isSatisfied(ConfigurationIn_t config) {
267
1/2
✓ Branch 2 taken 9281214 times.
✗ Branch 3 not taken.
 9281214 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