GCC Code Coverage Report

Directory: ./
File: src/solver/by-substitution.cc
Date: 2025-05-05 12:19:30
Exec Total Coverage
Lines: 151 181 83.4%
Branches: 153 356 43.0%
Line Branch Exec Source
1 // Copyright (c) 2017, 2018 CNRS
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/nvp.hpp>
30 #include <hpp/constraints/active-set-differentiable-function.hh>
31 #include <hpp/constraints/macros.hh>
32 #include <hpp/constraints/solver/by-substitution.hh>
33 #include <hpp/constraints/solver/impl/by-substitution.hh>
34 #include <hpp/constraints/solver/impl/hierarchical-iterative.hh>
35 #include <hpp/constraints/svd.hh>
36 #include <hpp/pinocchio/configuration.hh>
37 #include <hpp/pinocchio/liegroup-element.hh>
38 #include <hpp/pinocchio/util.hh>
39 #include <hpp/util/serialization.hh>
40
41 namespace hpp {
42 namespace constraints {
43 namespace solver {
44 namespace lineSearch {
45 template bool Constant::operator()(const BySubstitution& solver,
46 vectorOut_t arg, vectorOut_t darg);
47
48 template bool Backtracking::operator()(const BySubstitution& solver,
49 vectorOut_t arg, vectorOut_t darg);
50
51 template bool FixedSequence::operator()(const BySubstitution& solver,
52 vectorOut_t arg, vectorOut_t darg);
53
54 template bool ErrorNormBased::operator()(const BySubstitution& solver,
55 vectorOut_t arg, vectorOut_t darg);
56 } // namespace lineSearch
57
58 1023 BySubstitution::BySubstitution(const LiegroupSpacePtr_t& configSpace)
59 : HierarchicalIterative(configSpace),
60 1023 explicit_(configSpace),
61
3/6
✓ Branch 2 taken 1023 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1023 times.
✗ Branch 6 not taken.
✓ Branch 12 taken 1023 times.
✗ Branch 13 not taken.
 1023 JeExpanded_(configSpace->nv(), configSpace->nv()) {}
62
63 4 BySubstitution::BySubstitution(const BySubstitution& other)
64 : HierarchicalIterative(other),
65 4 explicit_(other.explicit_),
66
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 Je_(other.Je_),
67
2/4
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
 8 JeExpanded_(other.JeExpanded_) {
68 4 for (NumericalConstraints_t::iterator it(constraints_.begin());
69
2/2
✓ Branch 3 taken 12 times.
✓ Branch 4 taken 4 times.
 16 it != constraints_.end(); ++it) {
70 // assert (contains (*it));
71 }
72 4 }
73
74 1049 bool BySubstitution::add(const ImplicitPtr_t& nm, const std::size_t& priority) {
75
2/4
✓ Branch 1 taken 1049 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1049 times.
 1049 if (contains(nm)) {
76 hppDout(error, "Constraint " << nm->functionPtr()->name()
77 << " already in BySubstitution solver."
78 << std::endl);
79 ✗ return false;
80 }
81
2/4
✓ Branch 2 taken 1049 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1049 times.
✗ Branch 6 not taken.
 1049 ComparisonTypes_t types = nm->comparisonType();
82
83 1049 bool addedAsExplicit = false;
84 1049 ExplicitPtr_t enm(HPP_DYNAMIC_PTR_CAST(Explicit, nm));
85
2/2
✓ Branch 1 taken 26 times.
✓ Branch 2 taken 1023 times.
 1049 if (enm) {
86
1/2
✓ Branch 2 taken 26 times.
✗ Branch 3 not taken.
 26 addedAsExplicit = explicitConstraintSet().add(enm) >= 0;
87 26 if (!addedAsExplicit) {
88 hppDout(info, "Could not treat " << enm->explicitFunction()->name()
89 << " as an explicit function.");
90 }
91 }
92
93
2/2
✓ Branch 0 taken 26 times.
✓ Branch 1 taken 1023 times.
 1049 if (addedAsExplicit) {
94 // If added as explicit, add to the list of constraint of Hierarchical
95 // iterative
96
1/2
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
 26 constraints_.push_back(nm);
97 hppDout(info,
98 "Numerical constraint added as explicit function: "
99 << enm->explicitFunction()->name() << "with "
100 << "input conf " << Eigen::RowBlockIndices(enm->inputConf())
101 << "input vel" << Eigen::RowBlockIndices(enm->inputVelocity())
102 << "output conf " << Eigen::RowBlockIndices(enm->outputConf())
103 << "output vel "
104 << Eigen::RowBlockIndices(enm->outputVelocity()));
105
1/2
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
 26 explicitConstraintSetHasChanged();
106 } else
107
1/2
✓ Branch 1 taken 1023 times.
✗ Branch 2 not taken.
 1023 HierarchicalIterative::add(nm, priority);
108 hppDout(info, "Constraint has dimension " << dimension());
109
110
2/4
✓ Branch 1 taken 1049 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1049 times.
 1049 assert(contains(nm));
111 1049 return true;
112 1049 }
113
114 29 void BySubstitution::explicitConstraintSetHasChanged() {
115 // set free variables to indices that are not output of the explicit
116 // constraint.
117
2/4
✓ Branch 3 taken 29 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 29 times.
✗ Branch 7 not taken.
 29 freeVariables(explicit_.notOutDers().transpose());
118 29 }
119
120 2 segments_t BySubstitution::implicitDof() const {
121 2 const Eigen::MatrixXi& ioDep = explicit_.inOutDependencies();
122 2 const Eigen::VectorXi& derF = explicit_.derFunction();
123
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 ArrayXb adp(activeDerivativeParameters());
124
2/4
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
 2 Eigen::VectorXi out(Eigen::VectorXi::Zero(adp.size()));
125
126
2/2
✓ Branch 1 taken 6 times.
✓ Branch 2 taken 2 times.
 8 for (size_type i = 0; i < adp.size(); ++i) {
127
3/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 4 times.
✓ Branch 4 taken 2 times.
 6 if (adp(i)) {
128
3/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 3 times.
✓ Branch 4 taken 1 times.
 4 if (derF[i] >= 0) {
129
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 out += ioDep.row(derF[i]);
130
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 out(i) = 0;
131 } else
132
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 out(i) += 1;
133 }
134 }
135
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
 4 return BlockIndex::fromLogicalExpression(out.array().cast<bool>());
136 2 }
137
138 // Note that the jacobian of the implicit constraints have already
139 // been computed by computeValue <true>
140 // The Jacobian of the implicit constraint of priority i is stored in
141 // datas_ [i].jacobian
142 13396 void BySubstitution::updateJacobian(vectorIn_t arg) const {
143
2/2
✓ Branch 2 taken 13214 times.
✓ Branch 3 taken 182 times.
 13396 if (explicit_.inDers().nbCols() == 0) return;
144 /* ------
145 / in in u out \
146 | |
147 Je_ = | df |
148 | ---- (qin) 0 |
149 \ dqin /
150 */
151
2/4
✓ Branch 2 taken 182 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 182 times.
✗ Branch 6 not taken.
 182 explicit_.jacobian(JeExpanded_, arg);
152
1/2
✓ Branch 2 taken 182 times.
✗ Branch 3 not taken.
 182 Je_ = explicit_.jacobianNotOutToOut(JeExpanded_);
153
154 hppDnum(info, "Jacobian of explicit system is" << iendl << setpyformat
155 << pretty_print(Je_));
156
157
2/2
✓ Branch 1 taken 182 times.
✓ Branch 2 taken 182 times.
 364 for (std::size_t i = 0; i < stacks_.size(); ++i) {
158 182 Data& d = datas_[i];
159 hppDnum(
160 info,
161 "Jacobian of stack "
162 << i << " before update:" << iendl << pretty_print(d.reducedJ)
163 << iendl << "Jacobian of explicit variable of stack " << i << ":"
164 << iendl
165 << pretty_print(
166 explicit_.outDers().transpose().rview(d.jacobian).eval()));
167
2/4
✓ Branch 2 taken 182 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 182 times.
✗ Branch 6 not taken.
 364 d.reducedJ.noalias() += Eigen::MatrixBlocksRef<>(d.activeRowsOfJ.keepRows(),
168 182 explicit_.outDers())
169
1/2
✓ Branch 1 taken 182 times.
✗ Branch 2 not taken.
 182 .rview(d.jacobian)
170
1/2
✓ Branch 1 taken 182 times.
✗ Branch 2 not taken.
 364 .eval() *
171
2/4
✓ Branch 2 taken 182 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 182 times.
✗ Branch 6 not taken.
 546 Je_;
172 hppDnum(info, "Jacobian of stack " << i << " after update:" << iendl
173 << pretty_print(d.reducedJ)
174 << unsetpyformat);
175 }
176 }
177
178 1049 void BySubstitution::computeActiveRowsOfJ(std::size_t iStack) {
179 1049 Data& d = datas_[iStack];
180 const ImplicitConstraintSet::Implicits_t constraints(
181
1/2
✓ Branch 3 taken 1049 times.
✗ Branch 4 not taken.
 1049 stacks_[iStack].constraints());
182 1049 std::size_t row = 0;
183
184 /// ADP: Active Derivative Param
185
1/2
✓ Branch 1 taken 1049 times.
✗ Branch 2 not taken.
 1049 Eigen::MatrixXi explicitIOdep = explicit_.inOutDofDependencies();
186
4/8
✓ Branch 1 taken 1049 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1049 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1049 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✓ Branch 10 taken 1049 times.
 1049 assert((explicitIOdep.array() >= 0).all());
187
188 typedef Eigen::MatrixBlocks<false, false> BlockIndices;
189
190
2/4
✓ Branch 1 taken 1049 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1049 times.
✗ Branch 5 not taken.
 1049 ArrayXb adpF, adpC;
191 1049 BlockIndices::segments_t rows;
192
2/2
✓ Branch 1 taken 1058 times.
✓ Branch 2 taken 1049 times.
 2107 for (std::size_t i = 0; i < constraints.size(); ++i) {
193 bool active;
194
195 // Test on the variable left free by the explicit solver.
196 1058 adpF = freeVariables_
197
1/2
✓ Branch 2 taken 1058 times.
✗ Branch 3 not taken.
 2116 .rview(constraints[i]
198 1058 ->function()
199 1058 .activeDerivativeParameters()
200
1/2
✓ Branch 1 taken 1058 times.
✗ Branch 2 not taken.
 1058 .matrix())
201
1/2
✓ Branch 1 taken 1058 times.
✗ Branch 2 not taken.
 1058 .eval()
202
2/4
✓ Branch 1 taken 1058 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1058 times.
✗ Branch 5 not taken.
 1058 .array();
203
1/2
✓ Branch 1 taken 1058 times.
✗ Branch 2 not taken.
 1058 active = adpF.any();
204
6/6
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 1054 times.
✓ Branch 3 taken 3 times.
✓ Branch 4 taken 1 times.
✓ Branch 5 taken 3 times.
✓ Branch 6 taken 1055 times.
 1058 if (!active && explicitIOdep.size() > 0) {
205 // Test on the variable constrained by the explicit solver.
206 3 adpC = explicit_.outDers()
207
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 6 .rview(constraints[i]
208 3 ->function()
209 3 .activeDerivativeParameters()
210
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 .matrix())
211
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 .eval()
212
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 3 .array();
213
4/8
✓ 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.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
 3 adpF = (explicitIOdep.transpose() * adpC.cast<int>().matrix())
214
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 .array()
215
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 3 .cast<bool>();
216
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 active = adpF.any();
217 }
218
2/2
✓ Branch 0 taken 1056 times.
✓ Branch 1 taken 2 times.
 1058 if (active) { // If at least one element of adp is true
219
2/2
✓ Branch 7 taken 1056 times.
✓ Branch 8 taken 1056 times.
 2112 for (const segment_t s : constraints[i]->activeRows()) {
220
1/2
✓ Branch 1 taken 1056 times.
✗ Branch 2 not taken.
 1056 rows.emplace_back(s.first + row, s.second);
221 }
222 }
223 1058 row += constraints[i]->function().outputDerivativeSize();
224 }
225 d.activeRowsOfJ =
226
2/4
✓ Branch 1 taken 1049 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1049 times.
✗ Branch 5 not taken.
 1049 Eigen::MatrixBlocks<false, false>(rows, freeVariables_.m_rows);
227
1/2
✓ Branch 1 taken 1049 times.
✗ Branch 2 not taken.
 1049 d.activeRowsOfJ.updateRows<true, true, true>();
228 1049 }
229
230 ✗ void BySubstitution::projectVectorOnKernel(ConfigurationIn_t arg,
231 vectorIn_t darg,
232 vectorOut_t result) const {
233 ✗ if (constraints_.empty() || reducedDimension() == 0) {
234 ✗ result = darg;
235 ✗ return;
236 }
237 ✗ computeValue<true>(arg);
238 ✗ updateJacobian(arg);
239 ✗ getReducedJacobian(reducedJ_);
240
241 ✗ svd_.compute(reducedJ_);
242
243 // TODO the output of explicit solver should be set to zero ?
244 ✗ dqSmall_ = freeVariables_.rview(darg);
245
246 ✗ size_type rank = svd_.rank();
247 ✗ vector_t tmp(getV1(svd_, rank).adjoint() * dqSmall_);
248 ✗ dqSmall_.noalias() -= getV1(svd_, rank) * tmp;
249
250 // Otherwise two uninitialized values may sum up to NaN
251 ✗ result.setZero();
252 ✗ freeVariables_.lview(result) = dqSmall_;
253 }
254
255 ✗ void BySubstitution::projectOnKernel(ConfigurationIn_t from,
256 ConfigurationIn_t to,
257 ConfigurationOut_t result) {
258 // TODO equivalent
259 ✗ if (constraints_.empty()) {
260 ✗ result = to;
261 ✗ return;
262 }
263 typedef pinocchio::LiegroupElement Lge_t;
264 typedef pinocchio::LiegroupElementConstRef LgeConstRef_t;
265 ✗ LgeConstRef_t O(from, configSpace_);
266 ✗ LgeConstRef_t M(to, configSpace_);
267 ✗ OM_ = M - O;
268
269 ✗ projectVectorOnKernel(from, OM_, OP_);
270
271 ✗ assert(hpp::pinocchio::checkNormalized(M));
272 ✗ Lge_t P(O + OP_);
273 ✗ assert(hpp::pinocchio::checkNormalized(P));
274 ✗ saturate_->saturate(P.vector(), result, saturation_);
275 }
276
277 2 std::ostream& BySubstitution::print(std::ostream& os) const {
278 2 os << "BySubstitution" << incendl;
279 2 HierarchicalIterative::print(os) << iendl;
280 2 explicit_.print(os) << decindent;
281 2 return os;
282 }
283
284 1504 vector_t BySubstitution::rightHandSideFromConfig(ConfigurationIn_t config) {
285 1504 const size_type top = parent_t::rightHandSideSize();
286 1504 const size_type bot = explicit_.rightHandSideSize();
287
1/2
✓ Branch 1 taken 1504 times.
✗ Branch 2 not taken.
 1504 vector_t rhs(top + bot);
288
4/8
✓ Branch 1 taken 1504 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1504 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1504 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1504 times.
✗ Branch 11 not taken.
 1504 rhs.head(top) = parent_t::rightHandSideFromConfig(config);
289
4/8
✓ Branch 1 taken 1504 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1504 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1504 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1504 times.
✗ Branch 11 not taken.
 1504 rhs.tail(bot) = explicit_.rightHandSideFromInput(config);
290 1504 return rhs;
291 }
292
293 4103 bool BySubstitution::rightHandSideFromConfig(const ImplicitPtr_t& constraint,
294 ConfigurationIn_t config) {
295
4/6
✓ Branch 1 taken 4103 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4103 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2101 times.
✓ Branch 8 taken 2002 times.
 4103 if (parent_t::rightHandSideFromConfig(constraint, config)) return true;
296 2002 ExplicitPtr_t exp(HPP_DYNAMIC_PTR_CAST(Explicit, constraint));
297
1/2
✓ Branch 1 taken 2002 times.
✗ Branch 2 not taken.
 2002 if (exp) {
298
2/4
✓ Branch 1 taken 2002 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2002 times.
✗ Branch 5 not taken.
 2002 return explicit_.rightHandSideFromInput(exp, config);
299 }
300 ✗ return false;
301 2002 }
302
303 3103 bool BySubstitution::rightHandSide(const ImplicitPtr_t& constraint,
304 vectorIn_t rhs) {
305
4/6
✓ Branch 1 taken 3103 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3103 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2101 times.
✓ Branch 8 taken 1002 times.
 3103 if (parent_t::rightHandSide(constraint, rhs)) return true;
306 1002 ExplicitPtr_t exp(HPP_DYNAMIC_PTR_CAST(Explicit, constraint));
307
1/2
✓ Branch 1 taken 1002 times.
✗ Branch 2 not taken.
 1002 if (exp) {
308
2/4
✓ Branch 1 taken 1002 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1002 times.
✗ Branch 5 not taken.
 1002 return explicit_.rightHandSide(exp, rhs);
309 }
310 ✗ return false;
311 1002 }
312
313 5103 bool BySubstitution::getRightHandSide(const ImplicitPtr_t& constraint,
314 vectorOut_t rhs) const {
315
4/6
✓ Branch 1 taken 5103 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5103 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 3101 times.
✓ Branch 7 taken 2002 times.
 5103 if (parent_t::getRightHandSide(constraint, rhs)) return true;
316 2002 ExplicitPtr_t exp(HPP_DYNAMIC_PTR_CAST(Explicit, constraint));
317
1/2
✓ Branch 1 taken 2002 times.
✗ Branch 2 not taken.
 2002 if (exp) {
318
2/4
✓ Branch 1 taken 2002 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2002 times.
✗ Branch 5 not taken.
 2002 return explicit_.getRightHandSide(exp, rhs);
319 }
320 ✗ return false;
321 2002 }
322
323 100 void BySubstitution::rightHandSide(vectorIn_t rhs) {
324 100 const size_type top = parent_t::rightHandSideSize();
325 100 const size_type bot = explicit_.rightHandSideSize();
326
2/4
✓ Branch 2 taken 100 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.
 100 parent_t::rightHandSide(rhs.head(top));
327
2/4
✓ Branch 2 taken 100 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.
 100 explicit_.rightHandSide(rhs.head(bot));
328 100 }
329
330 400 vector_t BySubstitution::rightHandSide() const {
331 400 const size_type top = parent_t::rightHandSideSize();
332 400 const size_type bot = explicit_.rightHandSideSize();
333
1/2
✓ Branch 1 taken 400 times.
✗ Branch 2 not taken.
 400 vector_t rhs(top + bot);
334
3/6
✓ Branch 1 taken 400 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 400 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 400 times.
✗ Branch 8 not taken.
 400 rhs.head(top) = parent_t::rightHandSide();
335
3/6
✓ Branch 1 taken 400 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 400 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 400 times.
✗ Branch 8 not taken.
 400 rhs.tail(bot) = explicit_.rightHandSide();
336 400 return rhs;
337 }
338
339 1 size_type BySubstitution::rightHandSideSize() const {
340 1 const size_type top = parent_t::rightHandSideSize();
341 1 const size_type bot = explicit_.rightHandSideSize();
342 1 return top + bot;
343 }
344
345 6 bool BySubstitution::isConstraintSatisfied(const ImplicitPtr_t& constraint,
346 vectorIn_t arg, vectorOut_t error,
347 bool& constraintFound) const {
348 6 constraintFound = false;
349 bool satisfied(
350
2/4
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
 6 parent_t::isConstraintSatisfied(constraint, arg, error, constraintFound));
351
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 4 times.
 6 if (constraintFound) return satisfied;
352
2/4
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
 8 return explicit_.isConstraintSatisfied(constraint, arg, error,
353 4 constraintFound);
354 }
355
356 template <class Archive>
357 2 void BySubstitution::load(Archive& ar, const unsigned int version) {
358 using namespace boost::serialization;
359 (void)version;
360 2 LiegroupSpacePtr_t space;
361
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 ar& BOOST_SERIALIZATION_NVP(space);
362
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 explicit_.init(space);
363
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 ar& make_nvp("base", base_object<HierarchicalIterative>(*this));
364 }
365
366 template <class Archive>
367 2 void BySubstitution::save(Archive& ar, const unsigned int version) const {
368 using namespace boost::serialization;
369 (void)version;
370 2 LiegroupSpacePtr_t space(explicit_.configSpace());
371
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 ar& BOOST_SERIALIZATION_NVP(space);
372
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 ar& make_nvp("base", base_object<HierarchicalIterative>(*this));
373 }
374
375 HPP_SERIALIZATION_SPLIT_IMPLEMENT(BySubstitution);
376
377 template BySubstitution::Status BySubstitution::impl_solve(
378 vectorOut_t arg, bool optimize, lineSearch::Constant lineSearch) const;
379 template BySubstitution::Status BySubstitution::impl_solve(
380 vectorOut_t arg, bool optimize, lineSearch::Backtracking lineSearch) const;
381 template BySubstitution::Status BySubstitution::impl_solve(
382 vectorOut_t arg, bool optimize, lineSearch::FixedSequence lineSearch) const;
383 template BySubstitution::Status BySubstitution::impl_solve(
384 vectorOut_t arg, bool optimize,
385 lineSearch::ErrorNormBased lineSearch) const;
386 } // namespace solver
387 } // namespace constraints
388 } // namespace hpp
389
390 11 BOOST_CLASS_EXPORT(hpp::constraints::solver::BySubstitution)
391