This tests checks if the relative encoders measurements are consistent over time, by performing cyclic movements between two reference positions (min and max). More...

#include <opticalEncodersDrift.h>

Inherits yarp::robottestingframework::TestCase.

Public Member Functions
virtual bool	setup (yarp::os::Property &property)

virtual void	tearDown ()

virtual void	run ()

bool	goHome ()

void	setMode (int desired_mode)

bool	saveToFile (const std::string &filename, const yarp::os::Bottle &b)

Detailed Description

This tests checks if the relative encoders measurements are consistent over time, by performing cyclic movements between two reference positions (min and max).

The test collects data during the joint motion, saves data to a text file a plots the result. If the relative encoder is working correctly, the plot should have no drift. Otherwise, a drift in the plot may be caused by a damaged reflective encoder/ optical disk. For best reliability an high number of cycles (e.g. >100) is suggested.

example: testRunner -v -t OpticalEncodersDrift.dll -p "--robot icub --part head --joints ""(0 1 2)"" --home ""(0 0 0)" –speed "(20 20 20)" –max "(10 10 10)" –min "(-10 -10 -10)" –cycles 100 –tolerance 1.0 " example: testRunner -v -t OpticalEncodersDrift.dll -p "–robot icub –part head –joints ""(2)"" –home ""(0)"" –speed "(20 )" –max "(10 )" –min "(-10)" –cycles 100 –tolerance 1.0 " Check the following functions: \li IMotorEncoder::getMotorEncoders() Accepts the following parameters: <table class="markdownTable"> <tr class="markdownTableHead"> <th class="markdownTableHeadCenter"> Parameter name

Type

Units

Default Value

Required

Description

Notes

robot

string

-

Yes

The name of the robot.

e.g. icub

part

string

-

Yes

The name of trhe robot part.

e.g. left_arm

joints

vector of ints

-

Yes

List of joints to be tested

home

vector of doubles of size joints

deg

-

Yes

The home position for each joint

cycles

int

-

Yes

The number of test cycles (going from max to min position and viceversa)

Use values > 100

max

vector of doubles of size joints

deg

-

Yes

The max position using during the joint movement

min

vector of doubles of size joints

deg

-

Yes

The min position using during the joint movement

tolerance

vector of doubles of size joints

deg

-

Yes

The tolerance used when moving from min to max reference position and viceversa

speed

vector of doubles of size joints

deg/s

-

Yes

The reference speed used during the movement

Definition at line 62 of file opticalEncodersDrift.h.

Constructor & Destructor Documentation

◆ OpticalEncodersDrift()

OpticalEncodersDrift::OpticalEncodersDrift ( )

Definition at line 49 of file opticalEncodersDrift.cpp.

                                           : yarp::robottestingframework::TestCase("OpticalEncodersDrift") {
    jointsList=0;
    dd=0;
    ipos=0;
    icmd=0;
    iimd=0;
    ienc=0;
    imot=0;
    enc_jnt=0;
    enc_mot=0;
    home_enc_mot=0;
    end_enc_mot=0;
    err_enc_mot=0;
    cycles=100;
}

◆ ~OpticalEncodersDrift()

OpticalEncodersDrift::~OpticalEncodersDrift ( )

virtual

Definition at line 65 of file opticalEncodersDrift.cpp.

65{ }

Member Function Documentation

◆ goHome()

bool OpticalEncodersDrift::goHome ( )

Definition at line 190 of file opticalEncodersDrift.cpp.

{
    for (unsigned int i=0; i<jointsList.size(); i++)
    {
        ipos->setRefSpeed((int)jointsList[i],speed[i]);
        ipos->positionMove((int)jointsList[i],home[i]);
    }
 
    int timeout = 0;
    while (1)
    {
        int in_position=0;
        for (unsigned int i=0; i<jointsList.size(); i++)
        {
            double tmp=0;
            ienc->getEncoder((int)jointsList[i],&tmp);
            if (fabs(tmp-home[i])<tolerance) in_position++;
        }
        if (in_position==jointsList.size()) break;
        if (timeout>100)
        {
            ROBOTTESTINGFRAMEWORK_TEST_REPORT("Timeout while reaching home position");
            return false;
        }
        yarp::os::Time::delay(0.2);
        timeout++;
    }
    return true;
}

◆ run()

void OpticalEncodersDrift::run ( )

virtual

Definition at line 243 of file opticalEncodersDrift.cpp.

{
    setMode(VOCAB_CM_POSITION);
    ROBOTTESTINGFRAMEWORK_ASSERT_FAIL_IF_FALSE(goHome(), "Test can't run");
 
    bool go_to_max=false;
    for (unsigned int i=0; i<jointsList.size(); i++)
    {
        ipos->positionMove((int)jointsList[i], min[i]);
    }
 
    int  curr_cycle=0;
    double start_time = yarp::os::Time::now();
    Bottle dataToPlot;
 
    imot->getMotorEncoders             (home_enc_mot.data());
    while(1)
    {
        double curr_time = yarp::os::Time::now();
        double elapsed = curr_time-start_time;
 
        //get joint e motor encoders data for the whole robot part
        ienc->getEncoders                  (enc_jnt.data());
        imot->getMotorEncoders             (enc_mot.data());
        //extract only the joints of interest
        yarp::sig::Vector enc_jnt_of_interest (jointsList.size());
        yarp::sig::Vector enc_mot_of_interest (jointsList.size());
        for (size_t i =0; i< jointsList.size(); i++)
        {
            enc_jnt_of_interest[i] = enc_jnt[jointsList[i]];
            enc_mot_of_interest[i] = enc_mot[jointsList[i]];
        }
        //put the data into a Bottle
        //this is the output format: n values for the motor encoders, then n values for the jnt encoders
        Bottle& row = dataToPlot.addList();
        Bottle& b_mot = row.addList();
        Bottle& b_jnt = row.addList();
        b_mot.read(enc_mot_of_interest);
        b_jnt.read(enc_jnt_of_interest);
       
        bool reached= false;
        int in_position=0;
        for (unsigned int i=0; i<jointsList.size(); i++)
        {
            double curr_val=0;
            if (go_to_max==false) curr_val = min[i];
            else                  curr_val = max[i];
            if (fabs(enc_jnt[i]-curr_val)<tolerance) in_position++;
        }
        if (in_position==jointsList.size()) reached=true;
 
        if (elapsed >= 20.0)
        {
            ROBOTTESTINGFRAMEWORK_ASSERT_ERROR("Timeout while moving joint");
        }
 
        if (reached)
        {
            if (go_to_max==false)
            {
                for (unsigned int i=0; i<jointsList.size(); i++)
                    ipos->positionMove(i,max[i]);
                go_to_max=true;
                curr_cycle++;
                start_time = yarp::os::Time::now();
                if (curr_cycle % 10 == 0) ROBOTTESTINGFRAMEWORK_TEST_REPORT(Asserter::format("Cycle %d/%d completed", curr_cycle, cycles));
            }
            else
            {
                for (unsigned int i=0; i<jointsList.size(); i++)
                    ipos->positionMove(i,min[i]);
                go_to_max=false;
                curr_cycle++;
                start_time = yarp::os::Time::now();
                if (curr_cycle % 10 == 0) ROBOTTESTINGFRAMEWORK_TEST_REPORT(Asserter::format("Cycle %d/%d completed", curr_cycle, cycles));
            }
        }
 
        if (curr_cycle>=cycles) break;
 
        yarp::os::Time::delay(0.010);
    }
 
    bool isInHome = goHome();
    yarp::os::Time::delay(2.0);
 
    //automatic check, not complete yet
    {
        imot->getMotorEncoders             (end_enc_mot.data());
        for (int i=0; i<n_part_joints; i++)
        {
            err_enc_mot[i]=home_enc_mot[i]-end_enc_mot[i];
 
            if (fabs(err_enc_mot[i]) > tolerance)
            {
                //...assert something
            }
        }
    }
 
    time_t now = time(0);
    tm *ltm = localtime(&now);
 
    char folder_time_buffer[80];
    char file_time_buffer[80];
 
    strftime(folder_time_buffer, sizeof(folder_time_buffer), "%d%m%Y", ltm);
    strftime(file_time_buffer, sizeof(file_time_buffer), "%d%m%Y_%H%M", ltm);
 
    string folder_time_str(folder_time_buffer);
    string file_time_str(file_time_buffer); //This string contain also minutes
 
    // Create the filename with date and time
    string filename = "encDrift_plot_";
    filename += partName;
    filename += "_";
    filename += file_time_str;
    filename += ".txt";
    
    constexpr char default_robot_name[] = "RobotName";
 
    string robot_str = yarp::conf::environment::get_string("YARP_ROBOT_NAME", default_robot_name);
    string directory_tree = "results/" + robot_str + "/encoders-icub_" + folder_time_str + "/encDrift";
    create_directories(directory_tree); // This function return false if there is an error or if the directories already exist
    
    string filename_with_path = directory_tree + "/" + filename;
    bool saved_files = saveToFile(filename_with_path,dataToPlot);
    
    if(!saved_files)
    {
        ROBOTTESTINGFRAMEWORK_TEST_REPORT("Error saving files to plot!");
    }
 
    char plotstring[1000];
    sprintf (plotstring, "gnuplot -e \" unset key; plot for [col=1:%d] '%s' using col with lines \" -persist", (int)jointsList.size(),filename_with_path.c_str());
    
    if(plot)
    {
        system (plotstring);
    }
    else
    {
        ROBOTTESTINGFRAMEWORK_TEST_REPORT("Test is finished. Please check if collected date are ok, by using following command: ");
        ROBOTTESTINGFRAMEWORK_TEST_REPORT(robottestingframework::Asserter::format("%s", plotstring));
    }
 
    ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(isInHome, "This part is not in home. Suite test will be terminated!");
    ROBOTTESTINGFRAMEWORK_TEST_REPORT("Test is finished. Your files are saved in: \n" + filename_with_path);
 
}

◆ saveToFile()

bool OpticalEncodersDrift::saveToFile	(	const std::string &	filename,
		const yarp::os::Bottle &	b
	)

Definition at line 220 of file opticalEncodersDrift.cpp.

{
    std::fstream fs;
    fs.open (filename.c_str(), std::fstream::out);
 
    if ( (fs.rdstate() & std::ifstream::failbit ) != 0 )
    {
        std::cerr << "Error opening " << filename << "\n";
        return false;
    }
 
    for (unsigned int i=0; i<b.size(); i++)
    {
        std::string s = b.get(i).toString();
        std::replace(s.begin(), s.end(), '(', ' ');
        std::replace(s.begin(), s.end(), ')', ' ');
        fs << s << std::endl;
    }
 
    fs.close();
    return true;
}

◆ setMode()

void OpticalEncodersDrift::setMode ( int desired_mode )

Definition at line 158 of file opticalEncodersDrift.cpp.

{
    for (unsigned int i=0; i<jointsList.size(); i++)
    {
        icmd->setControlMode((int)jointsList[i],desired_mode);
        iimd->setInteractionMode((int)jointsList[i],VOCAB_IM_STIFF);
        yarp::os::Time::delay(0.010);
    }
 
    int cmode;
    yarp::dev::InteractionModeEnum imode;
    int timeout = 0;
 
    while (1)
    {
        int ok=0;
        for (unsigned int i=0; i<jointsList.size(); i++)
        {
            icmd->getControlMode ((int)jointsList[i],&cmode);
            iimd->getInteractionMode((int)jointsList[i],&imode);
            if (cmode==desired_mode && imode==VOCAB_IM_STIFF) ok++;
        }
        if (ok==jointsList.size()) break;
        if (timeout>100)
        {
            ROBOTTESTINGFRAMEWORK_ASSERT_ERROR("Unable to set control mode/interaction mode");
        }
        yarp::os::Time::delay(0.2);
        timeout++;
    }
}

◆ setup()

bool OpticalEncodersDrift::setup ( yarp::os::Property & property )

virtual

Definition at line 67 of file opticalEncodersDrift.cpp.

                                                         {
 
    if(property.check("name"))
        setName(property.find("name").asString());
 
    // updating parameters
    ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("robot"),     "The robot name must be given as the test parameter!");
    ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("part"),      "The part name must be given as the test parameter!");
    ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("joints"),    "The joints list must be given as the test parameter!");
    ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("home"),      "The home position must be given as the test parameter!");
    ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("max"),       "The max position must be given as the test parameter!");
    ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("min"),       "The min position must be given as the test parameter!");
    ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("speed"),     "The positionMove reference speed must be given as the test parameter!");
    ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("cycles"),    "The number of cycles of the control signal must be given as the test parameter!");
    ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("tolerance"), "The max error tolerance must be given as the test parameter!");
 
    robotName = property.find("robot").asString();
    
    partName = property.find("part").asString();
 
    Bottle* jointsBottle = property.find("joints").asList();
    ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(jointsBottle!=0,"unable to parse joints parameter");
 
    Bottle* homeBottle = property.find("home").asList();
    ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(homeBottle!=0,"unable to parse zero parameter");
 
    Bottle* maxBottle = property.find("max").asList();
    ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(maxBottle!=0,"unable to parse max parameter");
 
    Bottle* minBottle = property.find("min").asList();
    ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(minBottle!=0,"unable to parse min parameter");
 
    Bottle* speedBottle = property.find("speed").asList();
    ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(speedBottle!=0,"unable to parse speed parameter");
 
    tolerance = property.find("tolerance").asFloat64();
    ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(tolerance>=0,"invalid tolerance");
 
    cycles = property.find("cycles").asInt32();
    ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(cycles>=0,"invalid cycles");
 
    plot = property.find("plot_enabled").asBool();
 
    if(plot)
        ROBOTTESTINGFRAMEWORK_TEST_REPORT("This test will run gnuplot utility at the end.");
    else
        ROBOTTESTINGFRAMEWORK_TEST_REPORT("This test will NOT run gnuplot utility at the end.");
 
 
 
    Property options;
    options.put("device", "remote_controlboard");
    options.put("remote", "/"+robotName+"/"+partName);
    options.put("local", "/opticalEncodersDrift/"+robotName+"/"+partName);
 
    dd = new PolyDriver(options);
    ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->isValid(),"Unable to open device driver");
    ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(ienc),"Unable to open encoders interface");
    ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(ipos),"Unable to open position interface");
    ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(icmd),"Unable to open control mode interface");
    ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(iimd),"Unable to open interaction mode interface");
    ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(imot),"Unable to open motor encoders interface");
 
    if (!ienc->getAxes(&n_part_joints))
    {
        ROBOTTESTINGFRAMEWORK_ASSERT_ERROR("unable to get the number of joints of the part");
    }
 
    int n_cmd_joints = jointsBottle->size();
    ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(n_cmd_joints>0 && n_cmd_joints<=n_part_joints,"invalid number of joints, it must be >0 & <= number of part joints");
    for (int i=0; i <n_cmd_joints; i++) jointsList.push_back(jointsBottle->get(i).asInt32());
 
    enc_jnt.resize(n_part_joints);
    enc_mot.resize(n_part_joints);
    home_enc_mot.resize(n_part_joints);
    end_enc_mot.resize(n_part_joints);
    err_enc_mot.resize(n_part_joints);
 
    max.resize  (n_cmd_joints); for (int i=0; i< n_cmd_joints; i++) max[i]=maxBottle->get(i).asFloat64();
    min.resize  (n_cmd_joints); for (int i=0; i< n_cmd_joints; i++) min[i]=minBottle->get(i).asFloat64();
    home.resize (n_cmd_joints); for (int i=0; i< n_cmd_joints; i++) home[i]=homeBottle->get(i).asFloat64();
    speed.resize(n_cmd_joints); for (int i=0; i< n_cmd_joints; i++) speed[i]=speedBottle->get(i).asFloat64();
 
    return true;
}

◆ tearDown()

void OpticalEncodersDrift::tearDown ( )

virtual

Definition at line 153 of file opticalEncodersDrift.cpp.

{
    if (dd) {delete dd; dd =0;}
}

The documentation for this class was generated from the following files:

/home/runner/work/icub-tests/icub-tests/gh-pages/src/opticalEncoders-drift/opticalEncodersDrift.h
/home/runner/work/icub-tests/icub-tests/gh-pages/src/opticalEncoders-drift/opticalEncodersDrift.cpp

Public Member Functions

Detailed Description

Constructor & Destructor Documentation

◆ OpticalEncodersDrift()

◆ ~OpticalEncodersDrift()

Member Function Documentation

◆ goHome()

◆ run()

◆ saveToFile()

◆ setMode()

◆ setup()

◆ tearDown()