tutorial_actionPrimitives.cpp

Go to the documentation of this file.

/*
 * Copyright (C) 2010 RobotCub Consortium, European Commission FP6 Project IST-004370
 * Author: Ugo Pattacini
 * email:  ugo.pattacini@iit.it
 * website: www.robotcub.org
 * Permission is granted to copy, distribute, and/or modify this program
 * under the terms of the GNU General Public License, version 2 or any
 * later version published by the Free Software Foundation.
 *
 * A copy of the license can be found at
 * http://www.robotcub.org/icub/license/gpl.txt
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
 * Public License for more details
*/
 
#include <iostream>
#include <fstream>
#include <iomanip>
#include <string>
#include <deque>
 
#include <yarp/os/Network.h>
#include <yarp/os/RFModule.h>
#include <yarp/os/Bottle.h>
#include <yarp/os/BufferedPort.h>
#include <yarp/sig/Vector.h>
#include <yarp/math/Math.h>
#include <yarp/dev/Drivers.h>
 
#include <iCub/perception/models.h>
#include <iCub/action/actionPrimitives.h>
 
#define USE_LEFT    0
#define USE_RIGHT   1
 
#define AFFACTIONPRIMITIVESLAYER    ActionPrimitivesLayer1
 
using namespace std;
using namespace yarp::os;
using namespace yarp::sig;
using namespace yarp::dev;
using namespace yarp::math;
using namespace iCub::perception;
using namespace iCub::action;
 
 
/************************************************************************/
class ExampleModule: public RFModule
{
protected:
    AFFACTIONPRIMITIVESLAYER *action;
    BufferedPort<Bottle>      inPort;
    Port                      rpcPort;
 
    Vector graspOrien;
    Vector graspDisp;
    Vector dOffs;
    Vector dLift;
    Vector home_x;
 
    bool firstRun;
 
public:
    /************************************************************************/
    ExampleModule()
    {
        graspOrien.resize(4);
        graspDisp.resize(3);
        dOffs.resize(3);
        dLift.resize(3);
        home_x.resize(3);
 
        // default values for arm-dependent quantities
        graspOrien[0]=-0.171542;
        graspOrien[1]= 0.124396;
        graspOrien[2]=-0.977292;
        graspOrien[3]= 3.058211;
 
        graspDisp[0]=0.0;
        graspDisp[1]=0.0;
        graspDisp[2]=0.05;
 
        dOffs[0]=-0.03;
        dOffs[1]=-0.07;
        dOffs[2]=-0.02;
 
        dLift[0]=0.0;
        dLift[1]=0.0;
        dLift[2]=0.15;
 
        home_x[0]=-0.29;
        home_x[1]=-0.21;
        home_x[2]= 0.11;
 
        action=NULL;
        firstRun=true;
    }
 
    /************************************************************************/
    void getArmDependentOptions(Bottle &b, Vector &_gOrien, Vector &_gDisp,
                                Vector &_dOffs, Vector &_dLift, Vector &_home_x)
    {
        if (Bottle *pB=b.find("grasp_orientation").asList())
        {
            int sz=(int)pB->size();
            int len=(int)_gOrien.length();
            int l=len<sz?len:sz;
 
            for (int i=0; i<l; i++)
                _gOrien[i]=pB->get(i).asFloat64();
        }
 
        if (Bottle *pB=b.find("grasp_displacement").asList())
        {
            int sz=(int)pB->size();
            int len=(int)_gDisp.length();
            int l=len<sz?len:sz;
 
            for (int i=0; i<l; i++)
                _gDisp[i]=pB->get(i).asFloat64();
        }
 
        if (Bottle *pB=b.find("systematic_error_displacement").asList())
        {
            int sz=(int)pB->size();
            int len=(int)_dOffs.length();
            int l=len<sz?len:sz;
 
            for (int i=0; i<l; i++)
                _dOffs[i]=pB->get(i).asFloat64();
        }
 
        if (Bottle *pB=b.find("lifting_displacement").asList())
        {
            int sz=(int)pB->size();
            int len=(int)_dLift.length();
            int l=len<sz?len:sz;
 
            for (int i=0; i<l; i++)
                _dLift[i]=pB->get(i).asFloat64();
        }
 
        if (Bottle *pB=b.find("home_position").asList())
        {
            int sz=(int)pB->size();
            int len=(int)_home_x.length();
            int l=len<sz?len:sz;
 
            for (int i=0; i<l; i++)
                _home_x[i]=pB->get(i).asFloat64();
        }
    }
 
    /************************************************************************/
    bool configure(ResourceFinder &rf)
    {
        string name=rf.find("name").asString();
        setName(name.c_str());
 
        Property config; config.fromConfigFile(rf.findFile("from"));
        Bottle &bGeneral=config.findGroup("general");
        if (bGeneral.isNull())
        {
            cout<<"Error: group general is missing!"<<endl;
            return false;
        }
 
        // parsing general config options
        Property option(bGeneral.toString().c_str());
        option.put("local",name);
        option.put("part","left_arm");
        option.put("grasp_model_type",rf.find("grasp_model_type").asString());
        option.put("grasp_model_file",rf.findFile("grasp_model_file"));
        option.put("hand_sequences_file",rf.findFile("hand_sequences_file"));
 
        // parsing arm dependent config options
        Bottle &bArm=config.findGroup("arm_dependent");
        getArmDependentOptions(bArm,graspOrien,graspDisp,dOffs,dLift,home_x);
 
        cout<<"***** Instantiating primitives for left arm"<<endl;
        action=new AFFACTIONPRIMITIVESLAYER(option);
        if (!action->isValid())
        {
            delete action;
            return false;
        }
 
        deque<string> q=action->getHandSeqList();
        cout<<"***** List of available hand sequence keys:"<<endl;
        for (size_t i=0; i<q.size(); i++)
            cout<<q[i]<<endl;
 
        string fwslash="/";
        inPort.open(fwslash+name+"/in");
        rpcPort.open(fwslash+name+"/rpc");
        attach(rpcPort);
 
        // check whether the grasp model is calibrated,
        // otherwise calibrate it
        Model *model; action->getGraspModel(model);
        if (model!=NULL)
        {
            if (!model->isCalibrated())
            {
                Property prop;
                prop.put("finger","all");
                model->calibrate(prop);
            }
        }
 
        return true;
    }
 
    /************************************************************************/
    bool close()
    {
        if (action!=NULL)
            delete action;
 
        inPort.close();
        rpcPort.close();
 
        return true;
    }
 
    /************************************************************************/
    double getPeriod()
    {
        return 0.1;
    }
 
    /************************************************************************/
    void init()
    {
        bool f;
 
        action->pushAction(home_x,"open_hand");
        action->checkActionsDone(f,true);
        action->enableArmWaving(home_x);
    }
 
    // we don't need a thread since the actions library already
    // incapsulates one inside dealing with all the tight time constraints
    /************************************************************************/
    bool updateModule()
    {
        // do it only once
        if (firstRun)
        {
            init();
            firstRun=false;
        }
 
        // get a target object position from a YARP port
        Bottle *b=inPort.read();    // blocking call
 
        if (b!=NULL)
        {
            Vector xd(3);
            bool f;
 
            xd[0]=b->get(0).asFloat64();
            xd[1]=b->get(1).asFloat64();
            xd[2]=b->get(2).asFloat64();
 
            // apply systematic offset
            // due to uncalibrated kinematic
            xd=xd+dOffs;
 
            // safe thresholding
            xd[0]=xd[0]>-0.1?-0.1:xd[0];
 
            // grasp (wait until it's done)
            action->grasp(xd,graspOrien,graspDisp);
            action->checkActionsDone(f,true);
            action->areFingersInPosition(f);
 
            // if fingers are not in position,
            // it's likely that we've just grasped
            // something, so lift it up!
            if (!f)
            {
                cout<<"Wow, got something!"<<endl;
 
                // lift the object (wait until it's done)
                action->pushAction(xd+dLift,graspOrien);
                action->checkActionsDone(f,true);
            }
            else
                cout<<"Sorry :( ... nothing to grasp"<<endl;
 
            // release the object or just open the
            // hand (wait until it's done)
            action->pushAction("open_hand");
            action->checkActionsDone(f,true);
 
            // go home :) (wait until it's done, since
            // we may use two arms that share the torso)
            action->pushAction(home_x);
            action->checkActionsDone(f,true);
 
            // let the hand wave a bit around home position
            // the waving will be disabled before commencing
            // a new action
            action->enableArmWaving(home_x);
        }
 
        return true;
    }
 
    /************************************************************************/
    bool interruptModule()
    {
        // since a call to checkActionsDone() blocks
        // the execution until it's done, we need to
        // take control and exit from the waiting state
        action->syncCheckInterrupt(true);
 
        inPort.interrupt();
        rpcPort.interrupt();
 
        return true;
    }
};
 
 
/************************************************************************/
int main(int argc, char *argv[])
{
    Network yarp;
    if (!yarp.checkNetwork())
    {
        cout<<"YARP server not available!"<<endl;
        return 1;
    }
 
    ResourceFinder rf;
    rf.setDefaultConfigFile("config.ini");
    rf.setDefault("grasp_model_type","tactile");
    rf.setDefault("grasp_model_file","grasp_model.ini");
    rf.setDefault("hand_sequences_file","hand_sequences.ini");
    rf.setDefault("name","actionPrimitivesMod");
    rf.configure(argc,argv);
 
    ExampleModule mod;
    return mod.runModule(rf);
}