GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
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 |
✓✗✓✗ |
5 |
: task_("task" + name), time_(0) { |
84 |
✓✗ | 5 |
expectedTaskOutput_.resize(dim); |
85 |
5 |
} |
|
86 |
|||
87 |
5 |
virtual void init() { |
|
88 |
5 |
task_.addFeature(featureAbstract()); |
|
89 |
✓✗ | 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 |
✓✗✓✗ ✓✗ |
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 |
✓✓✗✓ ✗ |
32 |
Vector s_sd(lg.nv()); |
102 |
✓✗ | 32 |
lg.difference(sdes, s, s_sd); |
103 |
|||
104 |
✓✗✓✓ ✗✓✗ |
32 |
Eigen::Matrix<typename LG_t::Scalar, LG_t::NV, LG_t::NV> Jminus(lg.nv(), |
105 |
lg.nv()); |
||
106 |
✓✗ | 32 |
lg.template dDifference<pinocchio::ARG0>(sdes, s, Jminus); |
107 |
|||
108 |
✓✗✓✗ ✓✗ |
32 |
computeExpectedTaskOutput(s_sd, Jminus * sDesDot); |
109 |
32 |
} |
|
110 |
|||
111 |
26 |
void checkTaskOutput() { |
|
112 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
26 |
BOOST_REQUIRE_EQUAL(task_.taskSOUT.accessCopy().size(), |
113 |
expectedTaskOutput_.size()); |
||
114 |
✓✗✓✗ |
52 |
Vector taskOutput(expectedTaskOutput_.size()); |
115 |
✓✗✓✓ |
182 |
for (int i = 0; i < expectedTaskOutput_.size(); ++i) |
116 |
✓✗✓✗ |
156 |
taskOutput[i] = task_.taskSOUT.accessCopy()[i].getSingleBound(); |
117 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
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 |
✓✗ | 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 |
✓✗✓✗ ✓✗✗✓ |
26 |
BOOST_CHECK_EQUAL(time_, f.errorSOUT.getTime()); |
145 |
✓✗✓✗ ✓✗✗✓ |
26 |
BOOST_CHECK_EQUAL(time_, f.errordotSOUT.getTime()); |
146 |
✓✗✓✗ ✓✗✗✓ |
26 |
BOOST_CHECK_EQUAL(time_, task_.errorSOUT.getTime()); |
147 |
✓✗✓✗ ✓✗✗✓ |
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 |
feature_("feature" + name), |
|
172 |
1 |
featureDes_("featureDes" + name), |
|
173 |
✓✗✓✗ ✓✗✓✗ |
2 |
dim_(dim) { |
174 |
✓✗ | 1 |
feature_.setReference(&featureDes_); |
175 |
✓✗✓✗ |
1 |
feature_.selectionSIN = Flags(true); |
176 |
|||
177 |
✓✗ | 2 |
dynamicgraph::Matrix Jq(dim, dim); |
178 |
✓✗ | 1 |
Jq.setIdentity(); |
179 |
✓✗ | 1 |
feature_.jacobianSIN.setReference(&Jq); |
180 |
|||
181 |
✓✗ | 1 |
init(); |
182 |
1 |
} |
|
183 |
|||
184 |
11 |
FeatureAbstract &featureAbstract() { return feature_; } |
|
185 |
|||
186 |
10 |
void setInputs() { |
|
187 |
✓✗✓✗ ✓✗ |
20 |
dynamicgraph::Vector s(dim_), sd(dim_), vd(dim_); |
188 |
double gain; |
||
189 |
|||
190 |
✗✓✓✓ |
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 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
1 |
BOOST_TEST_MESSAGE(" ----- Test Position -----"); |
200 |
✓✗ | 1 |
s.setZero(); |
201 |
✓✗ | 1 |
sd.setConstant(2.); |
202 |
✓✗ | 1 |
vd.setZero(); |
203 |
1 |
gain = 1.0; |
|
204 |
1 |
break; |
|
205 |
1 |
case 2: |
|
206 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
1 |
BOOST_TEST_MESSAGE(" ----- Test both -----"); |
207 |
✓✗ | 1 |
s.setZero(); |
208 |
✓✗ | 1 |
sd.setConstant(2.); |
209 |
✓✗ | 1 |
vd.setConstant(1.); |
210 |
1 |
gain = 3.0; |
|
211 |
1 |
break; |
|
212 |
8 |
default: |
|
213 |
✓✗ | 8 |
s.setRandom(); |
214 |
✓✗ | 8 |
sd.setRandom(); |
215 |
✓✗ | 8 |
vd.setRandom(); |
216 |
8 |
gain = 1.0; |
|
217 |
} |
||
218 |
|||
219 |
✓✗ | 10 |
feature_.errorSIN = s; |
220 |
✓✗ | 10 |
feature_.errorSIN.access(time_); |
221 |
10 |
feature_.errorSIN.setReady(); |
|
222 |
|||
223 |
✓✗ | 10 |
featureDes_.errorSIN = sd; |
224 |
✓✗ | 10 |
featureDes_.errorSIN.access(time_); |
225 |
10 |
featureDes_.errorSIN.setReady(); |
|
226 |
|||
227 |
✓✗ | 10 |
featureDes_.errordotSIN = vd; |
228 |
✓✗ | 10 |
featureDes_.errordotSIN.access(time_); |
229 |
10 |
featureDes_.errordotSIN.setReady(); |
|
230 |
|||
231 |
✓✗ | 10 |
setGain(gain); |
232 |
10 |
} |
|
233 |
|||
234 |
10 |
void printInputs() { |
|
235 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
10 |
BOOST_TEST_MESSAGE("----- inputs -----"); |
236 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
10 |
BOOST_TEST_MESSAGE( |
237 |
"feature_.errorSIN: " << feature_.errorSIN(time_).transpose()); |
||
238 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
10 |
BOOST_TEST_MESSAGE( |
239 |
"featureDes_.errorSIN: " << featureDes_.errorSIN(time_).transpose()); |
||
240 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
10 |
BOOST_TEST_MESSAGE("featureDes_.errordotSIN: " |
241 |
<< featureDes_.errordotSIN(time_).transpose()); |
||
242 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
10 |
BOOST_TEST_MESSAGE("task.controlGain: " << task_.controlGainSIN(time_)); |
243 |
10 |
} |
|
244 |
|||
245 |
10 |
void recompute() { |
|
246 |
✓✗ | 10 |
FeatureTestBase::recompute(); |
247 |
|||
248 |
✓✗✓✗ |
20 |
dynamicgraph::Vector s = feature_.errorSIN; |
249 |
✓✗✓✗ |
20 |
dynamicgraph::Vector sd = featureDes_.errorSIN; |
250 |
✓✗✓✗ |
20 |
dynamicgraph::Vector vd = featureDes_.errordotSIN; |
251 |
|||
252 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
10 |
computeExpectedTaskOutput(s - sd, -vd); |
253 |
|||
254 |
✓✗ | 10 |
checkTaskOutput(); |
255 |
10 |
} |
|
256 |
|||
257 |
10 |
void printOutputs() { |
|
258 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
10 |
BOOST_TEST_MESSAGE("----- output -----"); |
259 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
10 |
BOOST_TEST_MESSAGE("time: " << time_); |
260 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
10 |
BOOST_TEST_MESSAGE( |
261 |
"feature.errorSOUT: " << feature_.errorSOUT(time_).transpose()); |
||
262 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
10 |
BOOST_TEST_MESSAGE( |
263 |
"feature.errordotSOUT: " << feature_.errordotSOUT(time_).transpose()); |
||
264 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
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 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
10 |
BOOST_TEST_MESSAGE( |
269 |
"Expected task output: " << expectedTaskOutput_.transpose()); |
||
270 |
10 |
} |
|
271 |
}; |
||
272 |
|||
273 |
BOOST_AUTO_TEST_SUITE(feature_generic) |
||
274 |
|||
275 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(check_value) { |
276 |
✓✗ | 4 |
std::string srobot("test"); |
277 |
2 |
unsigned int dim = 6; |
|
278 |
|||
279 |
✓✗ | 4 |
TestFeatureGeneric testFeatureGeneric(dim, srobot); |
280 |
|||
281 |
✓✓✓✗ |
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 |
✓✗ | 48 |
MatrixHomogeneous M; |
288 |
✓✗✓✗ |
48 |
M.translation().setRandom(); |
289 |
✓✗✓✗ |
48 |
Eigen::Vector3d w(Eigen::Vector3d::Random()); |
290 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
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 |
✓✗ | 32 |
v.head<3>() = M.translation(); |
299 |
✓✗✓✗ |
32 |
QuaternionMap(v.tail<4>().data()) = M.rotation(); |
300 |
32 |
return v; |
|
301 |
} |
||
302 |
|||
303 |
96 |
Vector toVector(const std::vector<MultiBound> &in) { |
|
304 |
✓✗ | 96 |
Vector out(in.size()); |
305 |
✓✓✓✗ ✓✗ |
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 |
feature_("feature" + name), |
||
323 |
relative_(relative), |
||
324 |
✓✗✓✗ ✓✗✓✗ |
8 |
data_(model_) { |
325 |
✓✗ | 8 |
pinocchio::buildModels::humanoid(model_, true); // use freeflyer |
326 |
✓✗✓✗ |
8 |
model_.lowerPositionLimit.head<3>().setConstant(-1.); |
327 |
✓✗✓✗ |
8 |
model_.upperPositionLimit.head<3>().setConstant(1.); |
328 |
✓✗✓✗ |
8 |
jb_ = model_.getJointId("rarm_wrist2_joint"); |
329 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
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 |
✓✓ | 8 |
if (relative) { |
334 |
✓✗✓✗ |
4 |
ja_ = model_.getJointId("larm_wrist2_joint"); |
335 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
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 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
fa_ = model_.addFrame(pinocchio::Model::Frame( |
342 |
"frame_a", 0, 0, SE3::Identity(), pinocchio::OP_FRAME)); |
||
343 |
} |
||
344 |
✓✗✓✗ |
8 |
data_ = pinocchio::Data(model_); |
345 |
|||
346 |
✓✗ | 8 |
init(); |
347 |
|||
348 |
✓✗ | 8 |
setJointFrame(); |
349 |
8 |
} |
|
350 |
|||
351 |
20 |
void _setFrame() { |
|
352 |
✓✗ | 20 |
setSignal( |
353 |
✓✗ | 20 |
feature_.jaMfa, |
354 |
20 |
MatrixHomogeneous(model_.frames[fa_].placement.toHomogeneousMatrix())); |
|
355 |
✓✗ | 20 |
setSignal( |
356 |
✓✗ | 20 |
feature_.jbMfb, |
357 |
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 |
✓✗ | 64 |
Vector q(pinocchio::randomConfiguration(model_)); |
376 |
✓✗ | 32 |
pinocchio::framesForwardKinematics(model_, data_, q); |
377 |
✓✗ | 32 |
pinocchio::computeJointJacobians(model_, data_); |
378 |
|||
379 |
// Poses |
||
380 |
✓✗✓✗ ✓✗ |
32 |
setSignal(feature_.oMjb, |
381 |
32 |
MatrixHomogeneous(data_.oMi[jb_].toHomogeneousMatrix())); |
|
382 |
✓✓ | 32 |
if (relative_) { |
383 |
✓✗✓✗ ✓✗ |
16 |
setSignal(feature_.oMja, |
384 |
16 |
MatrixHomogeneous(data_.oMi[ja_].toHomogeneousMatrix())); |
|
385 |
} |
||
386 |
|||
387 |
// Jacobians |
||
388 |
✓✗ | 64 |
Matrix J(6, model_.nv); |
389 |
✓✗ | 32 |
J.setZero(); |
390 |
✓✗ | 32 |
pinocchio::getJointJacobian(model_, data_, jb_, pinocchio::LOCAL, J); |
391 |
✓✗ | 32 |
setSignal(feature_.jbJjb, J); |
392 |
✓✓ | 32 |
if (relative_) { |
393 |
✓✗ | 16 |
J.setZero(); |
394 |
✓✗ | 16 |
pinocchio::getJointJacobian(model_, data_, ja_, pinocchio::LOCAL, J); |
395 |
✓✗ | 16 |
setSignal(feature_.jaJja, J); |
396 |
} |
||
397 |
|||
398 |
// Desired |
||
399 |
✓✗✓✗ |
32 |
setSignal(feature_.faMfbDes, randomM()); |
400 |
✓✗✓✗ |
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 |
✓✓ | 32 |
if (time_ % 2 != 0) gain = 1; |
406 |
✓✗ | 32 |
setGain(gain); |
407 |
32 |
} |
|
408 |
|||
409 |
32 |
void printInputs() { |
|
410 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
32 |
BOOST_TEST_MESSAGE("----- inputs -----"); |
411 |
// BOOST_TEST_MESSAGE("feature_.errorSIN: " << |
||
412 |
// feature_.errorSIN(time_).transpose()); |
||
413 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
32 |
BOOST_TEST_MESSAGE("task.controlGain: " << task_.controlGainSIN(time_)); |
414 |
32 |
} |
|
415 |
|||
416 |
32 |
void recompute() { |
|
417 |
✓✗ | 32 |
FeatureTestBase::recompute(); |
418 |
|||
419 |
✓✗✓✗ |
32 |
const SE3 oMfb = data_.oMf[fb_], oMfa = data_.oMf[fa_], |
420 |
✓✗ | 32 |
faMfb(feature_.faMfb.accessCopy().matrix()), |
421 |
✓✗✓✗ |
32 |
faMfbDes(feature_.faMfbDes.accessCopy().matrix()); |
422 |
✓✗ | 32 |
const Vector &nu(feature_.faNufafbDes.accessCopy()); |
423 |
|||
424 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
64 |
computeExpectedTaskOutput( |
425 |
toVector(oMfa.inverse() * oMfb), toVector(faMfbDes), |
||
426 |
(boost::is_same<LieGroup_t, SE3_t>::value |
||
427 |
✓✗✓✗ ✓✗ |
16 |
? faMfbDes.toActionMatrixInverse() * nu |
428 |
: (Vector6d() << nu.head<3>() - |
||
429 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
32 |
faMfb.translation().cross(nu.tail<3>()), |
430 |
✓✗✓✗ |
32 |
faMfbDes.rotation().transpose() * nu.tail<3>()) |
431 |
✓✗✓✗ |
16 |
.finished()), |
432 |
LieGroup_t()); |
||
433 |
|||
434 |
✓✗ | 32 |
checkTaskOutput(); |
435 |
32 |
} |
|
436 |
|||
437 |
32 |
void printOutputs() { |
|
438 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
32 |
BOOST_TEST_MESSAGE("----- output -----"); |
439 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
32 |
BOOST_TEST_MESSAGE("time: " << time_); |
440 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
32 |
BOOST_TEST_MESSAGE( |
441 |
"feature.errorSOUT: " << feature_.errorSOUT(time_).transpose()); |
||
442 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
32 |
BOOST_TEST_MESSAGE( |
443 |
"feature.errordotSOUT: " << feature_.errordotSOUT(time_).transpose()); |
||
444 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
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 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
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 |
✓✗ | 32 |
setGain(1.); |
457 |
|||
458 |
✓✗ | 64 |
Vector q(pinocchio::randomConfiguration(model_)); |
459 |
✓✗ | 32 |
pinocchio::framesForwardKinematics(model_, data_, q); |
460 |
✓✗ | 32 |
pinocchio::computeJointJacobians(model_, data_); |
461 |
|||
462 |
// Poses |
||
463 |
✓✗✓✗ ✓✗ |
32 |
setSignal(feature_.oMjb, |
464 |
32 |
MatrixHomogeneous(data_.oMi[jb_].toHomogeneousMatrix())); |
|
465 |
✓✓ | 32 |
if (relative_) { |
466 |
✓✗✓✗ ✓✗ |
16 |
setSignal(feature_.oMja, |
467 |
16 |
MatrixHomogeneous(data_.oMi[ja_].toHomogeneousMatrix())); |
|
468 |
} |
||
469 |
|||
470 |
// Jacobians |
||
471 |
✓✗ | 64 |
Matrix J(6, model_.nv); |
472 |
✓✗ | 32 |
J.setZero(); |
473 |
✓✗ | 32 |
pinocchio::getJointJacobian(model_, data_, jb_, pinocchio::LOCAL, J); |
474 |
✓✗ | 32 |
setSignal(feature_.jbJjb, J); |
475 |
✓✓ | 32 |
if (relative_) { |
476 |
✓✗ | 16 |
J.setZero(); |
477 |
✓✗ | 16 |
pinocchio::getJointJacobian(model_, data_, ja_, pinocchio::LOCAL, J); |
478 |
✓✗ | 16 |
setSignal(feature_.jaJja, J); |
479 |
} |
||
480 |
|||
481 |
// Desired |
||
482 |
✓✗✓✗ |
32 |
setSignal(feature_.faMfbDes, randomM()); |
483 |
|||
484 |
// Get task jacobian |
||
485 |
✓✗✓✗ |
64 |
Vector e_q = task_.errorSOUT.access(time_); |
486 |
✓✗✓✗ |
32 |
J = task_.jacobianSOUT.access(time_); |
487 |
|||
488 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
96 |
Eigen::IOFormat PyVectorFmt(Eigen::FullPrecision, 0, ", ", ", ", "", "", |
489 |
"[", "]\n"); |
||
490 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
96 |
Eigen::IOFormat PyMatrixFmt(Eigen::FullPrecision, 0, ", ", ",\n", "[", "]", |
491 |
"[", "]\n"); |
||
492 |
|||
493 |
✓✗✓✗ |
32 |
Vector qdot(Vector::Zero(model_.nv)); |
494 |
32 |
double eps = 1e-6; |
|
495 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
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 |
✓✓ | 1120 |
for (int i = 0; i < model_.nv; ++i) { |
505 |
1088 |
time_++; |
|
506 |
✓✗ | 1088 |
qdot(i) = eps; |
507 |
|||
508 |
// Update pose signals |
||
509 |
✓✗ | 2176 |
Vector q_qdot = pinocchio::integrate(model_, q, qdot); |
510 |
✓✗ | 1088 |
pinocchio::framesForwardKinematics(model_, data_, q_qdot); |
511 |
✓✗✓✗ ✓✗ |
1088 |
setSignal(feature_.oMjb, |
512 |
1088 |
MatrixHomogeneous(data_.oMi[jb_].toHomogeneousMatrix())); |
|
513 |
✓✓ | 1088 |
if (relative_) { |
514 |
✓✗✓✗ ✓✗ |
544 |
setSignal(feature_.oMja, |
515 |
544 |
MatrixHomogeneous(data_.oMi[ja_].toHomogeneousMatrix())); |
|
516 |
} |
||
517 |
|||
518 |
// Recompute output and check finite diff |
||
519 |
✓✗✓✗ |
1088 |
Vector e_q_dq = task_.errorSOUT.access(time_); |
520 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
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 |
✓✗ | 1088 |
qdot(i) = 0.; |
527 |
} |
||
528 |
32 |
time_++; |
|
529 |
32 |
} |
|
530 |
|||
531 |
32 |
virtual void checkFeedForward() { |
|
532 |
✓✗ | 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 |
✓✗ | 64 |
Vector q(pinocchio::randomConfiguration(model_)); |
542 |
✓✗ | 32 |
pinocchio::framesForwardKinematics(model_, data_, q); |
543 |
✓✗ | 32 |
pinocchio::computeJointJacobians(model_, data_); |
544 |
|||
545 |
✓✗ | 32 |
SE3 faMfb = data_.oMf[fa_].actInv(data_.oMf[fb_]); |
546 |
|||
547 |
// Poses |
||
548 |
✓✗✓✗ ✓✗ |
32 |
setSignal(feature_.oMjb, |
549 |
32 |
MatrixHomogeneous(data_.oMi[jb_].toHomogeneousMatrix())); |
|
550 |
✓✓ | 32 |
if (relative_) { |
551 |
✓✗✓✗ ✓✗ |
16 |
setSignal(feature_.oMja, |
552 |
16 |
MatrixHomogeneous(data_.oMi[ja_].toHomogeneousMatrix())); |
|
553 |
} |
||
554 |
|||
555 |
// Jacobians |
||
556 |
✓✗ | 64 |
Matrix J(6, model_.nv); |
557 |
✓✗ | 32 |
J.setZero(); |
558 |
✓✗ | 32 |
pinocchio::getJointJacobian(model_, data_, jb_, pinocchio::LOCAL, J); |
559 |
✓✗ | 32 |
setSignal(feature_.jbJjb, J); |
560 |
✓✓ | 32 |
if (relative_) { |
561 |
✓✗ | 16 |
J.setZero(); |
562 |
✓✗ | 16 |
pinocchio::getJointJacobian(model_, data_, ja_, pinocchio::LOCAL, J); |
563 |
✓✗ | 16 |
setSignal(feature_.jaJja, J); |
564 |
} |
||
565 |
|||
566 |
✓✗ | 64 |
Matrix faJfafb; |
567 |
✓✗ | 32 |
J.setZero(); |
568 |
✓✗ | 32 |
pinocchio::getFrameJacobian(model_, data_, fb_, pinocchio::LOCAL, J); |
569 |
✓✗✓✗ ✓✗ |
32 |
faJfafb = faMfb.toActionMatrix() * J; |
570 |
✓✗ | 32 |
J.setZero(); |
571 |
✓✗ | 32 |
pinocchio::getFrameJacobian(model_, data_, fa_, pinocchio::LOCAL, J); |
572 |
✓✗ | 32 |
faJfafb -= J; |
573 |
|||
574 |
// Desired |
||
575 |
✓✗✓✗ |
32 |
setSignal(feature_.faMfbDes, randomM()); |
576 |
|||
577 |
// Get task jacobian |
||
578 |
✓✗ | 32 |
task_.jacobianSOUT.recompute(time_); |
579 |
✓✗ | 32 |
J = task_.jacobianSOUT.accessCopy(); |
580 |
✓✗ | 64 |
Eigen::JacobiSVD<Matrix> svd(J, Eigen::ComputeThinU | Eigen::ComputeThinV); |
581 |
|||
582 |
✓✗✓✗ |
32 |
Vector faNufafbDes(Vector::Zero(6)); |
583 |
32 |
double eps = 1e-6; |
|
584 |
✓✓ | 224 |
for (int i = 0; i < 6; ++i) { |
585 |
192 |
time_++; |
|
586 |
✓✗ | 192 |
faNufafbDes(i) = 1.; |
587 |
✓✗ | 192 |
setSignal(feature_.faNufafbDes, faNufafbDes); |
588 |
✓✗ | 192 |
task_.taskSOUT.recompute(time_); |
589 |
|||
590 |
✓✗✓✗ ✓✗ |
384 |
Vector qdot = svd.solve(toVector(task_.taskSOUT.accessCopy())); |
591 |
|||
592 |
✓✗✓✗ |
192 |
Vector faNufafb = faJfafb * qdot; |
593 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
192 |
EIGEN_VECTOR_IS_APPROX(faNufafbDes, faNufafb, eps); |
594 |
|||
595 |
✓✗ | 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 |
✓✓ | 48 |
for (int i = 0; i < N; ++i) runner.checkValue(); |
606 |
✓✓ | 48 |
for (int i = 0; i < N; ++i) runner.checkJacobian(); |
607 |
✓✓ | 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 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
4 |
BOOST_TEST_MESSAGE("absolute " << repr); |
615 |
✓✗✓✗ |
8 |
TestFeaturePose<representation> testAbsolute(false, "abs" + repr); |
616 |
✓✗ | 4 |
testAbsolute.setJointFrame(); |
617 |
✓✗ | 4 |
runTest(testAbsolute); |
618 |
|||
619 |
✓✗ | 4 |
testAbsolute.setRandomFrame(); |
620 |
✓✗ | 4 |
runTest(testAbsolute); |
621 |
4 |
} |
|
622 |
|||
623 |
BOOST_AUTO_TEST_SUITE(absolute) |
||
624 |
|||
625 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(r3xso3) { |
626 |
✓✗✓✗ |
2 |
feature_pose_absolute_tpl<R3xSO3Representation>("R3xSO3"); |
627 |
2 |
} |
|
628 |
|||
629 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(se3) { |
630 |
✓✗✓✗ |
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 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
4 |
BOOST_TEST_MESSAGE("relative " << repr); |
638 |
✓✗✓✗ |
8 |
TestFeaturePose<representation> testRelative(true, "rel" + repr); |
639 |
✓✗ | 4 |
runTest(testRelative); |
640 |
|||
641 |
✓✗ | 4 |
testRelative.setRandomFrame(); |
642 |
✓✗ | 4 |
runTest(testRelative); |
643 |
4 |
} |
|
644 |
|||
645 |
BOOST_AUTO_TEST_SUITE(relative) |
||
646 |
|||
647 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(r3xso3) { |
648 |
✓✗✓✗ |
2 |
feature_pose_relative_tpl<R3xSO3Representation>("R3xSO3"); |
649 |
2 |
} |
|
650 |
|||
651 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(se3) { |
652 |
✓✗✓✗ |
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 |
Generated by: GCOVR (Version 4.2) |