int main(int argc, char** argv) { // If we specify an argument we run the test on a specific model bool test_success = true; if( argc > 2 ) { std::string urdf_file_name = argv[1]; test_success = checkURDF2DH(urdf_file_name); } else { // Otherwise run the test on all the models for(unsigned int mdl = 0; mdl < IDYNTREE_TESTS_URDFS_NR; mdl++ ) { std::string urdfFileName = getAbsModelPath(std::string(IDYNTREE_TESTS_URDFS[mdl])); bool ok = checkURDF2DH(urdfFileName); test_success = test_success && ok; } } if( test_success ) { return EXIT_SUCCESS; } else { return EXIT_FAILURE; } }
void threeLinksReducedTest() { // Check visualizer of simple model iDynTree::ModelLoader mdlLoader, mdlLoaderReduced; // Load full model bool ok = mdlLoader.loadModelFromFile(getAbsModelPath("threeLinks.urdf")); ASSERT_IS_TRUE(ok); // Check visualization of full model checkVizLoading(mdlLoader.model()); // Load reduced model std::vector<std::string> consideredJoints; consideredJoints.push_back("joint_2_3"); ok = mdlLoaderReduced.loadReducedModelFromFullModel(mdlLoader.model(),consideredJoints); ASSERT_IS_TRUE(ok); // Check vizualization for reduced model checkVizLoading(mdlLoaderReduced.model()); }
int main() { std::string urdf_to_test = getAbsModelPath("twoLinks.urdf"); srand(time(NULL)); //Creating an DynTree iCub::iDynTree::DynTree urdf_dyntree; bool ok = urdf_dyntree.loadURDFModel(urdf_to_test); //Creating a DynamicsComputations class iDynTree::HighLevel::DynamicsComputations urdf_dyncomp; ok = ok && urdf_dyncomp.loadRobotModelFromFile(urdf_to_test); if( !ok ) { std::cerr << "Impossible to load URDF " << urdf_to_test << std::endl; return EXIT_FAILURE; } // Create random joint configuration unsigned int dofs = urdf_dyncomp.getNrOfDegreesOfFreedom(); iDynTree::VectorDynSize q_idyntree(dofs),zero_idyntree(dofs); iDynTree::SpatialAcc gravity; yarp::sig::Vector q_yarp(dofs); for(unsigned int i=0; i < dofs; i++ ) { q_yarp(i) = q_idyntree(i) = random_double(); } urdf_dyntree.setAng(q_yarp); urdf_dyncomp.setRobotState(q_idyntree,zero_idyntree,zero_idyntree,gravity); //Check consistenct of all the transformations unsigned int nrOfFrames = urdf_dyncomp.getNrOfFrames(); for(unsigned refFrame = 0; refFrame < nrOfFrames; refFrame++ ) { for(unsigned frame = 0; frame < nrOfFrames; frame++ ) { KDL::Frame H_kdl = urdf_dyntree.getPositionKDL(refFrame,(int)frame); iDynTree::Transform H_transform = urdf_dyncomp.getRelativeTransform(refFrame,frame); if( !checkFrameConsistency(H_kdl,H_transform) ) { std::cerr << "Error : " << refFrame << "_T_" << frame << " is inconsistent " << std::endl; std::cerr << "DynamicsComputations : " << H_transform.toString() << std::endl; std::cerr << "DynTree " << H_kdl << std::endl; return EXIT_FAILURE; } } } // The same, but with names nrOfFrames = urdf_dyncomp.getNrOfFrames(); for(unsigned refFrame = 0; refFrame < nrOfFrames; refFrame++ ) { for(unsigned frame = 0; frame < nrOfFrames; frame++ ) { std::string refFrameName, frameName; urdf_dyntree.getFrameName(refFrame,refFrameName); urdf_dyntree.getFrameName(frame,frameName); KDL::Frame H_kdl = urdf_dyntree.getPositionKDL(refFrame,(int)frame); iDynTree::Transform H_transform = urdf_dyncomp.getRelativeTransform(refFrameName,frameName); if( !checkFrameConsistency(H_kdl,H_transform) ) { std::cerr << "Error : " << refFrameName << "_T_" << frameName << " is inconsistent " << std::endl; std::cerr << "DynamicsComputations : " << H_transform.toString() << std::endl; std::cerr << "DynTree " << H_kdl << std::endl; return EXIT_FAILURE; } if( refFrame != frame ) { std::cerr << "Check: " << refFrameName << "_T_" << frameName << " is inconsistent " << std::endl; std::cerr << "DynamicsComputations : " << H_transform.toString() << std::endl; std::cerr << "DynTree " << H_kdl << std::endl; std::cerr << "Rotation " << std::endl; std::cerr << "DynamicsComputations rot: " << H_transform.getRotation().toString() << std::endl; std::cerr << "DynTree rot " << H_kdl.M << std::endl; for(int row = 0; row <3; row++ ) { for(int col = 0; col < 3; col++ ) { } } } } } return EXIT_SUCCESS; }