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.
}
