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 setAllColors(const SkeletonPtr& object, const Eigen::Vector3d& color)
{
// Set the color of all the shapes in the object
for(size_t i=0; i < object->getNumBodyNodes(); ++i)
{
BodyNode* bn = object->getBodyNode(i);
for(size_t j=0; j < bn->getNumVisualizationShapes(); ++j)
bn->getVisualizationShape(j)->setColor(color);
}
}
SkeletonPtr createGround()
{
// Create a Skeleton to represent the ground
SkeletonPtr ground = Skeleton::create("ground");
Eigen::Isometry3d tf(Eigen::Isometry3d::Identity());
double thickness = 0.01;
tf.translation() = Eigen::Vector3d(0,0,-thickness/2.0);
WeldJoint::Properties joint;
joint.mT_ParentBodyToJoint = tf;
ground->createJointAndBodyNodePair<WeldJoint>(nullptr, joint);
ShapePtr groundShape =
std::make_shared<BoxShape>(Eigen::Vector3d(10,10,thickness));
groundShape->setColor(dart::Color::Blue(0.2));
ground->getBodyNode(0)->addVisualizationShape(groundShape);
ground->getBodyNode(0)->addCollisionShape(groundShape);
return ground;
}
SkeletonPtr createHubo()
{
dart::utils::DartLoader loader;
loader.addPackageDirectory("drchubo", DART_DATA_PATH"/urdf/drchubo");
SkeletonPtr hubo =
loader.parseSkeleton(DART_DATA_PATH"/urdf/drchubo/drchubo.urdf");
for(size_t i = 0; i < hubo->getNumBodyNodes(); ++i)
{
BodyNode* bn = hubo->getBodyNode(i);
if(bn->getName().substr(0, 7) == "Body_LF"
|| bn->getName().substr(0, 7) == "Body_RF"
|| bn->getName().substr(0, 7) == "Body_NK")
{
bn->remove();
--i;
}
}
hubo->setPosition(5, 0.97);
for(size_t i=1; i < hubo->getNumJoints(); ++i)
{
hubo->getJoint(i)->setActuatorType(Joint::VELOCITY);
}
for(size_t i=0; i < hubo->getNumBodyNodes(); ++i)
{
BodyNode* bn = hubo->getBodyNode(i);
for(size_t j=0; j < bn->getNumVisualizationShapes(); ++j)
{
const ShapePtr& shape = bn->getVisualizationShape(j);
shape->setColor(Eigen::Vector3d(0.2, 0.2, 0.2));
if(MeshShapePtr mesh = std::dynamic_pointer_cast<MeshShape>(shape))
mesh->setColorMode(MeshShape::SHAPE_COLOR);
}
}
hubo->setName("drchubo");
return hubo;
}
void setAllColors(const SkeletonPtr& object, const Eigen::Vector3d& color)
{
// Set the color of all the shapes in the object
for(size_t i=0; i < object->getNumBodyNodes(); ++i)
{
BodyNode* bn = object->getBodyNode(i);
auto visualShapeNodes = bn->getShapeNodesWith<VisualAddon>();
for(auto visualShapeNode : visualShapeNodes)
visualShapeNode->getVisualAddon()->setColor(color);
}
}
// Load a skateboard model and connect it to the biped model via an Euler joint
// (Lesson 5 Answer)
void modifyBipedWithSkateboard(SkeletonPtr biped)
{
// Load the Skeleton from a file
WorldPtr world = SkelParser::readWorld("dart://sample/skel/skateboard.skel");
SkeletonPtr skateboard = world->getSkeleton(0);
EulerJoint::Properties properties = EulerJoint::Properties();
properties.mT_ChildBodyToJoint.translation() = Eigen::Vector3d(0, 0.1, 0);
skateboard->getRootBodyNode()->moveTo<EulerJoint>
(biped->getBodyNode("h_heel_left"), properties);
}
TEST(Skeleton, ZeroDofJointInReferential)
{
// This is a regression test which makes sure that the BodyNodes of
// ZeroDofJoints will be correctly included in linkages.
SkeletonPtr skel = Skeleton::create();
BodyNode* bn = skel->createJointAndBodyNodePair<RevoluteJoint>().second;
BodyNode* zeroDof1 = skel->createJointAndBodyNodePair<WeldJoint>(bn).second;
bn = skel->createJointAndBodyNodePair<PrismaticJoint>(zeroDof1).second;
BodyNode* zeroDof2 = skel->createJointAndBodyNodePair<WeldJoint>(bn).second;
BranchPtr branch = Branch::create(skel->getBodyNode(0));
EXPECT_EQ(branch->getNumBodyNodes(), skel->getNumBodyNodes());
EXPECT_FALSE(branch->getIndexOf(zeroDof1) == INVALID_INDEX);
EXPECT_FALSE(branch->getIndexOf(zeroDof2) == INVALID_INDEX);
}
void enableDragAndDrops(osgDart::Viewer& viewer, const SkeletonPtr& atlas)
{
// Turn on drag-and-drop for the whole Skeleton
for(size_t i=0; i < atlas->getNumBodyNodes(); ++i)
viewer.enableDragAndDrop(atlas->getBodyNode(i), false, false);
for(size_t i=0; i < atlas->getNumEndEffectors(); ++i)
{
EndEffector* ee = atlas->getEndEffector(i);
if(!ee->getIK())
continue;
// Check whether the target is an interactive frame, and add it if it is
if(const auto& frame = std::dynamic_pointer_cast<osgDart::InteractiveFrame>(
ee->getIK()->getTarget()))
viewer.enableDragAndDrop(frame.get());
}
}
void checkForBodyNodes(
size_t& count,
const ReferentialSkeletonPtr& refSkel,
const SkeletonPtr& skel,
const std::string& name,
Args ... args)
{
bool contains = refSkel->getIndexOf(skel->getBodyNode(name)) != INVALID_INDEX;
EXPECT_TRUE(contains);
if(!contains)
{
dtmsg << "The ReferentialSkeleton [" << refSkel->getName() << "] does NOT "
<< "contain the BodyNode [" << name << "] of the Skeleton ["
<< skel->getName() << "]\n";
}
++count;
checkForBodyNodes(count, refSkel, skel, args...);
}
SkeletonPtr createAtlas()
{
// Parse in the atlas model
DartLoader urdf;
SkeletonPtr atlas =
urdf.parseSkeleton(DART_DATA_PATH"sdf/atlas/atlas_v3_no_head.urdf");
// Add a box to the root node to make it easier to click and drag
double scale = 0.25;
ShapePtr boxShape =
std::make_shared<BoxShape>(scale*Eigen::Vector3d(1.0, 1.0, 0.5));
boxShape->setColor(dart::Color::Black());
Eigen::Isometry3d tf(Eigen::Isometry3d::Identity());
tf.translation() = Eigen::Vector3d(0.1*Eigen::Vector3d(0.0, 0.0, 1.0));
boxShape->setLocalTransform(tf);
atlas->getBodyNode(0)->addVisualizationShape(boxShape);
return atlas;
}
//==============================================================================
TEST(FORWARD_KINEMATICS, YAW_ROLL)
{
// Checks forward kinematics for two DoF arm manipulators.
// NOTE: The following is the reference frame description of the world
// frame. The x-axis is into the page, z-axis is to the top of the
// page and the y-axis is to the left. At the zero angle, the links
// are parallel to the z-axis and face the +x-axis.
// Create the world
const double l1 = 1.5, l2 = 1.0;
SkeletonPtr robot = createTwoLinkRobot(Vector3d(0.3, 0.3, l1), DOF_YAW,
Vector3d(0.3, 0.3, l2), DOF_ROLL);
// Set the test cases with the joint values and the expected end-effector
// positions
const std::size_t numTests = 2;
double temp = sqrt(0.5*l2*l2);
Vector2d joints [numTests] = { Vector2d( constantsd::pi()/4.0, constantsd::pi()/2.0),
Vector2d(-constantsd::pi()/4.0, -constantsd::pi()/4.0) };
Vector3d expectedPos [numTests] = { Vector3d(temp, -temp, l1),
Vector3d(temp / sqrt(2.0),
temp / sqrt(2.0), l1+temp) };
// Check each case by setting the joint values and obtaining the end-effector
// position
for (std::size_t i = 0; i < numTests; i++)
{
robot->setPositions(Eigen::VectorXd(joints[i]));
BodyNode* bn = robot->getBodyNode("ee");
Vector3d actual = bn->getTransform().translation();
bool equality = equals(actual, expectedPos[i], 1e-3);
EXPECT_TRUE(equality);
if(!equality)
{
std::cout << "Joint values: " << joints[i].transpose() << std::endl;
std::cout << "Actual pos: " << actual.transpose() << std::endl;
std::cout << "Expected pos: " << expectedPos[i].transpose() << std::endl;
}
}
}
//==============================================================================
TEST(SdfParser, SDFSingleBodyWithoutJoint)
{
// Regression test for #444
WorldPtr world
= SdfParser::readSdfFile(
DART_DATA_PATH"/sdf/test/single_bodynode_skeleton.world");
EXPECT_TRUE(world != nullptr);
SkeletonPtr skel = world->getSkeleton(0);
EXPECT_TRUE(skel != nullptr);
EXPECT_EQ(skel->getNumBodyNodes(), 1u);
EXPECT_EQ(skel->getNumJoints(), 1u);
BodyNodePtr bodyNode = skel->getBodyNode(0);
EXPECT_TRUE(bodyNode != nullptr);
EXPECT_EQ(bodyNode->getNumVisualizationShapes(), 1u);
EXPECT_EQ(bodyNode->getNumCollisionShapes(), 1u);
JointPtr joint = skel->getJoint(0);
EXPECT_TRUE(joint != nullptr);
EXPECT_EQ(joint->getType(), FreeJoint::getStaticType());
}
TEST(Skeleton, NodePersistence)
{
SkeletonPtr skel = Skeleton::create();
skel->createJointAndBodyNodePair<FreeJoint>(nullptr);
{
EndEffector* manip =
skel->getBodyNode(0)->createEndEffector(Entity::Properties("manip"));
EXPECT_TRUE(skel->getEndEffector("manip") == manip);
EXPECT_TRUE(skel->getEndEffector(0) == manip);
EXPECT_TRUE(skel->getBodyNode(0)->getEndEffector(0) == manip);
WeakEndEffectorPtr weakManip = manip;
EXPECT_FALSE(weakManip.lock() == nullptr);
manip->remove();
// The Node has been removed, and no strong reference to it exists, so it
// should be gone from the Skeleton
EXPECT_TRUE(skel->getEndEffector("manip") == nullptr);
EXPECT_TRUE(skel->getNumEndEffectors() == 0);
EXPECT_TRUE(skel->getBodyNode(0)->getNumEndEffectors() == 0);
EXPECT_TRUE(weakManip.lock() == nullptr);
}
{
EndEffector* manip =
skel->getBodyNode(0)->createEndEffector(Entity::Properties("manip"));
EXPECT_TRUE(skel->getEndEffector("manip") == manip);
EXPECT_TRUE(skel->getEndEffector(0) == manip);
EXPECT_TRUE(skel->getBodyNode(0)->getEndEffector(0) == manip);
EndEffectorPtr strongManip = manip;
WeakEndEffectorPtr weakManip = strongManip;
EXPECT_FALSE(weakManip.lock() == nullptr);
manip->remove();
// The Node has been removed, but a strong reference to it still exists, so
// it will remain in the Skeleton for now
EXPECT_TRUE(skel->getEndEffector("manip") == manip);
EXPECT_TRUE(skel->getEndEffector(0) == manip);
EXPECT_TRUE(skel->getBodyNode(0)->getEndEffector(0) == manip);
EXPECT_FALSE(weakManip.lock() == nullptr);
strongManip = nullptr;
// The Node has been removed, and no strong reference to it exists any
// longer, so it should be gone from the Skeleton
EXPECT_TRUE(skel->getEndEffector("manip") == nullptr);
EXPECT_TRUE(skel->getNumEndEffectors() == 0);
EXPECT_TRUE(skel->getBodyNode(0)->getNumEndEffectors() == 0);
EXPECT_TRUE(weakManip.lock() == nullptr);
}
}
//==============================================================================
void ConstraintTest::SingleContactTest(const std::string& /*_fileName*/)
{
using namespace std;
using namespace Eigen;
using namespace dart::math;
using namespace dart::collision;
using namespace dart::constraint;
using namespace dart::dynamics;
using namespace dart::simulation;
using namespace dart::io;
//----------------------------------------------------------------------------
// Settings
//----------------------------------------------------------------------------
// Number of random state tests for each skeletons
#ifndef NDEBUG // Debug mode
// std::size_t testCount = 1;
#else
// std::size_t testCount = 1;
#endif
WorldPtr world = World::create();
EXPECT_TRUE(world != nullptr);
world->setGravity(Vector3d(0.0, -10.00, 0.0));
world->setTimeStep(0.001);
world->getConstraintSolver()->setCollisionDetector(
DARTCollisionDetector::create());
SkeletonPtr sphereSkel = createSphere(0.05, Vector3d(0.0, 1.0, 0.0));
BodyNode* sphere = sphereSkel->getBodyNode(0);
Joint* sphereJoint = sphere->getParentJoint();
sphereJoint->setVelocity(3, Random::uniform(-2.0, 2.0)); // x-axis
sphereJoint->setVelocity(5, Random::uniform(-2.0, 2.0)); // z-axis
world->addSkeleton(sphereSkel);
EXPECT_EQ(sphereSkel->getGravity(), world->getGravity());
assert(sphere);
SkeletonPtr boxSkel = createBox(Vector3d(1.0, 1.0, 1.0),
Vector3d(0.0, 1.0, 0.0));
BodyNode* box = boxSkel->getBodyNode(0);
Joint* boxJoint = box->getParentJoint();
boxJoint->setVelocity(3, Random::uniform(-2.0, 2.0)); // x-axis
boxJoint->setVelocity(5, Random::uniform(-2.0, 2.0)); // z-axis
// world->addSkeleton(boxSkel);
// EXPECT_EQ(boxSkel->getGravity(), world->getGravity());
// assert(box);
SkeletonPtr groundSkel = createGround(Vector3d(10000.0, 0.1, 10000.0),
Vector3d(0.0, -0.05, 0.0));
groundSkel->setMobile(false);
// BodyNode* ground = groundSkel->getBodyNode(0);
world->addSkeleton(groundSkel);
EXPECT_EQ(groundSkel->getGravity(), world->getGravity());
// assert(ground);
EXPECT_EQ((int)world->getNumSkeletons(), 2);
// Lower and upper bound of configuration for system
// double lb = -1.5 * constantsd::pi();
// double ub = 1.5 * constantsd::pi();
int maxSteps = 500;
for (int i = 0; i < maxSteps; ++i)
{
// Vector3d pos1 = sphere->getWorldTransform().translation();
// Vector3d vel1 = sphere->getWorldLinearVelocity(pos1);
// std::cout << "pos1:" << pos1.transpose() << std::endl;
// std::cout << "vel1:" << vel1.transpose() << std::endl;
if (!world->checkCollision())
{
world->step();
continue;
}
// for (std::size_t j = 0; j < cd->getNumContacts(); ++j)
// {
// Contact contact = cd->getContact(j);
// Vector3d pos1 = sphere->getTransform().inverse() * contact.point;
// Vector3d vel1 = sphere->getWorldLinearVelocity(pos1);
// std::cout << "pos1:" << pos1.transpose() << std::endl;
// std::cout << "vel1:" << vel1.transpose() << std::endl;
// }
world->step();
const auto& result = world->getConstraintSolver()->getLastCollisionResult();
for (const auto& contact : result.getContacts())
{
Vector3d pos1 = sphere->getTransform().inverse() * contact.point;
Vector3d vel1 = sphere->getLinearVelocity(pos1);
// std::cout << "pos1:" << pos1.transpose() << std::endl;
// std::cout << "pos1[1]: " << pos1[1] << std::endl;
// std::cout << "pos1:" << pos1.transpose() << std::endl;
std::cout << "vel1:" << vel1.transpose() << ", pos1[1]: " << pos1[1] << std::endl;
// EXPECT_NEAR(pos1[0], 0.0, 1e-9);
// EXPECT_NEAR(pos1[1], -0.05, 1e-2);
// EXPECT_NEAR(pos1[2], 0.0, 1e-9);
// EXPECT_NEAR(vel1[0], 0.0, 1e-9);
// EXPECT_NEAR(vel1[1], 0.0, 1e-9);
// EXPECT_NEAR(vel1[2], 0.0, 1e-9);
// if (!equals(vel1, Vector3d(0.0, 0.0, 0.0)))
// std::cout << "vel1:" << vel1.transpose() << std::endl;
// EXPECT_EQ(vel1, Vector3d::Zero());
}
// std::cout << std::endl;
break;
}
}
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());
}
//==============================================================================
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);
}
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(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, Linkage)
{
// Test a variety of uses of Linkage::Criteria
SkeletonPtr skel = constructLinkageTestSkeleton();
BranchPtr subtree = Branch::create(skel->getBodyNode("c3b3"), "subtree");
checkForBodyNodes(subtree, skel, true,
"c3b3", "c3b4", "c4b1", "c4b2", "c4b3");
ChainPtr midchain = Chain::create(skel->getBodyNode("c1b3"),
skel->getBodyNode("c3b4"), "midchain");
checkForBodyNodes(midchain, skel, true, "c3b1", "c3b2", "c3b3");
checkLinkageJointConsistency(midchain);
Linkage::Criteria criteria;
criteria.mStart = skel->getBodyNode("c5b2");
criteria.mTargets.push_back(
Linkage::Criteria::Target(skel->getBodyNode("c4b3")));
LinkagePtr path = Linkage::create(criteria, "path");
checkForBodyNodes(path, skel, true, "c5b2", "c5b1", "c1b3", "c3b1", "c3b2",
"c3b3", "c4b1", "c4b2", "c4b3");
checkLinkageJointConsistency(path);
skel->getBodyNode(0)->copyTo(nullptr);
criteria.mTargets.clear();
criteria.mStart = skel->getBodyNode("c3b1");
criteria.mStart.mPolicy = Linkage::Criteria::UPSTREAM;
criteria.mTargets.push_back(
Linkage::Criteria::Target(skel->getBodyNode("c3b1(1)"),
Linkage::Criteria::UPSTREAM));
LinkagePtr combinedTreeBases = Linkage::create(criteria, "combinedTreeBases");
checkForBodyNodes(combinedTreeBases, skel, true,
"c3b1", "c1b3", "c2b1", "c2b2", "c2b3",
"c3b1(1)", "c1b3(1)", "c2b1(1)", "c2b2(1)", "c2b3(1)",
"c5b1", "c5b2", "c1b2", "c1b1",
"c5b1(1)", "c5b2(1)", "c1b2(1)", "c1b1(1)");
checkLinkageJointConsistency(combinedTreeBases);
SkeletonPtr skel2 = skel->getBodyNode(0)->copyAs("skel2");
criteria.mTargets.clear();
criteria.mTargets.push_back(
Linkage::Criteria::Target(skel2->getBodyNode("c3b1"),
Linkage::Criteria::UPSTREAM));
LinkagePtr combinedSkelBases = Linkage::create(criteria, "combinedSkelBases");
size_t count = 0;
count += checkForBodyNodes(combinedSkelBases, skel, false,
"c3b1", "c1b3", "c2b1", "c2b2", "c2b3",
"c5b1", "c5b2", "c1b2", "c1b1");
count += checkForBodyNodes(combinedSkelBases, skel2, false,
"c3b1", "c1b3", "c2b1", "c2b2", "c2b3",
"c5b1", "c5b2", "c1b2", "c1b1");
EXPECT_TRUE( count == combinedSkelBases->getNumBodyNodes() );
ChainPtr downstreamFreeJoint = Chain::create(skel->getBodyNode("c1b1"),
skel->getBodyNode("c1b3"), Chain::IncludeBoth, "downstreamFreeJoint");
checkForBodyNodes(downstreamFreeJoint, skel, true, "c1b1");
checkLinkageJointConsistency(downstreamFreeJoint);
ChainPtr emptyChain = Chain::create(skel->getBodyNode("c1b1"),
skel->getBodyNode("c1b3"), "emptyChain");
checkForBodyNodes(emptyChain, skel, true);
checkLinkageJointConsistency(emptyChain);
ChainPtr chainFromNull = Chain::create(nullptr, skel->getBodyNode("c1b2"),
"chainFromNull");
checkForBodyNodes(chainFromNull, skel, true, "c1b1");
checkLinkageJointConsistency(chainFromNull);
ChainPtr upstreamFreeJoint = Chain::create(skel->getBodyNode("c1b3"),
skel->getBodyNode("c1b1"), "upstreamFreeJoint");
checkForBodyNodes(upstreamFreeJoint, skel, true, "c1b3", "c1b2");
checkLinkageJointConsistency(upstreamFreeJoint);
// Using nullptr as the target should bring us towards the root of the tree
ChainPtr upTowardsRoot =
Chain::create(skel->getBodyNode("c1b3"), nullptr, "upTowardsRoot");
checkForBodyNodes(upTowardsRoot, skel, true, "c1b3", "c1b2");
checkLinkageJointConsistency(upTowardsRoot);
criteria.mTargets.clear();
criteria.mTargets.push_back(skel->getBodyNode("c4b3"));
criteria.mStart = skel->getBodyNode("c1b3");
criteria.mTerminals.push_back(skel->getBodyNode("c3b2"));
LinkagePtr terminatedLinkage = Linkage::create(criteria, "terminatedLinkage");
checkForBodyNodes(terminatedLinkage, skel, true,
"c1b3", "c3b1", "c3b2");
checkLinkageJointConsistency(terminatedLinkage);
criteria.mStart = skel->getBodyNode("c1b1");
criteria.mStart.mPolicy = Linkage::Criteria::DOWNSTREAM;
criteria.mTargets.clear();
criteria.mTerminals.clear();
criteria.mTerminals.push_back(
Linkage::Criteria::Terminal(skel->getBodyNode("c2b1"), false));
criteria.mTerminals.push_back(skel->getBodyNode("c3b3"));
LinkagePtr terminatedSubtree = Linkage::create(criteria, "terminatedSubtree");
checkForBodyNodes(terminatedSubtree, skel, true,
"c1b1", "c1b2", "c1b3", "c5b1",
"c5b2", "c3b1", "c3b2", "c3b3");
checkLinkageJointConsistency(terminatedSubtree);
criteria.mStart.mPolicy = Linkage::Criteria::UPSTREAM;
criteria.mStart.mNode = skel->getBodyNode("c3b1");
LinkagePtr terminatedUpstream = Linkage::create(criteria, "terminatedUpstream");
checkForBodyNodes(terminatedUpstream, skel, true,
"c3b1", "c1b3", "c5b1", "c5b2", "c1b2", "c1b1");
checkLinkageJointConsistency(terminatedUpstream);
}
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);
}
