springyFingers.h

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
*/
 
#ifndef __PERCEPTIVEMODELS_SPRINGYFINGERS_H__
#define __PERCEPTIVEMODELS_SPRINGYFINGERS_H__
 
#include <mutex>
 
#include <yarp/os/all.h>
#include <yarp/dev/all.h>
#include <yarp/sig/all.h>
 
#include <iCub/learningMachine/FixedRangeScaler.h>
#include <iCub/learningMachine/LSSVMLearner.h>
#include <iCub/perception/sensors.h>
#include <iCub/perception/nodes.h>
#include <iCub/perception/models.h>
 
 
namespace iCub
{
 
namespace perception
{
 
class SpringyFinger : public Node
{
protected:
    mutable iCub::learningmachine::FixedRangeScaler scaler;
    mutable iCub::learningmachine::LSSVMLearner     lssvm;
    double  calibratingVelocity;
    double  outputGain;
    bool    calibrated;
 
    bool extractSensorsData(yarp::sig::Vector &in, yarp::sig::Vector &out) const;
 
public:
    bool fromProperty(const yarp::os::Property &options);
 
    void toProperty(yarp::os::Property &options) const;
 
    bool toStream(std::ostream &str) const;
 
    bool calibrate(const yarp::os::Property &options);    
 
    bool getSensorsData(yarp::os::Value &data) const;
 
    bool getOutput(yarp::os::Value &out) const;
 
    bool isCalibrated() const
    {
        return calibrated;
    }
 
    void setCalibVel(const double vel)
    {
        calibratingVelocity=vel;
    }
 
    double getCalibVel() const
    {
        return calibratingVelocity;
    }    
};
 
 
class SpringyFingersModel : public virtual Model
{
private:
    std::string type;
    std::string robot;
    std::string carrier;    
 
    SensorEncoders  sensEncs[5];
    SensorPort      sensPort[12];
    SpringyFinger   fingers[5];
    bool configured;
 
    yarp::os::BufferedPort<yarp::os::Bottle> *port;
    yarp::dev::PolyDriver                     driver;
 
    std::mutex mtx;
 
    class CalibThread : public yarp::os::Thread
    {
        SpringyFingersModel *model;
        SpringyFinger       *finger;
        double               min;
        double               max;
        int                  joint;
        bool                 done;
 
    public:
        CalibThread() : model(NULL), done(false) { }
 
        void setInfo(SpringyFingersModel *model, SpringyFinger &finger,
                     const int joint, const double min, const double max)
        {
            this->model=model;
            this->finger=&finger;
            this->joint=joint;
            this->min=min;
            this->max=max;
        }
 
        void run()
        {
            if (!done && (model!=NULL))
            {
                model->calibrateFinger(*finger,joint,min,max);
                done=true;
            }
 
            yarp::os::Time::delay(0.1);
        }
 
        bool isDone() const { return done; }
    };
    friend class CalibThread;
    
    void calibrateFinger(SpringyFinger &finger, const int joint,
                         const double min, const double max);
    void close();
 
public:
    SpringyFingersModel();
 
    bool fromProperty(const yarp::os::Property &options);
 
    void toProperty(yarp::os::Property &options) const;
 
    bool toStream(std::ostream &str) const;
 
    bool calibrate(const yarp::os::Property &options);
 
    bool isCalibrated() const;
 
    bool getOutput(yarp::os::Value &out) const;    
 
    virtual ~SpringyFingersModel();
};
 
 
}
 
}
 
#endif