icubsensors.cpp

/********************************************************************************
 *  FARSA Experiments Library                                                   *
 *  Copyright (C) 2007-2012                                                     *
 *  Gianluca Massera <emmegian@yahoo.it>                                        *
 *  Stefano Nolfi <stefano.nolfi@istc.cnr.it>                                   *
 *  Tomassino Ferrauto <tomassino.ferrauto@istc.cnr.it>                         *
 *  Onofrio Gigliotta <onofrio.gigliotta@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  *
 ********************************************************************************/

#ifdef FARSA_USE_YARP_AND_ICUB

#include "icubsensors.h"
#include "configurationhelper.h"
#include "motorcontrollers.h"
#include "logger.h"
#include "graphicalwobject.h"
#include <QStringList>

namespace farsa {

iCubSensor::iCubSensor(ConfigurationParameters& params, QString prefix) :
    Sensor(params, prefix),
    icubResource("robot"),
    neuronsIteratorResource("neuronsIterator")
{
    // Reading parameters
    icubResource = ConfigurationHelper::getString(params, prefix + "icub", icubResource);
    neuronsIteratorResource = ConfigurationHelper::getString(params, prefix + "neuronsIterator", neuronsIteratorResource);

    // Declaring the resources that are needed here
    usableResources(QStringList() << icubResource << neuronsIteratorResource);
}

iCubSensor::~iCubSensor()
{
    // Nothing to do here
}

void iCubSensor::save(ConfigurationParameters& params, QString prefix)
{
    // Calling parent function
    Sensor::save(params, prefix);

    // Saving parameters
    params.startObjectParameters(prefix, "iCubSensor", this);
    params.createParameter(prefix, "icub", icubResource);
    params.createParameter(prefix, "neuronsIterator", neuronsIteratorResource);
}

void iCubSensor::describe(QString type)
{
    // Calling parent function
    Sensor::describe(type);

    // Describing our parameters
    Descriptor d = addTypeDescription(type, "The base class for iCub sensors");
    d.describeString("icub").def("robot").help("the name of the resource associated with the iCub robot to use (default is \"robot\")");
    d.describeString("neuronsIterator").def("neuronsIterator").help("the name of the resource associated with the neural network iterator (default is \"neuronsIterator\")");
}

void iCubSensor::resourceChanged(QString resourceName, ResourceChangeType changeType)
{
    // Calling parent function
    Sensor::resourceChanged(resourceName, changeType);

    // Here we only check whether the resource has been deleted and reset the check flag, the
    // actual work is done in subclasses
    if (changeType == Deleted) {
        resetNeededResourcesCheck();
        return;
    }
}

iCubArmJointsSensor::iCubArmJointsSensor( ConfigurationParameters& params, QString prefix ) :
    iCubSensor(params, prefix),
    icubMotors(NULL) {
    icubArm = ConfigurationHelper::getString( params, prefix+"arm", "right" );
    // Declaring the resources that are needed here
    usableResources( QStringList() << icubResource << neuronsIteratorResource );
}

iCubArmJointsSensor::~iCubArmJointsSensor() {
    /* nothing to do */
}

void iCubArmJointsSensor::save( ConfigurationParameters& params, QString prefix ) {
    iCubSensor::save( params, prefix );
    params.startObjectParameters( prefix, "iCubArmJointsSensor", this );
    params.createParameter( prefix, "arm", icubArm );
}

void iCubArmJointsSensor::describe( QString type ) {
    iCubSensor::describe( type );
    Descriptor d = addTypeDescription( type, "Sensor for reading the joint angles of an iCub arm" );
    d.describeEnum( "arm" ).def( "right" ).values( QStringList()<<"right"<<"left" ).props( IsMandatory ).help( "The arm from which the joint angles are read" );
}

void iCubArmJointsSensor::update() {
    // Checking all resources we need exist
    checkAllNeededResourcesExist();

    // Acquiring the lock to get resources
    ResourcesLocker locker( this );

    QStringList values;
    for( int i=0; i<7; i++ ) {
        double value;
        icubMotors->getEncoder( i, &value );
        values << QString::number( value );
    }
    //exp->setStatus( QString("SENSOR Reading: <")+values.join(", ")+QString(">") );
    NeuronsIterator* evonetIt = getResource<NeuronsIterator>( neuronsIteratorResource );
    evonetIt->setCurrentBlock( name() );
    for( int i=0; i<7; i++, evonetIt->nextNeuron() ) {
        double min, max, value;
        icubMotors->getEncoder(i, &value);
        icubMotors->getLimits(i,&min,&max);
        //normalizziamo i valori dei motori tra 0 ed 1;
        evonetIt->setInput( linearMap(value,min,max,0,1) );
    }
}

int iCubArmJointsSensor::size() {
    return 7;
}

void iCubArmJointsSensor::resourceChanged(QString resourceName, ResourceChangeType changeType) {
    iCubSensor::resourceChanged(resourceName, changeType);

    if (changeType == Deleted) {
        return;
    }

    if (resourceName == icubResource) {
        iCubRobot* icub = getResource<iCubRobot>();
        if ( icubArm == "right" ) {
            icubMotors = icub->rightArmController();
        } else {
            icubMotors = icub->leftArmController();
        }
    } else if (resourceName == neuronsIteratorResource) {
        QString lbl;
        if ( icubArm == "right" ) {
            lbl = "R";
        } else {
            lbl = "L";
        }

        NeuronsIterator* evonetIt = getResource<NeuronsIterator>();
        evonetIt->setCurrentBlock( name() );
        for( int i=0; i<7; i++, evonetIt->nextNeuron() ) {
            evonetIt->setGraphicProperties( lbl + QString("a") + QString::number(i), 0.0, 1.0, Qt::red );
        }
    } else {
        Logger::info("Unknown resource " + resourceName + " for " + name());
    }
}

ColorCameraSensor::ColorCameraSensor(ConfigurationParameters& params, QString prefix) :
    iCubSensor(params, prefix),
    nObjects(3)
{
    nObjects = ConfigurationHelper::getInt(params, prefix + "nObjects", nObjects);
    // Declaring the resources that are needed here
    usableResources( QStringList() << icubResource << neuronsIteratorResource << "objects" );
}

void ColorCameraSensor::save( ConfigurationParameters& params, QString prefix ) {
    iCubSensor::save( params, prefix );
    params.startObjectParameters( prefix, "ColorCameraSensor", this );
    params.createParameter( prefix, "nObjects", QString::number(nObjects) );
}

void ColorCameraSensor::describe( QString type ) {
    iCubSensor::describe( type );
    Descriptor d = addTypeDescription( type, "Color Camera Sensor" );
    d.describeInt( "nObjects" ).def( 3 ).help( "Number of Objects" );
}

// update the camera on the basis of 3 objects that should be defined as red, green, and blue
void ColorCameraSensor::update()
{
    // Checking all resources we need exist
    checkAllNeededResourcesExist();

    // Acquiring the lock to get resources
    ResourcesLocker locker( this );

    QVector<WObject*>& objects = *(getResource<QVector<WObject*> >( "objects" ));
    iCubRobot* icub = getResource<iCubRobot>(icubResource);
    // Setting the eye matrix in the projector
    m_projector.setEyeMatrix(icub->headNeck()[4]->matrix());

    NeuronsIterator* evonetIt = getResource<NeuronsIterator>(neuronsIteratorResource);
    evonetIt->setCurrentBlock( name() );
    // Activating the three parts of the map: Red...
    for(int i = 0; i < nObjects; i++) {
        // Checking we don't try to access unexisting objects
        if (i >= objects.size()) {
            // Filling with 0.5
            // up-down
            evonetIt->setInput( 0.5 );
            evonetIt->nextNeuron();
            // right-left
            evonetIt->setInput( 0.5 );
            evonetIt->nextNeuron();
            continue;
        }

        // Computing the projection of the object on the retina
        m_projector.set3DPointWorld(objects[i]->matrix().w_pos);

        // If the object is within the retina, activating the map
        if (m_projector.pointInsideImage())
        {
            // mapPoint01.x.y = distance bteween the barycentre of the object and the border of the
            // field of view normalized in the range [0.0,1.0]
            const ImagePoint mapPoint01 = m_projector.getImagePoint01();

            // up-down
            //mapPoint01.y = distance normalized tra 0-1 baricentro oggetto e bord fieldofview
            evonetIt->setInput( 0.5 - mapPoint01.y );
            evonetIt->nextNeuron();
            // right-left
            evonetIt->setInput( 0.5 - mapPoint01.x );
            evonetIt->nextNeuron();

            // Storing the position on the retina of the current object
            m_objectsRetinaPosition.insert(objects[i],mapPoint01);
        } else {
            evonetIt->setInput( 0.0 );
            evonetIt->nextNeuron();
            evonetIt->setInput( 0.0 );
            evonetIt->nextNeuron();
            // Storing an invalid point for the current object
            m_objectsRetinaPosition.insert(objects[i], ImagePoint());
        }
    }
}

int ColorCameraSensor::size()
{
    return nObjects*2;
}

void ColorCameraSensor::resourceChanged(QString resourceName, ResourceChangeType changeType)
{
    iCubSensor::resourceChanged(resourceName, changeType);

    if (changeType == Deleted) {
        return;
    }

    if (resourceName == icubResource) {
        // Nothing to do here, we get the robot using getResource() in update()
    } else if (resourceName == neuronsIteratorResource) {
        QString lbl[2];
        lbl[0] = "H"; //horizontal
        lbl[1] = "V"; //vertical
        NeuronsIterator* evonetIt = getResource<NeuronsIterator>();
        evonetIt->setCurrentBlock( name() );
        for (int obj = 0; obj < nObjects; obj++) {
            for (int i=0; i < 2; i++, evonetIt->nextNeuron()) {
                QString label;
                switch (obj) {
                    case 0:
                        label = QString("Cr")+ lbl[i];//QString::number(i);
                        break;
                    case 1:
                        label = QString("Cg")+lbl[i];//QString::number(i);
                        break;
                    case 2:
                        label = QString("Cb")+lbl[i];//QString::number(i);
                        break;
                    default:
                        label = QString::number(obj)+QString::number(i);
                        break;
                }
                evonetIt->setGraphicProperties( label, -1.0, 1.0, Qt::red );
            }
        }
    } else if (resourceName == "objects") {
        // Nothing to do here, we get objects using getResource() in update()
    } else {
        Logger::info("Unknown resource " + resourceName + " for " + name());
    }
}

//iCubPalmTargetDistSensor : begin implementation
// it returns the distance between right or left palm and a defined target
iCubPalmTargetDistSensor::iCubPalmTargetDistSensor(ConfigurationParameters &params, QString prefix) :
    iCubSensor(params, prefix) {
    icubPalm = ConfigurationHelper::getString( params, prefix+"palm", "right" );
    targetName= ConfigurationHelper::getString( params, prefix+"target", "target" );
    QVector<double> vec1 = ConfigurationHelper::getVector( params, prefix+"bbMin" );
    QVector<double> vec2 = ConfigurationHelper::getVector( params, prefix+"bbMax" );
    if ( vec1.size() == 3 && vec2.size() == 3 ) {
        linearize = true;
        bbMin = wVector( vec1[0], vec1[1], vec1[2] );
        bbMax = wVector( vec2[0], vec2[1], vec2[2] );
    } else {
        linearize = false;
        if ( ! (vec1.isEmpty() && vec2.isEmpty()) ) {
            Logger::warning( QString("iCubPalmTargetDistSensor %1 - bbMin and/or bbMax parameters are not well specified; they will be ignored").arg(name()) );
        }
    }
    
    QVector<double> pal1 = ConfigurationHelper::getVector( params, prefix+"palmOffset" );
    if ( pal1.size() == 3 ) {
        addPalmOffset = true;
        palmOffset = wVector( pal1[0], pal1[1], pal1[2] );
    } else {
        addPalmOffset = false;
        if ( !pal1.isEmpty() ) {
            Logger::warning( QString("iCubPalmTargetDistSensor %1 - palmOffset parameter is not well specified; It will be ignored").arg(name()) );
        }
    }

    // Declaring the resources that are needed here
    usableResources( QStringList() << icubResource << targetName << neuronsIteratorResource );
}

iCubPalmTargetDistSensor::~iCubPalmTargetDistSensor() {
    /* nothing to do */
}

void iCubPalmTargetDistSensor::save(ConfigurationParameters &params, QString prefix)
{
    iCubSensor::save( params, prefix );
    params.startObjectParameters( prefix, "iCubPalmTargetDistSensor", this );
    params.createParameter( prefix, "palm", icubPalm);
    params.createParameter( prefix, "target", targetName);
    if ( linearize ) {
        params.createParameter( prefix, "bbMin", QString("%1 %2 %3").arg(bbMin[0]).arg(bbMin[1]).arg(bbMin[2]) );
        params.createParameter( prefix, "bbMax", QString("%1 %2 %3").arg(bbMax[0]).arg(bbMax[1]).arg(bbMax[2]) );
    }
}

void iCubPalmTargetDistSensor::describe( QString type ) {
    iCubSensor::describe( type );
    Descriptor d = addTypeDescription( type, "Sensor for reading the distance between right or left palm and a specified target" );
    d.describeEnum( "palm" ).def( "right" ).values( QStringList()<<"right"<<"left" ).props( IsMandatory ).help( "The palm from which the distance to the target is computed" );
    d.describeString( "target" ).def( "target" ).help( "The name of the resource associated with the target object" );
    d.describeReal( "bbMin" ).props( IsList ).help( "The minimum 3D point used for linearize the object position into [-1,1]" );
    d.describeReal( "bbMax" ).props( IsList ).help( "The maximum 3D point used for linearize the object position into [-1,1]" );
    d.describeReal( "palmOffset" ).props( IsList ).help( "The offset respect to the palm on which the distance will be computed" );
}

void iCubPalmTargetDistSensor::update() {
    // Checking all resources we need exist
    checkAllNeededResourcesExist();

    // Acquiring the lock to get resources
    ResourcesLocker locker( this );

    iCubRobot* icub = getResource<iCubRobot>( icubResource );
    WObject* target = getResource<WObject>( targetName );
    wVector targetPosInICub = icub->matrix().untransformVector( target->matrix().w_pos );
    wVector palmPosInICub;
    if ( isLeft ) {
        wMatrix t2 = icub->leftArm()[6]->matrix();
        if ( addPalmOffset ) {
            t2.w_pos += t2.rotateVector( palmOffset );
        }
        palmPosInICub = icub->matrix().untransformVector( t2.w_pos );
    } else {
        wMatrix t2 = icub->rightArm()[6]->matrix();
        if ( addPalmOffset ) {
            t2.w_pos += t2.rotateVector( palmOffset );
        }
        palmPosInICub = icub->matrix().untransformVector( t2.w_pos );
    }

    wVector distanceVec = palmPosInICub - targetPosInICub;
    NeuronsIterator* evonetIt = getResource<NeuronsIterator>( neuronsIteratorResource );
    evonetIt->setCurrentBlock( name() );
    for( int i=0; i<3; i++ ) {
        if ( linearize ) {
            // linearize into [-1,1]
            evonetIt->setInput( linearMap( distanceVec[i], bbMin[i], bbMax[i], -1, 1 ) );
        } else {
            evonetIt->setInput( distanceVec[i] );
        }
        evonetIt->nextNeuron();
    }
}

int iCubPalmTargetDistSensor::size() {
    return 3;
}

void iCubPalmTargetDistSensor::resourceChanged(QString resourceName, ResourceChangeType changeType) {
    iCubSensor::resourceChanged(resourceName, changeType);

    if (changeType == Deleted) {
        return;
    }

    if (resourceName == icubResource) {
        // Nothing to do here, we get the robot using getResource() in update()
    } else if (resourceName == neuronsIteratorResource) {
        QString lbl;
        if ( icubPalm == "right" ) {
            lbl="R";
            isLeft = false;
        } else {
            lbl="L";
            isLeft = true;
        }

        NeuronsIterator* evonetIt = getResource<NeuronsIterator>();
        evonetIt->setCurrentBlock( name() );
        for( int i=0; i<3; i++, evonetIt->nextNeuron() ) {
            evonetIt->setGraphicProperties( lbl+QString("d")+QString::number(i), -1.0, 1.0, Qt::red );
        }
    } else if (resourceName == targetName) {
        // Nothing to do here, we get the taget using getResource() in update()
    } else {
        Logger::info("Unknown resource " + resourceName + " for " + name());
    }
}

//iCubPalmTargetDistSensor : end implementation

// iCubPalmTouchSEnsor begin implementation
iCubPalmTouchSensor::iCubPalmTouchSensor(ConfigurationParameters &params, QString prefix) :
    iCubSensor(params, prefix) {
    wPalm=NULL;
    objects=NULL;
    icubPalm = ConfigurationHelper::getString( params, prefix+"palm", "right" );
    // Declaring the resources that are needed here
    usableResources( QStringList() << icubResource << "world" << "objects" << neuronsIteratorResource );
}

void iCubPalmTouchSensor::describe( QString type ) {
    iCubSensor::describe( type );
    Descriptor d = addTypeDescription( type, "Sensor for reading right or left palm touch sensor" );
    d.describeEnum( "palm" ).def( "right" ).values( QStringList()<<"right"<<"left" ).props( IsMandatory ).help( "The palm from which the touch sensor is read" );
    
}
void iCubPalmTouchSensor::update() {
    // Checking all resources we need exist
    checkAllNeededResourcesExist();

    // Acquiring the lock to get resources
    ResourcesLocker locker( this );

    double touch=0.0;
    //we should put following instruction in the resourceChanged method
    World *world = getResource<World>("world");
    if(objects!=NULL) {
        for(int i=0;i<objects->size();i++)
        {
            if(world->checkContacts((PhyObject*)wPalm,(PhyObject*)objects->at(i)))
                touch=1.0;
        }
    }
    NeuronsIterator* evonetIt = getResource<NeuronsIterator>( neuronsIteratorResource );
    evonetIt->setCurrentBlock( name() );
    
    for( int i=0; i<size(); i++, evonetIt->nextNeuron() ) {
        evonetIt->setInput(touch);
    }
}

int iCubPalmTouchSensor::size() {
    return 1;
}

iCubPalmTouchSensor::~iCubPalmTouchSensor()
{}

void iCubPalmTouchSensor::save(ConfigurationParameters &params, QString prefix)
{
    iCubSensor::save( params, prefix );
    params.startObjectParameters( prefix, "iCubPalmTouchSensor", this );
    params.createParameter( prefix, "palm", icubPalm);
    
}

void iCubPalmTouchSensor::resourceChanged(QString resourceName, ResourceChangeType changeType) {
    iCubSensor::resourceChanged(resourceName, changeType);

    if (changeType == Deleted) {
        return;
    }

    if (resourceName == icubResource) {
        iCubRobot *icub = getResource<iCubRobot>();

        if ( icubPalm == "right" ) {
            wPalm = icub->rightArm()[6];
        } else {
            wPalm = icub->leftArm()[6];
        }
    } else if (resourceName == "world") {
        // Nothing to do here
    } else if (resourceName == "objects") {
        objects = getResource<QVector<WObject*> >();
    } else if (resourceName == neuronsIteratorResource) {
        NeuronsIterator* evonetIt = getResource<NeuronsIterator>();
        evonetIt->setCurrentBlock( name() );
        for( int i=0; i<1; i++, evonetIt->nextNeuron() ) {
            if(icubPalm=="right") {
                evonetIt->setGraphicProperties( QString("Rpt"), 0, 1, Qt::red );
            } else {
                evonetIt->setGraphicProperties( QString("Lpt"), 0, 1, Qt::red );
            }
        }
    } else {
        Logger::info("Unknown resource " + resourceName + " for " + name());
    }
}

// iCubPalmTouchSEnsor end implementation

iCubHandTouchSensor::iCubHandTouchSensor(ConfigurationParameters& params, QString prefix) :
    iCubSensor(params, prefix),
    m_icubHand("right"),
    m_checkAllObjects(true),
    m_world(NULL),
    m_icubArm(),
    m_objects(),
    m_icub(NULL)
{
    m_icubHand = ConfigurationHelper::getString(params, prefix + "hand", "right");
    m_checkAllObjects = ConfigurationHelper::getBool(params, prefix + "checkAllObjects", true);
    // Declaring the resources that are needed here
    usableResources( QStringList() << icubResource << "world" << "objects" << neuronsIteratorResource );

}

iCubHandTouchSensor::~iCubHandTouchSensor()
{
}

void iCubHandTouchSensor::save(ConfigurationParameters& params, QString prefix)
{
    iCubSensor::save( params, prefix );
    params.startObjectParameters(prefix, "iCubHandTouchSensor", this);
    params.createParameter(prefix, "hand", m_icubHand);
    params.createParameter(prefix, "checkAllObjects", m_checkAllObjects ? QString("true") : QString("false"));
}

void iCubHandTouchSensor::describe(QString type)
{
    iCubSensor::describe( type );
    Descriptor d = addTypeDescription(type, "Hand touch sensor", "The touch sensor of the iCub hand. There are six sensors: one on the palm and one for each figertip");
    d.describeEnum("hand").def("right").values(QStringList() << "right" << "left").props(IsMandatory).help("The hand to use", "The hand whose distance from the object should be returned. Choose between \"right\" and \"left\"");
    d.describeBool("checkAllObjects").def(true).help("Wheter to check collisions with all objects or not", "If true, the collision of the hand touch sensors with all objects in the world is checked, otherwise only those in the objects vector are taken into account. Note that if the iCub is kinematic, only collisions with objects in the objects vector are checked regardless of the value of this parameter.");
}

void iCubHandTouchSensor::update()
{
    // Checking all resources we need exist
    checkAllNeededResourcesExist();

    // Acquiring the lock to get resources
    ResourcesLocker locker( this );

    m_objects = *(getResource<QVector<WObject*> >( "objects" ));

    NeuronsIterator* evonetIt = getResource<NeuronsIterator>( neuronsIteratorResource );
    evonetIt->setCurrentBlock(name());

    evonetIt->setInput(handPieceCollides(m_icubArm[6])); // Palm
    evonetIt->nextNeuron();
    evonetIt->setInput(handPieceCollides(m_icubArm[19])); // Index
    evonetIt->nextNeuron();
    evonetIt->setInput(handPieceCollides(m_icubArm[20])); // Middle
    evonetIt->nextNeuron();
    evonetIt->setInput(handPieceCollides(m_icubArm[21])); // Ring
    evonetIt->nextNeuron();
    evonetIt->setInput(handPieceCollides(m_icubArm[22])); // Little
    evonetIt->nextNeuron();
    evonetIt->setInput(handPieceCollides(m_icubArm[26])); // Thumb
    evonetIt->nextNeuron();
}

int iCubHandTouchSensor::size()
{
    return 6;
}

void iCubHandTouchSensor::resourceChanged(QString resourceName, ResourceChangeType changeType)
{
    iCubSensor::resourceChanged(resourceName, changeType);

    if (changeType == Deleted) {
        return;
    }

    if (resourceName == icubResource) {
        m_icub = getResource<iCubRobot>();
        if (m_icubHand == "left") {
            m_icubArm = m_icub->leftArm();
        } else {
            m_icubArm = m_icub->rightArm();
        }
    } else if (resourceName == "world") {
        m_world = getResource<World>();
    } else if (resourceName == "objects") {
        // Nothing to do here, we get objects using getResource() in update()
    } else if (resourceName == neuronsIteratorResource) {
        QString lbl;
        if (m_icubHand == "left") {
            lbl = "L";
        } else {
            lbl = "R";
        }

        NeuronsIterator* evonetIt = getResource<NeuronsIterator>();
        evonetIt->setCurrentBlock(name());
        evonetIt->setGraphicProperties(lbl + "pt", 0.0, 1.0, Qt::red);
        evonetIt->nextNeuron();
        evonetIt->setGraphicProperties(lbl + "f1", 0.0, 1.0, Qt::red);
        evonetIt->nextNeuron();
        evonetIt->setGraphicProperties(lbl + "f2", 0.0, 1.0, Qt::red);
        evonetIt->nextNeuron();
        evonetIt->setGraphicProperties(lbl + "f3", 0.0, 1.0, Qt::red);
        evonetIt->nextNeuron();
        evonetIt->setGraphicProperties(lbl + "f4", 0.0, 1.0, Qt::red);
        evonetIt->nextNeuron();
        evonetIt->setGraphicProperties(lbl + "f5", 0.0, 1.0, Qt::red);
    } else {
        Logger::info("Unknown resource " + resourceName + " for " + name());
    }
}

double iCubHandTouchSensor::handPieceCollides(PhyObject* handPiece)
{
    if (m_icub->isKinematic()) {
        for (int i = 0; i < m_objects.size(); i++) {
            PhyObject* obj = dynamic_cast<PhyObject*>(m_objects[i]);
            if ((obj != NULL) && (m_world->checkContacts(handPiece, (PhyObject*) m_objects[i]))) {
                return 1.0;
            }
        }

        return 0.0;
    } else {
        // Taking the vector of contacts. If no contact is present, this returns an empty vector
        const contactVec& c = m_world->contacts()[handPiece];

        if (c.size() == 0) {
            return 0.0;
        } else if (m_checkAllObjects) {
            return 1.0;
        } else {
            for (int i = 0; i < m_objects.size(); i++) {
                for (int j = 0; j < c.size(); j++) {
                    if (c[j].collide == m_objects[i]) {
                        return 1.0;
                    }
                }
            }

            return 0.0;
        }
    }

    return 0.0;
}

// iCubTorsoJointsSensor begin implementation
iCubTorsoJointsSensor::iCubTorsoJointsSensor(ConfigurationParameters &params, QString prefix) :
    iCubSensor(params, prefix) {
    // Declaring the resources that are needed here
    usableResources( QStringList() << icubResource << neuronsIteratorResource );
}

void iCubTorsoJointsSensor::describe( QString type ) {
    iCubSensor::describe( type );
    Descriptor d = addTypeDescription( type, "Sensor for reading right or left palm touch sensor" );
    
}

void iCubTorsoJointsSensor::update() {
    // Checking all resources we need exist
    checkAllNeededResourcesExist();

    // Acquiring the lock to get resources
    ResourcesLocker locker( this );

    double minRot, maxRot;
    double minFlex, maxFlex;
    double curRot;
    double curFlex;
    
    icubMotors->getLimits(0, &minRot, &maxRot);
    icubMotors->getLimits(2, &minFlex, &maxFlex);
    icubMotors->getEncoder(0, &curRot);
    icubMotors->getEncoder(2, &curFlex);

    NeuronsIterator* evonetIt = getResource<NeuronsIterator>( neuronsIteratorResource );
    evonetIt->setCurrentBlock( name() );
    evonetIt->setInput(((curRot - minRot) / (maxRot - minRot)) * 2.0 - 1.0);
    evonetIt->nextNeuron();
    evonetIt->setInput((curFlex - minFlex) / (maxFlex - minFlex));
    
}

int iCubTorsoJointsSensor::size() {
    return 2;
}

void iCubTorsoJointsSensor::resourceChanged(QString resourceName, ResourceChangeType changeType) {
    iCubSensor::resourceChanged(resourceName, changeType);

    if (changeType == Deleted) {
        return;
    }

    if (resourceName == icubResource) {
        iCubRobot* icub = getResource<iCubRobot>();
        icubMotors = icub->torsoController();
    } else if (resourceName == neuronsIteratorResource) {
        NeuronsIterator* evonetIt = getResource<NeuronsIterator>();
        evonetIt->setCurrentBlock( name() );

        evonetIt->setGraphicProperties( QString("t0"), 0, 1, Qt::red ); //rotation
        evonetIt->nextNeuron();
        evonetIt->setGraphicProperties( QString("t1"), 0, 1, Qt::red ); //flexion
        evonetIt->nextNeuron();
    } else {
        Logger::info("Unknown resource " + resourceName + " for " + name());
    }
}

iCubTorsoJointsSensor::~iCubTorsoJointsSensor()
{}

void iCubTorsoJointsSensor::save(ConfigurationParameters &params, QString prefix)
{
    iCubSensor::save( params, prefix );
    params.startObjectParameters( prefix, "iCubTorsoJointsSensor", this );
    
    
}
// iCubTorsoJointsSensor end implementation

// iCubHeadJointsSensor begin implementation
iCubHeadJointsSensor::iCubHeadJointsSensor(ConfigurationParameters &params, QString prefix) :
    iCubSensor(params, prefix) {
    // Declaring the resources that are needed here
    usableResources( QStringList() << icubResource << neuronsIteratorResource );
}

iCubHeadJointsSensor::~iCubHeadJointsSensor()
{}

void iCubHeadJointsSensor::save(ConfigurationParameters &params, QString prefix)
{
    iCubSensor::save( params, prefix );
    params.startObjectParameters( prefix, "iCubHeadJointsSensor", this );
}

void iCubHeadJointsSensor::describe( QString type ) {
    iCubSensor::describe( type );
    Descriptor d = addTypeDescription( type, "Sensor for reading head sensors" );
    
}

void iCubHeadJointsSensor::update() {
    // Checking all resources we need exist
    checkAllNeededResourcesExist();

    // Acquiring the lock to get resources
    ResourcesLocker locker( this );

    double minRot, maxRot;
    double minFlex, maxFlex;
    double curRot;
    double curFlex;
    
    icubMotors->getLimits(0, &minRot, &maxRot);
    icubMotors->getLimits(2, &minFlex, &maxFlex);
    icubMotors->getEncoder(0, &curRot);
    icubMotors->getEncoder(2, &curFlex);

    NeuronsIterator* evonetIt = getResource<NeuronsIterator>( neuronsIteratorResource );
    evonetIt->setCurrentBlock( name() );
    evonetIt->setInput(((curRot - minRot) / (maxRot - minRot)) );
    evonetIt->nextNeuron();
    evonetIt->setInput((curFlex - minFlex) / (maxFlex - minFlex));
    
}

int iCubHeadJointsSensor::size() {
    return 2;
}

void iCubHeadJointsSensor::resourceChanged(QString resourceName, ResourceChangeType changeType) {
    iCubSensor::resourceChanged(resourceName, changeType);

    if (changeType == Deleted) {
        return;
    }

    if (resourceName == icubResource) {
        iCubRobot* icub = getResource<iCubRobot>();
        icubMotors = icub->headNeckController();
    } else if (resourceName == neuronsIteratorResource) {
        NeuronsIterator* evonetIt = getResource<NeuronsIterator>();
        evonetIt->setCurrentBlock( name() );

        evonetIt->setGraphicProperties( QString("n0"), 0, 1, Qt::red ); //n stands for neck
        evonetIt->nextNeuron();
        evonetIt->setGraphicProperties( QString("n1"), 0, 1, Qt::red );
        evonetIt->nextNeuron();
    } else {
        Logger::info("Unknown resource " + resourceName + " for " + name());
    }
}

// iCubHeadJointsSensor end implementation

// end iCubHandJointsSensor
iCubHandJointsSensor::iCubHandJointsSensor( ConfigurationParameters& params, QString prefix ) :
    iCubSensor(params, prefix),
    icubMotors(NULL) {
    icubHand = ConfigurationHelper::getString( params, prefix+"hand", "right" );
    // Declaring the resources that are needed here
    usableResources( QStringList() << icubResource << neuronsIteratorResource );
}

iCubHandJointsSensor::~iCubHandJointsSensor() {
    /* nothing to do */
}

void iCubHandJointsSensor::save( ConfigurationParameters& params, QString prefix ) {
    iCubSensor::save( params, prefix );
    params.startObjectParameters( prefix, "iCubHandJointsSensor", this );
    params.createParameter( prefix, "hand", icubHand );
}

void iCubHandJointsSensor::describe( QString type ) {
    iCubSensor::describe( type );
    Descriptor d = addTypeDescription( type, "Sensor for reading the joint angles of an iCub hand" );
    d.describeEnum( "hand" ).def( "right" ).values( QStringList()<<"right"<<"left" ).props( IsMandatory ).help( "The hand from which the joint angles are read" );
}

void iCubHandJointsSensor::update() {
    // Checking all resources we need exist
    checkAllNeededResourcesExist();

    // Acquiring the lock to get resources
    ResourcesLocker locker( this );

//  QStringList values;
//  for( int i=9; i<16; i++ ) {
//      double value;
//      icubMotors->getEncoder( i, &value );
//      values << QString::number( value );
//  }
//  exp->setStatus( QString("SENSOR Reading: <")+values.join(", ")+QString(">") );
    NeuronsIterator* evonetIt = getResource<NeuronsIterator>( neuronsIteratorResource );
    evonetIt->setCurrentBlock( name() );
    for( int i=9; i<16; i++, evonetIt->nextNeuron() ) {
        double min, max, value;
        icubMotors->getEncoder(i, &value);
        icubMotors->getLimits(i,&min,&max);
        //normalizziamo i valori dei motori tra 0 ed 1;
        evonetIt->setInput( linearMap(value,min,max,0,1) );
    }
}

int iCubHandJointsSensor::size() {
    return 7;
}

void iCubHandJointsSensor::resourceChanged(QString resourceName, ResourceChangeType changeType) {
    iCubSensor::resourceChanged(resourceName, changeType);

    if (changeType == Deleted) {
        return;
    }

    if (resourceName == icubResource) {
        iCubRobot* icub = getResource<iCubRobot>();
        if ( icubHand == "right" ) {
            icubMotors = icub->rightArmController();
        } else {
            icubMotors = icub->leftArmController();
        }
    } else if (resourceName == neuronsIteratorResource) {
        QString label;
        if ( icubHand == "right" ) {
            label="R";
        } else {
            label="L";
        }

        NeuronsIterator* evonetIt = getResource<NeuronsIterator>();
        evonetIt->setCurrentBlock( name() );
        for( int i=0; i<7; i++, evonetIt->nextNeuron() ) {
            evonetIt->setGraphicProperties( label+QString("f")+QString::number(i), 0.0, 1.0, Qt::red ); //f stands for fingers
        }
    } else {
        Logger::info("Unknown resource " + resourceName + " for " + name());
    }
}
// end iCubHandJointsSensor

namespace __PalmAndFingertipTouchSensor_internal {
    #ifndef GLMultMatrix
    #define GLMultMatrix glMultMatrixf
    // for double glMultMatrixd
    #endif

    const float epsilon = 0.0001f;

    const float numDivisionsFor90Degrees = 5.0f;

    const int maxNumContacts = 20;

    class FingertipTouchSensorGraphic : public GraphicalWObject
    {
    public:
        FingertipTouchSensorGraphic(PhyObject *handPiece, double alpha, double h, bool isRight, bool isThumb, QString name = "unamed") :
            GraphicalWObject(handPiece->world(), name),
            m_handPiece(dynamic_cast<PhyCylinder*>(handPiece)),
            m_handPieceHeight(m_handPiece->height()),
            m_handPieceRadius(m_handPiece->radius()),
            m_alpha(alpha),
            m_h(h),
            m_alphaOffset(computeAlphaOffset(isRight, isThumb)),
            m_angularIncrement(m_alpha / (ceil(m_alpha / (M_PI / 2.0)) * numDivisionsFor90Degrees)),
            m_startingAngle((-m_alpha / 2.0) + m_alphaOffset),
            m_endingAngle((m_alpha / 2.0) + m_alphaOffset),
            m_isActive(false),
            m_isActiveMutex()
        {
            // Attaching to handPiece (which also becomes our owner)
            attachToObject(m_handPiece, true);

            // We also use our own color and texture
            setUseColorTextureOfOwner(false);
            setTexture("");
            setColor(Qt::cyan);
        }

        ~FingertipTouchSensorGraphic()
        {
        }

        void setActive(bool isActive)
        {
            m_isActiveMutex.lock();
                m_isActive = isActive;
            m_isActiveMutex.unlock();
        }

    protected:
        virtual void render(RenderWObject* renderer, QGLContext* gw)
        {
            // First of all changing our color depending on the value of m_isActive
            m_isActiveMutex.lock();
                if (m_isActive) {
                    setColor(Qt::red);
                } else {
                    setColor(Qt::cyan);
                }
            m_isActiveMutex.unlock();

            // Bringing the coordinate system on the fingerip
            wMatrix mtr = tm;
            mtr.w_pos += mtr.x_ax.scale(m_handPieceHeight / 2.0);

            glPushMatrix();
            renderer->container()->setupColorTexture(gw, renderer);
            GLMultMatrix(&mtr[0][0]);

            // Drawing the top part of the sensor
            glBegin(GL_TRIANGLES);

            // All normals here are along the x axis. All triangles have a vertex on
            // the axis of the cylinder
            const float adjustedRadius = m_handPieceRadius + epsilon;
            for (float angle = m_startingAngle; angle < m_endingAngle; angle += m_angularIncrement) {
                // Computing the next angle (we have to do this to avoid numerical errors)
                const float nextAngle = angle + m_angularIncrement;
                const float effectiveNextAngle = ((nextAngle > m_endingAngle) ? m_endingAngle : nextAngle);
                glNormal3f(1.0, 0.0, 0.0);
                glVertex3f(epsilon, 0.0, 0.0);
                glVertex3f(epsilon, adjustedRadius * sin(angle), adjustedRadius * cos(angle));
                glVertex3f(epsilon, adjustedRadius * sin(effectiveNextAngle), adjustedRadius * cos(effectiveNextAngle));
            }
            glEnd();

            // Now drawing the remaining part
            glBegin(GL_QUADS);

            // Here we have to compute the right normal for each face
            for (float angle = m_startingAngle; angle < m_endingAngle; angle += m_angularIncrement) {
                // Computing the next angle (we have to do this to avoid numerical errors)
                const float nextAngle = angle + m_angularIncrement;
                const float effectiveNextAngle = ((nextAngle > m_endingAngle) ? m_endingAngle : nextAngle);
                // To compute the normal we take two vectors along two adiacent sides of the quad, compute the cross
                // product and then normalize it (the product order is important, of course)
                const wVector v1(0.0, sin(angle) - sin(angle + m_angularIncrement), cos(angle) - cos(angle + m_angularIncrement));
                const wVector v2(1.0, 0.0, 0.0);
                const wVector normal = (v1 * v2).normalize();
                glNormal3f(normal.x, normal.y, normal.z);

                glVertex3f(epsilon, adjustedRadius * sin(angle), adjustedRadius * cos(angle));
                glVertex3f(epsilon, adjustedRadius * sin(effectiveNextAngle), adjustedRadius * cos(effectiveNextAngle));
                glVertex3f(-m_h, adjustedRadius * sin(effectiveNextAngle), adjustedRadius * cos(effectiveNextAngle));
                glVertex3f(-m_h, adjustedRadius * sin(angle), adjustedRadius * cos(angle));
            }
            glEnd();
            glPopMatrix();
        }

        static float computeAlphaOffset(bool isRight, bool isThumb) {
            // The angle we use when drawing is respect to the z axis on the yz plane. Here we compute
            // the offsets needed in different cases (see the comment in the function
            // iCubFingertipsTouchSensor::goodCollisionPoint for more information about the frame of
            // references of the various fingers in the two hands)
            float offset = 0.0;
            if (isRight) {
                offset = isThumb ? M_PI / 2.0 : 0.0;
            } else {
                offset = isThumb ? M_PI / 2.0 : M_PI;
            }

            return offset;
        }

        PhyCylinder *const m_handPiece;

        const real m_handPieceHeight;

        const real m_handPieceRadius;

        const float m_alpha;

        const float m_h;

        const float m_alphaOffset;

        const float m_angularIncrement;

        const float m_startingAngle;

        const float m_endingAngle;

        bool m_isActive;

        QMutex m_isActiveMutex;
    };

    class PalmPatchesTouchSensorGraphic : public GraphicalWObject
    {
    public:
        PalmPatchesTouchSensorGraphic(PhyObject *handPalm, const QVector<iCubPalmPatchesTouchSensor::Triangle>& patches, bool isRight, QString name = "unamed") :
            GraphicalWObject(handPalm->world(), name),
            m_handPalm(dynamic_cast<PhyBox*>(handPalm)),
            m_patches(patches),
            m_isRight(isRight),
            m_zAxisDirection(isRight ? 1.0 : -1.0),
            m_activations(m_patches.size(), false),
            m_activationsMutex()
        {
            // Attaching to handPalm (which also becomes our owner)
            attachToObject(m_handPalm, true);

            // We also use our own color and texture
            setUseColorTextureOfOwner(false);
            setTexture("");
            setColor(Qt::cyan);
        }

        ~PalmPatchesTouchSensorGraphic()
        {
        }

        void setActivations(const QVector<bool> activations)
        {
            m_activationsMutex.lock();
                m_activations = activations;
            m_activationsMutex.unlock();
        }

    protected:
        virtual void render(RenderWObject* renderer, QGLContext* gw)
        {
            // Copying the m_activations vector to a local vector to avoid concurrent accesses
            m_activationsMutex.lock();
                const QVector<bool> activations(m_activations);
            m_activationsMutex.unlock();

            // We receive the list of triangles from the sensor, we just need to display them
            glPushMatrix();
            renderer->container()->setupColorTexture(gw, renderer);
            GLMultMatrix(&tm[0][0]);

            // First drawing the triangles making up the sensor
            glBegin(GL_TRIANGLES);
            glNormal3f(0.0, 0.0, m_zAxisDirection);
            for (int i = 0; i < m_patches.size(); i++) {
                const iCubPalmPatchesTouchSensor::Triangle& t = m_patches[i];

                QColor col;
                if (activations[i]) {
                    col = Qt::red;
                } else {
                    col = Qt::cyan;
                }
                glColor4f(col.redF(), col.greenF(), col.blueF(), col.alphaF());

                glVertex3f(t.a.x, t.a.y, t.a.z + m_zAxisDirection * epsilon);
                glVertex3f(t.b.x, t.b.y, t.b.z + m_zAxisDirection * epsilon);
                glVertex3f(t.c.x, t.c.y, t.c.z + m_zAxisDirection * epsilon);
            }
            glEnd();

            // Now drawing the lines separating the triangles. Using the for we draw some line twice,
            // it's not a big problem
            glBegin(GL_LINES);
            glNormal3f(0.0, 0.0, m_zAxisDirection);
            glColor4f(0.0, 0.0, 0.0, 1.0);
            for (int i = 0; i < m_patches.size(); i++) {
                const iCubPalmPatchesTouchSensor::Triangle& t = m_patches[i];

                glVertex3f(t.a.x, t.a.y, t.a.z + m_zAxisDirection * epsilon);
                glVertex3f(t.b.x, t.b.y, t.b.z + m_zAxisDirection * epsilon);

                glVertex3f(t.b.x, t.b.y, t.b.z + m_zAxisDirection * epsilon);
                glVertex3f(t.c.x, t.c.y, t.c.z + m_zAxisDirection * epsilon);

                glVertex3f(t.c.x, t.c.y, t.c.z + m_zAxisDirection * epsilon);
                glVertex3f(t.a.x, t.a.y, t.a.z + m_zAxisDirection * epsilon);
            }
            glEnd();
            glPopMatrix();
        }

        PhyBox *const m_handPalm;

        const QVector<iCubPalmPatchesTouchSensor::Triangle> m_patches;

        const bool m_isRight;

        const float m_zAxisDirection;

        QVector<bool> m_activations;

        QMutex m_activationsMutex;
    };
}

using namespace __PalmAndFingertipTouchSensor_internal;

iCubFingertipsTouchSensor::iCubFingertipsTouchSensor(ConfigurationParameters& params, QString prefix) :
    iCubSensor(params, prefix),
    m_icubHand("right"),
    m_isRight(true),
    m_checkAllObjects(true),
    m_alpha(M_PI / 4.0), // 45°
    m_h(0.01),
    m_drawSensor(true),
    m_world(NULL),
    m_icubArm(),
    m_objects(NULL),
    m_icub(NULL),
    m_graphicalTouchSensors()
{
    m_icubHand = ConfigurationHelper::getString(params, prefix + "hand", m_icubHand);
    if (m_icubHand.toLower() == "right") {
        m_isRight = true;
    } else if (m_icubHand.toLower() == "left") {
        m_isRight = false;
    } else {
        ConfigurationHelper::throwUserConfigError(prefix + "hand", m_icubHand, "The hand parameter must be either \"right\" or \"left\" (case insensitive)");
    }
    m_checkAllObjects = ConfigurationHelper::getBool(params, prefix + "checkAllObjects", m_checkAllObjects);
    m_alpha = toRad(ConfigurationHelper::getDouble(params, prefix + "alpha", toDegree(m_alpha)));
    m_h = ConfigurationHelper::getDouble(params, prefix + "h", m_h);
    m_drawSensor = ConfigurationHelper::getBool(params, prefix + "drawSensor", m_drawSensor);

    // Declaring the resources that are needed here
    usableResources(QStringList() << icubResource << neuronsIteratorResource << "objects" << "world");
}

iCubFingertipsTouchSensor::~iCubFingertipsTouchSensor()
{
    // Nothing to do here, renderers are destroyed by their owners
}

void iCubFingertipsTouchSensor::save(ConfigurationParameters& params, QString prefix)
{
    iCubSensor::save(params, prefix);
    params.startObjectParameters(prefix, "iCubFingertipsTouchSensor", this);
    params.createParameter(prefix, "hand", m_icubHand);
    params.createParameter(prefix, "checkAllObjects", m_checkAllObjects ? QString("true") : QString("false"));
    params.createParameter(prefix, "alpha", QString::number(toDegree(m_alpha)));
    params.createParameter(prefix, "h", QString::number(m_h));
    params.createParameter(prefix, "drawSensor", m_drawSensor ? QString("true") : QString("false"));
}

void iCubFingertipsTouchSensor::describe(QString type)
{
    iCubSensor::describe(type);
    Descriptor d = addTypeDescription(type, "Hand fingertips touch sensor", "The touch sensor of the iCub fingertips. There are five sensors, one for each figertip");
    d.describeEnum("hand").def("right").values(QStringList() << "right" << "left").props(IsMandatory).help("The hand to use", "The hand whose distance from the object should be returned. Choose between \"right\" and \"left\"");
    d.describeBool("checkAllObjects").def(true).help("Wheter to check collisions with all objects or not", "If true, the collision of the hand touch sensors with all objects in the world is checked, otherwise only those in the objects vector are taken into account. Note that if the iCub is kinematic, only collisions with objects in the objects vector are checked regardless of the value of this parameter.");
    d.describeReal("alpha").def(45.0).help("The aperture of the sensor surface", "The aperture angle of the sensor surface in degrees (see the \"Fingertip Touch Sensor.png\" image for a graphical representation of sensor surface dimensions)");
    d.describeReal("h").def(0.01).help("The height of the sensor surface", "The height of the sensor surface (see the \"Fingertip Touch Sensor.png\" image for a graphical representation of sensor surface dimensions)");
    d.describeBool("drawSensor").def(true).help("Whether to draw sensor areas", "If true areas corresponding to the touch sensor surface are drawn on the fingertips when doing graphical simulations");
}

void iCubFingertipsTouchSensor::update()
{
    // Checking all resources we need exist
    checkAllNeededResourcesExist();

    // Acquiring the lock to get resources
    ResourcesLocker locker(this);

    EvonetIterator* evonetIt = getResource<EvonetIterator>(neuronsIteratorResource);
    evonetIt->setCurrentBlock(name());

    // These are the indexes of fingertips in the vector of icub arm parts
    static const unsigned int indexes[] = {19, 20, 21, 22, 26};
    for (unsigned int i = 0; i < 5; i++) {
        // The thumb is the last index (26)
        const double collision = handPieceCollides(m_icubArm[indexes[i]], (i == 4) ? true : false);
        // If sensors are also drawn, we change the color of the sensor depending on whether it
        // collides with an object or not
        if (m_drawSensor) {
            m_graphicalTouchSensors[i]->setActive((collision > 0.5));
        }
        evonetIt->setInput(collision);
        evonetIt->nextNeuron();
    }
}

int iCubFingertipsTouchSensor::size()
{
    return 5;
}

void iCubFingertipsTouchSensor::resourceChanged(QString resourceName, ResourceChangeType changeType)
{
    iCubSensor::resourceChanged(resourceName, changeType);

    if (changeType == Deleted) {
        return;
    }

    if (resourceName == icubResource) {
        m_icub = getResource<iCubRobot>();
        if (m_isRight) {
            m_icubArm = m_icub->rightArm();
        } else {
            m_icubArm = m_icub->leftArm();
        }

        // Checking if we have to draw the sensors. This is here because it requires a pointer to icub parts
        if (m_drawSensor) {
            m_graphicalTouchSensors.clear();

            // Creating graphical objects representing the touch sensors areas. They will set the finger piece as
            // their owner so that the destruction of the objects is handled by them
            m_graphicalTouchSensors.append(new FingertipTouchSensorGraphic(m_icubArm[19], m_alpha, m_h, m_isRight, false)); // Index
            m_graphicalTouchSensors.append(new FingertipTouchSensorGraphic(m_icubArm[20], m_alpha, m_h, m_isRight, false)); // Middle
            m_graphicalTouchSensors.append(new FingertipTouchSensorGraphic(m_icubArm[21], m_alpha, m_h, m_isRight, false)); // Ring
            m_graphicalTouchSensors.append(new FingertipTouchSensorGraphic(m_icubArm[22], m_alpha, m_h, m_isRight, false)); // Little
            m_graphicalTouchSensors.append(new FingertipTouchSensorGraphic(m_icubArm[26], m_alpha, m_h, m_isRight, true)); // Thumb
        }
    } else if (resourceName == neuronsIteratorResource) {
        QString lbl;
        if (m_isRight) {
            lbl = "R";
        } else {
            lbl = "L";
        }

        EvonetIterator* evonetIt = getResource<EvonetIterator>();
        evonetIt->setCurrentBlock(name());
        evonetIt->setGraphicProperties(lbl + "f1", 0.0, 1.0, Qt::red);
        evonetIt->nextNeuron();
        evonetIt->setGraphicProperties(lbl + "f2", 0.0, 1.0, Qt::red);
        evonetIt->nextNeuron();
        evonetIt->setGraphicProperties(lbl + "f3", 0.0, 1.0, Qt::red);
        evonetIt->nextNeuron();
        evonetIt->setGraphicProperties(lbl + "f4", 0.0, 1.0, Qt::red);
        evonetIt->nextNeuron();
        evonetIt->setGraphicProperties(lbl + "f5", 0.0, 1.0, Qt::red);
        evonetIt->nextNeuron();
    } else if (resourceName == "objects") {
        m_objects = getResource<QVector<WObject*> >();
    } else if (resourceName == "world") {
        m_world = getResource<World>();
    } else {
        Logger::info("Unknown resource " + resourceName + " for " + name());
    }
}

double iCubFingertipsTouchSensor::handPieceCollides(PhyObject* handPiece, bool isThumb) const
{
    if (m_icub->isKinematic() || !m_checkAllObjects) {
        for (int i = 0; i < m_objects->size(); i++) {
            PhyObject* obj = dynamic_cast<PhyObject*>(m_objects->at(i));
            QVector<wVector> contacts;
            if ((obj != NULL) && (m_world->smartCheckContacts(handPiece, (PhyObject*) m_objects->at(i), maxNumContacts, &contacts))) {
                for (int j = 0; j < contacts.size(); j++) {
                    if (goodCollisionPoint(handPiece, contacts[j], isThumb)) {
                        return 1.0;
                    }
                }
            }
        }

        return 0.0;
    } else {
        // Taking the vector of contacts. If no contact is present, this returns an empty vector
        const contactVec& c = m_world->contacts()[handPiece];

        for (int i = 0; i < c.size(); i++) {
            if (goodCollisionPoint(handPiece, c[i].pos, isThumb)) {
                return 1.0;
            }
        }

        return 0.0;
    }

    return 0.0;
}

bool iCubFingertipsTouchSensor::goodCollisionPoint(PhyObject* handPiece, const wVector& collisionPoint, bool isThumb) const
{
    // The various fingertips have frame of references with different orientations, so the direction towards
    // the palm (i.e. where the touch sensor area lies) is along different axes:
    //  - right hand:
    //      - thumb: +y axis
    //      - all other fingers: +z axis
    //  - left hand:
    //      - thumb: +y axis
    //      - all other fingers: -z axis
    // Here we calculate the angle on the yz plane, but the 0 angle is on different axes depending on the
    // hand piece, as in the list above

    float angle;
    if (isThumb) {
        angle = atan2(collisionPoint.z, collisionPoint.y);
    } else {
        if (m_isRight) {
            angle = atan2(collisionPoint.y, collisionPoint.z);
        } else {
            angle = atan2(collisionPoint.y, -collisionPoint.z);
        }
    }
    // Also computing the distance from the fingertip (to ease the check below)
    const float distFromFingertip = (dynamic_cast<PhyCylinder*>(handPiece))->height() / 2.0 - collisionPoint.x;

    // Checking if the collision point is good
    if ((angle >= -m_alpha) && (angle <= m_alpha) && (distFromFingertip <= m_h)) {
        return true;
    }

    return false;
}

iCubPalmPatchesTouchSensor::iCubPalmPatchesTouchSensor(ConfigurationParameters& params, QString prefix) :
    iCubSensor(params, prefix),
    m_icubHand("right"),
    m_isRight(true),
    m_checkAllObjects(true),
    m_drawSensor(true),
    m_world(NULL),
    m_icubArm(),
    m_objects(NULL),
    m_icub(NULL),
    m_handPalm(NULL),
    m_patches(),
    m_graphicalTouchSensor(NULL)
{
    m_icubHand = ConfigurationHelper::getString(params, prefix + "hand", m_icubHand);
    if (m_icubHand.toLower() == "right") {
        m_isRight = true;
    } else if (m_icubHand.toLower() == "left") {
        m_isRight = false;
    } else {
        ConfigurationHelper::throwUserConfigError(prefix + "hand", m_icubHand, "The hand parameter must be either \"right\" or \"left\" (case insensitive)");
    }
    m_checkAllObjects = ConfigurationHelper::getBool(params, prefix + "checkAllObjects", m_checkAllObjects);
    m_drawSensor = ConfigurationHelper::getBool(params, prefix + "drawSensor", m_drawSensor);

    // Declaring the resources that are needed here
    usableResources(QStringList() << icubResource << neuronsIteratorResource << "objects" << "world");
}

iCubPalmPatchesTouchSensor::~iCubPalmPatchesTouchSensor()
{
    // Nothing to do here, the renderer is destroyed by its owners
}

void iCubPalmPatchesTouchSensor::save(ConfigurationParameters& params, QString prefix)
{
    iCubSensor::save(params, prefix);
    params.startObjectParameters(prefix, "iCubPalmPatchesTouchSensor", this);
    params.createParameter(prefix, "hand", m_icubHand);
    params.createParameter(prefix, "checkAllObjects", m_checkAllObjects ? QString("true") : QString("false"));
    params.createParameter(prefix, "drawSensor", m_drawSensor ? QString("true") : QString("false"));
}

void iCubPalmPatchesTouchSensor::describe(QString type)
{
    iCubSensor::describe(type);
    Descriptor d = addTypeDescription(type, "Hand palm touch sensor", "The touch sensor of the iCub hand palm. There are four sensors, roughly in the same positions of the four patches on the real iCub hand");
    d.describeEnum("hand").def("right").values(QStringList() << "right" << "left").props(IsMandatory).help("The hand to use", "The hand whose distance from the object should be returned. Choose between \"right\" and \"left\"");
    d.describeBool("checkAllObjects").def(true).help("Wheter to check collisions with all objects or not", "If true, the collision of the hand touch sensors with all objects in the world is checked, otherwise only those in the objects vector are taken into account. Note that if the iCub is kinematic, only collisions with objects in the objects vector are checked regardless of the value of this parameter.");
    d.describeBool("drawSensor").def(true).help("Whether to draw sensor areas", "If true areas corresponding to the touch sensor surface are drawn on the fingertips when doing graphical simulations");
}

void iCubPalmPatchesTouchSensor::update()
{
    // Checking all resources we need exist
    checkAllNeededResourcesExist();

    // Acquiring the lock to get resources
    ResourcesLocker locker(this);

    EvonetIterator* evonetIt = getResource<EvonetIterator>(neuronsIteratorResource);
    evonetIt->setCurrentBlock(name());

    // First of all we have to get the list of collision points
    QVector<wVector> contacts;
    if (m_icub->isKinematic() || !m_checkAllObjects) {
        // Checking contacts with objects in the list. Appending all contacts to the contacts vector
        for (int i = 0; i < m_objects->size(); i++) {
            PhyObject* obj = dynamic_cast<PhyObject*>(m_objects->at(i));
            QVector<wVector> contactsWithObj;
            if (obj != NULL) {
                m_world->smartCheckContacts(m_handPalm, (PhyObject*) m_objects->at(i), maxNumContacts, &contactsWithObj);
                contacts << contactsWithObj;
            }
        }
    } else {
        // Taking the vector of contacts. If no contact is present, this returns an empty vector
        const contactVec& c = m_world->contacts()[m_handPalm];

        for (int i = 0; i < c.size(); i++) {
            contacts.append(c[i].pos);
        }
    }

    // Now we have to check each contact point for each triangle. We also save activations into a QVector
    // if the sensor is drawn
    QVector<bool> activations;
    if (m_drawSensor) {
        activations.fill(false, m_patches.size());
    }
    for (int i = 0; i < m_patches.size(); i++) {
        float activation = 0.0;
        for (int j = 0; j < contacts.size(); j++) {
            if (pointInPalmTriangle(contacts[j], m_patches[i])) {
                activation = 1.0;
                if (m_drawSensor) {
                    activations[i] = true;
                }
                break;
            }
        }
        evonetIt->setInput(activation);
        evonetIt->nextNeuron();
    }
    if (m_drawSensor) {
        m_graphicalTouchSensor->setActivations(activations);
    }
}

int iCubPalmPatchesTouchSensor::size()
{
    // The number of patches is always 4
    return 4;
}

void iCubPalmPatchesTouchSensor::resourceChanged(QString resourceName, ResourceChangeType changeType)
{
    iCubSensor::resourceChanged(resourceName, changeType);

    if (changeType == Deleted) {
        return;
    }

    if (resourceName == icubResource) {
        m_icub = getResource<iCubRobot>();
        if (m_isRight) {
            m_icubArm = m_icub->rightArm();
        } else {
            m_icubArm = m_icub->leftArm();
        }
        m_handPalm = dynamic_cast<PhyBox*>(m_icubArm[6]);

        // Creating the list of triangles. The palmDirection constant is needed because the right and
        // left hand have different frame of references: in the right hand the palm is towards +z, while
        // in the left hand it is towards -z. The patches center is not in the middle of the palm along
        // the y axis, it is slighly moved torads the side opposite to the thumb
        const float palmDirection = m_isRight ? +1.0 : -1.0;
        const float triangleSide = m_handPalm->sideX() / 2.0f;
        const float triangleHalfSide = triangleSide / 2.0f;
        const float triangleHeight = sqrt((triangleSide * triangleSide) - (triangleHalfSide * triangleHalfSide));
        const float palmZ = palmDirection * m_handPalm->sideZ() / 2.0;
        const float palmCenterY = (m_handPalm->sideY() / 2.0) - triangleHeight * 1.1;
        Triangle t;
        m_patches.clear();

        t.a = wVector(0.0, palmCenterY, palmZ);
        t.b = wVector(triangleSide, palmCenterY, palmZ);
        t.c = wVector(triangleHalfSide, palmCenterY - triangleHeight, palmZ);
        m_patches.append(t);

        t.a = wVector(0.0, palmCenterY, palmZ);
        t.b = wVector(triangleSide, palmCenterY, palmZ);
        t.c = wVector(triangleHalfSide, palmCenterY + triangleHeight, palmZ);
        m_patches.append(t);

        t.a = wVector(0.0, palmCenterY, palmZ);
        t.b = wVector(triangleHalfSide, palmCenterY + triangleHeight, palmZ);
        t.c = wVector(-triangleHalfSide, palmCenterY + triangleHeight, palmZ);
        m_patches.append(t);

        t.a = wVector(0.0, palmCenterY, palmZ);
        t.b = wVector(-triangleSide, palmCenterY, palmZ);
        t.c = wVector(-triangleHalfSide, palmCenterY + triangleHeight, palmZ);
        m_patches.append(t);

        if (m_drawSensor) {
            m_graphicalTouchSensor = new PalmPatchesTouchSensorGraphic(m_handPalm, m_patches, m_isRight);
        }
    } else if (resourceName == neuronsIteratorResource) {
        QString lbl;
        if (m_isRight) {
            lbl = "R";
        } else {
            lbl = "L";
        }

        EvonetIterator* evonetIt = getResource<EvonetIterator>();
        evonetIt->setCurrentBlock(name());
        evonetIt->setGraphicProperties(lbl + "p1", 0.0, 1.0, Qt::red);
        evonetIt->nextNeuron();
        evonetIt->setGraphicProperties(lbl + "p2", 0.0, 1.0, Qt::red);
        evonetIt->nextNeuron();
        evonetIt->setGraphicProperties(lbl + "p3", 0.0, 1.0, Qt::red);
        evonetIt->nextNeuron();
        evonetIt->setGraphicProperties(lbl + "p4", 0.0, 1.0, Qt::red);
        evonetIt->nextNeuron();
    } else if (resourceName == "objects") {
        m_objects = getResource<QVector<WObject*> >();
    } else if (resourceName == "world") {
        m_world = getResource<World>();
    } else {
        Logger::info("Unknown resource " + resourceName + " for " + name());
    }
}

bool iCubPalmPatchesTouchSensor::pointInPalmTriangle(const wVector& point, const Triangle& triangle) const
{
    // Checking that the point is on the palm side of the hand
    if (((m_isRight) && (point.z < 0.0)) || ((!m_isRight) && (point.z > 0.0))) {
        return false;
    }

    // The algorithm used here uses Barycentric Coordinates to check if a point is inside a triangle or not.
    // You can find mode information at the following links:
    //  http://en.wikipedia.org/wiki/Barycentric_coordinates_(mathematics)
    //  http://www.blackpawn.com/texts/pointinpoly/default.html
    // The version implemented here is directly taken from the second link (an offline version is in the
    // documentation). We discard the z coordinate (and do computations in 2D) because the check on z has
    // already been done before

    // Compute vectors
    const float v0x = triangle.c.x - triangle.a.x;
    const float v0y = triangle.c.y - triangle.a.y;
    const float v1x = triangle.b.x - triangle.a.x;
    const float v1y = triangle.b.y - triangle.a.y;
    const float v2x = point.x - triangle.a.x;
    const float v2y = point.y - triangle.a.y;

    // Compute dot products
    const float dot00 = v0x * v0x + v0y * v0y;
    const float dot01 = v0x * v1x + v0y * v1y;
    const float dot02 = v0x * v2x + v0y * v2y;
    const float dot11 = v1x * v1x + v1y * v1y;
    const float dot12 = v1x * v2x + v1y * v2y;

    // Compute barycentric coordinates
    const float invDenom = 1.0 / (dot00 * dot11 - dot01 * dot01);
    const float u = (dot11 * dot02 - dot01 * dot12) * invDenom;
    const float v = (dot00 * dot12 - dot01 * dot02) * invDenom;

    // Check if point is in triangle
    return (u >= 0) && (v >= 0) && (u + v < 1);
}

HandObjectVisualOffsetSensor::HandObjectVisualOffsetSensor(ConfigurationParameters& params, QString prefix) :
    iCubSensor(params, prefix),
    m_icubHand("right"),
    m_world(NULL),
    m_eye(NULL),
    m_hand(NULL),
    m_object(NULL)
{
    m_icubHand = ConfigurationHelper::getString(params, prefix + "hand", "right");

    // Declaring the resources that are needed here
    usableResources(QStringList() << icubResource << neuronsIteratorResource << "objects" << "world");
}

HandObjectVisualOffsetSensor::~HandObjectVisualOffsetSensor()
{
}

void HandObjectVisualOffsetSensor::save(ConfigurationParameters& params, QString prefix)
{
    iCubSensor::save(params, prefix);

    params.startObjectParameters(prefix, "HandObjectVisualOffsetSensor", this);
    params.createParameter(prefix, "hand", m_icubHand);
}

void HandObjectVisualOffsetSensor::describe(QString type)
{
    iCubSensor::describe(type);
    Descriptor d = addTypeDescription(type, "Visual offset between the hand and the object", "This sensor computes the distance between the hand and the first object in the visual field of the robot. Returns the distances on the vertical and horizontal axes");
    d.describeEnum("hand").def("right").values(QStringList() << "right" << "left").props(IsMandatory).help("The hand to use", "The hand whose distance from the object should be returned. Choose between \"right\" and \"left\"");
}

void HandObjectVisualOffsetSensor::update()
{
    // Checking all resources we need exist
    checkAllNeededResourcesExist();

    // Acquiring the lock to get resources
    ResourcesLocker locker(this);

    // We get this here because we are not notified if the vector changes (i.e. elemets are added or deleted),
    // only if the vector is replaced with another vector (and that doesn't happend)
    m_object = (*(getResource<QVector<WObject*> >("objects")))[0];

    // Setting the eye matrix in the projector
    Projector projector;
    projector.setEyeMatrix(m_eye->matrix());

    // Computing the projection of the object on the retina
    projector.set3DPointWorld(m_object->matrix().w_pos);
    const ImagePoint objPos = projector.getImagePoint01();

    // Computing the projection of the hand on the retina
    projector.set3DPointWorld(m_hand->matrix().w_pos);
    const ImagePoint handPos = projector.getImagePoint01();

    double dx, dy;
    if(objPos.isValid() && handPos.isValid()) {
        dx = objPos.x - handPos.x;
        dx = tanh(5*dx);

        dy = objPos.y - handPos.y;
        dy = tanh(5*dx);
    } else {
        dx = 0;
        dy = 0;
    }

    EvonetIterator* evonetIt = getResource<EvonetIterator>(neuronsIteratorResource);
    evonetIt->setCurrentBlock(name());
    evonetIt->setInput(dx);
    evonetIt->nextNeuron();
    evonetIt->setInput(dy);
}

int HandObjectVisualOffsetSensor::size()
{
    return 2;
}

void HandObjectVisualOffsetSensor::resourceChanged(QString resourceName, ResourceChangeType changeType)
{
    iCubSensor::resourceChanged(resourceName, changeType);

    if (changeType == Deleted) {
        return;
    }

    if (resourceName == icubResource) {
        iCubRobot* icub = getResource<iCubRobot>();
        m_eye = icub->headNeck()[4];
        if (m_icubHand == "left") {
            m_hand = icub->leftArm()[6];
        } else {
            m_hand = icub->rightArm()[6];
        }
    } else if (resourceName == neuronsIteratorResource) {
        EvonetIterator* evonetIt = getResource<EvonetIterator>();
        evonetIt->setCurrentBlock(name());
        evonetIt->setGraphicProperties("dx", -1.0, 1.0, Qt::red);
        evonetIt->nextNeuron();
        evonetIt->setGraphicProperties("dy", -1.0, 1.0, Qt::red);
    } else if (resourceName == "objects") {
        // Nothing to do here, we get objects with getResource() in update()
    } else if (resourceName == "world") {
        m_world = getResource<World>();
    } else {
        Logger::info("Unknown resource " + resourceName + " for " + name());
    }
}

} // end namespace farsa

#endif