GCC Code Coverage Report

Directory: ./
File: tests/explicit-constraint-set.cc
Date: 2025-05-05 12:19:30
Exec Total Coverage
Lines: 347 375 92.5%
Branches: 1047 2162 48.4%
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 #define BOOST_TEST_MODULE EXPLICIT_CONSTRAINT_SET
30 #include <../tests/util.hh>
31 #include <boost/test/unit_test.hpp>
32 #include <hpp/constraints/affine-function.hh>
33 #include <hpp/constraints/explicit-constraint-set.hh>
34 #include <hpp/constraints/explicit.hh>
35 #include <hpp/constraints/explicit/relative-pose.hh>
36 #include <hpp/constraints/generic-transformation.hh>
37 #include <hpp/constraints/locked-joint.hh>
38 #include <hpp/constraints/symbolic-calculus.hh>
39 #include <hpp/pinocchio/configuration.hh>
40 #include <hpp/pinocchio/device.hh>
41 #include <hpp/pinocchio/joint.hh>
42 #include <hpp/pinocchio/liegroup-element.hh>
43 #include <hpp/pinocchio/liegroup.hh>
44 #include <hpp/pinocchio/simple-device.hh>
45 #include <hpp/pinocchio/urdf/util.hh>
46 #include <pinocchio/algorithm/joint-configuration.hpp>
47
48 using Eigen::BlockIndex;
49 using Eigen::ColBlockIndices;
50 using Eigen::RowBlockIndices;
51
52 using hpp::constraints::AffineFunction;
53 using hpp::constraints::AffineFunctionPtr_t;
54 using hpp::constraints::ComparisonTypes_t;
55 using hpp::constraints::Configuration_t;
56 using hpp::constraints::Device;
57 using hpp::constraints::DevicePtr_t;
58 using hpp::constraints::DifferentiableFunction;
59 using hpp::constraints::Equality;
60 using hpp::constraints::EqualToZero;
61 using hpp::constraints::Explicit;
62 using hpp::constraints::ExplicitConstraintSet;
63 using hpp::constraints::ExplicitPtr_t;
64 using hpp::constraints::JointPtr_t;
65 using hpp::constraints::LiegroupElement;
66 using hpp::constraints::LiegroupElementRef;
67 using hpp::constraints::LiegroupSpace;
68 using hpp::constraints::LiegroupSpacePtr_t;
69 using hpp::constraints::LockedJoint;
70 using hpp::constraints::LockedJointPtr_t;
71 using hpp::constraints::matrix3_t;
72 using hpp::constraints::matrix_t;
73 using hpp::constraints::matrixOut_t;
74 using hpp::constraints::RelativeTransformation;
75 using hpp::constraints::RelativeTransformationPtr_t;
76 using hpp::constraints::segment_t;
77 using hpp::constraints::segments_t;
78 using hpp::constraints::size_type;
79 using hpp::constraints::Transform3s;
80 using hpp::constraints::value_type;
81 using hpp::constraints::vector3_t;
82 using hpp::constraints::vector_t;
83 using hpp::constraints::vectorIn_t;
84 using hpp::pinocchio::unittest::HumanoidSimple;
85 using hpp::pinocchio::unittest::makeDevice;
86
87 namespace Eigen {
88 namespace internal {
89 ✗ bool operator==(const empty_struct&, const empty_struct&) { return true; }
90 } // namespace internal
91
92 template <bool _allRows, bool _allCols>
93 12 bool operator==(const MatrixBlocks<_allRows, _allCols>& a,
94 const MatrixBlocks<_allRows, _allCols>& b) {
95 8 return (_allRows || a.nbRows() == b.nbRows()) &&
96 12 (_allCols || a.nbCols() == b.nbCols()) &&
97
2/4
✓ Branch 0 taken 6 times.
✗ Branch 1 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
 16 (_allRows || a.rows() == b.rows()) &&
98
1/2
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
 16 (_allCols || a.cols() == b.cols());
99 }
100 } // namespace Eigen
101
102 // Explicit expression of variables
103 // - [idxOut:idxOut+length] with respect to
104 // - [idxIn:idxIn+length]
105 class TestFunction : public AffineFunction {
106 public:
107 size_type idxIn_, idxOut_, length_;
108
109 2 TestFunction(size_type idxIn, size_type idxOut, size_type length)
110
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 : AffineFunction(matrix_t::Identity(length, length), "TestFunction"),
111 2 idxIn_(idxIn),
112 2 idxOut_(idxOut),
113
3/6
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2 times.
✗ Branch 9 not taken.
 4 length_(length) {}
114
115 // return configuration input interval
116 5 ExplicitConstraintSet::RowBlockIndices inArg() const {
117 5 ExplicitConstraintSet::RowBlockIndices ret;
118
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 ret.addRow(idxIn_, length_);
119 5 return ret;
120 }
121
122 // return configuration output interval
123 5 ExplicitConstraintSet::RowBlockIndices outArg() const {
124 5 ExplicitConstraintSet::RowBlockIndices ret;
125
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 ret.addRow(idxOut_, length_);
126 5 return ret;
127 }
128
129 // return velocity input interval
130 5 ExplicitConstraintSet::ColBlockIndices inDer() const {
131 5 ExplicitConstraintSet::ColBlockIndices ret;
132 ✗ ret.addCol(idxIn_ - 1,
133
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 length_); // TODO this assumes there is only the freeflyer
134 5 return ret;
135 }
136
137 // return velocity output interval
138 5 ExplicitConstraintSet::RowBlockIndices outDer() const {
139 5 ExplicitConstraintSet::RowBlockIndices ret;
140 ✗ ret.addRow(idxOut_ - 1,
141
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 length_); // TODO this assumes there is only the freeflyer
142 5 return ret;
143 }
144 }; // class TestFunction
145
146 ✗ matrix3_t exponential(const vector3_t& aa) {
147 ✗ matrix3_t R, xCross;
148 ✗ xCross.setZero();
149 ✗ xCross(1, 0) = +aa(2);
150 ✗ xCross(0, 1) = -aa(2);
151 ✗ xCross(2, 0) = -aa(1);
152 ✗ xCross(0, 2) = +aa(1);
153 ✗ xCross(2, 1) = +aa(0);
154 ✗ xCross(1, 2) = -aa(0);
155 ✗ R.setIdentity();
156 ✗ value_type theta = aa.norm();
157 ✗ if (theta < 1e-6) {
158 ✗ R += xCross;
159 ✗ R += 0.5 * xCross.transpose() * xCross;
160 } else {
161 ✗ R += sin(theta) / theta * xCross;
162 ✗ R += 2 * std::pow(sin(theta / 2), 2) / std::pow(theta, 2) * xCross * xCross;
163 }
164 ✗ return R;
165 }
166
167 class ExplicitTransformation : public DifferentiableFunction {
168 public:
169 JointPtr_t joint_;
170 size_type in_, inDer_;
171 RelativeTransformationPtr_t rt_;
172
173 1 ExplicitTransformation(JointPtr_t joint, size_type in, size_type l,
174 size_type inDer, size_type lDer)
175 1 : DifferentiableFunction(l, lDer,
176
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 LiegroupSpacePtr_t(LiegroupSpace::R3xSO3()),
177 "ExplicitTransformation"),
178 1 joint_(joint),
179 1 in_(in),
180
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 3 inDer_(inDer) {
181
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
 4 rt_ = RelativeTransformation::create("RT", joint_->robot(),
182
1/2
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 joint_->robot()->rootJoint(), joint_,
183
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 3 Transform3s::Identity());
184 1 }
185
186 1 ExplicitConstraintSet::RowBlockIndices inArg() const {
187 1 ExplicitConstraintSet::RowBlockIndices ret;
188
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 ret.addRow(in_, inputSize());
189 1 return ret;
190 }
191
192 1 ExplicitConstraintSet::RowBlockIndices outArg() const {
193 1 ExplicitConstraintSet::RowBlockIndices ret;
194
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 ret.addRow(0, 7);
195 1 return ret;
196 }
197
198 2 ExplicitConstraintSet::ColBlockIndices inDer() const {
199 2 ExplicitConstraintSet::ColBlockIndices ret;
200
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 ret.addCol(inDer_, inputDerivativeSize());
201 2 return ret;
202 }
203
204 1 ExplicitConstraintSet::RowBlockIndices outDer() const {
205 1 ExplicitConstraintSet::RowBlockIndices ret;
206
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 ret.addRow(0, 6);
207 1 return ret;
208 }
209
210 1 vector_t config(vectorIn_t arg) const {
211
1/2
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 vector_t q = joint_->robot()->neutralConfiguration();
212
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 q.segment(in_, inputSize()) = arg;
213 1 return q;
214 // joint_->robot()->currentConfiguration(q);
215 // joint_->robot()->computeForwardKinematics();
216 }
217
218 ✗ void impl_compute(LiegroupElementRef result, vectorIn_t arg) const {
219 // forwardKinematics(arg);
220 ✗ LiegroupElement transform(LiegroupSpace::Rn(6));
221 ✗ vector_t q = config(arg);
222 ✗ rt_->value(transform, q);
223 ✗ result.vector().head<3>() = transform.vector().head<3>();
224 ✗ result.vector().tail<4>() =
225 ✗ Eigen::Quaternion<value_type>(exponential(transform.vector().tail<3>()))
226 ✗ .coeffs();
227 }
228
229 1 void impl_jacobian(matrixOut_t jacobian, vectorIn_t arg) const {
230 // forwardKinematics(arg);
231
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 matrix_t J(6, rt_->inputDerivativeSize());
232
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 vector_t q = config(arg);
233
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 rt_->jacobian(J, q);
234
235
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 inDer().rview(J).writeTo(jacobian);
236 1 }
237 };
238
239 typedef hpp::shared_ptr<TestFunction> TestFunctionPtr_t;
240 typedef hpp::shared_ptr<ExplicitTransformation> ExplicitTransformationPtr_t;
241
242 template <int N>
243 6 void order_test(const AffineFunctionPtr_t f[N], const segment_t s[N + 1],
244 const std::vector<int> forder, const segments_t& inArgs,
245 const segments_t& outArgs) {
246
2/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
 6 ExplicitConstraintSet expression(LiegroupSpace::Rn(4));
247
2/2
✓ Branch 1 taken 18 times.
✓ Branch 2 taken 6 times.
 24 for (int i = 0; i < N; ++i) {
248 18 int fo = forder[i], si = forder[i], so = forder[i] + 1;
249
1/2
✓ Branch 3 taken 18 times.
✗ Branch 4 not taken.
 72 ExplicitPtr_t constraint(Explicit::create(
250
3/6
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 18 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 18 times.
✗ Branch 11 not taken.
 54 LiegroupSpace::Rn(4), f[fo], segments_t(1, s[si]), segments_t(1, s[so]),
251
2/4
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 18 times.
✗ Branch 7 not taken.
 36 segments_t(1, s[si]), segments_t(1, s[so])));
252
7/14
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 18 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 18 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 18 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 18 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 18 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 18 times.
 18 BOOST_CHECK(expression.add(constraint) >= 0);
253 }
254
5/10
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✓ Branch 12 taken 6 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 6 times.
✗ Branch 16 not taken.
✗ Branch 19 not taken.
✓ Branch 20 taken 6 times.
 6 BOOST_CHECK_EQUAL(expression.inArgs().rows(), inArgs);
255
5/10
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✓ Branch 12 taken 6 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 6 times.
✗ Branch 16 not taken.
✗ Branch 19 not taken.
✓ Branch 20 taken 6 times.
 6 BOOST_CHECK_EQUAL(expression.outArgs().rows(), outArgs);
256 6 }
257
258
33/66
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
 4 BOOST_AUTO_TEST_CASE(order) {
259
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Eigen::Matrix<value_type, 1, 1> M;
260
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 M(0, 0) = 1;
261
262 // dof : 0 -> 1 -> 2 -> 3
263 // function: f0 f1 f2
264 AffineFunctionPtr_t f[] = {AffineFunction::create(M),
265 AffineFunction::create(M),
266
9/18
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 28 taken 1 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 1 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 1 times.
✗ Branch 35 not taken.
 10 AffineFunction::create(M)};
267 2 segment_t s[] = {segment_t(0, 1), segment_t(1, 1), segment_t(2, 1),
268 segment_t(3, 1)};
269
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 4 segments_t inArgs{s[0]}, outArgs{s[1], s[2], s[3]};
270
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 BlockIndex::shrink(outArgs);
271
272
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 std::vector<int> order(3);
273
274
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 order = {0, 1, 2};
275
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 order_test<3>(f, s, order, inArgs, outArgs);
276
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 order = {0, 2, 1};
277
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 order_test<3>(f, s, order, inArgs, outArgs);
278
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 order = {1, 0, 2};
279
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 order_test<3>(f, s, order, inArgs, outArgs);
280
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 order = {1, 2, 0};
281
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 order_test<3>(f, s, order, inArgs, outArgs);
282
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 order = {2, 0, 1};
283
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 order_test<3>(f, s, order, inArgs, outArgs);
284
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 order = {2, 1, 0};
285
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 order_test<3>(f, s, order, inArgs, outArgs);
286
2/4
✓ Branch 3 taken 3 times.
✓ Branch 4 taken 1 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
 12 }
287
288
33/66
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
 4 BOOST_AUTO_TEST_CASE(jacobian1) {
289
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 4 matrix_t J[] = {(matrix_t(1, 1) << 1).finished(),
290
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 4 (matrix_t(1, 1) << 2).finished(),
291
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
 18 (matrix_t(1, 1) << 3).finished()};
292
293 // dof : 0 -> 1 -> 2 -> 3
294 // function: f0 f1 f2
295 AffineFunctionPtr_t f[] = {AffineFunction::create(J[0]),
296 AffineFunction::create(J[1]),
297
9/18
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 28 taken 1 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 1 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 1 times.
✗ Branch 35 not taken.
 10 AffineFunction::create(J[2])};
298 segments_t s[] = {
299 4 segments_t(1, segment_t(0, 1)), segments_t(1, segment_t(1, 1)),
300
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
 16 segments_t(1, segment_t(2, 1)), segments_t(1, segment_t(3, 1))};
301
302
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 ExplicitConstraintSet expression(LiegroupSpace::Rn(4));
303
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 1 times.
 8 for (int i = 0; i < 3; ++i) {
304
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 12 ExplicitPtr_t constraint(Explicit::create(LiegroupSpace::Rn(4), f[i], s[i],
305
1/2
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 18 s[i + 1], s[i], s[i + 1]));
306
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 6 expression.add(constraint);
307 6 }
308
309
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 vector_t x(4);
310
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 2 x << 1, 2, 3, 4;
311
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 vector_t xres = x;
312
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 1 times.
 2 BOOST_CHECK(expression.solve(xres));
313
314 // Check the solution
315
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 1 times.
 2 BOOST_CHECK_EQUAL(xres[0], x[0]);
316
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 1 times.
 8 for (int i = 0; i < 3; ++i)
317
9/18
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 3 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 3 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 3 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 3 times.
✗ Branch 20 not taken.
✓ Branch 24 taken 3 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 3 times.
✗ Branch 28 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 3 times.
 6 BOOST_CHECK_EQUAL(xres.segment<1>(i + 1),
318 (*f[i])(xres.segment<1>(i)).vector());
319
320 // Check the jacobian
321 // It should be ( J[0], J[1] * J[0], J[2] * J[1] * J[0])
322
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 2 matrix_t expjac(matrix_t::Zero(expression.nv(), expression.nv()));
323
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
 2 expjac.col(0) << 1, J[0], J[1] * J[0], J[2] * J[1] * J[0];
324
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 matrix_t jacobian(expression.nv(), expression.nv());
325
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 2 expression.jacobian(jacobian, xres);
326
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✓ Branch 18 taken 1 times.
 2 BOOST_CHECK_EQUAL(jacobian, expjac);
327
6/12
✓ Branch 5 taken 4 times.
✓ Branch 6 taken 1 times.
✓ Branch 7 taken 3 times.
✓ Branch 8 taken 1 times.
✓ Branch 9 taken 3 times.
✓ Branch 10 taken 1 times.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
 30 }
328
329
33/66
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
 4 BOOST_AUTO_TEST_CASE(jacobian2) {
330
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 4 matrix_t J[] = {(matrix_t(1, 2) << 3.2, -0.3).finished(),
331
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 4 (matrix_t(1, 1) << 4.1).finished(),
332
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
 18 (matrix_t(1, 2) << -0.3, 1.2).finished()};
333
334 /* dof : 1,2 -> 0 \
335 * function: f0 --> 4
336 * 1 -> 3 / f2
337 * f1
338 */
339 AffineFunctionPtr_t f[] = {AffineFunction::create(J[0]),
340 AffineFunction::create(J[1]),
341
9/18
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 28 taken 1 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 1 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 1 times.
✗ Branch 35 not taken.
 10 AffineFunction::create(J[2])};
342
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 std::vector<segments_t> s(6);
343
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 s[0] = {segment_t(1, 2)};
344
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 s[1] = {segment_t(0, 1)};
345
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 s[2] = {segment_t(3, 1)};
346
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 s[3] = {segment_t(4, 1)};
347
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 s[4] = {segment_t(0, 1), segment_t(3, 1)};
348
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 s[5] = {segment_t(1, 1)};
349
350
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 ExplicitConstraintSet expression(LiegroupSpace::Rn(5));
351 2 ExplicitPtr_t constraint;
352 constraint =
353
2/4
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 2 Explicit::create(LiegroupSpace::Rn(5), f[0], s[0], s[1], s[0], s[1]);
354
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 expression.add(constraint);
355 constraint =
356
2/4
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 2 Explicit::create(LiegroupSpace::Rn(5), f[2], s[4], s[3], s[4], s[3]);
357
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 expression.add(constraint);
358 constraint =
359
2/4
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 2 Explicit::create(LiegroupSpace::Rn(5), f[1], s[5], s[2], s[5], s[2]);
360
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 expression.add(constraint);
361
362
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Eigen::MatrixXi inOutDependencies(3, 5);
363
15/30
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 1 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 1 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 1 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 1 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 1 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 1 times.
✗ Branch 41 not taken.
✓ Branch 43 taken 1 times.
✗ Branch 44 not taken.
 2 inOutDependencies << 0, 1, 1, 0, 0, 0, 2, 1, 0, 0, 0, 1, 0, 0, 0;
364
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✗ Branch 18 not taken.
✓ Branch 19 taken 1 times.
 2 BOOST_CHECK_EQUAL(expression.inOutDependencies(), inOutDependencies);
365
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 inOutDependencies.resize(3, 2);
366
6/12
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
 2 inOutDependencies << 1, 1, 1, 0, 2, 1;
367
6/12
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✗ Branch 21 not taken.
✓ Branch 22 taken 1 times.
 2 BOOST_CHECK_EQUAL(expression.inOutDofDependencies(), inOutDependencies);
368
369
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
 2 segments_t inArgs = s[0], outArgs = {s[1][0], s[2][0], s[3][0]};
370
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 BlockIndex::shrink(outArgs);
371
372
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✗ Branch 19 not taken.
✓ Branch 20 taken 1 times.
 2 BOOST_CHECK_EQUAL(expression.inArgs().rows(), inArgs);
373
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✗ Branch 19 not taken.
✓ Branch 20 taken 1 times.
 2 BOOST_CHECK_EQUAL(expression.outArgs().rows(), outArgs);
374
375
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 vector_t x(5);
376
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
 2 x << 1, 2, 3, 4, 5;
377
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 vector_t xres = x;
378
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 1 times.
 2 BOOST_CHECK(expression.solve(xres));
379
380 // Check the solution
381
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 1 times.
 2 BOOST_CHECK_EQUAL(xres.segment<2>(1), x.segment<2>(1));
382
9/18
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 1 times.
 2 BOOST_CHECK_EQUAL(xres.segment<1>(0), (*f[0])(xres.segment<2>(1)).vector());
383
9/18
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 1 times.
 2 BOOST_CHECK_EQUAL(xres.segment<1>(3), (*f[1])(xres.segment<1>(1)).vector());
384
11/22
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 26 taken 1 times.
✗ Branch 27 not taken.
✓ Branch 31 taken 1 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 1 times.
✗ Branch 35 not taken.
✗ Branch 42 not taken.
✓ Branch 43 taken 1 times.
 2 BOOST_CHECK_EQUAL(xres.segment<1>(4),
385 (*f[2])(RowBlockIndices(s[4]).rview(xres).eval()).vector());
386
387 // Check the jacobian
388 // It should be ( J[0], J[1] * J[0], J[2] * J[1] * J[0])
389
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 2 matrix_t expjac(matrix_t::Zero(expression.nv(), expression.nv()));
390
12/24
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 1 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 1 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 1 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 1 times.
✗ Branch 35 not taken.
 2 expjac.block<5, 2>(0, 1) << J[0](0, 0), J[0](0, 1), 1, 0, 0, 1, J[1](0, 0), 0,
391
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
 2 J[2](0, 0) * J[0](0, 0) + J[2](0, 1) * J[1](0, 0),
392
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 2 J[2](0, 0) * J[0](0, 1);
393
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 matrix_t jacobian(expression.nv(), expression.nv());
394
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 2 expression.jacobian(jacobian, xres);
395
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✓ Branch 18 taken 1 times.
 2 BOOST_CHECK_EQUAL(jacobian, expjac);
396
397
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 matrix_t smallJ = expression.jacobianNotOutToOut(jacobian);
398
10/20
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✗ Branch 36 not taken.
✓ Branch 37 taken 1 times.
 2 BOOST_CHECK_EQUAL(expression.outArgs().rview(xres).eval(),
399 smallJ * expression.inArgs().rview(xres).eval());
400
4/8
✓ Branch 11 taken 3 times.
✓ Branch 12 taken 1 times.
✓ Branch 13 taken 3 times.
✓ Branch 14 taken 1 times.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
 20 }
401
402
33/66
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
 4 BOOST_AUTO_TEST_CASE(locked_joints) {
403
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 4 DevicePtr_t device(makeDevice(HumanoidSimple));
404
405
6/12
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 1 times.
 2 BOOST_REQUIRE(device);
406
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 2 device->rootJoint()->lowerBound(0, -1);
407
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 2 device->rootJoint()->lowerBound(1, -1);
408
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 2 device->rootJoint()->lowerBound(2, -1);
409
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 2 device->rootJoint()->upperBound(0, 1);
410
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 2 device->rootJoint()->upperBound(1, 1);
411
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 2 device->rootJoint()->upperBound(2, 1);
412
413
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 4 JointPtr_t ee1 = device->getJointByName("lleg5_joint"),
414
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 4 ee2 = device->getJointByName("rleg5_joint"),
415
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 4 ee3 = device->getJointByName("rleg4_joint");
416
417 LockedJointPtr_t l1(
418
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
 4 LockedJoint::create(ee1, ee1->configurationSpace()->neutral()));
419 LockedJointPtr_t l2(
420
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
 4 LockedJoint::create(ee2, ee2->configurationSpace()->neutral()));
421 LockedJointPtr_t l3(
422
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
 4 LockedJoint::create(ee3, ee3->configurationSpace()->neutral()));
423
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 TestFunctionPtr_t t1(new TestFunction(ee1->rankInConfiguration(),
424
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 2 ee2->rankInConfiguration(), 1));
425
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 TestFunctionPtr_t t2(new TestFunction(ee2->rankInConfiguration(),
426
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 2 ee1->rankInConfiguration(), 1));
427
428
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 RowBlockIndices expectedRow;
429
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 ColBlockIndices expectedCol;
430
431
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 Configuration_t q = device->currentConfiguration(),
432
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 qrand = ::pinocchio::randomConfiguration(device->model());
433
434 {
435
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 ExplicitConstraintSet expression(device->configSpace());
436 2 ExplicitPtr_t constraint;
437
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 1 times.
 2 BOOST_CHECK(expression.add(l1) >= 0);
438
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 1 times.
 2 BOOST_CHECK(expression.add(l1) < 0);
439
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 1 times.
 2 BOOST_CHECK(expression.add(l2) >= 0);
440
441
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 expectedRow = RowBlockIndices();
442
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 expectedRow.addRow(ee1->rankInConfiguration(), 1);
443
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 expectedRow.addRow(ee2->rankInConfiguration(), 1);
444
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 expectedRow.updateRows<true, true, true>();
445
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✗ Branch 18 not taken.
✓ Branch 19 taken 1 times.
 2 BOOST_CHECK_EQUAL(expression.outArgs(), expectedRow);
446
447
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 4 expectedRow = RowBlockIndices(BlockIndex::difference(
448
1/2
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 6 BlockIndex::segment_t(0, expression.nq()), expectedRow.rows()));
449
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✗ Branch 18 not taken.
✓ Branch 19 taken 1 times.
 2 BOOST_CHECK_EQUAL(expression.notOutArgs(), expectedRow);
450
451
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 expectedRow = RowBlockIndices();
452
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 expectedRow.addRow(ee1->rankInVelocity(), 1);
453
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 expectedRow.addRow(ee2->rankInVelocity(), 1);
454
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 expectedRow.updateRows<true, true, true>();
455
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✗ Branch 18 not taken.
✓ Branch 19 taken 1 times.
 2 BOOST_CHECK_EQUAL(expression.outDers(), expectedRow);
456
457
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 4 expectedCol = ColBlockIndices(BlockIndex::difference(
458
1/2
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 6 BlockIndex::segment_t(0, expression.nv()), expectedRow.rows()));
459
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✗ Branch 18 not taken.
✓ Branch 19 taken 1 times.
 2 BOOST_CHECK_EQUAL(expression.notOutDers(), expectedCol);
460
461
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 expectedRow = RowBlockIndices();
462
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✗ Branch 18 not taken.
✓ Branch 19 taken 1 times.
 2 BOOST_CHECK_EQUAL(expression.inArgs(), expectedRow);
463
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 expectedCol = ColBlockIndices();
464
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✗ Branch 18 not taken.
✓ Branch 19 taken 1 times.
 2 BOOST_CHECK_EQUAL(expression.inDers(), expectedCol);
465
466
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 1 times.
 2 BOOST_CHECK(expression.solve(qrand));
467
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 24 not taken.
✓ Branch 25 taken 1 times.
 2 BOOST_CHECK_EQUAL(qrand[ee1->rankInConfiguration()], 0);
468
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 24 not taken.
✓ Branch 25 taken 1 times.
 2 BOOST_CHECK_EQUAL(qrand[ee2->rankInConfiguration()], 0);
469
470
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 2 expression.rightHandSide(l1, vector_t::Ones(1));
471
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 2 expression.rightHandSide(l2, vector_t::Constant(1, -0.2));
472
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 1 times.
 2 BOOST_CHECK(expression.solve(qrand));
473
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 24 not taken.
✓ Branch 25 taken 1 times.
 2 BOOST_CHECK_EQUAL(qrand[ee1->rankInConfiguration()], 1);
474
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 24 not taken.
✓ Branch 25 taken 1 times.
 2 BOOST_CHECK_EQUAL(qrand[ee2->rankInConfiguration()], -0.2);
475
476
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
 2 matrix_t jacobian(device->numberDof(), device->numberDof());
477
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 2 expression.jacobian(jacobian, q);
478
9/18
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 1 times.
 2 BOOST_CHECK(expression.jacobianNotOutToOut(jacobian).eval().isZero());
479 2 }
480
481 {
482
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 ExplicitConstraintSet expression(device->configSpace());
483
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 1 times.
 2 BOOST_CHECK(expression.add(l1) >= 0);
484 2 ExplicitPtr_t constraint;
485
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 8 constraint = Explicit::create(
486
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 4 device->configSpace(), t1, t1->inArg().indices(),
487
6/12
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
 6 t1->outArg().indices(), t1->inDer().indices(), t1->outDer().indices());
488
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
 2 BOOST_CHECK(expression.add(constraint) >= 0);
489
490
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 1 times.
 2 BOOST_CHECK(expression.solve(qrand));
491
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 vector_t error(expression.outDers().nbIndices());
492
9/18
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1 times.
✗ Branch 27 not taken.
✗ Branch 32 not taken.
✓ Branch 33 taken 1 times.
 2 BOOST_CHECK(expression.isSatisfied(qrand, error));
493
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 24 not taken.
✓ Branch 25 taken 1 times.
 2 BOOST_CHECK_EQUAL(qrand[ee1->rankInConfiguration()], 0);
494
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 24 not taken.
✓ Branch 25 taken 1 times.
 2 BOOST_CHECK_EQUAL(qrand[ee2->rankInConfiguration()], 0);
495
496
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
 2 matrix_t jacobian(device->numberDof(), device->numberDof());
497
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 2 expression.jacobian(jacobian, q);
498
9/18
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 1 times.
 2 BOOST_CHECK(expression.jacobianNotOutToOut(jacobian).eval().isZero());
499 2 }
500
501 {
502
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 ExplicitConstraintSet expression(device->configSpace());
503 2 ExplicitPtr_t constraint;
504
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 8 constraint = Explicit::create(
505
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 4 device->configSpace(), t1, t1->inArg().indices(),
506
6/12
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
 6 t1->outArg().indices(), t1->inDer().indices(), t1->outDer().indices());
507
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
 2 BOOST_CHECK(expression.add(constraint) >= 0);
508
509
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
 2 matrix_t jacobian(device->numberDof(), device->numberDof());
510
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 2 expression.jacobian(jacobian, q);
511
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 1 times.
 2 BOOST_CHECK_EQUAL(jacobian(ee2->rankInVelocity(), ee1->rankInVelocity()),
512 1);
513
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 1 times.
 2 BOOST_CHECK_EQUAL(expression.jacobianNotOutToOut(jacobian).eval().norm(),
514 1);
515 2 }
516
517 {
518
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 ExplicitConstraintSet expression(device->configSpace());
519 2 ExplicitPtr_t constraint;
520
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 8 constraint = Explicit::create(
521
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 4 device->configSpace(), t1, t1->inArg().indices(),
522
6/12
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
 6 t1->outArg().indices(), t1->inDer().indices(), t1->outDer().indices());
523
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
 2 BOOST_CHECK(expression.add(constraint) >= 0);
524
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 8 constraint = Explicit::create(
525
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 4 device->configSpace(), t2, t2->inArg().indices(),
526
6/12
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
 6 t2->outArg().indices(), t2->inDer().indices(), t2->outDer().indices());
527
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
 2 BOOST_CHECK(expression.add(constraint) < 0);
528 2 }
529
530 {
531
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 ExplicitConstraintSet expression(device->configSpace());
532 2 ExplicitPtr_t constraint;
533
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 8 constraint = Explicit::create(
534
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 4 device->configSpace(), t1, t1->inArg().indices(),
535
6/12
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
 6 t1->outArg().indices(), t1->inDer().indices(), t1->outDer().indices());
536
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
 2 BOOST_CHECK(expression.add(constraint) >= 0);
537
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 1 times.
 2 BOOST_CHECK(expression.add(l2) < 0);
538
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 1 times.
 2 BOOST_CHECK(expression.add(l3) >= 0);
539
540
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
 2 matrix_t jacobian(device->numberDof(), device->numberDof());
541
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 2 expression.jacobian(jacobian, q);
542
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 1 times.
 2 BOOST_CHECK_EQUAL(jacobian(ee2->rankInVelocity(), ee1->rankInVelocity()),
543 1);
544
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 1 times.
 2 BOOST_CHECK_EQUAL(expression.jacobianNotOutToOut(jacobian).eval().norm(),
545 1);
546 2 }
547
548 {
549 // Find a joint such that the config parameters for the chain from the root
550 // joint to it are the n first parameters (i.e. q.segment(0, n)).
551 // We take the one which gives the longest block
552
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 JointPtr_t parent = device->rootJoint(),
553
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 current = device->getJointAtConfigRank(7);
554
3/4
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 9 taken 6 times.
✓ Branch 10 taken 1 times.
 14 while (current->parentJoint()->index() == parent->index()) {
555 12 parent = current;
556
1/2
✓ Branch 3 taken 6 times.
✗ Branch 4 not taken.
 24 current = device->getJointAtConfigRank(current->rankInConfiguration() +
557
2/4
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
 12 current->configSize());
558 }
559 // std::cout << parent->name() << std::endl;
560
561 ExplicitTransformationPtr_t et(new ExplicitTransformation(
562
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 parent, 7, 6, parent->rankInConfiguration() + parent->configSize() - 7,
563
5/10
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
 4 parent->rankInVelocity() + parent->numberDof() - 6));
564
565
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 ExplicitConstraintSet expression(device->configSpace());
566 2 ExplicitPtr_t constraint;
567
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 8 constraint = Explicit::create(
568
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 4 device->configSpace(), et, et->inArg().indices(),
569
6/12
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
 6 et->outArg().indices(), et->inDer().indices(), et->outDer().indices());
570
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
 2 BOOST_CHECK(expression.add(constraint) >= 0);
571
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 1 times.
 2 BOOST_CHECK(expression.add(l2) >= 0);
572
573
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
 2 matrix_t jacobian(device->numberDof(), device->numberDof());
574
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 2 expression.jacobian(jacobian, qrand);
575 2 }
576 2 }
577
578
33/66
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
 4 BOOST_AUTO_TEST_CASE(RelativePose) {
579 const std::string urdf(
580 "<robot name=\"two-freeflyers\">\n"
581 " <link name=\"base_link\">\n"
582 " </link>\n"
583
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 "</robot>");
584 // Make robot with two free flyers.
585
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 4 DevicePtr_t device(Device::create("two-freeflyers"));
586
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
 2 hpp::pinocchio::urdf::loadModelFromString(device, 0, "1", "freeflyer", urdf,
587 "");
588
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
 2 hpp::pinocchio::urdf::loadModelFromString(device, 0, "2", "freeflyer", urdf,
589 "");
590
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 1 times.
 2 assert(device->configSize() == 14);
591
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 1 times.
 2 assert(device->numberDof() == 12);
592 // Set joint bounds
593
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 hpp::pinocchio::JointPtr_t joint1(device->jointAt(0));
594
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 hpp::pinocchio::JointPtr_t joint2(device->jointAt(1));
595
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 vector_t low(device->configSize());
596
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 low.fill(-1);
597
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 vector_t up(device->configSize());
598
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 up.fill(1);
599
600
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 hpp::constraints::Transform3s frame1(pinocchio::SE3::Random());
601
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 hpp::constraints::Transform3s frame2(pinocchio::SE3::Random());
602 // explicit relative pose
603 hpp::constraints::ExplicitPtr_t constraint(
604
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 4 hpp::constraints::explicit_::RelativePose::create(
605 "explicit-relative-pose", device, joint1, joint2, frame1, frame2,
606
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 6 6 * Equality, std::vector<bool>(6, true)));
607
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
 2 assert(constraint->inputConf().size() == 1);
608
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
 2 assert(constraint->inputConf()[0].first == 0);
609
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
 2 assert(constraint->inputConf()[0].second == 7);
610
611
2/2
✓ Branch 0 taken 100 times.
✓ Branch 1 taken 1 times.
 202 for (size_type k = 0; k < 100; ++k) {
612 // Pick a random configuration
613
1/2
✓ Branch 3 taken 100 times.
✗ Branch 4 not taken.
 200 vector_t q_rand(pinocchio::randomConfiguration(device->model(), low, up));
614
2/2
✓ Branch 0 taken 6400 times.
✓ Branch 1 taken 100 times.
 13000 for (size_type i = 0; i < 64; ++i) {
615
1/2
✓ Branch 1 taken 6400 times.
✗ Branch 2 not taken.
 12800 vector_t q(q_rand);
616 12800 size_type m = 1;
617
1/2
✓ Branch 2 taken 6400 times.
✗ Branch 3 not taken.
 12800 hpp::constraints::ComparisonTypes_t comp(6 * EqualToZero);
618
2/2
✓ Branch 0 taken 38400 times.
✓ Branch 1 taken 6400 times.
 89600 for (size_type j = 0; j < 6; ++j) {
619 // m = 2^(j+1)
620
2/2
✓ Branch 0 taken 19200 times.
✓ Branch 1 taken 19200 times.
 76800 if ((m & i) == 0) {
621 38400 comp[j] = Equality;
622 } else {
623 38400 comp[j] = EqualToZero;
624 }
625 76800 m *= 2;
626 }
627
1/2
✓ Branch 2 taken 6400 times.
✗ Branch 3 not taken.
 12800 constraint->comparisonType(comp);
628
1/2
✓ Branch 3 taken 6400 times.
✗ Branch 4 not taken.
 12800 hpp::constraints::ExplicitConstraintSet ecs(device->configSpace());
629
1/2
✓ Branch 1 taken 6400 times.
✗ Branch 2 not taken.
 12800 size_type res(ecs.add(constraint));
630
6/12
✓ Branch 1 taken 6400 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6400 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 6400 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 6400 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 6400 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 6400 times.
 12800 BOOST_CHECK(res != -1);
631 // Initialize right hand side with initial value of q -> rhs0_impl
632
1/2
✓ Branch 4 taken 6400 times.
✗ Branch 5 not taken.
 12800 LiegroupElement rhs0_impl(constraint->function().outputSpace());
633
2/4
✓ Branch 1 taken 6400 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6400 times.
✗ Branch 5 not taken.
 12800 rhs0_impl.vector() = ecs.rightHandSideFromInput(q);
634
635 // Solve constraint
636
2/4
✓ Branch 1 taken 6400 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6400 times.
✗ Branch 5 not taken.
 12800 ecs.solve(q);
637 // Get value of function h for new value of q -> rhs2_impl
638
1/2
✓ Branch 4 taken 6400 times.
✗ Branch 5 not taken.
 12800 LiegroupElement rhs2_impl(constraint->function().outputSpace());
639
3/6
✓ Branch 3 taken 6400 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 6400 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 6400 times.
✗ Branch 10 not taken.
 12800 constraint->function().value(rhs2_impl, q);
640
1/2
✓ Branch 1 taken 6400 times.
✗ Branch 2 not taken.
 12800 vector_t logRhs0_impl = log(rhs0_impl);
641
1/2
✓ Branch 1 taken 6400 times.
✗ Branch 2 not taken.
 12800 vector_t logRhs2_impl = log(rhs2_impl);
642
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 6400 times.
 12800 assert(logRhs0_impl.size() == 6);
643
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 6400 times.
 12800 assert(logRhs2_impl.size() == 6);
644 // For each coordinate check that
645 // rhs2_impl [j] == 0 if comp[i] is EqualToZero
646 // rhs2_impl [j] == rhs0_impl [j] if comp[i] is Equality
647
2/2
✓ Branch 0 taken 38400 times.
✓ Branch 1 taken 6400 times.
 89600 for (size_type j = 0; j < 6; ++j) {
648
14/26
✓ Branch 1 taken 38400 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 38400 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 38400 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 19200 times.
✓ Branch 15 taken 19200 times.
✓ Branch 17 taken 19200 times.
✗ Branch 18 not taken.
✗ Branch 19 not taken.
✓ Branch 20 taken 19200 times.
✓ Branch 22 taken 19200 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 19200 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 19200 times.
✗ Branch 29 not taken.
✓ Branch 30 taken 19200 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 38400 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 38400 times.
✗ Branch 37 not taken.
✗ Branch 41 not taken.
✓ Branch 42 taken 38400 times.
 76800 BOOST_CHECK(
649 ((comp[j] == EqualToZero) && (fabs(logRhs2_impl[j]) < 1e-10)) ||
650 ((comp[j] == Equality) &&
651 (fabs(logRhs2_impl[j] - logRhs0_impl[j]) < 1e-10)));
652 }
653 12800 }
654 200 }
655 2 }
656