Debian Codegraph @

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