visitor.cpp
Go to the documentation of this file.
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 
17 : public pinocchio::fusion::JointUnaryVisitorBase<SimpleUnaryVisitor1>
18 {
19 
20  typedef bf::vector<const pinocchio::Model &,
23 
24  template<typename JointModel>
25  static void algo(const pinocchio::JointModelBase<JointModel> & jmodel,
27  const pinocchio::Model & model,
28  pinocchio::Data & data,
29  pinocchio::JointIndex jindex)
30  {
34  BOOST_CHECK(jindex == jmodel.id());
35  std::cout << "joint name: " << jmodel.shortname() << std::endl;
36  }
37 };
38 
40 : public pinocchio::fusion::JointUnaryVisitorBase<SimpleUnaryVisitor2>
41 {
42 
43  typedef bf::vector<const pinocchio::Model &,
46 
47  template<typename JointModel>
48  static void algo(const pinocchio::JointModelBase<JointModel> & jmodel,
49  const pinocchio::Model & model,
50  pinocchio::Data & data,
51  pinocchio::JointIndex jindex)
52  {
56  BOOST_CHECK(jindex == jmodel.id());
57  std::cout << "joint name: " << jmodel.shortname() << std::endl;
58  }
59 };
60 
62 : public pinocchio::fusion::JointUnaryVisitorBase<SimpleUnaryVisitor3>
63 {
64 
65  template<typename JointModel>
66  static void algo(const pinocchio::JointModelBase<JointModel> & jmodel)
67  {
68  BOOST_CHECK(!jmodel.shortname().empty());
69  std::cout << "joint name: " << jmodel.shortname() << std::endl;
70  }
71 };
72 
74 : public pinocchio::fusion::JointUnaryVisitorBase<SimpleUnaryVisitor4>
75 {
76 
77  template<typename JointModel>
78  static void algo(const pinocchio::JointModelBase<JointModel> & jmodel,
80  {
82  BOOST_CHECK(!jmodel.shortname().empty());
83  std::cout << "joint name: " << jmodel.shortname() << std::endl;
84  }
85 };
86 
88 : public pinocchio::fusion::JointBinaryVisitorBase<SimpleBinaryVisitor1>
89 {
90 
91  typedef bf::vector<const pinocchio::Model &,
94 
95  template<typename JointModel1, typename JointModel2>
96  static void algo(const pinocchio::JointModelBase<JointModel1> & jmodel1,
100  const pinocchio::Model & model,
101  pinocchio::Data & data,
102  pinocchio::JointIndex jindex)
103  {
109  BOOST_CHECK(jindex == jmodel1.id());
110  BOOST_CHECK(jindex == jmodel2.id());
111  std::cout << "joint1 name: " << jmodel1.shortname() << std::endl;
112  std::cout << "joint2 name: " << jmodel2.shortname() << std::endl;
113  }
114 };
115 
117 : public pinocchio::fusion::JointBinaryVisitorBase<SimpleBinaryVisitor2>
118 {
119 
120  typedef bf::vector<const pinocchio::Model &,
121  pinocchio::Data &,
123 
124  template<typename JointModel1, typename JointModel2>
125  static void algo(const pinocchio::JointModelBase<JointModel1> & jmodel1,
127  const pinocchio::Model & model,
128  pinocchio::Data & data,
129  pinocchio::JointIndex jindex)
130  {
134  BOOST_CHECK(jindex == jmodel1.id());
135  BOOST_CHECK(jindex == jmodel2.id());
136  std::cout << "joint1 name: " << jmodel1.shortname() << std::endl;
137  std::cout << "joint2 name: " << jmodel2.shortname() << std::endl;
138  }
139 };
140 
142 : public pinocchio::fusion::JointBinaryVisitorBase<SimpleBinaryVisitor3>
143 {
144  template<typename JointModel1, typename JointModel2>
145  static void algo(const pinocchio::JointModelBase<JointModel1> & jmodel1,
147  {
148  BOOST_CHECK(!jmodel1.shortname().empty());
149  BOOST_CHECK(!jmodel2.shortname().empty());
150  std::cout << "joint1 name: " << jmodel1.shortname() << std::endl;
151  std::cout << "joint2 name: " << jmodel2.shortname() << std::endl;
152  }
153 };
154 
156 : public pinocchio::fusion::JointBinaryVisitorBase<SimpleBinaryVisitor4>
157 {
158  template<typename JointModel1, typename JointModel2>
159  static void algo(const pinocchio::JointModelBase<JointModel1> & jmodel1,
163  {
166  BOOST_CHECK(!jmodel1.shortname().empty());
167  BOOST_CHECK(!jmodel2.shortname().empty());
168  std::cout << "joint1 name: " << jmodel1.shortname() << std::endl;
169  std::cout << "joint2 name: " << jmodel2.shortname() << std::endl;
170  std::cout << "jdata1 name: " << jdata1.classname() << std::endl;
171  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  static JointModel_ run(const pinocchio::Model &/* model*/)
183  {
184  JointModel_ jmodel;
185  return jmodel;
186  }
187 };
188 
189 template<typename Scalar, int Options>
190 struct init<pinocchio::JointModelRevoluteUnalignedTpl<Scalar,Options> >
191 {
193 
194  static JointModel run(const pinocchio::Model &/* model*/)
195  {
196  typedef typename JointModel::Vector3 Vector3;
197  JointModel jmodel(Vector3::Random().normalized());
198 
199  return jmodel;
200  }
201 };
202 
203 template<typename Scalar, int Options>
204 struct init<pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> >
205 {
207 
208  static JointModel run(const pinocchio::Model &/* model*/)
209  {
210  typedef typename JointModel::Vector3 Vector3;
211  JointModel jmodel(Vector3::Random().normalized());
212 
213  return jmodel;
214  }
215 };
216 
217 template<typename Scalar, int Options>
218 struct init<pinocchio::JointModelPrismaticUnalignedTpl<Scalar,Options> >
219 {
221 
222  static JointModel run(const pinocchio::Model &/* model*/)
223  {
224  typedef typename JointModel::Vector3 Vector3;
225  JointModel jmodel(Vector3::Random().normalized());
226 
227  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 {
235 
236  static JointModel run(const pinocchio::Model &/* model*/)
237  {
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 {
249 
250  static JointModel run(const pinocchio::Model &/* model*/)
251  {
255 
256  JointModel jmodel(JointModelRX(),pinocchio::SE3::Random());
259 
260  return jmodel;
261  }
262 };
263 
264 template<typename JointModel_>
265 struct init<pinocchio::JointModelMimic<JointModel_> >
266 {
268 
269  static JointModel run(const pinocchio::Model & model)
270  {
271  const pinocchio::JointIndex joint_id = model.getJointId(JointModel_::classname());
272 
273  JointModel jmodel(boost::get<JointModel_>(model.joints[joint_id]),1.,0.);
274 
275  return jmodel;
276  }
277 };
278 
280 {
282 
283  template<typename JointModel>
285  {
287  model.addJoint(model.joints.size()-1,jmodel,
288  pinocchio::SE3::Random(),jmodel.classname());
289  }
290 
292 };
293 
294 BOOST_AUTO_TEST_CASE(test_run_over_all_joints_unary_visitor)
295 {
296  using namespace pinocchio;
297 
299 
300  Model model;
301  boost::mpl::for_each<Variant::types>(AppendJointToModel(model));
302  Data data(model);
303 
304  for( Model::JointIndex i=1;i<(Model::JointIndex)model.njoints;++i )
305  {
306  SimpleUnaryVisitor1::run(model.joints[i],data.joints[i],
307  SimpleUnaryVisitor1::ArgsType(model,data,i));
308  }
309 
310  for( Model::JointIndex i=1;i<(Model::JointIndex)model.njoints;++i )
311  {
313  SimpleUnaryVisitor2::ArgsType(model,data,i));
314  }
315 
316  for( Model::JointIndex i=1;i<(Model::JointIndex)model.njoints;++i )
317  {
319  }
320 
321  for( Model::JointIndex i=1;i<(Model::JointIndex)model.njoints;++i )
322  {
323  SimpleUnaryVisitor4::run(model.joints[i],data.joints[i]);
324  }
325 }
326 
327 BOOST_AUTO_TEST_CASE(test_run_over_all_joints_binary_visitor)
328 {
329  using namespace pinocchio;
330 
332 
333  Model model;
334  boost::mpl::for_each<Variant::types>(AppendJointToModel(model));
335  Data data(model);
336 
337  for( Model::JointIndex i=1;i<(Model::JointIndex)model.njoints;++i )
338  {
339  SimpleBinaryVisitor1::run(model.joints[i],model.joints[i],
340  data.joints[i],data.joints[i],
341  SimpleBinaryVisitor1::ArgsType(model,data,i));
342  }
343 
344  for( Model::JointIndex i=1;i<(Model::JointIndex)model.njoints;++i )
345  {
346  SimpleBinaryVisitor2::run(model.joints[i],model.joints[i],
347  SimpleBinaryVisitor2::ArgsType(model,data,i));
348  }
349 
350  for( Model::JointIndex i=1;i<(Model::JointIndex)model.njoints;++i )
351  {
352  SimpleBinaryVisitor3::run(model.joints[i],model.joints[i]);
353  }
354 
355  for( Model::JointIndex i=1;i<(Model::JointIndex)model.njoints;++i )
356  {
357  SimpleBinaryVisitor4::run(model.joints[i],model.joints[i],
358  data.joints[i],data.joints[i]);
359  }
360 }
361 
362 BOOST_AUTO_TEST_SUITE_END()
pinocchio::Model & model
Definition: visitor.cpp:291
bf::vector< const pinocchio::Model &, pinocchio::Data &, pinocchio::JointIndex > ArgsType
Definition: visitor.cpp:122
pinocchio::JointModelMimic< JointModel_ > JointModel
Definition: visitor.cpp:267
JointModelVector joints
Model of joint i, encapsulated in a JointModelAccessor.
#define PINOCCHIO_UNUSED_VARIABLE(var)
Helper to declare that a parameter is unused.
Definition: src/macros.hpp:51
static JointModel_ run(const pinocchio::Model &)
Definition: visitor.cpp:182
JointModelDerived & addJoint(const JointModelBase< JointModel > &jmodel, const SE3 &placement=SE3::Identity())
Add a joint to the vector of joints.
Base structure for Binary visitation of two JointModels. This structure provides runners to call the ...
pinocchio::JointModelRevoluteUnboundedUnalignedTpl< Scalar, Options > JointModel
Definition: visitor.cpp:206
static void algo(const pinocchio::JointModelBase< JointModel1 > &jmodel1, const pinocchio::JointModelBase< JointModel2 > &jmodel2)
Definition: visitor.cpp:145
static void algo(const pinocchio::JointModelBase< JointModel1 > &jmodel1, const pinocchio::JointModelBase< JointModel2 > &jmodel2, pinocchio::JointDataBase< typename JointModel1::JointDataDerived > &jdata1, pinocchio::JointDataBase< typename JointModel2::JointDataDerived > &jdata2)
Definition: visitor.cpp:159
static void algo(const pinocchio::JointModelBase< JointModel > &jmodel)
Definition: visitor.cpp:66
int njoints
Number of joints.
std::string shortname() const
static void algo(const pinocchio::JointModelBase< JointModel1 > &jmodel1, const pinocchio::JointModelBase< JointModel2 > &jmodel2, const pinocchio::Model &model, pinocchio::Data &data, pinocchio::JointIndex jindex)
Definition: visitor.cpp:125
static JointModel run(const pinocchio::Model &)
Definition: visitor.cpp:236
static std::string classname()
static void algo(const pinocchio::JointModelBase< JointModel > &jmodel, pinocchio::JointDataBase< typename JointModel::JointDataDerived > &jdata)
Definition: visitor.cpp:78
BOOST_AUTO_TEST_CASE(test_run_over_all_joints_unary_visitor)
Definition: visitor.cpp:294
Eigen::Matrix< Scalar, 3, 1, _Options > Vector3
static void run(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel, JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata, ArgsTmp args)
static void algo(const pinocchio::JointModelBase< JointModel1 > &jmodel1, const pinocchio::JointModelBase< JointModel2 > &jmodel2, pinocchio::JointDataBase< typename JointModel1::JointDataDerived > &jdata1, pinocchio::JointDataBase< typename JointModel2::JointDataDerived > &jdata2, const pinocchio::Model &model, pinocchio::Data &data, pinocchio::JointIndex jindex)
Definition: visitor.cpp:96
JointModelRevoluteTpl< double, 0, 1 > JointModelRY
static void algo(const pinocchio::JointModelBase< JointModel > &jmodel, pinocchio::JointDataBase< typename JointModel::JointDataDerived > &jdata, const pinocchio::Model &model, pinocchio::Data &data, pinocchio::JointIndex jindex)
Definition: visitor.cpp:25
static JointModel run(const pinocchio::Model &)
Definition: visitor.cpp:194
Base structure for Unary visitation of a JointModel. This structure provides runners to call the righ...
JointModelRevoluteTpl< double, 0, 2 > JointModelRZ
AppendJointToModel(pinocchio::Model &model)
Definition: visitor.cpp:281
pinocchio::JointIndex JointIndex
static void run(const JointModelBase< JointModelDerived1 > &jmodel1, const JointModelBase< JointModelDerived2 > &jmodel2, typename JointModelBase< JointModelDerived1 >::JointDataDerived &jdata1, typename JointModelBase< JointModelDerived2 >::JointDataDerived &jdata2, ArgsTmp args)
boost::variant< JointModelRX, JointModelRY, JointModelRZ, JointModelMimicRX, JointModelMimicRY, JointModelMimicRZ, JointModelFreeFlyer, JointModelPlanar, JointModelRevoluteUnaligned, JointModelSpherical, JointModelSphericalZYX, JointModelPX, JointModelPY, JointModelPZ, JointModelPrismaticUnaligned, JointModelTranslation, JointModelRUBX, JointModelRUBY, JointModelRUBZ, JointModelRevoluteUnboundedUnaligned, boost::recursive_wrapper< JointModelComposite > > JointModelVariant
#define BOOST_TEST_MODULE
Eigen::Matrix< Scalar, 3, 1, _Options > Vector3
pinocchio::JointModelCompositeTpl< Scalar, Options, JointCollection > JointModel
Definition: visitor.cpp:248
pinocchio::JointModelRevoluteUnalignedTpl< Scalar, Options > JointModel
Definition: visitor.cpp:192
void operator()(const pinocchio::JointModelBase< JointModel > &) const
Definition: visitor.cpp:284
bf::vector< const pinocchio::Model &, pinocchio::Data &, pinocchio::JointIndex > ArgsType
Definition: visitor.cpp:22
static JointModel run(const pinocchio::Model &model)
Definition: visitor.cpp:269
JointIndex getJointId(const std::string &name) const
Return the index of a joint given by its name.
JointModelTpl< double > JointModel
pinocchio::JointModelPrismaticUnalignedTpl< Scalar, Options > JointModel
Definition: visitor.cpp:220
static JointModel_ run()
Definition: all-joints.cpp:24
pinocchio::JointModelTpl< Scalar, Options, JointCollection > JointModel
Definition: visitor.cpp:234
static void algo(const pinocchio::JointModelBase< JointModel > &jmodel, const pinocchio::Model &model, pinocchio::Data &data, pinocchio::JointIndex jindex)
Definition: visitor.cpp:48
JointModelRevoluteTpl< double, 0, 0 > JointModelRX
bf::vector< const pinocchio::Model &, pinocchio::Data &, pinocchio::JointIndex > ArgsType
Definition: visitor.cpp:93
bf::vector< const pinocchio::Model &, pinocchio::Data &, pinocchio::JointIndex > ArgsType
Definition: visitor.cpp:45


pinocchio
Author(s):
autogenerated on Fri Jun 23 2023 02:38:33