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