GCC Code Coverage Report

Directory: ./
File: tests/features/test_feature_generic.cpp
Date: 2025-05-13 12:28:21
Exec Total Coverage
Lines: 349 358 97.5%
Branches: 732 1443 50.7%
Line Branch Exec Source
1 /*
2 * Copyright 2019,
3 * François Bleibel,
4 * Olivier Stasse,
5 *
6 * CNRS/AIST
7 *
8 */
9
10 /* -------------------------------------------------------------------------- */
11 /* --- INCLUDES ------------------------------------------------------------- */
12 /* -------------------------------------------------------------------------- */
13 #include <iostream>
14 #include <pinocchio/algorithm/frames.hpp>
15 #include <pinocchio/algorithm/jacobian.hpp>
16 #include <pinocchio/algorithm/joint-configuration.hpp>
17 #include <pinocchio/algorithm/kinematics.hpp>
18 #include <pinocchio/multibody/data.hpp>
19 #include <pinocchio/multibody/liegroup/liegroup.hpp>
20 #include <pinocchio/multibody/model.hpp>
21 #include <pinocchio/parsers/sample-models.hpp>
22
23 #define BOOST_TEST_MODULE features
24 #include <dynamic-graph/factory.h>
25 #include <dynamic-graph/linear-algebra.h>
26
27 #include <Eigen/SVD>
28 #include <boost/test/unit_test.hpp>
29 #include <sot/core/debug.hh>
30 #include <sot/core/feature-abstract.hh>
31 #include <sot/core/feature-generic.hh>
32 #include <sot/core/feature-pose.hh>
33 #include <sot/core/gain-adaptive.hh>
34 #include <sot/core/sot.hh>
35 #include <sot/core/task.hh>
36
37 namespace dynamicgraph {
38 namespace sot {
39 namespace dg = dynamicgraph;
40
41 typedef pinocchio::CartesianProductOperation<
42 pinocchio::VectorSpaceOperationTpl<3, double>,
43 pinocchio::SpecialOrthogonalOperationTpl<3, double> >
44 R3xSO3_t;
45 typedef pinocchio::SpecialEuclideanOperationTpl<3, double> SE3_t;
46
47 namespace internal {
48 template <Representation_t representation>
49 struct LG_t {
50 typedef typename boost::mpl::if_c<representation == SE3Representation, SE3_t,
51 R3xSO3_t>::type type;
52 };
53 } // namespace internal
54 } // namespace sot
55 } // namespace dynamicgraph
56
57 using namespace std;
58 using namespace dynamicgraph::sot;
59 using namespace dynamicgraph;
60
61 #define EIGEN_VECTOR_IS_APPROX(Va, Vb, precision) \
62 BOOST_CHECK_MESSAGE((Va).isApprox(Vb, precision), \
63 "check " #Va ".isApprox(" #Vb \
64 ") failed " \
65 "[\n" \
66 << (Va).transpose() << "\n!=\n" \
67 << (Vb).transpose() << "\n]")
68 #define EIGEN_MATRIX_IS_APPROX(Va, Vb, precision) \
69 BOOST_CHECK_MESSAGE((Va).isApprox(Vb, precision), "check " #Va \
70 ".isApprox(" #Vb \
71 ") failed " \
72 "[\n" \
73 << (Va) << "\n!=\n" \
74 << (Vb) << "\n]")
75
76 class FeatureTestBase {
77 public:
78 Task task_;
79 int time_;
80 dynamicgraph::Vector expectedTaskOutput_;
81
82 5 FeatureTestBase(unsigned dim, const std::string &name)
83
2/4
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 5 times.
✗ Branch 7 not taken.
 5 : task_("task" + name), time_(0) {
84
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 expectedTaskOutput_.resize(dim);
85 5 }
86
87 5 virtual void init() {
88 5 task_.addFeature(featureAbstract());
89
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 task_.setWithDerivative(true);
90 5 }
91
92 26 void computeExpectedTaskOutput(const Vector &error,
93 const Vector &errorDrift) {
94 26 double gain = task_.controlGainSIN;
95
3/6
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 26 times.
✗ Branch 8 not taken.
 26 expectedTaskOutput_ = -gain * error - errorDrift;
96 26 }
97
98 template <typename LG_t>
99 32 void computeExpectedTaskOutput(const Vector &s, const Vector &sdes,
100 const Vector &sDesDot, const LG_t &lg) {
101
3/5
✓ Branch 1 taken 8 times.
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 32 Vector s_sd(lg.nv());
102
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 lg.difference(sdes, s, s_sd);
103
104
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 Eigen::Matrix<typename LG_t::Scalar, LG_t::NV, LG_t::NV> Jminus(lg.nv(),
105
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 32 lg.nv());
106
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 lg.template dDifference<pinocchio::ARG0>(sdes, s, Jminus);
107
108
3/6
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 16 times.
✗ Branch 8 not taken.
 32 computeExpectedTaskOutput(s_sd, Jminus * sDesDot);
109 32 }
110
111 26 void checkTaskOutput() {
112
5/10
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 26 times.
✗ Branch 6 not taken.
✓ Branch 13 taken 26 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 26 times.
✗ Branch 17 not taken.
✗ Branch 20 not taken.
✓ Branch 21 taken 26 times.
 26 BOOST_REQUIRE_EQUAL(task_.taskSOUT.accessCopy().size(),
113 expectedTaskOutput_.size());
114
1/2
✓ Branch 2 taken 26 times.
✗ Branch 3 not taken.
 26 Vector taskOutput(expectedTaskOutput_.size());
115
2/2
✓ Branch 1 taken 156 times.
✓ Branch 2 taken 26 times.
 182 for (int i = 0; i < expectedTaskOutput_.size(); ++i)
116
2/4
✓ Branch 3 taken 156 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 156 times.
✗ Branch 7 not taken.
 156 taskOutput[i] = task_.taskSOUT.accessCopy()[i].getSingleBound();
117
13/26
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 26 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 26 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 26 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 26 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 26 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 26 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 26 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 26 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 26 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 26 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 26 times.
✗ Branch 38 not taken.
✗ Branch 46 not taken.
✓ Branch 47 taken 26 times.
 26 EIGEN_VECTOR_IS_APPROX(taskOutput, expectedTaskOutput_, 1e-6);
118 26 }
119
120 58 void setGain(double gain) {
121 58 task_.controlGainSIN = gain;
122 58 task_.controlGainSIN.access(time_);
123 58 task_.controlGainSIN.setReady();
124 58 }
125
126 template <typename SignalType, typename ValueType>
127 2280 void setSignal(SignalType &sig, const ValueType &v) {
128
1/2
✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
 2280 sig = v;
129 2280 sig.access(time_);
130 2280 sig.setReady();
131 2280 }
132
133 virtual FeatureAbstract &featureAbstract() = 0;
134
135 virtual void setInputs() = 0;
136
137 ✗ virtual void printInputs() {}
138
139 26 virtual void recompute() {
140 26 task_.taskSOUT.recompute(time_);
141
142 // Check that recomputing went fine.
143 26 FeatureAbstract &f(featureAbstract());
144
4/8
✓ Branch 3 taken 26 times.
✗ Branch 4 not taken.
✓ Branch 9 taken 26 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 26 times.
✗ Branch 13 not taken.
✗ Branch 16 not taken.
✓ Branch 17 taken 26 times.
 26 BOOST_CHECK_EQUAL(time_, f.errorSOUT.getTime());
145
4/8
✓ Branch 3 taken 26 times.
✗ Branch 4 not taken.
✓ Branch 9 taken 26 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 26 times.
✗ Branch 13 not taken.
✗ Branch 16 not taken.
✓ Branch 17 taken 26 times.
 26 BOOST_CHECK_EQUAL(time_, f.errordotSOUT.getTime());
146
4/8
✓ Branch 3 taken 26 times.
✗ Branch 4 not taken.
✓ Branch 9 taken 26 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 26 times.
✗ Branch 13 not taken.
✗ Branch 16 not taken.
✓ Branch 17 taken 26 times.
 26 BOOST_CHECK_EQUAL(time_, task_.errorSOUT.getTime());
147
4/8
✓ Branch 3 taken 26 times.
✗ Branch 4 not taken.
✓ Branch 9 taken 26 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 26 times.
✗ Branch 13 not taken.
✗ Branch 16 not taken.
✓ Branch 17 taken 26 times.
 26 BOOST_CHECK_EQUAL(time_, task_.errorTimeDerivativeSOUT.getTime());
148
149 // TODO check that the output is correct
150 // computeExpectedTaskOutput (s - sd, - vd);
151 26 }
152
153 ✗ virtual void printOutputs() {}
154
155 26 virtual void checkValue() {
156 26 time_++;
157 26 setInputs();
158 26 printInputs();
159 26 recompute();
160 26 printOutputs();
161 26 }
162 };
163
164 class TestFeatureGeneric : public FeatureTestBase {
165 public:
166 FeatureGeneric feature_, featureDes_;
167 int dim_;
168
169 1 TestFeatureGeneric(unsigned dim, const std::string &name)
170 1 : FeatureTestBase(dim, name),
171
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 feature_("feature" + name),
172
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 featureDes_("featureDes" + name),
173
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 dim_(dim) {
174
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 feature_.setReference(&featureDes_);
175
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 feature_.selectionSIN = Flags(true);
176
177
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 dynamicgraph::Matrix Jq(dim, dim);
178
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 Jq.setIdentity();
179
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 feature_.jacobianSIN.setReference(&Jq);
180
181
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 init();
182 1 }
183
184 11 FeatureAbstract &featureAbstract() { return feature_; }
185
186 10 void setInputs() {
187
3/6
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
 10 dynamicgraph::Vector s(dim_), sd(dim_), vd(dim_);
188 double gain;
189
190
3/4
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 1 times.
✓ Branch 3 taken 8 times.
 10 switch (time_) {
191 ✗ case 0:
192 ✗ BOOST_TEST_MESSAGE(" ----- Test Velocity -----");
193 ✗ s.setZero();
194 ✗ sd.setZero();
195 ✗ vd.setZero();
196 ✗ gain = 0.0;
197 ✗ break;
198 1 case 1:
199
5/10
✓ 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 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
 1 BOOST_TEST_MESSAGE(" ----- Test Position -----");
200
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 s.setZero();
201
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 sd.setConstant(2.);
202
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 vd.setZero();
203 1 gain = 1.0;
204 1 break;
205 1 case 2:
206
5/10
✓ 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 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
 1 BOOST_TEST_MESSAGE(" ----- Test both -----");
207
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 s.setZero();
208
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 sd.setConstant(2.);
209
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 vd.setConstant(1.);
210 1 gain = 3.0;
211 1 break;
212 8 default:
213
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 s.setRandom();
214
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 sd.setRandom();
215
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 vd.setRandom();
216 8 gain = 1.0;
217 }
218
219
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 feature_.errorSIN = s;
220
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 feature_.errorSIN.access(time_);
221 10 feature_.errorSIN.setReady();
222
223
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 featureDes_.errorSIN = sd;
224
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 featureDes_.errorSIN.access(time_);
225 10 featureDes_.errorSIN.setReady();
226
227
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 featureDes_.errordotSIN = vd;
228
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 featureDes_.errordotSIN.access(time_);
229 10 featureDes_.errordotSIN.setReady();
230
231
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 setGain(gain);
232 10 }
233
234 10 void printInputs() {
235
5/10
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 10 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 10 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 10 times.
✗ Branch 16 not taken.
 10 BOOST_TEST_MESSAGE("----- inputs -----");
236
8/16
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 10 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 10 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 10 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 10 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 10 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 10 times.
✗ Branch 25 not taken.
 10 BOOST_TEST_MESSAGE(
237 "feature_.errorSIN: " << feature_.errorSIN(time_).transpose());
238
8/16
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 10 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 10 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 10 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 10 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 10 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 10 times.
✗ Branch 25 not taken.
 10 BOOST_TEST_MESSAGE(
239 "featureDes_.errorSIN: " << featureDes_.errorSIN(time_).transpose());
240
8/16
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 10 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 10 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 10 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 10 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 10 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 10 times.
✗ Branch 25 not taken.
 10 BOOST_TEST_MESSAGE("featureDes_.errordotSIN: "
241 << featureDes_.errordotSIN(time_).transpose());
242
7/14
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 10 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 10 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 10 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 10 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 10 times.
✗ Branch 22 not taken.
 10 BOOST_TEST_MESSAGE("task.controlGain: " << task_.controlGainSIN(time_));
243 10 }
244
245 10 void recompute() {
246
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 FeatureTestBase::recompute();
247
248
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
 10 dynamicgraph::Vector s = feature_.errorSIN;
249
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
 10 dynamicgraph::Vector sd = featureDes_.errorSIN;
250
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
 10 dynamicgraph::Vector vd = featureDes_.errordotSIN;
251
252
5/10
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
 10 computeExpectedTaskOutput(s - sd, -vd);
253
254
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 checkTaskOutput();
255 10 }
256
257 10 void printOutputs() {
258
5/10
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 10 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 10 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 10 times.
✗ Branch 16 not taken.
 10 BOOST_TEST_MESSAGE("----- output -----");
259
6/12
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 10 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 10 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 10 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 10 times.
✗ Branch 19 not taken.
 10 BOOST_TEST_MESSAGE("time: " << time_);
260
8/16
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 10 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 10 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 10 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 10 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 10 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 10 times.
✗ Branch 25 not taken.
 10 BOOST_TEST_MESSAGE(
261 "feature.errorSOUT: " << feature_.errorSOUT(time_).transpose());
262
8/16
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 10 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 10 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 10 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 10 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 10 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 10 times.
✗ Branch 25 not taken.
 10 BOOST_TEST_MESSAGE(
263 "feature.errordotSOUT: " << feature_.errordotSOUT(time_).transpose());
264
7/14
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 10 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 10 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 10 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 10 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 10 times.
✗ Branch 22 not taken.
 10 BOOST_TEST_MESSAGE("task.taskSOUT: " << task_.taskSOUT(time_));
265 // BOOST_TEST_MESSAGE("task.errorSOUT: " << task_.errorSOUT(time_));
266 // BOOST_TEST_MESSAGE("task.errordtSOUT: " <<
267 // task_.errorTimeDerivativeSOUT(time_));
268
7/14
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 10 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 10 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 10 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 10 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 10 times.
✗ Branch 22 not taken.
 10 BOOST_TEST_MESSAGE(
269 "Expected task output: " << expectedTaskOutput_.transpose());
270 10 }
271 };
272
273 BOOST_AUTO_TEST_SUITE(feature_generic)
274
275
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(check_value) {
276
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 std::string srobot("test");
277 2 unsigned int dim = 6;
278
279
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 TestFeatureGeneric testFeatureGeneric(dim, srobot);
280
281
3/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 10 times.
✓ Branch 4 taken 1 times.
 22 for (int i = 0; i < 10; ++i) testFeatureGeneric.checkValue();
282 2 }
283
284 BOOST_AUTO_TEST_SUITE_END() // feature_generic
285
286 48 MatrixHomogeneous randomM() {
287
1/2
✓ Branch 1 taken 48 times.
✗ Branch 2 not taken.
 48 MatrixHomogeneous M;
288
2/4
✓ Branch 1 taken 48 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 48 times.
✗ Branch 5 not taken.
 48 M.translation().setRandom();
289
2/4
✓ Branch 1 taken 48 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 48 times.
✗ Branch 5 not taken.
 48 Eigen::Vector3d w(Eigen::Vector3d::Random());
290
6/12
✓ Branch 1 taken 48 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 48 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 48 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 48 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 48 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 48 times.
✗ Branch 17 not taken.
 48 M.linear() = Eigen::AngleAxisd(w.norm(), w.normalized()).matrix();
291 96 return M;
292 }
293
294 typedef pinocchio::SE3 SE3;
295
296 32 Vector7 toVector(const pinocchio::SE3 &M) {
297 32 Vector7 v;
298
1/2
✓ Branch 3 taken 32 times.
✗ Branch 4 not taken.
 32 v.head<3>() = M.translation();
299
2/4
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
 32 QuaternionMap(v.tail<4>().data()) = M.rotation();
300 32 return v;
301 }
302
303 96 Vector toVector(const std::vector<MultiBound> &in) {
304
1/2
✓ Branch 2 taken 96 times.
✗ Branch 3 not taken.
 96 Vector out(in.size());
305
4/6
✓ Branch 2 taken 576 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 576 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 576 times.
✓ Branch 9 taken 96 times.
 672 for (int i = 0; i < (int)in.size(); ++i) out[i] = in[i].getSingleBound();
306 96 return out;
307 }
308
309 template <Representation_t representation>
310 class TestFeaturePose : public FeatureTestBase {
311 public:
312 typedef typename dg::sot::internal::LG_t<representation>::type LieGroup_t;
313 FeaturePose<representation> feature_;
314 bool relative_;
315 pinocchio::Model model_;
316 pinocchio::Data data_;
317 pinocchio::JointIndex ja_, jb_;
318 pinocchio::FrameIndex fa_, fb_;
319
320 8 TestFeaturePose(bool relative, const std::string &name)
321 : FeatureTestBase(6, name),
322
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 feature_("feature" + name),
323 8 relative_(relative),
324
3/6
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 4 times.
✗ Branch 9 not taken.
 16 data_(model_) {
325
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 pinocchio::buildModels::humanoid(model_, true); // use freeflyer
326
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 8 model_.lowerPositionLimit.head<3>().setConstant(-1.);
327
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 8 model_.upperPositionLimit.head<3>().setConstant(1.);
328
2/4
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
 8 jb_ = model_.getJointId("rarm_wrist2_joint");
329
7/14
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 4 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 4 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 4 times.
✗ Branch 21 not taken.
 16 fb_ = model_.addFrame(pinocchio::Model::Frame(
330 "frame_b", jb_,
331 8 model_.getFrameId("rarm_wrist2_joint", pinocchio::JOINT),
332 SE3::Identity(), pinocchio::OP_FRAME));
333
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 2 times.
 8 if (relative) {
334
2/4
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
 4 ja_ = model_.getJointId("larm_wrist2_joint");
335
7/14
✓ 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 14 taken 2 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 2 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 2 times.
✗ Branch 21 not taken.
 8 fa_ = model_.addFrame(pinocchio::Model::Frame(
336 "frame_a", ja_,
337 8 model_.getFrameId("larm_wrist2_joint", pinocchio::JOINT),
338 SE3::Identity(), pinocchio::OP_FRAME));
339 } else {
340 4 ja_ = 0;
341
5/10
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 2 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 2 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 2 times.
✗ Branch 15 not taken.
 4 fa_ = model_.addFrame(pinocchio::Model::Frame(
342 "frame_a", 0, 0, SE3::Identity(), pinocchio::OP_FRAME));
343 }
344
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 8 data_ = pinocchio::Data(model_);
345
346
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 init();
347
348
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 setJointFrame();
349 8 }
350
351 20 void _setFrame() {
352 40 setSignal(
353
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 20 feature_.jaMfa,
354
1/2
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
 20 MatrixHomogeneous(model_.frames[fa_].placement.toHomogeneousMatrix()));
355 40 setSignal(
356
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 20 feature_.jbMfb,
357
1/2
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
 20 MatrixHomogeneous(model_.frames[fb_].placement.toHomogeneousMatrix()));
358 20 }
359
360 12 void setJointFrame() {
361 12 model_.frames[fa_].placement.setIdentity();
362 12 model_.frames[fb_].placement.setIdentity();
363 12 _setFrame();
364 12 }
365
366 8 void setRandomFrame() {
367 8 model_.frames[fa_].placement.setRandom();
368 8 model_.frames[fb_].placement.setRandom();
369 8 _setFrame();
370 8 }
371
372 40 FeatureAbstract &featureAbstract() { return feature_; }
373
374 32 void setInputs() {
375
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 Vector q(pinocchio::randomConfiguration(model_));
376
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 pinocchio::framesForwardKinematics(model_, data_, q);
377
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 pinocchio::computeJointJacobians(model_, data_);
378
379 // Poses
380
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 setSignal(feature_.oMjb,
381
2/4
✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 16 times.
✗ Branch 6 not taken.
 32 MatrixHomogeneous(data_.oMi[jb_].toHomogeneousMatrix()));
382
2/2
✓ Branch 0 taken 8 times.
✓ Branch 1 taken 8 times.
 32 if (relative_) {
383
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 16 setSignal(feature_.oMja,
384
2/4
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 8 times.
✗ Branch 6 not taken.
 32 MatrixHomogeneous(data_.oMi[ja_].toHomogeneousMatrix()));
385 }
386
387 // Jacobians
388
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 Matrix J(6, model_.nv);
389
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 J.setZero();
390
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 pinocchio::getJointJacobian(model_, data_, jb_, pinocchio::LOCAL, J);
391
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 setSignal(feature_.jbJjb, J);
392
2/2
✓ Branch 0 taken 8 times.
✓ Branch 1 taken 8 times.
 32 if (relative_) {
393
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 16 J.setZero();
394
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 16 pinocchio::getJointJacobian(model_, data_, ja_, pinocchio::LOCAL, J);
395
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 16 setSignal(feature_.jaJja, J);
396 }
397
398 // Desired
399
2/4
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
 32 setSignal(feature_.faMfbDes, randomM());
400
2/4
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
 32 setSignal(feature_.faNufafbDes, Vector::Random(6));
401
402 32 double gain = 0;
403 // if (time_ % 5 != 0)
404 // gain = 2 * (double)rand() / RAND_MAX;
405
2/2
✓ Branch 0 taken 8 times.
✓ Branch 1 taken 8 times.
 32 if (time_ % 2 != 0) gain = 1;
406
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 setGain(gain);
407 32 }
408
409 32 void printInputs() {
410
5/10
✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 16 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 16 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 16 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 16 times.
✗ Branch 16 not taken.
 32 BOOST_TEST_MESSAGE("----- inputs -----");
411 // BOOST_TEST_MESSAGE("feature_.errorSIN: " <<
412 // feature_.errorSIN(time_).transpose());
413
7/14
✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 16 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 16 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 16 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 16 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 16 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 16 times.
✗ Branch 22 not taken.
 32 BOOST_TEST_MESSAGE("task.controlGain: " << task_.controlGainSIN(time_));
414 32 }
415
416 32 void recompute() {
417
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 FeatureTestBase::recompute();
418
419
2/4
✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 16 times.
✗ Branch 7 not taken.
 32 const SE3 oMfb = data_.oMf[fb_], oMfa = data_.oMf[fa_],
420
1/2
✓ Branch 3 taken 16 times.
✗ Branch 4 not taken.
 32 faMfb(feature_.faMfb.accessCopy().matrix()),
421
2/4
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 16 times.
✗ Branch 6 not taken.
 32 faMfbDes(feature_.faMfbDes.accessCopy().matrix());
422
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 const Vector &nu(feature_.faNufafbDes.accessCopy());
423
424
8/16
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 16 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 16 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 16 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 16 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 16 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 16 times.
✗ Branch 23 not taken.
 32 computeExpectedTaskOutput(
425 toVector(oMfa.inverse() * oMfb), toVector(faMfbDes),
426 (boost::is_same<LieGroup_t, SE3_t>::value
427
3/6
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
 32 ? faMfbDes.toActionMatrixInverse() * nu
428
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 32 : (Vector6d() << nu.head<3>() -
429
6/12
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 8 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 8 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 8 times.
✗ Branch 17 not taken.
 48 faMfb.translation().cross(nu.tail<3>()),
430
4/8
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 8 times.
✗ Branch 11 not taken.
 32 faMfbDes.rotation().transpose() * nu.tail<3>())
431 16 .finished()),
432
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 LieGroup_t());
433
434
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 checkTaskOutput();
435 32 }
436
437 32 void printOutputs() {
438
5/10
✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 16 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 16 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 16 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 16 times.
✗ Branch 16 not taken.
 32 BOOST_TEST_MESSAGE("----- output -----");
439
6/12
✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 16 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 16 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 16 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 16 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 16 times.
✗ Branch 19 not taken.
 32 BOOST_TEST_MESSAGE("time: " << time_);
440
8/16
✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 16 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 16 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 16 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 16 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 16 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 16 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 16 times.
✗ Branch 25 not taken.
 32 BOOST_TEST_MESSAGE(
441 "feature.errorSOUT: " << feature_.errorSOUT(time_).transpose());
442
8/16
✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 16 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 16 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 16 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 16 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 16 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 16 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 16 times.
✗ Branch 25 not taken.
 32 BOOST_TEST_MESSAGE(
443 "feature.errordotSOUT: " << feature_.errordotSOUT(time_).transpose());
444
7/14
✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 16 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 16 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 16 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 16 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 16 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 16 times.
✗ Branch 22 not taken.
 32 BOOST_TEST_MESSAGE("task.taskSOUT: " << task_.taskSOUT(time_));
445 // BOOST_TEST_MESSAGE("task.errorSOUT: " << task_.errorSOUT(time_));
446 // BOOST_TEST_MESSAGE("task.errordtSOUT: " <<
447 // task_.errorTimeDerivativeSOUT(time_));
448
7/14
✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 16 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 16 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 16 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 16 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 16 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 16 times.
✗ Branch 22 not taken.
 32 BOOST_TEST_MESSAGE(
449 "Expected task output: " << expectedTaskOutput_.transpose());
450 32 }
451
452 32 virtual void checkJacobian() {
453 32 time_++;
454 // We want to check that e (q+dq, t) ~ e(q, t) + de/dq(q,t) dq
455
456
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 setGain(1.);
457
458
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 Vector q(pinocchio::randomConfiguration(model_));
459
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 pinocchio::framesForwardKinematics(model_, data_, q);
460
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 pinocchio::computeJointJacobians(model_, data_);
461
462 // Poses
463
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 setSignal(feature_.oMjb,
464
2/4
✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 16 times.
✗ Branch 6 not taken.
 32 MatrixHomogeneous(data_.oMi[jb_].toHomogeneousMatrix()));
465
2/2
✓ Branch 0 taken 8 times.
✓ Branch 1 taken 8 times.
 32 if (relative_) {
466
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 16 setSignal(feature_.oMja,
467
2/4
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 8 times.
✗ Branch 6 not taken.
 32 MatrixHomogeneous(data_.oMi[ja_].toHomogeneousMatrix()));
468 }
469
470 // Jacobians
471
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 Matrix J(6, model_.nv);
472
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 J.setZero();
473
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 pinocchio::getJointJacobian(model_, data_, jb_, pinocchio::LOCAL, J);
474
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 setSignal(feature_.jbJjb, J);
475
2/2
✓ Branch 0 taken 8 times.
✓ Branch 1 taken 8 times.
 32 if (relative_) {
476
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 16 J.setZero();
477
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 16 pinocchio::getJointJacobian(model_, data_, ja_, pinocchio::LOCAL, J);
478
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 16 setSignal(feature_.jaJja, J);
479 }
480
481 // Desired
482
2/4
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
 32 setSignal(feature_.faMfbDes, randomM());
483
484 // Get task jacobian
485
2/4
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
 32 Vector e_q = task_.errorSOUT.access(time_);
486
2/4
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
 32 J = task_.jacobianSOUT.access(time_);
487
488
7/14
✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 16 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 16 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 16 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 16 times.
✗ Branch 19 not taken.
✓ Branch 22 taken 16 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 16 times.
✗ Branch 26 not taken.
 64 Eigen::IOFormat PyVectorFmt(Eigen::FullPrecision, 0, ", ", ", ", "", "",
489 "[", "]\n");
490
7/14
✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 16 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 16 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 16 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 16 times.
✗ Branch 19 not taken.
✓ Branch 22 taken 16 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 16 times.
✗ Branch 26 not taken.
 64 Eigen::IOFormat PyMatrixFmt(Eigen::FullPrecision, 0, ", ", ",\n", "[", "]",
491 "[", "]\n");
492
493
2/4
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
 32 Vector qdot(Vector::Zero(model_.nv));
494 32 double eps = 1e-6;
495
18/36
✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 16 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 16 times.
✗ Branch 9 not taken.
✓ Branch 13 taken 16 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 16 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 16 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 16 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 16 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 16 times.
✗ Branch 30 not taken.
✓ Branch 33 taken 16 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 16 times.
✗ Branch 37 not taken.
✓ Branch 39 taken 16 times.
✗ Branch 40 not taken.
✓ Branch 43 taken 16 times.
✗ Branch 44 not taken.
✓ Branch 46 taken 16 times.
✗ Branch 47 not taken.
✓ Branch 49 taken 16 times.
✗ Branch 50 not taken.
✓ Branch 52 taken 16 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 16 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 16 times.
✗ Branch 59 not taken.
 32 BOOST_TEST_MESSAGE(
496 data_.oMi[ja_].toHomogeneousMatrix().format(PyMatrixFmt)
497 << data_.oMi[jb_].toHomogeneousMatrix().format(PyMatrixFmt)
498 << model_.frames[fa_].placement.toHomogeneousMatrix().format(
499 PyMatrixFmt)
500 << model_.frames[fb_].placement.toHomogeneousMatrix().format(
501 PyMatrixFmt)
502 << J.format(PyMatrixFmt));
503
504
2/2
✓ Branch 1 taken 544 times.
✓ Branch 2 taken 16 times.
 1120 for (int i = 0; i < model_.nv; ++i) {
505 1088 time_++;
506
1/2
✓ Branch 1 taken 544 times.
✗ Branch 2 not taken.
 1088 qdot(i) = eps;
507
508 // Update pose signals
509
1/2
✓ Branch 1 taken 544 times.
✗ Branch 2 not taken.
 1088 Vector q_qdot = pinocchio::integrate(model_, q, qdot);
510
1/2
✓ Branch 1 taken 544 times.
✗ Branch 2 not taken.
 1088 pinocchio::framesForwardKinematics(model_, data_, q_qdot);
511
1/2
✓ Branch 1 taken 544 times.
✗ Branch 2 not taken.
 1088 setSignal(feature_.oMjb,
512
2/4
✓ Branch 2 taken 544 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 544 times.
✗ Branch 6 not taken.
 1088 MatrixHomogeneous(data_.oMi[jb_].toHomogeneousMatrix()));
513
2/2
✓ Branch 0 taken 272 times.
✓ Branch 1 taken 272 times.
 1088 if (relative_) {
514
1/2
✓ Branch 1 taken 272 times.
✗ Branch 2 not taken.
 544 setSignal(feature_.oMja,
515
2/4
✓ Branch 2 taken 272 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 272 times.
✗ Branch 6 not taken.
 1088 MatrixHomogeneous(data_.oMi[ja_].toHomogeneousMatrix()));
516 }
517
518 // Recompute output and check finite diff
519
2/4
✓ Branch 1 taken 544 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 544 times.
✗ Branch 5 not taken.
 1088 Vector e_q_dq = task_.errorSOUT.access(time_);
520
22/44
✓ Branch 1 taken 544 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 544 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 544 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 544 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 544 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 544 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 544 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 544 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 544 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 544 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 544 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 544 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 544 times.
✗ Branch 41 not taken.
✓ Branch 43 taken 544 times.
✗ Branch 44 not taken.
✓ Branch 46 taken 544 times.
✗ Branch 47 not taken.
✓ Branch 49 taken 544 times.
✗ Branch 50 not taken.
✓ Branch 52 taken 544 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 544 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 544 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 544 times.
✗ Branch 62 not taken.
✓ Branch 64 taken 544 times.
✗ Branch 65 not taken.
✗ Branch 77 not taken.
✓ Branch 78 taken 544 times.
 1088 BOOST_CHECK_MESSAGE(
521 (((e_q_dq - e_q) / eps) - J.col(i)).maxCoeff() < 1e-4,
522 "Jacobian col " << i << " does not match finite difference.\n"
523 << ((e_q_dq - e_q) / eps).format(PyVectorFmt) << '\n'
524 << J.col(i).format(PyVectorFmt) << '\n');
525
526
1/2
✓ Branch 1 taken 544 times.
✗ Branch 2 not taken.
 1088 qdot(i) = 0.;
527 }
528 32 time_++;
529 32 }
530
531 32 virtual void checkFeedForward() {
532
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 setGain(0.);
533 // random config
534 // set inputs
535 // compute e = task_.taskSOUT and J = task_.jacobianSOUT
536 // check that e (q + eps*qdot) - e (q) ~= eps * J * qdot
537 // compute qdot = J^+ * e
538 // check that faMfb (q+eps*qdot) ~= faMfb(q) + eps * faNufafbDes
539
540 32 time_++;
541
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 Vector q(pinocchio::randomConfiguration(model_));
542
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 pinocchio::framesForwardKinematics(model_, data_, q);
543
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 pinocchio::computeJointJacobians(model_, data_);
544
545
1/2
✓ Branch 3 taken 16 times.
✗ Branch 4 not taken.
 32 SE3 faMfb = data_.oMf[fa_].actInv(data_.oMf[fb_]);
546
547 // Poses
548
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 setSignal(feature_.oMjb,
549
2/4
✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 16 times.
✗ Branch 6 not taken.
 32 MatrixHomogeneous(data_.oMi[jb_].toHomogeneousMatrix()));
550
2/2
✓ Branch 0 taken 8 times.
✓ Branch 1 taken 8 times.
 32 if (relative_) {
551
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 16 setSignal(feature_.oMja,
552
2/4
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 8 times.
✗ Branch 6 not taken.
 32 MatrixHomogeneous(data_.oMi[ja_].toHomogeneousMatrix()));
553 }
554
555 // Jacobians
556
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 Matrix J(6, model_.nv);
557
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 J.setZero();
558
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 pinocchio::getJointJacobian(model_, data_, jb_, pinocchio::LOCAL, J);
559
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 setSignal(feature_.jbJjb, J);
560
2/2
✓ Branch 0 taken 8 times.
✓ Branch 1 taken 8 times.
 32 if (relative_) {
561
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 16 J.setZero();
562
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 16 pinocchio::getJointJacobian(model_, data_, ja_, pinocchio::LOCAL, J);
563
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 16 setSignal(feature_.jaJja, J);
564 }
565
566
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 Matrix faJfafb;
567
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 J.setZero();
568
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 pinocchio::getFrameJacobian(model_, data_, fb_, pinocchio::LOCAL, J);
569
3/6
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 16 times.
✗ Branch 8 not taken.
 32 faJfafb = faMfb.toActionMatrix() * J;
570
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 J.setZero();
571
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 pinocchio::getFrameJacobian(model_, data_, fa_, pinocchio::LOCAL, J);
572
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 faJfafb -= J;
573
574 // Desired
575
2/4
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
 32 setSignal(feature_.faMfbDes, randomM());
576
577 // Get task jacobian
578
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 task_.jacobianSOUT.recompute(time_);
579
1/2
✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
 32 J = task_.jacobianSOUT.accessCopy();
580
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 Eigen::JacobiSVD<Matrix> svd(J, Eigen::ComputeThinU | Eigen::ComputeThinV);
581
582
2/4
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
 32 Vector faNufafbDes(Vector::Zero(6));
583 32 double eps = 1e-6;
584
2/2
✓ Branch 1 taken 96 times.
✓ Branch 2 taken 16 times.
 224 for (int i = 0; i < 6; ++i) {
585 192 time_++;
586
1/2
✓ Branch 1 taken 96 times.
✗ Branch 2 not taken.
 192 faNufafbDes(i) = 1.;
587
1/2
✓ Branch 1 taken 96 times.
✗ Branch 2 not taken.
 192 setSignal(feature_.faNufafbDes, faNufafbDes);
588
1/2
✓ Branch 1 taken 96 times.
✗ Branch 2 not taken.
 192 task_.taskSOUT.recompute(time_);
589
590
3/6
✓ Branch 2 taken 96 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 96 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 96 times.
✗ Branch 9 not taken.
 192 Vector qdot = svd.solve(toVector(task_.taskSOUT.accessCopy()));
591
592
2/4
✓ Branch 1 taken 96 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 96 times.
✗ Branch 5 not taken.
 192 Vector faNufafb = faJfafb * qdot;
593
13/26
✓ Branch 1 taken 96 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 96 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 96 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 96 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 96 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 96 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 96 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 96 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 96 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 96 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 96 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 96 times.
✗ Branch 38 not taken.
✗ Branch 46 not taken.
✓ Branch 47 taken 96 times.
 192 EIGEN_VECTOR_IS_APPROX(faNufafbDes, faNufafb, eps);
594
595
1/2
✓ Branch 1 taken 96 times.
✗ Branch 2 not taken.
 192 faNufafbDes(i) = 0.;
596 }
597 32 time_++;
598 32 }
599 };
600
601 template <typename TestClass>
602 16 void runTest(TestClass &runner, int N = 2)
603 // int N = 10)
604 {
605
2/2
✓ Branch 1 taken 16 times.
✓ Branch 2 taken 8 times.
 48 for (int i = 0; i < N; ++i) runner.checkValue();
606
2/2
✓ Branch 1 taken 16 times.
✓ Branch 2 taken 8 times.
 48 for (int i = 0; i < N; ++i) runner.checkJacobian();
607
2/2
✓ Branch 1 taken 16 times.
✓ Branch 2 taken 8 times.
 48 for (int i = 0; i < N; ++i) runner.checkFeedForward();
608 16 }
609
610 BOOST_AUTO_TEST_SUITE(feature_pose)
611
612 template <Representation_t representation>
613 4 void feature_pose_absolute_tpl(const std::string &repr) {
614
6/12
✓ 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.
✓ Branch 12 taken 2 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 2 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 2 times.
✗ Branch 19 not taken.
 4 BOOST_TEST_MESSAGE("absolute " << repr);
615
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 4 TestFeaturePose<representation> testAbsolute(false, "abs" + repr);
616
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 testAbsolute.setJointFrame();
617
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 runTest(testAbsolute);
618
619
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 testAbsolute.setRandomFrame();
620
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 runTest(testAbsolute);
621 4 }
622
623 BOOST_AUTO_TEST_SUITE(absolute)
624
625
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(r3xso3) {
626
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 feature_pose_absolute_tpl<R3xSO3Representation>("R3xSO3");
627 2 }
628
629
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(se3) {
630
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 feature_pose_absolute_tpl<SE3Representation>("SE3");
631 2 }
632
633 BOOST_AUTO_TEST_SUITE_END() // absolute
634
635 template <Representation_t representation>
636 4 void feature_pose_relative_tpl(const std::string &repr) {
637
6/12
✓ 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.
✓ Branch 12 taken 2 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 2 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 2 times.
✗ Branch 19 not taken.
 4 BOOST_TEST_MESSAGE("relative " << repr);
638
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 4 TestFeaturePose<representation> testRelative(true, "rel" + repr);
639
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 runTest(testRelative);
640
641
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 testRelative.setRandomFrame();
642
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 runTest(testRelative);
643 4 }
644
645 BOOST_AUTO_TEST_SUITE(relative)
646
647
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(r3xso3) {
648
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 feature_pose_relative_tpl<R3xSO3Representation>("R3xSO3");
649 2 }
650
651
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(se3) {
652
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 feature_pose_relative_tpl<SE3Representation>("SE3");
653 2 }
654
655 BOOST_AUTO_TEST_SUITE_END() // relative
656
657 BOOST_AUTO_TEST_SUITE_END() // feature_pose
658