Debian Codegraph @

unittests/integration/test_DartLoader.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/dynamics/FreeJoint.hpp"
38 #include "dart/dynamics/MeshShape.hpp"
39 #include "dart/dynamics/WeldJoint.hpp"
40 #include "dart/utils/urdf/DartLoader.hpp"
41
42 #include "TestHelpers.hpp"
43
44 using namespace dart;
45 using dart::common::Uri;
46 using dart::utils::DartLoader;
47
48 //==============================================================================
49 TEST(DartLoader, parseSkeleton_NonExistantPathReturnsNull)
50 {
51 DartLoader loader;
52 EXPECT_EQ(
53 nullptr,
54 loader.parseSkeleton("dart://sample/skel/test/does_not_exist.urdf"));
55 }
56
57 //==============================================================================
58 TEST(DartLoader, parseSkeleton_InvalidUrdfReturnsNull)
59 {
60 DartLoader loader;
61 EXPECT_EQ(
62 nullptr, loader.parseSkeleton("dart://sample/urdf/test/invalid.urdf)"));
63 }
64
65 //==============================================================================
66 TEST(DartLoader, parseSkeleton_MissingMeshReturnsNull)
67 {
68 DartLoader loader;
69 EXPECT_EQ(
70 nullptr,
71 loader.parseSkeleton("dart://sample/urdf/test/missing_mesh.urdf"));
72 }
73
74 //==============================================================================
75 TEST(DartLoader, parseSkeleton_InvalidMeshReturnsNull)
76 {
77 DartLoader loader;
78 EXPECT_EQ(
79 nullptr,
80 loader.parseSkeleton("dart://sample/urdf/test/invalid_mesh.urdf"));
81 }
82
83 //==============================================================================
84 TEST(DartLoader, parseSkeleton_MissingPackageReturnsNull)
85 {
86 DartLoader loader;
87 EXPECT_EQ(
88 nullptr,
89 loader.parseSkeleton("dart://sample/urdf/test/missing_package.urdf"));
90 }
91
92 //==============================================================================
93 TEST(DartLoader, parseSkeleton_LoadsPrimitiveGeometry)
94 {
95 DartLoader loader;
96 EXPECT_TRUE(
97 nullptr
98 != loader.parseSkeleton(
99 "dart://sample/urdf/test/primitive_geometry.urdf"));
100 }
101
102 //==============================================================================
103 TEST(DartLoader, parseWorld)
104 {
105 DartLoader loader;
106 EXPECT_TRUE(
107 nullptr != loader.parseWorld("dart://sample/urdf/test/testWorld.urdf"));
108 }
109
110 //==============================================================================
111 TEST(DartLoader, parseJointProperties)
112 {
113 // clang-format off
114 std::string urdfStr = R"(
115 <robot name="testRobot">
116 <link name="link_0">
117 <visual>
118 <origin rpy="0 0 0" xyz="0 0 -0.087" />
119 <geometry>
120 <box size="1 1 1" />
121 </geometry>
122 </visual>
123 <visual>
124 <origin rpy="0 0 0" xyz="0 0 0" />
125 <geometry>
126 <box size="1 1 1" />
127 </geometry>
128 </visual>
129 </link>
130 <joint name="0_to_1" type="revolute">
131 <parent link="link_0" />
132 <child link="link_1" />
133 <limit effort="2.5" lower="-3.14159265359"
134 upper="3.14159265359" velocity="3.00545697193" />
135 <axis xyz="0 0 1" />
136 <dynamics damping="1.2" friction="2.3" />
137 </joint>
138 <link name="link_1">
139 <visual>
140 <origin rpy="0 0 0" xyz="0 0 -0.087" />
141 <geometry>
142 <box size="1 1 1" />
143 </geometry>
144 </visual>
145 <visual>
146 <origin rpy="0 0 0" xyz="0 0 0" />
147 <geometry>
148 <box size="1 1 1" />
149 </geometry>
150 </visual>
151 </link>
152 <joint name="1_to_2" type="continuous">
153 <parent link="link_1" />
154 <child link="link_2" />
155 <limit effort="2.5" velocity="3.00545697193" />
156 <axis xyz="0 0 1" />
157 <dynamics damping="1.2" friction="2.3" />
158 </joint>
159 <link name="link_2">
160 <visual>
161 <origin rpy="0 0 0" xyz="0 0 -0.087" />
162 <geometry>
163 <box size="1 1 1" />
164 </geometry>
165 </visual>
166 <visual>
167 <origin rpy="0 0 0" xyz="0 0 0" />
168 <geometry>
169 <box size="1 1 1" />
170 </geometry>
171 </visual>
172 </link>
173 </robot>
174 )";
175 // clang-format on
176
177 DartLoader loader;
178 auto robot = loader.parseSkeletonString(urdfStr, "");
179 ASSERT_TRUE(nullptr != robot);
180
181 auto joint1 = robot->getJoint(1);
182 ASSERT_TRUE(nullptr != joint1);
183 EXPECT_NEAR(joint1->getDampingCoefficient(0), 1.2, 1e-12);
184 EXPECT_NEAR(joint1->getCoulombFriction(0), 2.3, 1e-12);
185
186 auto joint2 = robot->getJoint(2);
187 EXPECT_DOUBLE_EQ(
188 joint2->getPositionLowerLimit(0), -dart::math::constantsd::inf());
189 EXPECT_DOUBLE_EQ(
190 joint2->getPositionUpperLimit(0), dart::math::constantsd::inf());
191 EXPECT_TRUE(joint2->isCyclic(0));
192 }
193
194 //==============================================================================
195 TEST(DartLoader, parseUrdfWithoutWorldLink)
196 {
197 // clang-format off
198 const std::string urdfStr = R"(
199 <robot name="testRobot">
200 <link name="link_0" />
201 <joint name="0_to_1" type="revolute">
202 <parent link="link_0" />
203 <child link="link_1" />
204 <limit effort="2.5" lower="-3.14159265359"
205 upper="3.14159265359" velocity="3.00545697193" />
206 <axis xyz="0 0 1" />
207 </joint>
208 <link name="link_1" />
209 </robot>
210 )";
211 // clang-format on
212
213 DartLoader loader;
214 DartLoader::Options options;
215
216 // Default
217 auto robot1 = loader.parseSkeletonString(urdfStr, "");
218 ASSERT_TRUE(nullptr != robot1);
219 EXPECT_EQ(
220 robot1->getRootJoint()->getType(), dynamics::FreeJoint::getStaticType());
221 EXPECT_EQ(robot1->getRootJoint()->getName(), "rootJoint");
222
223 // Floating
224 options.mDefaultRootJointType = DartLoader::RootJointType::FLOATING;
225 loader.setOptions(options);
226 auto robot2 = loader.parseSkeletonString(urdfStr, "");
227 ASSERT_TRUE(nullptr != robot2);
228 EXPECT_EQ(
229 robot2->getRootJoint()->getType(), dynamics::FreeJoint::getStaticType());
230 EXPECT_EQ(robot2->getRootJoint()->getName(), "rootJoint");
231
232 // Fixed
233 options.mDefaultRootJointType = DartLoader::RootJointType::FIXED;
234 loader.setOptions(options);
235 auto robot3 = loader.parseSkeletonString(urdfStr, "");
236 ASSERT_TRUE(nullptr != robot3);
237 EXPECT_EQ(
238 robot3->getRootJoint()->getType(), dynamics::WeldJoint::getStaticType());
239 EXPECT_EQ(robot3->getRootJoint()->getName(), "rootJoint");
240 }
241
242 //==============================================================================
243 TEST(DartLoader, mimicJoint)
244 {
245 // clang-format off
246 std::string urdfStr = R"(
247 <robot name="testRobot">
248 <link name="link_0">
249 <visual>
250 <origin rpy="0 0 0" xyz="0 0 -0.087" />
251 <geometry>
252 <box size="1 1 1" />
253 </geometry>
254 </visual>
255 <visual>
256 <origin rpy="0 0 0" xyz="0 0 0" />
257 <geometry>
258 <box size="1 1 1" />
259 </geometry>
260 </visual>
261 </link>
262 <joint name="0_to_1" type="revolute">
263 <parent link="link_0" />
264 <child link="link_1" />
265 <limit effort="2.5" lower="-3.14159265359"
266 upper="3.14159265359" velocity="3.00545697193" />
267 <axis xyz="0 0 1" />
268 <dynamics damping="1.2" friction="2.3" />
269 </joint>
270 <link name="link_1">
271 <visual>
272 <origin rpy="0 0 0" xyz="0 0 -0.087" />
273 <geometry>
274 <box size="1 1 1" />
275 </geometry>
276 </visual>
277 <visual>
278 <origin rpy="0 0 0" xyz="0 0 0" />
279 <geometry>
280 <box size="1 1 1" />
281 </geometry>
282 </visual>
283 </link>
284 <joint name="1_to_2" type="continuous">
285 <parent link="link_1" />
286 <child link="link_2" />
287 <limit effort="2.5" velocity="3.00545697193" />
288 <axis xyz="0 0 1" />
289 <dynamics damping="1.2" friction="2.3" />
290 <mimic joint="0_to_1" multiplier="2." offset="0.1" />
291 </joint>
292 <link name="link_2">
293 <visual>
294 <origin rpy="0 0 0" xyz="0 0 -0.087" />
295 <geometry>
296 <box size="1 1 1" />
297 </geometry>
298 </visual>
299 <visual>
300 <origin rpy="0 0 0" xyz="0 0 0" />
301 <geometry>
302 <box size="1 1 1" />
303 </geometry>
304 </visual>
305 </link>
306 </robot>
307 )";
308 // clang-format on
309
310 DartLoader loader;
311 auto robot = loader.parseSkeletonString(urdfStr, "");
312 ASSERT_TRUE(nullptr != robot);
313
314 auto joint1 = robot->getJoint(1);
315 ASSERT_TRUE(nullptr != joint1);
316 EXPECT_NEAR(joint1->getDampingCoefficient(0), 1.2, 1e-12);
317 EXPECT_NEAR(joint1->getCoulombFriction(0), 2.3, 1e-12);
318
319 auto joint2 = robot->getJoint(2);
320 EXPECT_DOUBLE_EQ(
321 joint2->getPositionLowerLimit(0), -dart::math::constantsd::inf());
322 EXPECT_DOUBLE_EQ(
323 joint2->getPositionUpperLimit(0), dart::math::constantsd::inf());
324 EXPECT_TRUE(joint2->isCyclic(0));
325
326 EXPECT_TRUE(joint2->getActuatorType() == dart::dynamics::Joint::MIMIC);
327 EXPECT_TRUE(nullptr != joint2->getMimicJoint());
328 EXPECT_DOUBLE_EQ(joint2->getMimicMultiplier(), 2.);
329 EXPECT_DOUBLE_EQ(joint2->getMimicOffset(), 0.1);
330 }
331
332 //==============================================================================
333 TEST(DartLoader, badMimicJoint)
334 {
335 // clang-format off
336 std::string urdfStr = R"(
337 <robot name="testRobot">
338 <link name="link_0">
339 <visual>
340 <origin rpy="0 0 0" xyz="0 0 -0.087" />
341 <geometry>
342 <box size="1 1 1" />
343 </geometry>
344 </visual>
345 <visual>
346 <origin rpy="0 0 0" xyz="0 0 0" />
347 <geometry>
348 <box size="1 1 1" />
349 </geometry>
350 </visual>
351 </link>
352 <joint name="0_to_1" type="revolute">
353 <parent link="link_0" />
354 <child link="link_1" />
355 <limit effort="2.5" lower="-3.14159265359"
356 upper="3.14159265359" velocity="3.00545697193" />
357 <axis xyz="0 0 1" />
358 <dynamics damping="1.2" friction="2.3" />
359 </joint>
360 <link name="link_1">
361 <visual>
362 <origin rpy="0 0 0" xyz="0 0 -0.087" />
363 <geometry>
364 <box size="1 1 1" />
365 </geometry>
366 </visual>
367 <visual>
368 <origin rpy="0 0 0" xyz="0 0 0" />
369 <geometry>
370 <box size="1 1 1" />
371 </geometry>
372 </visual>
373 </link>
374 <joint name="1_to_2" type="continuous">
375 <parent link="link_1" />
376 <child link="link_2" />
377 <limit effort="2.5" velocity="3.00545697193" />
378 <axis xyz="0 0 1" />
379 <dynamics damping="1.2" friction="2.3" />
380 <mimic joint="mjoint" multiplier="2." offset="0.1" />
381 </joint>
382 <link name="link_2">
383 <visual>
384 <origin rpy="0 0 0" xyz="0 0 -0.087" />
385 <geometry>
386 <box size="1 1 1" />
387 </geometry>
388 </visual>
389 <visual>
390 <origin rpy="0 0 0" xyz="0 0 0" />
391 <geometry>
392 <box size="1 1 1" />
393 </geometry>
394 </visual>
395 </link>
396 </robot>
397 )";
398 // clang-format on
399
400 DartLoader loader;
401 auto robot = loader.parseSkeletonString(urdfStr, "");
402 ASSERT_TRUE(nullptr != robot);
403
404 auto joint1 = robot->getJoint(1);
405 ASSERT_TRUE(nullptr != joint1);
406 EXPECT_NEAR(joint1->getDampingCoefficient(0), 1.2, 1e-12);
407 EXPECT_NEAR(joint1->getCoulombFriction(0), 2.3, 1e-12);
408
409 auto joint2 = robot->getJoint(2);
410 EXPECT_DOUBLE_EQ(
411 joint2->getPositionLowerLimit(0), -dart::math::constantsd::inf());
412 EXPECT_DOUBLE_EQ(
413 joint2->getPositionUpperLimit(0), dart::math::constantsd::inf());
414 EXPECT_TRUE(joint2->isCyclic(0));
415
416 EXPECT_TRUE(joint2->getActuatorType() != dart::dynamics::Joint::MIMIC);
417 EXPECT_TRUE(nullptr == joint2->getMimicJoint());
418 }
419
420 //==============================================================================
421 TEST(DartLoader, WorldShouldBeTreatedAsKeyword)
422 {
423 // clang-format off
424 const std::string urdfStr = R"(
425 <robot name="testRobot">
426 <link name="world" />
427 <joint name="world_to_1" type="revolute">
428 <parent link="world" />
429 <child link="link_0" />
430 <axis xyz="0 0 1" />
431 <limit effort="2.5" lower="-3.14159265359"
432 upper="3.14159265359" velocity="3.00545697193" />
433 <axis xyz="0 0 1" />
434 <dynamics damping="1.2" friction="2.3" />
435 </joint>
436 <link name="link_0" />
437 </robot>
438 )";
439 // clang-format on
440
441 DartLoader loader;
442 auto robot = loader.parseSkeletonString(urdfStr, "");
443 ASSERT_TRUE(nullptr != robot);
444
445 EXPECT_TRUE(robot->getNumBodyNodes() == 1u);
446 EXPECT_TRUE(robot->getRootBodyNode()->getName() == "link_0");
447 }
448
449 //==============================================================================
450 TEST(DartLoader, SingleLinkWithoutJoint)
451 {
452 // clang-format off
453 const std::string urdfStr = R"(
454 <robot name="testRobot">
455 <link name="link_0" />
456 </robot>
457 )";
458 // clang-format on
459
460 DartLoader loader;
461 auto robot = loader.parseSkeletonString(urdfStr, "");
462 ASSERT_TRUE(nullptr != robot);
463
464 EXPECT_TRUE(robot->getNumBodyNodes() == 1u);
465 EXPECT_TRUE(robot->getRootBodyNode()->getName() == "link_0");
466 EXPECT_EQ(
467 robot->getRootJoint()->getType(),
468 dart::dynamics::FreeJoint::getStaticType());
469 }
470
471 //==============================================================================
472 TEST(DartLoader, MultiTreeRobot)
473 {
474 // clang-format off
475 const std::string urdfStr = R"(
476 <robot name="testRobot">
477 <link name="world" />
478 <joint name="world_to_0" type="revolute">
479 <parent link="world" />
480 <child link="link_0" />
481 <axis xyz="0 0 1" />
482 <limit effort="2.5" lower="-3.14159265359"
483 upper="3.14159265359" velocity="3.00545697193" />
484 <axis xyz="0 0 1" />
485 <dynamics damping="1.2" friction="2.3" />
486 </joint>
487 <link name="link_0" />
488 <joint name="world_to_1" type="revolute">
489 <parent link="world" />
490 <child link="link_1" />
491 <axis xyz="0 0 1" />
492 <limit effort="2.5" lower="-3.14159265359"
493 upper="3.14159265359" velocity="3.00545697193" />
494 <axis xyz="0 0 1" />
495 <dynamics damping="1.2" friction="2.3" />
496 </joint>
497 <link name="link_1" />
498 </robot>
499 )";
500 // clang-format on
501
502 DartLoader loader;
503 auto robot = loader.parseSkeletonString(urdfStr, "");
504 ASSERT_TRUE(nullptr != robot);
505
506 EXPECT_EQ(robot->getNumBodyNodes(), 2u);
507 EXPECT_EQ(robot->getNumTrees(), 2u);
508 }
509
510 //==============================================================================
511 TEST(DartLoader, KR5MeshColor)
512 {
513 DartLoader loader;
514 auto robot
515 = loader.parseSkeleton("dart://sample/urdf/KR5/KR5 sixx R650.urdf");
516 ASSERT_TRUE(nullptr != robot);
517
518 EXPECT_EQ(robot->getNumBodyNodes(), 7u);
519 EXPECT_EQ(robot->getNumTrees(), 1u);
520
521 robot->eachBodyNode([](dynamics::BodyNode* bodyNode) {
522 bodyNode->eachShapeNodeWith<dynamics::VisualAspect>(
523 [&](dynamics::ShapeNode* shapeNode) {
524 auto shape = shapeNode->getShape();
525 if (auto mesh = shape->as<dynamics::MeshShape>())
526 {
527 EXPECT_EQ(mesh->getColorMode(), dynamics::MeshShape::SHAPE_COLOR);
528 }
529 });
530 });
531 }
532
533 //==============================================================================
534 TEST(DartLoader, parseVisualCollisionName)
535 {
536 // clang-format off
537 std::string urdfStr = R"(
538 <robot name="testRobot">
539 <link name="link_0">
540 <visual name="first_visual">
541 <origin rpy="0 0 0" xyz="0 0 -0.087" />
542 <geometry>
543 <box size="1 1 1" />
544 </geometry>
545 </visual>
546 <visual name="second_visual">
547 <origin rpy="0 0 0" xyz="0 0 0" />
548 <geometry>
549 <box size="1 1 1" />
550 </geometry>
551 </visual>
552 </link>
553 <joint name="0_to_1" type="revolute">
554 <parent link="link_0" />
555 <child link="link_1" />
556 <limit effort="2.5" lower="-3.14159265359"
557 upper="3.14159265359" velocity="3.00545697193" />
558 <axis xyz="0 0 1" />
559 <dynamics damping="1.2" friction="2.3" />
560 </joint>
561 <link name="link_1">
562 <visual>
563 <origin rpy="0 0 0" xyz="0 0 -0.087" />
564 <geometry>
565 <box size="1 1 1" />
566 </geometry>
567 </visual>
568 <visual>
569 <origin rpy="0 0 0" xyz="0 0 0" />
570 <geometry>
571 <box size="1 1 1" />
572 </geometry>
573 </visual>
574 </link>
575 </robot>
576 )";
577 // clang-format on
578
579 DartLoader loader;
580 auto robot = loader.parseSkeletonString(urdfStr, "");
581 ASSERT_TRUE(nullptr != robot);
582
583 auto body0 = robot->getBodyNode(0);
584 ASSERT_TRUE(nullptr != body0);
585 ASSERT_EQ(body0->getNumShapeNodes(), 2);
586 EXPECT_EQ(body0->getShapeNode(0)->getName(), "first_visual");
587 EXPECT_EQ(body0->getShapeNode(1)->getName(), "second_visual");
588
589 auto body1 = robot->getBodyNode(1);
590 ASSERT_TRUE(nullptr != body1);
591 ASSERT_EQ(body1->getNumShapeNodes(), 2);
592 // The ShapeNode naming pattern is infered from
593 // BodyNode::createShapeNodeWith(const ShapePtr& shape) sot this test could
594 // fail if the naming pattern in the function is changed.
595 EXPECT_EQ(
596 body1->getShapeNode(0)->getName(), body1->getName() + "_ShapeNode_0");
597 EXPECT_EQ(
598 body1->getShapeNode(1)->getName(), body1->getName() + "_ShapeNode_1");
599 }
600
601 //==============================================================================
602 TEST(DartLoader, Options)
603 {
604 // clang-format off
605 std::string urdfStr = R"(
606 <robot name="testRobot">
607 <link name="link_0">
608 </link>
609 </robot>
610 )";
611 // clang-format on
612
613 DartLoader loader;
614 DartLoader::Options options;
615
616 // Default inertia
617 auto robot = loader.parseSkeletonString(urdfStr, "");
618 ASSERT_TRUE(nullptr != robot);
619 auto link_0 = robot->getBodyNode("link_0");
620 ASSERT_TRUE(link_0 != nullptr);
621 EXPECT_TRUE(equals(
622 link_0->getInertia().getSpatialTensor(),
623 dynamics::Inertia().getSpatialTensor()));
624
625 // Custom inertia
626 options.mDefaultInertia.setMass(5);
627 options.mDefaultInertia.setMoment(1, 2, 3, 4, 5, 6);
628 loader.setOptions(options);
629 robot = loader.parseSkeletonString(urdfStr, "");
630 ASSERT_TRUE(nullptr != robot);
631 link_0 = robot->getBodyNode("link_0");
632 ASSERT_TRUE(link_0 != nullptr);
633 EXPECT_TRUE(equals(
634 link_0->getInertia().getSpatialTensor(),
635 options.mDefaultInertia.getSpatialTensor()));
636 }