<commit_before>/*
 * Copyright (C) 2015 Fondazione Istituto Italiano di Tecnologia
 * Authors: <NAME>
 * CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
 *
 */

#include "testModels.h"

#include <iDynTree/Core/TestUtils.h>

#include <iDynTree/Model/Model.h>
#include <iDynTree/ModelIO/URDFModelImport.h>
#include <iDynTree/ModelIO/URDFDofsImport.h>
#include <iDynTree/ModelIO/ModelLoader.h>

#include <cassert>
#include <cstdio>
#include <cstdlib>

using namespace iDynTree;


void checkParsingOfDofsFromURDF(std::string fileName,
                                size_t expectedNrOfDOFs)
{
    std::vector<std::string> dofsNameList;
    bool ok = dofsListFromURDF(fileName,dofsNameList);

    ASSERT_EQUAL_DOUBLE(dofsNameList.size(),expectedNrOfDOFs);
}

void checkURDF(std::string fileName,
                  unsigned int expectedNrOfLinks,
                  unsigned int expectedNrOfJoints,
                  unsigned int expectedNrOfDOFs,
                  unsigned int expectedNrOfFrames,
                  std::string expectedDefaultBase)
{
    Model model;
    bool ok = modelFromURDF(fileName,model);
    assert(ok);

    std::cerr << "Model loaded from " << fileName << std::endl;
    std::cerr << model.toString() << std::endl;

    ASSERT_EQUAL_DOUBLE(model.getNrOfLinks(),expectedNrOfLinks);
    ASSERT_EQUAL_DOUBLE(model.getNrOfJoints(),expectedNrOfJoints);
    ASSERT_EQUAL_DOUBLE(model.getNrOfDOFs(),expectedNrOfDOFs);
    ASSERT_EQUAL_DOUBLE(model.getNrOfFrames(),expectedNrOfFrames);
    ASSERT_EQUAL_STRING(model.getLinkName(model.getDefaultBaseLink()),expectedDefaultBase);

    checkParsingOfDofsFromURDF(fileName,expectedNrOfDOFs);

    // Check that the copy constructor works fine
    Model modelCopyConstruced = model;

    std::cerr << "Model copy constructed from " << fileName << std::endl;
    std::cerr << modelCopyConstruced.toString() << std::endl;

    ASSERT_EQUAL_DOUBLE(modelCopyConstruced.getNrOfLinks(),expectedNrOfLinks);
    ASSERT_EQUAL_DOUBLE(modelCopyConstruced.getNrOfJoints(),expectedNrOfJoints);
    ASSERT_EQUAL_DOUBLE(modelCopyConstruced.getNrOfDOFs(),expectedNrOfDOFs);
    ASSERT_EQUAL_DOUBLE(modelCopyConstruced.getNrOfFrames(),expectedNrOfFrames);
    ASSERT_EQUAL_STRING(modelCopyConstruced.getLinkName(modelCopyConstruced.getDefaultBaseLink()),expectedDefaultBase);

    // Check that the copy assignent works fine
    Model modelCopyAssigned;

    modelCopyAssigned = model;

    std::cerr << "Model copy assigned from " << fileName << std::endl;
    std::cerr << modelCopyAssigned.toString() << std::endl;


    ASSERT_EQUAL_DOUBLE(modelCopyAssigned.getNrOfLinks(),expectedNrOfLinks);
    ASSERT_EQUAL_DOUBLE(modelCopyAssigned.getNrOfJoints(),expectedNrOfJoints);
    ASSERT_EQUAL_DOUBLE(modelCopyAssigned.getNrOfDOFs(),expectedNrOfDOFs);
    ASSERT_EQUAL_DOUBLE(modelCopyAssigned.getNrOfFrames(),expectedNrOfFrames);
    ASSERT_EQUAL_STRING(modelCopyAssigned.getLinkName(modelCopyAssigned.getDefaultBaseLink()),expectedDefaultBase);
}

void checkModelLoderForURDFFile(std::string urdfFile)
{
    ModelLoader loader;
    bool result = loader.loadModelFromFile(urdfFile);
    ASSERT_IS_TRUE(result && loader.isValid());
}

void checkModelLoaderFromURDFString(std::string urdfString, bool shouldBeCorrect = true)
{
    ModelLoader loader;
    bool result = loader.loadModelFromString(urdfString);
    if (shouldBeCorrect) {
        ASSERT_IS_TRUE(result && loader.isValid());
    } else {
        ASSERT_IS_TRUE(!result && !loader.isValid());
    }

}

void checkLimitsForJointsAreDefined(Model & model)
{

    for(JointIndex jnt=0; jnt < model.getNrOfJoints(); jnt++)
    {
        IJointPtr jntPtr = model.getJoint(jnt);
        
        ASSERT_IS_TRUE(jntPtr != 0);
        
        if( jntPtr->getNrOfDOFs() == 1 )
        {
            ASSERT_IS_TRUE(jntPtr->hasPosLimits());
            
            double max = jntPtr->getMaxPosLimit(0);
            double min = jntPtr->getMinPosLimit(0);
            
            ASSERT_IS_TRUE(min <= max);
            ASSERT_IS_TRUE(min > -10e7);
            ASSERT_IS_TRUE(max < 10e7);
        }
    }
}

void checkLimitsForJointsAreDefinedFromFileName(std::string urdfFileName)
{
    Model model;
    bool ok = modelFromURDF(urdfFileName,model);
    ASSERT_IS_TRUE(ok);
    
    checkLimitsForJointsAreDefined(model);
    
    Model copyConstructedModel = model;
    
    checkLimitsForJointsAreDefined(copyConstructedModel);
    
    Model assignedModel;
    assignedModel = model;
    
    checkLimitsForJointsAreDefined(assignedModel);
    
    // Check the reduced model loader 
    std::vector<std::string> dofsOfModel;
    ok = dofsListFromURDF(urdfFileName,dofsOfModel);
    ASSERT_IS_TRUE(ok);

    ModelLoader loader;
    ok = loader.loadReducedModelFromFile(urdfFileName,dofsOfModel);
    
    ASSERT_IS_TRUE(ok);
    
    Model reducedModel = loader.model();
    
    checkLimitsForJointsAreDefined(reducedModel);
}

int main()
{
    checkURDF(getAbsModelPath("/oneLink.urdf"),1,0,0,7,"link1");
    checkURDF(getAbsModelPath("twoLinks.urdf"),2,1,1,6,"link1");
    checkURDF(getAbsModelPath("icub_skin_frames.urdf"),39,38,32,62,"root_link");
    //checkURDF(getAbsModelPath("iCubGenova02.urdf"),33,32,26,111,"root_link");

    checkModelLoderForURDFFile(getAbsModelPath("/oneLink.urdf"));
    checkModelLoaderFromURDFString("this is not an xml", false);
    
    checkLimitsForJointsAreDefinedFromFileName(getAbsModelPath("iCubGenova02.urdf"));

    return EXIT_SUCCESS;
}<commit_msg>Removed commented test<commit_after>/*
 * Copyright (C) 2015 <NAME> Italiano di Tecnologia
 * Authors: <NAME>
 * CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
 *
 */

#include "testModels.h"

#include <iDynTree/Core/TestUtils.h>

#include <iDynTree/Model/Model.h>
#include <iDynTree/ModelIO/URDFModelImport.h>
#include <iDynTree/ModelIO/URDFDofsImport.h>
#include <iDynTree/ModelIO/ModelLoader.h>

#include <cassert>
#include <cstdio>
#include <cstdlib>

using namespace iDynTree;


void checkParsingOfDofsFromURDF(std::string fileName,
                                size_t expectedNrOfDOFs)
{
    std::vector<std::string> dofsNameList;
    bool ok = dofsListFromURDF(fileName,dofsNameList);

    ASSERT_EQUAL_DOUBLE(dofsNameList.size(),expectedNrOfDOFs);
}

void checkURDF(std::string fileName,
                  unsigned int expectedNrOfLinks,
                  unsigned int expectedNrOfJoints,
                  unsigned int expectedNrOfDOFs,
                  unsigned int expectedNrOfFrames,
                  std::string expectedDefaultBase)
{
    Model model;
    bool ok = modelFromURDF(fileName,model);
    assert(ok);

    std::cerr << "Model loaded from " << fileName << std::endl;
    std::cerr << model.toString() << std::endl;

    ASSERT_EQUAL_DOUBLE(model.getNrOfLinks(),expectedNrOfLinks);
    ASSERT_EQUAL_DOUBLE(model.getNrOfJoints(),expectedNrOfJoints);
    ASSERT_EQUAL_DOUBLE(model.getNrOfDOFs(),expectedNrOfDOFs);
    ASSERT_EQUAL_DOUBLE(model.getNrOfFrames(),expectedNrOfFrames);
    ASSERT_EQUAL_STRING(model.getLinkName(model.getDefaultBaseLink()),expectedDefaultBase);

    checkParsingOfDofsFromURDF(fileName,expectedNrOfDOFs);

    // Check that the copy constructor works fine
    Model modelCopyConstruced = model;

    std::cerr << "Model copy constructed from " << fileName << std::endl;
    std::cerr << modelCopyConstruced.toString() << std::endl;

    ASSERT_EQUAL_DOUBLE(modelCopyConstruced.getNrOfLinks(),expectedNrOfLinks);
    ASSERT_EQUAL_DOUBLE(modelCopyConstruced.getNrOfJoints(),expectedNrOfJoints);
    ASSERT_EQUAL_DOUBLE(modelCopyConstruced.getNrOfDOFs(),expectedNrOfDOFs);
    ASSERT_EQUAL_DOUBLE(modelCopyConstruced.getNrOfFrames(),expectedNrOfFrames);
    ASSERT_EQUAL_STRING(modelCopyConstruced.getLinkName(modelCopyConstruced.getDefaultBaseLink()),expectedDefaultBase);

    // Check that the copy assignent works fine
    Model modelCopyAssigned;

    modelCopyAssigned = model;

    std::cerr << "Model copy assigned from " << fileName << std::endl;
    std::cerr << modelCopyAssigned.toString() << std::endl;


    ASSERT_EQUAL_DOUBLE(modelCopyAssigned.getNrOfLinks(),expectedNrOfLinks);
    ASSERT_EQUAL_DOUBLE(modelCopyAssigned.getNrOfJoints(),expectedNrOfJoints);
    ASSERT_EQUAL_DOUBLE(modelCopyAssigned.getNrOfDOFs(),expectedNrOfDOFs);
    ASSERT_EQUAL_DOUBLE(modelCopyAssigned.getNrOfFrames(),expectedNrOfFrames);
    ASSERT_EQUAL_STRING(modelCopyAssigned.getLinkName(modelCopyAssigned.getDefaultBaseLink()),expectedDefaultBase);
}

void checkModelLoderForURDFFile(std::string urdfFile)
{
    ModelLoader loader;
    bool result = loader.loadModelFromFile(urdfFile);
    ASSERT_IS_TRUE(result && loader.isValid());
}

void checkModelLoaderFromURDFString(std::string urdfString, bool shouldBeCorrect = true)
{
    ModelLoader loader;
    bool result = loader.loadModelFromString(urdfString);
    if (shouldBeCorrect) {
        ASSERT_IS_TRUE(result && loader.isValid());
    } else {
        ASSERT_IS_TRUE(!result && !loader.isValid());
    }

}

void checkLimitsForJointsAreDefined(Model & model)
{

    for(JointIndex jnt=0; jnt < model.getNrOfJoints(); jnt++)
    {
        IJointPtr jntPtr = model.getJoint(jnt);
        
        ASSERT_IS_TRUE(jntPtr != 0);
        
        if( jntPtr->getNrOfDOFs() == 1 )
        {
            ASSERT_IS_TRUE(jntPtr->hasPosLimits());
            
            double max = jntPtr->getMaxPosLimit(0);
            double min = jntPtr->getMinPosLimit(0);
            
            ASSERT_IS_TRUE(min <= max);
            ASSERT_IS_TRUE(min > -10e7);
            ASSERT_IS_TRUE(max < 10e7);
        }
    }
}

void checkLimitsForJointsAreDefinedFromFileName(std::string urdfFileName)
{
    Model model;
    bool ok = modelFromURDF(urdfFileName,model);
    ASSERT_IS_TRUE(ok);
    
    checkLimitsForJointsAreDefined(model);
    
    Model copyConstructedModel = model;
    
    checkLimitsForJointsAreDefined(copyConstructedModel);
    
    Model assignedModel;
    assignedModel = model;
    
    checkLimitsForJointsAreDefined(assignedModel);
    
    // Check the reduced model loader 
    std::vector<std::string> dofsOfModel;
    ok = dofsListFromURDF(urdfFileName,dofsOfModel);
    ASSERT_IS_TRUE(ok);

    ModelLoader loader;
    ok = loader.loadReducedModelFromFile(urdfFileName,dofsOfModel);
    
    ASSERT_IS_TRUE(ok);
    
    Model reducedModel = loader.model();
    
    checkLimitsForJointsAreDefined(reducedModel);
}

int main()
{
    checkURDF(getAbsModelPath("/oneLink.urdf"),1,0,0,7,"link1");
    checkURDF(getAbsModelPath("twoLinks.urdf"),2,1,1,6,"link1");
    checkURDF(getAbsModelPath("icub_skin_frames.urdf"),39,38,32,62,"root_link");
    checkURDF(getAbsModelPath("iCubGenova02.urdf"),33,32,26,111,"root_link");

    checkModelLoderForURDFFile(getAbsModelPath("/oneLink.urdf"));
    checkModelLoaderFromURDFString("this is not an xml", false);
    
    checkLimitsForJointsAreDefinedFromFileName(getAbsModelPath("iCubGenova02.urdf"));

    return EXIT_SUCCESS;
}
{'fraction_non_alphanumeric': 0.10837259876766944, 'fraction_numerical': 0.008698803914461761, 'mean_word_length': 4.201225259189444, 'pattern_counts': {'":': 0, '<': 92, '<?xml version=': 0, '>': 36, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 2}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '6340259', 'n_tokens_mistral': 3752, 'n_tokens_neox': 3656, 'n_words': 511}