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