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