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