GCC Code Coverage Report

Directory: ./
File: src/solver/hierarchical-iterative.cc
Date: 2025-05-05 12:19:30
Exec Total Coverage
Lines: 539 578 93.3%
Branches: 451 870 51.8%
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 <boost/serialization/shared_ptr.hpp>
30 #include <boost/serialization/vector.hpp>
31 #include <hpp/constraints/implicit.hh>
32 #include <hpp/constraints/macros.hh>
33 #include <hpp/constraints/solver/hierarchical-iterative.hh>
34 #include <hpp/constraints/solver/impl/hierarchical-iterative.hh>
35 #include <hpp/constraints/svd.hh>
36 #include <hpp/pinocchio/device.hh>
37 #include <hpp/pinocchio/joint-collection.hh>
38 #include <hpp/pinocchio/liegroup-element.hh>
39 #include <hpp/pinocchio/serialization.hh>
40 #include <hpp/pinocchio/util.hh>
41 #include <hpp/util/debug.hh>
42 #include <hpp/util/serialization.hh>
43 #include <hpp/util/timer.hh>
44 #include <limits>
45 #include <pinocchio/multibody/model.hpp>
46 #include <pinocchio/serialization/eigen.hpp>
47
48 // #define SVD_THRESHOLD Eigen::NumTraits<value_type>::dummy_precision()
49 #define SVD_THRESHOLD 1e-8
50
51 namespace hpp {
52 namespace constraints {
53 namespace solver {
54 namespace {
55 template <bool Superior, bool ComputeJac, typename Derived>
56 2618 void compare(value_type& val, const Eigen::MatrixBase<Derived>& jac,
57 const value_type& thr) {
58
2/2
✓ Branch 0 taken 410 times.
✓ Branch 1 taken 899 times.
 2618 if ((Superior && val < thr) || (!Superior && -thr < val)) {
59 if (Superior)
60 352 val -= thr;
61 else
62 468 val += thr;
63 } else {
64 1798 val = 0;
65 if (ComputeJac)
66 842 const_cast<Eigen::MatrixBase<Derived>&>(jac).derived().setZero();
67 }
68 2618 }
69
70 template <bool ComputeJac>
71 51854 void applyComparison(const ComparisonTypes_t comparison,
72 const std::vector<std::size_t>& indices, vector_t& value,
73 matrix_t& jacobian, const value_type& thr) {
74
2/2
✓ Branch 1 taken 1309 times.
✓ Branch 2 taken 25927 times.
 54472 for (std::size_t i = 0; i < indices.size(); ++i) {
75 2618 const std::size_t j = indices[i];
76
2/2
✓ Branch 1 taken 523 times.
✓ Branch 2 taken 786 times.
 2618 if (comparison[j] == Superior) {
77
2/4
✓ Branch 2 taken 523 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 523 times.
✗ Branch 6 not taken.
 1046 compare<true, ComputeJac>(value[j], jacobian.row(j), thr);
78
1/2
✓ Branch 1 taken 786 times.
✗ Branch 2 not taken.
 1572 } else if (comparison[j] == Inferior) {
79
2/4
✓ Branch 2 taken 786 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 786 times.
✗ Branch 6 not taken.
 1572 compare<false, ComputeJac>(value[j], jacobian.row(j), thr);
80 }
81 }
82 51854 }
83 } // namespace
84
85 namespace lineSearch {
86 template bool Constant::operator()(const HierarchicalIterative& solver,
87 vectorOut_t arg, vectorOut_t darg);
88
89
2/4
✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 13 times.
✗ Branch 6 not taken.
 13 Backtracking::Backtracking() : c(0.001), tau(0.7), smallAlpha(0.2) {}
90 template bool Backtracking::operator()(const HierarchicalIterative& solver,
91 vectorOut_t arg, vectorOut_t darg);
92
93 2041 FixedSequence::FixedSequence() : alpha(.2), alphaMax(.95), K(.8) {}
94 template bool FixedSequence::operator()(const HierarchicalIterative& solver,
95 vectorOut_t arg, vectorOut_t darg);
96
97 ✗ ErrorNormBased::ErrorNormBased(value_type alphaMin, value_type _a,
98 ✗ value_type _b)
99 ✗ : C(0.5 + alphaMin / 2), K((1 - alphaMin) / 2), a(_a), b(_b) {}
100
101 2 ErrorNormBased::ErrorNormBased(value_type alphaMin)
102 2 : C(0.5 + alphaMin / 2), K((1 - alphaMin) / 2) {
103 static const value_type delta = 0.02;
104 static const value_type r_half = 1e6;
105
106 2 a = atanh((delta - 1 + C) / K) / (1 - r_half);
107 2 b = -r_half * a;
108 2 }
109
110 template bool ErrorNormBased::operator()(const HierarchicalIterative& solver,
111 vectorOut_t arg, vectorOut_t darg);
112 } // namespace lineSearch
113
114 namespace saturation {
115 39730 bool Base::saturate(vectorIn_t q, vectorOut_t qSat,
116 Eigen::VectorXi& saturation) {
117 39730 qSat = q;
118 39730 saturation.setZero();
119 39730 return false;
120 }
121
122 138010 bool clamp(const value_type& lb, const value_type& ub, const value_type& v,
123 value_type& vsat, int& s) {
124
2/2
✓ Branch 0 taken 3527 times.
✓ Branch 1 taken 134483 times.
 138010 if (v <= lb) {
125 3527 vsat = lb;
126 3527 s = -1;
127 3527 return true;
128
2/2
✓ Branch 0 taken 2790 times.
✓ Branch 1 taken 131693 times.
 134483 } else if (v >= ub) {
129 2790 vsat = ub;
130 2790 s = 1;
131 2790 return true;
132 } else {
133 131693 vsat = v;
134 131693 s = 0;
135 131693 return false;
136 }
137 }
138
139 465 bool Bounds::saturate(vectorIn_t q, vectorOut_t qSat,
140 Eigen::VectorXi& saturation) {
141 465 bool sat = false;
142
2/2
✓ Branch 1 taken 2388 times.
✓ Branch 2 taken 465 times.
 2853 for (size_type i = 0; i < q.size(); ++i)
143
5/8
✓ Branch 5 taken 2388 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2388 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 2388 times.
✗ Branch 12 not taken.
✓ Branch 13 taken 250 times.
✓ Branch 14 taken 2138 times.
 2388 if (clamp(lb[i], ub[i], q[i], qSat[i], saturation[iq2iv_[i]])) sat = true;
144 465 return sat;
145 }
146
147 3604 bool Device::saturate(vectorIn_t q, vectorOut_t qSat, Eigen::VectorXi& sat) {
148 3604 bool ret = false;
149 3604 const pinocchio::Model& m = device->model();
150
151
2/2
✓ Branch 1 taken 113998 times.
✓ Branch 2 taken 3604 times.
 117602 for (std::size_t i = 1; i < m.joints.size(); ++i) {
152
1/2
✓ Branch 2 taken 113998 times.
✗ Branch 3 not taken.
 113998 const size_type nq = m.joints[i].nq();
153
1/2
✓ Branch 2 taken 113998 times.
✗ Branch 3 not taken.
 113998 const size_type nv = m.joints[i].nv();
154
1/2
✓ Branch 2 taken 113998 times.
✗ Branch 3 not taken.
 113998 const size_type idx_q = m.joints[i].idx_q();
155
1/2
✓ Branch 2 taken 113998 times.
✗ Branch 3 not taken.
 113998 const size_type idx_v = m.joints[i].idx_v();
156
2/2
✓ Branch 0 taken 135622 times.
✓ Branch 1 taken 113998 times.
 249620 for (size_type j = 0; j < nq; ++j) {
157 135622 const size_type iq = idx_q + j;
158 135622 const size_type iv = idx_v + std::min(j, nv - 1);
159
5/8
✓ Branch 1 taken 135622 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 135622 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 135622 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 6067 times.
✓ Branch 10 taken 129555 times.
 135622 if (clamp(m.lowerPositionLimit[iq], m.upperPositionLimit[iq], q[iq],
160
3/6
✓ Branch 1 taken 135622 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 135622 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 135622 times.
✗ Branch 8 not taken.
 135622 qSat[iq], sat[iv]))
161 6067 ret = true;
162 }
163 }
164
165 3604 const hpp::pinocchio::ExtraConfigSpace& ecs = device->extraConfigSpace();
166 3604 const size_type& d = ecs.dimension();
167
168
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 3604 times.
 3604 for (size_type k = 0; k < d; ++k) {
169 ✗ const size_type iq = m.nq + k;
170 ✗ const size_type iv = m.nv + k;
171 ✗ if (clamp(ecs.lower(k), ecs.upper(k), q[iq], qSat[iq], sat[iv])) ret = true;
172 }
173 3604 return ret;
174 }
175 } // namespace saturation
176
177 typedef std::pair<DifferentiableFunctionPtr_t, size_type> iqIt_t;
178 typedef std::pair<DifferentiableFunctionPtr_t, std::size_t> priorityIt_t;
179
180 1034 HierarchicalIterative::HierarchicalIterative(
181 1034 const LiegroupSpacePtr_t& configSpace)
182 1034 : squaredErrorThreshold_(0),
183 1034 inequalityThreshold_(0),
184 1034 maxIterations_(0),
185 1034 stacks_(),
186 1034 configSpace_(configSpace),
187 1034 dimension_(0),
188 1034 reducedDimension_(0),
189 1034 lastIsOptional_(false),
190 1034 solveLevelByLevel_(false),
191
1/2
✓ Branch 1 taken 1034 times.
✗ Branch 2 not taken.
 1034 freeVariables_(),
192
2/4
✓ Branch 1 taken 1034 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1034 times.
✗ Branch 6 not taken.
 1034 saturate_(new saturation::Base()),
193 1034 constraints_(),
194 1034 iq_(),
195 1034 iv_(),
196 1034 priority_(),
197 1034 sigma_(0),
198
1/2
✓ Branch 1 taken 1034 times.
✗ Branch 2 not taken.
 1034 dq_(),
199
1/2
✓ Branch 1 taken 1034 times.
✗ Branch 2 not taken.
 1034 dqSmall_(),
200
1/2
✓ Branch 1 taken 1034 times.
✗ Branch 2 not taken.
 1034 reducedJ_(),
201
1/2
✓ Branch 3 taken 1034 times.
✗ Branch 4 not taken.
 1034 saturation_(configSpace->nv()),
202
1/2
✓ Branch 1 taken 1034 times.
✗ Branch 2 not taken.
 1034 reducedSaturation_(),
203
1/2
✓ Branch 3 taken 1034 times.
✗ Branch 4 not taken.
 1034 qSat_(configSpace_->nq()),
204
1/2
✓ Branch 1 taken 1034 times.
✗ Branch 2 not taken.
 1034 tmpSat_(),
205 1034 squaredNorm_(0),
206 1034 datas_(),
207
1/2
✓ Branch 1 taken 1034 times.
✗ Branch 2 not taken.
 1034 svd_(),
208
1/2
✓ Branch 3 taken 1034 times.
✗ Branch 4 not taken.
 1034 OM_(configSpace->nv()),
209
1/2
✓ Branch 4 taken 1034 times.
✗ Branch 5 not taken.
 2068 OP_(configSpace->nv()) {
210 // Initialize freeVariables_ to all indices.
211
1/2
✓ Branch 3 taken 1034 times.
✗ Branch 4 not taken.
 1034 freeVariables_.addRow(0, configSpace_->nv());
212 1034 }
213
214 4 HierarchicalIterative::HierarchicalIterative(const HierarchicalIterative& other)
215 4 : squaredErrorThreshold_(other.squaredErrorThreshold_),
216 4 inequalityThreshold_(other.inequalityThreshold_),
217 4 maxIterations_(other.maxIterations_),
218 4 stacks_(other.stacks_),
219 4 configSpace_(other.configSpace_),
220 4 dimension_(other.dimension_),
221 4 reducedDimension_(other.reducedDimension_),
222 4 lastIsOptional_(other.lastIsOptional_),
223
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 freeVariables_(other.freeVariables_),
224 4 saturate_(other.saturate_),
225
1/2
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
 4 constraints_(other.constraints_.size()),
226
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 iq_(other.iq_),
227
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 iv_(other.iv_),
228
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 priority_(other.priority_),
229 4 sigma_(other.sigma_),
230
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 dq_(other.dq_),
231
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 dqSmall_(other.dqSmall_),
232
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 reducedJ_(other.reducedJ_),
233
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 saturation_(other.saturation_),
234
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 reducedSaturation_(other.reducedSaturation_),
235
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 qSat_(other.qSat_),
236
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 tmpSat_(other.tmpSat_),
237 4 squaredNorm_(other.squaredNorm_),
238
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 datas_(other.datas_),
239
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 svd_(other.svd_),
240
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 OM_(other.OM_),
241
1/2
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 8 OP_(other.OP_) {
242
2/2
✓ Branch 1 taken 12 times.
✓ Branch 2 taken 4 times.
 16 for (std::size_t i = 0; i < constraints_.size(); ++i)
243
1/2
✓ Branch 3 taken 12 times.
✗ Branch 4 not taken.
 12 constraints_[i] = other.constraints_[i]->copy();
244 4 }
245
246 2108 bool HierarchicalIterative::contains(
247 const ImplicitPtr_t& numericalConstraint) const {
248
1/2
✓ Branch 3 taken 2108 times.
✗ Branch 4 not taken.
 2108 return find_if(constraints_.begin(), constraints_.end(),
249 1159 [&numericalConstraint](const ImplicitPtr_t& arg) {
250 1159 return *arg == *numericalConstraint;
251 4216 }) != constraints_.end();
252 }
253
254 1043 bool HierarchicalIterative::add(const ImplicitPtr_t& constraint,
255 const std::size_t& priority) {
256 1043 DifferentiableFunctionPtr_t f(constraint->functionPtr());
257
1/2
✓ Branch 3 taken 1043 times.
✗ Branch 4 not taken.
 1043 if (find_if(priority_.begin(), priority_.end(),
258 1064 [&f](const priorityIt_t& arg) { return *arg.first == *f; }) !=
259
2/2
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 1042 times.
 2086 priority_.end()) {
260
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 std::ostringstream oss;
261
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
 1 oss << "Contraint \"" << f->name() << "\" already in solver";
262
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 1 throw std::logic_error(oss.str().c_str());
263 1 }
264
1/2
✓ Branch 1 taken 1042 times.
✗ Branch 2 not taken.
 1042 priority_[f] = priority;
265
2/4
✓ Branch 2 taken 1042 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1042 times.
✗ Branch 6 not taken.
 1042 const ComparisonTypes_t comp(constraint->comparisonType());
266
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 1042 times.
 1042 assert((size_type)comp.size() == f->outputDerivativeSize());
267 1042 const std::size_t minSize = priority + 1;
268
2/2
✓ Branch 1 taken 1032 times.
✓ Branch 2 taken 10 times.
 1042 if (stacks_.size() < minSize) {
269
2/4
✓ Branch 1 taken 1032 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1032 times.
✗ Branch 5 not taken.
 1032 stacks_.resize(minSize, ImplicitConstraintSet());
270
2/4
✓ Branch 1 taken 1032 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1032 times.
✗ Branch 5 not taken.
 1032 datas_.resize(minSize, Data());
271 }
272 1042 Data& d = datas_[priority];
273 // Store rank in output vector value
274
1/2
✓ Branch 5 taken 1042 times.
✗ Branch 6 not taken.
 1042 iq_[f] = datas_[priority].output.space()->nq();
275 // Store rank in output vector derivative
276
1/2
✓ Branch 5 taken 1042 times.
✗ Branch 6 not taken.
 1042 iv_[f] = datas_[priority].output.space()->nv();
277 // warning adding constraint to the stack modifies behind the stage
278 // the dimension of datas_ [priority].output.space (). It should
279 // therefore be done after the previous lines.
280
1/2
✓ Branch 2 taken 1042 times.
✗ Branch 3 not taken.
 1042 stacks_[priority].add(constraint);
281
2/2
✓ Branch 1 taken 10103 times.
✓ Branch 2 taken 1042 times.
 11145 for (std::size_t i = 0; i < comp.size(); ++i) {
282
6/6
✓ Branch 1 taken 10100 times.
✓ Branch 2 taken 3 times.
✓ Branch 4 taken 5 times.
✓ Branch 5 taken 10095 times.
✓ Branch 6 taken 8 times.
✓ Branch 7 taken 10095 times.
 10103 if ((comp[i] == Superior) || (comp[i] == Inferior)) {
283
1/2
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
 8 d.inequalityIndices.push_back(d.comparison.size());
284
2/2
✓ Branch 1 taken 6058 times.
✓ Branch 2 taken 4037 times.
 10095 } else if (comp[i] == Equality) {
285
1/2
✓ Branch 2 taken 6058 times.
✗ Branch 3 not taken.
 6058 d.equalityIndices.addRow(d.comparison.size(), 1);
286 }
287
1/2
✓ Branch 2 taken 10103 times.
✗ Branch 3 not taken.
 10103 d.comparison.push_back(comp[i]);
288 }
289
1/2
✓ Branch 1 taken 1042 times.
✗ Branch 2 not taken.
 1042 d.equalityIndices.updateRows<true, true, true>();
290
1/2
✓ Branch 1 taken 1042 times.
✗ Branch 2 not taken.
 1042 constraints_.push_back(constraint);
291
1/2
✓ Branch 1 taken 1042 times.
✗ Branch 2 not taken.
 1042 update();
292
293 1042 return true;
294 1043 }
295
296 1 void HierarchicalIterative::merge(const HierarchicalIterative& other) {
297 std::size_t priority;
298 1 for (NumericalConstraints_t::const_iterator it(other.constraints_.begin());
299
2/2
✓ Branch 3 taken 2 times.
✓ Branch 4 taken 1 times.
 3 it != other.constraints_.end(); ++it) {
300
3/4
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
✓ Branch 5 taken 1 times.
 2 if (!this->contains(*it)) {
301 1 const DifferentiableFunctionPtr_t& f = (*it)->functionPtr();
302 std::map<DifferentiableFunctionPtr_t, std::size_t>::const_iterator itp(
303
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 std::find_if(
304 other.priority_.begin(), other.priority_.end(),
305 1 [&f](const priorityIt_t& arg) { return *arg.first == *f; }));
306
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 if (itp == other.priority_.end()) {
307 // If priority is not set, constraint is explicit
308 1 priority = 0;
309 } else {
310 ✗ priority = itp->second;
311 }
312
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 this->add(*it, priority);
313 }
314 }
315 1 }
316
317 ✗ ArrayXb HierarchicalIterative::activeParameters() const {
318 ✗ ArrayXb ap(ArrayXb::Constant(configSpace_->nq(), false));
319 ✗ for (std::size_t i = 0; i < stacks_.size(); ++i) {
320 #ifndef NDEBUG
321 ✗ dynamic_cast<const DifferentiableFunctionSet&>(stacks_[i].function());
322 #endif
323 const DifferentiableFunctionSet& dfs(
324 ✗ dynamic_cast<const DifferentiableFunctionSet&>(stacks_[i].function()));
325 ✗ ap = ap || dfs.activeParameters();
326 }
327 ✗ return ap;
328 }
329
330 2 ArrayXb HierarchicalIterative::activeDerivativeParameters() const {
331
2/4
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
 2 ArrayXb ap(ArrayXb::Constant(configSpace_->nv(), false));
332
2/2
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 2 times.
 4 for (std::size_t i = 0; i < stacks_.size(); ++i) {
333 #ifndef NDEBUG
334
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 dynamic_cast<const DifferentiableFunctionSet&>(stacks_[i].function());
335 #endif
336 const DifferentiableFunctionSet& dfs(
337
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 dynamic_cast<const DifferentiableFunctionSet&>(stacks_[i].function()));
338
2/4
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
 2 ap = ap || dfs.activeDerivativeParameters();
339 }
340 2 return ap;
341 }
342
343 1071 void HierarchicalIterative::update() {
344 // Compute reduced size
345 1071 std::size_t reducedSize = freeVariables_.nbIndices();
346
347 1071 dimension_ = 0;
348 1071 reducedDimension_ = 0;
349
2/2
✓ Branch 1 taken 1073 times.
✓ Branch 2 taken 1071 times.
 2144 for (std::size_t i = 0; i < stacks_.size(); ++i) {
350 1073 computeActiveRowsOfJ(i);
351
352 1073 const ImplicitConstraintSet& constraints(stacks_[i]);
353 #ifndef NDEBUG
354
1/2
✓ Branch 1 taken 1073 times.
✗ Branch 2 not taken.
 1073 dynamic_cast<const DifferentiableFunctionSet&>(constraints.function());
355 #endif
356 const DifferentiableFunctionSet& f(
357 1073 static_cast<const DifferentiableFunctionSet&>(constraints.function()));
358 1073 dimension_ += f.outputDerivativeSize();
359 1073 reducedDimension_ += datas_[i].activeRowsOfJ.nbRows();
360
1/2
✓ Branch 2 taken 1073 times.
✗ Branch 3 not taken.
 1073 datas_[i].output = LiegroupElement(f.outputSpace());
361
1/2
✓ Branch 2 taken 1073 times.
✗ Branch 3 not taken.
 1073 datas_[i].rightHandSide = LiegroupElement(f.outputSpace());
362 1073 datas_[i].rightHandSide.setNeutral();
363
1/2
✓ Branch 5 taken 1073 times.
✗ Branch 6 not taken.
 1073 datas_[i].error.resize(f.outputSpace()->nv());
364
365
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 1073 times.
 1073 assert(configSpace_->nv() == f.inputDerivativeSize());
366 1073 datas_[i].jacobian.resize(f.outputDerivativeSize(),
367 f.inputDerivativeSize());
368 1073 datas_[i].jacobian.setZero();
369 1073 datas_[i].reducedJ.resize(datas_[i].activeRowsOfJ.nbRows(), reducedSize);
370
371 2146 datas_[i].svd = SVD_t(
372 f.outputDerivativeSize(), reducedSize,
373
2/2
✓ Branch 1 taken 1068 times.
✓ Branch 2 taken 5 times.
 1073 Eigen::ComputeThinU | (i == stacks_.size() - 1 ? Eigen::ComputeThinV
374 1073 : Eigen::ComputeFullV));
375
1/2
✓ Branch 2 taken 1073 times.
✗ Branch 3 not taken.
 1073 datas_[i].svd.setThreshold(SVD_THRESHOLD);
376 1073 datas_[i].PK.resize(reducedSize, reducedSize);
377
378 1073 datas_[i].maxRank = 0;
379 }
380
381
1/2
✓ Branch 4 taken 1071 times.
✗ Branch 5 not taken.
 1071 dq_ = vector_t::Zero(configSpace_->nv());
382 1071 dqSmall_.resize(reducedSize);
383 1071 reducedJ_.resize(reducedDimension_, reducedSize);
384 2142 svd_ = SVD_t(reducedDimension_, reducedSize,
385 1071 Eigen::ComputeThinU | Eigen::ComputeThinV);
386 1071 }
387
388 24 void HierarchicalIterative::computeActiveRowsOfJ(std::size_t iStack) {
389 24 Data& d = datas_[iStack];
390 const ImplicitConstraintSet::Implicits_t constraints(
391
1/2
✓ Branch 3 taken 24 times.
✗ Branch 4 not taken.
 24 stacks_[iStack].constraints());
392 24 std::size_t offset = 0;
393
394 typedef Eigen::MatrixBlocks<false, false> BlockIndices;
395 24 BlockIndices::segments_t rows;
396 // Loop over functions of the stack
397
2/2
✓ Branch 1 taken 34 times.
✓ Branch 2 taken 24 times.
 58 for (std::size_t i = 0; i < constraints.size(); ++i) {
398 34 ArrayXb adp = freeVariables_
399
1/2
✓ Branch 2 taken 34 times.
✗ Branch 3 not taken.
 68 .rview(constraints[i]
400 34 ->function()
401 34 .activeDerivativeParameters()
402
1/2
✓ Branch 1 taken 34 times.
✗ Branch 2 not taken.
 34 .matrix())
403
2/4
✓ Branch 1 taken 34 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34 times.
✗ Branch 5 not taken.
 34 .eval();
404
2/4
✓ Branch 1 taken 34 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 34 times.
✗ Branch 4 not taken.
 34 if (adp.any()) // If at least one element of adp is true
405
2/2
✓ Branch 7 taken 35 times.
✓ Branch 8 taken 34 times.
 69 for (const segment_t s : constraints[i]->activeRows()) {
406
1/2
✓ Branch 1 taken 35 times.
✗ Branch 2 not taken.
 35 rows.emplace_back(s.first + offset, s.second);
407 }
408 34 offset += constraints[i]->function().outputDerivativeSize();
409 34 }
410 d.activeRowsOfJ =
411
2/4
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24 times.
✗ Branch 5 not taken.
 24 Eigen::MatrixBlocks<false, false>(rows, freeVariables_.m_rows);
412
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 24 d.activeRowsOfJ.updateRows<true, true, true>();
413 24 }
414
415 1909 vector_t HierarchicalIterative::rightHandSideFromConfig(
416 ConfigurationIn_t config) {
417
2/2
✓ Branch 1 taken 1505 times.
✓ Branch 2 taken 1909 times.
 3414 for (std::size_t i = 0; i < stacks_.size(); ++i) {
418 1505 ImplicitConstraintSet& ics = stacks_[i];
419 1505 Data& d = datas_[i];
420
2/4
✓ Branch 2 taken 1505 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1505 times.
✗ Branch 6 not taken.
 1505 ics.rightHandSideFromConfig(config, d.rightHandSide);
421 }
422 1909 return rightHandSide();
423 }
424
425 4103 bool HierarchicalIterative::rightHandSideFromConfig(
426 const ImplicitPtr_t& constraint, ConfigurationIn_t config) {
427 4103 const DifferentiableFunctionPtr_t& f(constraint->functionPtr());
428
429 std::map<DifferentiableFunctionPtr_t, size_type>::iterator iqi(
430
1/2
✓ Branch 3 taken 4103 times.
✗ Branch 4 not taken.
 4103 find_if(iq_.begin(), iq_.end(),
431 4103 [&f](const iqIt_t& arg) { return *arg.first == *f; }));
432
2/2
✓ Branch 2 taken 2002 times.
✓ Branch 3 taken 2101 times.
 4103 if (iqi == iq_.end()) return false;
433 2101 LiegroupSpacePtr_t space(f->outputSpace());
434 2101 size_type iq = iqi->second;
435
436 std::map<DifferentiableFunctionPtr_t, std::size_t>::iterator prioi(
437
1/2
✓ Branch 3 taken 2101 times.
✗ Branch 4 not taken.
 2101 find_if(priority_.begin(), priority_.end(),
438 2101 [&f](const iqIt_t& arg) { return *arg.first == *f; }));
439
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 2101 times.
 2101 if (prioi == priority_.end()) return false;
440 2101 std::size_t i = prioi->second;
441
442 2101 size_type nq = space->nq();
443 2101 Data& d = datas_[i];
444 LiegroupElementRef rhs(
445
3/6
✓ Branch 3 taken 2101 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 2101 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 2101 times.
✗ Branch 10 not taken.
 2101 space->elementRef(d.rightHandSide.vector().segment(iq, nq)));
446
3/6
✓ Branch 2 taken 2101 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2101 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2101 times.
✗ Branch 9 not taken.
 2101 constraint->rightHandSideFromConfig(config, rhs);
447 2101 return true;
448 2101 }
449
450 3103 bool HierarchicalIterative::rightHandSide(const ImplicitPtr_t& constraint,
451 vectorIn_t rightHandSide) {
452 3103 const DifferentiableFunctionPtr_t& f(constraint->functionPtr());
453 3103 LiegroupSpacePtr_t space(f->outputSpace());
454
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 3103 times.
 3103 assert(rightHandSide.size() == space->nq());
455
456 std::map<DifferentiableFunctionPtr_t, size_type>::iterator iqi(
457
1/2
✓ Branch 3 taken 3103 times.
✗ Branch 4 not taken.
 3103 find_if(iq_.begin(), iq_.end(),
458 3106 [&f](const iqIt_t& arg) { return *arg.first == *f; }));
459
2/2
✓ Branch 2 taken 1002 times.
✓ Branch 3 taken 2101 times.
 3103 if (iqi == iq_.end()) return false;
460 2101 size_type iq = iqi->second;
461 2101 size_type nq = space->nq();
462
463 std::map<DifferentiableFunctionPtr_t, std::size_t>::iterator prioi(
464
1/2
✓ Branch 3 taken 2101 times.
✗ Branch 4 not taken.
 2101 find_if(priority_.begin(), priority_.end(),
465 2102 [&f](const iqIt_t& arg) { return *arg.first == *f; }));
466
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 2101 times.
 2101 if (prioi == priority_.end()) return false;
467 2101 std::size_t i = prioi->second;
468
469 2101 Data& d = datas_[i];
470 #ifndef NDEBUG
471 2101 size_type nv = space->nv();
472
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2101 times.
 2101 assert(d.error.size() >= nv);
473 #endif
474 pinocchio::LiegroupElementConstRef inRhs(
475
2/4
✓ Branch 2 taken 2101 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2101 times.
✗ Branch 6 not taken.
 2101 space->elementConstRef(rightHandSide));
476 LiegroupElementRef rhs(
477
3/6
✓ Branch 3 taken 2101 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 2101 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 2101 times.
✗ Branch 10 not taken.
 2101 space->elementRef(d.rightHandSide.vector().segment(iq, nq)));
478
1/2
✓ Branch 1 taken 2101 times.
✗ Branch 2 not taken.
 2101 rhs = inRhs;
479
3/6
✓ Branch 2 taken 2101 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2101 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 2101 times.
 2101 assert(constraint->checkRightHandSide(inRhs));
480 2101 return true;
481 3103 }
482
483 5103 bool HierarchicalIterative::getRightHandSide(const ImplicitPtr_t& constraint,
484 vectorOut_t rightHandSide) const {
485 5103 const DifferentiableFunctionPtr_t& f(constraint->functionPtr());
486 std::map<DifferentiableFunctionPtr_t, std::size_t>::const_iterator itp(
487
1/2
✓ Branch 3 taken 5103 times.
✗ Branch 4 not taken.
 5103 find_if(priority_.begin(), priority_.end(),
488 5106 [&f](const priorityIt_t& arg) { return *arg.first == *f; }));
489
2/2
✓ Branch 2 taken 2002 times.
✓ Branch 3 taken 3101 times.
 5103 if (itp == priority_.end()) {
490 2002 return false;
491 }
492 std::map<DifferentiableFunctionPtr_t, size_type>::const_iterator itIq(
493
1/2
✓ Branch 3 taken 3101 times.
✗ Branch 4 not taken.
 3101 find_if(iq_.begin(), iq_.end(),
494 3102 [&f](const iqIt_t& arg) { return *arg.first == *f; }));
495
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 3101 times.
 3101 if (itIq == iq_.end()) {
496 ✗ return false;
497 }
498 3101 LiegroupSpacePtr_t space(f->outputSpace());
499 3101 std::size_t i = itp->second;
500 3101 size_type iq = itIq->second;
501 3101 Data& d = datas_[i];
502
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 3101 times.
 3101 assert(rightHandSide.size() == space->nq());
503
1/2
✗ Branch 5 not taken.
✓ Branch 6 taken 3101 times.
 3101 assert(d.rightHandSide.space()->nq() >= iq + space->nq());
504
2/4
✓ Branch 4 taken 3101 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3101 times.
✗ Branch 8 not taken.
 3101 rightHandSide = d.rightHandSide.vector().segment(iq, space->nq());
505 3101 return true;
506 3101 }
507
508 8 bool HierarchicalIterative::isConstraintSatisfied(
509 const ImplicitPtr_t& constraint, vectorIn_t arg, vectorOut_t error,
510 bool& constraintFound) const {
511 8 const DifferentiableFunctionPtr_t& f(constraint->functionPtr());
512
1/2
✗ Branch 5 not taken.
✓ Branch 6 taken 8 times.
 8 assert(error.size() == f->outputSpace()->nv());
513 std::map<DifferentiableFunctionPtr_t, std::size_t>::const_iterator itp(
514
1/2
✓ Branch 3 taken 8 times.
✗ Branch 4 not taken.
 8 find_if(priority_.begin(), priority_.end(),
515 8 [&f](const priorityIt_t& arg) { return *arg.first == *f; }));
516
2/2
✓ Branch 2 taken 4 times.
✓ Branch 3 taken 4 times.
 8 if (itp == priority_.end()) {
517 4 constraintFound = false;
518 4 return false;
519 }
520 4 constraintFound = true;
521 std::map<DifferentiableFunctionPtr_t, size_type>::const_iterator itIq(
522
1/2
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
 4 find_if(iq_.begin(), iq_.end(),
523 4 [&f](const iqIt_t& arg) { return *arg.first == *f; }));
524
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 4 times.
 4 assert(itIq != iq_.end());
525 std::map<DifferentiableFunctionPtr_t, size_type>::const_iterator itIv(
526
1/2
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
 4 find_if(iv_.begin(), iv_.end(),
527 4 [&f](const iqIt_t& arg) { return *arg.first == *f; }));
528
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 4 times.
 4 assert(itIv != iv_.end());
529 4 size_type priority(itp->second);
530 4 Data& d = datas_[priority];
531 // Evaluate constraint function
532 4 size_type iq = itIq->second, nq = f->outputSpace()->nq();
533
1/2
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 4 LiegroupElementRef output(d.output.vector().segment(iq, nq),
534
2/4
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 4 times.
✗ Branch 7 not taken.
 8 f->outputSpace());
535
1/2
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 4 LiegroupElementRef rhs(d.rightHandSide.vector().segment(iq, nq),
536
2/4
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 4 times.
✗ Branch 7 not taken.
 8 f->outputSpace());
537
3/6
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 4 times.
✗ Branch 9 not taken.
 4 f->value(output, arg);
538
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 4 error = output - rhs;
539
2/4
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
 4 constraint->setInactiveRowsToZero(error);
540
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 return (error.squaredNorm() < squaredErrorThreshold_);
541 4 }
542
543 100 void HierarchicalIterative::rightHandSide(vectorIn_t rightHandSide) {
544 100 size_type iq = 0, iv = 0;
545
2/2
✓ Branch 1 taken 100 times.
✓ Branch 2 taken 100 times.
 200 for (std::size_t i = 0; i < stacks_.size(); ++i) {
546 100 Data& d = datas_[i];
547 100 LiegroupSpacePtr_t space(d.rightHandSide.space());
548 100 size_type nq = space->nq();
549 100 size_type nv = space->nv();
550 pinocchio::LiegroupElementConstRef output(
551
3/6
✓ Branch 2 taken 100 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 100 times.
✗ Branch 9 not taken.
 100 space->elementConstRef(rightHandSide.segment(iq, nq)));
552 LiegroupElementRef rhs(
553
3/6
✓ Branch 3 taken 100 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 100 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 100 times.
✗ Branch 10 not taken.
 100 space->elementRef(d.rightHandSide.vector().segment(iq, nq)));
554
555 // d.error is used here as an intermediate storage. The value
556 // computed is not the error
557
2/4
✓ Branch 2 taken 100 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.
 100 d.error = output - space->neutral(); // log (rightHandSide)
558
2/2
✓ Branch 0 taken 600 times.
✓ Branch 1 taken 100 times.
 700 for (size_type k = 0; k < nv; ++k) {
559
1/6
✗ Branch 1 not taken.
✓ Branch 2 taken 600 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 600 if (d.comparison[iv + k] != Equality) assert(d.error[k] == 0);
560 }
561
4/8
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 100 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 100 times.
✗ Branch 12 not taken.
 100 rhs = space->neutral() + d.error; // exp (d.error)
562 100 iq += nq;
563 100 iv += nv;
564 100 }
565
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 100 times.
 100 assert(iq == rightHandSide.size());
566 100 }
567
568 ✗ void HierarchicalIterative::rightHandSideAt(const value_type& s) {
569 ✗ for (std::size_t i = 0; i < constraints_.size(); ++i) {
570 ✗ ImplicitPtr_t implicit = constraints_[i];
571 // If constraint has no right hand side function set, do nothing
572 ✗ if ((implicit->parameterSize() != 0) &&
573 ✗ (implicit->rightHandSideFunction())) {
574 ✗ rightHandSide(implicit, implicit->rightHandSideAt(s));
575 }
576 }
577 }
578
579 2309 vector_t HierarchicalIterative::rightHandSide() const {
580
1/2
✓ Branch 2 taken 2309 times.
✗ Branch 3 not taken.
 2309 vector_t rhs(rightHandSideSize());
581 2309 size_type iq = 0;
582
2/2
✓ Branch 1 taken 1905 times.
✓ Branch 2 taken 2309 times.
 4214 for (std::size_t i = 0; i < stacks_.size(); ++i) {
583 1905 const Data& d = datas_[i];
584 1905 size_type nq = d.rightHandSide.space()->nq();
585 // this does not take the comparison type into account.
586 // It shouldn't matter as rhs should be zero when comparison type is
587 // not Equality
588
3/6
✓ Branch 1 taken 1905 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1905 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1905 times.
✗ Branch 8 not taken.
 1905 rhs.segment(iq, nq) = d.rightHandSide.vector();
589 1905 iq += nq;
590 }
591
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2309 times.
 2309 assert(iq == rhs.size());
592 2309 return rhs;
593 }
594
595 4314 size_type HierarchicalIterative::rightHandSideSize() const {
596 4314 size_type rhsSize = 0;
597
2/2
✓ Branch 1 taken 3506 times.
✓ Branch 2 taken 4314 times.
 7820 for (std::size_t i = 0; i < stacks_.size(); ++i)
598 3506 rhsSize += stacks_[i].function().outputSize();
599 4314 return rhsSize;
600 }
601
602 template <bool ComputeJac>
603 52078 void HierarchicalIterative::computeValue(vectorIn_t config) const {
604
2/2
✓ Branch 1 taken 25927 times.
✓ Branch 2 taken 26039 times.
 103932 for (std::size_t i = 0; i < stacks_.size(); ++i) {
605 51854 const ImplicitConstraintSet& constraints(stacks_[i]);
606 51854 const DifferentiableFunction& f = constraints.function();
607 51854 Data& d = datas_[i];
608
609
2/4
✓ Branch 2 taken 25927 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 25927 times.
✗ Branch 6 not taken.
 51854 f.value(d.output, config);
610
2/4
✓ Branch 1 taken 25927 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 25927 times.
 51854 assert(hpp::pinocchio::checkNormalized(d.output));
611
2/4
✓ Branch 1 taken 25927 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 25927 times.
 51854 assert(hpp::pinocchio::checkNormalized(d.rightHandSide));
612 51854 d.error = d.output - d.rightHandSide;
613
1/2
✓ Branch 2 taken 25927 times.
✗ Branch 3 not taken.
 51854 constraints.setInactiveRowsToZero(d.error);
614 if (ComputeJac) {
615
2/4
✓ Branch 2 taken 23739 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 23739 times.
✗ Branch 6 not taken.
 47478 f.jacobian(d.jacobian, config);
616
3/6
✓ Branch 4 taken 23739 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 23739 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 23739 times.
✗ Branch 11 not taken.
 142434 d.output.space()->dDifference_dq1<pinocchio::DerivativeTimesInput>(
617 94956 d.rightHandSide.vector(), d.output.vector(), d.jacobian);
618 }
619
1/2
✓ Branch 1 taken 25927 times.
✗ Branch 2 not taken.
 51854 applyComparison<ComputeJac>(d.comparison, d.inequalityIndices, d.error,
620 51854 d.jacobian, inequalityThreshold_);
621
622 // Copy columns that are not reduced
623
1/2
✓ Branch 2 taken 23739 times.
✗ Branch 3 not taken.
 47478 if (ComputeJac) d.reducedJ = d.activeRowsOfJ.rview(d.jacobian);
624 }
625 52078 }
626
627 template void HierarchicalIterative::computeValue<false>(
628 vectorIn_t config) const;
629 template void HierarchicalIterative::computeValue<true>(
630 vectorIn_t config) const;
631
632 22138 void HierarchicalIterative::computeSaturation(vectorIn_t config) const {
633
2/4
✓ Branch 3 taken 22138 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 22138 times.
✗ Branch 7 not taken.
 22138 bool applySaturate = saturate_->saturate(config, qSat_, saturation_);
634
2/2
✓ Branch 0 taken 20657 times.
✓ Branch 1 taken 1481 times.
 22138 if (!applySaturate) return;
635
636
1/2
✓ Branch 2 taken 1481 times.
✗ Branch 3 not taken.
 1481 reducedSaturation_ = freeVariables_.rview(saturation_);
637
10/20
✓ Branch 1 taken 1481 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1481 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1481 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1481 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1481 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1481 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1481 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1481 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 1481 times.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 1481 times.
 1481 assert((reducedSaturation_.array() == -1 || reducedSaturation_.array() == 0 ||
638 reducedSaturation_.array() == 1)
639 .all());
640
641
2/2
✓ Branch 1 taken 1492 times.
✓ Branch 2 taken 1481 times.
 2973 for (std::size_t i = 0; i < stacks_.size(); ++i) {
642 1492 Data& d = datas_[i];
643
644
3/6
✓ Branch 1 taken 1492 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1492 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1492 times.
✗ Branch 8 not taken.
 1492 vector_t error = d.activeRowsOfJ.keepRows().rview(d.error);
645 ✗ tmpSat_ = (reducedSaturation_.cast<value_type>()
646
3/6
✓ Branch 1 taken 1492 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1492 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1492 times.
✗ Branch 8 not taken.
 1492 .cwiseProduct(d.reducedJ.transpose() * error)
647
4/8
✓ Branch 1 taken 1492 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1492 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1492 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1492 times.
✗ Branch 11 not taken.
 2984 .array() < 0);
648
2/2
✓ Branch 1 taken 49502 times.
✓ Branch 2 taken 1492 times.
 50994 for (size_type j = 0; j < tmpSat_.size(); ++j)
649
5/8
✓ Branch 1 taken 49502 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 2108 times.
✓ Branch 4 taken 47394 times.
✓ Branch 6 taken 2108 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 2108 times.
✗ Branch 10 not taken.
 49502 if (tmpSat_[j]) d.reducedJ.col(j).setZero();
650 1492 }
651 }
652
653 ✗ void HierarchicalIterative::getValue(vectorOut_t v) const {
654 ✗ size_type row = 0;
655 ✗ for (std::size_t i = 0; i < datas_.size(); ++i) {
656 ✗ const Data& d = datas_[i];
657 ✗ v.segment(row, d.output.vector().rows()) = d.output.vector();
658 ✗ row += d.output.vector().rows();
659 }
660 ✗ assert(v.rows() == row);
661 }
662
663 6 void HierarchicalIterative::getReducedJacobian(matrixOut_t J) const {
664 6 size_type row = 0;
665
2/2
✓ Branch 1 taken 6 times.
✓ Branch 2 taken 6 times.
 12 for (std::size_t i = 0; i < datas_.size(); ++i) {
666 6 const Data& d = datas_[i];
667
1/2
✓ Branch 3 taken 6 times.
✗ Branch 4 not taken.
 6 J.middleRows(row, d.reducedJ.rows()) = d.reducedJ;
668 6 row += d.reducedJ.rows();
669 }
670
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 6 times.
 6 assert(J.rows() == row);
671 6 }
672
673 26033 void HierarchicalIterative::computeError() const {
674 const std::size_t end =
675
2/2
✓ Branch 0 taken 7 times.
✓ Branch 1 taken 26026 times.
 26033 (lastIsOptional_ ? stacks_.size() - 1 : stacks_.size());
676 26033 squaredNorm_ = 0;
677
2/2
✓ Branch 0 taken 25914 times.
✓ Branch 1 taken 26033 times.
 51947 for (std::size_t i = 0; i < end; ++i) {
678 const ImplicitConstraintSet::Implicits_t constraints(
679
1/2
✓ Branch 3 taken 25914 times.
✗ Branch 4 not taken.
 25914 stacks_[i].constraints());
680 25914 const Data& d = datas_[i];
681 25914 size_type iv = 0;
682
2/2
✓ Branch 1 taken 26877 times.
✓ Branch 2 taken 25914 times.
 52791 for (std::size_t j = 0; j < constraints.size(); ++j) {
683 26877 size_type nv(constraints[j]->function().outputDerivativeSize());
684 26877 squaredNorm_ =
685
2/4
✓ Branch 1 taken 26877 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26877 times.
✗ Branch 5 not taken.
 26877 std::max(squaredNorm_, d.error.segment(iv, nv).squaredNorm());
686 26877 iv += nv;
687 }
688 25914 }
689 26033 }
690
691 21925 bool HierarchicalIterative::integrate(vectorIn_t from, vectorIn_t velocity,
692 vectorOut_t result) const {
693 typedef pinocchio::LiegroupElementRef LgeRef_t;
694
1/2
✓ Branch 1 taken 21925 times.
✗ Branch 2 not taken.
 21925 result = from;
695
1/2
✓ Branch 1 taken 21925 times.
✗ Branch 2 not taken.
 21925 LgeRef_t M(result, configSpace_);
696
2/4
✓ Branch 1 taken 21925 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 21925 times.
✗ Branch 5 not taken.
 21925 M += velocity;
697
3/6
✓ Branch 2 taken 21925 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 21925 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 21925 times.
✗ Branch 9 not taken.
 43850 return saturate_->saturate(result, result, saturation_);
698 21925 }
699
700 1392 void HierarchicalIterative::residualError(vectorOut_t error) const {
701 1392 size_type row = 0;
702
2/2
✓ Branch 1 taken 1388 times.
✓ Branch 2 taken 1392 times.
 2780 for (std::size_t i = 0; i < datas_.size(); ++i) {
703 1388 const Data& d = datas_[i];
704
1/2
✓ Branch 3 taken 1388 times.
✗ Branch 4 not taken.
 1388 error.segment(row, d.error.size()) = d.error;
705 1388 row += d.error.size();
706 }
707 1392 }
708
709 6 bool HierarchicalIterative::definesSubmanifoldOf(
710 const HierarchicalIterative& solver) const {
711 6 for (NumericalConstraints_t::const_iterator it(solver.constraints().begin());
712
2/2
✓ Branch 4 taken 8 times.
✓ Branch 5 taken 6 times.
 14 it != solver.constraints().end(); ++it) {
713 8 const DifferentiableFunctionPtr_t& f = (*it)->functionPtr();
714
1/2
✓ Branch 3 taken 8 times.
✗ Branch 4 not taken.
 8 if (find_if(constraints_.begin(), constraints_.end(),
715 10 [&f](const ImplicitPtr_t& arg) {
716 10 return arg->function() == *f;
717
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 8 times.
 16 }) == constraints_.end())
718 ✗ return false;
719 }
720 6 return true;
721 }
722
723 22138 void HierarchicalIterative::computeDescentDirection() const {
724 22138 sigma_ = std::numeric_limits<value_type>::max();
725
726
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 22138 times.
 22138 if (stacks_.empty()) {
727 ✗ dq_.setZero();
728 ✗ return;
729 }
730
1/2
✓ Branch 1 taken 22138 times.
✗ Branch 2 not taken.
 22138 vector_t err;
731
2/2
✓ Branch 1 taken 21996 times.
✓ Branch 2 taken 142 times.
 22138 if (stacks_.size() == 1) { // one level only
732 21996 Data& d = datas_[0];
733
1/2
✓ Branch 1 taken 21996 times.
✗ Branch 2 not taken.
 21996 d.svd.compute(d.reducedJ);
734 HPP_DEBUG_SVDCHECK(d.svd);
735 // TODO Eigen::JacobiSVD does a dynamic allocation here.
736
4/8
✓ Branch 1 taken 21996 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 21996 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 21996 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 21996 times.
✗ Branch 11 not taken.
 21996 err = d.activeRowsOfJ.keepRows().rview(-d.error);
737
2/4
✓ Branch 1 taken 21996 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 21996 times.
✗ Branch 5 not taken.
 21996 dqSmall_ = d.svd.solve(err);
738
1/2
✓ Branch 1 taken 21996 times.
✗ Branch 2 not taken.
 21996 d.maxRank = std::max(d.maxRank, d.svd.rank());
739
2/2
✓ Branch 0 taken 21995 times.
✓ Branch 1 taken 1 times.
 21996 if (d.maxRank > 0)
740
2/4
✓ Branch 1 taken 21995 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 21995 times.
✗ Branch 5 not taken.
 21995 sigma_ = std::min(sigma_, d.svd.singularValues()[d.maxRank - 1]);
741 } else {
742 // dq = dQ_0 + P_0 * v_1
743 // f_1(q+dq) = f_1(q) + J_1 * dQ_0 + M_1 * v_1
744 // M_1 = J_1 * P_0
745 // v_1 = M+_1 * (-f_1(q) - J_1 * dQ_1) + K_1 * v_2
746 // dq = dQ_0 + P_0 * M+_1 * (-f_1(q) - J_1 * dQ_1) + P_0 * K_1 * v_2
747 // = dQ_1 + P_1 * b_2
748 //
749 // dQ_1 = dQ_0 + P_0 * M+_1 * (-f_1(q) - J_1 * dQ_1)
750 // P_1 = P_0 * K_1
751 142 matrix_t* projector = NULL;
752
1/2
✓ Branch 1 taken 284 times.
✗ Branch 2 not taken.
 284 for (std::size_t i = 0; i < stacks_.size(); ++i) {
753 284 Data& d = datas_[i];
754
755 // TODO: handle case where this is the first element of the stack and it
756 // has no functions
757
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 284 times.
 284 if (d.reducedJ.rows() == 0) continue;
758 /// projector is of size numberDof
759 284 bool first = (i == 0);
760 284 bool last = (i == stacks_.size() - 1);
761
2/2
✓ Branch 0 taken 142 times.
✓ Branch 1 taken 142 times.
 284 if (first) {
762
4/8
✓ Branch 1 taken 142 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 142 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 142 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 142 times.
✗ Branch 11 not taken.
 142 err = d.activeRowsOfJ.keepRows().rview(-d.error);
763 // dq should be zero and projector should be identity
764
1/2
✓ Branch 1 taken 142 times.
✗ Branch 2 not taken.
 142 d.svd.compute(d.reducedJ);
765 HPP_DEBUG_SVDCHECK(d.svd);
766 // TODO Eigen::JacobiSVD does a dynamic allocation here.
767
2/4
✓ Branch 1 taken 142 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 142 times.
✗ Branch 5 not taken.
 142 dqSmall_ = d.svd.solve(err);
768 } else {
769
4/8
✓ Branch 1 taken 142 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 142 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 142 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 142 times.
✗ Branch 11 not taken.
 142 err = d.activeRowsOfJ.keepRows().rview(-d.error);
770
3/6
✓ Branch 1 taken 142 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 142 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 142 times.
✗ Branch 8 not taken.
 142 err.noalias() -= d.reducedJ * dqSmall_;
771
772
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 142 times.
 142 if (projector == NULL) {
773 ✗ d.svd.compute(d.reducedJ);
774 // TODO Eigen::JacobiSVD does a dynamic allocation here.
775 ✗ dqSmall_ += d.svd.solve(err);
776 } else {
777
3/6
✓ Branch 1 taken 142 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 142 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 142 times.
✗ Branch 8 not taken.
 142 d.svd.compute(d.reducedJ * *projector);
778 // TODO Eigen::JacobiSVD does a dynamic allocation here.
779
3/6
✓ Branch 1 taken 142 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 142 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 142 times.
✗ Branch 8 not taken.
 142 dqSmall_ += *projector * d.svd.solve(err);
780 }
781 HPP_DEBUG_SVDCHECK(d.svd);
782 }
783 // Update sigma
784
1/2
✓ Branch 1 taken 284 times.
✗ Branch 2 not taken.
 284 const size_type rank = d.svd.rank();
785 284 d.maxRank = std::max(d.maxRank, rank);
786
2/2
✓ Branch 0 taken 282 times.
✓ Branch 1 taken 2 times.
 284 if (d.maxRank > 0)
787
2/4
✓ Branch 1 taken 282 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 282 times.
✗ Branch 5 not taken.
 282 sigma_ = std::min(sigma_, d.svd.singularValues()[d.maxRank - 1]);
788
2/8
✗ Branch 0 not taken.
✓ Branch 1 taken 284 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 284 times.
 284 if (solveLevelByLevel_ && err.squaredNorm() > squaredErrorThreshold_)
789 142 break;
790
2/2
✓ Branch 0 taken 142 times.
✓ Branch 1 taken 142 times.
 284 if (last) break; // No need to compute projector for next step.
791
792
2/4
✓ Branch 1 taken 142 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 142 times.
 142 if (d.svd.matrixV().cols() == rank) break; // The kernel is { 0 }
793 /// compute projector for next step.
794
1/2
✓ Branch 0 taken 142 times.
✗ Branch 1 not taken.
 142 if (projector == NULL)
795
3/6
✓ Branch 1 taken 142 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 142 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 142 times.
✗ Branch 8 not taken.
 142 d.PK.noalias() = getV2<SVD_t>(d.svd, rank);
796 else
797 ✗ d.PK.noalias() = *projector * getV2<SVD_t>(d.svd, rank);
798 142 projector = &d.PK;
799 }
800 }
801
1/2
✓ Branch 1 taken 22138 times.
✗ Branch 2 not taken.
 22138 expandDqSmall();
802 22138 }
803
804 22138 void HierarchicalIterative::expandDqSmall() const {
805 44276 Eigen::MatrixBlockView<vector_t, Eigen::Dynamic, 1, false, true>(
806
1/2
✓ Branch 4 taken 22138 times.
✗ Branch 5 not taken.
 22138 dq_, freeVariables_.nbIndices(), freeVariables_.indices()) = dqSmall_;
807 22138 }
808
809 3 std::ostream& HierarchicalIterative::print(std::ostream& os) const {
810 3 os << "HierarchicalIterative, " << stacks_.size() << " level." << iendl
811 3 << "max iter: " << maxIterations()
812 3 << ", error threshold: " << errorThreshold() << iendl << "dimension "
813 3 << dimension() << iendl << "reduced dimension " << reducedDimension()
814 3 << iendl << "free variables: " << freeVariables_ << incindent;
815 const std::size_t end =
816
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 3 times.
 3 (lastIsOptional_ ? stacks_.size() - 1 : stacks_.size());
817
2/2
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 3 times.
 6 for (std::size_t i = 0; i < stacks_.size(); ++i) {
818 const ImplicitConstraintSet::Implicits_t constraints(
819
1/2
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
 3 stacks_[i].constraints());
820 3 const Data& d = datas_[i];
821
3/6
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
 3 os << iendl << "Level " << i;
822
1/6
✗ Branch 0 not taken.
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 3 if (lastIsOptional_ && i == end) os << " (optional)";
823
4/8
✓ Branch 1 taken 3 times.
✗ Branch 2 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.
 3 os << ": Stack of " << constraints.size() << " functions" << incindent;
824 3 size_type rv = 0, rq = 0;
825
2/2
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 3 times.
 8 for (std::size_t j = 0; j < constraints.size(); ++j) {
826 5 const DifferentiableFunctionPtr_t& f(constraints[j]->functionPtr());
827
6/12
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 5 times.
✗ Branch 14 not taken.
✓ Branch 18 taken 5 times.
✗ Branch 19 not taken.
 5 os << iendl << j << ": [" << rv << ", " << f->outputDerivativeSize()
828
5/10
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 5 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 5 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 5 times.
✗ Branch 15 not taken.
 5 << "]," << incindent << *f << iendl << "Rhs: "
829
4/8
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 5 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 5 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 5 times.
✗ Branch 13 not taken.
 5 << condensed(d.rightHandSide.vector().segment(rq, f->outputSize()))
830
4/8
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 5 times.
✗ Branch 14 not taken.
 5 << iendl << "active rows: " << condensed(constraints[j]->activeRows())
831
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 << decindent;
832 5 rv += f->outputDerivativeSize();
833 5 rq += f->outputSize();
834 }
835
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 os << decendl;
836
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 3 os << "Equality idx: " << d.equalityIndices;
837
3/6
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
 3 os << iendl << "Active rows: " << d.activeRowsOfJ;
838 3 }
839 3 return os << decindent;
840 }
841
842 template HierarchicalIterative::Status HierarchicalIterative::solve(
843 vectorOut_t arg, lineSearch::Constant lineSearch) const;
844 template HierarchicalIterative::Status HierarchicalIterative::solve(
845 vectorOut_t arg, lineSearch::Backtracking lineSearch) const;
846 template HierarchicalIterative::Status HierarchicalIterative::solve(
847 vectorOut_t arg, lineSearch::FixedSequence lineSearch) const;
848 template HierarchicalIterative::Status HierarchicalIterative::solve(
849 vectorOut_t arg, lineSearch::ErrorNormBased lineSearch) const;
850
851 template <class Archive>
852 2 void HierarchicalIterative::load(Archive& ar, const unsigned int version) {
853 (void)version;
854
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 ar& BOOST_SERIALIZATION_NVP(squaredErrorThreshold_);
855
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 ar& BOOST_SERIALIZATION_NVP(inequalityThreshold_);
856
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 ar& BOOST_SERIALIZATION_NVP(maxIterations_);
857
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 ar& BOOST_SERIALIZATION_NVP(configSpace_);
858
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 ar& BOOST_SERIALIZATION_NVP(lastIsOptional_);
859
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 ar& BOOST_SERIALIZATION_NVP(saturate_);
860
861
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 saturation_.resize(configSpace_->nq());
862
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 qSat_.resize(configSpace_->nq());
863
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 OM_.resize(configSpace_->nv());
864
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 OP_.resize(configSpace_->nv());
865 // Initialize freeVariables_ to all indices.
866
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 freeVariables_.addRow(0, configSpace_->nv());
867
868 2 NumericalConstraints_t constraints;
869 2 std::vector<std::size_t> priorities;
870
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 ar& boost::serialization::make_nvp("constraints_", constraints);
871
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 ar& BOOST_SERIALIZATION_NVP(priorities);
872
873
2/2
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 1 times.
 8 for (std::size_t i = 0; i < constraints.size(); ++i)
874
1/2
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
 6 add(constraints[i], priorities[i]);
875 // TODO load the right hand side.
876 }
877
878 template <class Archive>
879 2 void HierarchicalIterative::save(Archive& ar,
880 const unsigned int version) const {
881 (void)version;
882
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 ar& BOOST_SERIALIZATION_NVP(squaredErrorThreshold_);
883
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 ar& BOOST_SERIALIZATION_NVP(inequalityThreshold_);
884
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 ar& BOOST_SERIALIZATION_NVP(maxIterations_);
885
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 ar& BOOST_SERIALIZATION_NVP(configSpace_);
886
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 ar& BOOST_SERIALIZATION_NVP(lastIsOptional_);
887
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 ar& BOOST_SERIALIZATION_NVP(saturate_);
888
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 ar& BOOST_SERIALIZATION_NVP(constraints_);
889
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 std::vector<std::size_t> priorities(constraints_.size());
890
2/2
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 1 times.
 8 for (std::size_t i = 0; i < constraints_.size(); ++i) {
891 6 const DifferentiableFunctionPtr_t& f = constraints_[i]->functionPtr();
892 std::map<DifferentiableFunctionPtr_t, std::size_t>::const_iterator c(
893
1/2
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
 6 find_if(priority_.begin(), priority_.end(),
894 5 [&f](const priorityIt_t& arg) { return *arg.first == *f; }));
895
2/2
✓ Branch 2 taken 1 times.
✓ Branch 3 taken 2 times.
 6 if (c == priority_.end())
896 2 priorities[i] = 0;
897 else
898 4 priorities[i] = c->second;
899 }
900
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 ar& BOOST_SERIALIZATION_NVP(priorities);
901 // TODO save the right hand side.
902 }
903
904 HPP_SERIALIZATION_SPLIT_IMPLEMENT(HierarchicalIterative);
905 } // namespace solver
906 } // namespace constraints
907 } // namespace hpp
908
909 12 BOOST_CLASS_EXPORT(hpp::constraints::solver::saturation::Bounds)
910 12 BOOST_CLASS_EXPORT(hpp::constraints::solver::saturation::Device)
911
912 namespace boost {
913 namespace serialization {
914 using namespace hpp::constraints::solver;
915
916 template <class Archive>
917 4 void serialize(Archive&, saturation::Base&, const unsigned int) {}
918 template <class Archive>
919 4 void serialize(Archive& ar, saturation::Device& o, const unsigned int version) {
920 (void)version;
921
1/2
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
 4 ar& make_nvp("base", base_object<saturation::Base>(o));
922
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 4 ar& make_nvp("device", o.device);
923 }
924 template <class Archive>
925 ✗ void serialize(Archive& ar, saturation::Bounds& o, const unsigned int version) {
926 (void)version;
927 ✗ ar& make_nvp("base", base_object<saturation::Base>(o));
928 ✗ hpp::serialization::remove_duplicate::serialize_vector(ar, "lb", o.lb,
929 version);
930 ✗ hpp::serialization::remove_duplicate::serialize_vector(ar, "ub", o.ub,
931 version);
932 }
933 } // namespace serialization
934 } // namespace boost
935