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());
}
