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); } }
// Solve for a balanced pose using IK (Lesson 7 Answer) Eigen::VectorXd solveIK(SkeletonPtr biped) { // Modify the intial pose to one-foot stance before IK biped->setPosition(biped->getDof("j_shin_right")-> getIndexInSkeleton(), -1.4); biped->setPosition(biped->getDof("j_bicep_left_x")-> getIndexInSkeleton(), 0.8); biped->setPosition(biped->getDof("j_bicep_right_x")-> getIndexInSkeleton(), -0.8); Eigen::VectorXd newPose = biped->getPositions(); BodyNodePtr leftHeel = biped->getBodyNode("h_heel_left"); BodyNodePtr leftToe = biped->getBodyNode("h_toe_left"); double initialHeight = -0.8; for(std::size_t i = 0; i < default_ik_iterations; ++i) { Eigen::Vector3d deviation = biped->getCOM() - leftHeel->getCOM(); Eigen::Vector3d localCOM = leftHeel->getCOM(leftHeel); LinearJacobian jacobian = biped->getCOMLinearJacobian() - biped->getLinearJacobian(leftHeel, localCOM); // Sagittal deviation double error = deviation[0]; Eigen::VectorXd gradient = jacobian.row(0); Eigen::VectorXd newDirection = -0.2 * error * gradient; // Lateral deviation error = deviation[2]; gradient = jacobian.row(2); newDirection += -0.2 * error * gradient; // Position constraint on four (approximated) corners of the left foot Eigen::Vector3d offset(0.0, -0.04, -0.03); error = (leftHeel->getTransform() * offset)[1] - initialHeight; gradient = biped->getLinearJacobian(leftHeel, offset).row(1); newDirection += -0.2 * error * gradient; offset[2] = 0.03; error = (leftHeel->getTransform() * offset)[1] - initialHeight; gradient = biped->getLinearJacobian(leftHeel, offset).row(1); newDirection += -0.2 * error * gradient; offset[0] = 0.04; error = (leftToe->getTransform() * offset)[1] - initialHeight; gradient = biped->getLinearJacobian(leftToe, offset).row(1); newDirection += -0.2 * error * gradient; offset[2] = -0.03; error = (leftToe->getTransform() * offset)[1] - initialHeight; gradient = biped->getLinearJacobian(leftToe, offset).row(1); newDirection += -0.2 * error * gradient; newPose += newDirection; biped->setPositions(newPose); biped->computeForwardKinematics(true, false, false); } return newPose; }
void setupStartConfiguration(const SkeletonPtr& atlas) { // Squat with the right leg atlas->getDof("r_leg_hpy")->setPosition(-45.0*M_PI/180.0); atlas->getDof("r_leg_kny")->setPosition( 90.0*M_PI/180.0); atlas->getDof("r_leg_aky")->setPosition(-45.0*M_PI/180.0); // Squat with the left left atlas->getDof("l_leg_hpy")->setPosition(-45.0*M_PI/180.0); atlas->getDof("l_leg_kny")->setPosition( 90.0*M_PI/180.0); atlas->getDof("l_leg_aky")->setPosition(-45.0*M_PI/180.0); // Get the right arm into a comfortable position atlas->getDof("r_arm_shx")->setPosition( 65.0*M_PI/180.0); atlas->getDof("r_arm_ely")->setPosition( 90.0*M_PI/180.0); atlas->getDof("r_arm_elx")->setPosition(-90.0*M_PI/180.0); atlas->getDof("r_arm_wry")->setPosition( 65.0*M_PI/180.0); // Get the left arm into a comfortable position atlas->getDof("l_arm_shx")->setPosition(-65.0*M_PI/180.0); atlas->getDof("l_arm_ely")->setPosition( 90.0*M_PI/180.0); atlas->getDof("l_arm_elx")->setPosition( 90.0*M_PI/180.0); atlas->getDof("l_arm_wry")->setPosition( 65.0*M_PI/180.0); // Prevent the knees from bending backwards atlas->getDof("r_leg_kny")->setPositionLowerLimit( 10*M_PI/180.0); atlas->getDof("l_leg_kny")->setPositionLowerLimit( 10*M_PI/180.0); }
int main(int argc, char* argv[]) { // Create an empty Skeleton with the name "pendulum" SkeletonPtr pendulum = Skeleton::create("pendulum"); // Add each body to the last BodyNode in the pendulum BodyNode* bn = makeRootBody(pendulum, "body1"); bn = addBody(pendulum, bn, "body2"); bn = addBody(pendulum, bn, "body3"); bn = addBody(pendulum, bn, "body4"); bn = addBody(pendulum, bn, "body5"); // Set the initial position of the first DegreeOfFreedom so that the pendulum // starts to swing right away pendulum->getDof(1)->setPosition(120 * M_PI / 180.0); // Create a world and add the pendulum to the world WorldPtr world(new World); world->addSkeleton(pendulum); // Create a window for rendering the world and handling user input MyWindow window(world); // Print instructions std::cout << "space bar: simulation on/off" << std::endl; std::cout << "'p': replay simulation" << std::endl; std::cout << "'1' -> '9': apply torque to a pendulum body" << std::endl; std::cout << "'-': Change sign of applied joint torques" << std::endl; std::cout << "'q': Increase joint rest positions" << std::endl; std::cout << "'a': Decrease joint rest positions" << std::endl; std::cout << "'w': Increase joint spring stiffness" << std::endl; std::cout << "'s': Decrease joint spring stiffness" << std::endl; std::cout << "'e': Increase joint damping" << std::endl; std::cout << "'d': Decrease joint damping" << std::endl; std::cout << "'r': add/remove constraint on the end of the chain" << std::endl; std::cout << "'f': switch between applying joint torques and body forces" << std::endl; // Initialize glut, initialize the window, and begin the glut event loop glutInit(&argc, argv); window.initWindow(640, 480, "Multi-Pendulum Tutorial"); glutMainLoop(); }
// Set initial configuration (Lesson 2 Answer) void setInitialPose(SkeletonPtr biped) { biped->setPosition(biped->getDof("j_thigh_left_z")-> getIndexInSkeleton(), 0.15); biped->setPosition(biped->getDof("j_thigh_right_z")-> getIndexInSkeleton(), 0.15); biped->setPosition(biped->getDof("j_shin_left")-> getIndexInSkeleton(), -0.4); biped->setPosition(biped->getDof("j_shin_right")-> getIndexInSkeleton(), -0.4); biped->setPosition(biped->getDof("j_heel_left_1")-> getIndexInSkeleton(), 0.25); biped->setPosition(biped->getDof("j_heel_right_1")-> getIndexInSkeleton(), 0.25); }
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); }
void setupEndEffectors(const SkeletonPtr& atlas) { // Apply very small weights to the gradient of the root joint in order to // encourage the arms to use arm joints instead of only moving around the root // joint Eigen::VectorXd rootjoint_weights = Eigen::VectorXd::Ones(6); rootjoint_weights = 0.01*rootjoint_weights; // Setting the bounds to be infinite allows the end effector to be implicitly // unconstrained Eigen::Vector3d linearBounds = Eigen::Vector3d::Constant(std::numeric_limits<double>::infinity()); Eigen::Vector3d angularBounds = Eigen::Vector3d::Constant(std::numeric_limits<double>::infinity()); // -- Set up the left hand -- // Create a relative transform for the EndEffector frame. This is the // transform that the left hand will have relative to the BodyNode that it is // attached to Eigen::Isometry3d tf_hand(Eigen::Isometry3d::Identity()); tf_hand.translation() = Eigen::Vector3d(0.0009, 0.1254, 0.012); tf_hand.rotate(Eigen::AngleAxisd(90.0*M_PI/180.0, Eigen::Vector3d::UnitZ())); // Create the left hand's end effector and set its relative transform EndEffector* l_hand = atlas->getBodyNode("l_hand")->createEndEffector("l_hand"); l_hand->setDefaultRelativeTransform(tf_hand, true); // Create an interactive frame to use as the target for the left hand osgDart::InteractiveFramePtr lh_target(new osgDart::InteractiveFrame( Frame::World(), "lh_target")); // Set the target of the left hand to the interactive frame. We pass true into // the function to tell it that it should create the IK module if it does not // already exist. If we don't do that, then calling getIK() could return a // nullptr if the IK module was never created. l_hand->getIK(true)->setTarget(lh_target); // Tell the left hand to use the whole body for its IK l_hand->getIK()->useWholeBody(); // Set the weights for the gradient l_hand->getIK()->getGradientMethod().setComponentWeights(rootjoint_weights); // Set the bounds for the IK to be infinite so that the hands start out // unconstrained l_hand->getIK()->getErrorMethod().setLinearBounds( -linearBounds, linearBounds); l_hand->getIK()->getErrorMethod().setAngularBounds( -angularBounds, angularBounds); // -- Set up the right hand -- // The orientation of the right hand frame is different than the left, so we // need to adjust the signs of the relative transform tf_hand.translation()[0] = -tf_hand.translation()[0]; tf_hand.translation()[1] = -tf_hand.translation()[1]; tf_hand.linear() = tf_hand.linear().inverse(); // Create the right hand's end effector and set its relative transform EndEffector* r_hand = atlas->getBodyNode("r_hand")->createEndEffector("r_hand"); r_hand->setDefaultRelativeTransform(tf_hand, true); // Create an interactive frame to use as the target for the right hand osgDart::InteractiveFramePtr rh_target(new osgDart::InteractiveFrame( Frame::World(), "rh_target")); // Create the right hand's IK and set its target r_hand->getIK(true)->setTarget(rh_target); // Tell the right hand to use the whole body for its IK r_hand->getIK()->useWholeBody(); // Set the weights for the gradient r_hand->getIK()->getGradientMethod().setComponentWeights(rootjoint_weights); // Set the bounds for the IK to be infinite so that the hands start out // unconstrained r_hand->getIK()->getErrorMethod().setLinearBounds( -linearBounds, linearBounds); r_hand->getIK()->getErrorMethod().setAngularBounds( -angularBounds, angularBounds); // Define the support geometry for the feet. These points will be used to // compute the convex hull of the robot's support polygon dart::math::SupportGeometry support; const double sup_pos_x = 0.10-0.186; const double sup_neg_x = -0.03-0.186; const double sup_pos_y = 0.03; const double sup_neg_y = -0.03; support.push_back(Eigen::Vector3d(sup_neg_x, sup_neg_y, 0.0)); support.push_back(Eigen::Vector3d(sup_pos_x, sup_neg_y, 0.0)); support.push_back(Eigen::Vector3d(sup_pos_x, sup_pos_y, 0.0)); support.push_back(Eigen::Vector3d(sup_neg_x, sup_pos_y, 0.0)); // Create a relative transform that goes from the center of the feet to the // bottom of the feet Eigen::Isometry3d tf_foot(Eigen::Isometry3d::Identity()); tf_foot.translation() = Eigen::Vector3d(0.186, 0.0, -0.08); // Constrain the feet to snap to the ground linearBounds[2] = 1e-8; // Constrain the feet to lie flat on the ground angularBounds[0] = 1e-8; angularBounds[1] = 1e-8; // Create an end effector for the left foot and set its relative transform EndEffector* l_foot = atlas->getBodyNode("l_foot")->createEndEffector("l_foot"); l_foot->setRelativeTransform(tf_foot); // Create an interactive frame to use as the target for the left foot osgDart::InteractiveFramePtr lf_target(new osgDart::InteractiveFrame( Frame::World(), "lf_target")); // Create the left foot's IK and set its target l_foot->getIK(true)->setTarget(lf_target); // Set the left foot's IK hierarchy level to 1. This will project its IK goals // through the null space of any IK modules that are on level 0. This means // that it will try to accomplish its goals while also accommodating the goals // of other modules. l_foot->getIK()->setHierarchyLevel(1); // Use the bounds defined above l_foot->getIK()->getErrorMethod().setLinearBounds( -linearBounds, linearBounds); l_foot->getIK()->getErrorMethod().setAngularBounds( -angularBounds, angularBounds); // Create Support for the foot and give it geometry l_foot->getSupport(true)->setGeometry(support); // Turn on support mode so that it can be used as a foot l_foot->getSupport()->setActive(); // Create an end effector for the right foot and set its relative transform EndEffector* r_foot = atlas->getBodyNode("r_foot")->createEndEffector("r_foot"); r_foot->setRelativeTransform(tf_foot); // Create an interactive frame to use as the target for the right foot osgDart::InteractiveFramePtr rf_target(new osgDart::InteractiveFrame( Frame::World(), "rf_target")); // Create the right foot's IK module and set its target r_foot->getIK(true)->setTarget(rf_target); // Set the right foot's IK hierarchy level to 1 r_foot->getIK()->setHierarchyLevel(1); // Use the bounds defined above r_foot->getIK()->getErrorMethod().setLinearBounds( -linearBounds, linearBounds); r_foot->getIK()->getErrorMethod().setAngularBounds( -angularBounds, angularBounds); // Create Support for the foot and give it geometry r_foot->getSupport(true)->setGeometry(support); // Turn on support mode so that it can be used as a foot r_foot->getSupport()->setActive(); // Move atlas to the ground so that it starts out squatting with its feet on // the ground double heightChange = -r_foot->getWorldTransform().translation()[2]; atlas->getDof(5)->setPosition(heightChange); // Now that the feet are on the ground, we should set their target transforms l_foot->getIK()->getTarget()->setTransform(l_foot->getTransform()); r_foot->getIK()->getTarget()->setTransform(r_foot->getTransform()); }