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 "TestHelpers.hpp"
36
37 #include "dart/dynamics/PlanarJoint.hpp"
38 #include "dart/dynamics/RevoluteJoint.hpp"
39 #include "dart/dynamics/Skeleton.hpp"
40 #include "dart/dynamics/SoftBodyNode.hpp"
41 #include "dart/simulation/World.hpp"
42 #include "dart/utils/sdf/SdfParser.hpp"
43
44 using namespace dart;
45 using namespace math;
46 using namespace dynamics;
47 using namespace simulation;
48 using namespace utils;
49
50 //==============================================================================
TEST(SdfParser,SDFSingleBodyWithoutJoint)51 TEST(SdfParser, SDFSingleBodyWithoutJoint)
52 {
53 // Regression test for #444
54 WorldPtr world = SdfParser::readWorld(
55 "dart://sample/sdf/test/single_bodynode_skeleton.world");
56 EXPECT_TRUE(world != nullptr);
57
58 SkeletonPtr skel = world->getSkeleton(0);
59 EXPECT_TRUE(skel != nullptr);
60 EXPECT_EQ(skel->getNumBodyNodes(), 1u);
61 EXPECT_EQ(skel->getNumJoints(), 1u);
62
63 BodyNodePtr bodyNode = skel->getBodyNode(0);
64 EXPECT_TRUE(bodyNode != nullptr);
65 EXPECT_EQ(bodyNode->getNumShapeNodesWith<VisualAspect>(), 1u);
66 EXPECT_EQ(bodyNode->getNumShapeNodesWith<CollisionAspect>(), 1u);
67
68 JointPtr joint = skel->getJoint(0);
69 EXPECT_TRUE(joint != nullptr);
70 EXPECT_EQ(joint->getType(), FreeJoint::getStaticType());
71 }
72
73 //==============================================================================
TEST(SdfParser,SDFJointProperties)74 TEST(SdfParser, SDFJointProperties)
75 {
76 WorldPtr world = SdfParser::readWorld(
77 "dart://sample/sdf/test/test_skeleton_joint.world");
78 EXPECT_TRUE(world != nullptr);
79
80 SkeletonPtr skel = world->getSkeleton(0);
81 EXPECT_TRUE(skel != nullptr);
82 EXPECT_EQ(skel->getNumBodyNodes(), 5u);
83 EXPECT_EQ(skel->getNumJoints(), 5u);
84
85 const double epsilon = 1e-7;
86
87 auto testProperties = [epsilon](const Joint* joint, const size_t idx) {
88 EXPECT_NEAR(joint->getPositionLowerLimit(idx), 0, epsilon);
89 EXPECT_NEAR(joint->getPositionUpperLimit(idx), 3, epsilon);
90 EXPECT_NEAR(joint->getDampingCoefficient(idx), 0, epsilon);
91 EXPECT_NEAR(joint->getCoulombFriction(idx), 1, epsilon);
92 EXPECT_NEAR(joint->getRestPosition(idx), 2, epsilon);
93 EXPECT_NEAR(joint->getSpringStiffness(idx), 3, epsilon);
94 };
95
96 for (auto& joint : skel->getJoints())
97 {
98 if (joint->getType() == PrismaticJoint::getStaticType()
99 || joint->getType() == RevoluteJoint::getStaticType()
100 || joint->getType() == ScrewJoint::getStaticType())
101 {
102 testProperties(joint, 0);
103 }
104 else if (joint->getType() == UniversalJoint::getStaticType())
105 {
106 testProperties(joint, 0);
107 testProperties(joint, 1);
108 }
109 }
110 }
111
112 //==============================================================================
TEST(SdfParser,ParsingSDFFiles)113 TEST(SdfParser, ParsingSDFFiles)
114 {
115 const auto numSteps = 10u;
116
117 // Create a list of sdf files to test with where the sdf files contains World
118 std::vector<std::string> worldFiles;
119 worldFiles.push_back("dart://sample/sdf/benchmark.world");
120 worldFiles.push_back("dart://sample/sdf/double_pendulum.world");
121 worldFiles.push_back("dart://sample/sdf/double_pendulum_with_base.world");
122 worldFiles.push_back("dart://sample/sdf/empty.world");
123 worldFiles.push_back("dart://sample/sdf/ground.world");
124 worldFiles.push_back("dart://sample/sdf/test/single_bodynode_skeleton.world");
125
126 std::vector<WorldPtr> worlds;
127 for (const auto& worldFile : worldFiles)
128 worlds.push_back(SdfParser::readWorld(worldFile));
129
130 for (auto world : worlds)
131 {
132 EXPECT_TRUE(nullptr != world);
133
134 for (auto i = 0u; i < numSteps; ++i)
135 world->step();
136 }
137
138 // Create another list of sdf files to test with where the sdf files contains
139 // Skeleton
140 std::vector<common::Uri> skeletonFiles;
141 skeletonFiles.push_back("dart://sample/sdf/atlas/atlas_v3_no_head.sdf");
142 skeletonFiles.push_back(
143 "dart://sample/sdf/atlas/atlas_v3_no_head_soft_feet.sdf");
144
145 auto world = std::make_shared<World>();
146 std::vector<SkeletonPtr> skeletons;
147 for (const auto& skeletonFile : skeletonFiles)
148 skeletons.push_back(SdfParser::readSkeleton(skeletonFile));
149
150 for (auto skeleton : skeletons)
151 {
152 EXPECT_TRUE(nullptr != skeleton);
153
154 world->addSkeleton(skeleton);
155 for (auto i = 0u; i < numSteps; ++i)
156 world->step();
157
158 world->removeAllSkeletons();
159 }
160 }
161
162 //==============================================================================
TEST(SdfParser,ReadMaterial)163 TEST(SdfParser, ReadMaterial)
164 {
165 std::string sdf_filename = "dart://sample/sdf/quad.sdf";
166 SkeletonPtr skeleton = SdfParser::readSkeleton(sdf_filename);
167 EXPECT_TRUE(nullptr != skeleton);
168 auto bodynode = skeleton->getBodyNode(0);
169
170 for (auto shapenode : bodynode->getShapeNodes())
171 {
172 if (shapenode->has<dart::dynamics::VisualAspect>())
173 {
174 Eigen::Vector4d color = shapenode->getVisualAspect()->getRGBA();
175 Eigen::Vector4d expected_color(0.5, 0.6, 0.8, 1.0);
176 double diff = (color - expected_color).norm();
177 EXPECT_LT(diff, 1e-4);
178 }
179 }
180 }
181