18 #include <boost/test/unit_test.hpp>
19 #include <boost/filesystem/fstream.hpp>
24 struct TemporaryFileHolder
27 :
path(
boost::filesystem::temp_directory_path() /
boost::filesystem::unique_path())
31 ~TemporaryFileHolder()
33 boost::filesystem::remove(
path);
36 TemporaryFileHolder(
const TemporaryFileHolder &) =
delete;
37 TemporaryFileHolder &
operator=(
const TemporaryFileHolder &) =
delete;
39 TemporaryFileHolder(TemporaryFileHolder &&) =
default;
40 TemporaryFileHolder &
operator=(TemporaryFileHolder &&) =
default;
42 std::string
name()
const
54 TemporaryFileHolder temp_file;
57 boost::filesystem::ofstream file_stream(temp_file.path);
60 std::cerr <<
"Error opening file for writing" << std::endl;
63 file_stream <<
data.rdbuf();
73 if (pt1.size() != pt2.size())
77 for (
auto it1 = pt1.begin(), it2 = pt2.begin(); it1 != pt1.end(); ++it1, ++it2)
80 if (it1->first != it2->first)
84 if (it1->second.data() != it2->second.data())
95 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
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"/>
111 boost::property_tree::read_xml(xmlData, pt);
113 typedef ::pinocchio::mjcf::details::MjcfGraph MjcfGraph;
117 MjcfGraph
graph(visitor,
"fakeMjcf");
122 Matrix3 inertia_matrix;
124 inertia_matrix << 0.00315, 8.2904e-7, 0.00015, 8.2904e-7, 0.00388, 8.2299e-6, 0.00015, 8.2299e-6,
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"/>
145 boost::property_tree::read_xml(xmlData, pt);
147 typedef ::pinocchio::mjcf::details::MjcfGraph MjcfGraph;
151 MjcfGraph
graph(visitor,
"fakeMjcf");
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;
168 double pi = boost::math::constants::pi<double>();
170 std::istringstream xmlData(R
"(<mujoco model="inertiaFromGeom">
171 <compiler inertiafromgeom="true" />
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"/>
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"/>
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"/>
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"/>
197 typedef ::pinocchio::mjcf::details::MjcfGraph MjcfGraph;
201 MjcfGraph
graph(visitor,
"fakeMjcf");
202 graph.parseGraphFromXML(namefile.name());
206 double mass = 1000 * std::pow(0.01, 2) * 0.25 * pi * 2;
208 Eigen::Vector2d sizeCy;
209 sizeCy << 0.01, 0.25;
218 bodyTest =
graph.mapOfBodies.at(
"bodyBox");
219 mass = 1000 * 0.02 * 0.02 * 0.5;
221 Eigen::Vector3d sizeB;
222 sizeB << 0.02, 0.02, .5;
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);
234 Eigen::Vector2d sizeCa;
235 sizeCa << 0.01, 0.25;
244 bodyTest =
graph.mapOfBodies.at(
"bodySphere");
245 mass = 1000 * 4.0 / 3 * pi * std::pow(0.01, 3);
247 Eigen::VectorXd sizeS(1);
257 bodyTest =
graph.mapOfBodies.at(
"bodyEllip");
258 mass = 1000 * 4.0 / 3 * pi * 0.01 * 0.01 * 0.25;
260 Eigen::Vector3d sizeEl;
261 sizeEl << 0.01, 0.01, 0.25;
276 std::istringstream xmlData(R
"(<mujoco model="inertiaFromGeom">
277 <compiler inertiafromgeom="true" />
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"/>
288 typedef ::pinocchio::mjcf::details::MjcfGraph MjcfGraph;
292 MjcfGraph
graph(visitor,
"fakeMjcf");
293 graph.parseGraphFromXML(namefile.name());
294 graph.parseRootTree();
299 double massBigBox = 1000 * 0.02 * 0.02 * 0.04;
305 inertBigBox = placementBigBox.act(inertBigBox);
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"/>
328 boost::property_tree::read_xml(xmlData, pt);
330 typedef ::pinocchio::mjcf::details::MjcfGraph MjcfGraph;
334 MjcfGraph
graph(visitor,
"fakeMjcf");
336 graph.parseCompiler(pt.get_child(
"compiler"));
344 Matrix3 rotation_matrix;
345 rotation_matrix << 1., 0., 0., 0., 0., 1., 0., -1., 0.;
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));
360 namespace pt = boost::property_tree;
361 std::istringstream xmlIn(R
"(
363 <default class="mother">
364 <joint A="a0" B="b0" />
366 <default class="layer1">
367 <joint A="a1" C="c1" />
368 <default class="layer2">
373 <default class="layerP">
382 pt::read_xml(xmlIn, ptr, pt::xml_parser::trim_whitespace);
384 typedef ::pinocchio::mjcf::details::MjcfGraph MjcfGraph;
388 MjcfGraph
graph(visitor,
"fakeMjcf");
389 graph.parseDefault(ptr.get_child(
"default"), ptr);
391 std::unordered_map<std::string, pt::ptree> TrueMap;
393 std::istringstream xml1(R
"(<default class="mother">
394 <joint A="a0" B="b0" />
396 <default class="layer1">
397 <joint A="a1" C="c1" />
398 <default class="layer2">
403 <default class="layerP">
409 pt::read_xml(xml1, p1, pt::xml_parser::trim_whitespace);
413 std::istringstream xml2(R
"(<default class="layer1">
414 <joint A="a1" B="b0" C="c1" />
415 <default class="layer2">
422 pt::read_xml(xml2, p2, pt::xml_parser::trim_whitespace);
425 std::string
name =
"layer1";
426 TrueMap.insert(std::make_pair(
name, p2.get_child(
"default")));
428 std::istringstream xml3(R
"(<default class="layer2">
429 <joint A="a1" B="b2" C="c1"/>
430 <geom A="a0" C="c2"/>
433 pt::read_xml(xml3, p3, pt::xml_parser::trim_whitespace);
437 std::istringstream xml4(R
"(<default class="layerP">
438 <joint A="a0" B="b0" K="b2"/>
443 pt::read_xml(xml4, p4, pt::xml_parser::trim_whitespace);
448 #ifdef PINOCCHIO_WITH_URDFDOM
460 std::string file_u =
PINOCCHIO_MODEL_DIR + std::string(
"/../unittest/models/test_mjcf.urdf");
473 #endif // PINOCCHIO_WITH_URDFDOM
479 std::istringstream xmlDataNoStrip(R
"(<mujoco model="parseDirs">
480 <compiler meshdir="meshes" texturedir="textures"/>
482 <texture name="testTexture" file="texture.png" type="2d"/>
483 <material name="matTest" texture="testTexture"/>
484 <mesh file="C://auto/mesh.obj"/>
488 std::istringstream xmlDataNoStrip(R
"(<mujoco model="parseDirs">
489 <compiler meshdir="meshes" texturedir="textures"/>
491 <texture name="testTexture" file="texture.png" type="2d"/>
492 <material name="matTest" texture="testTexture"/>
493 <mesh file="/auto/mesh.obj"/>
500 typedef ::pinocchio::mjcf::details::MjcfGraph MjcfGraph;
504 MjcfGraph
graph(visitor,
"/fakeMjcf/fake.xml");
505 graph.parseGraphFromXML(namefile.name());
509 BOOST_CHECK_EQUAL(text.
textType,
"2d");
514 BOOST_CHECK_EQUAL(
mat.texture,
"testTexture");
528 std::istringstream xmlDataNoStrip(R
"(<mujoco model="parseDirs">
529 <compiler meshdir="meshes" texturedir="textures" strippath="true"/>
531 <mesh file="/auto/mesh.obj"/>
537 typedef ::pinocchio::mjcf::details::MjcfGraph MjcfGraph;
541 MjcfGraph
graph(visitor,
"/fakeMjcf/fake.xml");
542 graph.parseGraphFromXML(namefile.name());
556 std::istringstream xmlData(R
"(
557 <mujoco model="model_RX">
560 <body name="link1" pos="0 0 0">
561 <joint name="joint1" type="hinge" axis="1 0 0"/>
569 typedef ::pinocchio::mjcf::details::MjcfGraph MjcfGraph;
573 MjcfGraph
graph(visitor,
"fakeMjcf");
574 graph.parseGraphFromXML(namefile.name());
575 graph.parseRootTree();
593 std::istringstream xmlData(R
"(
594 <mujoco model="model_PX">
597 <body name="link1" pos="0 0 0">
598 <joint name="joint1" type="slide" axis="1 0 0"/>
606 typedef ::pinocchio::mjcf::details::MjcfGraph MjcfGraph;
610 MjcfGraph
graph(visitor,
"fakeMjcf");
611 graph.parseGraphFromXML(namefile.name());
612 graph.parseRootTree();
630 std::istringstream xmlData(R
"(
631 <mujoco model="model_Sphere">
634 <body name="link1" pos="0 0 0">
635 <joint name="joint1" type="ball"/>
643 typedef ::pinocchio::mjcf::details::MjcfGraph MjcfGraph;
647 MjcfGraph
graph(visitor,
"fakeMjcf");
648 graph.parseGraphFromXML(namefile.name());
649 graph.parseRootTree();
667 std::istringstream xmlData(R
"(
668 <mujoco model="model_Free">
671 <body name="link1" pos="0 0 0">
672 <freejoint name="joint1"/>
680 typedef ::pinocchio::mjcf::details::MjcfGraph MjcfGraph;
684 MjcfGraph
graph(visitor,
"fakeMjcf");
685 graph.parseGraphFromXML(namefile.name());
686 graph.parseRootTree();
704 std::istringstream xmlData(R
"(
705 <mujoco model="composite_RXRY">
708 <body name="link1" pos="0 0 0">
709 <joint name="joint1" type="hinge" axis="1 0 0"/>
710 <joint name="joint2" type="hinge" axis="0 1 0"/>
718 typedef ::pinocchio::mjcf::details::MjcfGraph MjcfGraph;
722 MjcfGraph
graph(visitor,
"fakeMjcf");
723 graph.parseGraphFromXML(namefile.name());
724 graph.parseRootTree();
746 std::istringstream xmlData(R
"(
747 <mujoco model="composite_PXPY">
750 <body name="link1" pos="0 0 0">
751 <joint name="joint1" type="slide" axis="1 0 0"/>
752 <joint name="joint2" type="slide" axis="0 1 0"/>
760 typedef ::pinocchio::mjcf::details::MjcfGraph MjcfGraph;
764 MjcfGraph
graph(visitor,
"fakeMjcf");
765 graph.parseGraphFromXML(namefile.name());
766 graph.parseRootTree();
788 std::istringstream xmlData(R
"(
789 <mujoco model="composite_PXRY">
792 <body name="link1" pos="0 0 0">
793 <joint name="joint1" type="slide" axis="1 0 0"/>
794 <joint name="joint2" type="hinge" axis="0 1 0"/>
802 typedef ::pinocchio::mjcf::details::MjcfGraph MjcfGraph;
806 MjcfGraph
graph(visitor,
"fakeMjcf");
807 graph.parseGraphFromXML(namefile.name());
808 graph.parseRootTree();
830 std::istringstream xmlData(R
"(
831 <mujoco model="composite_PXSphere">
834 <body name="link1" pos="0 0 0">
835 <joint name="joint1" type="slide" axis="1 0 0"/>
836 <joint name="joint2" type="ball"/>
844 typedef ::pinocchio::mjcf::details::MjcfGraph MjcfGraph;
848 MjcfGraph
graph(visitor,
"fakeMjcf");
849 graph.parseGraphFromXML(namefile.name());
850 graph.parseRootTree();
878 q << 1.57079633, 0.3, 0.5;
884 Eigen::Matrix3d refOrient;
885 refOrient << 0, -.9553, .2955, 1, 0, 0, 0, .2955, .9553;
887 pos << .8522, 2, 0.5223;
890 f_id =
model.getFrameId(
"body2");
891 pinPos =
data.oMf[f_id];
893 refOrient << 0, -.9553, .2955, 1, 0, 0, 0, .2955, .9553;
895 pos << .8522, 4, 0.5223;
902 std::istringstream xmlData(R
"(
903 <mujoco model="testKeyFrame">
905 <position ctrllimited="true" ctrlrange="-.1 .1" kp="30"/>
906 <default class="joint">
907 <geom type="cylinder" size=".006" fromto="0 0 0 0 0 .05" rgba=".9 .6 1 1"/>
913 <geom type="capsule" size=".01" fromto="0 0 0 .2 0 0"/>
914 <body pos=".2 0 0" name="body2">
915 <joint type="ball" damping=".1"/>
916 <geom type="capsule" size=".01" fromto="0 -.15 0 0 0 0"/>
923 0.988015 0 0.154359 0
924 0.988015 0 0.154359 0"/>
935 Eigen::VectorXd vect_ref(model_m.
nq);
936 vect_ref << 0, 0, 0.596, 0, 0.154359, 0, 0.988015, 0, 0.154359, 0, 0.988015;
945 std::istringstream xmlData(R
"(
946 <mujoco model="testRefPose">
947 <compiler angle="radian"/>
950 <body pos=".2 0 0" name="body2">
951 <joint type="slide" ref="0.14"/>
952 <body pos=".2 0 0" name="body3">
953 <joint type="hinge" ref="0.1"/>
959 <key name="test" qpos=".8 .5"/>
969 Eigen::VectorXd vect_ref(model_m.
nq);
970 vect_ref << 0.66, 0.4;
979 std::istringstream xmlData(R
"(
980 <mujoco model="keyframeComposite">
981 <compiler angle="radian"/>
984 <body pos=".2 0 0" name="body2">
985 <joint type="slide"/>
986 <joint type="hinge"/>
987 <body pos=".2 0 0" name="body3">
988 <joint type="hinge"/>
994 <key name="test" qpos=".8 .5 .5"/>
1004 Eigen::VectorXd vect_ref(model_m.
nq);
1005 vect_ref << 0.8, 0.5, 0.5;
1007 BOOST_CHECK(vect_model.size() == vect_ref.size());
1017 std::istringstream xmlData(R
"(
1018 <mujoco model="site">
1019 <compiler angle="radian"/>
1022 <body pos=".2 0 0" name="body2">
1023 <joint type="hinge"/>
1024 <body pos=".2 0 0" name="body3">
1025 <joint type="hinge"/>
1026 <site name="testSite" pos="0.03 0 -0.05"/>
1040 Matrix3 rotation_matrix;
1041 rotation_matrix << 1., 0., 0., 0., 1., 0., 0., 0., 1.;
1057 BOOST_CHECK_EQUAL(model_m.
nq, 29);
1060 #ifdef PINOCCHIO_WITH_URDFDOM
1073 const std::string filename_urdf =
PINOCCHIO_MODEL_DIR + std::string(
"/simple_humanoid.urdf");
1095 typename ConfigVectorMap::const_iterator it_model_urdf =
1099 std::advance(it, k);
1100 std::advance(it_model_urdf, k);
1101 BOOST_CHECK(it->second.size() == it_model_urdf->second.size());
1133 for (
size_t k = 1; k < model_m.
inertias.size(); ++k)
1146 for (
size_t k = 1; k < model_urdf.
frames.size(); ++k)
1151 #endif // PINOCCHIO_WITH_URDFDOM
1153 #if defined(PINOCCHIO_WITH_HPP_FCL)
1160 std::istringstream xmlData(R
"(<mujoco model="inertiaFromGeom">
1161 <compiler inertiafromgeom="true" />
1163 <body pos="0 0 0" name="bodyCylinder">
1164 <geom type="cylinder" size=".01 0.25" pos="0 0 0" quat="1 0 0 0"/>
1165 <body pos="0 0 0" name="bodyBox">
1166 <geom type="box" size=".01 0.01 0.25" pos="0 0 0" quat="1 0 0 0"/>
1168 <body pos="0 0 0" name="bodyCapsule">
1169 <geom type="capsule" size=".01 0.25" pos="0 0 0" quat="1 0 0 0"/>
1171 <body pos="0 0 0" name="bodySphere">
1172 <geom type="sphere" size=".01" pos="0 0 0" quat="1 0 0 0"/>
1174 <body pos="0 0 0" name="bodyEllip">
1175 <geom type="ellipsoid" size=".01 0.01 0.25" pos="0 0 0" quat="1 0 0 0"/>
1207 Eigen::Vector3d sides;
1208 sides << 0.01, 0.01, 0.25;
1215 #endif // if defined(PINOCCHIO_WITH_HPP_FCL)
1217 BOOST_AUTO_TEST_SUITE_END()