icubsensors.h

/********************************************************************************
 *  FARSA Experimentes Library                                                  *
 *  Copyright (C) 2007-2012                                                     *
 *  Gianluca Massera <emmegian@yahoo.it>                                        *
 *  Stefano Nolfi <stefano.nolfi@istc.cnr.it>                                   *
 *  Tomassino Ferrauto <tomassino.ferrauto@istc.cnr.it>                         *
 *                                                                              *
 *  This program is free software; you can redistribute it and/or modify        *
 *  it under the terms of the GNU General Public License as published by        *
 *  the Free Software Foundation; either version 2 of the License, or           *
 *  (at your option) any later version.                                         *
 *                                                                              *
 *  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.                                *
 *                                                                              *
 *  You should have received a copy of the GNU General Public License           *
 *  along with this program; if not, write to the Free Software                 *
 *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA  *
 ********************************************************************************/

#ifndef ICUBSENSORS_H
#define ICUBSENSORS_H

#ifdef FARSA_USE_YARP_AND_ICUB

#include "experimentsconfig.h"
#include "neuroninterfaces.h"
#include "projector.h"
#include "world.h"
#include "robots.h"
#include "phybox.h"
#include "phycylinder.h"

namespace farsa {

class MultiMotorController;

class FARSA_EXPERIMENTS_API iCubSensor : public Sensor
{
public:
    iCubSensor(ConfigurationParameters& params, QString prefix);

    virtual ~iCubSensor();

    virtual void save(ConfigurationParameters& params, QString prefix);

    static void describe(QString type);

protected:
    virtual void resourceChanged(QString resourceName, ResourceChangeType changeType);

    QString icubResource;

    QString neuronsIteratorResource;
};

class FARSA_EXPERIMENTS_API iCubArmJointsSensor : public iCubSensor {
public:
    iCubArmJointsSensor( ConfigurationParameters& params, QString prefix );
    ~iCubArmJointsSensor();
    void save( ConfigurationParameters& params, QString prefix );
    static void describe( QString type );
    void update();
    int size();
protected:
    void resourceChanged(QString resourceName, ResourceChangeType changeType);
    QString icubArm;
    MultiMotorController* icubMotors;
};

class FARSA_EXPERIMENTS_API ColorCameraSensor : public iCubSensor {
public:
    ColorCameraSensor(ConfigurationParameters& params, QString prefix);
    void save( ConfigurationParameters& params, QString prefix );
    static void describe( QString type );
    void update();
    int size();
    QMap<WObject*, ImagePoint> m_objectsRetinaPosition;
private:
    void resourceChanged(QString resourceName, ResourceChangeType changeType);

    int nObjects;

    Projector m_projector;
};

class FARSA_EXPERIMENTS_API LinearCameraSensor : public iCubSensor {
public:
    LinearCameraSensor(ConfigurationParameters& params, QString prefix);
    void save( ConfigurationParameters& params, QString prefix );
    static void describe( QString type );
    void update();
    int size();
    QMap<WObject*, ImagePoint> m_objectsRetinaPosition;
private:
    void resourceChanged(QString resourceName, ResourceChangeType changeType);

    int nObjects;
    int nReceptors;
    int projectionType;
    double *receptors; //with size of nReceptors
    double *xcoors;


    Projector m_projector;
};

class FARSA_EXPERIMENTS_API iCubPalmTargetDistSensor : public iCubSensor {
public:
    iCubPalmTargetDistSensor( ConfigurationParameters& params, QString prefix );
    ~iCubPalmTargetDistSensor();
    void save( ConfigurationParameters& params, QString prefix );
    static void describe( QString type );
    void update();
    int size();
protected:
    void resourceChanged(QString resourceName, ResourceChangeType changeType);
    QString icubPalm;
    QString targetName;
    bool isLeft;
    bool linearize;
    wVector bbMin;
    wVector bbMax;
    wVector palmOffset;
    bool addPalmOffset;
};

class FARSA_EXPERIMENTS_API iCubPalmTouchSensor : public iCubSensor {
public:
    iCubPalmTouchSensor( ConfigurationParameters& params, QString prefix );
    ~iCubPalmTouchSensor();
    void save( ConfigurationParameters& params, QString prefix );
    static void describe( QString type );
    void update();
    int size();
protected:
    void resourceChanged(QString resourceName, ResourceChangeType changeType);
    QString icubPalm;
    WObject* wPalm;
    QVector<WObject*>* objects;
};

class FARSA_EXPERIMENTS_API iCubHandTouchSensor : public iCubSensor
{
public:
    iCubHandTouchSensor(ConfigurationParameters& params, QString prefix);

    ~iCubHandTouchSensor();

    virtual void save(ConfigurationParameters& params, QString prefix);

    static void describe(QString type);

    virtual void update();

    virtual int size();

private:
    virtual void resourceChanged(QString resourceName, ResourceChangeType changeType);

    double handPieceCollides(PhyObject* handPiece);

    QString m_icubHand;

    bool m_checkAllObjects;

    World* m_world;

    QVector<PhyObject*> m_icubArm;

    QVector<WObject*> m_objects;

    iCubRobot* m_icub;
};

class FARSA_EXPERIMENTS_API iCubTorsoJointsSensor : public iCubSensor {
public:
    iCubTorsoJointsSensor( ConfigurationParameters& params, QString prefix );
    ~iCubTorsoJointsSensor();
    void save( ConfigurationParameters& params, QString prefix );
    static void describe( QString type );
    void update();
    int size();
protected:
    void resourceChanged(QString resourceName, ResourceChangeType changeType);
    MultiMotorController* icubMotors;
};

class FARSA_EXPERIMENTS_API iCubHeadJointsSensor : public iCubSensor {
public:
    iCubHeadJointsSensor( ConfigurationParameters& params, QString prefix );
    ~iCubHeadJointsSensor();
    void save( ConfigurationParameters& params, QString prefix );
    static void describe( QString type );
    void update();
    int size();
protected:
    void resourceChanged(QString resourceName, ResourceChangeType changeType);
    MultiMotorController* icubMotors;
};

class FARSA_EXPERIMENTS_API iCubPCHeadJointsSensor : public iCubSensor {
public:
    iCubPCHeadJointsSensor( ConfigurationParameters& params, QString prefix );
    ~iCubPCHeadJointsSensor();
    void save( ConfigurationParameters& params, QString prefix );
    static void describe( QString type );
    void update();
    int size();
protected:
    int nNeurons;
    bool headDofs[6];
    double headEncoderValues[6];
    void resourceChanged(QString resourceName, ResourceChangeType changeType);
    MultiMotorController* icubMotors;
    double neuronActivation(int n, int j); //returns the activation of the nth neuron of the population for the jth joint
};

class FARSA_EXPERIMENTS_API iCubHandJointsSensor : public iCubSensor {
public:
    iCubHandJointsSensor( ConfigurationParameters& params, QString prefix );
    ~iCubHandJointsSensor();
    void save( ConfigurationParameters& params, QString prefix );
    static void describe( QString type );
    void update();
    int size();
protected:
    void resourceChanged(QString resourceName, ResourceChangeType changeType);
    QString icubHand;
    MultiMotorController* icubMotors;
};

namespace __PalmAndFingertipTouchSensor_internal {
    class FingertipTouchSensorGraphic;
    class PalmPatchesTouchSensorGraphic;
}

class FARSA_EXPERIMENTS_API iCubFingertipsTouchSensor : public iCubSensor
{
public:
    iCubFingertipsTouchSensor(ConfigurationParameters& params, QString prefix);

    ~iCubFingertipsTouchSensor();

    virtual void save(ConfigurationParameters& params, QString prefix);

    static void describe(QString type);

    virtual void update();

    virtual int size();

private:
    virtual void resourceChanged(QString resourceName, ResourceChangeType changeType);

    double handPieceCollides(PhyObject* handPiece, bool isThumb) const;

    bool goodCollisionPoint(PhyObject* handPiece, const wVector& collisionPoint, bool isThumb) const;

    QString m_icubHand;

    bool m_isRight;

    bool m_checkAllObjects;

    double m_alpha;

    double m_h;

    bool m_drawSensor;

    World* m_world;

    QVector<PhyObject*> m_icubArm;

    QVector<WObject*>* m_objects;

    iCubRobot* m_icub;

    QVector<__PalmAndFingertipTouchSensor_internal::FingertipTouchSensorGraphic*> m_graphicalTouchSensors;
};

class FARSA_EXPERIMENTS_API iCubPalmPatchesTouchSensor : public iCubSensor
{
public:
    struct Triangle
    {
        wVector a;

        wVector b;

        wVector c;
    };

public:
    iCubPalmPatchesTouchSensor(ConfigurationParameters& params, QString prefix);

    ~iCubPalmPatchesTouchSensor();

    virtual void save(ConfigurationParameters& params, QString prefix);

    static void describe(QString type);

    virtual void update();

    virtual int size();

private:
    virtual void resourceChanged(QString resourceName, ResourceChangeType changeType);

    bool pointInPalmTriangle(const wVector& point, const Triangle& triangle) const;

    QString m_icubHand;

    bool m_isRight;

    bool m_checkAllObjects;

    bool m_drawSensor;

    World* m_world;

    QVector<PhyObject*> m_icubArm;

    QVector<WObject*>* m_objects;

    iCubRobot* m_icub;

    PhyBox* m_handPalm;

    QVector<Triangle> m_patches;

    __PalmAndFingertipTouchSensor_internal::PalmPatchesTouchSensorGraphic* m_graphicalTouchSensor;
};

class FARSA_EXPERIMENTS_API HandObjectVisualOffsetSensor : public iCubSensor
{
public:
    HandObjectVisualOffsetSensor(ConfigurationParameters& params, QString prefix);

    ~HandObjectVisualOffsetSensor();

    virtual void save(ConfigurationParameters& params, QString prefix);

    static void describe(QString type);

    virtual void update();

    virtual int size();

private:
    virtual void resourceChanged(QString resourceName, ResourceChangeType changeType);

    QString m_icubHand;

    World* m_world;

    WObject* m_eye;

    WObject* m_hand;

    WObject* m_object;
};

} // end namespace farsa

#endif

#endif