/* * 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/SoftBodyNode.hpp" #include "TestHelpers.hpp" using namespace dart; using namespace math; using namespace dynamics; //============================================================================== TEST(NameManagement, Skeleton) { SkeletonPtr skel = Skeleton::create(); std::pair pair; pair = skel->createJointAndBodyNodePair( nullptr, GenericJoint::Properties(std::string("joint"))); Joint* joint1 = pair.first; BodyNode* body1 = pair.second; pair = skel->createJointAndBodyNodePair( body1, GenericJoint::Properties(std::string("joint"))); Joint* joint2 = pair.first; BodyNode* body2 = pair.second; pair = skel->createJointAndBodyNodePair( body2, GenericJoint::Properties(std::string("joint"))); Joint* joint3 = pair.first; BodyNode* body3 = pair.second; // Testing whether the repeated names of BodyNodes and Joints get resolved // correctly as BodyNodes get added to the Skeleton EXPECT_FALSE(body1->getName() == body2->getName()); EXPECT_FALSE(body2->getName() == body3->getName()); EXPECT_FALSE(body3->getName() == body1->getName()); EXPECT_TRUE(body1->getName() == "BodyNode"); EXPECT_TRUE(body2->getName() == "BodyNode(1)"); EXPECT_TRUE(body3->getName() == "BodyNode(2)"); EXPECT_FALSE(joint1->getName() == joint2->getName()); EXPECT_FALSE(joint2->getName() == joint3->getName()); EXPECT_FALSE(joint3->getName() == joint1->getName()); EXPECT_TRUE(joint1->getDof(0)->getName() == "joint"); EXPECT_TRUE(joint2->getDof(0)->getName() == "joint(1)_x"); EXPECT_TRUE(joint2->getDof(1)->getName() == "joint(1)_y"); EXPECT_TRUE(joint2->getDof(2)->getName() == "joint(1)_z"); EXPECT_TRUE(joint3->getDof(0)->getName() == "joint(2)_rot_x"); EXPECT_TRUE(joint3->getDof(1)->getName() == "joint(2)_rot_y"); EXPECT_TRUE(joint3->getDof(2)->getName() == "joint(2)_rot_z"); EXPECT_TRUE(joint3->getDof(3)->getName() == "joint(2)_pos_x"); EXPECT_TRUE(joint3->getDof(4)->getName() == "joint(2)_pos_y"); EXPECT_TRUE(joint3->getDof(5)->getName() == "joint(2)_pos_z"); // Testing whether the repeated names of BodyNodes get resolved correctly // as they are changed with BodyNode::setName(~) std::string newname1 = body1->setName("same_name"); std::string newname2 = body2->setName("same_name"); std::string newname3 = body3->setName("same_name"); EXPECT_FALSE(body1->getName() == body2->getName()); EXPECT_FALSE(body2->getName() == body3->getName()); EXPECT_FALSE(body3->getName() == body1->getName()); EXPECT_TRUE(body1->getName() == newname1); EXPECT_TRUE(body2->getName() == newname2); EXPECT_TRUE(body3->getName() == newname3); EXPECT_TRUE(skel->getBodyNode(newname1) == body1); EXPECT_TRUE(skel->getBodyNode(newname2) == body2); EXPECT_TRUE(skel->getBodyNode(newname3) == body3); // Testing whether the repeated names of Joints get resolved correctly // as they are changed with Joint::setName(~) newname1 = joint1->setName("another_name"); newname2 = joint2->setName("another_name"); newname3 = joint3->setName("another_name"); EXPECT_FALSE(joint1->getName() == joint2->getName()); EXPECT_FALSE(joint2->getName() == joint3->getName()); EXPECT_FALSE(joint3->getName() == joint1->getName()); EXPECT_TRUE(joint1->getName() == newname1); EXPECT_TRUE(joint2->getName() == newname2); EXPECT_TRUE(joint3->getName() == newname3); EXPECT_TRUE(skel->getJoint(newname1) == joint1); EXPECT_TRUE(skel->getJoint(newname2) == joint2); EXPECT_TRUE(skel->getJoint(newname3) == joint3); // Testing whether unique names get accidentally changed by the NameManager std::string unique_name = body2->setName("a_unique_name"); EXPECT_TRUE(body2->getName() == unique_name); EXPECT_TRUE(body2->getName() == "a_unique_name"); EXPECT_TRUE(skel->getBodyNode("a_unique_name") == body2); EXPECT_FALSE(body1->getName() == body2->getName()); EXPECT_FALSE(body2->getName() == body3->getName()); EXPECT_FALSE(body3->getName() == body1->getName()); unique_name = joint3->setName("a_unique_name"); EXPECT_TRUE(joint3->getName() == unique_name); EXPECT_TRUE(joint3->getName() == "a_unique_name"); EXPECT_TRUE(skel->getJoint("a_unique_name") == joint3); // Testing whether the DegreeOfFreedom names get updated correctly upon their // joint's name change EXPECT_TRUE(joint3->getDof(0)->getName() == "a_unique_name_rot_x"); EXPECT_TRUE(joint3->getDof(1)->getName() == "a_unique_name_rot_y"); EXPECT_TRUE(joint3->getDof(2)->getName() == "a_unique_name_rot_z"); EXPECT_TRUE(joint3->getDof(3)->getName() == "a_unique_name_pos_x"); EXPECT_TRUE(joint3->getDof(4)->getName() == "a_unique_name_pos_y"); EXPECT_TRUE(joint3->getDof(5)->getName() == "a_unique_name_pos_z"); EXPECT_TRUE(joint3->getDof(0) == skel->getDof("a_unique_name_rot_x")); EXPECT_TRUE(joint3->getDof(3) == skel->getDof("a_unique_name_pos_x")); // Note: The following assumes the joint order in the Skeleton is: // RevoluteJoint -> TranslationalJoint -> FreeJoint EXPECT_TRUE(joint1->getDof(0) == skel->getDof(0)); EXPECT_TRUE(joint2->getDof(0) == skel->getDof(1)); EXPECT_TRUE(joint2->getDof(1) == skel->getDof(2)); EXPECT_TRUE(joint2->getDof(2) == skel->getDof(3)); EXPECT_TRUE(joint3->getDof(0) == skel->getDof(4)); EXPECT_TRUE(joint3->getDof(1) == skel->getDof(5)); EXPECT_TRUE(joint3->getDof(2) == skel->getDof(6)); EXPECT_TRUE(joint3->getDof(3) == skel->getDof(7)); EXPECT_TRUE(joint3->getDof(4) == skel->getDof(8)); EXPECT_TRUE(joint3->getDof(5) == skel->getDof(9)); // Test whether the return of getIndexInSkeleton() and the index of the // corresponding DegreeOfFreedom in the Skeleton are same for (std::size_t i = 0; i < skel->getNumDofs(); ++i) EXPECT_TRUE(skel->getDof(i)->getIndexInSkeleton() == i); // Test whether all the joint names are still unique EXPECT_FALSE(joint1->getName() == joint2->getName()); EXPECT_FALSE(joint2->getName() == joint3->getName()); EXPECT_FALSE(joint3->getName() == joint1->getName()); // Make sure that the Skeleton gives back nullptr for non existent names EXPECT_TRUE(skel->getBodyNode("nonexistent_name") == nullptr); EXPECT_TRUE(skel->getJoint("nonexistent_name") == nullptr); EXPECT_TRUE(skel->getSoftBodyNode("nonexistent_name") == nullptr); // Test Node Names EndEffector* ee1 = body1->createEndEffector("ee"); EndEffector* ee2 = body1->createEndEffector("ee"); EXPECT_TRUE(skel->getEndEffector("ee") == ee1); EXPECT_TRUE(skel->getEndEffector("ee(1)") == ee2); } //============================================================================== TEST(NameManagement, SetPattern) { dart::common::NameManager > test_mgr("test", "name"); std::shared_ptr entity0(new SimpleFrame(Frame::World(), "name")); std::shared_ptr entity1(new SimpleFrame(Frame::World(), "name")); std::shared_ptr entity2(new SimpleFrame(Frame::World(), "name")); test_mgr.setPattern("%s(%d)"); test_mgr.issueNewNameAndAdd(entity0->getName(), entity0); test_mgr.issueNewNameAndAdd(entity1->getName(), entity1); test_mgr.issueNewNameAndAdd(entity2->getName(), entity2); EXPECT_TRUE(test_mgr.getObject("name") == entity0); EXPECT_TRUE(test_mgr.getObject("name(1)") == entity1); EXPECT_TRUE(test_mgr.getObject("name(2)") == entity2); test_mgr.clear(); entity0->setName("Entity"); entity1->setName("Entity"); entity2->setName("Entity"); test_mgr.setPattern("(%d)-%s"); test_mgr.issueNewNameAndAdd(entity0->getName(), entity0); test_mgr.issueNewNameAndAdd(entity1->getName(), entity1); test_mgr.issueNewNameAndAdd(entity2->getName(), entity2); EXPECT_TRUE(test_mgr.getObject("Entity") == entity0); EXPECT_TRUE(test_mgr.getObject("(1)-Entity") == entity1); EXPECT_TRUE(test_mgr.getObject("(2)-Entity") == entity2); } //============================================================================== TEST(NameManagement, Regression554) { dart::common::NameManager > test_mgr("test", "name"); std::shared_ptr int0(new int(0)); std::shared_ptr int1(new int(1)); std::shared_ptr int2(new int(2)); std::shared_ptr int_another0(new int(0)); test_mgr.issueNewNameAndAdd(std::to_string(*int0), int0); test_mgr.issueNewNameAndAdd(std::to_string(*int1), int1); test_mgr.issueNewNameAndAdd(std::to_string(*int2), int2); EXPECT_TRUE(test_mgr.getObject("0") == int0); EXPECT_TRUE(test_mgr.getObject("1") == int1); EXPECT_TRUE(test_mgr.getObject("2") == int2); bool res1 = test_mgr.addName(std::to_string(*int_another0), int_another0); EXPECT_FALSE(res1); test_mgr.removeEntries(std::to_string(*int_another0), int_another0); bool res2 = test_mgr.addName(std::to_string(*int_another0), int_another0); EXPECT_TRUE(res2); EXPECT_TRUE(test_mgr.getObject("0") == int_another0); EXPECT_TRUE(test_mgr.getObject("1") == int1); EXPECT_TRUE(test_mgr.getObject("2") == int2); } //============================================================================== TEST(NameManagement, WorldSkeletons) { dart::simulation::WorldPtr world1(new dart::simulation::World); world1->setName("world1"); dart::dynamics::SkeletonPtr skel0 = dart::dynamics::Skeleton::create(); dart::dynamics::SkeletonPtr skel1 = dart::dynamics::Skeleton::create(); dart::dynamics::SkeletonPtr skel2 = dart::dynamics::Skeleton::create(); world1->addSkeleton(skel0); world1->addSkeleton(skel1); world1->addSkeleton(skel2); EXPECT_TRUE(skel0->getName() == "Skeleton"); EXPECT_TRUE(skel1->getName() == "Skeleton(1)"); EXPECT_TRUE(skel2->getName() == "Skeleton(2)"); skel1->setName("Skeleton"); EXPECT_TRUE(skel1->getName() == "Skeleton(1)"); skel1->setName("OtherName"); EXPECT_TRUE(skel1->getName() == "OtherName"); skel2->setName("Skeleton"); EXPECT_TRUE(skel2->getName() == "Skeleton(1)"); skel2->setName("OtherName"); EXPECT_TRUE(skel2->getName() == "OtherName(1)"); EXPECT_TRUE(skel0 == world1->getSkeleton(skel0->getName())); EXPECT_TRUE(skel1 == world1->getSkeleton(skel1->getName())); EXPECT_TRUE(skel2 == world1->getSkeleton(skel2->getName())); dart::simulation::WorldPtr world2(new dart::simulation::World); world2->setName("world2"); dart::dynamics::SkeletonPtr skel3 = dart::dynamics::Skeleton::create("OtherName"); world2->addSkeleton(skel3); world2->addSkeleton(skel2); world2->addSkeleton(skel1); EXPECT_TRUE(skel3->getName() == "OtherName"); EXPECT_TRUE(skel1->getName() == "OtherName(2)"); EXPECT_TRUE(skel2->getName() == "OtherName(1)"); skel3->setName("Skeleton(1)"); skel1->setName("Skeleton"); EXPECT_TRUE(skel3->getName() == "Skeleton(1)"); EXPECT_TRUE(skel1->getName() == "Skeleton(1)(1)"); EXPECT_TRUE(skel0 == world1->getSkeleton(skel0->getName())); EXPECT_TRUE(skel1 == world1->getSkeleton(skel1->getName())); EXPECT_TRUE(skel2 == world1->getSkeleton(skel2->getName())); EXPECT_TRUE(skel3 == world2->getSkeleton(skel3->getName())); EXPECT_TRUE(skel1 == world2->getSkeleton(skel1->getName())); EXPECT_TRUE(skel2 == world2->getSkeleton(skel2->getName())); world2->removeSkeleton(skel1); skel1->setName("Skeleton"); EXPECT_TRUE(skel1->getName() == "Skeleton(1)"); } //============================================================================== TEST(NameManagement, WorldSimpleFrames) { dart::simulation::WorldPtr world1(new dart::simulation::World); dart::dynamics::SimpleFramePtr frame0( new dart::dynamics::SimpleFrame(Frame::World(), "Frame")); dart::dynamics::SimpleFramePtr frame1( new dart::dynamics::SimpleFrame(Frame::World(), "Frame")); dart::dynamics::SimpleFramePtr frame2( new dart::dynamics::SimpleFrame(Frame::World(), "Frame")); world1->addSimpleFrame(frame0); world1->addSimpleFrame(frame1); world1->addSimpleFrame(frame2); EXPECT_TRUE(frame0->getName() == "Frame"); EXPECT_TRUE(frame1->getName() == "Frame(1)"); EXPECT_TRUE(frame2->getName() == "Frame(2)"); frame1->setName("Frame"); EXPECT_TRUE(frame1->getName() == "Frame(1)"); frame1->setName("OtherName"); EXPECT_TRUE(frame1->getName() == "OtherName"); frame2->setName("Frame"); EXPECT_TRUE(frame2->getName() == "Frame(1)"); frame2->setName("OtherName"); EXPECT_TRUE(frame2->getName() == "OtherName(1)"); EXPECT_TRUE(frame0 == world1->getSimpleFrame(frame0->getName())); EXPECT_TRUE(frame1 == world1->getSimpleFrame(frame1->getName())); EXPECT_TRUE(frame2 == world1->getSimpleFrame(frame2->getName())); dart::simulation::WorldPtr world2(new dart::simulation::World); world2->setName("world2"); dart::dynamics::SimpleFramePtr frame3( new dart::dynamics::SimpleFrame(Frame::World(), "OtherName")); world2->addSimpleFrame(frame3); world2->addSimpleFrame(frame2); world2->addSimpleFrame(frame1); EXPECT_TRUE(frame3->getName() == "OtherName"); EXPECT_TRUE(frame1->getName() == "OtherName(2)"); EXPECT_TRUE(frame2->getName() == "OtherName(1)"); frame3->setName("Frame(1)"); frame1->setName("Frame"); EXPECT_TRUE(frame3->getName() == "Frame(1)"); EXPECT_TRUE(frame1->getName() == "Frame(1)(1)"); EXPECT_TRUE(frame0 == world1->getSimpleFrame(frame0->getName())); EXPECT_TRUE(frame1 == world1->getSimpleFrame(frame1->getName())); EXPECT_TRUE(frame2 == world1->getSimpleFrame(frame2->getName())); EXPECT_TRUE(frame3 == world2->getSimpleFrame(frame3->getName())); EXPECT_TRUE(frame1 == world2->getSimpleFrame(frame1->getName())); EXPECT_TRUE(frame2 == world2->getSimpleFrame(frame2->getName())); world2->removeSimpleFrame(frame1); frame1->setName("Frame"); EXPECT_TRUE(frame1->getName() == "Frame(1)"); } //============================================================================== TEST(NameManagement, JointDegreeOfFreedom) { SkeletonPtr subtree = Skeleton::create("subtree"); EulerJoint::Properties jointProperties; jointProperties.mName = "j_bicep_right"; JointPtr joint = subtree ->createJointAndBodyNodePair(nullptr, jointProperties) .first; std::string newName = "j_bicep_right_inverse"; EXPECT_TRUE(joint->getDof(0)->getName() == "j_bicep_right_x"); EXPECT_TRUE(joint->getDof(1)->getName() == "j_bicep_right_y"); EXPECT_TRUE(joint->getDof(2)->getName() == "j_bicep_right_z"); joint->setName(newName, false); EXPECT_TRUE(joint->getDof(0)->getName() == "j_bicep_right_x"); EXPECT_TRUE(joint->getDof(1)->getName() == "j_bicep_right_y"); EXPECT_TRUE(joint->getDof(2)->getName() == "j_bicep_right_z"); joint->setName(newName, true); EXPECT_TRUE(joint->getDof(0)->getName() == "j_bicep_right_inverse_x"); EXPECT_TRUE(joint->getDof(1)->getName() == "j_bicep_right_inverse_y"); EXPECT_TRUE(joint->getDof(2)->getName() == "j_bicep_right_inverse_z"); newName = "j_bicep_left"; joint->setName(newName, true); EXPECT_TRUE(joint->getDof(0)->getName() == "j_bicep_left_x"); EXPECT_TRUE(joint->getDof(1)->getName() == "j_bicep_left_y"); EXPECT_TRUE(joint->getDof(2)->getName() == "j_bicep_left_z"); }