//============================================================================== 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"; } } } }