Skeleton * BVHParser::createSkeleton() { Skeleton * s = new Skeleton(); // set default pose... Joint * b = createJoint(_root); if( !s->setJoints(b) ) { delete s; return 0; } Pose * pose = new Pose(s->getNumJoints()); for(int i = 0; i < _linearNodes.size(); i++ ) { BVHNode * n = _linearNodes[i]; pose->transforms[i].rotation.identity(); pose->transforms[i].position(0,0,0); } s->pose = pose; s->init(); return s; }
Skeleton* Mapper::newSkeleton(string id) { Skeleton skeleton; skeleton.init(id); skeleton.setSmoothing(defaultSmoothing); skeletons->push_back(skeleton); return &skeletons->at(skeletons->size()-1); }
void CollisionInterface::addRigidBody(RigidBody *_rb, const std::string& name) { Skeleton *skel = new Skeleton(); BodyNode *bn = new BodyNode(); bn->setParentJoint( new dart::dynamics::FreeJoint("freeJoint") ); bn->addCollisionShape(_rb->mShape); skel->addBodyNode( bn ); skel->setName( name ); skel->disableSelfCollision(); skel->init(); mCollisionChecker->addCollisionSkeletonNode(bn); mNodeMap[bn] = _rb; }
//============================================================================== TEST(InverseKinematics, FittingVelocity) { const double TOLERANCE = 1e-4; #ifdef BUILD_TYPE_RELEASE const size_t numRandomTests = 100; #else const size_t numRandomTests = 10; #endif // Create two link robot const double l1 = 1.5; const double l2 = 1.0; Skeleton* robot = createFreeFloatingTwoLinkRobot( Vector3d(0.3, 0.3, l1), Vector3d(0.3, 0.3, l2), DOF_ROLL); robot->init(); BodyNode* body1 = robot->getBodyNode(0); // BodyNode* body2 = robot->getBodyNode(1); Joint* joint1 = body1->getParentJoint(); // Joint* joint2 = body2->getParentJoint(); //------------------------- Free joint test ---------------------------------- // The parent joint of body1 is free joint so body1 should be able to // transform to arbitrary tramsformation. for (size_t i = 0; i < numRandomTests; ++i) { // Test for linear velocity Vector3d desiredVel = Vector3d::Random(); body1->fitWorldLinearVel(desiredVel); Vector3d fittedLinVel = body1->getWorldVelocity().tail<3>(); Vector3d fittedAngVel = body1->getWorldVelocity().head<3>(); Vector3d diff = fittedLinVel - desiredVel; EXPECT_NEAR(diff.dot(diff), 0.0, TOLERANCE); EXPECT_NEAR(fittedAngVel.dot(fittedAngVel), 0.0, TOLERANCE); joint1->setGenVels(Vector6d::Zero(), true, true); robot->setState(robot->getState(), true, true, false); // Test for angular velocity desiredVel = Vector3d::Random(); body1->fitWorldAngularVel(desiredVel); fittedLinVel = body1->getWorldVelocity().tail<3>(); fittedAngVel = body1->getWorldVelocity().head<3>(); diff = fittedAngVel - desiredVel; EXPECT_NEAR(fittedLinVel.dot(fittedLinVel), 0.0, TOLERANCE); EXPECT_NEAR(diff.dot(diff), 0.0, TOLERANCE); joint1->setGenVels(Vector6d::Zero(), true, true); robot->setState(robot->getState(), true, true, false); } }
Skeleton* createFreeFloatingTwoLinkRobot(Vector3d dim1, Vector3d dim2, TypeOfDOF type2, bool finished = true) { Skeleton* robot = new Skeleton(); // Create the first link, the joint with the ground and its shape double mass = 1.0; BodyNode* node = new BodyNode("link1"); Joint* joint = new FreeJoint(); joint->setName("joint1"); Shape* shape = new BoxShape(dim1); node->setLocalCOM(Vector3d(0.0, 0.0, dim1(2)/2.0)); node->addVisualizationShape(shape); node->addCollisionShape(shape); node->setMass(mass); node->setParentJoint(joint); robot->addBodyNode(node); // Create the second link, the joint with link1 and its shape BodyNode* parent_node = robot->getBodyNode("link1"); node = new BodyNode("link2"); joint = create1DOFJoint(0.0, -DART_PI, DART_PI, type2); joint->setName("joint2"); Eigen::Isometry3d T = Eigen::Isometry3d::Identity(); T.translate(Eigen::Vector3d(0.0, 0.0, dim1(2))); joint->setTransformFromParentBodyNode(T); shape = new BoxShape(dim2); node->setLocalCOM(Vector3d(0.0, 0.0, dim2(2)/2.0)); node->addVisualizationShape(shape); node->addCollisionShape(shape); node->setMass(mass); node->setParentJoint(joint); parent_node->addChildBodyNode(node); robot->addBodyNode(node); // If finished, initialize the skeleton if(finished) { addEndEffector(robot, node, dim2); robot->init(); } return robot; }
//============================================================================== TEST(InverseKinematics, FittingTransformation) { const double TOLERANCE = 1e-6; #ifdef BUILD_TYPE_RELEASE const size_t numRandomTests = 100; #else const size_t numRandomTests = 10; #endif // Create two link robot const double l1 = 1.5; const double l2 = 1.0; Skeleton* robot = createFreeFloatingTwoLinkRobot( Vector3d(0.3, 0.3, l1), Vector3d(0.3, 0.3, l2), DOF_ROLL); robot->init(); size_t dof = robot->getNumGenCoords(); VectorXd oldConfig = robot->getConfigs(); BodyNode* body1 = robot->getBodyNode(0); BodyNode* body2 = robot->getBodyNode(1); // Joint* joint1 = body1->getParentJoint(); Joint* joint2 = body2->getParentJoint(); //------------------------- Free joint test ---------------------------------- // The parent joint of body1 is free joint so body1 should be able to // transform to arbitrary tramsformation. for (size_t i = 0; i < numRandomTests; ++i) { // Get desiredT2 by transforming body1 to arbitrary transformation Isometry3d desiredT1 = math::expMap(Vector6d::Random()); body1->fitWorldTransform(desiredT1); // Check Isometry3d newT1 = body1->getWorldTransform(); EXPECT_NEAR(math::logMap(newT1.inverse() * desiredT1).norm(), 0.0, TOLERANCE); // Set to initial configuration robot->setConfigs(oldConfig, true, false, false); } //----------------------- Revolute joint test --------------------------------- // The parent joint of body2 is revolute joint so body2 can rotate along the // axis of the revolute joint. for (size_t i = 0; i < numRandomTests; ++i) { // Store the original transformation and joint angle Isometry3d oldT2 = body2->getWorldTransform(); VectorXd oldQ2 = joint2->getConfigs(); // Get desiredT2 by rotating the revolute joint with random angle joint2->setConfigs(VectorXd::Random(1), true, false, false); Isometry3d desiredT2 = body2->getWorldTransform(); // Transform body2 to the original transofrmation and check if it is done // well joint2->setConfigs(oldQ2, true, false, false); EXPECT_NEAR( math::logMap(oldT2.inverse() * body2->getWorldTransform()).norm(), 0.0, TOLERANCE); // Try to find optimal joint angle body2->fitWorldTransform(desiredT2); // Check Isometry3d newT2 = body2->getWorldTransform(); EXPECT_NEAR(math::logMap(newT2.inverse() * desiredT2).norm(), 0.0, TOLERANCE); } //---------------- Revolute joint test with joint limit ---------------------- for (size_t i = 0; i < numRandomTests; ++i) { // Set joint limit joint2->getGenCoord(0)->setConfigMin(DART_RADIAN * 0.0); joint2->getGenCoord(0)->setConfigMax(DART_RADIAN * 15.0); // Store the original transformation and joint angle Isometry3d oldT2 = body2->getWorldTransform(); VectorXd oldQ2 = joint2->getConfigs(); // Get desiredT2 by rotating the revolute joint with random angle out of // the joint limit range joint2->getGenCoord(0)->setConfig(math::random(DART_RADIAN * 15.5, DART_PI)); robot->setConfigs(robot->getConfigs(), true, false, false); Isometry3d desiredT2 = body2->getWorldTransform(); // Transform body2 to the original transofrmation and check if it is done // well joint2->setConfigs(oldQ2, true, false, false); EXPECT_NEAR( math::logMap(oldT2.inverse() * body2->getWorldTransform()).norm(), 0.0, TOLERANCE); // Try to find optimal joint angle without joint limit constraint body2->fitWorldTransform(desiredT2, BodyNode::IKP_PARENT_JOINT, false); // Check if the optimal body2 transformation is reached to the desired one Isometry3d newT2 = body2->getWorldTransform(); EXPECT_NEAR(math::logMap(newT2.inverse() * desiredT2).norm(), 0.0, TOLERANCE); // Try to find optimal joint angle with joint limit constraint body2->fitWorldTransform(desiredT2, BodyNode::IKP_PARENT_JOINT, true); // Check if the optimal joint anlge is in the range double newQ2 = joint2->getGenCoord(0)->getConfig(); EXPECT_GE(newQ2, DART_RADIAN * 0.0); EXPECT_LE(newQ2, DART_RADIAN * 15.0); } }
//============================================================================== void JOINTS::kinematicsTest(Joint* _joint) { assert(_joint); int numTests = 1; _joint->setTransformFromChildBodyNode( math::expMap(Eigen::Vector6d::Random())); _joint->setTransformFromParentBodyNode( math::expMap(Eigen::Vector6d::Random())); BodyNode* bodyNode = new BodyNode(); bodyNode->setParentJoint(_joint); Skeleton skeleton; skeleton.addBodyNode(bodyNode); skeleton.init(); int dof = _joint->getNumDofs(); //-------------------------------------------------------------------------- // //-------------------------------------------------------------------------- VectorXd q = VectorXd::Zero(dof); VectorXd dq = VectorXd::Zero(dof); for (int idxTest = 0; idxTest < numTests; ++idxTest) { double q_delta = 0.000001; for (int i = 0; i < dof; ++i) { q(i) = random(-DART_PI*1.0, DART_PI*1.0); dq(i) = random(-DART_PI*1.0, DART_PI*1.0); } skeleton.setPositions(q); skeleton.setVelocities(dq); skeleton.computeForwardKinematics(true, true, false); if (_joint->getNumDofs() == 0) return; Eigen::Isometry3d T = _joint->getLocalTransform(); Jacobian J = _joint->getLocalJacobian(); Jacobian dJ = _joint->getLocalJacobianTimeDeriv(); //-------------------------------------------------------------------------- // Test T //-------------------------------------------------------------------------- EXPECT_TRUE(math::verifyTransform(T)); //-------------------------------------------------------------------------- // Test analytic Jacobian and numerical Jacobian // J == numericalJ //-------------------------------------------------------------------------- Jacobian numericJ = Jacobian::Zero(6,dof); for (int i = 0; i < dof; ++i) { // a Eigen::VectorXd q_a = q; _joint->setPositions(q_a); skeleton.computeForwardKinematics(true, false, false); Eigen::Isometry3d T_a = _joint->getLocalTransform(); // b Eigen::VectorXd q_b = q; q_b(i) += q_delta; _joint->setPositions(q_b); skeleton.computeForwardKinematics(true, false, false); Eigen::Isometry3d T_b = _joint->getLocalTransform(); // Eigen::Isometry3d Tinv_a = T_a.inverse(); Eigen::Matrix4d Tinv_a_eigen = Tinv_a.matrix(); // dTdq Eigen::Matrix4d T_a_eigen = T_a.matrix(); Eigen::Matrix4d T_b_eigen = T_b.matrix(); Eigen::Matrix4d dTdq_eigen = (T_b_eigen - T_a_eigen) / q_delta; //Matrix4d dTdq_eigen = (T_b_eigen * T_a_eigen.inverse()) / dt; // J(i) Eigen::Matrix4d Ji_4x4matrix_eigen = Tinv_a_eigen * dTdq_eigen; Eigen::Vector6d Ji; Ji[0] = Ji_4x4matrix_eigen(2,1); Ji[1] = Ji_4x4matrix_eigen(0,2); Ji[2] = Ji_4x4matrix_eigen(1,0); Ji[3] = Ji_4x4matrix_eigen(0,3); Ji[4] = Ji_4x4matrix_eigen(1,3); Ji[5] = Ji_4x4matrix_eigen(2,3); numericJ.col(i) = Ji; } if (dynamic_cast<BallJoint*>(_joint) || dynamic_cast<FreeJoint*>(_joint)) { // Skip this test for BallJoint and FreeJoint since Jacobian of BallJoint // and FreeJoint is not obtained by the above method. } else { for (int i = 0; i < dof; ++i) for (int j = 0; j < 6; ++j) EXPECT_NEAR(J.col(i)(j), numericJ.col(i)(j), JOINT_TOL); } //-------------------------------------------------------------------------- // Test first time derivative of analytic Jacobian and numerical Jacobian // dJ == numerical_dJ //-------------------------------------------------------------------------- Jacobian numeric_dJ = Jacobian::Zero(6,dof); for (int i = 0; i < dof; ++i) { // a Eigen::VectorXd q_a = q; _joint->setPositions(q_a); skeleton.computeForwardKinematics(true, false, false); Jacobian J_a = _joint->getLocalJacobian(); // b Eigen::VectorXd q_b = q; q_b(i) += q_delta; _joint->setPositions(q_b); skeleton.computeForwardKinematics(true, false, false); Jacobian J_b = _joint->getLocalJacobian(); // Jacobian dJ_dq = (J_b - J_a) / q_delta; // J(i) numeric_dJ += dJ_dq * dq(i); } if (dynamic_cast<BallJoint*>(_joint) || dynamic_cast<FreeJoint*>(_joint)) { // Skip this test for BallJoint and FreeJoint since time derivative of // Jacobian of BallJoint and FreeJoint is not obtained by the above // method. } else { for (int i = 0; i < dof; ++i) for (int j = 0; j < 6; ++j) EXPECT_NEAR(dJ.col(i)(j), numeric_dJ.col(i)(j), JOINT_TOL); } } // Forward kinematics test with high joint position double posMin = -1e+64; double posMax = +1e+64; for (int idxTest = 0; idxTest < numTests; ++idxTest) { for (int i = 0; i < dof; ++i) q(i) = random(posMin, posMax); skeleton.setPositions(q); skeleton.computeForwardKinematics(true, false, false); if (_joint->getNumDofs() == 0) return; Eigen::Isometry3d T = _joint->getLocalTransform(); EXPECT_TRUE(math::verifyTransform(T)); } }
//============================================================================== TEST_F(JOINTS, CONVENIENCE_FUNCTIONS) { // -- set up the root BodyNode BodyNode* root = new BodyNode("root"); WeldJoint* rootjoint = new WeldJoint("base"); root->setParentJoint(rootjoint); // -- set up the FreeJoint BodyNode* freejoint_bn = new BodyNode("freejoint_bn"); FreeJoint* freejoint = new FreeJoint("freejoint"); freejoint_bn->setParentJoint(freejoint); root->addChildBodyNode(freejoint_bn); freejoint->setTransformFromParentBodyNode(random_transform()); freejoint->setTransformFromChildBodyNode(random_transform()); // -- set up the EulerJoint BodyNode* eulerjoint_bn = new BodyNode("eulerjoint_bn"); EulerJoint* eulerjoint = new EulerJoint("eulerjoint"); eulerjoint_bn->setParentJoint(eulerjoint); root->addChildBodyNode(eulerjoint_bn); eulerjoint->setTransformFromParentBodyNode(random_transform()); eulerjoint->setTransformFromChildBodyNode(random_transform()); // -- set up the BallJoint BodyNode* balljoint_bn = new BodyNode("balljoint_bn"); BallJoint* balljoint = new BallJoint("balljoint"); balljoint_bn->setParentJoint(balljoint); root->addChildBodyNode(balljoint_bn); balljoint->setTransformFromParentBodyNode(random_transform()); balljoint->setTransformFromChildBodyNode(random_transform()); // -- set up Skeleton and compute forward kinematics Skeleton* skel = new Skeleton; skel->addBodyNode(root); skel->addBodyNode(freejoint_bn); skel->addBodyNode(eulerjoint_bn); skel->addBodyNode(balljoint_bn); skel->init(); // Test a hundred times for(size_t n=0; n<100; ++n) { // -- convert transforms to positions and then positions back to transforms Eigen::Isometry3d desired_freejoint_tf = random_transform(); freejoint->setPositions(FreeJoint::convertToPositions(desired_freejoint_tf)); Eigen::Isometry3d actual_freejoint_tf = FreeJoint::convertToTransform( freejoint->getPositions()); Eigen::Isometry3d desired_eulerjoint_tf = random_transform(); desired_eulerjoint_tf.translation() = Eigen::Vector3d::Zero(); eulerjoint->setPositions( eulerjoint->convertToPositions(desired_eulerjoint_tf.linear())); Eigen::Isometry3d actual_eulerjoint_tf = eulerjoint->convertToTransform( eulerjoint->getPositions()); Eigen::Isometry3d desired_balljoint_tf = random_transform(); desired_balljoint_tf.translation() = Eigen::Vector3d::Zero(); balljoint->setPositions( BallJoint::convertToPositions(desired_balljoint_tf.linear())); Eigen::Isometry3d actual_balljoint_tf = BallJoint::convertToTransform( balljoint->getPositions()); skel->computeForwardKinematics(true, false, false); // -- collect everything so we can cycle through the tests std::vector<Joint*> joints; std::vector<BodyNode*> bns; std::vector<Eigen::Isometry3d> desired_tfs; std::vector<Eigen::Isometry3d> actual_tfs; joints.push_back(freejoint); bns.push_back(freejoint_bn); desired_tfs.push_back(desired_freejoint_tf); actual_tfs.push_back(actual_freejoint_tf); joints.push_back(eulerjoint); bns.push_back(eulerjoint_bn); desired_tfs.push_back(desired_eulerjoint_tf); actual_tfs.push_back(actual_eulerjoint_tf); joints.push_back(balljoint); bns.push_back(balljoint_bn); desired_tfs.push_back(desired_balljoint_tf); actual_tfs.push_back(actual_balljoint_tf); for(size_t i=0; i<joints.size(); ++i) { Joint* joint = joints[i]; BodyNode* bn = bns[i]; Eigen::Isometry3d tf = desired_tfs[i]; bool check_transform_conversion = equals(predict_joint_transform(joint, tf).matrix(), get_relative_transform(bn, bn->getParentBodyNode()).matrix()); EXPECT_TRUE(check_transform_conversion); if(!check_transform_conversion) { std::cout << "[" << joint->getName() << " Failed]\n"; std::cout << "Predicted:\n" << predict_joint_transform(joint, tf).matrix() << "\n\nActual:\n" << get_relative_transform(bn, bn->getParentBodyNode()).matrix() << "\n\n"; } bool check_full_cycle = equals(desired_tfs[i].matrix(), actual_tfs[i].matrix()); EXPECT_TRUE(check_full_cycle); if(!check_full_cycle) { std::cout << "[" << joint->getName() << " Failed]\n"; std::cout << "Desired:\n" << desired_tfs[i].matrix() << "\n\nActual:\n" << actual_tfs[i].matrix() << "\n\n"; } } } }