GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
Line | Branch | Exec | Source |
1 |
// |
||
2 |
// Copyright (c) 2015-2019 CNRS INRIA |
||
3 |
// |
||
4 |
|||
5 |
#include "pinocchio/multibody/model.hpp" |
||
6 |
#include "pinocchio/multibody/data.hpp" |
||
7 |
#include "pinocchio/multibody/visitor.hpp" |
||
8 |
|||
9 |
#include <iostream> |
||
10 |
|||
11 |
#include <boost/test/unit_test.hpp> |
||
12 |
#include <boost/utility/binary.hpp> |
||
13 |
|||
14 |
namespace bf = boost::fusion; |
||
15 |
|||
16 |
struct SimpleUnaryVisitor1 |
||
17 |
: public pinocchio::fusion::JointUnaryVisitorBase<SimpleUnaryVisitor1> |
||
18 |
{ |
||
19 |
|||
20 |
typedef bf::vector<const pinocchio::Model &, |
||
21 |
pinocchio::Data &, |
||
22 |
pinocchio::JointIndex> ArgsType; |
||
23 |
|||
24 |
template<typename JointModel> |
||
25 |
42 |
static void algo(const pinocchio::JointModelBase<JointModel> & jmodel, |
|
26 |
pinocchio::JointDataBase<typename JointModel::JointDataDerived> & jdata, |
||
27 |
const pinocchio::Model & model, |
||
28 |
pinocchio::Data & data, |
||
29 |
pinocchio::JointIndex jindex) |
||
30 |
{ |
||
31 |
PINOCCHIO_UNUSED_VARIABLE(jdata); |
||
32 |
PINOCCHIO_UNUSED_VARIABLE(model); |
||
33 |
PINOCCHIO_UNUSED_VARIABLE(data); |
||
34 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
42 |
BOOST_CHECK(jindex == jmodel.id()); |
35 |
✓✗✓✗ |
42 |
std::cout << "joint name: " << jmodel.shortname() << std::endl; |
36 |
42 |
} |
|
37 |
}; |
||
38 |
|||
39 |
struct SimpleUnaryVisitor2 |
||
40 |
: public pinocchio::fusion::JointUnaryVisitorBase<SimpleUnaryVisitor2> |
||
41 |
{ |
||
42 |
|||
43 |
typedef bf::vector<const pinocchio::Model &, |
||
44 |
pinocchio::Data &, |
||
45 |
pinocchio::JointIndex> ArgsType; |
||
46 |
|||
47 |
template<typename JointModel> |
||
48 |
42 |
static void algo(const pinocchio::JointModelBase<JointModel> & jmodel, |
|
49 |
const pinocchio::Model & model, |
||
50 |
pinocchio::Data & data, |
||
51 |
pinocchio::JointIndex jindex) |
||
52 |
{ |
||
53 |
PINOCCHIO_UNUSED_VARIABLE(model); |
||
54 |
PINOCCHIO_UNUSED_VARIABLE(data); |
||
55 |
PINOCCHIO_UNUSED_VARIABLE(jindex); |
||
56 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
42 |
BOOST_CHECK(jindex == jmodel.id()); |
57 |
✓✗✓✗ |
42 |
std::cout << "joint name: " << jmodel.shortname() << std::endl; |
58 |
42 |
} |
|
59 |
}; |
||
60 |
|||
61 |
struct SimpleUnaryVisitor3 |
||
62 |
: public pinocchio::fusion::JointUnaryVisitorBase<SimpleUnaryVisitor3> |
||
63 |
{ |
||
64 |
|||
65 |
template<typename JointModel> |
||
66 |
42 |
static void algo(const pinocchio::JointModelBase<JointModel> & jmodel) |
|
67 |
{ |
||
68 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
42 |
BOOST_CHECK(!jmodel.shortname().empty()); |
69 |
✓✗✓✗ |
42 |
std::cout << "joint name: " << jmodel.shortname() << std::endl; |
70 |
42 |
} |
|
71 |
}; |
||
72 |
|||
73 |
struct SimpleUnaryVisitor4 |
||
74 |
: public pinocchio::fusion::JointUnaryVisitorBase<SimpleUnaryVisitor4> |
||
75 |
{ |
||
76 |
|||
77 |
template<typename JointModel> |
||
78 |
42 |
static void algo(const pinocchio::JointModelBase<JointModel> & jmodel, |
|
79 |
pinocchio::JointDataBase<typename JointModel::JointDataDerived> & jdata) |
||
80 |
{ |
||
81 |
PINOCCHIO_UNUSED_VARIABLE(jdata); |
||
82 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
42 |
BOOST_CHECK(!jmodel.shortname().empty()); |
83 |
✓✗✓✗ |
42 |
std::cout << "joint name: " << jmodel.shortname() << std::endl; |
84 |
42 |
} |
|
85 |
}; |
||
86 |
|||
87 |
struct SimpleBinaryVisitor1 |
||
88 |
: public pinocchio::fusion::JointBinaryVisitorBase<SimpleBinaryVisitor1> |
||
89 |
{ |
||
90 |
|||
91 |
typedef bf::vector<const pinocchio::Model &, |
||
92 |
pinocchio::Data &, |
||
93 |
pinocchio::JointIndex> ArgsType; |
||
94 |
|||
95 |
template<typename JointModel1, typename JointModel2> |
||
96 |
42 |
static void algo(const pinocchio::JointModelBase<JointModel1> & jmodel1, |
|
97 |
const pinocchio::JointModelBase<JointModel2> & jmodel2, |
||
98 |
pinocchio::JointDataBase<typename JointModel1::JointDataDerived> & jdata1, |
||
99 |
pinocchio::JointDataBase<typename JointModel2::JointDataDerived> & jdata2, |
||
100 |
const pinocchio::Model & model, |
||
101 |
pinocchio::Data & data, |
||
102 |
pinocchio::JointIndex jindex) |
||
103 |
{ |
||
104 |
PINOCCHIO_UNUSED_VARIABLE(jdata1); |
||
105 |
PINOCCHIO_UNUSED_VARIABLE(jdata2); |
||
106 |
PINOCCHIO_UNUSED_VARIABLE(model); |
||
107 |
PINOCCHIO_UNUSED_VARIABLE(data); |
||
108 |
PINOCCHIO_UNUSED_VARIABLE(jindex); |
||
109 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
42 |
BOOST_CHECK(jindex == jmodel1.id()); |
110 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
42 |
BOOST_CHECK(jindex == jmodel2.id()); |
111 |
✓✗✓✗ |
42 |
std::cout << "joint1 name: " << jmodel1.shortname() << std::endl; |
112 |
✓✗✓✗ |
42 |
std::cout << "joint2 name: " << jmodel2.shortname() << std::endl; |
113 |
} |
||
114 |
}; |
||
115 |
|||
116 |
struct SimpleBinaryVisitor2 |
||
117 |
: public pinocchio::fusion::JointBinaryVisitorBase<SimpleBinaryVisitor2> |
||
118 |
{ |
||
119 |
|||
120 |
typedef bf::vector<const pinocchio::Model &, |
||
121 |
pinocchio::Data &, |
||
122 |
pinocchio::JointIndex> ArgsType; |
||
123 |
|||
124 |
template<typename JointModel1, typename JointModel2> |
||
125 |
42 |
static void algo(const pinocchio::JointModelBase<JointModel1> & jmodel1, |
|
126 |
const pinocchio::JointModelBase<JointModel2> & jmodel2, |
||
127 |
const pinocchio::Model & model, |
||
128 |
pinocchio::Data & data, |
||
129 |
pinocchio::JointIndex jindex) |
||
130 |
{ |
||
131 |
PINOCCHIO_UNUSED_VARIABLE(model); |
||
132 |
PINOCCHIO_UNUSED_VARIABLE(data); |
||
133 |
PINOCCHIO_UNUSED_VARIABLE(jindex); |
||
134 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
42 |
BOOST_CHECK(jindex == jmodel1.id()); |
135 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
42 |
BOOST_CHECK(jindex == jmodel2.id()); |
136 |
✓✗✓✗ |
42 |
std::cout << "joint1 name: " << jmodel1.shortname() << std::endl; |
137 |
✓✗✓✗ |
42 |
std::cout << "joint2 name: " << jmodel2.shortname() << std::endl; |
138 |
} |
||
139 |
}; |
||
140 |
|||
141 |
struct SimpleBinaryVisitor3 |
||
142 |
: public pinocchio::fusion::JointBinaryVisitorBase<SimpleBinaryVisitor3> |
||
143 |
{ |
||
144 |
template<typename JointModel1, typename JointModel2> |
||
145 |
42 |
static void algo(const pinocchio::JointModelBase<JointModel1> & jmodel1, |
|
146 |
const pinocchio::JointModelBase<JointModel2> & jmodel2) |
||
147 |
{ |
||
148 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
42 |
BOOST_CHECK(!jmodel1.shortname().empty()); |
149 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
42 |
BOOST_CHECK(!jmodel2.shortname().empty()); |
150 |
✓✗✓✗ |
42 |
std::cout << "joint1 name: " << jmodel1.shortname() << std::endl; |
151 |
✓✗✓✗ |
42 |
std::cout << "joint2 name: " << jmodel2.shortname() << std::endl; |
152 |
} |
||
153 |
}; |
||
154 |
|||
155 |
struct SimpleBinaryVisitor4 |
||
156 |
: public pinocchio::fusion::JointBinaryVisitorBase<SimpleBinaryVisitor4> |
||
157 |
{ |
||
158 |
template<typename JointModel1, typename JointModel2> |
||
159 |
42 |
static void algo(const pinocchio::JointModelBase<JointModel1> & jmodel1, |
|
160 |
const pinocchio::JointModelBase<JointModel2> & jmodel2, |
||
161 |
pinocchio::JointDataBase<typename JointModel1::JointDataDerived> & jdata1, |
||
162 |
pinocchio::JointDataBase<typename JointModel2::JointDataDerived> & jdata2) |
||
163 |
{ |
||
164 |
PINOCCHIO_UNUSED_VARIABLE(jdata1); |
||
165 |
PINOCCHIO_UNUSED_VARIABLE(jdata2); |
||
166 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
42 |
BOOST_CHECK(!jmodel1.shortname().empty()); |
167 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
42 |
BOOST_CHECK(!jmodel2.shortname().empty()); |
168 |
✓✗✓✗ |
42 |
std::cout << "joint1 name: " << jmodel1.shortname() << std::endl; |
169 |
✓✗✓✗ |
42 |
std::cout << "joint2 name: " << jmodel2.shortname() << std::endl; |
170 |
✓✗✓✗ |
42 |
std::cout << "jdata1 name: " << jdata1.classname() << std::endl; |
171 |
✓✗✓✗ |
42 |
std::cout << "jdata2 name: " << jdata2.classname() << std::endl; |
172 |
} |
||
173 |
}; |
||
174 |
|||
175 |
BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) |
||
176 |
|||
177 |
template<typename JointModel_> struct init; |
||
178 |
|||
179 |
template<typename JointModel_> |
||
180 |
struct init |
||
181 |
{ |
||
182 |
56 |
static JointModel_ run(const pinocchio::Model &/* model*/) |
|
183 |
{ |
||
184 |
56 |
JointModel_ jmodel; |
|
185 |
56 |
return jmodel; |
|
186 |
} |
||
187 |
}; |
||
188 |
|||
189 |
template<typename Scalar, int Options> |
||
190 |
struct init<pinocchio::JointModelRevoluteUnalignedTpl<Scalar,Options> > |
||
191 |
{ |
||
192 |
typedef pinocchio::JointModelRevoluteUnalignedTpl<Scalar,Options> JointModel; |
||
193 |
|||
194 |
2 |
static JointModel run(const pinocchio::Model &/* model*/) |
|
195 |
{ |
||
196 |
typedef typename JointModel::Vector3 Vector3; |
||
197 |
✓✗✓✗ |
2 |
JointModel jmodel(Vector3::Random().normalized()); |
198 |
|||
199 |
2 |
return jmodel; |
|
200 |
} |
||
201 |
}; |
||
202 |
|||
203 |
template<typename Scalar, int Options> |
||
204 |
struct init<pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> > |
||
205 |
{ |
||
206 |
typedef pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> JointModel; |
||
207 |
|||
208 |
2 |
static JointModel run(const pinocchio::Model &/* model*/) |
|
209 |
{ |
||
210 |
typedef typename JointModel::Vector3 Vector3; |
||
211 |
✓✗✓✗ |
2 |
JointModel jmodel(Vector3::Random().normalized()); |
212 |
|||
213 |
2 |
return jmodel; |
|
214 |
} |
||
215 |
}; |
||
216 |
|||
217 |
template<typename Scalar, int Options> |
||
218 |
struct init<pinocchio::JointModelPrismaticUnalignedTpl<Scalar,Options> > |
||
219 |
{ |
||
220 |
typedef pinocchio::JointModelPrismaticUnalignedTpl<Scalar,Options> JointModel; |
||
221 |
|||
222 |
2 |
static JointModel run(const pinocchio::Model &/* model*/) |
|
223 |
{ |
||
224 |
typedef typename JointModel::Vector3 Vector3; |
||
225 |
✓✗✓✗ |
2 |
JointModel jmodel(Vector3::Random().normalized()); |
226 |
|||
227 |
2 |
return jmodel; |
|
228 |
} |
||
229 |
}; |
||
230 |
|||
231 |
template<typename Scalar, int Options, template<typename,int> class JointCollection> |
||
232 |
struct init<pinocchio::JointModelTpl<Scalar,Options,JointCollection> > |
||
233 |
{ |
||
234 |
typedef pinocchio::JointModelTpl<Scalar,Options,JointCollection> JointModel; |
||
235 |
|||
236 |
static JointModel run(const pinocchio::Model &/* model*/) |
||
237 |
{ |
||
238 |
typedef pinocchio::JointModelRevoluteTpl<Scalar,Options,0> JointModelRX; |
||
239 |
JointModel jmodel((JointModelRX())); |
||
240 |
|||
241 |
return jmodel; |
||
242 |
} |
||
243 |
}; |
||
244 |
|||
245 |
template<typename Scalar, int Options, template<typename,int> class JointCollection> |
||
246 |
struct init<pinocchio::JointModelCompositeTpl<Scalar,Options,JointCollection> > |
||
247 |
{ |
||
248 |
typedef pinocchio::JointModelCompositeTpl<Scalar,Options,JointCollection> JointModel; |
||
249 |
|||
250 |
2 |
static JointModel run(const pinocchio::Model &/* model*/) |
|
251 |
{ |
||
252 |
typedef pinocchio::JointModelRevoluteTpl<Scalar,Options,0> JointModelRX; |
||
253 |
typedef pinocchio::JointModelRevoluteTpl<Scalar,Options,1> JointModelRY; |
||
254 |
typedef pinocchio::JointModelRevoluteTpl<Scalar,Options,2> JointModelRZ; |
||
255 |
|||
256 |
✓✗✓✗ |
2 |
JointModel jmodel(JointModelRX(),pinocchio::SE3::Random()); |
257 |
✓✗✓✗ ✓✗ |
2 |
jmodel.addJoint(JointModelRY(),pinocchio::SE3::Random()); |
258 |
✓✗✓✗ ✓✗ |
2 |
jmodel.addJoint(JointModelRZ(),pinocchio::SE3::Random()); |
259 |
|||
260 |
2 |
return jmodel; |
|
261 |
} |
||
262 |
}; |
||
263 |
|||
264 |
template<typename JointModel_> |
||
265 |
struct init<pinocchio::JointModelMimic<JointModel_> > |
||
266 |
{ |
||
267 |
typedef pinocchio::JointModelMimic<JointModel_> JointModel; |
||
268 |
|||
269 |
12 |
static JointModel run(const pinocchio::Model & model) |
|
270 |
{ |
||
271 |
✓✗ | 12 |
const pinocchio::JointIndex joint_id = model.getJointId(JointModel_::classname()); |
272 |
|||
273 |
✓✗✓✗ |
12 |
JointModel jmodel(boost::get<JointModel_>(model.joints[joint_id]),1.,0.); |
274 |
|||
275 |
12 |
return jmodel; |
|
276 |
} |
||
277 |
}; |
||
278 |
|||
279 |
struct AppendJointToModel |
||
280 |
{ |
||
281 |
2 |
AppendJointToModel(pinocchio::Model & model) : model(model) {} |
|
282 |
|||
283 |
template<typename JointModel> |
||
284 |
84 |
void operator()(const pinocchio::JointModelBase<JointModel> &) const |
|
285 |
{ |
||
286 |
✓✗ | 84 |
JointModel jmodel = init<JointModel>::run(model); |
287 |
✓✗✓✗ ✓✗✓✗ |
84 |
model.addJoint(model.joints.size()-1,jmodel, |
288 |
pinocchio::SE3::Random(),jmodel.classname()); |
||
289 |
84 |
} |
|
290 |
|||
291 |
pinocchio::Model & model; |
||
292 |
}; |
||
293 |
|||
294 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_run_over_all_joints_unary_visitor) |
295 |
{ |
||
296 |
using namespace pinocchio; |
||
297 |
|||
298 |
typedef JointCollectionDefault::JointModelVariant Variant; |
||
299 |
|||
300 |
✓✗ | 4 |
Model model; |
301 |
✓✗ | 2 |
boost::mpl::for_each<Variant::types>(AppendJointToModel(model)); |
302 |
✓✗ | 4 |
Data data(model); |
303 |
|||
304 |
✓✓ | 44 |
for( Model::JointIndex i=1;i<(Model::JointIndex)model.njoints;++i ) |
305 |
{ |
||
306 |
✓✗ | 42 |
SimpleUnaryVisitor1::run(model.joints[i],data.joints[i], |
307 |
✓✗ | 84 |
SimpleUnaryVisitor1::ArgsType(model,data,i)); |
308 |
} |
||
309 |
|||
310 |
✓✓ | 44 |
for( Model::JointIndex i=1;i<(Model::JointIndex)model.njoints;++i ) |
311 |
{ |
||
312 |
✓✗ | 42 |
SimpleUnaryVisitor2::run(model.joints[i], |
313 |
✓✗ | 84 |
SimpleUnaryVisitor2::ArgsType(model,data,i)); |
314 |
} |
||
315 |
|||
316 |
✓✓ | 44 |
for( Model::JointIndex i=1;i<(Model::JointIndex)model.njoints;++i ) |
317 |
{ |
||
318 |
✓✗ | 42 |
SimpleUnaryVisitor3::run(model.joints[i]); |
319 |
} |
||
320 |
|||
321 |
✓✓ | 44 |
for( Model::JointIndex i=1;i<(Model::JointIndex)model.njoints;++i ) |
322 |
{ |
||
323 |
✓✗ | 42 |
SimpleUnaryVisitor4::run(model.joints[i],data.joints[i]); |
324 |
} |
||
325 |
2 |
} |
|
326 |
|||
327 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_run_over_all_joints_binary_visitor) |
328 |
{ |
||
329 |
using namespace pinocchio; |
||
330 |
|||
331 |
typedef JointCollectionDefault::JointModelVariant Variant; |
||
332 |
|||
333 |
✓✗ | 4 |
Model model; |
334 |
✓✗ | 2 |
boost::mpl::for_each<Variant::types>(AppendJointToModel(model)); |
335 |
✓✗ | 4 |
Data data(model); |
336 |
|||
337 |
✓✓ | 44 |
for( Model::JointIndex i=1;i<(Model::JointIndex)model.njoints;++i ) |
338 |
{ |
||
339 |
✓✗ | 42 |
SimpleBinaryVisitor1::run(model.joints[i],model.joints[i], |
340 |
42 |
data.joints[i],data.joints[i], |
|
341 |
✓✗ | 84 |
SimpleBinaryVisitor1::ArgsType(model,data,i)); |
342 |
} |
||
343 |
|||
344 |
✓✓ | 44 |
for( Model::JointIndex i=1;i<(Model::JointIndex)model.njoints;++i ) |
345 |
{ |
||
346 |
✓✗ | 42 |
SimpleBinaryVisitor2::run(model.joints[i],model.joints[i], |
347 |
✓✗ | 84 |
SimpleBinaryVisitor2::ArgsType(model,data,i)); |
348 |
} |
||
349 |
|||
350 |
✓✓ | 44 |
for( Model::JointIndex i=1;i<(Model::JointIndex)model.njoints;++i ) |
351 |
{ |
||
352 |
✓✗ | 42 |
SimpleBinaryVisitor3::run(model.joints[i],model.joints[i]); |
353 |
} |
||
354 |
|||
355 |
✓✓ | 44 |
for( Model::JointIndex i=1;i<(Model::JointIndex)model.njoints;++i ) |
356 |
{ |
||
357 |
✓✗ | 42 |
SimpleBinaryVisitor4::run(model.joints[i],model.joints[i], |
358 |
42 |
data.joints[i],data.joints[i]); |
|
359 |
} |
||
360 |
2 |
} |
|
361 |
|||
362 |
BOOST_AUTO_TEST_SUITE_END() |
Generated by: GCOVR (Version 4.2) |