SkeletonPtr createFloor()
{
SkeletonPtr floor = Skeleton::create("floor");
// Give the floor a body
BodyNodePtr body
= floor->createJointAndBodyNodePair<WeldJoint>(nullptr).second;
// Give the body a shape
double floor_width = 10.0;
double floor_height = 0.01;
std::shared_ptr<BoxShape> box(
new BoxShape(Eigen::Vector3d(floor_width, floor_height, floor_width)));
auto shapeNode = body->createShapeNodeWith<
VisualAspect,
CollisionAspect,
DynamicsAspect>(box);
shapeNode->getVisualAspect()->setColor(dart::Color::Black());
// Put the body into position
Eigen::Isometry3d tf(Eigen::Isometry3d::Identity());
tf.translation() = Eigen::Vector3d(0.0, -1.0, 0.0);
body->getParentJoint()->setTransformFromParentBodyNode(tf);
return floor;
}
// 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 setGeometry(const BodyNodePtr& bn)
{
// Create a BoxShape to be used for both visualization and collision checking
std::shared_ptr<BoxShape> box(new BoxShape(
Eigen::Vector3d(default_width, default_depth, default_height)));
box->setColor(dart::Color::Blue());
// Set the location of the Box
Eigen::Isometry3d box_tf(Eigen::Isometry3d::Identity());
Eigen::Vector3d center = Eigen::Vector3d(0, 0, default_height / 2.0);
box_tf.translation() = center;
box->setLocalTransform(box_tf);
// Add it as a visualization and collision shape
bn->addVisualizationShape(box);
bn->addCollisionShape(box);
// Move the center of mass to the center of the object
bn->setLocalCOM(center);
}
//==============================================================================
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());
}
BodyNode* makeRootBody(const SkeletonPtr& pendulum, const std::string& name)
{
BallJoint::Properties properties;
properties.mName = name + "_joint";
properties.mRestPositions = Eigen::Vector3d::Constant(default_rest_position);
properties.mSpringStiffnesses = Eigen::Vector3d::Constant(default_stiffness);
properties.mDampingCoefficients = Eigen::Vector3d::Constant(default_damping);
BodyNodePtr bn = pendulum->createJointAndBodyNodePair<BallJoint>(
nullptr, properties, BodyNode::Properties(name)).second;
// Make a shape for the Joint
const double& R = default_width;
std::shared_ptr<EllipsoidShape> ball(
new EllipsoidShape(sqrt(2) * Eigen::Vector3d(R, R, R)));
ball->setColor(dart::Color::Blue());
bn->addVisualizationShape(ball);
// Set the geometry of the Body
setGeometry(bn);
return bn;
}
BodyNode* addBody(const SkeletonPtr& pendulum, BodyNode* parent,
const std::string& name)
{
// Set up the properties for the Joint
RevoluteJoint::Properties properties;
properties.mName = name + "_joint";
properties.mAxis = Eigen::Vector3d::UnitY();
properties.mT_ParentBodyToJoint.translation() =
Eigen::Vector3d(0, 0, default_height);
properties.mRestPosition = default_rest_position;
properties.mSpringStiffness = default_stiffness;
properties.mDampingCoefficient = default_damping;
// Create a new BodyNode, attached to its parent by a RevoluteJoint
BodyNodePtr bn = pendulum->createJointAndBodyNodePair<RevoluteJoint>(
parent, properties, BodyNode::Properties(name)).second;
// Make a shape for the Joint
const double R = default_width / 2.0;
const double h = default_depth;
std::shared_ptr<CylinderShape> cyl(
new CylinderShape(R, h));
cyl->setColor(dart::Color::Blue());
// Line up the cylinder with the Joint axis
Eigen::Isometry3d tf(Eigen::Isometry3d::Identity());
tf.linear() = dart::math::eulerXYZToMatrix(
Eigen::Vector3d(90.0 * M_PI / 180.0, 0, 0));
cyl->setLocalTransform(tf);
bn->addVisualizationShape(cyl);
// Set the geometry of the Body
setGeometry(bn);
return bn;
}
void setGeometry(const BodyNodePtr& bn)
{
// Create a BoxShape to be used for both visualization and collision checking
std::shared_ptr<BoxShape> box(new BoxShape(
Eigen::Vector3d(default_width, default_depth, default_height)));
// Create a shpae node for visualization and collision checking
auto shapeNode
= bn->createShapeNodeWith<VisualAspect, CollisionAspect, DynamicsAspect>(box);
shapeNode->getVisualAspect()->setColor(dart::Color::Blue());
// Set the location of the shape node
Eigen::Isometry3d box_tf(Eigen::Isometry3d::Identity());
Eigen::Vector3d center = Eigen::Vector3d(0, 0, default_height / 2.0);
box_tf.translation() = center;
shapeNode->setRelativeTransform(box_tf);
// Move the center of mass to the center of the object
bn->setLocalCOM(center);
}
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);
}
