mjcf.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2015-2024 CNRS INRIA
3 //
4 
5 #include <iostream>
6 #include <cstdio> // for std::tmpnam
7 
8 #include "pinocchio/multibody/model.hpp"
9 
10 #include "pinocchio/parsers/mjcf.hpp"
11 #include "pinocchio/parsers/urdf.hpp"
12 
13 #include "pinocchio/algorithm/joint-configuration.hpp"
14 #include "pinocchio/algorithm/frames.hpp"
15 
16 #include "pinocchio/multibody/joint/joints.hpp"
17 
18 #include <boost/test/unit_test.hpp>
19 #include <boost/filesystem/fstream.hpp>
20 
21 namespace
22 {
23 
24  struct TemporaryFileHolder
25  {
26  TemporaryFileHolder()
27  : path(boost::filesystem::temp_directory_path() / boost::filesystem::unique_path())
28  {
29  }
30 
31  ~TemporaryFileHolder()
32  {
33  boost::filesystem::remove(path);
34  }
35 
36  TemporaryFileHolder(const TemporaryFileHolder &) = delete;
37  TemporaryFileHolder & operator=(const TemporaryFileHolder &) = delete;
38 
39  TemporaryFileHolder(TemporaryFileHolder &&) = default;
40  TemporaryFileHolder & operator=(TemporaryFileHolder &&) = default;
41 
42  std::string name() const
43  {
44  return path.string();
45  }
46 
47  boost::filesystem::path path;
48  };
49 
50 } // namespace
51 
52 static TemporaryFileHolder createTempFile(const std::istringstream & data)
53 {
54  TemporaryFileHolder temp_file;
55 
56  // Write the XML data to the temporary file
57  boost::filesystem::ofstream file_stream(temp_file.path);
58  if (!file_stream)
59  {
60  std::cerr << "Error opening file for writing" << std::endl;
61  exit(EXIT_FAILURE);
62  }
63  file_stream << data.rdbuf();
64  file_stream.close();
65 
66  return temp_file;
67 }
68 
69 static bool comparePropertyTrees(
70  const boost::property_tree::ptree & pt1, const boost::property_tree::ptree & pt2)
71 {
72  // Check if the number of children is the same
73  if (pt1.size() != pt2.size())
74  return false;
75 
76  // Iterate over all children
77  for (auto it1 = pt1.begin(), it2 = pt2.begin(); it1 != pt1.end(); ++it1, ++it2)
78  {
79  // Compare keys
80  if (it1->first != it2->first)
81  return false;
82 
83  // Compare values
84  if (it1->second.data() != it2->second.data())
85  return false;
86 
87  // Recursively compare child trees
88  if (!comparePropertyTrees(it1->second, it2->second))
89  return false;
90  }
91 
92  return true;
93 }
94 
95 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
96 
97 // /// @brief Test for the inertia conversion from mjcf to pinocchio inertia
98 // /// @param
99 BOOST_AUTO_TEST_CASE(convert_inertia_fullinertia)
100 {
101  typedef pinocchio::SE3::Vector3 Vector3;
102  typedef pinocchio::SE3::Matrix3 Matrix3;
103  // Parse the XML
104  std::istringstream xmlData(R"(
105  <inertial mass="0.629769" pos="-0.041018 -0.00014 0.049974"
106  fullinertia="0.00315 0.00388 0.004285 8.2904e-7 0.00015 8.2299e-6"/>
107  )");
108 
109  // Create a Boost Property Tree
110  boost::property_tree::ptree pt;
111  boost::property_tree::read_xml(xmlData, pt);
112 
113  typedef ::pinocchio::mjcf::details::MjcfGraph MjcfGraph;
114  pinocchio::Model model;
115  MjcfGraph::UrdfVisitor visitor(model);
116 
117  MjcfGraph graph(visitor, "fakeMjcf");
118 
119  // // Try to get the "name" element using get_optional
120  pinocchio::Inertia inertia = graph.convertInertiaFromMjcf(pt.get_child("inertial"));
121 
122  Matrix3 inertia_matrix;
123  // M(1,1), M(2,2), M(3,3), M(1,2), M(1,3), M(2,3)
124  inertia_matrix << 0.00315, 8.2904e-7, 0.00015, 8.2904e-7, 0.00388, 8.2299e-6, 0.00015, 8.2299e-6,
125  0.004285;
126 
127  pinocchio::Inertia real_inertia(0.629769, Vector3(-0.041018, -0.00014, 0.049974), inertia_matrix);
128  BOOST_CHECK(inertia.isEqual(real_inertia));
129 }
130 
131 // /// @brief Test for the inertia conversion from mjcf to pinocchio inertia
132 // /// @param
133 BOOST_AUTO_TEST_CASE(convert_inertia_diaginertia)
134 {
135  typedef pinocchio::SE3::Vector3 Vector3;
136  typedef pinocchio::SE3::Matrix3 Matrix3;
137  // Parse the XML
138  std::istringstream xmlData(R"(
139  <inertial mass="0.629769" pos="-0.041018 -0.00014 0.049974"
140  diaginertia="0.00315 0.00388 0.004285"/>
141  )");
142 
143  // Create a Boost Property Tree
144  boost::property_tree::ptree pt;
145  boost::property_tree::read_xml(xmlData, pt);
146 
147  typedef ::pinocchio::mjcf::details::MjcfGraph MjcfGraph;
148  pinocchio::Model model;
149  MjcfGraph::UrdfVisitor visitor(model);
150 
151  MjcfGraph graph(visitor, "fakeMjcf");
152 
153  // // Try to get the "name" element using get_optional
154  pinocchio::Inertia inertia = graph.convertInertiaFromMjcf(pt.get_child("inertial"));
155 
156  Matrix3 inertia_matrix = Eigen::Matrix3d::Zero();
157  inertia_matrix(0, 0) = 0.00315;
158  inertia_matrix(1, 1) = 0.00388;
159  inertia_matrix(2, 2) = 0.004285;
160  pinocchio::Inertia real_inertia(0.629769, Vector3(-0.041018, -0.00014, 0.049974), inertia_matrix);
161 
162  BOOST_CHECK(inertia.isApprox(real_inertia, 1e-7));
163 }
164 
166 BOOST_AUTO_TEST_CASE(geoms_construction)
167 {
168  double pi = boost::math::constants::pi<double>();
169  // Parse the XML
170  std::istringstream xmlData(R"(<mujoco model="inertiaFromGeom">
171  <compiler inertiafromgeom="true" />
172  <worldbody>
173  <body pos="0 0 0" name="bodyCylinder">
174  <geom type="cylinder" size=".01" fromto="0 0 0 0 0 .5"/>
175  <geom type="cylinder" size=".01 0.25" pos="0 0 0" quat="1 0 0 0"/>
176  <body pos="0 0 0" name="bodyBox">
177  <geom type="box" size=".01" fromto="0 0 0 0 0 .5"/>
178  <geom type="box" size=".01 0.01 0.25" pos="0 0 0" quat="1 0 0 0"/>
179  </body>
180  <body pos="0 0 0" name="bodyCapsule">
181  <geom type="capsule" size=".01" fromto="0 0 0 0 0 .5"/>
182  <geom type="capsule" size=".01 0.25" pos="0 0 0" quat="1 0 0 0"/>
183  </body>
184  <body pos="0 0 0" name="bodySphere">
185  <geom type="sphere" size=".01 0.25" pos="0 0 0" quat="1 0 0 0"/>
186  </body>
187  <body pos="0 0 0" name="bodyEllip">
188  <geom type="ellipsoid" size=".01" fromto="0 0 0 0 0 .5"/>
189  <geom type="ellipsoid" size=".01 0.01 0.25" pos="0 0 0" quat="1 0 0 0"/>
190  </body>
191  </body>
192  </worldbody>
193  </mujoco>)");
194 
195  auto namefile = createTempFile(xmlData);
196 
197  typedef ::pinocchio::mjcf::details::MjcfGraph MjcfGraph;
198  pinocchio::Model model_m;
199  MjcfGraph::UrdfVisitor visitor(model_m);
200 
201  MjcfGraph graph(visitor, "fakeMjcf");
202  graph.parseGraphFromXML(namefile.name());
203 
204  // Test Cylinder
205  pinocchio::mjcf::details::MjcfBody bodyTest = graph.mapOfBodies.at("bodyCylinder");
206  double mass = 1000 * std::pow(0.01, 2) * 0.25 * pi * 2;
207  pinocchio::Inertia inertTest = pinocchio::Inertia::FromCylinder(mass, 0.01, 0.25 * 2);
208  Eigen::Vector2d sizeCy;
209  sizeCy << 0.01, 0.25;
210  for (const auto & geom : bodyTest.geomChildren)
211  {
212  // Check size
213  BOOST_CHECK(geom.size == sizeCy);
214  // Check inertia
215  BOOST_CHECK(geom.geomInertia.isApprox(inertTest));
216  }
217  // Test Box
218  bodyTest = graph.mapOfBodies.at("bodyBox");
219  mass = 1000 * 0.02 * 0.02 * 0.5;
220  inertTest = pinocchio::Inertia::FromBox(mass, 0.02, 0.02, 0.5);
221  Eigen::Vector3d sizeB;
222  sizeB << 0.02, 0.02, .5;
223  for (const auto & geom : bodyTest.geomChildren)
224  {
225  // Check size
226  BOOST_CHECK(geom.size == sizeB);
227  // Check inertia
228  BOOST_CHECK(geom.geomInertia.isApprox(inertTest));
229  }
230  // Test Capsule
231  bodyTest = graph.mapOfBodies.at("bodyCapsule");
232  mass = 1000 * (4.0 / 3 * pi * std::pow(0.01, 3) + 2 * pi * std::pow(0.01, 2) * 0.25);
233  inertTest = pinocchio::Inertia::FromCapsule(mass, 0.01, 0.25 * 2);
234  Eigen::Vector2d sizeCa;
235  sizeCa << 0.01, 0.25;
236  for (const auto & geom : bodyTest.geomChildren)
237  {
238  // Check size
239  BOOST_CHECK(geom.size == sizeCa);
240  // Check inertia
241  BOOST_CHECK(geom.geomInertia.isApprox(inertTest));
242  }
243  // Test Sphere
244  bodyTest = graph.mapOfBodies.at("bodySphere");
245  mass = 1000 * 4.0 / 3 * pi * std::pow(0.01, 3);
246  inertTest = pinocchio::Inertia::FromSphere(mass, 0.01);
247  Eigen::VectorXd sizeS(1);
248  sizeS << 0.01;
249  for (const auto & geom : bodyTest.geomChildren)
250  {
251  // Check size
252  BOOST_CHECK(geom.size == sizeS);
253  // Check inertia
254  BOOST_CHECK(geom.geomInertia.isApprox(inertTest));
255  }
256  // Test Ellipsoid
257  bodyTest = graph.mapOfBodies.at("bodyEllip");
258  mass = 1000 * 4.0 / 3 * pi * 0.01 * 0.01 * 0.25;
259  inertTest = pinocchio::Inertia::FromEllipsoid(mass, 0.01, 0.01, 0.25);
260  Eigen::Vector3d sizeEl;
261  sizeEl << 0.01, 0.01, 0.25;
262  for (const auto & geom : bodyTest.geomChildren)
263  {
264  // Check size
265  BOOST_CHECK(geom.size == sizeEl);
266  // Check inertia
267  BOOST_CHECK(geom.geomInertia.isApprox(inertTest));
268  }
269 }
270 
273 BOOST_AUTO_TEST_CASE(inertia_from_geom)
274 {
275  // Parse the XML
276  std::istringstream xmlData(R"(<mujoco model="inertiaFromGeom">
277  <compiler inertiafromgeom="true" />
278  <worldbody>
279  <body pos="0 0 0" name="body0">
280  <geom type="box" size=".01 .01 .01" pos="0 0 0.01"/>
281  <geom type="box" size=".01 .01 .01" pos="0 0 0.03"/>
282  </body>
283  </worldbody>
284  </mujoco>)");
285 
286  auto namefile = createTempFile(xmlData);
287 
288  typedef ::pinocchio::mjcf::details::MjcfGraph MjcfGraph;
289  pinocchio::Model model_m;
290  MjcfGraph::UrdfVisitor visitor(model_m);
291 
292  MjcfGraph graph(visitor, "fakeMjcf");
293  graph.parseGraphFromXML(namefile.name());
294  graph.parseRootTree();
295 
296  // only one inertias since only one body
297  pinocchio::Inertia inertBody = model_m.inertias[0];
298 
299  double massBigBox = 1000 * 0.02 * 0.02 * 0.04; // density * volume
300  pinocchio::Inertia inertBigBox = pinocchio::Inertia::FromBox(massBigBox, 0.02, 0.02, 0.04);
301  // Create a frame on the bottom face of BigBox and express inertia
302  Eigen::Vector3d pl;
303  pl << 0, 0, 0.02;
304  pinocchio::SE3 placementBigBox(Eigen::Matrix3d::Identity(), pl);
305  inertBigBox = placementBigBox.act(inertBigBox);
306 
307  BOOST_CHECK(inertBigBox.isApprox(inertBody));
308 }
309 
310 // /// @brief Test for the pose conversion from mjcf model to pinocchio
311 // /// @param
312 BOOST_AUTO_TEST_CASE(convert_orientation)
313 {
314  typedef pinocchio::SE3::Vector3 Vector3;
315  typedef pinocchio::SE3::Matrix3 Matrix3;
316 
317  std::istringstream xmlData(R"(
318  <compiler angle="radian" eulerseq="xyz"/>
319  <quaternion pos="0.3 0.2 0.5" quat="1 -1 0 0"/>
320  <axis pos="0.3 0.2 0.5" axisangle="-1 0 0 1.5707963"/>
321  <euler pos="0.3 0.2 0.5" euler="-1.57079633 0 0"/>
322  <xyaxes pos="0.3 0.2 0.5" xyaxes="1 0 0 0 0 -1"/>
323  <zaxis pos="0.3 0.2 0.5" zaxis="0 1 0"/>
324  )");
325 
326  // Create a Boost Property Tree
327  boost::property_tree::ptree pt;
328  boost::property_tree::read_xml(xmlData, pt);
329 
330  typedef ::pinocchio::mjcf::details::MjcfGraph MjcfGraph;
331  pinocchio::Model model;
332  MjcfGraph::UrdfVisitor visitor(model);
333 
334  MjcfGraph graph(visitor, "fakeMjcf");
335 
336  graph.parseCompiler(pt.get_child("compiler"));
337 
338  pinocchio::SE3 placement_q = graph.convertPosition(pt.get_child("quaternion"));
339  pinocchio::SE3 placement_a = graph.convertPosition(pt.get_child("axis"));
340  pinocchio::SE3 placement_e = graph.convertPosition(pt.get_child("euler"));
341  pinocchio::SE3 placement_xy = graph.convertPosition(pt.get_child("xyaxes"));
342  pinocchio::SE3 placement_z = graph.convertPosition(pt.get_child("zaxis"));
343 
344  Matrix3 rotation_matrix;
345  rotation_matrix << 1., 0., 0., 0., 0., 1., 0., -1., 0.;
346 
347  pinocchio::SE3 real_placement(rotation_matrix, Vector3(0.3, 0.2, 0.5));
348 
349  BOOST_CHECK(placement_q.isApprox(real_placement, 1e-7));
350  BOOST_CHECK(placement_e.isApprox(real_placement, 1e-7));
351  BOOST_CHECK(placement_a.isApprox(real_placement, 1e-7));
352  BOOST_CHECK(placement_xy.isApprox(real_placement, 1e-7));
353  BOOST_CHECK(placement_z.isApprox(real_placement, 1e-7));
354 }
355 
358 BOOST_AUTO_TEST_CASE(merge_default)
359 {
360  namespace pt = boost::property_tree;
361  std::istringstream xmlIn(R"(
362  <default>
363  <default class="mother">
364  <joint A="a0" B="b0" />
365  <geom A="a0" />
366  <default class="layer1">
367  <joint A="a1" C="c1" />
368  <default class="layer2">
369  <joint B="b2" />
370  <geom C="c2" />
371  </default>
372  </default>
373  <default class="layerP">
374  <joint K="b2" />
375  <site H="f1"/>
376  </default>
377  </default>
378  </default>)");
379 
380  // Create a Boost Property Tree
381  pt::ptree ptr;
382  pt::read_xml(xmlIn, ptr, pt::xml_parser::trim_whitespace);
383 
384  typedef ::pinocchio::mjcf::details::MjcfGraph MjcfGraph;
385  pinocchio::Model model;
386  MjcfGraph::UrdfVisitor visitor(model);
387 
388  MjcfGraph graph(visitor, "fakeMjcf");
389  graph.parseDefault(ptr.get_child("default"), ptr, "default");
390 
391  std::unordered_map<std::string, pt::ptree> TrueMap;
392 
393  std::istringstream xml1(R"(<default class="mother">
394  <joint A="a0" B="b0" />
395  <geom A="a0" />
396  <default class="layer1">
397  <joint A="a1" C="c1" />
398  <default class="layer2">
399  <joint B="b2" />
400  <geom C="c2" />
401  </default>
402  </default>
403  <default class="layerP">
404  <joint K="b2" />
405  <site H="f1"/>
406  </default>
407  </default>)");
408  pt::ptree p1;
409  pt::read_xml(xml1, p1, pt::xml_parser::trim_whitespace);
410  BOOST_CHECK(
411  comparePropertyTrees(graph.mapOfClasses.at("mother").classElement, p1.get_child("default")));
412 
413  std::istringstream xml2(R"(<default class="layer1">
414  <joint A="a1" B="b0" C="c1" />
415  <default class="layer2">
416  <joint B="b2" />
417  <geom C="c2" />
418  </default>
419  <geom A="a0" />
420  </default>)");
421  pt::ptree p2;
422  pt::read_xml(xml2, p2, pt::xml_parser::trim_whitespace);
423  BOOST_CHECK(
424  comparePropertyTrees(graph.mapOfClasses.at("layer1").classElement, p2.get_child("default")));
425  std::string name = "layer1";
426  TrueMap.insert(std::make_pair(name, p2.get_child("default")));
427 
428  std::istringstream xml3(R"(<default class="layer2">
429  <joint A="a1" B="b2" C="c1"/>
430  <geom A="a0" C="c2"/>
431  </default>)");
432  pt::ptree p3;
433  pt::read_xml(xml3, p3, pt::xml_parser::trim_whitespace);
434  BOOST_CHECK(
435  comparePropertyTrees(graph.mapOfClasses.at("layer2").classElement, p3.get_child("default")));
436 
437  std::istringstream xml4(R"(<default class="layerP">
438  <joint A="a0" B="b0" K="b2"/>
439  <site H="f1"/>
440  <geom A="a0"/>
441  </default>)");
442  pt::ptree p4;
443  pt::read_xml(xml4, p4, pt::xml_parser::trim_whitespace);
444  BOOST_CHECK(
445  comparePropertyTrees(graph.mapOfClasses.at("layerP").classElement, p4.get_child("default")));
446 }
447 
448 #ifdef PINOCCHIO_WITH_URDFDOM
449 // @brief Test to check that default classes and child classes are taken into account
450 BOOST_AUTO_TEST_CASE(parse_default_class)
451 {
452  typedef pinocchio::SE3::Vector3 Vector3;
453  typedef pinocchio::SE3::Matrix3 Matrix3;
454 
455  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(pinocchio::RigidConstraintModel) contact_models;
456  std::string filename = PINOCCHIO_MODEL_DIR + std::string("/../unittest/models/test_mjcf.xml");
457 
458  pinocchio::Model model_m;
459  pinocchio::mjcf::buildModel(filename, model_m, contact_models);
460 
461  std::string file_u = PINOCCHIO_MODEL_DIR + std::string("/../unittest/models/test_mjcf.urdf");
462  pinocchio::Model model_u;
463  pinocchio::urdf::buildModel(file_u, model_u);
464 
465  pinocchio::Model::JointIndex idx;
466  pinocchio::Inertia inertia(0, Vector3(0.0, 0., 0.0), Matrix3::Identity());
467  idx = model_u.addJoint(
468  size_t(model_u.njoints - 1), pinocchio::JointModelSpherical(), pinocchio::SE3::Identity(),
469  "joint3");
470  model_u.appendBodyToJoint(idx, inertia);
471 
472  for (size_t i = 0; i < size_t(model_m.njoints); i++)
473  BOOST_CHECK_EQUAL(model_m.joints[i], model_u.joints[i]);
474 }
475 #endif // PINOCCHIO_WITH_URDFDOM
476 
478 BOOST_AUTO_TEST_CASE(parse_dirs_no_strippath)
479 {
480 #ifdef _WIN32
481  std::istringstream xmlDataNoStrip(R"(<mujoco model="parseDirs">
482  <compiler meshdir="meshes" texturedir="textures"/>
483  <asset>
484  <texture name="testTexture" file="texture.png" type="2d"/>
485  <material name="matTest" texture="testTexture"/>
486  <mesh file="C:/auto/mesh.obj"/>
487  </asset>
488  </mujoco>)");
489 #else
490  std::istringstream xmlDataNoStrip(R"(<mujoco model="parseDirs">
491  <compiler meshdir="meshes" texturedir="textures"/>
492  <asset>
493  <texture name="testTexture" file="texture.png" type="2d"/>
494  <material name="matTest" texture="testTexture"/>
495  <mesh file="/auto/mesh.obj"/>
496  </asset>
497  </mujoco>)");
498 #endif
499 
500  auto namefile = createTempFile(xmlDataNoStrip);
501 
502  typedef ::pinocchio::mjcf::details::MjcfGraph MjcfGraph;
503  pinocchio::Model model_m;
504  MjcfGraph::UrdfVisitor visitor(model_m);
505 
506  MjcfGraph graph(visitor, "/fakeMjcf/fake.xml");
507  graph.parseGraphFromXML(namefile.name());
508 
509  // Test texture
510  pinocchio::mjcf::details::MjcfTexture text = graph.mapOfTextures.at("testTexture");
511  BOOST_CHECK_EQUAL(text.textType, "2d");
512  BOOST_CHECK_EQUAL(
513  boost::filesystem::path(text.filePath).generic_string(), "/fakeMjcf/textures/texture.png");
514  // Test Material
515  pinocchio::mjcf::details::MjcfMaterial mat = graph.mapOfMaterials.at("matTest");
516  BOOST_CHECK_EQUAL(mat.texture, "testTexture");
517  // Test Meshes
518  pinocchio::mjcf::details::MjcfMesh mesh = graph.mapOfMeshes.at("mesh");
519 #ifdef _WIN32
520  BOOST_CHECK_EQUAL(boost::filesystem::path(mesh.filePath).generic_string(), "C:/auto/mesh.obj");
521 #else
522  BOOST_CHECK_EQUAL(boost::filesystem::path(mesh.filePath).generic_string(), "/auto/mesh.obj");
523 #endif
524 }
525 
528 BOOST_AUTO_TEST_CASE(parse_dirs_strippath)
529 {
530  std::istringstream xmlDataNoStrip(R"(<mujoco model="parseDirs">
531  <compiler meshdir="meshes" texturedir="textures" strippath="true"/>
532  <asset>
533  <mesh file="/auto/mesh.obj"/>
534  </asset>
535  </mujoco>)");
536 
537  auto namefile = createTempFile(xmlDataNoStrip);
538 
539  typedef ::pinocchio::mjcf::details::MjcfGraph MjcfGraph;
540  pinocchio::Model model_m;
541  MjcfGraph::UrdfVisitor visitor(model_m);
542 
543  MjcfGraph graph(visitor, "/fakeMjcf/fake.xml");
544  graph.parseGraphFromXML(namefile.name());
545 
546  // Test Meshes
547  pinocchio::mjcf::details::MjcfMesh mesh = graph.mapOfMeshes.at("mesh");
548  BOOST_CHECK_EQUAL(
549  boost::filesystem::path(mesh.filePath).generic_string(), "/fakeMjcf/meshes/mesh.obj");
550 }
551 
552 // Test for parsing Revolute
553 BOOST_AUTO_TEST_CASE(parse_RX)
554 {
555  typedef pinocchio::SE3::Vector3 Vector3;
556  typedef pinocchio::SE3::Matrix3 Matrix3;
557 
558  std::istringstream xmlData(R"(
559  <mujoco model="model_RX">
560  <worldbody>
561  <body name="link0">
562  <body name="link1" pos="0 0 0">
563  <joint name="joint1" type="hinge" axis="1 0 0"/>
564  </body>
565  </body>
566  </worldbody>
567  </mujoco>)");
568 
569  auto namefile = createTempFile(xmlData);
570 
571  typedef ::pinocchio::mjcf::details::MjcfGraph MjcfGraph;
572  pinocchio::Model model_m, modelRX;
573  MjcfGraph::UrdfVisitor visitor(model_m);
574 
575  MjcfGraph graph(visitor, "fakeMjcf");
576  graph.parseGraphFromXML(namefile.name());
577  graph.parseRootTree();
578 
579  pinocchio::Model::JointIndex idx;
580  pinocchio::Inertia inertia(0, Vector3(0.0, 0., 0.0), Matrix3::Identity());
581 
582  idx = modelRX.addJoint(0, pinocchio::JointModelRX(), pinocchio::SE3::Identity(), "rx");
583  modelRX.appendBodyToJoint(idx, inertia);
584 
585  for (size_t i = 0; i < size_t(model_m.njoints); i++)
586  BOOST_CHECK_EQUAL(model_m.joints[i], modelRX.joints[i]);
587 }
588 
589 // Test for parsing prismatic
590 BOOST_AUTO_TEST_CASE(parse_PX)
591 {
592  typedef pinocchio::SE3::Vector3 Vector3;
593  typedef pinocchio::SE3::Matrix3 Matrix3;
594 
595  std::istringstream xmlData(R"(
596  <mujoco model="model_PX">
597  <worldbody>
598  <body name="link0">
599  <body name="link1" pos="0 0 0">
600  <joint name="joint1" type="slide" axis="1 0 0"/>
601  </body>
602  </body>
603  </worldbody>
604  </mujoco>)");
605 
606  auto namefile = createTempFile(xmlData);
607 
608  typedef ::pinocchio::mjcf::details::MjcfGraph MjcfGraph;
609  pinocchio::Model model_m, modelPX;
610  MjcfGraph::UrdfVisitor visitor(model_m);
611 
612  MjcfGraph graph(visitor, "fakeMjcf");
613  graph.parseGraphFromXML(namefile.name());
614  graph.parseRootTree();
615 
616  pinocchio::Model::JointIndex idx;
617  pinocchio::Inertia inertia(0, Vector3(0.0, 0., 0.0), Matrix3::Identity());
618 
619  idx = modelPX.addJoint(0, pinocchio::JointModelPX(), pinocchio::SE3::Identity(), "px");
620  modelPX.appendBodyToJoint(idx, inertia);
621 
622  for (size_t i = 0; i < size_t(model_m.njoints); i++)
623  BOOST_CHECK_EQUAL(model_m.joints[i], modelPX.joints[i]);
624 }
625 
626 // Test parsing sphere
627 BOOST_AUTO_TEST_CASE(parse_Sphere)
628 {
629  typedef pinocchio::SE3::Vector3 Vector3;
630  typedef pinocchio::SE3::Matrix3 Matrix3;
631 
632  std::istringstream xmlData(R"(
633  <mujoco model="model_Sphere">
634  <worldbody>
635  <body name="link0">
636  <body name="link1" pos="0 0 0">
637  <joint name="joint1" type="ball"/>
638  </body>
639  </body>
640  </worldbody>
641  </mujoco>)");
642 
643  auto namefile = createTempFile(xmlData);
644 
645  typedef ::pinocchio::mjcf::details::MjcfGraph MjcfGraph;
646  pinocchio::Model model_m, modelS;
647  MjcfGraph::UrdfVisitor visitor(model_m);
648 
649  MjcfGraph graph(visitor, "fakeMjcf");
650  graph.parseGraphFromXML(namefile.name());
651  graph.parseRootTree();
652 
653  pinocchio::Model::JointIndex idx;
654  pinocchio::Inertia inertia(0, Vector3(0.0, 0., 0.0), Matrix3::Identity());
655 
656  idx = modelS.addJoint(0, pinocchio::JointModelSpherical(), pinocchio::SE3::Identity(), "s");
657  modelS.appendBodyToJoint(idx, inertia);
658 
659  for (size_t i = 0; i < size_t(model_m.njoints); i++)
660  BOOST_CHECK_EQUAL(model_m.joints[i], modelS.joints[i]);
661 }
662 
663 // Test parsing free flyer
664 BOOST_AUTO_TEST_CASE(parse_Free)
665 {
666  typedef pinocchio::SE3::Vector3 Vector3;
667  typedef pinocchio::SE3::Matrix3 Matrix3;
668 
669  std::istringstream xmlData(R"(
670  <mujoco model="model_Free">
671  <worldbody>
672  <body name="link0">
673  <body name="link1" pos="0 0 0">
674  <freejoint name="joint1"/>
675  </body>
676  </body>
677  </worldbody>
678  </mujoco>)");
679 
680  auto namefile = createTempFile(xmlData);
681 
682  typedef ::pinocchio::mjcf::details::MjcfGraph MjcfGraph;
683  pinocchio::Model model_m, modelF;
684  MjcfGraph::UrdfVisitor visitor(model_m);
685 
686  MjcfGraph graph(visitor, "fakeMjcf");
687  graph.parseGraphFromXML(namefile.name());
688  graph.parseRootTree();
689 
690  pinocchio::Model::JointIndex idx;
691  pinocchio::Inertia inertia(0, Vector3(0.0, 0., 0.0), Matrix3::Identity());
692 
693  idx = modelF.addJoint(0, pinocchio::JointModelFreeFlyer(), pinocchio::SE3::Identity(), "f");
694  modelF.appendBodyToJoint(idx, inertia);
695 
696  for (size_t i = 0; i < size_t(model_m.njoints); i++)
697  BOOST_CHECK_EQUAL(model_m.joints[i], modelF.joints[i]);
698 }
699 
700 // Test for Composite RXRY
701 BOOST_AUTO_TEST_CASE(parse_composite_RXRY)
702 {
703  typedef pinocchio::SE3::Vector3 Vector3;
704  typedef pinocchio::SE3::Matrix3 Matrix3;
705 
706  std::istringstream xmlData(R"(
707  <mujoco model="composite_RXRY">
708  <worldbody>
709  <body name="link0">
710  <body name="link1" pos="0 0 0">
711  <joint name="joint1" type="hinge" axis="1 0 0"/>
712  <joint name="joint2" type="hinge" axis="0 1 0"/>
713  </body>
714  </body>
715  </worldbody>
716  </mujoco>)");
717 
718  auto namefile = createTempFile(xmlData);
719 
720  typedef ::pinocchio::mjcf::details::MjcfGraph MjcfGraph;
721  pinocchio::Model model_m, modelRXRY;
722  MjcfGraph::UrdfVisitor visitor(model_m);
723 
724  MjcfGraph graph(visitor, "fakeMjcf");
725  graph.parseGraphFromXML(namefile.name());
726  graph.parseRootTree();
727 
728  pinocchio::Model::JointIndex idx;
729  pinocchio::Inertia inertia(0, Vector3(0.0, 0., 0.0), Matrix3::Identity());
730 
731  pinocchio::JointModelComposite joint_model_RXRY;
732  joint_model_RXRY.addJoint(pinocchio::JointModelRX());
733  joint_model_RXRY.addJoint(pinocchio::JointModelRY());
734 
735  idx = modelRXRY.addJoint(0, joint_model_RXRY, pinocchio::SE3::Identity(), "rxry");
736  modelRXRY.appendBodyToJoint(idx, inertia);
737 
738  for (size_t i = 0; i < size_t(model_m.njoints); i++)
739  BOOST_CHECK_EQUAL(model_m.joints[i], modelRXRY.joints[i]);
740 }
741 
742 // Test for Composite PXPY
743 BOOST_AUTO_TEST_CASE(parse_composite_PXPY)
744 {
745  typedef pinocchio::SE3::Vector3 Vector3;
746  typedef pinocchio::SE3::Matrix3 Matrix3;
747 
748  std::istringstream xmlData(R"(
749  <mujoco model="composite_PXPY">
750  <worldbody>
751  <body name="link0">
752  <body name="link1" pos="0 0 0">
753  <joint name="joint1" type="slide" axis="1 0 0"/>
754  <joint name="joint2" type="slide" axis="0 1 0"/>
755  </body>
756  </body>
757  </worldbody>
758  </mujoco>)");
759 
760  auto namefile = createTempFile(xmlData);
761 
762  typedef ::pinocchio::mjcf::details::MjcfGraph MjcfGraph;
763  pinocchio::Model model_m, modelPXPY;
764  MjcfGraph::UrdfVisitor visitor(model_m);
765 
766  MjcfGraph graph(visitor, "fakeMjcf");
767  graph.parseGraphFromXML(namefile.name());
768  graph.parseRootTree();
769 
770  pinocchio::Model::JointIndex idx;
771  pinocchio::Inertia inertia(0, Vector3(0.0, 0., 0.0), Matrix3::Identity());
772 
773  pinocchio::JointModelComposite joint_model_PXPY;
774  joint_model_PXPY.addJoint(pinocchio::JointModelPX());
775  joint_model_PXPY.addJoint(pinocchio::JointModelPY());
776 
777  idx = modelPXPY.addJoint(0, joint_model_PXPY, pinocchio::SE3::Identity(), "pxpy");
778  modelPXPY.appendBodyToJoint(idx, inertia);
779 
780  for (size_t i = 0; i < size_t(model_m.njoints); i++)
781  BOOST_CHECK_EQUAL(model_m.joints[i], modelPXPY.joints[i]);
782 }
783 
784 // Test for Composite PXRY
785 BOOST_AUTO_TEST_CASE(parse_composite_PXRY)
786 {
787  typedef pinocchio::SE3::Vector3 Vector3;
788  typedef pinocchio::SE3::Matrix3 Matrix3;
789 
790  std::istringstream xmlData(R"(
791  <mujoco model="composite_PXRY">
792  <worldbody>
793  <body name="link0">
794  <body name="link1" pos="0 0 0">
795  <joint name="joint1" type="slide" axis="1 0 0"/>
796  <joint name="joint2" type="hinge" axis="0 1 0"/>
797  </body>
798  </body>
799  </worldbody>
800  </mujoco>)");
801 
802  auto namefile = createTempFile(xmlData);
803 
804  typedef ::pinocchio::mjcf::details::MjcfGraph MjcfGraph;
805  pinocchio::Model model_m, modelPXRY;
806  MjcfGraph::UrdfVisitor visitor(model_m);
807 
808  MjcfGraph graph(visitor, "fakeMjcf");
809  graph.parseGraphFromXML(namefile.name());
810  graph.parseRootTree();
811 
812  pinocchio::Model::JointIndex idx;
813  pinocchio::Inertia inertia(0, Vector3(0.0, 0., 0.0), Matrix3::Identity());
814 
815  pinocchio::JointModelComposite joint_model_PXRY;
816  joint_model_PXRY.addJoint(pinocchio::JointModelPX());
817  joint_model_PXRY.addJoint(pinocchio::JointModelRY());
818 
819  idx = modelPXRY.addJoint(0, joint_model_PXRY, pinocchio::SE3::Identity(), "pxry");
820  modelPXRY.appendBodyToJoint(idx, inertia);
821 
822  for (size_t i = 0; i < size_t(model_m.njoints); i++)
823  BOOST_CHECK_EQUAL(model_m.joints[i], modelPXRY.joints[i]);
824 }
825 
826 // Test for Composite PXSphere
827 BOOST_AUTO_TEST_CASE(parse_composite_PXSphere)
828 {
829  typedef pinocchio::SE3::Vector3 Vector3;
830  typedef pinocchio::SE3::Matrix3 Matrix3;
831 
832  std::istringstream xmlData(R"(
833  <mujoco model="composite_PXSphere">
834  <worldbody>
835  <body name="link0">
836  <body name="link1" pos="0 0 0">
837  <joint name="joint1" type="slide" axis="1 0 0"/>
838  <joint name="joint2" type="ball"/>
839  </body>
840  </body>
841  </worldbody>
842  </mujoco>)");
843 
844  auto namefile = createTempFile(xmlData);
845 
846  typedef ::pinocchio::mjcf::details::MjcfGraph MjcfGraph;
847  pinocchio::Model model_m, modelPXSphere;
848  MjcfGraph::UrdfVisitor visitor(model_m);
849 
850  MjcfGraph graph(visitor, "fakeMjcf");
851  graph.parseGraphFromXML(namefile.name());
852  graph.parseRootTree();
853 
854  pinocchio::Model::JointIndex idx;
855  pinocchio::Inertia inertia(0, Vector3(0.0, 0., 0.0), Matrix3::Identity());
856 
857  pinocchio::JointModelComposite joint_model_PXSphere;
858  joint_model_PXSphere.addJoint(pinocchio::JointModelPX());
859  joint_model_PXSphere.addJoint(pinocchio::JointModelSpherical());
860 
861  idx = modelPXSphere.addJoint(0, joint_model_PXSphere, pinocchio::SE3::Identity(), "pxsphere");
862  modelPXSphere.appendBodyToJoint(idx, inertia);
863 
864  for (size_t i = 0; i < size_t(model_m.njoints); i++)
865  BOOST_CHECK_EQUAL(model_m.joints[i], modelPXSphere.joints[i]);
866 }
867 
868 // Test loading a mujoco composite and compare body position with mujoco results
869 BOOST_AUTO_TEST_CASE(parse_composite_Mujoco_comparison)
870 {
871  using namespace pinocchio;
872  std::string filename =
873  PINOCCHIO_MODEL_DIR + std::string("/../unittest/models/test_composite.xml");
874 
875  pinocchio::Model model;
876  pinocchio::mjcf::buildModel(filename, model);
877 
878  Data data(model);
879  Eigen::Vector3d q;
880  q << 1.57079633, 0.3, 0.5;
881  framesForwardKinematics(model, data, q);
882 
883  FrameIndex f_id = model.getFrameId("body1");
884  SE3 pinPos = data.oMf[f_id];
885 
886  Eigen::Matrix3d refOrient;
887  refOrient << 0, -.9553, .2955, 1, 0, 0, 0, .2955, .9553;
888  Eigen::Vector3d pos;
889  pos << .8522, 2, 0.5223;
890  BOOST_CHECK(pinPos.isApprox(SE3(refOrient, pos), 1e-4));
891 
892  f_id = model.getFrameId("body2");
893  pinPos = data.oMf[f_id];
894 
895  refOrient << 0, -.9553, .2955, 1, 0, 0, 0, .2955, .9553;
896 
897  pos << .8522, 4, 0.5223;
898  BOOST_CHECK(pinPos.isApprox(SE3(refOrient, pos), 1e-4));
899 }
900 
901 // Test laoding a model with a spherical joint and verify that keyframe is valid
902 BOOST_AUTO_TEST_CASE(adding_keyframes)
903 {
904  std::istringstream xmlData(R"(
905  <mujoco model="testKeyFrame">
906  <default>
907  <position ctrllimited="true" ctrlrange="-.1 .1" kp="30"/>
908  <default class="joint">
909  <geom type="cylinder" size=".006" fromto="0 0 0 0 0 .05" rgba=".9 .6 1 1"/>
910  </default>
911  </default>
912  <worldbody>
913  <body name="body1">
914  <freejoint/>
915  <geom type="capsule" size=".01" fromto="0 0 0 .2 0 0"/>
916  <body pos=".2 0 0" name="body2">
917  <joint type="ball" damping=".1"/>
918  <geom type="capsule" size=".01" fromto="0 -.15 0 0 0 0"/>
919  </body>
920  </body>
921  </worldbody>
922  <keyframe>
923  <key name="test"
924  qpos="0 0 0.596
925  0.988015 0 0.154359 0
926  0.988015 0 0.154359 0"/>
927  </keyframe>
928  </mujoco>)");
929 
930  auto namefile = createTempFile(xmlData);
931 
932  pinocchio::Model model_m;
933  pinocchio::mjcf::buildModel(namefile.name(), model_m);
934 
935  Eigen::VectorXd vect_model = model_m.referenceConfigurations.at("test");
936 
937  Eigen::VectorXd vect_ref(model_m.nq);
938  vect_ref << 0, 0, 0.596, 0, 0.154359, 0, 0.988015, 0, 0.154359, 0, 0.988015;
939 
940  BOOST_CHECK(vect_model.size() == vect_ref.size());
941  BOOST_CHECK(vect_model == vect_ref);
942 }
943 
944 // Test on which joints inertias are append
945 BOOST_AUTO_TEST_CASE(joint_and_inertias)
946 {
947  typedef pinocchio::SE3::Vector3 Vector3;
948  typedef pinocchio::SE3::Matrix3 Matrix3;
949 
950  std::istringstream xmlData(R"(
951  <mujoco model="testJointInertia">
952  <worldbody>
953  <body name="body1">
954  <freejoint/>
955  <inertial mass="0.629769" pos="-0.041018 -0.00014 0.049974"
956  diaginertia="0.00315 0.00388 0.004285"/>
957  </body>
958  </worldbody>
959  </mujoco>)");
960 
961  auto namefile = createTempFile(xmlData);
962 
963  pinocchio::Model model_m;
964  pinocchio::mjcf::buildModel(namefile.name(), model_m);
965 
966  BOOST_CHECK(model_m.inertias[0].isApprox(pinocchio::Inertia::Zero()));
967 
968  Matrix3 inertia_matrix = Eigen::Matrix3d::Zero();
969  inertia_matrix(0, 0) = 0.00315;
970  inertia_matrix(1, 1) = 0.00388;
971  inertia_matrix(2, 2) = 0.004285;
972  pinocchio::Inertia real_inertia(0.629769, Vector3(-0.041018, -0.00014, 0.049974), inertia_matrix);
973 
974  BOOST_CHECK(model_m.inertias[1].isApprox(real_inertia));
975 }
976 
977 BOOST_AUTO_TEST_CASE(armature)
978 {
979  typedef pinocchio::SE3::Vector3 Vector3;
980  typedef pinocchio::SE3::Matrix3 Matrix3;
981  std::cout << " Armature ------------ " << std::endl;
982  std::istringstream xmlData(R"(
983  <mujoco model="model_RX">
984  <default>
985  <joint armature="1" damping="1" limited="true"/>
986  </default>
987  <worldbody>
988  <body name="link0">
989  <body name="link1" pos="0 0 0">
990  <joint name="joint1" type="hinge" axis="1 0 0" armature="1.3"/>
991  <joint name="joint2" type="hinge" axis="0 1 0" armature="2.4"/>
992  <joint name="joint3" type="hinge" axis="0 0 1" armature="0.4"/>
993  <body pos=".2 0 0" name="body2">
994  <joint type="ball"/>
995  </body>
996  </body>
997  </body>
998  </worldbody>
999  </mujoco>)");
1000 
1001  auto namefile = createTempFile(xmlData);
1002 
1003  typedef ::pinocchio::mjcf::details::MjcfGraph MjcfGraph;
1004  pinocchio::Model model_m;
1005  MjcfGraph::UrdfVisitor visitor(model_m);
1006 
1007  MjcfGraph graph(visitor, "fakeMjcf");
1008  graph.parseGraphFromXML(namefile.name());
1009  graph.parseRootTree();
1010 
1011  Eigen::VectorXd armature_real(model_m.nv);
1012  armature_real << 1.3, 2.4, 0.4, 1, 1, 1;
1013 
1014  for (size_t i = 0; i < size_t(model_m.nv); i++)
1015  BOOST_CHECK_EQUAL(model_m.armature[i], armature_real[i]);
1016 }
1017 
1018 // Test reference positions and how it's included in keyframe
1019 BOOST_AUTO_TEST_CASE(reference_positions)
1020 {
1021  std::istringstream xmlData(R"(
1022  <mujoco model="testRefPose">
1023  <compiler angle="radian"/>
1024  <worldbody>
1025  <body name="body1">
1026  <body pos=".2 0 0" name="body2">
1027  <joint type="slide" ref="0.14"/>
1028  <body pos=".2 0 0" name="body3">
1029  <joint type="hinge" ref="0.1"/>
1030  </body>
1031  </body>
1032  </body>
1033  </worldbody>
1034  <keyframe>
1035  <key name="test" qpos=".8 .5"/>
1036  </keyframe>
1037  </mujoco>)");
1038 
1039  auto namefile = createTempFile(xmlData);
1040 
1041  pinocchio::Model model_m;
1042  pinocchio::mjcf::buildModel(namefile.name(), model_m);
1043 
1044  Eigen::VectorXd vect_model = model_m.referenceConfigurations.at("test");
1045  Eigen::VectorXd vect_ref(model_m.nq);
1046  vect_ref << 0.66, 0.4;
1047 
1048  BOOST_CHECK(vect_model.size() == vect_ref.size());
1049  BOOST_CHECK(vect_model == vect_ref);
1050 }
1051 
1052 // Test keyframe and composite joint
1053 BOOST_AUTO_TEST_CASE(keyframe_and_composite)
1054 {
1055  std::istringstream xmlData(R"(
1056  <mujoco model="keyframeComposite">
1057  <compiler angle="radian"/>
1058  <worldbody>
1059  <body name="body1">
1060  <body pos=".2 0 0" name="body2">
1061  <joint type="slide"/>
1062  <joint type="hinge"/>
1063  <body pos=".2 0 0" name="body3">
1064  <joint type="hinge"/>
1065  </body>
1066  </body>
1067  </body>
1068  </worldbody>
1069  <keyframe>
1070  <key name="test" qpos=".8 .5 .5"/>
1071  </keyframe>
1072  </mujoco>)");
1073 
1074  auto namefile = createTempFile(xmlData);
1075 
1076  pinocchio::Model model_m;
1077  pinocchio::mjcf::buildModel(namefile.name(), model_m);
1078 
1079  Eigen::VectorXd vect_model = model_m.referenceConfigurations.at("test");
1080  Eigen::VectorXd vect_ref(model_m.nq);
1081  vect_ref << 0.8, 0.5, 0.5;
1082 
1083  BOOST_CHECK(vect_model.size() == vect_ref.size());
1084  BOOST_CHECK(vect_model == vect_ref);
1085 }
1086 
1087 // Test site of mjcf model
1088 BOOST_AUTO_TEST_CASE(adding_site)
1089 {
1090  typedef pinocchio::SE3::Vector3 Vector3;
1091  typedef pinocchio::SE3::Matrix3 Matrix3;
1092 
1093  std::istringstream xmlData(R"(
1094  <mujoco model="site">
1095  <compiler angle="radian"/>
1096  <worldbody>
1097  <body name="body1">
1098  <body pos=".2 0 0" name="body2">
1099  <joint type="hinge"/>
1100  <body pos=".2 0 0" name="body3">
1101  <joint type="hinge"/>
1102  <site name="testSite" pos="0.03 0 -0.05"/>
1103  </body>
1104  </body>
1105  </body>
1106  </worldbody>
1107  </mujoco>)");
1108 
1109  auto namefile = createTempFile(xmlData);
1110 
1111  pinocchio::Model model_m;
1112  pinocchio::mjcf::buildModel(namefile.name(), model_m);
1113 
1114  pinocchio::Data data(model_m);
1115 
1116  Matrix3 rotation_matrix;
1117  rotation_matrix << 1., 0., 0., 0., 1., 0., 0., 0., 1.;
1118  pinocchio::SE3 real_placement(rotation_matrix, Vector3(0.03, 0, -0.05));
1119 
1120  BOOST_CHECK(model_m.frames[model_m.getFrameId("testSite")].placement.isApprox(real_placement));
1121 }
1122 
1125 BOOST_AUTO_TEST_CASE(build_model_no_root_joint)
1126 {
1127  using namespace pinocchio;
1128  const std::string filename = PINOCCHIO_MODEL_DIR + std::string("/simple_humanoid.xml");
1129 
1130  pinocchio::Model model_m;
1131  pinocchio::mjcf::buildModel(filename, model_m);
1132 
1133  BOOST_CHECK_EQUAL(model_m.nq, 29);
1134 }
1135 
1136 BOOST_AUTO_TEST_CASE(build_model_with_root_joint_name)
1137 {
1138  const std::string filename = PINOCCHIO_MODEL_DIR + std::string("/simple_humanoid.xml");
1139  const std::string dir = PINOCCHIO_MODEL_DIR;
1140 
1141  pinocchio::Model model;
1142  pinocchio::mjcf::buildModel(filename, pinocchio::JointModelFreeFlyer(), model);
1143  BOOST_CHECK(model.names[1] == "root_joint");
1144 
1145  pinocchio::Model model_name;
1146  const std::string name_ = "freeFlyer_joint";
1147  pinocchio::mjcf::buildModel(filename, pinocchio::JointModelFreeFlyer(), name_, model_name);
1148  BOOST_CHECK(model_name.names[1] == name_);
1149 }
1150 
1151 #ifdef PINOCCHIO_WITH_URDFDOM
1152 BOOST_AUTO_TEST_CASE(compare_to_urdf)
1155 {
1156  using namespace pinocchio;
1157  typedef typename pinocchio::Model::ConfigVectorMap ConfigVectorMap;
1158 
1159  const std::string filename = PINOCCHIO_MODEL_DIR + std::string("/simple_humanoid.xml");
1160 
1161  Model model_m;
1162  pinocchio::mjcf::buildModel(filename, pinocchio::JointModelFreeFlyer(), model_m);
1163 
1164  const std::string filename_urdf = PINOCCHIO_MODEL_DIR + std::string("/simple_humanoid.urdf");
1165  const std::string dir_urdf = PINOCCHIO_MODEL_DIR;
1166  pinocchio::Model model_urdf;
1167  pinocchio::urdf::buildModel(filename_urdf, pinocchio::JointModelFreeFlyer(), model_urdf);
1168 
1169  BOOST_CHECK(model_urdf.nq == model_m.nq);
1170  BOOST_CHECK(model_urdf.nv == model_m.nv);
1171  BOOST_CHECK(model_urdf.njoints == model_m.njoints);
1172  BOOST_CHECK(model_urdf.nbodies == model_m.nbodies);
1173  BOOST_CHECK(model_urdf.nframes == model_m.nframes);
1174  BOOST_CHECK(model_urdf.parents == model_m.parents);
1175  BOOST_CHECK(model_urdf.children == model_m.children);
1176  BOOST_CHECK(model_urdf.names == model_m.names);
1177  BOOST_CHECK(model_urdf.subtrees == model_m.subtrees);
1178  BOOST_CHECK(model_urdf.gravity == model_m.gravity);
1179  BOOST_CHECK(model_urdf.name == model_m.name);
1180  BOOST_CHECK(model_urdf.idx_qs == model_m.idx_qs);
1181  BOOST_CHECK(model_urdf.nqs == model_m.nqs);
1182  BOOST_CHECK(model_urdf.idx_vs == model_m.idx_vs);
1183  BOOST_CHECK(model_urdf.nvs == model_m.nvs);
1184 
1185  typename ConfigVectorMap::const_iterator it = model_m.referenceConfigurations.begin();
1186  typename ConfigVectorMap::const_iterator it_model_urdf =
1187  model_urdf.referenceConfigurations.begin();
1188  for (long k = 0; k < (long)model_m.referenceConfigurations.size(); ++k)
1189  {
1190  std::advance(it, k);
1191  std::advance(it_model_urdf, k);
1192  BOOST_CHECK(it->second.size() == it_model_urdf->second.size());
1193  BOOST_CHECK(it->second == it_model_urdf->second);
1194  }
1195 
1196  BOOST_CHECK(model_urdf.armature.size() == model_m.armature.size());
1197 
1198  BOOST_CHECK(model_urdf.armature == model_m.armature);
1199  BOOST_CHECK(model_urdf.friction.size() == model_m.friction.size());
1200  BOOST_CHECK(model_urdf.friction == model_m.friction);
1201 
1202  BOOST_CHECK(model_urdf.damping.size() == model_m.damping.size());
1203 
1204  BOOST_CHECK(model_urdf.damping == model_m.damping);
1205 
1206  BOOST_CHECK(model_urdf.rotorInertia.size() == model_m.rotorInertia.size());
1207 
1208  BOOST_CHECK(model_urdf.rotorInertia == model_m.rotorInertia);
1209 
1210  BOOST_CHECK(model_urdf.rotorGearRatio.size() == model_m.rotorGearRatio.size());
1211 
1212  BOOST_CHECK(model_urdf.rotorGearRatio == model_m.rotorGearRatio);
1213 
1214  BOOST_CHECK(model_urdf.effortLimit.size() == model_m.effortLimit.size());
1215  BOOST_CHECK(model_urdf.effortLimit == model_m.effortLimit);
1216  // Cannot test velocity limit since it does not exist in mjcf
1217 
1218  BOOST_CHECK(model_urdf.lowerPositionLimit.size() == model_m.lowerPositionLimit.size());
1219  BOOST_CHECK(model_urdf.lowerPositionLimit == model_m.lowerPositionLimit);
1220 
1221  BOOST_CHECK(model_urdf.upperPositionLimit.size() == model_m.upperPositionLimit.size());
1222  BOOST_CHECK(model_urdf.upperPositionLimit == model_m.upperPositionLimit);
1223 
1224  for (size_t k = 1; k < model_m.inertias.size(); ++k)
1225  {
1226  BOOST_CHECK(model_urdf.inertias[k].isApprox(model_m.inertias[k]));
1227  }
1228 
1229  for (size_t k = 1; k < model_urdf.jointPlacements.size(); ++k)
1230  {
1231  BOOST_CHECK(model_urdf.jointPlacements[k] == model_m.jointPlacements[k]);
1232  }
1233 
1234  BOOST_CHECK(model_urdf.joints == model_m.joints);
1235 
1236  BOOST_CHECK(model_urdf.frames.size() == model_m.frames.size());
1237  for (size_t k = 1; k < model_urdf.frames.size(); ++k)
1238  {
1239  BOOST_CHECK(model_urdf.frames[k] == model_m.frames[k]);
1240  }
1241 }
1242 #endif // PINOCCHIO_WITH_URDFDOM
1243 
1244 #if defined(PINOCCHIO_WITH_HPP_FCL)
1245 BOOST_AUTO_TEST_CASE(test_geometry_parsing)
1246 {
1247  typedef pinocchio::Model Model;
1248  typedef pinocchio::GeometryModel GeometryModel;
1249 
1250  // Parse the XML
1251  std::istringstream xmlData(R"(<mujoco model="inertiaFromGeom">
1252  <compiler inertiafromgeom="true" />
1253  <worldbody>
1254  <body pos="0 0 0" name="bodyCylinder">
1255  <geom type="cylinder" size=".01 0.25" pos="0 0 0" quat="1 0 0 0"/>
1256  <body pos="0 0 0" name="bodyBox">
1257  <geom type="box" size=".01 0.01 0.25" pos="0 0 0" quat="1 0 0 0"/>
1258  </body>
1259  <body pos="0 0 0" name="bodyCapsule">
1260  <geom type="capsule" size=".01 0.25" pos="0 0 0" quat="1 0 0 0"/>
1261  </body>
1262  <body pos="0 0 0" name="bodySphere">
1263  <geom type="sphere" size=".01" pos="0 0 0" quat="1 0 0 0"/>
1264  </body>
1265  <body pos="0 0 0" name="bodyEllip">
1266  <geom type="ellipsoid" size=".01 0.01 0.25" pos="0 0 0" quat="1 0 0 0"/>
1267  </body>
1268  </body>
1269  </worldbody>
1270  </mujoco>)");
1271 
1272  auto namefile = createTempFile(xmlData);
1273 
1274  Model model_m;
1275  pinocchio::mjcf::buildModel(namefile.name(), model_m);
1276 
1277  GeometryModel geomModel_m;
1278  pinocchio::mjcf::buildGeom(model_m, namefile.name(), pinocchio::COLLISION, geomModel_m);
1279 
1280  BOOST_CHECK(geomModel_m.ngeoms == 5);
1281 
1282  auto * cyl = dynamic_cast<hpp::fcl::Cylinder *>(geomModel_m.geometryObjects.at(0).geometry.get());
1283  BOOST_REQUIRE(cyl);
1284  BOOST_CHECK(cyl->halfLength == 0.25);
1285  BOOST_CHECK(cyl->radius == 0.01);
1286 
1287  auto * cap = dynamic_cast<hpp::fcl::Capsule *>(geomModel_m.geometryObjects.at(2).geometry.get());
1288  BOOST_REQUIRE(cap);
1289  BOOST_CHECK(cap->halfLength == 0.25);
1290  BOOST_CHECK(cap->radius == 0.01);
1291 
1292  auto * s = dynamic_cast<hpp::fcl::Sphere *>(geomModel_m.geometryObjects.at(3).geometry.get());
1293  BOOST_REQUIRE(s);
1294  BOOST_CHECK(s->radius == 0.01);
1295 
1296  auto * b = dynamic_cast<hpp::fcl::Box *>(geomModel_m.geometryObjects.at(1).geometry.get());
1297  BOOST_REQUIRE(b);
1298  Eigen::Vector3d sides;
1299  sides << 0.01, 0.01, 0.25;
1300  BOOST_CHECK(b->halfSide == sides);
1301 
1302  auto * e = dynamic_cast<hpp::fcl::Ellipsoid *>(geomModel_m.geometryObjects.at(4).geometry.get());
1303  BOOST_REQUIRE(e);
1304  BOOST_CHECK(e->radii == sides);
1305 }
1306 #endif // if defined(PINOCCHIO_WITH_HPP_FCL)
1307 
1308 BOOST_AUTO_TEST_CASE(test_contact_parsing)
1309 {
1310  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(pinocchio::RigidConstraintModel) contact_models;
1311  std::string filename = PINOCCHIO_MODEL_DIR + std::string("/../unittest/models/closed_chain.xml");
1312 
1313  pinocchio::Model model;
1314  pinocchio::mjcf::buildModel(filename, model, contact_models);
1315 
1316  BOOST_CHECK_EQUAL(contact_models.size(), 4);
1317  BOOST_CHECK_EQUAL(
1318  contact_models[0].joint1_placement.translation(), pinocchio::SE3::Vector3(0.50120, 0, 0));
1319  BOOST_CHECK_EQUAL(
1320  contact_models[1].joint1_placement.translation(), pinocchio::SE3::Vector3(0.35012, 0, 0));
1321  BOOST_CHECK_EQUAL(
1322  contact_models[2].joint1_placement.translation(), pinocchio::SE3::Vector3(0.50120, 0, 0));
1323  BOOST_CHECK_EQUAL(
1324  contact_models[3].joint1_placement.translation(), pinocchio::SE3::Vector3(0.35012, 0, 0));
1325  for (const auto & cm : contact_models)
1326  {
1327  BOOST_CHECK(cm.joint2_placement.isApprox(cm.joint1_placement.inverse()));
1328  }
1329 }
1330 
1331 BOOST_AUTO_TEST_SUITE_END()
meshcat-viewer.mesh
mesh
Definition: meshcat-viewer.py:54
pinocchio::InertiaTpl< context::Scalar, context::Options >
pinocchio::InertiaTpl::isEqual
bool isEqual(const InertiaTpl &Y2) const
Definition: spatial/inertia.hpp:620
pinocchio::FrameIndex
Index FrameIndex
Definition: multibody/fwd.hpp:28
frames.hpp
pinocchio::ModelTpl::names
std::vector< std::string > names
Name of the joints.
Definition: multibody/model.hpp:142
pinocchio::InertiaTpl< context::Scalar, context::Options >::FromEllipsoid
static InertiaTpl FromEllipsoid(const Scalar mass, const Scalar x, const Scalar y, const Scalar z)
Computes the Inertia of an ellipsoid defined by its mass and main semi-axis dimensions (x,...
Definition: spatial/inertia.hpp:389
quadrotor-ocp.path
string path
Definition: quadrotor-ocp.py:11
pinocchio::SE3Tpl< context::Scalar, context::Options >::Vector3
traits< SE3Tpl >::Vector3 Vector3
Definition: spatial/se3-tpl.hpp:55
pinocchio::ModelTpl::frames
FrameVector frames
Vector of operational frames registered on the model.
Definition: multibody/model.hpp:176
pinocchio::DataTpl
Definition: context/generic.hpp:25
pinocchio::mjcf::details::MjcfTexture::textType
std::string textType
Definition: mjcf-graph.hpp:218
pinocchio::ModelTpl::jointPlacements
SE3Vector jointPlacements
Vector of joint placements: placement of a joint i wrt its parent joint frame.
Definition: multibody/model.hpp:116
pinocchio::mjcf::details::MjcfMesh
All informations related to a mesh are stored here.
Definition: mjcf-graph.hpp:206
pinocchio::ModelTpl::lowerPositionLimit
ConfigVectorType lowerPositionLimit
Lower joint configuration limit.
Definition: multibody/model.hpp:170
pinocchio::mjcf::details::UrdfVisitor
pinocchio::urdf::details::UrdfVisitor< double, 0, ::pinocchio::JointCollectionDefaultTpl > UrdfVisitor
Definition: mjcf-graph.cpp:18
mjcf.hpp
pinocchio::SE3Tpl
Definition: context/casadi.hpp:29
inverse-kinematics.i
int i
Definition: inverse-kinematics.py:17
model.hpp
hpp::fcl::Sphere
setup.data
data
Definition: cmake/cython/setup.in.py:48
simulation-closed-kinematic-chains.joint1_placement
joint1_placement
Definition: simulation-closed-kinematic-chains.py:54
boost
pinocchio::ModelTpl::joints
JointModelVector joints
Vector of joint models.
Definition: multibody/model.hpp:119
pinocchio::JointModelFreeFlyerTpl
Definition: multibody/joint/fwd.hpp:110
pinocchio::mjcf::details::MjcfMaterial
All informations related to material are stored here.
Definition: mjcf-graph.hpp:226
pinocchio::ModelTpl::name
std::string name
Model name.
Definition: multibody/model.hpp:197
pinocchio::python::context::Model
ModelTpl< Scalar, Options > Model
Definition: bindings/python/context/generic.hpp:63
pinocchio::JointModelCompositeTpl::addJoint
JointModelDerived & addJoint(const JointModelBase< JointModel > &jmodel, const SE3 &placement=SE3::Identity())
Add a joint to the vector of joints.
Definition: joint-composite.hpp:278
hpp::fcl::Cylinder
pinocchio::ModelTpl::addJoint
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.
b
Vec3f b
pinocchio::ModelTpl::rotorInertia
TangentVectorType rotorInertia
Vector of rotor inertia parameters.
Definition: multibody/model.hpp:152
anymal-simulation.model
model
Definition: anymal-simulation.py:8
pinocchio::ModelTpl::inertias
InertiaVector inertias
Vector of spatial inertias supported by each joint.
Definition: multibody/model.hpp:113
pinocchio::SE3Tpl< context::Scalar, context::Options >::Matrix3
traits< SE3Tpl >::Matrix3 Matrix3
Definition: spatial/se3-tpl.hpp:56
pinocchio::InertiaTpl< context::Scalar, context::Options >::Zero
static InertiaTpl Zero()
Definition: spatial/inertia.hpp:337
pinocchio::ModelTpl::JointIndex
pinocchio::JointIndex JointIndex
Definition: multibody/model.hpp:67
pinocchio::ModelTpl::referenceConfigurations
ConfigVectorMap referenceConfigurations
Map of reference configurations, indexed by user given names.
Definition: multibody/model.hpp:145
pinocchio::ModelTpl::subtrees
std::vector< IndexVector > subtrees
Vector of joint subtrees. subtree[j] corresponds to the subtree supported by the joint j....
Definition: multibody/model.hpp:188
pinocchio::GeometryModel::geometryObjects
GeometryObjectVector geometryObjects
Vector of GeometryObjects used for collision computations.
Definition: multibody/geometry.hpp:217
hpp::fcl::Ellipsoid
pinocchio::mjcf::details::MjcfBody::geomChildren
std::vector< MjcfGeom > geomChildren
Definition: mjcf-graph.hpp:110
joint-configuration.hpp
pinocchio::JointModelPrismaticTpl
Definition: multibody/joint/fwd.hpp:89
pinocchio::ModelTpl::nqs
std::vector< int > nqs
Vector of dimension of the joint configuration subspace.
Definition: multibody/model.hpp:125
pinocchio::ModelTpl::nvs
std::vector< int > nvs
Dimension of the *i*th joint tangent subspace.
Definition: multibody/model.hpp:131
pinocchio::ModelTpl::appendBodyToJoint
void appendBodyToJoint(const JointIndex joint_index, const Inertia &Y, const SE3 &body_placement=SE3::Identity())
Append a body to a given joint of the kinematic tree.
pinocchio::RigidConstraintModelTpl
Definition: algorithm/constraints/fwd.hpp:14
pinocchio::ModelTpl::children
std::vector< IndexVector > children
Vector of children index. Chidren of the i*th joint, denoted *mu(i) corresponds to the set (i==parent...
Definition: multibody/model.hpp:139
pinocchio::ModelTpl::idx_qs
std::vector< int > idx_qs
Vector of starting index of the *i*th joint in the configuration space.
Definition: multibody/model.hpp:122
pinocchio::mjcf::details::MjcfTexture::filePath
std::string filePath
Definition: mjcf-graph.hpp:220
filename
filename
pinocchio::ModelTpl::getFrameId
FrameIndex getFrameId(const std::string &name, const FrameType &type=(FrameType)(JOINT|FIXED_JOINT|BODY|OP_FRAME|SENSOR)) const
Returns the index of a frame given by its name.
urdf.hpp
mat
mat
pinocchio::ModelTpl::rotorGearRatio
TangentVectorType rotorGearRatio
Vector of rotor gear ratio parameters.
Definition: multibody/model.hpp:155
pinocchio::ModelTpl::armature
VectorXs armature
Vector of armature values expressed at the joint level This vector may contain the contribution of ro...
Definition: multibody/model.hpp:149
pinocchio::InertiaTpl< context::Scalar, context::Options >::FromCapsule
static InertiaTpl FromCapsule(const Scalar mass, const Scalar radius, const Scalar height)
Computes the Inertia of a capsule defined by its mass, radius and length along the Z axis....
Definition: spatial/inertia.hpp:439
comparePropertyTrees
static bool comparePropertyTrees(const boost::property_tree::ptree &pt1, const boost::property_tree::ptree &pt2)
Definition: mjcf.cpp:69
pinocchio::urdf::buildModel
ModelTpl< Scalar, Options, JointCollectionTpl > & buildModel(const std::string &filename, const typename ModelTpl< Scalar, Options, JointCollectionTpl >::JointModel &rootJoint, const std::string &rootJointName, ModelTpl< Scalar, Options, JointCollectionTpl > &model, const bool verbose=false)
Build the model from a URDF file with a particular joint as root of the model tree inside the model g...
pinocchio::JointModelSphericalTpl
Definition: multibody/joint/fwd.hpp:73
pos
pos
pinocchio::ModelTpl::nframes
int nframes
Number of operational frames.
Definition: multibody/model.hpp:110
pinocchio::context::Vector3
Eigen::Matrix< Scalar, 3, 1, Options > Vector3
Definition: context/generic.hpp:53
pinocchio::mjcf::details::ptree
boost::property_tree::ptree ptree
Definition: mjcf-graph.cpp:15
pinocchio::ModelTpl::nbodies
int nbodies
Number of bodies.
Definition: multibody/model.hpp:107
pinocchio::ModelTpl::friction
TangentVectorType friction
Vector of joint friction parameters.
Definition: multibody/model.hpp:158
setup.name
name
Definition: cmake/cython/setup.in.py:179
hpp::fcl::Capsule
pinocchio::GeometryModel::ngeoms
Index ngeoms
The number of GeometryObjects.
Definition: multibody/geometry.hpp:214
pinocchio::framesForwardKinematics
void framesForwardKinematics(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
First calls the forwardKinematics on the model, then computes the placement of each frame....
pinocchio::mjcf::buildGeom
GeometryModel & buildGeom(ModelTpl< Scalar, Options, JointCollectionTpl > &model, const std::string &filename, const GeometryType type, GeometryModel &geom_model, ::hpp::fcl::MeshLoaderPtr mesh_loader=::hpp::fcl::MeshLoaderPtr())
Build The GeometryModel from a Mjcf file.
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(convert_inertia_fullinertia)
Definition: mjcf.cpp:99
PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR
#define PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(T)
Definition: container/aligned-vector.hpp:13
inverse-kinematics-3d.it
int it
Definition: inverse-kinematics-3d.py:17
pinocchio::JointModelRevoluteTpl
Definition: multibody/joint/fwd.hpp:33
anymal-simulation.mass
mass
Definition: anymal-simulation.py:62
pinocchio::ModelTpl::gravity
Motion gravity
Spatial gravity of the model.
Definition: multibody/model.hpp:191
pinocchio::q
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
Definition: joint-configuration.hpp:1083
p1
tuple p1
quadrotor-ocp.cm
cm
Definition: quadrotor-ocp.py:22
pinocchio::JointModelCompositeTpl
Definition: multibody/joint/fwd.hpp:141
pinocchio::mjcf::buildModel
ModelTpl< Scalar, Options, JointCollectionTpl > & buildModel(const std::string &filename, ModelTpl< Scalar, Options, JointCollectionTpl > &model, const bool verbose=false)
Build the model from a MJCF file with a fixed joint as root of the model tree.
createTempFile
static TemporaryFileHolder createTempFile(const std::istringstream &data)
Definition: mjcf.cpp:52
pinocchio::ModelTpl::damping
TangentVectorType damping
Vector of joint damping parameters.
Definition: multibody/model.hpp:161
pinocchio::ModelTpl::nv
int nv
Dimension of the velocity vector space.
Definition: multibody/model.hpp:101
pinocchio::ModelTpl::ConfigVectorMap
std::map< std::string, ConfigVectorType > ConfigVectorMap
Map between a string (key) and a configuration vector.
Definition: multibody/model.hpp:90
pinocchio::SE3Tpl::Identity
static SE3Tpl Identity()
Definition: spatial/se3-tpl.hpp:136
graph
Definition: graph.py:1
pinocchio::mjcf::details::MjcfBody
All Bodies informations extracted from mjcf model.
Definition: mjcf-graph.hpp:89
joints.hpp
pinocchio::ModelTpl::effortLimit
TangentVectorType effortLimit
Vector of maximal joint torques.
Definition: multibody/model.hpp:164
pinocchio::InertiaTpl< context::Scalar, context::Options >::FromBox
static InertiaTpl FromBox(const Scalar mass, const Scalar x, const Scalar y, const Scalar z)
Computes the Inertia of a box defined by its mass and main dimensions (x,y,z).
Definition: spatial/inertia.hpp:423
pinocchio::GeometryModel
Definition: multibody/geometry.hpp:50
pinocchio::ModelTpl::parents
std::vector< JointIndex > parents
Vector of parent joint indexes. The parent of joint i, denoted li, corresponds to li==parents[i].
Definition: multibody/model.hpp:135
contact-cholesky.contact_models
list contact_models
Definition: contact-cholesky.py:22
pinocchio::COLLISION
@ COLLISION
Definition: multibody/geometry-object.hpp:27
pinocchio::InertiaTpl< context::Scalar, context::Options >::FromSphere
static InertiaTpl FromSphere(const Scalar mass, const Scalar radius)
Computes the Inertia of a sphere defined by its mass and its radius.
Definition: spatial/inertia.hpp:374
pinocchio::ModelTpl::idx_vs
std::vector< int > idx_vs
Starting index of the *i*th joint in the tangent configuration space.
Definition: multibody/model.hpp:128
pinocchio::ModelTpl
Definition: context/generic.hpp:20
pinocchio::ModelTpl::upperPositionLimit
ConfigVectorType upperPositionLimit
Upper joint configuration limit.
Definition: multibody/model.hpp:173
pinocchio::InertiaTpl< context::Scalar, context::Options >::FromCylinder
static InertiaTpl FromCylinder(const Scalar mass, const Scalar radius, const Scalar length)
Computes the Inertia of a cylinder defined by its mass, radius and length along the Z axis.
Definition: spatial/inertia.hpp:406
operator=
AABB & operator=(const AABB &other)=default
pinocchio::ModelTpl::nq
int nq
Dimension of the configuration vector representation.
Definition: multibody/model.hpp:98
pinocchio::mjcf::details::MjcfTexture
All informations related to a texture are stored here.
Definition: mjcf-graph.hpp:215
pinocchio::model
JointCollectionTpl & model
Definition: joint-configuration.hpp:1082
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:27
pinocchio::ModelTpl::njoints
int njoints
Number of joints.
Definition: multibody/model.hpp:104
hpp::fcl::Box
PINOCCHIO_MODEL_DIR
#define PINOCCHIO_MODEL_DIR
Definition: build-reduced-model.cpp:11

pinocchio
Author(s):
autogenerated on Sun Dec 22 2024 03:41:11