experiments/src/motors.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>                         *
 *                                                                              *
 *  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  *
 ********************************************************************************/

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

namespace farsa {

//implementing fake motors
FakeMotor::FakeMotor( ConfigurationParameters& params, QString prefix ) :
    Motor(params, prefix)
    {
    nFakeMotors=0;
    nFakeMotors = ConfigurationHelper::getInt( params, prefix+"nMotors", 0 );
    // Declaring the resources that are needed here
    usableResources( QStringList() << "neuronsIterator" );
}

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

void FakeMotor::save( ConfigurationParameters& params, QString prefix ) {
    Motor::save( params, prefix );
    params.startObjectParameters( prefix, "FakeMotor", this );
    params.createParameter( prefix, "nMotors", QString::number(nFakeMotors) );
}

void FakeMotor::describe( QString type ) {
    Motor::describe( type );
    Descriptor d = addTypeDescription( type, "Fake motors: useful to add additional outputs (not linked with real motors) to the neural controller" );
    d.describeInt( "nFakeMotors" ).def(0).limits(0,100).props( IsMandatory ).help( "Numbers of additional motors not linked with any actual motors (i.e. fake). Useful to add additional outputs" );
}

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

int FakeMotor::size() {
    return nFakeMotors;
}

void FakeMotor::resourceChanged(QString resourceName, ResourceChangeType changeType) {
    if (changeType == Deleted) {
        resetNeededResourcesCheck();
        return;
    }

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

ProportionalController::ProportionalController(){

    setControlParameters(0.3,20);
}
ProportionalController::~ProportionalController(){
    /*so far, nothing to do */
}

void ProportionalController::setControlParameters(double k, double maxvel){
    if (k >= 0.0f) {
        m_k = k;
    }
    if (maxvel >= 0.0f) {
        m_maxvel = maxvel;
    }
}

double ProportionalController::getK() const {
    return m_k;
}

void ProportionalController::setK(double k){
    if (k >= 0.0f)
        m_k = k;
}
double ProportionalController::getMaxVelocity(){
    return m_maxvel;
}
void ProportionalController::setMaxVelocity(double maxVel){
    if (maxVel >= 0.0f)
        m_maxvel = maxVel;
}


double ProportionalController::velocityForJoint(double desired, double current){
    double vel = m_k * (desired - current);

    if (vel > +m_maxvel) {
        vel = +m_maxvel;
    } else if (vel < -m_maxvel) {
        vel = -m_maxvel;
    }

    return vel;
}

} // end namespace farsa