/* * Copyright (c) 2011-2022, The DART development contributors * All rights reserved. * * The list of contributors can be found at: * https://github.com/dartsim/dart/blob/master/LICENSE * * This file is provided under the following "BSD-style" License: * Redistribution and use in source and binary forms, with or * without modification, are permitted provided that the following * conditions are met: * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #include #include #include "dart/dynamics/PlanarJoint.hpp" #include "dart/dynamics/RevoluteJoint.hpp" #include "dart/dynamics/Skeleton.hpp" #include "dart/dynamics/SoftBodyNode.hpp" #include "dart/simulation/World.hpp" #include "dart/utils/sdf/SdfParser.hpp" #include "TestHelpers.hpp" using namespace dart; using namespace math; using namespace dynamics; using namespace simulation; using namespace utils; //============================================================================== TEST(SdfParser, SDFSingleBodyWithoutJoint) { // Regression test for #444 WorldPtr world = SdfParser::readWorld( "dart://sample/sdf/test/single_bodynode_skeleton.world"); EXPECT_TRUE(world != nullptr); SkeletonPtr skel = world->getSkeleton(0); EXPECT_TRUE(skel != nullptr); EXPECT_EQ(skel->getNumBodyNodes(), 1u); EXPECT_EQ(skel->getNumJoints(), 1u); BodyNodePtr bodyNode = skel->getBodyNode(0); EXPECT_TRUE(bodyNode != nullptr); EXPECT_EQ(bodyNode->getNumShapeNodesWith(), 1u); EXPECT_EQ(bodyNode->getNumShapeNodesWith(), 1u); JointPtr joint = skel->getJoint(0); EXPECT_TRUE(joint != nullptr); EXPECT_EQ(joint->getType(), FreeJoint::getStaticType()); } //============================================================================== TEST(SdfParser, SDFJointProperties) { WorldPtr world = SdfParser::readWorld( "dart://sample/sdf/test/test_skeleton_joint.world"); EXPECT_TRUE(world != nullptr); SkeletonPtr skel = world->getSkeleton(0); EXPECT_TRUE(skel != nullptr); EXPECT_EQ(skel->getNumBodyNodes(), 5u); EXPECT_EQ(skel->getNumJoints(), 5u); const double epsilon = 1e-7; auto testProperties = [epsilon](const Joint* joint, const size_t idx) { EXPECT_NEAR(joint->getPositionLowerLimit(idx), 0, epsilon); EXPECT_NEAR(joint->getPositionUpperLimit(idx), 3, epsilon); EXPECT_NEAR(joint->getDampingCoefficient(idx), 0, epsilon); EXPECT_NEAR(joint->getCoulombFriction(idx), 1, epsilon); EXPECT_NEAR(joint->getRestPosition(idx), 2, epsilon); EXPECT_NEAR(joint->getSpringStiffness(idx), 3, epsilon); }; for (auto& joint : skel->getJoints()) { if (joint->getType() == PrismaticJoint::getStaticType() || joint->getType() == RevoluteJoint::getStaticType() || joint->getType() == ScrewJoint::getStaticType()) { testProperties(joint, 0); } else if (joint->getType() == UniversalJoint::getStaticType()) { testProperties(joint, 0); testProperties(joint, 1); } } } //============================================================================== TEST(SdfParser, ParsingSDFFiles) { const auto numSteps = 10u; // Create a list of sdf files to test with where the sdf files contains World std::vector worldFiles; worldFiles.push_back("dart://sample/sdf/benchmark.world"); worldFiles.push_back("dart://sample/sdf/double_pendulum.world"); worldFiles.push_back("dart://sample/sdf/double_pendulum_with_base.world"); worldFiles.push_back("dart://sample/sdf/empty.world"); worldFiles.push_back("dart://sample/sdf/ground.world"); worldFiles.push_back("dart://sample/sdf/test/single_bodynode_skeleton.world"); std::vector worlds; for (const auto& worldFile : worldFiles) worlds.push_back(SdfParser::readWorld(worldFile)); for (auto world : worlds) { EXPECT_TRUE(nullptr != world); for (auto i = 0u; i < numSteps; ++i) world->step(); } // Create another list of sdf files to test with where the sdf files contains // Skeleton std::vector skeletonFiles; skeletonFiles.push_back("dart://sample/sdf/atlas/atlas_v3_no_head.sdf"); skeletonFiles.push_back( "dart://sample/sdf/atlas/atlas_v3_no_head_soft_feet.sdf"); auto world = std::make_shared(); std::vector skeletons; for (const auto& skeletonFile : skeletonFiles) skeletons.push_back(SdfParser::readSkeleton(skeletonFile)); for (auto skeleton : skeletons) { EXPECT_TRUE(nullptr != skeleton); world->addSkeleton(skeleton); for (auto i = 0u; i < numSteps; ++i) world->step(); world->removeAllSkeletons(); } } //============================================================================== TEST(SdfParser, ReadMaterial) { std::string sdf_filename = "dart://sample/sdf/quad.sdf"; SkeletonPtr skeleton = SdfParser::readSkeleton(sdf_filename); EXPECT_TRUE(nullptr != skeleton); auto bodyNode = skeleton->getBodyNode(0); bodyNode->eachShapeNodeWith( [](dart::dynamics::ShapeNode* shapeNode) { Eigen::Vector4d color = shapeNode->getVisualAspect()->getRGBA(); Eigen::Vector4d expected_color(0.5, 0.6, 0.8, 1.0); double diff = (color - expected_color).norm(); EXPECT_LT(diff, 1e-4); }); }