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