int main() { WorldPtr world(new dart::simulation::World); world->setTimeStep(1.0/frequency); SkeletonPtr hubo = createHubo(); world->addSkeleton(hubo); world->addSkeleton(createGround()); std::string name = "/home/grey/projects/protoHuboGUI/trajectory.dat"; std::ifstream file; file.open(name); std::vector<Eigen::VectorXd> trajectory; if(file.is_open()) { while(!file.eof()) { trajectory.push_back(Eigen::VectorXd(hubo->getNumDofs())); Eigen::VectorXd& q = trajectory.back(); for(size_t i=0; i < hubo->getNumDofs(); ++i) file >> q[i]; } } else {
/// Constructor Controller(const SkeletonPtr& biped) : mBiped(biped), mPreOffset(0.0), mSpeed(0.0) { int nDofs = mBiped->getNumDofs(); mForces = Eigen::VectorXd::Zero(nDofs); mKp = Eigen::MatrixXd::Identity(nDofs, nDofs); mKd = Eigen::MatrixXd::Identity(nDofs, nDofs); for(std::size_t i = 0; i < 6; ++i) { mKp(i, i) = 0.0; mKd(i, i) = 0.0; } for(std::size_t i = 6; i < biped->getNumDofs(); ++i) { mKp(i, i) = 1000; mKd(i, i) = 50; } setTargetPositions(mBiped->getPositions()); }
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); } }
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, 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); }
void setupWholeBodySolver(const SkeletonPtr& atlas) { // The default std::shared_ptr<dart::optimizer::GradientDescentSolver> solver = std::dynamic_pointer_cast<dart::optimizer::GradientDescentSolver>( atlas->getIK(true)->getSolver()); solver->setNumMaxIterations(10); size_t nDofs = atlas->getNumDofs(); double default_weight = 0.01; Eigen::VectorXd weights = default_weight * Eigen::VectorXd::Ones(nDofs); weights[2] = 0.0; weights[3] = 0.0; weights[4] = 0.0; weights[6] *= 0.2; weights[7] *= 0.2; weights[8] *= 0.2; Eigen::VectorXd lower_posture = Eigen::VectorXd::Constant( nDofs, -std::numeric_limits<double>::infinity()); lower_posture[0] = -0.35; lower_posture[1] = -0.35; lower_posture[5] = 0.600; lower_posture[6] = -0.1; lower_posture[7] = -0.1; lower_posture[8] = -0.1; Eigen::VectorXd upper_posture = Eigen::VectorXd::Constant( nDofs, std::numeric_limits<double>::infinity()); upper_posture[0] = 0.35; upper_posture[1] = 0.35; upper_posture[5] = 0.885; upper_posture[6] = 0.1; upper_posture[7] = 0.1; upper_posture[8] = 0.1; std::shared_ptr<RelaxedPosture> objective = std::make_shared<RelaxedPosture>( atlas->getPositions(), lower_posture, upper_posture, weights); atlas->getIK()->setObjective(objective); std::shared_ptr<dart::constraint::BalanceConstraint> balance = std::make_shared<dart::constraint::BalanceConstraint>(atlas->getIK()); atlas->getIK()->getProblem()->addEqConstraint(balance); // // Shift the center of mass towards the support polygon center while trying // // to keep the support polygon where it is // balance->setErrorMethod(dart::constraint::BalanceConstraint::FROM_CENTROID); // balance->setBalanceMethod(dart::constraint::BalanceConstraint::SHIFT_COM); // // Keep shifting the center of mass towards the center of the support // // polygon, even if it is already inside. This is useful for trying to // // optimize a stance // balance->setErrorMethod(dart::constraint::BalanceConstraint::OPTIMIZE_BALANCE); // balance->setBalanceMethod(dart::constraint::BalanceConstraint::SHIFT_COM); // // Try to leave the center of mass where it is while moving the support // // polygon to be under the current center of mass location balance->setErrorMethod(dart::constraint::BalanceConstraint::FROM_CENTROID); balance->setBalanceMethod(dart::constraint::BalanceConstraint::SHIFT_SUPPORT); // // Try to leave the center of mass where it is while moving the support // // point that is closest to the center of mass // balance->setErrorMethod(dart::constraint::BalanceConstraint::FROM_EDGE); // balance->setBalanceMethod(dart::constraint::BalanceConstraint::SHIFT_SUPPORT); // Note that using the FROM_EDGE error method is liable to leave the center of // mass visualization red even when the constraint was successfully solved. // This is because the constraint solver has a tiny bit of tolerance that // allows the Problem to be considered solved when the center of mass is // microscopically outside of the support polygon. This is an inherent risk of // using FROM_EDGE instead of FROM_CENTROID. // Feel free to experiment with the different balancing methods. You will find // that some work much better for user interaction than others. }