main.cpp

Go to the documentation of this file.

/* 
 * Copyright (C) 2011 Department of Robotics Brain and Cognitive Sciences - Istituto Italiano di Tecnologia
 * Author: Ugo Pattacini
 * email:  ugo.pattacini@iit.it
 * 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 <yarp/os/all.h>
#include <iCub/iKin/iKinSlv.h>
#include <fakeMotorDevice.h>
 
#include <iostream>
#include <iomanip>
#include <string>
 
using namespace std;
using namespace yarp::os;
using namespace iCub::iKin;
 
class fakeRobotCartesianSolver : public CartesianSolver
{
protected:
    PartDescriptor *getPartDesc(Searchable &options)
    {
        if (!options.check("CustomKinFile"))
        {
            cout<<"Error: \"CustomKinFile\" option is missing!"<<endl;
            return NULL;
        }
 
        string robot=options.check("robot",Value("fake_robot")).asString();
        string part="fake_part";
 
        // here we declare everything is required to open up
        // the device driver to access the fake robot
        Property optPart;
        optPart.put("device","fakeyClient");
        optPart.put("remote","/"+robot+"/"+part);
        optPart.put("local","/"+slvName+"/"+part);
        optPart.put("part",part);
 
        // we grab info on the fake robot's kinematics
        Property linksOptions;
        linksOptions.fromConfigFile(options.find("CustomKinFile").asString());
        iKinLimb *limb=new iKinLimb(linksOptions);
        if (!limb->isValid())
        {
            cout<<"Error: invalid links parameters!"<<endl;
            delete limb;
            return NULL;
        }
 
        // we fill in the descriptor fields
        PartDescriptor *p=new PartDescriptor;
        p->lmb=limb;                // a pointer to the iKinLimb
        p->chn=limb->asChain();     // the associated iKinChain object
        p->cns=NULL;                // any further (linear) constraints on the joints other than the bounds? This requires some more effort
        p->prp.push_back(optPart);  // attach the options to open the device driver of the fake part
        p->rvs.push_back(false);    // it may happen that the motor commands to be sent are in reversed order wrt the order of kinematics links (e.g. the iCub torso); if so put here "true"
        p->num=1;                   // only one device driver for the whole limb (see below)
 
        // whenever a limb is actuated resorting to more than one device
        // (e.g. for iCub: torso+arm), the following applies:
        // 
        // p->prp.push_back(optDevice_1);
        // p->prp.push_back(optDevice_2);
        // p->rvs.push_back(true);
        // p->rvs.push_back(false);
        // p->num=2;
 
        return p;
    }
 
public:
    /**********************************************************/
    fakeRobotCartesianSolver(const string &name) : CartesianSolver(name) { }
};
 
class SolverModule: public RFModule
{
protected:
    CartesianSolver *solver;
 
public:
    /**********************************************************/
    SolverModule() : solver(NULL) { }
 
    /**********************************************************/
    bool configure(ResourceFinder &rf)
    {                
        if (!rf.check("name"))
        {
            cout<<"Error: \"name\" option is missing!"<<endl;
            return false;
        }
 
        string solverName=rf.find("name").asString();
        string pathToKin=rf.findFile("kinematics_file");
 
        Property config;
        config.fromConfigFile(rf.findFile("from"));
        config.put("CustomKinFile",pathToKin);
 
        solver=new fakeRobotCartesianSolver(solverName);
        if (!solver->open(config))
        {    
            delete solver;
            return false;
        }
 
        return true;
    }
 
    /************************************************************************/
    bool interruptModule()
    {
        if (solver!=NULL)
            solver->interrupt();
 
        return true;
    }
 
    /**********************************************************/
    bool close()
    {
        delete solver;
        return true;
    }
 
    /**********************************************************/
    double getPeriod()
    {
        return 1.0;
    }
 
    /**********************************************************/
    bool updateModule()
    {
        if (solver->isClosed() || solver->getTimeoutFlag())
            return false;
        else
            return true;
    }
};
 
/**********************************************************/
int main(int argc, char *argv[])
{
    Network yarp;
    if (!yarp.checkNetwork())
    {
        cout<<"Error: yarp server does not seem available"<<endl;
        return 1;
    }
 
    // register here the new yarp devices
    // for dealing with the fake robot
    registerFakeMotorDevices();
 
    ResourceFinder rf;
    rf.setDefaultConfigFile("solver.ini");
    rf.setDefault("kinematics_file","kinematics.ini");
    rf.configure(argc,argv);
 
    SolverModule solver;
    return solver.runModule(rf);
}