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