experiments/evorobot/src/evorobotexperiment.cpp

/********************************************************************************
 *  FARSA Experiments Library                                                   *
 *  Copyright (C) 2007-2012                                                     *
 *  Stefano Nolfi <stefano.nolfi@istc.cnr.it>                                   *
 *  Onofrio Gigliotta <onofrio.gigliotta@istc.cnr.it>                           *
 *  Gianluca Massera <emmegian@yahoo.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 "evorobotexperiment.h"
#include "sensors.h"
#include "motors.h"
#include "evoga.h"
#include "configurationhelper.h"
#include "factory.h"
#include "logger.h"

#include <QFile>
#include <QTextStream>
#include <QString>
#include <QTime>
#include <QThread>
#include <iostream>
#include <cstdlib>

// All the suff below is to avoid warnings on Windows about the use of unsafe
// functions. This should be only a temporary workaround, the solution is stop
// using C string and file functions...
#if defined(_MSC_VER)
    #pragma warning(push)
    #pragma warning(disable:4996)
#endif

namespace farsa {

/*
 * Experiment constructor
 */
EvoRobotExperiment::EvoRobotExperiment()
{
    ga = NULL;
    evonet = NULL;
    neuronsIterator = new EvonetIterator();
    robot = NULL;
    world = NULL;
    savedConfigurationParameters = NULL;
    savedPrefix = NULL;
    ntrial = 0;
    stopCurrentTrial = false;
    skipCurrentTrial = false;
    restartCurrentTrial = false;
    endCurrentIndividualLife = false;

    gaPhase = NONE;

    // fitness and additional fitness components are not displayed by default

    // By default we don't run in batch mode
    batchRunning = false;

    // Stating which resources we use here. This is here in the constructor so that we are sure to
    // be the first to declare resources (if we did this later we should have used addUsableResources
    // because child classes could declare they will use resources before us)
    usableResources( QStringList() << "world" << "evonet" << "neuronsIterator" << "experiment" );
}

EvoRobotExperiment::~EvoRobotExperiment()
{
    delete savedConfigurationParameters;
    delete savedPrefix;
    delete evonet;
    for (int i = 0; i < sensors.size(); i++) {
        delete sensors[i];
    }
    for (int i = 0; i < motors.size(); i++) {
        delete motors[i];
    }
    delete world;
}

void EvoRobotExperiment::configure(ConfigurationParameters& params, QString prefix)
{
    // Saving configuration parameters and prefix for cloning
    delete savedConfigurationParameters;
    delete savedPrefix;
    savedConfigurationParameters = new ConfigurationParameters(params);
    savedPrefix = new QString(prefix);

    // Setting ourserlf as resource manger in the configuration parameters object
    params.setResourcesUser(this);

    ntrials = 1;
    nsteps = 1;

    batchRunning = ConfigurationHelper::getBool(params, "__INTERNAL__/BatchRunning", batchRunning); // If we are running in batch or not
    ntrials = ConfigurationHelper::getInt(params, prefix + "ntrials", ntrials); // number of trials to do
    nsteps = ConfigurationHelper::getInt(params, prefix + "nsteps", nsteps); // number of step for each trial

    // Reading the sensors parameters. For each sensor there must be a subgroup Sensor:NN where NN is a progressive number
    // (needed to specify the sensors order). Here we also actually create sensors
    QStringList sensorsList = params.getGroupsWithPrefixList(prefix, "Sensor:");
    sensorsList.sort();
    foreach( QString sensorGroup, sensorsList ) {
        Sensor* sensor = params.getObjectFromGroup<Sensor>(prefix + sensorGroup); 
        if ( sensor == NULL ) {
            Logger::error("Cannot create the Sensor from group " + prefix + sensorGroup + ". Aborting");
            abort();
        }
        // in order to avoid name clash when using more than one Sensor,
        // the Sensors are renamed using the same name of the Group when they don't have a name assigned
        if ( sensor->name() == QString("unnamed") ) {
            sensor->setName( sensorGroup );
        }
        sensors.append( sensor );
        sensors.last()->shareResourcesWith( this );
        Logger::info( "Created a Sensor named "+sensor->name() );
        // if the user manually set the name and create a name clash, it is only reported as error in Logger
        if ( sensorsMap.contains( sensor->name() ) ) {
            Logger::error( "More than one sensor has name "+sensor->name()+" !! The name has to be unique !!" );
        } else {
            // add to the map
            sensorsMap[sensor->name()] = sensor;
        }
    }

    // Now we do for motors what we did for sensors. Motor groups are in the form Motor:NN
    QStringList motorsList = params.getGroupsWithPrefixList(prefix, "Motor:");
    motorsList.sort();
    foreach( QString motorGroup, motorsList ) {
        Motor* motor = params.getObjectFromGroup<Motor>(prefix + motorGroup);
        if (motor == NULL) {
            Logger::error("Cannot create the Motor from group " + prefix + motorGroup + ". Aborting");
            abort();
        }
        // in order to avoid name clash when using more than one Motor,
        // the Motors are renamed using the same name of the Group when they don't have a name assigned
        if ( motor->name() == QString("unnamed") ) {
            motor->setName( motorGroup );
        }
        motors.append( motor );
        motors.last()->shareResourcesWith( this );
        Logger::info( "Created a Motor named "+motor->name() );
        // if the user manually set the name and create a name clash, it is only reported as error in Logger
        if ( motorsMap.contains( motor->name() ) ) {
            Logger::error( "More than one motor has name "+motor->name()+" !! The name has to be unique !!" );
        } else {
            // add to the map
            motorsMap[motor->name()] = motor;
        }
    }

    // create a World and iCub by default in order to exit from here with all configured properly
    // if they are already created it will not destroy and recreate
    recreateWorld();
    recreateRobot();
    // create the neural network - it will destroy the previous net
    recreateNeuralNetwork();
    // declaring the resources
    declareResource( "neuronsIterator", neuronsIterator, "evonet" );
    declareResource( "experiment", static_cast<ParameterSettableWithConfigureFunction*>(this) );

    Logger::info( params.getValue(prefix+"type") + " Configured" );
}

void EvoRobotExperiment::save(ConfigurationParameters&, QString)
{
    Logger::error("NOT IMPLEMENTED (EvoRobotExperiment::save)");
    abort();
}

void EvoRobotExperiment::describe( QString type ) {
    Descriptor d = addTypeDescription( type, "The experimental setup that defines the conditions and the fitness function of the evolutionary experiment" );
    d.describeInt( "ntrials" ).def(1).limits(1,MaxInteger).help("The number of trials the individual will be tested to calculate its fitness");
    d.describeInt( "nsteps" ).def(1).limits(1,MaxInteger).help("The number of step a trials will last");
    d.describeSubgroup( "NET" ).props( IsMandatory ).type( "Evonet" ).help( "The Neural Network controlling the robot");
    d.describeSubgroup( "ROBOT" ).props( IsMandatory ).type( "Robot" ).help( "The Neural Network controlling the robot");
    d.describeSubgroup( "Sensor" ).props( AllowMultiple ).type( "Sensor" ).help( "One of the Sensors from which the neural network will receive information about the environment" );
    d.describeSubgroup( "Motor" ).props( AllowMultiple ).type( "Motor" ).help( "One of the Motors with which the neural network acts on the robot and on the environment" );
}

void EvoRobotExperiment::postConfigureInitialization()
{
    if (!batchRunning) {
        // preventing gas from using multithread, which is not supported if the GUI is present
        ga->doNotUseMultipleThreads();
    }
    // Doing evolution by default
    gaPhase=EvoRobotExperiment::INEVOLUTION;
}

void EvoRobotExperiment::doTrial()
{
    restartCurrentTrial = false;
    stopCurrentTrial = false;
    trialFitnessValue = 0.0;
    for(nstep = 0; nstep < nsteps; nstep++) {
        initStep( nstep );
        if ( ga->commitStep() || restartCurrentTrial ) {
            break;
        }
        doStep();
        if ( ga->commitStep() ) break;
        endStep( nstep );
        if (ga->commitStep() || stopCurrentTrial || restartCurrentTrial) {
            break;
        }
    }
}

void EvoRobotExperiment::doAllTrialsForIndividual(int individual)
{
    endCurrentIndividualLife = false;
    totalFitnessValue = 0.0;

    initIndividual(individual);
    if ( ga->commitStep() ) return;

    for (ntrial = 0; ntrial < ntrials; ntrial++) {
        skipCurrentTrial = false;

        initTrial(ntrial);
        if ( ga->commitStep() ) break;
        if (skipCurrentTrial) { // && !ga->commitStep()) {
            continue;
        }
        if (!endCurrentIndividualLife) {
            doTrial();
        }
        if ( ga->isStopped() ) break;
        if (restartCurrentTrial) {
            ntrial--;
            continue;
        }
        endTrial(ntrial);

        if (gaPhase == INTEST) {
            Logger::info("Fitness for trial: " + QString::number(trialFitnessValue));
        }

        if ( ga->commitStep() || endCurrentIndividualLife ) {
            break;
        }

    }

    endIndividual(individual);
    ga->commitStep();
}

void EvoRobotExperiment::initGeneration(int)
{
}

void EvoRobotExperiment::initIndividual(int)
{
}

void EvoRobotExperiment::initTrial(int)
{
    // reset the neural controller
    ResourcesLocker locker(this);
    evonet->resetNet();
}

void EvoRobotExperiment::initStep(int)
{
}

double EvoRobotExperiment::getFitness()
{
    return totalFitnessValue;
}

void EvoRobotExperiment::afterSensorsUpdate()
{
}

void EvoRobotExperiment::beforeMotorsUpdate()
{
}

void EvoRobotExperiment::endTrial(int)
{
    totalFitnessValue += trialFitnessValue;
}

void EvoRobotExperiment::endIndividual(int)
{
}

void EvoRobotExperiment::endGeneration(int)
{
}

void EvoRobotExperiment::doStep()
{
    // There is no getResource below, but we are actually using resources so we must take the lock.
    // We don't acquire the lock here, but lock and unlock when needed in the body of the function
    ResourcesLocker locker(this, false);

    // update sensors
    for (int s = 0; s < sensors.size(); s++) {
        sensors[s]->update();
    }
    afterSensorsUpdate();
    // update the neural controller
    locker.lock();
    evonet->updateNet();
    locker.unlock();
    if (!batchRunning) {
        // To use platform-independent sleep functions we have to do this...
        class T : public QThread
        {
        public:
            using QThread::sleep;
            using QThread::msleep;
            using QThread::usleep;
        };
        // We sleep for few milliseconds when the GUI is active to avoid flooding it with
        // events and freeze it
        T::msleep(3);
    }
    beforeMotorsUpdate();
    // setting motors
    for (int m = 0; m < motors.size(); m++) {
        motors[m]->update();
    }
    // advance the world simulation
    locker.lock();
    world->advance();
    locker.unlock();
}

void EvoRobotExperiment::stopTrial()
{
    stopCurrentTrial = true;
}

void EvoRobotExperiment::skipTrial()
{
    skipCurrentTrial = true;
}

void EvoRobotExperiment::restartTrial()
{
    restartCurrentTrial = true;
}

void EvoRobotExperiment::endIndividualLife()
{
    endCurrentIndividualLife = true;
    stopCurrentTrial = true;
}

int EvoRobotExperiment::getGenomeLength()
{
    ResourcesLocker locker(this);

    return evonet->freeParameters();
}

Sensor* EvoRobotExperiment::getSensor( QString name ) {
    if ( sensorsMap.contains( name ) ) {
        return sensorsMap[name];
    } else {
        Logger::error( "getSensor returned NULL pointer because there is no sensor named "+name );
        return NULL;
    }
}

Motor* EvoRobotExperiment::getMotor( QString name ) {
    if ( motorsMap.contains( name ) ) {
        return motorsMap[name];
    } else {
        Logger::error( "getMotor returned NULL pointer because there is no motor named "+name );
        return NULL;
    }
}

bool EvoRobotExperiment::inBatchRunning()
{
    return batchRunning;
}

Evonet* EvoRobotExperiment::getNeuralNetwork()
{
    return evonet;
}

void EvoRobotExperiment::setEvoga( Evoga* ga )
{
    this->ga = ga;
}

Evoga* EvoRobotExperiment::getEvoga() {
    return ga;
}

void EvoRobotExperiment::setNetParameters(int *genes)
{
    ResourcesLocker locker(this);

    evonet->getParameters(genes);
}

void EvoRobotExperiment::setTestingAgentAndSeed(int,int)
{
    Logger::error("EvoRobotExperiment::setTestingAgentAndSeed() not yet implemented");
}

void EvoRobotExperiment::recreateWorld() {
    // Saving the old robot and world to delete them after the new world has been created (as
    // world is a resource, we need the old instance to exists during notifications. It can be
    // safely deleted afterward)
    Robot* const old_robot = robot;
    World* const old_world = world;
    robot = NULL;

    // TODO: parametrize the name and the dontUseYarp and all other parameters
    world = new World( "World", true );
    world->setTimeStep(0.05f);
    world->setSize( wVector( -2.0f, -2.0f, -0.50f ), wVector( +2.0f, +2.0f, +2.0f ) );
    world->setFrictionModel( "exact" );
    world->setSolverModel( "exact" );
    world->setMultiThread( 1 );

    // Removing deleted resources (if they existed) and then re-declaring world
    if ( old_robot != NULL ) {
        deleteResource( "robot" );
        robot = NULL;
    }
    declareResource( "world", world );

    // Now we can actually free memory
    delete old_robot;
    delete old_world;
}

void EvoRobotExperiment::recreateRobot() {
    // to be sure that a World exist
    if ( !world ) {
        recreateWorld();
    }

    // Taking lock because we need to use world
    ResourcesLocker locker(this);
    // Saving the old robot to delete it after the new robot has been created
    Robot* const old_robot = robot;

    // Now creating the robot. We first set ourself as the resouce manager
    savedConfigurationParameters->setResourcesUser(this);
    robot = savedConfigurationParameters->getObjectFromGroup<Robot>((*savedPrefix) + "ROBOT");

    // Unlocking before redeclaring the robot resource
    locker.unlock();

    declareResource( "robot", robot, "world" );
    delete old_robot;
}

void EvoRobotExperiment::recreateNeuralNetwork() {
    // Saving the old evonet to delete it after the new evonet has been created
    Evonet* const old_evonet = evonet;

    // Check the subgroup [NET]
    if ( savedConfigurationParameters->getValue( (*savedPrefix)+"NET/netFile" ).isEmpty() ) {
        // calculate the number of sensors and motors neurons
        int nSensors = 0;
        foreach( Sensor* sensor, sensors ) {
            nSensors += sensor->size();
        }
        int nMotors = 0;
        foreach( Motor* motor, motors ) {
            nMotors += motor->size();
        }
        // it inject the calculated nSensor and nMotors
        savedConfigurationParameters->createParameter( (*savedPrefix)+"NET", "nSensors", QString::number(nSensors) );
        savedConfigurationParameters->createParameter( (*savedPrefix)+"NET", "nMotors", QString::number(nMotors) );
    }
    // Now creating the neural network. We first set ourself as the resouce manager, then we lock resources (because during configuration
    // evonet could use resources, but the resource user it will use is not thread safe (SimpleResourceUser))
    ResourcesLocker locker(this);
    savedConfigurationParameters->setResourcesUser(this);
    evonet = savedConfigurationParameters->getObjectFromGroup<Evonet>( (*savedPrefix)+"NET" );
    locker.unlock();

    declareResource("evonet", static_cast<farsa::ParameterSettable*>(evonet));

    // Here we have to take the lock again because we are going to change neuronsIterator
    locker.lock();
    delete old_evonet;
    neuronsIterator->setEvonet( evonet );
    // create the blocks associated to the network
    int startIndex = 0;
    foreach( Sensor* sensor, sensors ) {
        neuronsIterator->defineBlock( sensor->name(), EvonetIterator::InputLayer, startIndex, sensor->size() );
        startIndex += sensor->size();
    }
    startIndex = 0;
    foreach( Motor* motor, motors ) {
        neuronsIterator->defineBlock( motor->name(), EvonetIterator::OutputLayer, startIndex, motor->size() );
        startIndex += motor->size();
    }
}

} // end namespace farsa

// All the suff below is to restore the warning state on Windows
#if defined(_MSC_VER)
    #pragma warning(pop)
#endif