GCC Code Coverage Report

Directory: ./
File: src/explicit-constraint-set.cc
Date: 2025-05-05 12:19:30
Exec Total Coverage
Lines: 242 284 85.2%
Branches: 271 544 49.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 <hpp/constraints/explicit-constraint-set.hh>
30 #include <hpp/constraints/explicit.hh>
31 #include <hpp/constraints/matrix-view.hh>
32 #include <hpp/pinocchio/device.hh>
33 #include <hpp/pinocchio/liegroup.hh>
34 #include <hpp/pinocchio/util.hh>
35 #include <hpp/util/indent.hh>
36 #include <queue>
37
38 namespace hpp {
39 namespace constraints {
40 typedef Eigen::MatrixBlocksRef<false, false> MatrixBlocksRef;
41
42 namespace {
43 /// Append all indices of a set of segments to a queue of indices
44 6476 void append(const Eigen::RowBlockIndices& rbi, std::queue<size_type>& q) {
45
2/2
✓ Branch 2 taken 6460 times.
✓ Branch 3 taken 6476 times.
 12936 for (std::size_t i = 0; i < rbi.indices().size(); ++i)
46
2/2
✓ Branch 2 taken 44914 times.
✓ Branch 3 taken 6460 times.
 51374 for (size_type j = 0; j < rbi.indices()[i].second; ++j)
47
1/2
✓ Branch 3 taken 44914 times.
✗ Branch 4 not taken.
 44914 q.push(rbi.indices()[i].first + j);
48 6476 }
49 } // namespace
50
51 ✗ Eigen::ColBlockIndices ExplicitConstraintSet::activeParameters() const {
52 ✗ return inArgs_.transpose();
53 }
54
55 const Eigen::ColBlockIndices&
56 ✗ ExplicitConstraintSet::activeDerivativeParameters() const {
57 ✗ return inDers_;
58 }
59
60 34838 bool ExplicitConstraintSet::solve(vectorOut_t arg) const {
61
2/2
✓ Branch 1 taken 7316 times.
✓ Branch 2 taken 34838 times.
 42154 for (std::size_t i = 0; i < data_.size(); ++i) {
62
1/2
✓ Branch 3 taken 7316 times.
✗ Branch 4 not taken.
 7316 solveExplicitConstraint(computationOrder_[i], arg);
63 }
64 34838 return true;
65 }
66
67 1012 bool ExplicitConstraintSet::isSatisfied(vectorIn_t arg, vectorOut_t error,
68 value_type errorThreshold) const {
69 // Recover default value
70
2/2
✓ Branch 0 taken 1007 times.
✓ Branch 1 taken 5 times.
 1012 if (errorThreshold == -1) errorThreshold = errorThreshold_;
71 1012 value_type squaredNorm = 0;
72
73 1012 size_type row = 0;
74
2/2
✓ Branch 1 taken 17 times.
✓ Branch 2 taken 1012 times.
 1029 for (std::size_t i = 0; i < data_.size(); ++i) {
75 17 const Data& d(data_[i]);
76 17 const DifferentiableFunction& h(d.constraint->function());
77
3/6
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 17 times.
✗ Branch 8 not taken.
 17 h.value(d.h_value, arg);
78 17 size_type nRows(h.outputSpace()->nv());
79
2/4
✓ Branch 5 taken 17 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 17 times.
 17 assert(*(d.h_value.space()) == *(d.rhs_implicit.space()));
80
3/6
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 17 times.
✗ Branch 8 not taken.
 17 error.segment(row, nRows) = d.h_value - d.rhs_implicit;
81 17 squaredNorm =
82
2/4
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17 times.
✗ Branch 5 not taken.
 17 std::max(squaredNorm, error.segment(row, nRows).squaredNorm());
83 17 row += nRows;
84 }
85
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1012 times.
 1012 assert(row == error.size());
86 hppDout(info, "Max squared error norm is " << squaredNorm);
87 1012 return squaredNorm < errorThreshold * errorThreshold;
88 }
89
90 5 bool ExplicitConstraintSet::isSatisfied(vectorIn_t arg,
91 value_type errorThreshold) const {
92 // Recover default value
93
1/2
✓ Branch 0 taken 5 times.
✗ Branch 1 not taken.
 5 if (errorThreshold == -1) errorThreshold = errorThreshold_;
94 5 diffSmall_.resize(errorSize_);
95
2/4
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
 5 return isSatisfied(arg, diffSmall_, errorThreshold);
96 }
97
98 4 bool ExplicitConstraintSet::isConstraintSatisfied(
99 const ImplicitPtr_t& constraint, vectorIn_t arg, vectorOut_t error,
100 bool& constraintFound) const {
101 4 value_type squaredNorm = 0;
102 4 constraintFound = false;
103
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 for (std::size_t i = 0; i < data_.size(); ++i) {
104 6 const Data& d(data_[i]);
105
4/4
✓ Branch 5 taken 2 times.
✓ Branch 6 taken 4 times.
✓ Branch 7 taken 4 times.
✓ Branch 8 taken 2 times.
 8 if (d.constraint->functionPtr() == constraint->functionPtr() ||
106
1/2
✗ Branch 5 not taken.
✓ Branch 6 taken 2 times.
 2 d.constraint->function() == constraint->function()) {
107 4 const DifferentiableFunction& h(d.constraint->function());
108
2/4
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
 4 h.value(d.h_value, arg);
109
1/2
✗ Branch 4 not taken.
✓ Branch 5 taken 4 times.
 4 assert(error.size() == h.outputSpace()->nv());
110
1/2
✗ Branch 5 not taken.
✓ Branch 6 taken 4 times.
 4 assert(*(d.h_value.space()) == *(d.rhs_implicit.space()));
111
1/2
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 4 error = d.h_value - d.rhs_implicit;
112 4 squaredNorm = error.squaredNorm();
113 4 constraintFound = true;
114 4 return squaredNorm < errorThreshold_ * errorThreshold_;
115 }
116 }
117 ✗ return false;
118 }
119
120 ✗ size_type size(const segments_t& intervals) {
121 ✗ size_type res = 0;
122 ✗ for (std::size_t i = 0; i < intervals.size(); ++i) {
123 ✗ res += intervals[i].second;
124 }
125 ✗ return res;
126 }
127
128 6461 ExplicitConstraintSet::Data::Data(const ExplicitPtr_t& _constraint)
129 6461 : constraint(_constraint),
130
1/2
✓ Branch 6 taken 6461 times.
✗ Branch 7 not taken.
 6461 rhs_implicit(_constraint->functionPtr()->outputSpace()->neutral()),
131
1/2
✓ Branch 5 taken 6461 times.
✗ Branch 6 not taken.
 6461 h_value(_constraint->functionPtr()->outputSpace()),
132
1/2
✓ Branch 5 taken 6461 times.
✗ Branch 6 not taken.
 6461 f_value(_constraint->explicitFunction()->outputSpace()),
133
5/10
✓ Branch 1 taken 6461 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6461 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6461 times.
✗ Branch 8 not taken.
✓ Branch 14 taken 6461 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 6461 times.
✗ Branch 20 not taken.
 19383 res_qout(_constraint->explicitFunction()->outputSpace()) {
134
1/2
✓ Branch 7 taken 6461 times.
✗ Branch 8 not taken.
 6461 jacobian.resize(_constraint->explicitFunction()->outputDerivativeSize(),
135 12922 _constraint->explicitFunction()->inputDerivativeSize());
136
3/4
✓ Branch 2 taken 44992 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 38531 times.
✓ Branch 6 taken 6461 times.
 44992 for (std::size_t i = 0; i < constraint->comparisonType().size(); ++i) {
137
3/4
✓ Branch 2 taken 38531 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 19229 times.
✓ Branch 6 taken 19302 times.
 38531 if (constraint->comparisonType()[i] == Equality) {
138
1/2
✓ Branch 1 taken 19229 times.
✗ Branch 2 not taken.
 19229 equalityIndices.addRow(i, 1);
139 }
140 }
141
1/2
✓ Branch 1 taken 6461 times.
✗ Branch 2 not taken.
 6461 equalityIndices.updateRows<true, true, true>();
142 6461 }
143
144 6464 size_type ExplicitConstraintSet::add(const ExplicitPtr_t& constraint) {
145
1/2
✓ Branch 3 taken 6464 times.
✗ Branch 4 not taken.
 6464 assert(constraint->outputConf().size() == 1 &&
146 "Only contiguous function output is supported.");
147
1/2
✓ Branch 3 taken 6464 times.
✗ Branch 4 not taken.
 6464 assert(constraint->outputVelocity().size() == 1 &&
148 "Only contiguous function output is supported.");
149 typedef Eigen::RowBlockIndices RowBlockIndices;
150 typedef Eigen::ColBlockIndices ColBlockIndices;
151 6464 const RowBlockIndices::segment_t& outIdx(constraint->outputConf()[0]);
152 6464 const RowBlockIndices::segment_t& outDerIdx(constraint->outputVelocity()[0]);
153
154 // TODO This sanity check should not be necessary
155 // It is done in the while loop below
156 // Sanity check: is it explicit ?
157
2/2
✓ Branch 3 taken 6447 times.
✓ Branch 4 taken 6464 times.
 12911 for (std::size_t i = 0; i < constraint->inputConf().size(); ++i)
158
2/4
✓ Branch 4 taken 6447 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 6447 times.
 6447 if (BlockIndex::overlap(constraint->inputConf()[i], outIdx)) {
159 ✗ size_type f0(constraint->inputConf()[i].first);
160 ✗ size_type s0(f0 + constraint->inputConf()[i].second - 1);
161 ✗ size_type f1(outIdx.first);
162 ✗ size_type s1(f1 + outIdx.second - 1);
163 ✗ std::ostringstream oss;
164 ✗ oss << "Explicit constraint \"" << constraint->function().name()
165 ✗ << "\" is malformed.";
166 ✗ oss << " input [" << f0 << "," << s0 << "] and output [" << f1 << ","
167 ✗ << s1 << "] segments overlap.";
168 ✗ throw std::logic_error(oss.str().c_str());
169 }
170 // Sanity check: Comparison type must be either EqualToZero or Equality
171
1/2
✓ Branch 2 taken 6464 times.
✗ Branch 3 not taken.
 6464 const ComparisonTypes_t& comp(constraint->comparisonType());
172
1/2
✗ Branch 4 not taken.
✓ Branch 5 taken 6464 times.
 6464 assert(comp.size() ==
173 (std::size_t)constraint->function().outputDerivativeSize());
174
2/2
✓ Branch 1 taken 38534 times.
✓ Branch 2 taken 6464 times.
 44998 for (std::size_t i = 0; i < comp.size(); ++i)
175
4/6
✓ Branch 1 taken 19231 times.
✓ Branch 2 taken 19303 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 19231 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 38534 times.
 38534 if (comp[i] != EqualToZero && comp[i] != Equality) return -1;
176 // Check that no other function already computes its outputs.
177
1/2
✓ Branch 3 taken 6464 times.
✗ Branch 4 not taken.
 6464 RowBlockIndices outArg(constraint->outputConf());
178
7/12
✓ Branch 1 taken 6464 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6464 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6464 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6464 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 6464 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 2 times.
✓ Branch 17 taken 6462 times.
 6464 if ((outArg.rview(argFunction_).eval().array() >= 0).any()) return -1;
179 // Check that it does not insert a loop.
180
1/2
✓ Branch 1 taken 6462 times.
✗ Branch 2 not taken.
 6462 std::queue<size_type> idxArg;
181 // Put all indices of inArg into idxArg
182
1/2
✓ Branch 3 taken 6462 times.
✗ Branch 4 not taken.
 6462 RowBlockIndices inArg(constraint->inputConf());
183
1/2
✓ Branch 1 taken 6462 times.
✗ Branch 2 not taken.
 6462 append(inArg, idxArg);
184
2/2
✓ Branch 1 taken 44914 times.
✓ Branch 2 taken 6461 times.
 51375 while (!idxArg.empty()) {
185 // iArg must be computed before f
186 44914 size_type iArg = idxArg.front();
187 44914 idxArg.pop();
188 // iArg is an output of the new function -> new function should be
189 // computed earlier -> incompatible.
190
4/4
✓ Branch 0 taken 59 times.
✓ Branch 1 taken 44855 times.
✓ Branch 2 taken 1 times.
✓ Branch 3 taken 58 times.
 44914 if (iArg >= outIdx.first && iArg < outIdx.first + outIdx.second) return -1;
191 // iArg is not computed by any function
192
3/4
✓ Branch 1 taken 44913 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 44899 times.
✓ Branch 4 taken 14 times.
 44913 if (argFunction_[iArg] < 0) continue;
193
1/2
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
 14 const Data& d = data_[argFunction_[iArg]];
194
2/4
✓ Branch 3 taken 14 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 14 times.
✗ Branch 7 not taken.
 14 append(d.constraint->inputConf(), idxArg);
195 }
196 6461 size_type nq(configSpace_->nq());
197 6461 size_type nv(configSpace_->nv());
198 // Add the function
199 6461 int idx = int(data_.size());
200
2/4
✓ Branch 1 taken 6461 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6461 times.
✗ Branch 5 not taken.
 6461 outArg.lview(argFunction_).setConstant(idx);
201
1/2
✓ Branch 3 taken 6461 times.
✗ Branch 4 not taken.
 6461 RowBlockIndices(constraint->outputVelocity())
202
1/2
✓ Branch 1 taken 6461 times.
✗ Branch 2 not taken.
 12922 .lview(derFunction_)
203
1/2
✓ Branch 1 taken 6461 times.
✗ Branch 2 not taken.
 6461 .setConstant(idx);
204
2/4
✓ Branch 1 taken 6461 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6461 times.
✗ Branch 5 not taken.
 6461 data_.push_back(Data(constraint));
205 6461 errorSize_ += data_.back().rhs_implicit.space()->nv();
206
207 // Update the free dofs
208
1/2
✓ Branch 1 taken 6461 times.
✗ Branch 2 not taken.
 6461 outArgs_.addRow(outIdx.first, outIdx.second);
209
1/2
✓ Branch 1 taken 6461 times.
✗ Branch 2 not taken.
 6461 outArgs_.updateIndices<true, true, true>();
210
1/2
✓ Branch 1 taken 6461 times.
✗ Branch 2 not taken.
 12922 notOutArgs_ = RowBlockIndices(
211
3/6
✓ Branch 1 taken 6461 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6461 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6461 times.
✗ Branch 9 not taken.
 19383 BlockIndex::difference(BlockIndex::segment_t(0, nq), outArgs_.indices()));
212
213
1/2
✓ Branch 2 taken 6461 times.
✗ Branch 3 not taken.
 6461 BlockIndex::add(inArgs_.m_rows, inArg.rows());
214 inArgs_ =
215
3/6
✓ Branch 3 taken 6461 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 6461 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 6461 times.
✗ Branch 10 not taken.
 6461 RowBlockIndices(BlockIndex::difference(inArgs_.rows(), outArgs_.rows()));
216 // should be sorted already
217
1/2
✓ Branch 1 taken 6461 times.
✗ Branch 2 not taken.
 6461 inArgs_.updateIndices<false, true, true>();
218
219
1/2
✓ Branch 1 taken 6461 times.
✗ Branch 2 not taken.
 6461 outDers_.addRow(outDerIdx.first, outDerIdx.second);
220
1/2
✓ Branch 1 taken 6461 times.
✗ Branch 2 not taken.
 6461 outDers_.updateIndices<true, true, true>();
221
1/2
✓ Branch 1 taken 6461 times.
✗ Branch 2 not taken.
 12922 notOutDers_ = ColBlockIndices(
222
3/6
✓ Branch 1 taken 6461 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6461 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6461 times.
✗ Branch 9 not taken.
 19383 BlockIndex::difference(BlockIndex::segment_t(0, nv), outDers_.indices()));
223
224
1/2
✓ Branch 1 taken 6461 times.
✗ Branch 2 not taken.
 6461 BlockIndex::add(inDers_.m_cols,
225
1/2
✓ Branch 3 taken 6461 times.
✗ Branch 4 not taken.
 12922 ColBlockIndices(constraint->inputVelocity()).cols());
226 inDers_ =
227
3/6
✓ Branch 3 taken 6461 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 6461 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 6461 times.
✗ Branch 10 not taken.
 6461 ColBlockIndices(BlockIndex::difference(inDers_.cols(), outDers_.rows()));
228 // should be sorted already
229
1/2
✓ Branch 1 taken 6461 times.
✗ Branch 2 not taken.
 6461 inDers_.updateIndices<false, true, true>();
230
231 /// Computation order
232 6461 std::size_t order = 0;
233
1/2
✓ Branch 2 taken 6461 times.
✗ Branch 3 not taken.
 6461 computationOrder_.resize(data_.size());
234
2/4
✓ Branch 2 taken 6461 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6461 times.
✗ Branch 6 not taken.
 6461 inOutDependencies_ = Eigen::MatrixXi::Zero(data_.size(), nv);
235
1/2
✓ Branch 3 taken 6461 times.
✗ Branch 4 not taken.
 6461 Computed_t computed(data_.size(), false);
236
2/2
✓ Branch 1 taken 6500 times.
✓ Branch 2 taken 6461 times.
 12961 for (std::size_t i = 0; i < data_.size(); ++i)
237
1/2
✓ Branch 1 taken 6500 times.
✗ Branch 2 not taken.
 6500 computeOrder(i, order, computed);
238
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 6461 times.
 6461 assert(order == data_.size());
239 6461 return data_.size() - 1;
240 6464 }
241
242 ✗ bool ExplicitConstraintSet::contains(
243 const ExplicitPtr_t& numericalConstraint) const {
244 ✗ for (std::vector<Data>::const_iterator it = data_.begin(); it != data_.end();
245 ✗ ++it) {
246 ✗ if ((it->constraint == numericalConstraint) ||
247 ✗ (*(it->constraint) == *numericalConstraint) ||
248 ✗ (*(it->constraint->functionPtr()) ==
249 ✗ *numericalConstraint->functionPtr()))
250 ✗ return true;
251 }
252 ✗ return false;
253 }
254
255 7316 void ExplicitConstraintSet::solveExplicitConstraint(const std::size_t& iF,
256 vectorOut_t arg) const {
257 7316 const Data& d = data_[iF];
258 // Compute this function
259
2/4
✓ Branch 4 taken 7316 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7316 times.
✗ Branch 8 not taken.
 7316 d.qin = RowBlockIndices(d.constraint->inputConf()).rview(arg);
260
3/6
✓ Branch 3 taken 7316 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 7316 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 7316 times.
✗ Branch 10 not taken.
 7316 d.constraint->outputValue(d.res_qout, d.qin, d.rhs_implicit);
261
2/4
✓ Branch 5 taken 7316 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7316 times.
✗ Branch 9 not taken.
 7316 RowBlockIndices(d.constraint->outputConf()).lview(arg) = d.res_qout.vector();
262
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 7316 times.
 7316 assert(!arg.hasNaN());
263 7316 }
264
265 189 void ExplicitConstraintSet::jacobian(matrixOut_t jacobian,
266 vectorIn_t arg) const {
267 // TODO this could be done only on the complement of inDers_
268 189 jacobian.setZero();
269
2/4
✓ Branch 2 taken 189 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 189 times.
✗ Branch 6 not taken.
 189 MatrixBlocksRef(notOutDers_, notOutDers_).lview(jacobian).setIdentity();
270 // Compute the function jacobians
271
2/2
✓ Branch 1 taken 265 times.
✓ Branch 2 taken 189 times.
 454 for (std::size_t i = 0; i < data_.size(); ++i) {
272 265 const Data& d = data_[i];
273
2/4
✓ Branch 4 taken 265 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 265 times.
✗ Branch 8 not taken.
 265 d.qin = RowBlockIndices(d.constraint->inputConf()).rview(arg);
274 // Compute Jacobian of f(qin) + rhs
275 // with respect to qin.
276
4/8
✓ Branch 3 taken 265 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 265 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 265 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 265 times.
✗ Branch 13 not taken.
 265 d.constraint->jacobianOutputValue(d.qin, d.f_value, d.rhs_implicit,
277 d.jacobian);
278 }
279
2/2
✓ Branch 1 taken 265 times.
✓ Branch 2 taken 189 times.
 454 for (std::size_t i = 0; i < data_.size(); ++i) {
280
1/2
✓ Branch 3 taken 265 times.
✗ Branch 4 not taken.
 265 computeJacobian(computationOrder_[i], jacobian);
281 }
282 189 }
283
284 265 void ExplicitConstraintSet::computeJacobian(const std::size_t& iE,
285 matrixOut_t J) const {
286 265 const Data& d = data_[iE];
287
1/2
✓ Branch 3 taken 265 times.
✗ Branch 4 not taken.
 265 ColBlockIndices inDer(d.constraint->inputVelocity());
288
3/6
✓ Branch 1 taken 265 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 265 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 265 times.
✗ Branch 8 not taken.
 265 matrix_t Jin(MatrixBlocksRef(inDer, inDers_).rview(J));
289 // Jout = d.jacobian * Jin
290
2/4
✓ Branch 3 taken 265 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 265 times.
✗ Branch 7 not taken.
 265 MatrixBlocksRef(RowBlockIndices(d.constraint->outputVelocity()), inDers_)
291
3/6
✓ Branch 1 taken 265 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 265 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 265 times.
✗ Branch 8 not taken.
 530 .lview(J) = d.jacobian * Jin;
292 265 }
293
294 6555 void ExplicitConstraintSet::computeOrder(const std::size_t& iE,
295 std::size_t& iOrder,
296 Computed_t& computed) {
297
2/2
✓ Branch 2 taken 55 times.
✓ Branch 3 taken 6500 times.
 6555 if (computed[iE]) return;
298 6500 const Data& d = data_[iE];
299
2/2
✓ Branch 3 taken 6483 times.
✓ Branch 4 taken 6500 times.
 12983 for (std::size_t i = 0; i < d.constraint->inputVelocity().size(); ++i) {
300 6483 const BlockIndex::segment_t& segment(d.constraint->inputVelocity()[i]);
301
2/2
✓ Branch 0 taken 38575 times.
✓ Branch 1 taken 6483 times.
 45058 for (size_type j = 0; j < segment.second; ++j) {
302
2/2
✓ Branch 1 taken 38520 times.
✓ Branch 2 taken 55 times.
 38575 if (derFunction_[segment.first + j] < 0) {
303 38520 inOutDependencies_(iE, segment.first + j) += 1;
304 } else {
305
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 55 times.
 55 assert((std::size_t)derFunction_[segment.first + j] < data_.size());
306
1/2
✓ Branch 2 taken 55 times.
✗ Branch 3 not taken.
 55 computeOrder(derFunction_[segment.first + j], iOrder, computed);
307
1/2
✓ Branch 1 taken 55 times.
✗ Branch 2 not taken.
 55 inOutDependencies_.row(iE) +=
308
1/2
✓ Branch 3 taken 55 times.
✗ Branch 4 not taken.
 110 inOutDependencies_.row(derFunction_[segment.first + j]);
309 }
310 }
311 }
312 6500 computationOrder_[iOrder] = iE;
313 6500 ++iOrder;
314 6500 computed[iE] = true;
315 }
316
317 7904 vector_t ExplicitConstraintSet::rightHandSideFromInput(vectorIn_t arg) {
318 // Compute right hand side of explicit formulation
319
2/2
✓ Branch 1 taken 6804 times.
✓ Branch 2 taken 7904 times.
 14708 for (std::size_t i = 0; i < data_.size(); ++i) {
320
1/2
✓ Branch 2 taken 6804 times.
✗ Branch 3 not taken.
 6804 rightHandSideFromInput(i, arg);
321 }
322 7904 return rightHandSide();
323 }
324
325 2002 bool ExplicitConstraintSet::rightHandSideFromInput(
326 const ExplicitPtr_t& constraint, vectorIn_t arg) {
327
1/2
✓ Branch 1 taken 2003 times.
✗ Branch 2 not taken.
 2003 for (std::size_t i = 0; i < data_.size(); ++i) {
328 2003 Data& d = data_[i];
329
6/6
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 2000 times.
✓ Branch 6 taken 1 times.
✓ Branch 7 taken 2 times.
✓ Branch 8 taken 2002 times.
✓ Branch 9 taken 1 times.
 2004 if ((d.constraint == constraint) || (*d.constraint == *constraint) ||
330
1/2
✗ Branch 5 not taken.
✓ Branch 6 taken 1 times.
 1 d.constraint->function() == constraint->function()) {
331
1/2
✓ Branch 2 taken 2002 times.
✗ Branch 3 not taken.
 2002 rightHandSideFromInput(i, arg);
332 2002 return true;
333 }
334 }
335 ✗ return false;
336 }
337
338 8806 void ExplicitConstraintSet::rightHandSideFromInput(const size_type& i,
339 vectorIn_t arg) {
340 8806 Data& d = data_[i];
341 // compute right hand side of implicit formulation that might be
342 // different (RelativePose)
343
3/6
✓ Branch 3 taken 8806 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 8806 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 8806 times.
✗ Branch 10 not taken.
 8806 d.constraint->function().value(d.h_value, arg);
344
1/2
✓ Branch 1 taken 8806 times.
✗ Branch 2 not taken.
 8806 vector_t logRhs(log(d.h_value));
345 // Equality indices apply on the log of the right hand side
346 // This is necessary for constraints built with
347 // RelativeTransformationR3xSO3.
348
2/4
✓ Branch 4 taken 8806 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8806 times.
✗ Branch 8 not taken.
 8806 vector_t logRhsImplicit(vector_t::Zero(d.rhs_implicit.space()->nv()));
349
3/6
✓ Branch 1 taken 8806 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8806 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8806 times.
✗ Branch 8 not taken.
 8806 d.equalityIndices.lview(logRhsImplicit) = d.equalityIndices.rview(logRhs);
350
2/4
✓ Branch 3 taken 8806 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 8806 times.
✗ Branch 7 not taken.
 8806 d.rhs_implicit = d.rhs_implicit.space()->exp(logRhsImplicit);
351 8806 }
352
353 100 void ExplicitConstraintSet::rightHandSide(vectorIn_t rhs) {
354 100 size_type row = 0;
355
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 100 times.
 100 for (std::size_t i = 0; i < data_.size(); ++i) {
356 ✗ Data& d = data_[i];
357 // comparison types are applied to the log of the right hand side.
358 // Initialize a zero vector of velocity size.
359 ✗ vector_t logRhsImplicit(vector_t::Zero(d.rhs_implicit.space()->nv()));
360 // Build liegroupElement from value extracted from input rhs
361 LiegroupElementConstRef inputRhs(
362 ✗ rhs.segment(row, d.rhs_implicit.space()->nq()), d.rhs_implicit.space());
363 ✗ d.equalityIndices.lview(logRhsImplicit) =
364 ✗ d.equalityIndices.rview(log(inputRhs));
365 ✗ ComparisonTypes_t ct(d.constraint->comparisonType());
366 ✗ for (std::size_t i = 0; i < ct.size(); ++i) {
367 ✗ assert(ct[i] == Equality || logRhsImplicit[i] == 0);
368 }
369 ✗ d.rhs_implicit = d.rhs_implicit.space()->exp(logRhsImplicit);
370 ✗ row += d.rhs_implicit.space()->nq();
371 }
372
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 100 times.
 100 assert(row == rhs.size());
373 100 }
374
375 1004 bool ExplicitConstraintSet::rightHandSide(const ExplicitPtr_t& constraint,
376 vectorIn_t rhs) {
377
1/2
✓ Branch 1 taken 1006 times.
✗ Branch 2 not taken.
 1006 for (std::size_t i = 0; i < data_.size(); ++i) {
378 1006 Data& d = data_[i];
379
5/6
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 1004 times.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 1004 times.
✓ Branch 9 taken 2 times.
 1008 if ((d.constraint == constraint) || (*d.constraint == *constraint) ||
380
1/2
✗ Branch 5 not taken.
✓ Branch 6 taken 2 times.
 2 d.constraint->function() == constraint->function()) {
381
1/2
✓ Branch 2 taken 1004 times.
✗ Branch 3 not taken.
 1004 rightHandSide(i, rhs);
382 1004 return true;
383 }
384 }
385 ✗ return false;
386 }
387
388 2002 bool ExplicitConstraintSet::getRightHandSide(const ExplicitPtr_t& constraint,
389 vectorOut_t rhs) const {
390
1/2
✓ Branch 1 taken 2003 times.
✗ Branch 2 not taken.
 2003 for (std::size_t i = 0; i < data_.size(); ++i) {
391 2003 const Data& d = data_[i];
392
5/6
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 2002 times.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 2002 times.
✓ Branch 9 taken 1 times.
 2004 if ((d.constraint == constraint) || (*d.constraint == *constraint) ||
393
1/2
✗ Branch 5 not taken.
✓ Branch 6 taken 1 times.
 1 d.constraint->function() == constraint->function()) {
394 2002 rhs = d.rhs_implicit.vector();
395 2002 return true;
396 }
397 }
398 ✗ return false;
399 }
400
401 1004 void ExplicitConstraintSet::rightHandSide(const size_type& i, vectorIn_t rhs) {
402
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1004 times.
 1004 assert(i < (size_type)data_.size());
403 1004 Data& d = data_[i];
404
1/2
✓ Branch 2 taken 1004 times.
✗ Branch 3 not taken.
 1004 LiegroupElementConstRef rhs_lge(rhs, d.rhs_implicit.space());
405
2/4
✓ Branch 4 taken 1004 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1004 times.
✗ Branch 8 not taken.
 1004 vector_t logRhsImplicit(vector_t::Zero(d.rhs_implicit.space()->nv()));
406
1/2
✓ Branch 1 taken 1004 times.
✗ Branch 2 not taken.
 1004 vector_t logRhsInput(log(rhs_lge));
407
1/2
✓ Branch 1 taken 1004 times.
✗ Branch 2 not taken.
 1004 d.equalityIndices.lview(logRhsImplicit) =
408
2/4
✓ Branch 1 taken 1004 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1004 times.
✗ Branch 5 not taken.
 2008 d.equalityIndices.rview(logRhsInput);
409
2/4
✓ Branch 3 taken 1004 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1004 times.
✗ Branch 7 not taken.
 1004 d.rhs_implicit = d.rhs_implicit.space()->exp(logRhsImplicit);
410
2/4
✓ Branch 2 taken 1004 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1004 times.
✗ Branch 6 not taken.
 1004 ComparisonTypes_t ct(d.constraint->comparisonType());
411
2/2
✓ Branch 1 taken 6004 times.
✓ Branch 2 taken 1004 times.
 7008 for (std::size_t i = 0; i < ct.size(); ++i) {
412
5/8
✓ Branch 1 taken 2001 times.
✓ Branch 2 taken 4003 times.
✓ Branch 4 taken 2001 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2001 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✓ Branch 10 taken 2001 times.
 6004 assert(ct[i] == Equality ||
413 logRhsInput[i] * logRhsInput[i] < errorThreshold_ * errorThreshold_);
414 }
415 1004 }
416
417 8304 vector_t ExplicitConstraintSet::rightHandSide() const {
418
1/2
✓ Branch 2 taken 8304 times.
✗ Branch 3 not taken.
 8304 vector_t rhs(rightHandSideSize());
419 8304 size_type row = 0;
420
2/2
✓ Branch 1 taken 6804 times.
✓ Branch 2 taken 8304 times.
 15108 for (std::size_t i = 0; i < data_.size(); ++i) {
421 6804 const Data& d = data_[i];
422 6804 const size_type nRows = d.rhs_implicit.space()->nq();
423
1/2
✓ Branch 1 taken 6804 times.
✗ Branch 2 not taken.
 6804 vector_t::SegmentReturnType seg = rhs.segment(row, nRows);
424
2/4
✓ Branch 1 taken 6804 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6804 times.
✗ Branch 5 not taken.
 6804 seg = d.rhs_implicit.vector();
425 6804 row += nRows;
426 }
427
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 8304 times.
 8304 assert(row == rhs.size());
428 8304 return rhs;
429 }
430
431 10309 size_type ExplicitConstraintSet::rightHandSideSize() const {
432 10309 size_type rhsSize = 0;
433
2/2
✓ Branch 1 taken 7208 times.
✓ Branch 2 taken 10309 times.
 17517 for (std::size_t i = 0; i < data_.size(); ++i)
434 7208 rhsSize += data_[i].rhs_implicit.space()->nq();
435 10309 return rhsSize;
436 }
437
438 2 std::ostream& ExplicitConstraintSet::print(std::ostream& os) const {
439 2 os << "ExplicitConstraintSet, " << data_.size() << " functions." << incendl
440 2 << "Other args: " << notOutArgs_ << iendl << "Params: " << inArgs_
441 2 << " -> " << outArgs_ << iendl << "Dofs: " << inDers_ << " -> " << outDers_
442 2 << iendl << "Functions" << incindent;
443
2/2
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 2 times.
 4 for (std::size_t i = 0; i < data_.size(); ++i) {
444 2 const Data& d = data_[computationOrder_[i]];
445
1/2
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
 4 os << iendl << i << ": " << RowBlockIndices(d.constraint->inputConf())
446
4/8
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 2 times.
✗ Branch 13 not taken.
 4 << " -> " << RowBlockIndices(d.constraint->outputConf()) << incendl
447
2/4
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
 4 << *d.constraint->explicitFunction() << iendl
448
5/10
✓ 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.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
 2 << "Rhs implicit: " << condensed(d.rhs_implicit.vector()) << iendl
449
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.
 2 << "Equality: " << d.equalityIndices << decindent;
450 }
451 2 return os << decindent << decindent;
452 }
453
454 1050 Eigen::MatrixXi ExplicitConstraintSet::inOutDofDependencies() const {
455
1/2
✓ Branch 3 taken 1050 times.
✗ Branch 4 not taken.
 1050 Eigen::MatrixXi iod(nv(), inDers_.nbCols());
456
2/2
✓ Branch 1 taken 1028 times.
✓ Branch 2 taken 22 times.
 1050 if (inDers_.nbCols() == 0) return iod;
457
1/2
✓ Branch 2 taken 22 times.
✗ Branch 3 not taken.
 22 Eigen::RowVectorXi tmp(inDers_.nbCols());
458
2/2
✓ Branch 1 taken 36 times.
✓ Branch 2 taken 22 times.
 58 for (std::size_t i = 0; i < data_.size(); ++i) {
459 36 const Data& d = data_[i];
460
3/6
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 36 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 36 times.
✗ Branch 8 not taken.
 36 tmp = inDers_.rview(inOutDependencies_.row(i));
461
4/8
✓ Branch 3 taken 36 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 36 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 36 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 36 times.
✗ Branch 13 not taken.
 36 RowBlockIndices(d.constraint->outputVelocity()).lview(iod).rowwise() = tmp;
462 }
463
2/4
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 22 times.
✗ Branch 5 not taken.
 22 return outDers_.rview(iod);
464 1050 }
465 } // namespace constraints
466 } // namespace hpp
467