main.cpp

Go to the documentation of this file.

 
#include <yarp/os/RFModule.h>
#include <yarp/os/Time.h>
#include <yarp/os/Network.h>
#include <yarp/os/Log.h>
#include <yarp/os/LogStream.h>
#include <yarp/dev/PolyDriver.h>
#include <yarp/dev/ControlBoardInterfaces.h>
#include <iCub/iDyn/iDyn.h>
#include <iCub/iDyn/iDynBody.h>
 
#include <iostream>
 
#include "gravityThread.h"
 
using namespace yarp::os;
using namespace yarp::sig;
using namespace yarp::math;
using namespace yarp::dev;
using namespace iCub::ctrl;
using namespace iCub::iDyn;
using namespace std;
 
 
 
class gravityModuleCompensator: public RFModule
{
private:
    int rate{};
    gravityCompensatorThread *g_comp{};
 
    Property OptionsLeftArm;
    Property OptionsRightArm;
    Property OptionsHead;
    Property OptionsLeftLeg;
    Property OptionsRightLeg;
    Property OptionsTorso;
 
    Port rpcPort;
 
    PolyDriver *dd_left_arm;
    PolyDriver *dd_right_arm;
    PolyDriver *dd_head;
    PolyDriver *dd_left_leg;
    PolyDriver *dd_right_leg;
    PolyDriver *dd_torso;
 
    bool legs_enabled;
    bool right_arm_enabled;
    bool left_arm_enabled;
    bool torso_enabled;
    bool head_enabled;
    string m_side;
    string m_part;
 
public:
    gravityModuleCompensator()
    {
        legs_enabled  = true;
        left_arm_enabled  = true;
        right_arm_enabled  = true;
        torso_enabled  = true;
        head_enabled  = true;
        dd_left_arm   = nullptr;
        dd_right_arm  = nullptr;
        dd_head       = nullptr;
        dd_left_leg   = nullptr;
        dd_right_leg  = nullptr;
        dd_torso      = nullptr;
        m_side = "right";
        m_part = "arm";
    }
 
   virtual bool createDriver(PolyDriver *&_dd, Property options)
    {
        int trials=0;
        double start_time = yarp::os::Time::now();
 
        do
        {
            double current_time = yarp::os::Time::now();
 
            //remove previously existing drivers
            if (_dd)
            {
                delete _dd;
                _dd=nullptr;
            }
 
            //creates the new device driver
            _dd = new PolyDriver(options);
            bool connected =_dd->isValid();
 
            //check if the driver is connected
            if (connected) break;
        
            //check if the timeout (60s) is expired
            if (current_time-start_time > 60.0)
            {
                yError("It is not possible to instantiate the device driver. I tried %d times!\n", trials);
                return false;
            }
 
            yarp::os::Time::delay(5);
            trials++;
            yWarning("Unable to connect the device driver, trying again...\n");
        }
        while (true);
 
        IEncoders         *encs     = nullptr;
        IControlMode      *ctrlMode = nullptr;
        IImpedanceControl *imp      = nullptr;
        ITorqueControl    *tqs      = nullptr;
 
        bool ok = true;
        ok = ok & _dd->view(encs);
        ok = ok & _dd->view(ctrlMode);
        ok = ok & _dd->view(imp);
        ok = ok & _dd->view(tqs);
        if (!ok)
        {
            yError("One or more devices has not been viewed\nreturning...");
            return false;
        }
 
        return true;
    }
 
    bool configure(ResourceFinder &rf) override
    {
        string fwdSlash = "/";
 
        string name;
        name = "gravityCompensator";    
 
        if (rf.check("period"))
            rate = rf.find("period").asInt32();
        else rate = 20;
 
        if (rf.check("rate"))
        {
            yError ("'rate' parameter is deprecated. Use 'period' instead");
            return false;
        }
 
        //-----------------GET THE ROBOT NAME-------------------//
        string robot_name;
        if (rf.check("robot"))
             robot_name = rf.find("robot").asString();
        else robot_name = "icub";
 
        //------------SPECIAL PARAM TP DEFINE THE HEAD TYPE-----//
        version_tag icub_type;
        if (rf.check("headV2"))
        {
            yInfo("'headV2' option found. Using icubV2 head kinematics.\n");
            icub_type.head_version = 2;
        }
 
        if (rf.check("headV2.6"))
        {
            yInfo("'headV2.6' option found. Using icubV2.6 head kinematics.\n");
            icub_type.head_version = 2;
            icub_type.head_subversion = 6;
        }
 
        if (rf.check("headV2.7"))
        {
            yInfo("'headV2.7' option found. Using icubV2.7 head kinematics.\n");
            icub_type.head_version = 2;
            icub_type.head_subversion = 7;
        }
 
        //------------------CHECK IF LEGS ARE ENABLED-----------//
        if (rf.check("no_legs"))
        {
            legs_enabled= false;
            yInfo("'no_legs' option found. Legs will be disabled.\n");
        }
        //------------------CHECK IF ARMS ARE ENABLED-----------//
        if (rf.check("no_left_arm"))
        {
            left_arm_enabled= false;
            yInfo("'no_left_arm' option found. Left arm will be disabled.\n");
        }
        //------------------CHECK IF ARMS ARE ENABLED-----------//
        if (rf.check("no_right_arm"))
        {
            right_arm_enabled= false;
            yInfo("'no_right_arm' option found. Right arm will be disabled.\n");
        }
        //------------------CHECK IF TORSO IS ENABLED-----------//
        if (rf.check("no_torso_legs"))
        {
            torso_enabled= false;
            legs_enabled= false;
            yInfo("no_torso_legs' option found. Torso and legs will be disabled.\n");
        }
        if (rf.check("no_torso"))
        {
            torso_enabled= false;
            yInfo("'no_torso' option found. Torso will be disabled.\n");
        }
        //------------------CHECK IF HEAD IS ENABLED-----------//
        if (rf.check("no_head"))
        {
            head_enabled= false;
            yInfo("'no_head' option found. Head will be disabled.\n");
        }
        //---------------------DEVICES--------------------------//
        if (head_enabled)
        {
            OptionsHead.put("device","remote_controlboard");
            OptionsHead.put("local","/gravityCompensator/head/client");
            OptionsHead.put("remote","/"+robot_name+"/head");
 
            if (!createDriver(dd_head, OptionsHead))
            {
                yError("unable to create head device driver...quitting\n");
                return false;
            }
        }
 
        if (left_arm_enabled)
        {
            OptionsLeftArm.put("device","remote_controlboard");
            OptionsLeftArm.put("local","/gravityCompensator/left_arm/client");
            OptionsLeftArm.put("remote","/"+robot_name+"/left_arm");
            if (!createDriver(dd_left_arm,OptionsLeftArm))
            {
                yError("unable to create left arm device driver...quitting\n");
                return false;
            }
        }
 
        if (right_arm_enabled)
        {
            OptionsRightArm.put("device","remote_controlboard");
            OptionsRightArm.put("local","/gravityCompensator/right_arm/client");
            OptionsRightArm.put("remote","/"+robot_name+"/right_arm");
            if (!createDriver(dd_right_arm,OptionsRightArm))
            {
                yError("unable to create right arm device driver...quitting\n");
                return false;
            }
        }
 
        if (legs_enabled)
        {
            OptionsLeftLeg.put("device","remote_controlboard");
            OptionsLeftLeg.put("local","/gravityCompensator/left_leg/client");
            OptionsLeftLeg.put("remote","/"+robot_name+"/left_leg");
            if (!createDriver(dd_left_leg,OptionsLeftLeg))
            {
                yError("unable to create left leg device driver...quitting\n");
                return false;
            }
 
            OptionsRightLeg.put("device","remote_controlboard");
            OptionsRightLeg.put("local","/gravityCompensator/right_leg/client");
            OptionsRightLeg.put("remote","/"+robot_name+"/right_leg");
            if (!createDriver(dd_right_leg,OptionsRightLeg))
            {
                yError("unable to create right leg device driver...quitting\n");
                return false;
            }
        }
        
        if (torso_enabled)
        {
            OptionsTorso.put("device","remote_controlboard");
            OptionsTorso.put("local","/gravityCompensator/torso/client");
            OptionsTorso.put("remote","/"+robot_name+"/torso");
 
            if (!createDriver(dd_torso,OptionsTorso))
            {
                yError("unable to create torso device driver...quitting\n");
                return false;
            }
        }
        
        yInfo("device driver created\n");
 
        rpcPort.open("/"+name+"/rpc");
        attach(rpcPort);        
 
        //------------------CHECK FOR WHOLEBODYNAME -----------//
        std::string wholeBodyName = "wholeBodyDynamics";
        if (rf.check("wholebody_name"))
        {
            wholeBodyName = rf.find("wholebody_name").asString();
            yInfo("'wholeBodyName' option found. Using /%s prefix for connections.\n", wholeBodyName.c_str());
        }
 
        //------------------CHECK FOR INERTIAL -----------//
        bool inertial_enabled=true;
        if (rf.check("no_inertial"))
        {
            inertial_enabled=false;
            yInfo("'no_inertial' option found. Disabling inertial measurment.\n");
        }
 
        //--------------------------THREAD--------------------------
 
        g_comp = new gravityCompensatorThread(wholeBodyName, rate, dd_left_arm, dd_right_arm, dd_head, dd_left_leg, dd_right_leg, dd_torso, icub_type, inertial_enabled);
        yInfo("ft thread istantiated...\n");
        g_comp->start();
        yInfo("thread started\n");
 
        //------------------CHECK CONTROL OPTIONS -----------//
        if (rf.check("gravity_on"))
        {
            g_comp->gravity_mode = GRAVITY_COMPENSATION_ON;
        }
        if (rf.check("gravity_off"))
        {
            g_comp->gravity_mode = GRAVITY_COMPENSATION_OFF;
        }
        if (rf.check("external_on"))
        {
            g_comp->external_mode = EXTERNAL_TRQ_ON;
        }
        if (rf.check("external_off"))
        {
            g_comp->external_mode = EXTERNAL_TRQ_OFF;
        }
        if (g_comp->gravity_mode== GRAVITY_COMPENSATION_ON)       yInfo("gravity compensation on");
        else if (g_comp->gravity_mode== GRAVITY_COMPENSATION_OFF) yInfo("gravity compensation off");
        if (g_comp->external_mode==EXTERNAL_TRQ_ON)               yInfo("external input on");
        else if (g_comp->external_mode==EXTERNAL_TRQ_OFF)         yInfo("external input off");
 
        return true;
    }
 
    bool respond(const Bottle& command, Bottle& reply) override
    {
        reply.addVocab32("many");
        reply.addString("Available commands:");
        reply.addString("calib all");
 
        Bottle position_bot;
        string helpMessage =  getName() +
                            " commands are: \n" +  
                            "help         to display this message\n" + 
                            "gravity_on   to enable the gravity compensation \n" + 
                            "gravity_off  to disbale the gravity compensation \n" +
                            "external_on  to enable the external input torque \n" +
                            "external_off to disable the external input torque \n";
 
          reply.clear(); 
        if (command.get(0).asString()=="help")
        {
            cout << helpMessage;
            reply.addVocab32("many");
            reply.addString(" commands are: \n" );
            reply.addString(helpMessage.c_str());
        }
        else if (command.get(0).asString()=="gravity_on")
        {
            if (g_comp) 
            {
                g_comp->gravity_mode = GRAVITY_COMPENSATION_ON;
                reply.addString("gravity compensation on");
            }
        }
        else if (command.get(0).asString()=="gravity_off")
        {
            if (g_comp)
            {
                g_comp->gravity_mode = GRAVITY_COMPENSATION_OFF;
                reply.addString("gravity compensation off");
            }
        }
        else if (command.get(0).asString()=="external_on")
        {
            if (g_comp) 
            {
                g_comp->external_mode = EXTERNAL_TRQ_ON;
                reply.addString("external input on");
            }
        }
        else if (command.get(0).asString()=="external_off")
        {
            if (g_comp)
            {
                g_comp->external_mode = EXTERNAL_TRQ_OFF;
                reply.addString("external input off");
            }
        }
        else
        {
            reply.addString("unknown command. type help.");
        }
        
        return true;
    }
 
 
    bool close() override
    {
        //stop thread 
        if(g_comp)
        {
            g_comp->stop();
            delete g_comp; g_comp = nullptr;
        }
        
        //closing interfaces
        if (dd_left_arm)    {delete dd_left_arm;  dd_left_arm=nullptr;  }
        if (dd_right_arm)   {delete dd_right_arm; dd_right_arm=nullptr; }
        if (dd_left_leg)    {delete dd_left_leg;  dd_left_leg=nullptr;  }
        if (dd_right_leg)   {delete dd_right_leg; dd_right_leg=nullptr; }
        if (dd_head)        {delete dd_head;      dd_head=nullptr;      }
        if (dd_torso)       {delete dd_torso;     dd_torso=nullptr;     }
 
        //closing ports
        rpcPort.interrupt();
        rpcPort.close();
 
        return true;
    }
 
    double getPeriod() override { return 1.0;  }
    bool updateModule() override
    {
        static unsigned long int alive_counter = 0;
        static double curr_time = Time::now();
        if (Time::now() - curr_time > 60)
        {
            yInfo ("gravityCompensator is alive! running for %ld mins.\n",++alive_counter);
            curr_time = Time::now();
        }
 
        if (g_comp==nullptr) return false;
 
        thread_status_enum thread_status = g_comp->getThreadStatus();
 
        if (thread_status==STATUS_OK)
            return true;
        else if (thread_status==STATUS_DISCONNECTED)
        {
            yError("gravityCompensator module lost connection with iCubInterface, now closing...\n");
            return false;
        }
        else
        {
            yInfo("gravityCompensator module was closed successfully! \n");    
            return true;
        }
    }
};
 
 
int main(int argc, char * argv[])
{
    ResourceFinder rf;
    rf.configure(argc,argv);
 
    if (rf.check("help"))
    {
        yInfo() << "Options:";
        yInfo() << "--context context: where to find the called resource (referred to $ICUB_ROOT/app:)";
        yInfo() << "--from       from: the name of the file.ini to be used for calibration";
        yInfo() << "--rate       rate: the period used by the module. default 100ms (not less than 15ms)";
        yInfo() << "--no_legs    this option disables the gravity compensation for the legs joints" ;
        yInfo() << "--headV2     use the model of the headV2";
        yInfo() << "--headV2.6   use the model of the headV2.6";
        yInfo() << "--headV2.7   use the model of the headV2.7";
        yInfo() << "--no_left_arm      disables the left arm";
        yInfo() << "--no_right_arm     disables the right arm";
        yInfo() << "--no_legs          disables the legs";
        yInfo() << "--no_left_arm      disables the left arm";
        yInfo() << "--no_torso         disables the torso";
        yInfo() << "--no_torso_legs    disables the torso and the legs";
        yInfo() << "--no_head          disables the head";
        yInfo() << "--wholebody_name   the wholeBodyDyanmics port prefix (e.g. 'wholeBodyDynamics' / 'wholeBodyDynamicsTree')";
        yInfo() << "--no_inertial      disables the inertial";
        yInfo() << "--gravity_on       enables gravity compensation (default)";
        yInfo() << "--gravity_off      disables gravity compensation";
        yInfo() << "--external_on      enables external torque command (default)";
        yInfo() << "--external_off     disables external torque command";
        return 0;
    }
 
    Network yarp;
 
    if (!yarp.checkNetwork())
        return 1;
 
    gravityModuleCompensator gcomp;
 
    return gcomp.runModule(rf);
}