Directory: | ./ |
---|---|
File: | tests/features/test_feature_generic.cpp |
Date: | 2024-08-13 12:13:25 |
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 |