JointLimits Class Reference

Check if the software joint limits are properly set. More...

#include <jointLimits.h>

Inherits yarp::robottestingframework::TestCase.

Public Member Functions
virtual bool	setup (yarp::os::Property &property)
virtual void	tearDown ()
virtual void	run ()
void	goTo (yarp::sig::Vector position)
bool	goToSingle (int i, double pos, double *reached_pos)
bool	goToSingleExceed (int i, double position_to_reach, double limit, double reachedLimit, double *reached_pos)
void	setMode (int desired_mode)
void	saveToFile (std::string filename, yarp::os::Bottle &b)

Detailed Description

Check if the software joint limits are properly set.

The limits are set in the robot configuration files. The test asks the limits to robotInterface using the IControlLimits interface. The test moves each joint first to the max lim, then to the min lim, then to home. If the joint is unable to reach the software limits within a certain tolerance, the test fails and the joint is put back in home position. The timeout for each joint to reach the limit position is fixed to 20 seconds. The test uses an limited output to avoid to damage the joint if, for example, an hardware limit is reached before the software limit. If this limit is too small, the the joint may be unable to reach the limit (e.g. because of friction), so the value must be chosen accurately. The test assumes the the position control is properly working and the position pid is properly tuned. After testing the limits, this test also tries to move the joint out of the limits on puropose (adding to the joint limits the value of outOfBoundPosition). The test is successfull if the position move command is correctly stopped at the limit.

Example: testRunner -v -t JointLimits.dll -p "--robot icub --part head --joints ""(0 1 2)"" --home ""(0 0 0)"" --speed ""(20 20 20)"" --outputLimitPercent ""(30 30 30)"" --outOfBoundPosition ""(2 2 2)"" --tolerance 0.2"

Check the following functions:

• IControlLimits::getLimits()

Support functions:

• IPositionControl::getAxes()
• IPositionControl::positionMove()
• IPositionContol::setRefSpeeds()
• IEncoders::getEncoder()
• IEncoders::getEncoders()
• IPid::getPid()/IPid::setPid()
• IControlMode::getControlMode()/setControlMode()
• IInteractionMode::getInteractionMode()/setInteractionMode()

Accepts the following parameters:

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
speed	vector of doubles of size joints	deg/s	-	Yes	The reference speed used furing the movement
tolerance	vector of doubles of size joints	deg	-	Yes	The position tolerance used to check if the limit has been properly reached.	Typical value = 0.2 deg.
outputLimitPercent	vector of doubles of size joints	%	-	Yes	The maximum motor output (expressed as percentage).	Safe values can be, for example, 30%.
outOfBoundPosition	vector of doubles of size joints	%	-	Yes	This value is added the joint limit to test that a position command is not able to move out of the joint limits	Typical value 2 deg.

Definition at line 74 of file jointLimits.h.

Constructor & Destructor Documentation

JointLimits()

JointLimits::JointLimits

(

)

Definition at line 42 of file jointLimits.cpp.

                         : yarp::robottestingframework::TestCase("JointLimits") {
    jointsList=0;
    dd=0;
    ipos=0;
    icmd=0;
    iimd=0;
    ienc=0;
    ilim=0;
    enc_jnt=0;
    original_pids=0;
    pids_saved=false;
}

~JointLimits()

JointLimits::~JointLimits ( )

virtual

Definition at line 55 of file jointLimits.cpp.

{ }

Member Function Documentation

goTo()

void JointLimits::goTo

(

yarp::sig::Vector

position

)

Definition at line 220 of file jointLimits.cpp.

{
    for (unsigned int i=0; i<jointsList.size(); i++)
    {
        ipos->setRefSpeed((int)jointsList[i],speed[i]);
        ipos->positionMove((int)jointsList[i],position[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-position[i])<toleranceList[i]) in_position++;
        }
        if (in_position==jointsList.size()) break;
        if (timeout>100)
        {
            ROBOTTESTINGFRAMEWORK_ASSERT_ERROR("Timeout while reaching desired position");
        }
        yarp::os::Time::delay(0.2);
        timeout++;
    }
}

goToSingle()

bool JointLimits::goToSingle	(	int	i,
		double	pos,
		double *	reached_pos
	)

Definition at line 248 of file jointLimits.cpp.

{
    ipos->setRefSpeed((int)jointsList[i],speed[i]);
    ipos->positionMove((int)jointsList[i],pos);
    double tmp=0;
 
    int timeout = 0;
    while (1)
    {
        ienc->getEncoder((int)jointsList[i],&tmp);
        if (fabs(tmp-pos)<toleranceList[i]) break;
 
        if (timeout>100)
        {
            if(reached_pos != NULL)
            {
                *reached_pos = tmp;
            }
            return(false);
        }
 
        yarp::os::Time::delay(0.2);
        timeout++;
    }
    if(reached_pos != NULL)
    {
        *reached_pos = tmp;
    }
    return(true);
}

goToSingleExceed()

bool JointLimits::goToSingleExceed	(	int	i,
		double	position_to_reach,
		double	limit,
		double	reachedLimit,
		double *	reached_pos
	)

Definition at line 279 of file jointLimits.cpp.

{
    ipos->setRefSpeed((int)jointsList[i], speed[i]);
    ipos->positionMove((int)jointsList[i], position_to_reach);
    double tmp = 0;
    bool excededLimit = false;
    int timeout = 0;
    double limitToCheck;
    if(fabs(reachedLimit-limit)>toleranceList[i])
    {
        //if i'm here joint did NOT reached the limit and I would check joint doesn't exced reached limit
        limitToCheck = reachedLimit;
    }
    else
    {
        limitToCheck = limit;
    }
    while (1)
    {
        ienc->getEncoder((int)jointsList[i], &tmp);
        if(fabs(tmp - limitToCheck)>toleranceList[i])
        {
            excededLimit = true;
            //ROBOTTESTINGFRAMEWORK_TEST_REPORT (Asserter::format("j %d exced limitTocheck %f: reached %f",(int)jointsList[i],  limitToCheck, tmp));
            break;
        }
        if (fabs(tmp - position_to_reach)<toleranceList[i])
        {
            excededLimit = true;
            //ROBOTTESTINGFRAMEWORK_TEST_REPORT (Asserter::format("j %d is near position_to_reach %f: reached %f",(int)jointsList[i],  position_to_reach, tmp));
            break;
        }
 
        if (timeout>50) break;
        yarp::os::Time::delay(0.2);
        timeout++;
    }
 
    *reached_pos = tmp;
 
/*    if (fabs(tmp - position_to_reach)<toleranceList[i])
    {
        //I reached the out of bound target. That's bad!
        return (true);
    }
    if (fabs(tmp - limit)<toleranceList[i])
    {
        //I'm still near the joint limit. That's fine.
        return (false);
    }
    //I dont know where I am, that's bad!
    return(true);
*/
 
    if( excededLimit)
        return (true);
    else
        return(false);
}

run()

void JointLimits::run ( )

virtual

Definition at line 340 of file jointLimits.cpp.

{
    char buff[500];
    setMode(VOCAB_CM_POSITION);
    ROBOTTESTINGFRAMEWORK_TEST_REPORT("all joints are going to home....");
    goTo(home);
 
    for (unsigned int i=0; i<jointsList.size(); i++)
    {
        ilim->getLimits((int)jointsList[i],&min_lims[i],&max_lims[i]);
        ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(max_lims[i]!=min_lims[i],"Invalid limit: max==min?");
        ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(max_lims[i]>min_lims[i],"Invalid limit: max<min?");
        if (max_lims[i] == 0 && min_lims[i] == 0) ROBOTTESTINGFRAMEWORK_ASSERT_ERROR("Invalid limit: max==min==0");
    }
 
    for (unsigned int i=0; i<jointsList.size(); i++)
    {
        bool res;
        double reached_pos=0;
        double excededpos_reached = 0;
 
    //1) Check max limit
        sprintf(buff,"Testing if max limit is reachable, joint %d, max limit: %f",(int)jointsList[i],max_lims[i]);ROBOTTESTINGFRAMEWORK_TEST_REPORT(buff);
        //check that max_limit is reachable
        res = goToSingle(i, max_lims[i], &reached_pos);
        ROBOTTESTINGFRAMEWORK_TEST_CHECK (res, Asserter::format("joint %d moved to max limit: %f reached: %f",  (int)jointsList[i], max_lims[i], reached_pos));
        //if(!res)
        //{
        //    goToSingle(i,home[i], NULL); //I need to go to home in order to leave joint in safe position for further tests (other body parts)
        //    sprintf(buff, "Timeout while reaching desired position(%.2f). Reached pos=%.2f", max_lims[i], reached_pos);ROBOTTESTINGFRAMEWORK_ASSERT_ERROR(buff);
        //}
        //else { sprintf(buff, "Test successfull, joint %d, max limit: %f reached: %f", (int)jointsList[i], max_lims[i], reached_pos); ROBOTTESTINGFRAMEWORK_TEST_REPORT(buff); }
 
    //2) check that max_limit + outOfBoundPos is NOT reachable
        sprintf(buff, "Testing that max limit cannot be exceeded, joint %d, max limit: %f", (int)jointsList[i], max_lims[i]); ROBOTTESTINGFRAMEWORK_TEST_REPORT(buff);
        res = goToSingleExceed(i, max_lims[i] + outOfBoundPos[i], max_lims[i], reached_pos, &excededpos_reached);
        ROBOTTESTINGFRAMEWORK_TEST_CHECK (!res, Asserter::format("check if joint %d desn't exced max limit. target was: %f reached: %f, limit %f ",  (int)jointsList[i], max_lims[i] + outOfBoundPos[i], excededpos_reached, max_lims[i]));
        //if (res)
        //{
        //    goToSingle(i, home[i], NULL); //I need to go to home in order to leave joint in safe position for further tests (other body parts)
        //    sprintf(buff, "Limit execeeded! Limit was (%.2f). Reached pos=%.2f", max_lims[i], reached_pos); ROBOTTESTINGFRAMEWORK_ASSERT_ERROR(buff);
        //}
        //else { sprintf(buff, "Test successfull, joint %d, target was: %f reached: %f, limit %f ", (int)jointsList[i], max_lims[i] + outOfBoundPos[i], reached_pos, max_lims[i]); ROBOTTESTINGFRAMEWORK_TEST_REPORT(buff); }
 
    //3) Check min limit
        //check that min_limit is reachable
        sprintf(buff,"Testing if min limit is reachable, joint %d, min limit: %f",(int)jointsList[i],min_lims[i]);ROBOTTESTINGFRAMEWORK_TEST_REPORT(buff);
        res = goToSingle(i, min_lims[i], &reached_pos);
        ROBOTTESTINGFRAMEWORK_TEST_CHECK (res, Asserter::format("joint %d moved to min limit: %f reached: %f",  (int)jointsList[i], min_lims[i], reached_pos));
 
        /*if(!res)
        {
            goToSingle(i,home[i], NULL);
            sprintf(buff, "Timeout while reaching desired position(%.2f). Reached pos=%.2f", min_lims[i], reached_pos);ROBOTTESTINGFRAMEWORK_ASSERT_ERROR(buff);
        }
        else { sprintf(buff, "Test successfull, joint %d, min limit: %f reached: %f", (int)jointsList[i], min_lims[i], reached_pos); ROBOTTESTINGFRAMEWORK_TEST_REPORT(buff); }*/
 
    //4) check that min_limit - outOfBoundPos is NOT reachable
        sprintf(buff, "Testing that min limit cannot be exceeded, joint %d, min limit: %f", (int)jointsList[i], min_lims[i]); ROBOTTESTINGFRAMEWORK_TEST_REPORT(buff);
        res = goToSingleExceed(i, min_lims[i] - outOfBoundPos[i], min_lims[i], reached_pos, & excededpos_reached);
        ROBOTTESTINGFRAMEWORK_TEST_CHECK (!res, Asserter::format("check if joint %d desn't exced min limit. target was: %f reached: %f, limit %f ",  (int)jointsList[i], min_lims[i] - outOfBoundPos[i], excededpos_reached, min_lims[i]));
 
        //if (res)
        //{
        //    goToSingle(i, home[i], NULL); //I need to go to home in order to leave joint in safe position for further tests (other body parts)
        //    sprintf(buff, "Limit execeeded! Limit was (%.2f). Reached pos=%.2f", min_lims[i], reached_pos); ROBOTTESTINGFRAMEWORK_ASSERT_ERROR(buff);
        //}
        //else { sprintf(buff, "Test successfull, joint %d, target was: %f reached: %f, limit %f ", (int)jointsList[i], min_lims[i] - outOfBoundPos[i], reached_pos, min_lims[i]); ROBOTTESTINGFRAMEWORK_TEST_REPORT(buff); }
 
    //5) Check home position
        sprintf(buff,"Testing joint %d, homing to: %f",(int)jointsList[i],home[i]);ROBOTTESTINGFRAMEWORK_TEST_REPORT(buff);
        res = goToSingle(i, home[i], &reached_pos);
        if(!res)
        {
            sprintf(buff, "Timeout while reaching desired position(%.2f). Reached pos=%.2f", home[i], reached_pos);ROBOTTESTINGFRAMEWORK_ASSERT_ERROR(buff);
        }
        else{ sprintf(buff, "Homing joint %d complete", (int)jointsList[i]); ROBOTTESTINGFRAMEWORK_TEST_REPORT(buff); }
 
    }
    ienc->getEncoders(enc_jnt.data());
    ROBOTTESTINGFRAMEWORK_TEST_REPORT("Test ends. All joints are going to home....");
    goTo(home);
    yarp::os::Time::delay(2.0);
}

setMode()

void JointLimits::setMode

(

int

desired_mode

)

Definition at line 188 of file jointLimits.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 JointLimits::setup ( yarp::os::Property &  property )

virtual

Definition at line 57 of file jointLimits.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("speed"),       "The positionMove reference speed must be given as the test parameter!");
    ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("outOfBoundPosition"), "The outOfBoundPosition parameter must be given as the test parameter!");
    ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("outputLimitPercent"),  "The outputLimitPercent 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();
 
    char buff[500];
    sprintf(buff, "setting up test for %s", partName.c_str());
    ROBOTTESTINGFRAMEWORK_TEST_REPORT(buff);
 
    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* speedBottle = property.find("speed").asList();
    ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(speedBottle!=0,"unable to parse speed parameter");
 
    Bottle* outOfBoundPosition = property.find("outOfBoundPosition").asList();
    ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(outOfBoundPosition != 0, "unable to parse outOfBoundPosition parameter");
 
    Bottle* outputLimitBottle = property.find("outputLimitPercent").asList();
    ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(outputLimitBottle!=0,"unable to parse speed parameter");
 
    tolerance = property.find("tolerance").asFloat64();
    ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(tolerance>=0,"invalid tolerance");
 
    Bottle* toleranceListBottle = property.find("toleranceList").asList(); //optional param
 
    Property options;
    options.put("device", "remote_controlboard");
    options.put("remote", "/"+robotName+"/"+partName);
    options.put("local", "/jointsLimitsTest/"+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(ilim),"Unable to open limits interface");
    ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(ipid),"Unable to open pid 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);
    max_lims.resize(n_cmd_joints);
    min_lims.resize(n_cmd_joints);
 
    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();
    outputLimit.resize(n_cmd_joints);for (int i=0; i< n_cmd_joints; i++) outputLimit[i]=outputLimitBottle->get(i).asFloat64();
    outOfBoundPos.resize(n_cmd_joints); for (int i = 0; i < n_cmd_joints; i++) { outOfBoundPos[i] = outOfBoundPosition->get(i).asFloat64(); ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(outOfBoundPos[i] > 0 , "outOfBoundPosition must be > 0"); }
    toleranceList.resize(n_cmd_joints);
    for (int i = 0; i < n_cmd_joints; i++)
    {
        if(toleranceListBottle)
        {
            toleranceList[i] = toleranceListBottle->get(i).asFloat64();
            ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(toleranceList[i] >= 0 , "toleranceList must be > 0");
        }
        else
            toleranceList[i] = tolerance;
    }
 
    original_pids = new yarp::dev::Pid[n_cmd_joints];
    for (unsigned int i=0; i<jointsList.size(); i++)
    {
        ipid->getPid(yarp::dev::PidControlTypeEnum::VOCAB_PIDTYPE_POSITION, (int)jointsList[i],&original_pids[i]);
    }
    pids_saved=true;
 
    for (unsigned int i=0; i<jointsList.size(); i++)
    {
        yarp::dev::Pid p = original_pids[i];
        p.max_output = p.max_output/100*outputLimit[i];
        p.max_int =    p.max_int/100*outputLimit[i];
        ipid->setPid(yarp::dev::PidControlTypeEnum::VOCAB_PIDTYPE_POSITION, (int)jointsList[i],p);
        yarp::os::Time::delay(0.010);
        yarp::dev::Pid t;
        ipid->getPid(yarp::dev::PidControlTypeEnum::VOCAB_PIDTYPE_POSITION, (int)jointsList[i],&t);
 
        //since pid values are double, the returned values may differ from those sent due to conversion.
        if (fabs(t.max_output-p.max_output) > 1.0  ||
            fabs(t.max_int-p.max_int) > 1.0  ||
            fabs(t.kp-p.kp) > 1.0 ||
            fabs(t.kd-p.kd) > 1.0 ||
            fabs(t.ki-p.ki) > 1.0)
        {
            ROBOTTESTINGFRAMEWORK_ASSERT_ERROR("Unable to set output limits");
        }
 
    }
    return true;
}

tearDown()

void JointLimits::tearDown ( )

virtual

Definition at line 172 of file jointLimits.cpp.

{
    if (original_pids)
    {
        if (pids_saved)
        {
            for (unsigned int i=0; i<jointsList.size(); i++)
            {
                ipid->setPid(yarp::dev::PidControlTypeEnum::VOCAB_PIDTYPE_POSITION, (int)jointsList[i],original_pids[i]);
            }
        }
        delete[] original_pids; original_pids=0;
    }
    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/jointLimits/jointLimits.h
• /home/runner/work/icub-tests/icub-tests/gh-pages/src/jointLimits/jointLimits.cpp