// 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]);
}
