Debian Codegraph @

unittests/integration/test_MjcfParser.cpp

raw
1 /*
2 * Copyright (c) 2011-2022, The DART development contributors
3 * All rights reserved.
4 *
5 * The list of contributors can be found at:
6 * https://github.com/dartsim/dart/blob/master/LICENSE
7 *
8 * This file is provided under the following "BSD-style" License:
9 * Redistribution and use in source and binary forms, with or
10 * without modification, are permitted provided that the following
11 * conditions are met:
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
19 * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
20 * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
21 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
22 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
23 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
24 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
25 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
26 * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30 * POSSIBILITY OF SUCH DAMAGE.
31 */
32
33 #include <iostream>
34
35 #include <gtest/gtest.h>
36
37 #include "dart/dart.hpp"
38 #include "dart/utils/mjcf/detail/MujocoModel.hpp"
39 #include "dart/utils/mjcf/detail/Types.hpp"
40 #include "dart/utils/mjcf/detail/Utils.hpp"
41 #include "dart/utils/utils.hpp"
42
43 #include "TestHelpers.hpp"
44
45 using namespace dart;
46 using namespace utils::MjcfParser::detail;
47
48 //==============================================================================
49 common::ResourceRetrieverPtr createRetriever()
50 {
51 auto newRetriever = std::make_shared<utils::CompositeResourceRetriever>();
52 newRetriever->addSchemaRetriever(
53 "file", std::make_shared<common::LocalResourceRetriever>());
54 newRetriever->addSchemaRetriever(
55 "dart", utils::DartResourceRetriever::create());
56 return newRetriever;
57 }
58
59 //==============================================================================
60 TEST(MjcfParserTest, ParseDetailMujocoAnt)
61 {
62 const auto uri = "dart://sample/mjcf/openai/ant.xml";
63
64 auto mujoco = utils::MjcfParser::detail::MujocoModel();
65 auto errors = mujoco.read(uri, createRetriever());
66 ASSERT_TRUE(errors.empty());
67
68 EXPECT_EQ(mujoco.getModel(), "ant");
69
70 const auto& compiler = mujoco.getCompiler();
71 EXPECT_EQ(compiler.getAngle(), Angle::DEGREE);
72 EXPECT_EQ(compiler.getCoordinate(), Coordinate::LOCAL);
73
74 const auto& option = mujoco.getOption();
75 EXPECT_EQ(option.getIntegrator(), Integrator::RK4);
76 EXPECT_DOUBLE_EQ(option.getTimestep(), 0.01);
77
78 const auto& worldbody = mujoco.getWorldbody();
79
80 ASSERT_EQ(worldbody.getNumGeoms(), 1);
81
82 ASSERT_EQ(worldbody.getNumRootBodies(), 1);
83 const auto& rootBody0 = worldbody.getRootBody(0);
84 EXPECT_EQ(rootBody0.getName(), "torso");
85 EXPECT_TRUE(equals(
86 rootBody0.getRelativeTransform().translation().eval(),
87 Eigen::Vector3d(0, 0, 0.75)));
88
89 ASSERT_EQ(rootBody0.getNumJoints(), 1);
90 const auto& rootJoint0 = rootBody0.getJoint(0);
91 EXPECT_EQ(rootJoint0.getType(), JointType::FREE);
92 EXPECT_EQ(rootJoint0.getName(), "root");
93 }
94
95 //==============================================================================
96 TEST(MjcfParserTest, DefaultSettings)
97 {
98 // For the details, see http://mujoco.org/book/modeling.html#CDefault
99
100 const auto uri = "dart://sample/mjcf/test/default.xml";
101 const auto options = utils::MjcfParser::Options();
102 auto world = utils::MjcfParser::readWorld(uri);
103 ASSERT_NE(world, nullptr);
104
105 ASSERT_EQ(world->getNumSkeletons(), 2);
106
107 auto boxSkel = world->getSkeleton(options.mGeomSkeletonNamePrefix);
108 ASSERT_NE(boxSkel, nullptr);
109 auto boxShapeNode = boxSkel->getShapeNode(0);
110 ASSERT_NE(boxShapeNode, nullptr);
111 auto boxVisualAspect = boxShapeNode->getVisualAspect();
112 ASSERT_NE(boxVisualAspect, nullptr);
113 EXPECT_TRUE(boxVisualAspect->getRGBA().isApprox(Eigen::Vector4d(1, 0, 0, 1)));
114
115 auto bodySkel = world->getSkeleton("body0");
116 ASSERT_NE(bodySkel, nullptr);
117
118 auto ellipsoidShapeNode = bodySkel->getShapeNode(0);
119 ASSERT_NE(ellipsoidShapeNode, nullptr);
120 auto ellipsoidVisualAspect = ellipsoidShapeNode->getVisualAspect();
121 ASSERT_NE(ellipsoidVisualAspect, nullptr);
122 ellipsoidVisualAspect->getRGBA().isApprox(Eigen::Vector4d(0, 1, 0, 1));
123
124 auto sphereShapeNode = bodySkel->getShapeNode(1);
125 ASSERT_NE(sphereShapeNode, nullptr);
126 auto sphereVisualAspect = sphereShapeNode->getVisualAspect();
127 ASSERT_NE(sphereVisualAspect, nullptr);
128 sphereVisualAspect->getRGBA().isApprox(Eigen::Vector4d(0, 0, 1, 1));
129
130 auto cylinderShapeNode = bodySkel->getShapeNode(2);
131 ASSERT_NE(cylinderShapeNode, nullptr);
132 auto cylinderVisualAspect = cylinderShapeNode->getVisualAspect();
133 ASSERT_NE(cylinderVisualAspect, nullptr);
134 cylinderVisualAspect->getRGBA().isApprox(Eigen::Vector4d(1, 0, 0, 1));
135 }
136
137 //==============================================================================
138 TEST(MjcfParserTest, IncludeDefaultSettings)
139 {
140 // For the details, see http://mujoco.org/book/modeling.html#CDefault
141
142 const auto uri = "dart://sample/mjcf/test/include_main.xml";
143 const auto options = utils::MjcfParser::Options();
144 auto world = utils::MjcfParser::readWorld(uri);
145 ASSERT_NE(world, nullptr);
146
147 ASSERT_EQ(world->getNumSkeletons(), 2);
148
149 auto boxSkel = world->getSkeleton(options.mGeomSkeletonNamePrefix);
150 ASSERT_NE(boxSkel, nullptr);
151 auto boxShapeNode = boxSkel->getShapeNode(0);
152 ASSERT_NE(boxShapeNode, nullptr);
153 auto boxVisualAspect = boxShapeNode->getVisualAspect();
154 ASSERT_NE(boxVisualAspect, nullptr);
155 EXPECT_TRUE(boxVisualAspect->getRGBA().isApprox(Eigen::Vector4d(1, 0, 0, 1)));
156
157 auto bodySkel = world->getSkeleton("body0");
158 ASSERT_NE(bodySkel, nullptr);
159
160 auto ellipsoidShapeNode = bodySkel->getShapeNode(0);
161 ASSERT_NE(ellipsoidShapeNode, nullptr);
162 auto ellipsoidVisualAspect = ellipsoidShapeNode->getVisualAspect();
163 ASSERT_NE(ellipsoidVisualAspect, nullptr);
164 ellipsoidVisualAspect->getRGBA().isApprox(Eigen::Vector4d(0, 1, 0, 1));
165
166 auto sphereShapeNode = bodySkel->getShapeNode(1);
167 ASSERT_NE(sphereShapeNode, nullptr);
168 auto sphereVisualAspect = sphereShapeNode->getVisualAspect();
169 ASSERT_NE(sphereVisualAspect, nullptr);
170 sphereVisualAspect->getRGBA().isApprox(Eigen::Vector4d(0, 0, 1, 1));
171
172 auto cylinderShapeNode = bodySkel->getShapeNode(2);
173 ASSERT_NE(cylinderShapeNode, nullptr);
174 auto cylinderVisualAspect = cylinderShapeNode->getVisualAspect();
175 ASSERT_NE(cylinderVisualAspect, nullptr);
176 cylinderVisualAspect->getRGBA().isApprox(Eigen::Vector4d(1, 0, 0, 1));
177 }
178
179 //==============================================================================
180 TEST(MjcfParserTest, Ant)
181 {
182 const auto uri = "dart://sample/mjcf/openai/ant.xml";
183 const auto options = utils::MjcfParser::Options();
184 auto world = utils::MjcfParser::readWorld(uri);
185 ASSERT_NE(world, nullptr);
186
187 // The number of skeletons are the sum of the number of root <geom>s and root
188 // <body>s in <worldbody>. In this case, there are one root <geom> and one
189 // root <body> so it's 2.
190 ASSERT_EQ(world->getNumSkeletons(), 2);
191
192 auto antSkel = world->getSkeleton("torso");
193 ASSERT_NE(antSkel, nullptr);
194 // The number of bodies are 13: one for the torso and three bodies in each
195 // four legs.
196 ASSERT_EQ(antSkel->getNumBodyNodes(), 13);
197 // The ant model is free floating.
198 EXPECT_EQ(
199 antSkel->getRootJoint()->getType(), dynamics::FreeJoint::getStaticType());
200
201 auto floorSkele
202 = world->getSkeleton(options.mGeomSkeletonNamePrefix + "floor");
203 ASSERT_NE(floorSkele, nullptr);
204 }
205
206 //==============================================================================
207 TEST(MjcfParserTest, InvertedPendulum)
208 {
209 const auto uri = "dart://sample/mjcf/openai/inverted_pendulum.xml";
210 const auto options = utils::MjcfParser::Options();
211 auto world = utils::MjcfParser::readWorld(uri);
212 ASSERT_NE(world, nullptr);
213
214 // The number of skeletons are the sum of the number of root <geom>s and root
215 // <body>s in <worldbody>.
216 ASSERT_EQ(world->getNumSkeletons(), 2);
217
218 auto invertedPendulumSkel = world->getSkeleton("cart");
219 ASSERT_NE(invertedPendulumSkel, nullptr);
220 ASSERT_EQ(invertedPendulumSkel->getNumBodyNodes(), 2);
221 EXPECT_EQ(
222 invertedPendulumSkel->getRootJoint()->getType(),
223 dynamics::PrismaticJoint::getStaticType());
224
225 auto railSkel = world->getSkeleton(options.mGeomSkeletonNamePrefix + "rail");
226 ASSERT_NE(railSkel, nullptr);
227 }
228
229 //==============================================================================
230 TEST(MjcfParserTest, InvertedDoublePendulum)
231 {
232 const auto uri = "dart://sample/mjcf/openai/inverted_double_pendulum.xml";
233 const auto options = utils::MjcfParser::Options();
234 auto world = utils::MjcfParser::readWorld(uri);
235 ASSERT_NE(world, nullptr);
236
237 // The number of skeletons are the sum of the number of root <geom>s and root
238 // <body>s in <worldbody>.
239 ASSERT_EQ(world->getNumSkeletons(), 3);
240
241 auto invertedPendulumSkel = world->getSkeleton("cart");
242 ASSERT_NE(invertedPendulumSkel, nullptr);
243 ASSERT_EQ(invertedPendulumSkel->getNumBodyNodes(), 3);
244 EXPECT_EQ(
245 invertedPendulumSkel->getRootJoint()->getType(),
246 dynamics::PrismaticJoint::getStaticType());
247
248 auto floorSkele
249 = world->getSkeleton(options.mGeomSkeletonNamePrefix + "floor");
250 ASSERT_NE(floorSkele, nullptr);
251
252 auto railSkel = world->getSkeleton(options.mGeomSkeletonNamePrefix + "rail");
253 ASSERT_NE(railSkel, nullptr);
254 }
255
256 //==============================================================================
257 TEST(MjcfParserTest, Reacher)
258 {
259 const auto uri = "dart://sample/mjcf/openai/reacher.xml";
260 const auto options = utils::MjcfParser::Options();
261 auto world = utils::MjcfParser::readWorld(uri);
262 ASSERT_NE(world, nullptr);
263
264 // The number of skeletons are the sum of the number of root <geom>s and root
265 // <body>s in <worldbody>.
266 ASSERT_EQ(world->getNumSkeletons(), 8);
267
268 auto reacherSkel = world->getSkeleton("body0");
269 ASSERT_NE(reacherSkel, nullptr);
270 ASSERT_EQ(reacherSkel->getNumBodyNodes(), 3);
271 EXPECT_EQ(
272 reacherSkel->getRootJoint()->getType(),
273 dynamics::RevoluteJoint::getStaticType());
274
275 auto targetSkel = world->getSkeleton("target");
276 ASSERT_NE(targetSkel, nullptr);
277
278 EXPECT_TRUE(world->hasSkeleton(options.mGeomSkeletonNamePrefix + "ground"));
279 EXPECT_TRUE(world->hasSkeleton(options.mGeomSkeletonNamePrefix + "sideS"));
280 EXPECT_TRUE(world->hasSkeleton(options.mGeomSkeletonNamePrefix + "sideE"));
281 EXPECT_TRUE(world->hasSkeleton(options.mGeomSkeletonNamePrefix + "sideN"));
282 EXPECT_TRUE(world->hasSkeleton(options.mGeomSkeletonNamePrefix + "sideW"));
283 EXPECT_TRUE(world->hasSkeleton(options.mGeomSkeletonNamePrefix + "root"));
284 }
285
286 //==============================================================================
287 TEST(MjcfParserTest, Striker)
288 {
289 const auto uri = "dart://sample/mjcf/openai/striker.xml";
290 const auto options = utils::MjcfParser::Options();
291 auto world = utils::MjcfParser::readWorld(uri);
292 ASSERT_NE(world, nullptr);
293
294 // The number of skeletons are the sum of the number of root <geom>s and root
295 // <body>s in <worldbody>.
296 ASSERT_EQ(world->getNumSkeletons(), 4);
297
298 auto strikerSkel = world->getSkeleton("r_shoulder_pan_link");
299 ASSERT_NE(strikerSkel, nullptr);
300 ASSERT_EQ(strikerSkel->getNumBodyNodes(), 10);
301 EXPECT_EQ(
302 strikerSkel->getRootJoint()->getType(),
303 dynamics::RevoluteJoint::getStaticType());
304
305 auto objectSkel = world->getSkeleton("object");
306 ASSERT_NE(objectSkel, nullptr);
307
308 auto goalSkel = world->getSkeleton("goal");
309 ASSERT_NE(goalSkel, nullptr);
310
311 EXPECT_TRUE(world->hasSkeleton(options.mGeomSkeletonNamePrefix + "table"));
312 }
313
314 //==============================================================================
315 TEST(MjcfParserTest, Thrower)
316 {
317 const auto uri = "dart://sample/mjcf/openai/thrower.xml";
318 const auto options = utils::MjcfParser::Options();
319 auto world = utils::MjcfParser::readWorld(uri);
320 ASSERT_NE(world, nullptr);
321
322 // The number of skeletons are the sum of the number of root <geom>s and root
323 // <body>s in <worldbody>.
324 ASSERT_EQ(world->getNumSkeletons(), 4);
325
326 auto throwerSkel = world->getSkeleton("r_shoulder_pan_link");
327 ASSERT_NE(throwerSkel, nullptr);
328 ASSERT_EQ(throwerSkel->getNumBodyNodes(), 16);
329 EXPECT_EQ(
330 throwerSkel->getRootJoint()->getType(),
331 dynamics::RevoluteJoint::getStaticType());
332
333 auto goalSkel = world->getSkeleton("goal");
334 ASSERT_NE(goalSkel, nullptr);
335
336 auto ballSkel = world->getSkeleton("ball");
337 ASSERT_NE(ballSkel, nullptr);
338
339 EXPECT_TRUE(world->hasSkeleton(options.mGeomSkeletonNamePrefix + ""));
340 }
341
342 //==============================================================================
343 TEST(MjcfParserTest, RoboticsFetch)
344 {
345 const common::Uri uri
346 = "dart://sample/mjcf/openai/robotics/fetch/pick_and_place.xml";
347 auto world = utils::MjcfParser::readWorld(uri);
348 ASSERT_NE(world, nullptr);
349
350 ASSERT_EQ(world->getNumSkeletons(), 6);
351 }