unittest/serialization.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2019-2021 INRIA
3 //
4 
5 #include "pinocchio/multibody/data.hpp"
6 #include "pinocchio/algorithm/joint-configuration.hpp"
7 #include "pinocchio/algorithm/kinematics.hpp"
8 #include "pinocchio/algorithm/geometry.hpp"
9 
10 #include "pinocchio/serialization/fwd.hpp"
11 #include "pinocchio/serialization/archive.hpp"
12 
13 #include "pinocchio/serialization/spatial.hpp"
14 
15 #include "pinocchio/serialization/frame.hpp"
16 
17 #include "pinocchio/serialization/joints.hpp"
18 #include "pinocchio/serialization/model.hpp"
19 #include "pinocchio/serialization/data.hpp"
20 
21 #include "pinocchio/serialization/geometry.hpp"
22 
23 #include "pinocchio/parsers/sample-models.hpp"
24 
25 #include <iostream>
26 
27 #include <boost/test/unit_test.hpp>
28 #include <boost/utility/binary.hpp>
29 
30 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
31 
32 template<typename T1, typename T2 = T1>
34 {
35  static bool run(const T1 & v1, const T2 & v2)
36  {
37  return v1 == v2;
38  }
39 };
40 
41 template<typename T>
42 bool run_call_equality_op(const T & v1, const T & v2)
43 {
44  return call_equality_op<T,T>::run(v1,v2);
45 }
46 
47 // Bug fix in Eigen::Tensor
48 #ifdef PINOCCHIO_WITH_EIGEN_TENSOR_MODULE
49 template<typename Scalar, int NumIndices, int Options, typename IndexType>
50 struct call_equality_op< pinocchio::Tensor<Scalar,NumIndices,Options,IndexType> >
51 {
53 
54  static bool run(const T & v1, const T & v2)
55  {
56  typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> VectorXd;
57  Eigen::Map<const VectorXd> map1(v1.data(),v1.size(),1);
58  Eigen::Map<const VectorXd> map2(v2.data(),v2.size(),1);
59  return map1 == map2;
60  }
61 };
62 #endif
63 
64 template<typename T>
65 void generic_test(const T & object,
66  const std::string & filename,
67  const std::string & tag_name)
68 {
69  using namespace pinocchio::serialization;
70 
71  // Load and save as TXT
72  const std::string txt_filename = filename + ".txt";
73  saveToText(object,txt_filename);
74 
75  {
76  T object_loaded;
77  loadFromText(object_loaded,txt_filename);
78 
79  // Check
80  BOOST_CHECK(run_call_equality_op(object_loaded,object));
81  }
82 
83  // Load and save as string stream (TXT format)
84  std::stringstream ss_out;
85  saveToStringStream(object,ss_out);
86 
87  {
88  T object_loaded;
89  std::istringstream is(ss_out.str());
90  loadFromStringStream(object_loaded,is);
91 
92  // Check
93  BOOST_CHECK(run_call_equality_op(object_loaded,object));
94  }
95 
96  // Load and save as string
97  std::string str_out = saveToString(object);
98 
99  {
100  T object_loaded;
101  std::string str_in(str_out);
102  loadFromString(object_loaded,str_in);
103 
104  // Check
105  BOOST_CHECK(run_call_equality_op(object_loaded,object));
106  }
107 
108  // Load and save as XML
109  const std::string xml_filename = filename + ".xml";
110  saveToXML(object,xml_filename,tag_name);
111 
112  {
113  T object_loaded;
114  loadFromXML(object_loaded,xml_filename,tag_name);
115 
116  // Check
117  BOOST_CHECK(run_call_equality_op(object_loaded,object));
118  }
119 
120  // Load and save as binary
121  const std::string bin_filename = filename + ".bin";
122  saveToBinary(object,bin_filename);
123 
124  {
125  T object_loaded;
126  loadFromBinary(object_loaded,bin_filename);
127 
128  // Check
129  BOOST_CHECK(run_call_equality_op(object_loaded,object));
130  }
131 
132  // Load and save as binary stream
133  boost::asio::streambuf buffer;
134  saveToBinary(object,buffer);
135 
136  {
137  T object_loaded;
138  loadFromBinary(object_loaded,buffer);
139 
140  // Check
141  BOOST_CHECK(run_call_equality_op(object_loaded,object));
142  }
143 
144  // Load and save as static binary stream
145  pinocchio::serialization::StaticBuffer static_buffer(10000000);
146  saveToBinary(object,static_buffer);
147 
148  {
149  T object_loaded;
150  loadFromBinary(object_loaded,static_buffer);
151 
152  // Check
153  BOOST_CHECK(run_call_equality_op(object_loaded,object));
154  }
155 }
156 
157 BOOST_AUTO_TEST_CASE(test_static_buffer)
158 {
159  using namespace pinocchio::serialization;
160  const size_t size = 10000000;
161  StaticBuffer static_buffer(size);
162  BOOST_CHECK(size == static_buffer.size());
163 
164  const size_t new_size = 2*size;
165  static_buffer.resize(new_size);
166  BOOST_CHECK(new_size == static_buffer.size());
167 
168  BOOST_CHECK(static_buffer.data() != NULL);
169  BOOST_CHECK(reinterpret_cast<const StaticBuffer &>(static_buffer).data() != NULL);
170  BOOST_CHECK(reinterpret_cast<const StaticBuffer &>(static_buffer).data() == static_buffer.data());
171 }
172 
173 BOOST_AUTO_TEST_CASE(test_eigen_serialization)
174 {
175  using namespace pinocchio;
176 
177  const Eigen::DenseIndex num_cols = 10;
178  const Eigen::DenseIndex num_rows = 20;
179 
180  const Eigen::DenseIndex array_size = 3;
181 
182  Eigen::MatrixXd Mat = Eigen::MatrixXd::Random(num_rows,num_cols);
183  generic_test(Mat,TEST_SERIALIZATION_FOLDER"/eigen_matrix","matrix");
184 
185  Eigen::VectorXd Vec = Eigen::VectorXd::Random(num_rows*num_cols);
186  generic_test(Vec,TEST_SERIALIZATION_FOLDER"/eigen_vector","vector");
187 
188  Eigen::array<Eigen::DenseIndex,array_size> array = { 1, 2, 3 };
189  generic_test(array,TEST_SERIALIZATION_FOLDER"/eigen_array","array");
190 
191  const Eigen::DenseIndex tensor_size = 3;
192  const Eigen::DenseIndex x_dim = 10, y_dim = 20, z_dim = 30;
193 
194  typedef pinocchio::Tensor<double,tensor_size> Tensor3x;
195  Tensor3x tensor(x_dim,y_dim,z_dim);
196 
197  Eigen::Map<Eigen::VectorXd>(tensor.data(),tensor.size(),1).setRandom();
198 
199  generic_test(tensor,TEST_SERIALIZATION_FOLDER"/eigen_tensor","tensor");
200 }
201 
202 BOOST_AUTO_TEST_CASE(test_spatial_serialization)
203 {
204  using namespace pinocchio;
205 
206  SE3 M(SE3::Random());
207  generic_test(M,TEST_SERIALIZATION_FOLDER"/SE3","SE3");
208 
209  Motion m(Motion::Random());
210  generic_test(m,TEST_SERIALIZATION_FOLDER"/Motion","Motion");
211 
212  Force f(Force::Random());
213  generic_test(f,TEST_SERIALIZATION_FOLDER"/Force","Force");
214 
215  Symmetric3 S(Symmetric3::Random());
216  generic_test(S,TEST_SERIALIZATION_FOLDER"/Symmetric3","Symmetric3");
217 
218  Inertia I(Inertia::Random());
219  generic_test(I,TEST_SERIALIZATION_FOLDER"/Inertia","Inertia");
220 }
221 
222 BOOST_AUTO_TEST_CASE(test_multibody_serialization)
223 {
224  using namespace pinocchio;
225 
226  Frame frame("frame",0,0,SE3::Random(),SENSOR);
227  generic_test(frame,TEST_SERIALIZATION_FOLDER"/Frame","Frame");
228 }
229 
230 template<typename JointModel_> struct init;
231 
232 template<typename JointModel_>
233 struct init
234 {
235  static JointModel_ run()
236  {
237  JointModel_ jmodel;
238  jmodel.setIndexes(0,0,0);
239  return jmodel;
240  }
241 };
242 
243 template<typename Scalar, int Options>
244 struct init<pinocchio::JointModelRevoluteUnalignedTpl<Scalar,Options> >
245 {
247 
248  static JointModel run()
249  {
250  typedef typename JointModel::Vector3 Vector3;
251  JointModel jmodel(Vector3::Random().normalized());
252 
253  jmodel.setIndexes(0,0,0);
254  return jmodel;
255  }
256 };
257 
258 template<typename Scalar, int Options>
259 struct init<pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> >
260 {
262 
263  static JointModel run()
264  {
265  typedef typename JointModel::Vector3 Vector3;
266  JointModel jmodel(Vector3::Random().normalized());
267 
268  jmodel.setIndexes(0,0,0);
269  return jmodel;
270  }
271 };
272 
273 template<typename Scalar, int Options>
274 struct init<pinocchio::JointModelPrismaticUnalignedTpl<Scalar,Options> >
275 {
277 
278  static JointModel run()
279  {
280  typedef typename JointModel::Vector3 Vector3;
281  JointModel jmodel(Vector3::Random().normalized());
282 
283  jmodel.setIndexes(0,0,0);
284  return jmodel;
285  }
286 };
287 
288 template<typename Scalar, int Options, template<typename,int> class JointCollection>
289 struct init<pinocchio::JointModelTpl<Scalar,Options,JointCollection> >
290 {
292 
293  static JointModel run()
294  {
296  JointModel jmodel((JointModelRX()));
297 
298  jmodel.setIndexes(0,0,0);
299  return jmodel;
300  }
301 };
302 
303 template<typename Scalar, int Options, template<typename,int> class JointCollection>
304 struct init<pinocchio::JointModelCompositeTpl<Scalar,Options,JointCollection> >
305 {
307 
308  static JointModel run()
309  {
312  JointModel jmodel((JointModelRX()));
313  jmodel.addJoint(JointModelRY());
314 
315  jmodel.setIndexes(0,0,0);
316  return jmodel;
317  }
318 };
319 
320 template<typename JointModel_>
321 struct init<pinocchio::JointModelMimic<JointModel_> >
322 {
324 
325  static JointModel run()
326  {
327  JointModel_ jmodel_ref = init<JointModel_>::run();
328 
329  JointModel jmodel(jmodel_ref,1.,0.);
330 
331  return jmodel;
332  }
333 };
334 
335 struct TestJointModel
336 {
337  template <typename JointModel>
339  {
341  test(jmodel);
342  }
343 
344  template<typename JointType>
345  static void test(JointType & jmodel)
346  {
347  generic_test(jmodel,TEST_SERIALIZATION_FOLDER"/Joint","jmodel");
348  }
349 
350 };
351 
352 BOOST_AUTO_TEST_CASE(test_multibody_joints_model_serialization)
353 {
354  using namespace pinocchio;
355  boost::mpl::for_each<JointModelVariant::types>(TestJointModel());
356 }
357 
359 {
360  template <typename JointModel>
362  {
363  typedef typename JointModel::JointDerived JointDerived;
364  typedef typename pinocchio::traits<JointDerived>::Transformation_t Transform;
365  typedef typename pinocchio::traits<JointDerived>::Constraint_t Constraint;
369 
370  JointData jdata = jmodel.createData();
371  JointDataBase & jdata_base = static_cast<JointDataBase &>(jdata);
372 
373  typedef typename pinocchio::LieGroup<JointModel>::type LieGroupType;
374  LieGroupType lg;
375 
376  Eigen::VectorXd lb(Eigen::VectorXd::Constant(jmodel.nq(),-1.));
377  Eigen::VectorXd ub(Eigen::VectorXd::Constant(jmodel.nq(), 1.));
378 
379  Eigen::VectorXd q_random = lg.randomConfiguration(lb,ub);
380 
381  jmodel.calc(jdata,q_random);
382  Transform & m = jdata_base.M();
383  test(m);
384 
385  Constraint & S = jdata_base.S();
386  test(S);
387  }
388 
389  template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
391  {
392  // Do nothing
393  }
394 
395  template<typename JointModel>
397  {
399  typedef typename JointModelMimic::JointDerived JointDerived;
400  typedef typename pinocchio::traits<JointDerived>::Transformation_t Transform;
401  typedef typename pinocchio::traits<JointDerived>::Constraint_t Constraint;
404  JointModelMimic jmodel_mimic = init<JointModelMimic>::run();
406 
407  JointDataMimic jdata_mimic = jmodel_mimic.createData();
408  JointDataBase & jdata_mimic_base = static_cast<JointDataBase &>(jdata_mimic);
409 
410  typedef typename pinocchio::LieGroup<JointModel>::type LieGroupType;
411  LieGroupType lg;
412 
413  Eigen::VectorXd lb(Eigen::VectorXd::Constant(jmodel.nq(),-1.));
414  Eigen::VectorXd ub(Eigen::VectorXd::Constant(jmodel.nq(), 1.));
415 
416  Eigen::VectorXd q_random = lg.randomConfiguration(lb,ub);
417 
418  jmodel_mimic.calc(jdata_mimic,q_random);
419  Transform & m = jdata_mimic_base.M();
420  test(m);
421 
422  Constraint & S = jdata_mimic_base.S();
423  test(S);
424  }
425 
426  template<typename Transform>
427  static void test(Transform & m)
428  {
429  generic_test(m,TEST_SERIALIZATION_FOLDER"/JointTransform","transform");
430  }
431 
432 };
433 
434 BOOST_AUTO_TEST_CASE(test_multibody_joints_transform_serialization)
435 {
436  using namespace pinocchio;
437  boost::mpl::for_each<JointModelVariant::types>(TestJointTransform());
438 }
439 
441 {
442  template <typename JointModel>
444  {
445  typedef typename JointModel::JointDerived JointDerived;
447  typedef typename pinocchio::traits<JointDerived>::Bias_t Bias;
451 
452  JointData jdata = jmodel.createData();
453  JointDataBase & jdata_base = static_cast<JointDataBase &>(jdata);
454 
455  typedef typename pinocchio::LieGroup<JointModel>::type LieGroupType;
456  LieGroupType lg;
457 
458  Eigen::VectorXd lb(Eigen::VectorXd::Constant(jmodel.nq(),-1.));
459  Eigen::VectorXd ub(Eigen::VectorXd::Constant(jmodel.nq(), 1.));
460 
461  Eigen::VectorXd q_random = lg.randomConfiguration(lb,ub);
462  Eigen::VectorXd v_random = Eigen::VectorXd::Random(jmodel.nv());
463 
464  jmodel.calc(jdata,q_random,v_random);
465  Motion & m = jdata_base.v();
466 
467  test(m);
468 
469  Bias & b = jdata_base.c();
470  test(b);
471  }
472 
473  template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
475  {
476  // Do nothing
477  }
478 
479  template<typename JointModel>
481  {
483  typedef typename JointModelMimic::JointDerived JointDerived;
485  typedef typename pinocchio::traits<JointDerived>::Bias_t Bias;
488  JointModelMimic jmodel_mimic = init<JointModelMimic>::run();
490 
491  JointDataMimic jdata_mimic = jmodel_mimic.createData();
492  JointDataBase & jdata_mimic_base = static_cast<JointDataBase &>(jdata_mimic);
493 
494  typedef typename pinocchio::LieGroup<JointModel>::type LieGroupType;
495  LieGroupType lg;
496 
497  Eigen::VectorXd lb(Eigen::VectorXd::Constant(jmodel.nq(),-1.));
498  Eigen::VectorXd ub(Eigen::VectorXd::Constant(jmodel.nq(), 1.));
499 
500  Eigen::VectorXd q_random = lg.randomConfiguration(lb,ub);
501  Eigen::VectorXd v_random = Eigen::VectorXd::Random(jmodel.nv());
502 
503  jmodel_mimic.calc(jdata_mimic,q_random,v_random);
504  Motion & m = jdata_mimic_base.v();
505 
506  test(m);
507 
508  Bias & b = jdata_mimic_base.c();
509  test(b);
510  }
511 
512  template<typename Motion>
513  static void test(Motion & m)
514  {
515  generic_test(m,TEST_SERIALIZATION_FOLDER"/JointMotion","motion");
516  }
517 
518 };
519 
520 BOOST_AUTO_TEST_CASE(test_multibody_joints_motion_serialization)
521 {
522  using namespace pinocchio;
523  boost::mpl::for_each<JointModelVariant::types>(TestJointMotion());
524 }
525 
527 {
528  template <typename JointModel>
530  {
531  typedef typename JointModel::JointDerived JointDerived;
534 
535  JointData jdata = jmodel.createData();
536 
537  typedef typename pinocchio::LieGroup<JointModel>::type LieGroupType;
538  LieGroupType lg;
539 
540  Eigen::VectorXd lb(Eigen::VectorXd::Constant(jmodel.nq(),-1.));
541  Eigen::VectorXd ub(Eigen::VectorXd::Constant(jmodel.nq(), 1.));
542 
543  Eigen::VectorXd q_random = lg.randomConfiguration(lb,ub);
544  Eigen::VectorXd v_random = Eigen::VectorXd::Random(jmodel.nv());
545 
546  jmodel.calc(jdata,q_random,v_random);
547  pinocchio::Inertia::Matrix6 I(pinocchio::Inertia::Matrix6::Identity());
548  jmodel.calc_aba(jdata,I,false);
549  test(jdata);
550  }
551 
552  template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
554  {
556  typedef typename JointModel::JointDerived JointDerived;
558 
559  JointModel jmodel_build = init<JointModel>::run();
560 
562  model.addJoint(0, jmodel_build, pinocchio::SE3::Random(), "model");
563  model.lowerPositionLimit.fill(-1.);
564  model.upperPositionLimit.fill( 1.);
565 
566  JointModel & jmodel = boost::get<JointModel>(model.joints[1]);
568  Eigen::VectorXd v_random = Eigen::VectorXd::Random(model.nv);
569 
570  pinocchio::Data data(model);
571  JointData & jdata = boost::get<JointData>(data.joints[1]);
572 
573  jmodel.calc(jdata,q_random,v_random);
574  pinocchio::Inertia::Matrix6 I(pinocchio::Inertia::Matrix6::Identity());
575  jmodel.calc_aba(jdata,I,false);
576 
577  test(jdata);
578  }
579 
580  template<typename JointModel>
582  {
584  typedef typename JointModelMimic::JointDerived JointDerived;
586  JointModelMimic jmodel_mimic = init<JointModelMimic>::run();
588 
589  JointDataMimic jdata_mimic = jmodel_mimic.createData();
590 
591  typedef typename pinocchio::LieGroup<JointModel>::type LieGroupType;
592  LieGroupType lg;
593 
594  Eigen::VectorXd lb(Eigen::VectorXd::Constant(jmodel.nq(),-1.));
595  Eigen::VectorXd ub(Eigen::VectorXd::Constant(jmodel.nq(), 1.));
596 
597  Eigen::VectorXd q_random = lg.randomConfiguration(lb,ub);
598  Eigen::VectorXd v_random = Eigen::VectorXd::Random(jmodel.nv());
599 
600  jmodel_mimic.calc(jdata_mimic,q_random,v_random);
601  pinocchio::Inertia::Matrix6 I(pinocchio::Inertia::Matrix6::Identity());
602  jmodel_mimic.calc_aba(jdata_mimic,I,false);
603  test(jdata_mimic);
604  }
605 
606  template<typename JointData>
607  static void test(JointData & joint_data)
608  {
609  generic_test(joint_data,TEST_SERIALIZATION_FOLDER"/JointData","data");
610  }
611 
612 };
613 
614 BOOST_AUTO_TEST_CASE(test_multibody_joints_data_serialization)
615 {
616  using namespace pinocchio;
617  boost::mpl::for_each<JointModelVariant::types>(TestJointData());
618 }
619 
620 BOOST_AUTO_TEST_CASE(test_model_serialization)
621 {
622  using namespace pinocchio;
623 
624  Model model;
626 
627  generic_test(model,TEST_SERIALIZATION_FOLDER"/Model","Model");
628 }
629 
630 BOOST_AUTO_TEST_CASE(test_throw_extension)
631 {
632  using namespace pinocchio;
633 
634  Model model;
636 
637  const std::string & fake_filename = "this_is_a_fake_filename";
638 
639  {
640  const std::string complete_filename = fake_filename + ".txt";
641  BOOST_REQUIRE_THROW(loadFromText(model,complete_filename),
642  std::invalid_argument);
643  }
644 
645  saveToText(model,TEST_SERIALIZATION_FOLDER"/model.txt");
646  saveToXML(model,TEST_SERIALIZATION_FOLDER"/model.xml","model");
647  saveToBinary(model,TEST_SERIALIZATION_FOLDER"/model.bin");
648 
649  {
650  const std::string complete_filename = fake_filename + ".txte";
651 
652  BOOST_REQUIRE_THROW(loadFromText(model,complete_filename),
653  std::invalid_argument);
654  }
655 
656  {
657  const std::string complete_filename = fake_filename + ".xmle";
658  BOOST_REQUIRE_THROW(loadFromXML(model,complete_filename,"model"),
659  std::invalid_argument);
660  }
661 
662  {
663  const std::string complete_filename = fake_filename + ".bine";
664  BOOST_REQUIRE_THROW(loadFromBinary(model,complete_filename),
665  std::invalid_argument);
666  }
667 
668 }
669 
670 BOOST_AUTO_TEST_CASE(test_data_serialization)
671 {
672  using namespace pinocchio;
673 
674  Model model;
676 
677  Data data(model);
678 
679  generic_test(data,TEST_SERIALIZATION_FOLDER"/Data","Data");
680 }
681 
682 BOOST_AUTO_TEST_CASE(test_geometry_data_serialization)
683 {
684  using namespace pinocchio;
685 
688  pinocchio::Data data(model);
689 
690 #ifdef PINOCCHIO_WITH_HPP_FCL
692  pinocchio::buildModels::humanoidGeometries(model,geom_model);
694 
695  const Eigen::VectorXd q = pinocchio::neutral(model);
696  pinocchio::forwardKinematics(model,data,q);
697  pinocchio::updateGeometryPlacements(model,data,geom_model,geom_data,q);
698 
699  generic_test(geom_data,TEST_SERIALIZATION_FOLDER"/GeomData","GeomData");
700 #endif
701 
702 
703 }
704 
705 BOOST_AUTO_TEST_SUITE_END()
static void test(Motion &m)
void operator()(const pinocchio::JointModelBase< JointModel > &) const
def test(dtype)
void operator()(const pinocchio::JointModelCompositeTpl< Scalar, Options, JointCollectionTpl > &)
pinocchio::JointModelMimic< JointModel_ > JointModel
JointModelVector joints
Model of joint i, encapsulated in a JointModelAccessor.
void loadFromXML(T &object, const std::string &filename, const std::string &tag_name)
Loads an object from a XML file.
Definition: archive.hpp:179
void operator()(const pinocchio::JointModelBase< JointModel > &) const
q
BOOST_AUTO_TEST_CASE(test_static_buffer)
void saveToStringStream(const T &object, std::stringstream &ss)
Saves an object inside a std::stringstream.
Definition: archive.hpp:129
void calc_aba(JointDataDerived &data, const Eigen::MatrixBase< Matrix6Like > &I, const bool update_I) const
void setIndexes(JointIndex id, int q, int v)
Vec3f b
JointModelDerived & addJoint(const JointModelBase< JointModel > &jmodel, const SE3 &placement=SE3::Identity())
Add a joint to the vector of joints.
std::string saveToString(const T &object)
Saves an object inside a std::string.
Definition: archive.hpp:162
pinocchio::JointModelRevoluteUnboundedUnalignedTpl< Scalar, Options > JointModel
BiasTypeConstRef c() const
A Plucker coordinate frame attached to a parent joint inside a kinematic tree.
JointDataVector joints
Vector of pinocchio::JointData associated to the pinocchio::JointModel stored in model, encapsulated in JointDataAccessor.
JointDataTpl< double > JointData
ConfigVectorType lowerPositionLimit
Lower joint configuration limit.
void saveToText(const T &object, const std::string &filename)
Saves an object inside a TXT file.
Definition: archive.hpp:88
TansformTypeConstRef M() const
MotionTypeConstRef v() const
void operator()(const pinocchio::JointModelBase< JointModel > &) const
void operator()(const pinocchio::JointModelMimic< JointModel > &)
char * data()
Returns the pointer on the data.
Eigen::Matrix< Scalar, 3, 1, _Options > Vector3
void randomConfiguration(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &lowerLimits, const Eigen::MatrixBase< ConfigVectorIn2 > &upperLimits, const Eigen::MatrixBase< ReturnType > &qout)
Generate a configuration vector uniformly sampled among provided limits.
void humanoid(ModelTpl< Scalar, Options, JointCollectionTpl > &model, bool usingFF=true)
Create a 28-DOF kinematic chain of a floating humanoid robot.
void operator()(const pinocchio::JointModelMimic< JointModel > &)
void loadFromBinary(T &object, boost::asio::streambuf &buffer)
void forwardKinematics(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
Update the joint placements according to the current joint configuration.
JointModelRevoluteTpl< double, 0, 1 > JointModelRY
Static buffer with pre-allocated memory.
JointIndex addJoint(const JointIndex parent, const JointModel &joint_model, const SE3 &joint_placement, const std::string &joint_name)
Add a joint to the kinematic tree with infinite bounds.
size_t size() const
Returns the current size of the buffer.
void operator()(const pinocchio::JointModelBase< JointModel > &) const
static void test(JointData &joint_data)
MotionTpl< double, 0 > Motion
void generic_test(const T &object, const std::string &filename, const std::string &tag_name)
void calc_aba(JointDataDerived &data, const Eigen::MatrixBase< Matrix6Like > &I, const bool update_I) const
void loadFromText(T &object, const std::string &filename)
Loads an object from a TXT file.
Definition: archive.hpp:61
Eigen::Matrix< Scalar, 3, 1, _Options > Vector3
static bool run(const T1 &v1, const T2 &v2)
float m
pinocchio::JointModelCompositeTpl< Scalar, Options, JointCollection > JointModel
void resize(const size_t new_size)
Increase the capacity of the vector to a value that&#39;s greater or equal to new_size.
pinocchio::JointModelRevoluteUnalignedTpl< Scalar, Options > JointModel
void setIndexes(JointIndex id, int nq, int nv)
void loadFromString(T &object, const std::string &str)
Loads an object from a std::string.
Definition: archive.hpp:145
void loadFromStringStream(T &object, std::istringstream &is)
Loads an object from a std::stringstream.
Definition: archive.hpp:113
FCL_REAL size() const
void neutral(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ReturnType > &qout)
Return the neutral configuration element related to the model configuration space.
void saveToXML(const T &object, const std::string &filename, const std::string &tag_name)
Saves an object inside a XML file.
Definition: archive.hpp:210
EIGEN_DONT_INLINE void calc(JointDataDerived &jdata, const typename Eigen::MatrixBase< ConfigVector > &qs) const
int nv
Dimension of the velocity vector space.
JointModelTpl< double > JointModel
pinocchio::JointModelPrismaticUnalignedTpl< Scalar, Options > JointModel
Common traits structure to fully define base classes for CRTP.
Definition: src/fwd.hpp:44
static JointModel_ run()
void operator()(const pinocchio::JointModelCompositeTpl< Scalar, Options, JointCollectionTpl > &)
ConstraintTypeConstRef S() const
ConfigVectorType upperPositionLimit
Upper joint configuration limit.
sensor frame: defined in a sensor element
M
pinocchio::JointModelTpl< Scalar, Options, JointCollection > JointModel
bool run_call_equality_op(const T &v1, const T &v2)
JointDataDerived createData() const
void updateGeometryPlacements(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const GeometryModel &geom_model, GeometryData &geom_data, const Eigen::MatrixBase< ConfigVectorType > &q)
Apply a forward kinematics and update the placement of the geometry objects.
void operator()(const pinocchio::JointModelMimic< JointModel > &)
void operator()(const pinocchio::JointModelCompositeTpl< Scalar, Options, JointCollectionTpl > &)
static void test(JointType &jmodel)
void humanoidRandom(ModelTpl< Scalar, Options, JointCollectionTpl > &model, bool usingFF=true)
Create a humanoid kinematic tree with 6-DOF limbs and random joint placements.
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > VectorXd
Definition: conversions.cpp:14
void calc(JointDataDerived &data, const Eigen::MatrixBase< ConfigVector > &q) const
JointModelRevoluteTpl< double, 0, 0 > JointModelRX
JointDataDerived createData() const
static void test(Transform &m)
void saveToBinary(const T &object, boost::asio::streambuf &buffer)


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