// Set the actuator type for four wheel joints to "VELOCITY" (Lesson 6 Answer)
void setVelocityAccuators(SkeletonPtr biped)
{
Joint* wheel1 = biped->getJoint("joint_front_left");
Joint* wheel2 = biped->getJoint("joint_front_right");
Joint* wheel3 = biped->getJoint("joint_back_left");
Joint* wheel4 = biped->getJoint("joint_back_right");
wheel1->setActuatorType(Joint::VELOCITY);
wheel2->setActuatorType(Joint::VELOCITY);
wheel3->setActuatorType(Joint::VELOCITY);
wheel4->setActuatorType(Joint::VELOCITY);
}
void check_self_consistency(SkeletonPtr skeleton)
{
for(size_t i=0; i<skeleton->getNumBodyNodes(); ++i)
{
BodyNode* bn = skeleton->getBodyNode(i);
EXPECT_TRUE(bn->getIndexInSkeleton() == i);
EXPECT_TRUE(skeleton->getBodyNode(bn->getName()) == bn);
Joint* joint = bn->getParentJoint();
EXPECT_TRUE(skeleton->getJoint(joint->getName()) == joint);
for(size_t j=0; j<joint->getNumDofs(); ++j)
{
DegreeOfFreedom* dof = joint->getDof(j);
EXPECT_TRUE(dof->getIndexInJoint() == j);
EXPECT_TRUE(skeleton->getDof(dof->getName()) == dof);
}
}
for(size_t i=0; i<skeleton->getNumDofs(); ++i)
{
DegreeOfFreedom* dof = skeleton->getDof(i);
EXPECT_TRUE(dof->getIndexInSkeleton() == i);
EXPECT_TRUE(skeleton->getDof(dof->getName()) == dof);
}
}
// Load a biped model and enable joint limits and self-collision
// (Lesson 1 Answer)
SkeletonPtr loadBiped()
{
// Create the world with a skeleton
WorldPtr world = SkelParser::readWorld(DART_DATA_PATH"skel/biped.skel");
assert(world != nullptr);
SkeletonPtr biped = world->getSkeleton("biped");
// Set joint limits
for(size_t i = 0; i < biped->getNumJoints(); ++i)
biped->getJoint(i)->setPositionLimitEnforced(true);
// Enable self collision check but ignore adjacent bodies
biped->enableSelfCollision();
return biped;
}
SkeletonPtr createHubo()
{
dart::utils::DartLoader loader;
loader.addPackageDirectory("drchubo", DART_DATA_PATH"/urdf/drchubo");
SkeletonPtr hubo =
loader.parseSkeleton(DART_DATA_PATH"/urdf/drchubo/drchubo.urdf");
for(size_t i = 0; i < hubo->getNumBodyNodes(); ++i)
{
BodyNode* bn = hubo->getBodyNode(i);
if(bn->getName().substr(0, 7) == "Body_LF"
|| bn->getName().substr(0, 7) == "Body_RF"
|| bn->getName().substr(0, 7) == "Body_NK")
{
bn->remove();
--i;
}
}
hubo->setPosition(5, 0.97);
for(size_t i=1; i < hubo->getNumJoints(); ++i)
{
hubo->getJoint(i)->setActuatorType(Joint::VELOCITY);
}
for(size_t i=0; i < hubo->getNumBodyNodes(); ++i)
{
BodyNode* bn = hubo->getBodyNode(i);
for(size_t j=0; j < bn->getNumVisualizationShapes(); ++j)
{
const ShapePtr& shape = bn->getVisualizationShape(j);
shape->setColor(Eigen::Vector3d(0.2, 0.2, 0.2));
if(MeshShapePtr mesh = std::dynamic_pointer_cast<MeshShape>(shape))
mesh->setColorMode(MeshShape::SHAPE_COLOR);
}
}
hubo->setName("drchubo");
return hubo;
}
//==============================================================================
TEST(SdfParser, SDFSingleBodyWithoutJoint)
{
// Regression test for #444
WorldPtr world
= SdfParser::readSdfFile(
DART_DATA_PATH"/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->getNumVisualizationShapes(), 1u);
EXPECT_EQ(bodyNode->getNumCollisionShapes(), 1u);
JointPtr joint = skel->getJoint(0);
EXPECT_TRUE(joint != nullptr);
EXPECT_EQ(joint->getType(), FreeJoint::getStaticType());
}
TEST(Skeleton, Group)
{
SkeletonPtr skel = constructLinkageTestSkeleton();
// Make twice as many BodyNodes in the Skeleton
SkeletonPtr skel2 = constructLinkageTestSkeleton();
skel2->getRootBodyNode()->moveTo(skel, nullptr);
// Test nullptr construction
GroupPtr nullGroup = Group::create("null_group", nullptr);
EXPECT_EQ(nullGroup->getNumBodyNodes(), 0u);
EXPECT_EQ(nullGroup->getNumJoints(), 0u);
EXPECT_EQ(nullGroup->getNumDofs(), 0u);
// Test conversion from Skeleton
GroupPtr skel1Group = Group::create("skel1_group", skel);
EXPECT_EQ(skel1Group->getNumBodyNodes(), skel->getNumBodyNodes());
EXPECT_EQ(skel1Group->getNumJoints(), skel->getNumJoints());
EXPECT_EQ(skel1Group->getNumDofs(), skel->getNumDofs());
for(size_t i=0; i < skel1Group->getNumBodyNodes(); ++i)
EXPECT_EQ(skel1Group->getBodyNode(i), skel->getBodyNode(i));
for(size_t i=0; i < skel1Group->getNumJoints(); ++i)
EXPECT_EQ(skel1Group->getJoint(i), skel->getJoint(i));
for(size_t i=0; i < skel1Group->getNumDofs(); ++i)
EXPECT_EQ(skel1Group->getDof(i), skel->getDof(i));
// Test arbitrary Groups by plucking random BodyNodes, Joints, and
// DegreesOfFreedom from a Skeleton.
GroupPtr group = Group::create();
std::vector<BodyNode*> bodyNodes;
std::vector<Joint*> joints;
std::vector<DegreeOfFreedom*> dofs;
for(size_t i=0; i < 2*skel->getNumBodyNodes(); ++i)
{
size_t randomIndex = floor(random(0, skel->getNumBodyNodes()));
BodyNode* bn = skel->getBodyNode(randomIndex);
if(group->addBodyNode(bn, false))
bodyNodes.push_back(bn);
randomIndex = floor(random(0, skel->getNumJoints()));
Joint* joint = skel->getJoint(randomIndex);
if(group->addJoint(joint, false, false))
joints.push_back(joint);
randomIndex = floor(random(0, skel->getNumDofs()));
DegreeOfFreedom* dof = skel->getDof(randomIndex);
if(group->addDof(dof, false, false))
dofs.push_back(dof);
}
EXPECT_EQ(group->getNumBodyNodes(), bodyNodes.size());
EXPECT_EQ(group->getNumJoints(), joints.size());
EXPECT_EQ(group->getNumDofs(), dofs.size());
for(size_t i=0; i < group->getNumBodyNodes(); ++i)
EXPECT_EQ(group->getBodyNode(i), bodyNodes[i]);
for(size_t i=0; i < group->getNumJoints(); ++i)
EXPECT_EQ(group->getJoint(i), joints[i]);
for(size_t i=0; i < group->getNumDofs(); ++i)
EXPECT_EQ(group->getDof(i), dofs[i]);
}
TEST(Skeleton, Persistence)
{
WeakBodyNodePtr weakBnPtr;
SoftBodyNodePtr softBnPtr;
WeakSoftBodyNodePtr weakSoftBnPtr;
WeakSkeletonPtr weakSkelPtr;
{
BodyNodePtr strongPtr;
{
{
SkeletonPtr skeleton = createThreeLinkRobot(
Eigen::Vector3d(1.0, 1.0, 1.0), DOF_X,
Eigen::Vector3d(1.0, 1.0, 1.0), DOF_Y,
Eigen::Vector3d(1.0, 1.0, 1.0), DOF_Z);
weakSkelPtr = skeleton;
// Test usability of the BodyNodePtr
strongPtr = skeleton->getBodyNode(0);
weakBnPtr = strongPtr;
ConstBodyNodePtr constPtr = strongPtr;
EXPECT_FALSE( strongPtr == nullptr );
EXPECT_FALSE( nullptr == strongPtr );
EXPECT_TRUE( strongPtr == skeleton->getBodyNode(0) );
EXPECT_TRUE( skeleton->getBodyNode(0) == strongPtr );
EXPECT_TRUE( constPtr == strongPtr );
EXPECT_TRUE( weakBnPtr.lock() == strongPtr );
EXPECT_FALSE( strongPtr < constPtr );
EXPECT_FALSE( strongPtr < skeleton->getBodyNode(0) );
EXPECT_FALSE( strongPtr < weakBnPtr.lock() );
EXPECT_FALSE( skeleton->getBodyNode(0) < strongPtr );
EXPECT_FALSE( weakBnPtr.lock() < strongPtr);
EXPECT_FALSE( strongPtr > constPtr );
EXPECT_FALSE( strongPtr > skeleton->getBodyNode(0) );
EXPECT_FALSE( strongPtr > weakBnPtr.lock() );
EXPECT_FALSE( skeleton->getBodyNode(0) > strongPtr );
EXPECT_FALSE( weakBnPtr.lock() > strongPtr );
BodyNodePtr tail = skeleton->getBodyNode(skeleton->getNumBodyNodes()-1);
std::pair<Joint*, SoftBodyNode*> pair =
skeleton->createJointAndBodyNodePair<RevoluteJoint, SoftBodyNode>(
tail);
softBnPtr = pair.second;
weakSoftBnPtr = softBnPtr;
WeakBodyNodePtr otherWeakPtr = weakSoftBnPtr; // Test convertability
// Test usability of the DegreeOfFreedomPtr
DegreeOfFreedomPtr dof = skeleton->getDof(1);
WeakDegreeOfFreedomPtr weakdof = dof;
ConstDegreeOfFreedomPtr const_dof = dof;
WeakConstDegreeOfFreedomPtr const_weakdof = weakdof;
const_weakdof = const_dof;
EXPECT_TRUE( dof == skeleton->getDof(1) );
EXPECT_TRUE( dof == const_dof );
EXPECT_TRUE( weakdof.lock() == const_weakdof.lock() );
EXPECT_TRUE( const_weakdof.lock() == skeleton->getDof(1) );
EXPECT_TRUE( skeleton->getDof(1) == const_weakdof.lock() );
EXPECT_FALSE( dof < const_dof );
EXPECT_FALSE( dof < skeleton->getDof(1) );
EXPECT_FALSE( dof < weakdof.lock() );
EXPECT_FALSE( skeleton->getDof(1) < dof );
EXPECT_FALSE( weakdof.lock() < dof );
EXPECT_FALSE( dof > const_dof );
EXPECT_FALSE( dof > skeleton->getDof(1) );
EXPECT_FALSE( dof > weakdof.lock() );
EXPECT_FALSE( skeleton->getDof(1) > dof );
EXPECT_FALSE( weakdof.lock() > dof );
dof = nullptr;
weakdof = nullptr;
const_dof = nullptr;
const_weakdof = nullptr;
EXPECT_TRUE( dof == nullptr );
EXPECT_TRUE( nullptr == dof );
EXPECT_TRUE( weakdof.lock() == nullptr );
EXPECT_TRUE( nullptr == weakdof.lock() );
EXPECT_TRUE( const_dof == nullptr );
EXPECT_TRUE( const_weakdof.lock() == nullptr );
EXPECT_FALSE( dof < const_dof );
// Test usability of the JointPtr
JointPtr joint = skeleton->getJoint(1);
WeakJointPtr weakjoint = joint;
ConstJointPtr const_joint = joint;
WeakConstJointPtr const_weakjoint = const_joint;
EXPECT_TRUE( joint == skeleton->getJoint(1) );
EXPECT_TRUE( joint == const_joint );
EXPECT_TRUE( weakjoint.lock() == const_weakjoint.lock() );
EXPECT_TRUE( const_weakjoint.lock() == skeleton->getJoint(1) );
EXPECT_FALSE( joint < const_joint );
EXPECT_FALSE( joint < skeleton->getJoint(1) );
EXPECT_FALSE( joint < weakjoint.lock() );
EXPECT_FALSE( skeleton->getJoint(1) < joint );
EXPECT_FALSE( weakjoint.lock() < joint );
EXPECT_FALSE( joint > const_joint );
EXPECT_FALSE( joint > skeleton->getJoint(1) );
EXPECT_FALSE( joint > weakjoint.lock() );
EXPECT_FALSE( skeleton->getJoint(1) > joint );
EXPECT_FALSE( weakjoint.lock() > joint );
joint = nullptr;
weakjoint = nullptr;
const_joint = nullptr;
const_weakjoint = nullptr;
EXPECT_TRUE( joint == nullptr );
EXPECT_TRUE( weakjoint.lock() == nullptr );
EXPECT_TRUE( const_joint == nullptr );
EXPECT_TRUE( const_weakjoint.lock() == nullptr );
}
// The BodyNode should still be alive, because a BodyNodePtr still
// references it
EXPECT_FALSE(weakBnPtr.expired());
// The Skeleton should still be alive, because a BodyNodePtr still
// references one of its BodyNodes
EXPECT_FALSE(weakSkelPtr.lock() == nullptr);
// Take the BodyNode out of its Skeleton and put it into a temporary one
strongPtr->remove();
// The BodyNode should still be alive, because a BodyNodePtr still
// references it
EXPECT_FALSE(weakBnPtr.expired());
// The Skeleton should be destroyed, because it lost the only BodyNode
// that still had a reference
EXPECT_TRUE(weakSkelPtr.lock() == nullptr);
// Update the weakSkelPtr so it references the Skeleton that still exists
weakSkelPtr = strongPtr->getSkeleton();
EXPECT_FALSE(weakSkelPtr.lock() == nullptr);
// Change the BodyNode that this BodyNodePtr is referencing
strongPtr = strongPtr->getChildBodyNode(0);
// Make sure the Skeleton is still alive. If the SkeletonPtr being used
// by the BodyNodePtr is not swapped atomically, then this will fail,
// which means we cannot rely on BodyNodePtr to keep BodyNodes alive.
EXPECT_FALSE(weakSkelPtr.lock() == nullptr);
}
SkeletonPtr other_skeleton = createThreeLinkRobot(
Eigen::Vector3d(1.0, 1.0, 1.0), DOF_X,
Eigen::Vector3d(1.0, 1.0, 1.0), DOF_Y,
Eigen::Vector3d(1.0, 1.0, 1.0), DOF_Z);
BodyNode* tail = other_skeleton->getBodyNode(
other_skeleton->getNumBodyNodes()-1);
WeakConstBodyNodePtr weakParentPtr;
{
ConstBodyNodePtr parent = strongPtr;
parent = parent->getParentBodyNode();
weakParentPtr = parent;
strongPtr->moveTo(tail);
// The Skeleton should still be alive because 'parent' exists
EXPECT_FALSE(weakSkelPtr.lock() == nullptr);
}
// Now that 'parent' is out of scope, the Skeleton should be gone
EXPECT_TRUE(weakSkelPtr.lock() == nullptr);
EXPECT_TRUE(weakParentPtr.lock() == nullptr);
weakBnPtr = strongPtr;
weakSkelPtr = strongPtr->getSkeleton();
EXPECT_FALSE(weakBnPtr.expired());
EXPECT_FALSE(weakSkelPtr.expired());
}
// softBnPtr still exists, so it should be keeping the Skeleton active
EXPECT_FALSE(weakBnPtr.expired());
std::weak_ptr<Skeleton> weakSkel = softBnPtr->remove();
// Now that the SoftBodyNode which is holding the reference has been moved to
// another Skeleton, the weakBnPtr and weakSkelPtr should disappear
EXPECT_TRUE(weakBnPtr.expired());
EXPECT_TRUE(weakSkelPtr.expired());
// The WeakSoftBodyNodePtr should not have expired yet, because a strong
// reference to its SoftBodyNode still exists
EXPECT_FALSE(weakSoftBnPtr.expired());
// Test the user-defined copy constructor
SoftBodyNodePtr otherSoftBnPtr = softBnPtr;
softBnPtr = nullptr;
EXPECT_FALSE(weakSkel.lock() == nullptr);
EXPECT_FALSE(weakSoftBnPtr.lock() == nullptr);
BodyNodePtr strongPtr = otherSoftBnPtr;
otherSoftBnPtr = nullptr;
BodyNodePtr otherStrongPtr = strongPtr;
strongPtr = nullptr;
EXPECT_FALSE(weakSkel.lock() == nullptr);
EXPECT_FALSE(weakSoftBnPtr.lock() == nullptr);
otherStrongPtr = nullptr;
// Now that all the strong BodyNodePtrs have been cleared, the
// WeakSoftBodyNodePtr should also be cleared
EXPECT_TRUE(weakSoftBnPtr.lock() == nullptr);
EXPECT_TRUE(weakSkel.lock() == nullptr);
}
TEST(Skeleton, Restructuring)
{
std::vector<SkeletonPtr> skeletons = getSkeletons();
#ifndef NDEBUG
size_t numIterations = 10;
#else
size_t numIterations = 2*skeletons.size();
#endif
// Test moves within the current Skeleton
for(size_t i=0; i<numIterations; ++i)
{
size_t index = floor(math::random(0, skeletons.size()));
index = std::min(index, skeletons.size()-1);
SkeletonPtr skeleton = skeletons[index];
SkeletonPtr original = skeleton->clone();
size_t maxNode = skeleton->getNumBodyNodes()-1;
BodyNode* bn1 = skeleton->getBodyNode(floor(math::random(0, maxNode)));
BodyNode* bn2 = skeleton->getBodyNode(ceil(math::random(0, maxNode)));
if(bn1 == bn2)
{
--i;
continue;
}
BodyNode* child = bn1->descendsFrom(bn2)? bn1 : bn2;
BodyNode* parent = child == bn1? bn2 : bn1;
child->moveTo(parent);
EXPECT_TRUE(skeleton->getNumBodyNodes() == original->getNumBodyNodes());
if(skeleton->getNumBodyNodes() == original->getNumBodyNodes())
{
for(size_t j=0; j<skeleton->getNumBodyNodes(); ++j)
{
// Make sure no BodyNodes have been lost or gained in translation
std::string name = original->getBodyNode(j)->getName();
BodyNode* bn = skeleton->getBodyNode(name);
EXPECT_FALSE(bn == nullptr);
if(bn)
EXPECT_TRUE(bn->getName() == name);
name = skeleton->getBodyNode(j)->getName();
bn = original->getBodyNode(name);
EXPECT_FALSE(bn == nullptr);
if(bn)
EXPECT_TRUE(bn->getName() == name);
// Make sure no Joints have been lost or gained in translation
name = original->getJoint(j)->getName();
Joint* joint = skeleton->getJoint(name);
EXPECT_FALSE(joint == nullptr);
if(joint)
EXPECT_TRUE(joint->getName() == name);
name = skeleton->getJoint(j)->getName();
joint = original->getJoint(name);
EXPECT_FALSE(joint == nullptr);
if(joint)
EXPECT_TRUE(joint->getName() == name);
}
}
EXPECT_TRUE(skeleton->getNumDofs() == original->getNumDofs());
for(size_t j=0; j<skeleton->getNumDofs(); ++j)
{
std::string name = original->getDof(j)->getName();
DegreeOfFreedom* dof = skeleton->getDof(name);
EXPECT_FALSE(dof == nullptr);
if(dof)
EXPECT_TRUE(dof->getName() == name);
name = skeleton->getDof(j)->getName();
dof = original->getDof(name);
EXPECT_FALSE(dof == nullptr);
if(dof)
EXPECT_TRUE(dof->getName() == name);
}
}
// Test moves between Skeletons
for(size_t i=0; i<numIterations; ++i)
{
size_t fromIndex = floor(math::random(0, skeletons.size()));
fromIndex = std::min(fromIndex, skeletons.size()-1);
SkeletonPtr fromSkel = skeletons[fromIndex];
if(fromSkel->getNumBodyNodes() == 0)
{
--i;
continue;
}
size_t toIndex = floor(math::random(0, skeletons.size()));
toIndex = std::min(toIndex, skeletons.size()-1);
SkeletonPtr toSkel = skeletons[toIndex];
if(toSkel->getNumBodyNodes() == 0)
{
--i;
continue;
}
BodyNode* childBn = fromSkel->getBodyNode(
floor(math::random(0, fromSkel->getNumBodyNodes()-1)));
BodyNode* parentBn = toSkel->getBodyNode(
floor(math::random(0, toSkel->getNumBodyNodes()-1)));
if(fromSkel == toSkel)
{
if(childBn == parentBn)
{
--i;
continue;
}
if(parentBn->descendsFrom(childBn))
{
BodyNode* tempBn = childBn;
childBn = parentBn;
parentBn = tempBn;
SkeletonPtr tempSkel = fromSkel;
fromSkel = toSkel;
toSkel = tempSkel;
}
}
BodyNode* originalParent = childBn->getParentBodyNode();
std::vector<BodyNode*> subtree;
constructSubtree(subtree, childBn);
// Move to a new Skeleton
childBn->moveTo(parentBn);
// Make sure all the objects have moved
for(size_t j=0; j<subtree.size(); ++j)
{
BodyNode* bn = subtree[j];
EXPECT_TRUE(bn->getSkeleton() == toSkel);
}
// Move to the Skeleton's root while producing a new Joint type
sub_ptr<Joint> originalJoint = childBn->getParentJoint();
childBn->moveTo<FreeJoint>(nullptr);
// The original parent joint should be deleted now
EXPECT_TRUE(originalJoint == nullptr);
// The BodyNode should now be a root node
EXPECT_TRUE(childBn->getParentBodyNode() == nullptr);
// The subtree should still be in the same Skeleton
for(size_t j=0; j<subtree.size(); ++j)
{
BodyNode* bn = subtree[j];
EXPECT_TRUE(bn->getSkeleton() == toSkel);
}
// Create some new Skeletons and mangle them all up
childBn->copyTo<RevoluteJoint>(fromSkel, originalParent);
SkeletonPtr temporary = childBn->split("temporary");
SkeletonPtr other_temporary =
childBn->split<PrismaticJoint>("other temporary");
SkeletonPtr another_temporary = childBn->copyAs("another temporary");
SkeletonPtr last_temporary = childBn->copyAs<ScrewJoint>("last temporary");
childBn->copyTo(another_temporary->getBodyNode(
another_temporary->getNumBodyNodes()-1));
childBn->copyTo<PlanarJoint>(another_temporary->getBodyNode(0));
childBn->copyTo<TranslationalJoint>(temporary, nullptr);
childBn->moveTo(last_temporary,
last_temporary->getBodyNode(last_temporary->getNumBodyNodes()-1));
childBn->moveTo<BallJoint>(last_temporary, nullptr);
childBn->moveTo<EulerJoint>(last_temporary,
last_temporary->getBodyNode(0));
childBn->changeParentJointType<FreeJoint>();
// Test the non-recursive copying
if(toSkel->getNumBodyNodes() > 1)
{
SkeletonPtr singleBodyNode =
toSkel->getBodyNode(0)->copyAs("single", false);
EXPECT_TRUE(singleBodyNode->getNumBodyNodes() == 1);
std::pair<Joint*, BodyNode*> singlePair =
toSkel->getBodyNode(0)->copyTo(nullptr, false);
EXPECT_TRUE(singlePair.second->getNumChildBodyNodes() == 0);
}
// Check that the mangled Skeletons are all self-consistent
check_self_consistency(fromSkel);
check_self_consistency(toSkel);
check_self_consistency(temporary);
check_self_consistency(other_temporary);
check_self_consistency(another_temporary);
check_self_consistency(last_temporary);
}
}
//==============================================================================
TEST(NameManagement, Skeleton)
{
SkeletonPtr skel = Skeleton::create();
std::pair<Joint*, BodyNode*> pair;
pair = skel->createJointAndBodyNodePair<RevoluteJoint>(
nullptr, SingleDofJoint::Properties(std::string("joint")));
Joint* joint1 = pair.first;
BodyNode* body1 = pair.second;
pair = skel->createJointAndBodyNodePair<TranslationalJoint>(
body1, MultiDofJoint<3>::Properties(std::string("joint")));
Joint* joint2 = pair.first;
BodyNode* body2 = pair.second;
pair = skel->createJointAndBodyNodePair<FreeJoint>(
body2, MultiDofJoint<6>::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_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 (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);
}
