TorqueControlGravityConsistency Class Reference

The test is supposed to be run with the iCub fixed to the pole, with the pole leveled with respect to the gravity (please check this with a level before running the test) and with the wholeBodyDynamics(Tree) running. More...

#include <TorqueControlGravityConsistency.h>

Inherits yarp::robottestingframework::TestCase.

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

Detailed Description

The test is supposed to be run with the iCub fixed to the pole, with the pole leveled with respect to the gravity (please check this with a level before running the test) and with the wholeBodyDynamics(Tree) running.

The tests opens the wholeBodyInterface (to be migrated to use material available on YARP and iDynTree) and compares the gravity compensation torque coming from the model and assuming that the gravity is fixed in the based with the joint torques measured by iCub (that actually come from the wholeBodyDynamics(Tree) ).

Example: testRunner -v -t TorqueControlGravityConsistency.dll -p ""

Definition at line 45 of file TorqueControlGravityConsistency.h.

Constructor & Destructor Documentation

TorqueControlGravityConsistency()

TorqueControlGravityConsistency::TorqueControlGravityConsistency

(

)

Definition at line 39 of file TorqueControlGravityConsistency.cpp.

                                                                 : yarp::robottestingframework::TestCase("TorqueControlGravityConsistency"),
                                                                     yarpRobot(0)
{
 
}

~TorqueControlGravityConsistency()

TorqueControlGravityConsistency::~TorqueControlGravityConsistency ( )

virtual

Definition at line 45 of file TorqueControlGravityConsistency.cpp.

{
}

Member Function Documentation

run()

void TorqueControlGravityConsistency::run ( )

virtual

Definition at line 109 of file TorqueControlGravityConsistency.cpp.

{
    //Get the number of controlled degrees of freedom of the robot
    int dof = yarpRobot->getDoFs();
    printf("Number of (internal) controlled DoFs: %d\n", dof);
 
    //Allocate yarp vector of the right dimensions
    Vector q(dof), trqMeasured(dof), trqGravity(dof), generalizedBiasForces(dof+6), dq(dof);
 
    //Read position and torque estimates
    yarpRobot->getEstimates(wbi::ESTIMATE_JOINT_POS, q.data());
    yarpRobot->getEstimates(wbi::ESTIMATE_JOINT_VEL, dq.data());
    yarpRobot->getEstimates(wbi::ESTIMATE_JOINT_TORQUE, trqMeasured.data());
 
    // Compute gravity compensation torques
    wbi::Frame world2base;
    world2base.identity();
    Vector baseTwist(6);
    baseTwist.zero();
 
    double m_gravity[3];
    m_gravity[0] = 0;
    m_gravity[1] = 0;
    m_gravity[2] = -9.81;
 
    yarpRobot->computeGeneralizedBiasForces(q.data(),world2base,dq.data(),baseTwist.data(),m_gravity,generalizedBiasForces.data());
 
    // We extract the joint torques from the generalized bias forces
    trqGravity = generalizedBiasForces.subVector(6,6+dof-1);
 
    for(size_t i=0; i < dof; i++)
    {
        wbi::ID wbiID;
        yarpRobot->getJointList().indexToID(i,wbiID);
        std::cerr << "Joint " << wbiID.toString() << " : " << std::endl;
        std::cerr << "\t Measured torque: " << trqMeasured[i] << "Nm" << std::endl;
        std::cerr << "\t Gravity  torque: " << trqGravity[i] << "Nm" << std::endl;
        std::cerr << "\t Difference     : " << trqMeasured[i]-trqGravity[i] << "Nm" << std::endl;
    }
}

setup()

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

virtual

Definition at line 49 of file TorqueControlGravityConsistency.cpp.

{
    yarp::os::ResourceFinder & rf = yarp::os::ResourceFinder::getResourceFinderSingleton();
 
    yarp::os::Property yarpWbiConfiguration;
    std::string yarpWbiConfigurationFile;
    if(property.check("wbi_conf_file") && property.find("wbi_conf_file").isString())
    {
        yarpWbiConfigurationFile = rf.findFileByName(property.find("wbi_conf_file").asString());
    }
    else
    {
        yarpWbiConfigurationFile = rf.findFileByName("yarpWholeBodyInterface.ini");
    }
 
    //It may be convenient to overload some option of the configuration file,
    // so we load in the yarpWbiConfiguration also the option passed in the command line
    yarpWbiConfiguration.fromConfigFile(yarpWbiConfigurationFile);
 
    yarpWbiConfiguration.fromString(property.toString().c_str(),false);
 
    // Create yarpWholeBodyInterface
    std::string localName = "wbiTest";
    if( property.check("local") )
    {
        localName = property.find("local").asString();
    }
 
    wbi::wholeBodyInterface *yarpRobot = new yarpWbi::yarpWholeBodyInterface (localName.c_str(), yarpWbiConfiguration);
 
    wbi::IDList RobotMainJoints;
    std::string RobotMainJointsListName = "ROBOT_TORQUE_CONTROL_JOINTS";
    if( !yarpWbi::loadIdListFromConfig(RobotMainJointsListName,yarpWbiConfiguration,RobotMainJoints) )
    {
        fprintf(stderr, "[ERR] yarpWholeBodyInterface: impossible to load wbiId joint list with name %s\n",RobotMainJointsListName.c_str());
        return false;
    }
 
    yarpRobot->addJoints(RobotMainJoints);
 
    std::cout << "Joints added, calling init method" <<  std::endl;
 
    if(!yarpRobot->init())
    {
        std::cout << "Error: init() method failed" << std::endl;
        return false;
    }
 
    Time::delay(0.5);
 
    return true;
}

tearDown()

void TorqueControlGravityConsistency::tearDown ( )

virtual

Definition at line 102 of file TorqueControlGravityConsistency.cpp.

{
    yarpRobot->close();
    delete yarpRobot;
    yarpRobot = 0;
}

---

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

• /home/runner/work/icub-tests/icub-tests/gh-pages/src/torqueControl-gravityConsistency/TorqueControlGravityConsistency.h
• /home/runner/work/icub-tests/icub-tests/gh-pages/src/torqueControl-gravityConsistency/TorqueControlGravityConsistency.cpp