icubmotors.cpp

/********************************************************************************
 *  FARSA Experiments Library                                                   *
 *  Copyright (C) 2007-2013                                                     *
 *  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  *
 ********************************************************************************/

#ifdef FARSA_USE_YARP_AND_ICUB

#include "icubmotors.h"
#include "configurationhelper.h"
#include "phyicub.h"
#include "randomgenerator.h"
#include "motorcontrollers.h"
#include "logger.h"
#include <QStringList>
#include <QMap>

namespace farsa {

iCubMotor::iCubMotor(ConfigurationParameters& params, QString prefix) :
    Motor(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);
}

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

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

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

void iCubMotor::describe(QString type)
{
    // Calling parent function
    Motor::describe(type);

    // Describing our parameters
    Descriptor d = addTypeDescription(type, "The base class for iCub motors");
    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 iCubMotor::resourceChanged(QString resourceName, ResourceChangeType changeType)
{
    // Calling parent function
    Motor::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;
    }
}

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

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

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

void iCubArmRandomMotor::describe( QString type ) {
    iCubMotor::describe( type );
    Descriptor d = addTypeDescription( type, "An example of Motor implementation that randomly set a position of the arm using configurePosture" );
    d.describeEnum( "arm" ).def( "right" ).values( QStringList()<<"right"<<"left" ).props( IsMandatory ).help( "The arm to move" );
}

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

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

    QMap<int, real> posture;
    QStringList values;
    for( int i=0; i<7; i++ ) {
        double min, max;
        icubMotors->getLimits( i, &min, &max );
        double value = globalRNG->getDouble( min, max );
        posture[startJointId+i] = value;
        values << QString::number( value );
    }
    icub->configurePosture( posture );
    //exp->setStatus( QString("MOTOR Settings: <")+values.join(", ")+QString(">") );
    /*
    for( int i=0; i<7; i++, gaexp->cupdate++ ) {
        double min, max, value;
        icubMotors->getEncoder(i, &value);
        icubMotors->getLimits(i,&min,&max);
        //normalizziamo i valori dei motori tra 0 ed 1;
        gaexp->rajoints[i]=linearMap(value,min,max,0,1);
        gaexp->evonet->setInput(gaexp->cupdate,gaexp->rajoints[i]);
    }
    */
}

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

void iCubArmRandomMotor::resourceChanged(QString resourceName, ResourceChangeType changeType) {
    iCubMotor::resourceChanged(resourceName, changeType);

    if (changeType == Deleted) {
        return;
    }

    if (resourceName == icubResource) {
        icub = getResource<iCubRobot>();
        if ( icubArm == "right" ) {
            startJointId = iCubRobot::right_shoulder_pitch;
            icubMotors = icub->rightArmController();
        } else {
            startJointId = iCubRobot::left_shoulder_pitch;
            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());
    }
}

iCubArmPosToPostureMotor::iCubArmPosToPostureMotor( ConfigurationParameters& params, QString prefix ) :
    iCubMotor(params, prefix),
    icub(NULL),
    icubMotors(NULL) {
    icubArm = ConfigurationHelper::getString( params, prefix+"arm", "right" );
    is7Dof = ConfigurationHelper::getBool( params, prefix+"full", true );
    // Declaring the resources that are needed here
    usableResources( QStringList() << icubResource << neuronsIteratorResource );
}

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

void iCubArmPosToPostureMotor::save( ConfigurationParameters& params, QString prefix ) {
    iCubMotor::save( params, prefix );
    params.startObjectParameters( prefix, "iCubArmPosToPostureMotor", this );
    params.createParameter( prefix, "arm", icubArm );
    params.createParameter( prefix, "full", (is7Dof ? "true" : "false" ) );
}

void iCubArmPosToPostureMotor::describe( QString type ) {
    iCubMotor::describe( type );
    Descriptor d = addTypeDescription( type, "Move the arm in the position using the configurePosture method. The movement of this motor is instantaneuos, it only require one step. And hence, any physics is ignored. Use this motor only in kinematic mode of the iCub." );
    d.describeEnum( "arm" ).def( "right" ).values( QStringList()<<"right"<<"left" ).props( IsMandatory ).help( "The arm to move" );
    d.describeBool( "full" ).def( "true" ).help( "If full is false only the first 4 DoF will be moved; if full is true all 7 DoF of the arm (including the wrist) will be moved" );
}

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

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

    QMap<int, real> posture;
    NeuronsIterator* evonetIt = getResource<NeuronsIterator>( neuronsIteratorResource );
    evonetIt->setCurrentBlock( name() );
    for( int i=0; i<(is7Dof ? 7 : 4); i++, evonetIt->nextNeuron() ) {
        double actual;
        double min,max;
        icubMotors->getEncoder(i,&actual);
        icubMotors->getLimits(i,&min,&max);
        double a = evonetIt->getOutput();
        posture[startJointId+i] = linearMap(a,0.0,1.0,min, max);
    }
    icub->configurePosture( posture );
}

int iCubArmPosToPostureMotor::size() {
    return (is7Dof ? 7 : 4);
}

void iCubArmPosToPostureMotor::resourceChanged(QString resourceName, ResourceChangeType changeType) {
    iCubMotor::resourceChanged(resourceName, changeType);

    if (changeType == Deleted) {
        return;
    }

    if (resourceName == icubResource) {
        icub = getResource<iCubRobot>();
        if ( icubArm == "right" ) {
            startJointId = iCubRobot::right_shoulder_pitch;
            icubMotors = icub->rightArmController();
        } else {
            startJointId = iCubRobot::left_shoulder_pitch;
            icubMotors = icub->leftArmController();
        }
    } else if (resourceName == neuronsIteratorResource) {
        NeuronsIterator* evonetIt = getResource<NeuronsIterator>();
        evonetIt->setCurrentBlock( name() );
        for( int i=0; i<(is7Dof ? 7 : 4); i++, evonetIt->nextNeuron() ) {
            evonetIt->setGraphicProperties( QString("m")+QString::number(i), 0.0, 1.0, Qt::red );
        }
    } else {
        Logger::info("Unknown resource " + resourceName + " for " + name());
    }
}

iCubTorsoPosToPostureMotor::iCubTorsoPosToPostureMotor( ConfigurationParameters& params, QString prefix ) :
    iCubMotor(params, prefix),
    icub(NULL),
    icubMotors(NULL) {
    torsoDofs[0] = ! ConfigurationHelper::getBool( params, prefix+"disableYaw", false );
    torsoDofs[1] = ! ConfigurationHelper::getBool( params, prefix+"disableRoll", false );
    torsoDofs[2] = ! ConfigurationHelper::getBool( params, prefix+"disablePitch", false );
    // Declaring the resources that are needed here
    usableResources( QStringList() << icubResource << neuronsIteratorResource );
}

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

void iCubTorsoPosToPostureMotor::save( ConfigurationParameters& params, QString prefix ) {
    iCubMotor::save( params, prefix );
    params.startObjectParameters( prefix, "iCubTorsoPosToPostureMotor", this );
    if ( !torsoDofs[0] ) {
        params.createParameter( prefix, "disableYaw", "true" );
    }
    if ( !torsoDofs[1] ) {
        params.createParameter( prefix, "disableRoll", "true" );
    }
    if ( !torsoDofs[2] ) {
        params.createParameter( prefix, "disablePitch", "true" );
    }
}

void iCubTorsoPosToPostureMotor::describe( QString type ) {
    iCubMotor::describe( type );
    Descriptor d = addTypeDescription( type, "Move the torso in the position using the configurePosture method. The movement of this motor is instantaneuos, it only require one step. And hence, any physics is ignored. Use this motor only in kinematic mode of the iCub." );
    d.describeBool( "disableYaw" ).def(false).help( "Disable the movement of the first joint of the Torso corresponding to the yaw rotation" );
    d.describeBool( "disableRoll" ).def(false).help( "Disable the movement of the second joint of the Torso corresponding to the roll rotation" );
    d.describeBool( "disablePitch" ).def(false).help( "Disable the movement of the third joint of the Torso corresponding to the pitch rotation" );
}

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

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

    QMap<int, real> posture;
    NeuronsIterator* evonetIt = getResource<NeuronsIterator>( neuronsIteratorResource );
    evonetIt->setCurrentBlock( name() );
    for( int i=0; i<3; i++ ) {
        if ( torsoDofs[i] ) {
            double actual;
            double min,max;
            icubMotors->getEncoder(i,&actual);
            icubMotors->getLimits(i,&min,&max);
            double a = evonetIt->getOutput();
            posture[iCubRobot::torso_yaw+i] = linearMap(a,0.0,1.0,min, max);
            evonetIt->nextNeuron();
        } else {
            posture[iCubRobot::torso_yaw+i] = 0.0;
        }
    }
    icub->configurePosture( posture );
}

int iCubTorsoPosToPostureMotor::size() {
    return torsoDofs[0]+torsoDofs[1]+torsoDofs[2];
}

void iCubTorsoPosToPostureMotor::resourceChanged(QString resourceName, ResourceChangeType changeType) {
    iCubMotor::resourceChanged(resourceName, changeType);

    if (changeType == Deleted) {
        return;
    }

    if (resourceName == icubResource) {
        icub = getResource<iCubRobot>();
        icubMotors = icub->torsoController();
    } else if (resourceName == neuronsIteratorResource) {
        NeuronsIterator* evonetIt = getResource<NeuronsIterator>();
        evonetIt->setCurrentBlock( name() );
        int numDofs = torsoDofs[0]+torsoDofs[1]+torsoDofs[2];
        for( int i=0; i<numDofs; i++, evonetIt->nextNeuron() ) {
            evonetIt->setGraphicProperties( QString("m")+QString::number(i), 0.0, 1.0, Qt::red );
        }
    } else {
        Logger::info("Unknown resource " + resourceName + " for " + name());
    }
}

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

iCubArmMusclesMotor::~iCubArmMusclesMotor() {
    /*here we need to delete muscles objects */
    for(int i=0;i<7;i++)
        delete musclePairs[i];
    delete[] musclePairs;

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

void iCubArmMusclesMotor::describe( QString type ) {
    iCubMotor::describe( type );
    Descriptor d = addTypeDescription( type, "Muscular control of the right/left arm, 2 muscles for each joint" );
    d.describeEnum( "arm" ).def( "right" ).values( QStringList()<<"right"<<"left" ).props( IsMandatory ).help( "The arm to move" );
}
void iCubArmMusclesMotor::update() {
    // Checking all resources we need exist
    checkAllNeededResourcesExist();

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

    //to do:
    NeuronsIterator* evonetIt = getResource<NeuronsIterator>( neuronsIteratorResource );
    evonetIt->setCurrentBlock( name() );
    for( int i=0; i<7; i++ ) {

        double a=evonetIt->getOutput();
        evonetIt->nextNeuron();
        double b=evonetIt->getOutput();
        evonetIt->nextNeuron();
        musclePairs[i]->setActivation(a,b);
        icubMotors->velocityMove(i,musclePairs[i]->apply());
    }
}

int iCubArmMusclesMotor::size() {
    return 14;
}

void iCubArmMusclesMotor::resourceChanged(QString resourceName, ResourceChangeType changeType) {
    iCubMotor::resourceChanged(resourceName, changeType);

    if (changeType == Deleted) {
        return;
    }

    if (resourceName == icubResource) {
        const float maxVel = 50;
        // const float viscos = 0.04;

        icub = getResource<iCubRobot>();
        if ( icubArm == "right" ) {
            icubMotors = icub->rightArmController();
        } else {
            icubMotors = icub->leftArmController();
        }

        if (musclePairs != NULL) {
            for(int i = 0; i < 7; i++) {
                delete musclePairs[i];
            }
            delete[] musclePairs;
        }

        musclePairs = new MusclePair*[7];
        for (int i = 0; i < 7; i++) {
            musclePairs[i] = new MusclePair(icubMotors->motors()[i],3.0f, maxVel, 2.5f, 3.7f);
        }
    } else if (resourceName == neuronsIteratorResource) {
        QString label;
        if ( icubArm == "right" ) {
            label="R";
        } else {
            label="L";
        }

        NeuronsIterator* evonetIt = getResource<NeuronsIterator>();
        evonetIt->setCurrentBlock( name() );
        for( int i = 0; i<7; i++ ) {
            for (int m = 0;m < 2; m++) {
                if (m == 0) {
                    evonetIt->setGraphicProperties(label + QString("A") + QString::number(i) + QString("'"), 0.0, 1.0, Qt::red );
                } else {
                    evonetIt->setGraphicProperties(label + QString("A") + QString::number(i) + QString("\""), 0.0, 1.0, Qt::red );
                }

                evonetIt->nextNeuron();
            }
        }
    } else {
        Logger::info("Unknown resource " + resourceName + " for " + name());
    }
}

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

iCubHandMusclesMotor::~iCubHandMusclesMotor() {
    /*here we need to delete muscles objects */
    for(int i=0;i<9;i++)
        delete musclePairs[i];
    delete[] musclePairs;

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

void iCubHandMusclesMotor::describe( QString type ) {
    iCubMotor::describe( type );
    Descriptor d = addTypeDescription( type, "Muscular control of the right/left hand, 2 muscles for each joint" );
    d.describeEnum( "hand" ).def( "right" ).values( QStringList()<<"right"<<"left" ).props( IsMandatory ).help( "The hand to move" );
}
void iCubHandMusclesMotor::update() {
    // Checking all resources we need exist
    checkAllNeededResourcesExist();

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

    //to do:
    NeuronsIterator* evonetIt = getResource<NeuronsIterator>( neuronsIteratorResource );
    evonetIt->setCurrentBlock( name() );
    for( int i=0; i<9; i++ ) {

        double a=evonetIt->getOutput();
        evonetIt->nextNeuron();
        double b=evonetIt->getOutput();
        evonetIt->nextNeuron();
        musclePairs[i]->setActivation(a,b);
        icubMotors->velocityMove(i+7,musclePairs[i]->apply());
    }
}

int iCubHandMusclesMotor::size() {
    return 18;
}

void iCubHandMusclesMotor::resourceChanged(QString resourceName, ResourceChangeType changeType) {
    iCubMotor::resourceChanged(resourceName, changeType);

    if (changeType == Deleted) {
        return;
    }

    if (resourceName == icubResource) {
        const float maxVel = 50;
        // const float viscos = 0.04;

        icub = getResource<iCubRobot>();
        if ( icubHand == "right" ) {
            icubMotors = icub->rightArmController();
        } else {
            icubMotors = icub->leftArmController();
        }

        if (musclePairs != NULL) {
            for(int i = 0; i < 9; i++) {
                delete musclePairs[i];
            }
            delete[] musclePairs;
        }

        musclePairs = new MusclePair*[9];
        for (int i = 0; i < 9; i++) {
            musclePairs[i] = new MusclePair(icubMotors->motors()[i+7],3.0f, maxVel, 2.5f, 3.7f);
        }
    } 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 < 9; i++ ) {
            for (int m = 0; m < 2; m++ ) {
                if ( m==0 ) {
                    evonetIt->setGraphicProperties(label + QString("F") + QString::number(i) + QString("'"), 0.0, 1.0, Qt::red );
                } else {
                    evonetIt->setGraphicProperties(label + QString("F") + QString::number(i) + QString("\""), 0.0, 1.0, Qt::red );
                }

                evonetIt->nextNeuron();
            }
        }
    } else {
        Logger::info("Unknown resource " + resourceName + " for " + name());
    }
}

iCubTorsoMusclesMotor::iCubTorsoMusclesMotor(farsa::ConfigurationParameters &params, QString prefix):
    iCubMotor(params, prefix),
    musclePairs(NULL),
    icub(NULL),
    icubMotors(NULL) {
    // Declaring the resources that are needed here
    usableResources( QStringList() << icubResource << neuronsIteratorResource );
}

iCubTorsoMusclesMotor::~iCubTorsoMusclesMotor() {
    /*here we need to delete muscles objects */
    for(int i=0;i<2;i++)
        delete musclePairs[i];
    delete[] musclePairs;

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

void iCubTorsoMusclesMotor::describe( QString type ) {
    iCubMotor::describe( type );
    Descriptor d = addTypeDescription( type, "Muscular control of the Torso, 2 muscles for each joint" );
}
void iCubTorsoMusclesMotor::update() {
    // Checking all resources we need exist
    checkAllNeededResourcesExist();

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

    //to do:
    NeuronsIterator* evonetIt = getResource<NeuronsIterator>( neuronsIteratorResource );
    evonetIt->setCurrentBlock( name() );
    for( int i=0; i<2; i++ ) {

        int id=0;
        double a=evonetIt->getOutput();
        evonetIt->nextNeuron();
        double b=evonetIt->getOutput();
        evonetIt->nextNeuron();
        musclePairs[i]->setActivation(a,b);
        if (i==0) id=0;
        if (i==1) id=2;
        icubMotors->velocityMove(id,musclePairs[i]->apply());
    }
}

int iCubTorsoMusclesMotor::size() {
    return 4;
}

void iCubTorsoMusclesMotor::resourceChanged(QString resourceName, ResourceChangeType changeType) {
    iCubMotor::resourceChanged(resourceName, changeType);

    if (changeType == Deleted) {
        return;
    }

    if (resourceName == icubResource) {
        const float maxVel = 50;
        // const float viscos = 0.04;

        icub = getResource<iCubRobot>();
        icubMotors = icub->torsoController();

        if (musclePairs != NULL) {
            for(int i = 0; i < 2; i++) {
                delete musclePairs[i];
            }
            delete[] musclePairs;
        }

        musclePairs = new MusclePair*[2];
        for (int i = 0; i < 2; i++) {
            int ir;

            if (i==0) ir=0;
            if (i==1) ir=2;

            musclePairs[i] = new MusclePair(icubMotors->motors()[ir],3.0f, maxVel, 2.5f, 3.7f);
        }
    } else if (resourceName == neuronsIteratorResource) {
        NeuronsIterator* evonetIt = getResource<NeuronsIterator>();
        evonetIt->setCurrentBlock( name() );

        for( int i = 0; i < 2; i++ ) {
            for (int m = 0; m < 2; m++) {
                if (m == 0) {
                    evonetIt->setGraphicProperties( QString("T") + QString::number(i) + QString("'"), 0.0, 1.0, Qt::red );
                } else {
                    evonetIt->setGraphicProperties( QString("T") + QString::number(i) + QString("\""), 0.0, 1.0, Qt::red );
                }

                evonetIt->nextNeuron();
            }
        }
    } else {
        Logger::info("Unknown resource " + resourceName + " for " + name());
    }
}

iCubHeadMusclesMotor::iCubHeadMusclesMotor(farsa::ConfigurationParameters &params, QString prefix):
    iCubMotor(params, prefix),
    musclePairs(NULL),
    icub(NULL),
    icubMotors(NULL) {
    // Declaring the resources that are needed here
    usableResources( QStringList() << icubResource << neuronsIteratorResource );
}

iCubHeadMusclesMotor::~iCubHeadMusclesMotor() {
    /*here we need to delete muscles objects */
    for(int i=0;i<2;i++)
        delete musclePairs[i];
    delete[] musclePairs;

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

void iCubHeadMusclesMotor::describe( QString type ) {
    iCubMotor::describe( type );
    Descriptor d = addTypeDescription( type, "Muscular control of the head, 2 muscles for each joint" );
}

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

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

    //to do:
    NeuronsIterator* evonetIt = getResource<NeuronsIterator>( neuronsIteratorResource );
    evonetIt->setCurrentBlock( name() );
    for( int i=0; i<2; i++ ) {

        int id=0;
        double a=evonetIt->getOutput();
        evonetIt->nextNeuron();
        double b=evonetIt->getOutput();
        evonetIt->nextNeuron();
        musclePairs[i]->setActivation(a,b);
        if (i==0) id=0;
        if (i==1) id=2;
        icubMotors->velocityMove(i,musclePairs[id]->apply());
    }
}

int iCubHeadMusclesMotor::size() {
    return 4;
}

void iCubHeadMusclesMotor::resourceChanged(QString resourceName, ResourceChangeType changeType) {
    iCubMotor::resourceChanged(resourceName, changeType);

    if (changeType == Deleted) {
        return;
    }

    if (resourceName == icubResource) {
        // getting the resources and initializing MotorController
        const float maxVel = 50;
        // const float viscos = 0.04;

        icub = getResource<iCubRobot>();
        icubMotors = icub->headNeckController();

        if (musclePairs != NULL) {
            for(int i = 0; i < 2; i++) {
                delete musclePairs[i];
            }
            delete[] musclePairs;
        }

        musclePairs = new MusclePair*[2];
        for (int i = 0; i < 2; i++) {
            musclePairs[i] = new MusclePair(icubMotors->motors()[i],3.0f, maxVel, 2.5f, 3.7f);
        }
    } else if (resourceName == neuronsIteratorResource) {
        NeuronsIterator* evonetIt = getResource<NeuronsIterator>();
        evonetIt->setCurrentBlock( name() );
        for( int i = 0; i < 2; i++ ) {
            for (int m = 0;m < 2; m++) {
                if ( m == 0 ) {
                    evonetIt->setGraphicProperties( QString("N") + QString::number(i) + QString("'"), 0.0, 1.0, Qt::red );
                } else {
                    evonetIt->setGraphicProperties( QString("N") + QString::number(i) + QString("\""), 0.0, 1.0, Qt::red );
                }

                evonetIt->nextNeuron();
            }
        }
    } else {
        Logger::info("Unknown resource " + resourceName + " for " + name());
    }
}

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

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

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

void iCubArmPosToVelMotor::describe( QString type ) {
    iCubMotor::describe( type );
    Descriptor d = addTypeDescription( type, "Position mapped in velocity control of the right/left arm" );
    d.describeEnum( "arm" ).def( "right" ).values( QStringList()<<"right"<<"left" ).props( IsMandatory ).help( "The arm to move" );
}

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

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

    //to do:
    NeuronsIterator* evonetIt = getResource<NeuronsIterator>( neuronsIteratorResource );
    evonetIt->setCurrentBlock( name() );
    for( int i=0; i<7; i++ ) {
        double actual;
        double min,max;
        double desiredPosture;
        icubMotors->getEncoder(i,&actual);
        icubMotors->getLimits(i,&min,&max);
        double a=evonetIt->getOutput();
        desiredPosture=linearMap(a,0.0,1.0,min, max);
        evonetIt->nextNeuron();         
        icubMotors->velocityMove(i,pcontrol.velocityForJoint(desiredPosture, actual));
    }
}

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

void iCubArmPosToVelMotor::resourceChanged(QString resourceName, ResourceChangeType changeType) {
    iCubMotor::resourceChanged(resourceName, changeType);

    if (changeType == Deleted) {
        return;
    }

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

        NeuronsIterator* evonetIt = getResource<NeuronsIterator>();
        evonetIt->setCurrentBlock( name() );
        for( int i=0; i<7; i++ ) {
            evonetIt->setGraphicProperties(label+ QString("A")+QString::number(i), 0.0, 1.0, Qt::red ); //ex Ap2V

            evonetIt->nextNeuron();
        }
    } else {
        Logger::info("Unknown resource " + resourceName + " for " + name());
    }
}

iCubTorsoPosToVelMotor::iCubTorsoPosToVelMotor(farsa::ConfigurationParameters &params, QString prefix):
    iCubMotor(params, prefix),
    icub(NULL),
    icubMotors(NULL) {
    torsoDofs[0] = ! ConfigurationHelper::getBool( params, prefix+"disableYaw", false );
    torsoDofs[1] = ! ConfigurationHelper::getBool( params, prefix+"disableRoll", false );
    torsoDofs[2] = ! ConfigurationHelper::getBool( params, prefix+"disablePitch", false );
    // Declaring the resources that are needed here
    usableResources( QStringList() << icubResource << neuronsIteratorResource );
}

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

void iCubTorsoPosToVelMotor::save( ConfigurationParameters& params, QString prefix ) {
    iCubMotor::save( params, prefix );
    params.startObjectParameters( prefix, "iCubTorsoPosToVelMotor", this );
    if ( !torsoDofs[0] ) {
        params.createParameter( prefix, "disableYaw", "true" );
    }
    if ( !torsoDofs[1] ) {
        params.createParameter( prefix, "disableRoll", "true" );
    }
    if ( !torsoDofs[2] ) {
        params.createParameter( prefix, "disablePitch", "true" );
    }
}

void iCubTorsoPosToVelMotor::describe( QString type ) {
    iCubMotor::describe( type );
    Descriptor d = addTypeDescription( type, "Position mapped in velocity control of the torso" );
    d.describeBool( "disableYaw" ).def(false).help( "Disable the movement of the first joint of the Torso corresponding to the yaw rotation" );
    d.describeBool( "disableRoll" ).def(false).help( "Disable the movement of the second joint of the Torso corresponding to the roll rotation" );
    d.describeBool( "disablePitch" ).def(false).help( "Disable the movement of the third joint of the Torso corresponding to the pitch rotation" );
}

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

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

    NeuronsIterator* evonetIt = getResource<NeuronsIterator>( neuronsIteratorResource );
    evonetIt->setCurrentBlock( name() );
    for( int i=0; i<3; i++ ) {
        double vel = 0.0;
        if ( torsoDofs[i] ) {
            double actual;
            double min,max;
            double desiredPosture;
            icubMotors->getEncoder(i,&actual);
            icubMotors->getLimits(i,&min,&max);
            double a=evonetIt->getOutput();
            desiredPosture=linearMap(a,0.0,1.0,min, max);
            vel = pcontrol.velocityForJoint(desiredPosture, actual);
            evonetIt->nextNeuron();
        }
        icubMotors->velocityMove( i, vel );
    }
}

int iCubTorsoPosToVelMotor::size() {
    return torsoDofs[0]+torsoDofs[1]+torsoDofs[2];
}

void iCubTorsoPosToVelMotor::resourceChanged(QString resourceName, ResourceChangeType changeType) {
    iCubMotor::resourceChanged(resourceName, changeType);

    if (changeType == Deleted) {
        return;
    }

    if (resourceName == icubResource) {
        icub = getResource<iCubRobot>();
        icubMotors=icub->torsoController();
    } else if (resourceName == neuronsIteratorResource) {
        NeuronsIterator* evonetIt = getResource<NeuronsIterator>();
        evonetIt->setCurrentBlock( name() );
        int numDofs = torsoDofs[0]+torsoDofs[1]+torsoDofs[2];
        for( int i=0; i<numDofs; i++ ) {
            evonetIt->setGraphicProperties( QString("T")+QString::number(i), 0.0, 1.0, Qt::red );
            evonetIt->nextNeuron();
        }
    } else {
        Logger::info("Unknown resource " + resourceName + " for " + name());
    }
}

iCubHeadPosToVelMotor::iCubHeadPosToVelMotor(farsa::ConfigurationParameters &params, QString prefix):
    iCubMotor(params, prefix),
    icub(NULL),
    icubMotors(NULL) {
    headDofs[0] =! ConfigurationHelper::getBool(params, prefix+"disableNeckPitch",false);
    headDofs[1] =! ConfigurationHelper::getBool(params, prefix+"disableNeckRoll",false);
    headDofs[2] =! ConfigurationHelper::getBool(params, prefix+"disableNeckYaw",false);
    headDofs[3] =! ConfigurationHelper::getBool(params, prefix+"disableEyesTilt",false);
    headDofs[4] =! ConfigurationHelper::getBool(params, prefix+"disableEyesVersion",false);
    headDofs[5] =! ConfigurationHelper::getBool(params, prefix+"disableEyesVergence",false);

    const double k = ConfigurationHelper::getDouble(params, prefix + "k", 0.3);
    const double maxvel = ConfigurationHelper::getDouble(params, prefix + "maxvel", 20.0);
    pcontrol.setControlParameters(k, maxvel);
    

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

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

void iCubHeadPosToVelMotor::save( ConfigurationParameters& params, QString prefix ) {
    iCubMotor::save( params, prefix );
    params.startObjectParameters( prefix, "iCubHeadPosToVelMotor", this );
    if (!headDofs[0])
        params.createParameter(prefix,"disableNeckPitch","true");
    if (!headDofs[1])
        params.createParameter(prefix,"disableNeckRoll","true");
    if (!headDofs[2])
        params.createParameter(prefix,"disableNeckYaw","true");
    if (!headDofs[3])
        params.createParameter(prefix,"disableEyesTilt","true");
    if (!headDofs[4])
        params.createParameter(prefix,"disableEyesVersion","true");
    if (!headDofs[5])
        params.createParameter(prefix,"disableEyesVergence","true");

        params.createParameter(prefix, "k", QString::number(pcontrol.getK()));
        params.createParameter(prefix, "maxvel", QString::number(pcontrol.getMaxVelocity()));
}

void iCubHeadPosToVelMotor::describe( QString type ) {
    iCubMotor::describe( type );
    Descriptor d = addTypeDescription( type, "Position mapped in velocity control of the Head" );
    d.describeBool("disableNeckPitch").def(false).help("Disables the #0 joint of the head: Neck Pitch");
    d.describeBool("disableNeckRoll").def(false).help("Disables the #1 joint of the head: Neck Roll");
    d.describeBool("disableNeckYaw").def(false).help("Disables the #2 joint of the head: Neck Yaw");
    d.describeBool("disableEyesTilt").def(false).help("Disables the #3 joint of the head: Eyes tilt");
    d.describeBool("disableEyesVersion").def(false).help("Disables the #4 joint of the head: Eyes version");
    d.describeBool("disableEyesVergence").def(false).help("Disables the #5 joint of the head: Eyes vergence");
    d.describeReal("k").def(0.3).limits(0.0, +std::numeric_limits<double>::infinity()).help("The gain for the proportional controller", "This parameter is the gain for the proportional controller converting position displacement (desired position minus current position) to a velocity to apply to the joint");
    d.describeReal("maxvel").def(20.0).limits(0.0, +std::numeric_limits<double>::infinity()).help("The maximum velocity for joints", "This is the maximum allowed velocity that can be applied to a joint (absolute value). Velocities greater than this are truncated to +maxvel or -maxvel");

}

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

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

    //to do:
    int id=0;
    NeuronsIterator* evonetIt = getResource<NeuronsIterator>( neuronsIteratorResource );
    evonetIt->setCurrentBlock( name() );

    for(int i=0;i<6;i++)
    {
        if(headDofs[i])
        {
        double actual;
        double min,max;
        double desiredPosture;
        double a;
    
        icubMotors->getEncoder(i,&actual);
        icubMotors->getLimits(i,&min,&max);
        a=evonetIt->getOutput();
        evonetIt->nextNeuron();
        desiredPosture=linearMap(a,0.0,1.0,min, max);
        
        icubMotors->velocityMove(i, pcontrol.velocityForJoint(desiredPosture, actual));
        }else
        {
        icubMotors->velocityMove(i,0);
        }
    }

    
}

int iCubHeadPosToVelMotor::size() {
    int s=0;
    for(int i=0;i<6;i++) s+=headDofs[i];
    return s;
}

void iCubHeadPosToVelMotor::resourceChanged(QString resourceName, ResourceChangeType changeType) {
    iCubMotor::resourceChanged(resourceName, changeType);

    if (changeType == Deleted) {
        return;
    }

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

        for( int i=0; i<size(); i++ ) {
            evonetIt->setGraphicProperties(QString("N")+QString::number(i), 0.0, 1.0, Qt::red );//ex Hp2v
            evonetIt->nextNeuron();
        }
    } else {
        Logger::info("Unknown resource " + resourceName + " for " + name());
    }
}

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

iCubHandPosToVelMotor::~iCubHandPosToVelMotor() {
    /*nothing to do */
    

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

void iCubHandPosToVelMotor::describe( QString type ) {
    iCubMotor::describe( type );
    Descriptor d = addTypeDescription( type, "Position mapped in velocity control of the right or left Hand" );
    d.describeEnum( "hand" ).def( "right" ).values( QStringList()<<"right"<<"left" ).props( IsMandatory ).help( "The hand to move" );
}

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

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

    //to do:
    int id=0;
    NeuronsIterator* evonetIt = getResource<NeuronsIterator>( neuronsIteratorResource );
    evonetIt->setCurrentBlock( name() );
    for( int i=7; i<16; i++ ) {
        double actual;
        double min,max;
        double desiredPosture;
        icubMotors->getEncoder(i,&actual);
        icubMotors->getLimits(i,&min,&max);
        double a=evonetIt->getOutput();
        desiredPosture=linearMap(a,0.0,1.0,min, max);
        evonetIt->nextNeuron(); 
        icubMotors->velocityMove(i,pcontrol.velocityForJoint(desiredPosture, actual));
    }
}

int iCubHandPosToVelMotor::size() {
    return 9;
}

void iCubHandPosToVelMotor::resourceChanged(QString resourceName, ResourceChangeType changeType) {
    iCubMotor::resourceChanged(resourceName, changeType);

    if (changeType == Deleted) {
        return;
    }

    if (resourceName == icubResource) {
        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<9; i++ ) {
            evonetIt->setGraphicProperties(label+QString("F")+QString::number(i), 0.0, 1.0, Qt::red ); //ex HNp2v

            evonetIt->nextNeuron();
        }
    } else {
        Logger::info("Unknown resource " + resourceName + " for " + name());
    }
}

iCubHeadVelocityMotor::iCubHeadVelocityMotor(ConfigurationParameters& params, QString prefix) :
    iCubMotor(params, prefix),
    m_headNeckController(NULL)
{
    // Declaring the resources that are needed here
    usableResources(QStringList() << icubResource << neuronsIteratorResource);
}

iCubHeadVelocityMotor::~iCubHeadVelocityMotor()
{
}

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

void iCubHeadVelocityMotor::describe(QString type)
{
    iCubMotor::describe(type);
    Descriptor d = addTypeDescription(type, "Head velocity motor", "The motor to move the head of the icub in velocity. This controls two degrees of freedom: pan (head joint 2) and tilt (head joint 0)");
}

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

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

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

    const double pan = evonetIt->getOutput();
    evonetIt->nextNeuron();
    const double tilt = evonetIt->getOutput();
    evonetIt->nextNeuron(); // Removed to avoid a warning from NeuronsIterator //remotion works only for the last sensor 
    const double maxVelocity = 50.0;

    // Computing the velocity (in degrees/sec)
    double panVelocity = (pan - 0.5) * 2.0 * maxVelocity;
    double tiltVelocity = (tilt - 0.5) * 2.0 * maxVelocity;

    // Now moving the head
    m_headNeckController->velocityMove(2, panVelocity);
    m_headNeckController->velocityMove(0, tiltVelocity);
}

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

void iCubHeadVelocityMotor::resourceChanged(QString resourceName, ResourceChangeType changeType)
{
    iCubMotor::resourceChanged(resourceName, changeType);

    if (changeType == Deleted) {
        return;
    }

    if (resourceName == icubResource) {
        m_headNeckController = getResource<iCubRobot>()->headNeckController();
    } else if (resourceName == neuronsIteratorResource) {
        NeuronsIterator* evonetIt = getResource<NeuronsIterator>();
        evonetIt->setCurrentBlock(name());
        evonetIt->setGraphicProperties(QString("N0"), 0.0, 1.0, Qt::blue); //ex hp
        evonetIt->nextNeuron();
        evonetIt->setGraphicProperties(QString("N1"), 0.0, 1.0, Qt::blue); //ex ht
    } else {
        Logger::info("Unknown resource " + resourceName + " for " + name());
    }
}

iCubConfigurableHandPosToVelMotor::iCubConfigurableHandPosToVelMotor(ConfigurationParameters& params, QString prefix) :
    iCubMotor(params, prefix),
    m_icubHand("right"),
    m_jointsGroups(),
    m_maxClosure(1.0),
    m_armController(NULL),
    m_pcontrol()
{
    m_icubHand = ConfigurationHelper::getString(params, prefix + "hand", "right");

    // Reading the fingers joints configuration. The configuration format is like this: joint1,joint2/joint5,joint6,joint3/joint4.
    // In this case we have three groups of joints: (joint1 and joint2), (joint5, joint6 and joint3) and (joint4). Joints in the
    // same group are controlled together. This parameter is compulsory, if it is not present, the program throws an exception.
    // The joint names are: Aperture, Thumb1, Thumb2, Thumb3, Index1, Index2, Middle1, Middle2, Others (case insensitive)
    const QString paramName = prefix + "jointsGroups";
    QString str = params.getValue(paramName);
    if (str.isEmpty()) {
        ConfigurationHelper::throwUserConfigError(paramName, "", "the parameter is compulsory but is not present in the configuration");
    }

    const QStringList groups = str.split("/", QString::SkipEmptyParts);
    if (groups.size() == 0) {
        ConfigurationHelper::throwUserConfigError(paramName, str, "invalid format (no joint group)");
    }

    // Resizing the vector of joint groups
    m_jointsGroups.resize(groups.size());
    for (int i = 0; i < m_jointsGroups.size(); i++) {
        // Splitting again
        const QStringList joints = groups[i].split(",", QString::SkipEmptyParts);
        if (joints.size() == 0) {
            ConfigurationHelper::throwUserConfigError(paramName, str, "a joint group has no components");
        }

        m_jointsGroups[i].resize(joints.size());
        for (int j = 0; j < m_jointsGroups[i].size(); j++) {
            // Now converting joint name into joint index. First converting to uppercase
            const QString jointName = joints[j].toUpper().trimmed();
            if (jointName == "APERTURE") {
                m_jointsGroups[i][j] = 7;
            } else if (jointName == "THUMB1") {
                m_jointsGroups[i][j] = 8;
            } else if (jointName == "THUMB2") {
                m_jointsGroups[i][j] = 9;
            } else if (jointName == "THUMB3") {
                m_jointsGroups[i][j] = 10;
            } else if (jointName == "INDEX1") {
                m_jointsGroups[i][j] = 11;
            } else if (jointName == "INDEX2") {
                m_jointsGroups[i][j] = 12;
            } else if (jointName == "MIDDLE1") {
                m_jointsGroups[i][j] = 13;
            } else if (jointName == "MIDDLE2") {
                m_jointsGroups[i][j] = 14;
            } else if (jointName == "OTHERS") {
                m_jointsGroups[i][j] = 15;
            } else {
                ConfigurationHelper::throwUserConfigError(paramName, str, "unknown joint name (" + jointName + ")");
            }
        }
    }

    // Now reading the parameter with the maximum joint closure
    m_maxClosure = ConfigurationHelper::getDouble(params, prefix + "maxClosure", 1.0);

    const double k = ConfigurationHelper::getDouble(params, prefix + "k", 0.3);
    const double maxvel = ConfigurationHelper::getDouble(params, prefix + "maxvel", 20.0);
    m_pcontrol.setControlParameters(k, maxvel);

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

iCubConfigurableHandPosToVelMotor::~iCubConfigurableHandPosToVelMotor()
{
}

void iCubConfigurableHandPosToVelMotor::save(ConfigurationParameters& params, QString prefix)
{
    iCubMotor::save( params, prefix );
    params.startObjectParameters(prefix, "ConfigurableHandPosToVelMotor", this);
    params.createParameter(prefix, "hand", m_icubHand);

    QString strJointsGroup;
    for (int g = 0; g < m_jointsGroups.size(); g++) {
        for (int j = 0; j < m_jointsGroups[g].size(); j++) {
            if (m_jointsGroups[g][j] == 7) {
                strJointsGroup += "Aperture";
            } else if (m_jointsGroups[g][j] == 8) {
                strJointsGroup += "Thumb1";
            } else if (m_jointsGroups[g][j] == 9) {
                strJointsGroup += "Thumb2";
            } else if (m_jointsGroups[g][j] == 10) {
                strJointsGroup += "Thumb3";
            } else if (m_jointsGroups[g][j] == 11) {
                strJointsGroup += "Index1";
            } else if (m_jointsGroups[g][j] == 12) {
                strJointsGroup += "Index2";
            } else if (m_jointsGroups[g][j] == 13) {
                strJointsGroup += "Middle1";
            } else if (m_jointsGroups[g][j] == 14) {
                strJointsGroup += "Middle2";
            } else if (m_jointsGroups[g][j] == 15) {
                strJointsGroup += "Others";
            } else {
                strJointsGroup += "ERROR";
            }

            if (j != (m_jointsGroups[g].size() - 1)) {
                strJointsGroup += ", ";
            }
        }
        if (g != (m_jointsGroups.size() - 1)) {
            strJointsGroup += " / ";
        }
    }
    params.createParameter(prefix, "jointsGroups", strJointsGroup);

    params.createParameter(prefix, "maxClosure", QString::number(m_maxClosure));
    params.createParameter(prefix, "k", QString::number(m_pcontrol.getK()));
    params.createParameter(prefix, "maxvel", QString::number(m_pcontrol.getMaxVelocity()));
}

void iCubConfigurableHandPosToVelMotor::describe(QString type)
{
    iCubMotor::describe( type );
    Descriptor d = addTypeDescription(type, "Hand motor", "A motor for the iCub hand. This allows to specify which joints should be controlled together (see the jointsGroups parameter)");
    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.describeString("jointsGroups").def("Aperture, Thumb1, Thumb2, Thumb3, Index1, Index2, Middle1, Middle2, Others").props(IsMandatory).help("The parameter which specifies the joint configuration", "This parameter allows specifying which joints should be controlled together. The format is like this: joint1,joint2/joint5,joint6,joint3/joint4. In this case we have three groups of joints: (joint1 and joint2), (joint5, joint6 and joint3) and (joint4). Joints in the same group are controlled together, while joints that are not listed in any group are not controlled. This parameter is compulsory, if it is not present, the program throws an exception. The allowed joint names are: Aperture, Thumb1, Thumb2, Thumb3, Index1, Index2, Middle1, Middle2, Others (case insensitive)");
    d.describeReal("maxClosure").def(1.0).limits(0.0, 1.0).help("The limit to joint movement", "This parameter allows limiting joint excursion to a portion of the original one. If set to 1.0 all joints move within their limits, otherwise their upper limit is set to maxClosure portion of the original upper limit. This applies to all controlled joints. A value of 0.0 disables all joint movements");
    d.describeReal("k").def(0.3).limits(0.0, +std::numeric_limits<double>::infinity()).help("The gain for the proportional controller", "This parameter is the gain for the proportional controller converting position displacement (desired position minus current position) to a velocity to apply to the joint");
    d.describeReal("maxvel").def(20.0).limits(0.0, +std::numeric_limits<double>::infinity()).help("The maximum velocity for joints", "This is the maximum allowed velocity that can be applied to a joint (absolute value). Velocities greater than this are truncated to +maxvel or -maxvel");
}

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

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

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

    for (int i = 0; i < m_jointsGroups.size(); i++, evonetIt->nextNeuron()) {
        // The network output value is multiplied by m_maxClosure so that the maximum closure of fingers can be
        // constrained
        const double out = m_maxClosure * evonetIt->getOutput();

        for (int j = 0; j < m_jointsGroups[i].size(); j++) {
            double actual;
            double min, max;

            m_armController->getEncoder(m_jointsGroups[i][j], &actual);
            m_armController->getLimits(m_jointsGroups[i][j], &min, &max);

            const double desiredPosture = linearMap(out, 0.0, 1.0, min, max); // normalize the neuron output with respect to the joint range

            m_armController->velocityMove(m_jointsGroups[i][j], m_pcontrol.velocityForJoint(desiredPosture, actual));
        }
    }
}

int iCubConfigurableHandPosToVelMotor::size()
{
    return m_jointsGroups.size();
}

void iCubConfigurableHandPosToVelMotor::resourceChanged(QString resourceName, ResourceChangeType changeType)
{
    iCubMotor::resourceChanged(resourceName, changeType);

    if (changeType == Deleted) {
        return;
    }

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

        NeuronsIterator* evonetIt = getResource<NeuronsIterator>();
        evonetIt->setCurrentBlock(name());
        for (int i = 0; i < m_jointsGroups.size(); i++, evonetIt->nextNeuron()) {
            evonetIt->setGraphicProperties(label + "F" + QString::number(i), 0.0, 1.0, Qt::blue);
        }
    } else {
        Logger::info("Unknown resource " + resourceName + " for " + name());
    }
}

iCubArmVelocityMotor::iCubArmVelocityMotor( ConfigurationParameters& params, QString prefix ) :
    iCubMotor(params, prefix),
    icub(NULL),
    icubMotors(NULL) {
    icubArm = ConfigurationHelper::getString( params, prefix+"arm", "right" );
    is7Dof = ConfigurationHelper::getBool( params, prefix+"full", true );
    maxVelocity = ConfigurationHelper::getDouble( params, prefix+"maxVelocity", 50.0 );
    // Declaring the resources that are needed here
    usableResources( QStringList() << icubResource << neuronsIteratorResource );
}

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

void iCubArmVelocityMotor::save( ConfigurationParameters& params, QString prefix ) {
    iCubMotor::save( params, prefix );
    params.startObjectParameters( prefix, "iCubArmVelocityMotor", this );
    params.createParameter( prefix, "arm", icubArm );
    params.createParameter( prefix, "full", (is7Dof ? "true" : "false" ) );
    params.createParameter( prefix, "maxVelocity", QString::number(maxVelocity) );
}

void iCubArmVelocityMotor::describe( QString type ) {
    iCubMotor::describe( type );
    Descriptor d = addTypeDescription( type, "Move the arm by velocity." );
    d.describeEnum( "arm" ).def( "right" ).values( QStringList()<<"right"<<"left" ).props( IsMandatory ).help( "The arm to move" );
    d.describeBool( "full" ).def( "true" ).help( "If full is false only the first 4 DoF will be moved; if full is true all 7 DoF of the arm (including the wrist) will be moved" );
    d.describeReal( "maxVelocity").def(50.0).limits(0,100).help( "The maximum velocity allowed." );
}

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

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

    double vels[7] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
    NeuronsIterator* evonetIt = getResource<NeuronsIterator>( neuronsIteratorResource );
    evonetIt->setCurrentBlock( name() );
    for( int i=0; i<(is7Dof ? 7 : 4); i++, evonetIt->nextNeuron() ) {
        vels[i] = (evonetIt->getOutput()-0.5)*2.0*maxVelocity;
    }
    for( int i=0; i<7; i++ ) {
        icubMotors->velocityMove( i, vels[i] );
    }
}

int iCubArmVelocityMotor::size() {
    return (is7Dof ? 7 : 4);
}

void iCubArmVelocityMotor::resourceChanged(QString resourceName, ResourceChangeType changeType) {
    iCubMotor::resourceChanged(resourceName, changeType);

    if (changeType == Deleted) {
        return;
    }

    if (resourceName == icubResource) {
        icub = getResource<iCubRobot>();
        if ( icubArm == "right" ) {
            startJointId = iCubRobot::right_shoulder_pitch;
            icubMotors = icub->rightArmController();
        } else {
            startJointId = iCubRobot::left_shoulder_pitch;
            icubMotors = icub->leftArmController();
        }
    } else if (resourceName == neuronsIteratorResource) {
        NeuronsIterator* evonetIt = getResource<NeuronsIterator>();
        evonetIt->setCurrentBlock( name() );
        for( int i=0; i<(is7Dof ? 7 : 4); i++, evonetIt->nextNeuron() ) {
            evonetIt->setGraphicProperties( QString("m")+QString::number(i), 0.0, 1.0, Qt::red );
        }
    } else {
        Logger::info("Unknown resource " + resourceName + " for " + name());
    }
}

iCubTorsoVelocityMotor::iCubTorsoVelocityMotor( ConfigurationParameters& params, QString prefix ) :
    iCubMotor(params, prefix),
    icub(NULL),
    icubMotors(NULL) {
    torsoDofs[0] = ! ConfigurationHelper::getBool( params, prefix+"disableYaw", false );
    torsoDofs[1] = ! ConfigurationHelper::getBool( params, prefix+"disableRoll", false );
    torsoDofs[2] = ! ConfigurationHelper::getBool( params, prefix+"disablePitch", false );
    maxVelocity = ConfigurationHelper::getDouble( params, prefix+"maxVelocity", 50.0 );
    // Declaring the resources that are needed here
    usableResources( QStringList() << icubResource << neuronsIteratorResource );
}

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

void iCubTorsoVelocityMotor::save( ConfigurationParameters& params, QString prefix ) {
    iCubMotor::save( params, prefix );
    params.startObjectParameters( prefix, "iCubTorsoVelocityMotor", this );
    if ( !torsoDofs[0] ) {
        params.createParameter( prefix, "disableYaw", "true" );
    }
    if ( !torsoDofs[1] ) {
        params.createParameter( prefix, "disableRoll", "true" );
    }
    if ( !torsoDofs[2] ) {
        params.createParameter( prefix, "disablePitch", "true" );
    }
    params.createParameter( prefix, "maxVelocity", QString::number(maxVelocity) );
}

void iCubTorsoVelocityMotor::describe( QString type ) {
    iCubMotor::describe( type );
    Descriptor d = addTypeDescription( type, "Move the torso by velocity." );
    d.describeBool( "disableYaw" ).def(false).help( "Disable the movement of the first joint of the Torso corresponding to the yaw rotation" );
    d.describeBool( "disableRoll" ).def(false).help( "Disable the movement of the second joint of the Torso corresponding to the roll rotation" );
    d.describeBool( "disablePitch" ).def(false).help( "Disable the movement of the third joint of the Torso corresponding to the pitch rotation" );
    d.describeReal( "maxVelocity").def(50.0).limits(0,100).help( "The maximum velocity allowed." );
}

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

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

    NeuronsIterator* evonetIt = getResource<NeuronsIterator>( neuronsIteratorResource );
    evonetIt->setCurrentBlock( name() );
    for( int i=0; i<3; i++ ) {
        double vel = 0.0;
        if ( torsoDofs[i] ) {
            vel = (evonetIt->getOutput()-0.5)*2.0*maxVelocity;
            evonetIt->nextNeuron();
        }
        icubMotors->velocityMove( i, vel );
    }
}

int iCubTorsoVelocityMotor::size() {
    return torsoDofs[0]+torsoDofs[1]+torsoDofs[2];
}

void iCubTorsoVelocityMotor::resourceChanged(QString resourceName, ResourceChangeType changeType) {
    iCubMotor::resourceChanged(resourceName, changeType);

    if (changeType == Deleted) {
        return;
    }

    if (resourceName == icubResource) {
        icub = getResource<iCubRobot>();
        icubMotors = icub->torsoController();
    } else if (resourceName == neuronsIteratorResource) {
        NeuronsIterator* evonetIt = getResource<NeuronsIterator>();
        evonetIt->setCurrentBlock( name() );
        int numDofs = torsoDofs[0]+torsoDofs[1]+torsoDofs[2];
        for( int i=0; i<numDofs; i++, evonetIt->nextNeuron() ) {
            evonetIt->setGraphicProperties( QString("m")+QString::number(i), 0.0, 1.0, Qt::red );
        }
    } else {
        Logger::info("Unknown resource " + resourceName + " for " + name());
    }
}

} // end namespace farsa

#endif