CartesianControlReachingToleranceTest Class Reference

This test verifies the point-to-point cartesian movement. More...

#include <CartesianControlReachingToleranceTest.h>

Inherits yarp::robottestingframework::TestCase.

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

Detailed Description

This test verifies the point-to-point cartesian movement.

Accepts the following parameters:

Parameter name	Type	Units	Default Value	Required	Description	Notes
robot	string	-	icubSim	No	robot name	-
arm-type	string	-	left	No	left or right	-

Definition at line 40 of file CartesianControlReachingToleranceTest.h.

Constructor & Destructor Documentation

CartesianControlReachingToleranceTest()

CartesianControlReachingToleranceTest::CartesianControlReachingToleranceTest

(

)

Definition at line 44 of file CartesianControlReachingToleranceTest.cpp.

                                                                             :
                                       yarp::robottestingframework::TestCase("CartesianControlReachingToleranceTest")
{
}

~CartesianControlReachingToleranceTest()

CartesianControlReachingToleranceTest::~CartesianControlReachingToleranceTest ( )

virtual

Definition at line 51 of file CartesianControlReachingToleranceTest.cpp.

{
}

Member Function Documentation

run()

void CartesianControlReachingToleranceTest::run ( )

virtual

Definition at line 116 of file CartesianControlReachingToleranceTest.cpp.

{
    ICartesianControl *iarm;
    ROBOTTESTINGFRAMEWORK_TEST_CHECK(drvCart.view(iarm),"Opening the view on the Cartesian device!");
 
    IEncoders *ienc;
    ROBOTTESTINGFRAMEWORK_TEST_CHECK(drvJoint.view(ienc),"Opening the view on the Joint device!");
 
    bool done;
 
    Vector x,o;
    double t0=Time::now();
    while (Time::now()-t0<5.0)
    {
        done=iarm->getPose(x,o);
        if (done)
            break;
        Time::delay(0.1);
    }
    ROBOTTESTINGFRAMEWORK_TEST_CHECK(done,"Initial pose retrieved!");
 
    ROBOTTESTINGFRAMEWORK_TEST_REPORT("Setting up the context");
    int context;
    iarm->storeContext(&context);
 
    Vector dof;
    iarm->getDOF(dof); dof=1.0;
    dof[0]=dof[1]=dof[2]=0.0;
    iarm->setDOF(dof,dof);
    iarm->setTrajTime(1.0);
    iarm->setInTargetTol(0.02);
 
    double tol;
    iarm->getInTargetTol(&tol);
 
    Vector pos2reach(3,0.0);
    pos2reach[0]=-0.35;
    pos2reach[1]=0.0;
    pos2reach[2]=0.15;
 
    Matrix dcm2reach=zeros(3,3);
    dcm2reach(0,0)=dcm2reach(2,1)=dcm2reach(1,2)=-1.0;
    Vector ori2reach=dcm2axis(dcm2reach);
 
    ROBOTTESTINGFRAMEWORK_TEST_REPORT(Asserter::format("Reaching for the target: (%s, %s)",
                                     pos2reach.toString(3,3).c_str(),ori2reach.toString(3,3).c_str()));
    Vector xh,oh,qh;
    iarm->goToPoseSync(pos2reach,ori2reach);
    iarm->getDesired(xh,oh,qh);
 
    ROBOTTESTINGFRAMEWORK_TEST_REPORT("Waiting");
    done=iarm->waitMotionDone(0.1,10.0);
    iarm->stopControl();
 
    Vector xf,of;
    iarm->getPose(xf,of);
 
    int nJoints;
    ienc->getAxes(&nJoints);
    Vector qf(nJoints);
    ienc->getEncoders(qf.data());
 
    ROBOTTESTINGFRAMEWORK_TEST_CHECK(done,"Target reached!");
    ROBOTTESTINGFRAMEWORK_TEST_REPORT(Asserter::format("Reaching tolerance: %g",tol));
    ROBOTTESTINGFRAMEWORK_TEST_REPORT(Asserter::format("Solved pose: (%s, %s)",
                                     xh.toString(3,3).c_str(),oh.toString(3,3).c_str()));
    ROBOTTESTINGFRAMEWORK_TEST_REPORT(Asserter::format("Reached pose: (%s, %s)",
                                     xf.toString(3,3).c_str(),of.toString(3,3).c_str()));
    ROBOTTESTINGFRAMEWORK_TEST_REPORT(Asserter::format("Solved joints qh: (%s)",
                                     qh.subVector(3,dof.length()-1).toString(3,3).c_str()));
    ROBOTTESTINGFRAMEWORK_TEST_REPORT(Asserter::format("Reached joints qf: (%s)",
                                     qf.subVector(0,6).toString(3,3).c_str()));
    ROBOTTESTINGFRAMEWORK_TEST_REPORT(Asserter::format("Error: %g",compute_error(xh,oh,xf,of)));
 
    ROBOTTESTINGFRAMEWORK_TEST_REPORT("Going back to starting pose");
    iarm->goToPoseSync(x,o);
 
    ROBOTTESTINGFRAMEWORK_TEST_REPORT("Waiting");
    done=iarm->waitMotionDone(1.0,5.0);
    ROBOTTESTINGFRAMEWORK_TEST_CHECK(done,"Starting pose reached!");
 
    ROBOTTESTINGFRAMEWORK_TEST_REPORT("Cleaning up the context");
    iarm->restoreContext(context);
    iarm->deleteContext(context);
}

setup()

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

virtual

Definition at line 57 of file CartesianControlReachingToleranceTest.cpp.

{
    string robot=property.check("robot",Value("icubSim")).asString();
    string arm=property.check("arm-type",Value("left")).asString();
 
    Property optCart;
    optCart.put("device","cartesiancontrollerclient");
    optCart.put("remote",("/"+robot+"/"+"cartesianController/"+arm+"_arm"));
    optCart.put("local",("/"+getName()+"/cartesian/"+arm+"_arm"));
 
    Property optJoint;
    optJoint.put("device","remote_controlboard");
    optJoint.put("remote",("/"+robot+"/"+arm+"_arm"));
    optJoint.put("local",("/"+getName()+"/joint/"+arm+"_arm"));
 
    ROBOTTESTINGFRAMEWORK_TEST_REPORT(Asserter::format("Opening Cartesian Controller Client for %s_arm",arm.c_str()));
    ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(drvCart.open(optCart),"Unable to open the client!");
 
    ROBOTTESTINGFRAMEWORK_TEST_REPORT(Asserter::format("Opening Joint Controller Client for %s_arm",arm.c_str()));
    ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(drvJoint.open(optJoint),"Unable to open the client!");
    return true;
}

tearDown()

void CartesianControlReachingToleranceTest::tearDown ( )

virtual

Definition at line 82 of file CartesianControlReachingToleranceTest.cpp.

{
    ROBOTTESTINGFRAMEWORK_TEST_REPORT("Closing Cartesian Controller Client");
    ROBOTTESTINGFRAMEWORK_ASSERT_FAIL_IF_FALSE(drvCart.close(),"Unable to close the client!");
 
    ROBOTTESTINGFRAMEWORK_TEST_REPORT("Closing Joint Controller Client");
    ROBOTTESTINGFRAMEWORK_ASSERT_FAIL_IF_FALSE(drvJoint.close(),"Unable to close the client!");
}

---

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

• /home/runner/work/icub-tests/icub-tests/gh-pages/src/cartesian-control/reaching-tolerance/CartesianControlReachingToleranceTest.h
• /home/runner/work/icub-tests/icub-tests/gh-pages/src/cartesian-control/reaching-tolerance/CartesianControlReachingToleranceTest.cpp