icubsimulator.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  *
 ********************************************************************************/

#ifdef FARSA_USE_YARP_AND_ICUB

#include "icubsimulator.h"
#include "configurationhelper.h"
#include "randomgenerator.h"
#include "icubsimulatorviewer.h"
#include <QStringList>

namespace farsa {

iCubSimulator::iCubSimulator() :
    Component(),
    world(NULL),
    icub(NULL) {
    // the world and the icub will be created in the configure
}

iCubSimulator::~iCubSimulator() {
    delete icub;
    delete world;
}

ParameterSettableUI* iCubSimulator::getUIManager() {
    return new iCubSimulatorViewer( this );
}

void iCubSimulator::configure( ConfigurationParameters& params, QString prefix) {
    // eliminate any previous world, icub and objects created
    if ( world ) {
        delete icub;
        delete world;
    }
    // retrieve data about the world
    QString name = ConfigurationHelper::getString( params, prefix+"/WORLD/name", "world" );
    QVector<double> worldsize = ConfigurationHelper::getVector( params, prefix+"/WORLD/size" );
    // defaul min and max point for the dimension of the world
    wVector min( -2, -2, 0 );
    wVector max( +2, +2, 2 );
    // the min and max point are calculated from the size in the following way:
    //  - the X and Y dimension are divided in two in order to spawn in both direction from the center
    //  - the Z dimension start always from zero and spawn only in positive direction
    if ( worldsize.size() == 3 ) {
        min[0] = -worldsize[0]/2.0;
        min[1] = -worldsize[1]/2.0;
        min[2] = 0.0f;
        max[0] = +worldsize[0]/2.0;
        max[1] = +worldsize[1]/2.0;
        max[2] = worldsize[2];
    }
    world = new World( name );
    double timestep = ConfigurationHelper::getDouble( params, prefix+"/WORLD/timestep", 0.05 );
    bool realtime = ConfigurationHelper::getBool( params, prefix+"/WORLD/realTime", true );
    world->setSize( min, max );
    world->setTimeStep( timestep );
    world->setIsRealTime( realtime );
    // retrieve data about the iCub
    name = ConfigurationHelper::getString( params, prefix+"/ICUB/name", "icub" );
    QVector<double> icubpos = ConfigurationHelper::getVector( params, prefix+"/ICUB/position" );
    wMatrix icubtm = wMatrix::identity();
    if ( icubpos.size() == 3 ) {
        icubtm.w_pos[0] = icubpos[0];
        icubtm.w_pos[1] = icubpos[1];
        icubtm.w_pos[2] = icubpos[2];
    }
    icub = new PhyiCub( world, name, icubtm );
    // TODO: the OBJECT groups are now ignored

    setStatus( "Ready to Start" );
}

void iCubSimulator::save( ConfigurationParameters& params, QString prefix ) {
    QString worldGroup = params.createSubGroup( prefix, "WORLD" );
    params.createParameter( worldGroup, "name", world->name() );
    params.createParameter( worldGroup, "timestep", QString::number(world->timeStep()) );
    wVector min, max;
    world->size( min, max );
    params.createParameter( worldGroup, "size", QString::number(max[0]-min[0]) + " " + QString::number(max[1]-min[1]) + " " + QString::number(max[2]) );
    params.createParameter( worldGroup, "realTime", world->isRealTime() ? "true" : "false" );
    
    QString icubGroup = params.createSubGroup( prefix, "ICUB" );
    params.createParameter( icubGroup, "name", icub->name() );
    wMatrix tm = icub->matrix();
    params.createParameter( icubGroup, "position", QString::number(tm.w_pos[0]) + " " + QString::number(tm.w_pos[1]) + " " + QString::number(tm.w_pos[2]) );
    // TODO: OBJECTs are not saved
    
}

void iCubSimulator::postConfigureInitialization() {
}

void iCubSimulator::describe( QString type ) {
    Descriptor d = addTypeDescription( type, "iCub Simulator" );
    SubgroupDescriptor sub = d.describeSubgroup( "WORLD" ).props( IsMandatory ).help( "This group contains all parameters regarding the simulated world" );
        sub.describeString( "name" ).def( "world" ).props( IsMandatory ).help( "The name of the world - this will also correspond to the root of all yarp port created" );
        sub.describeReal( "timestep" ).def( 0.05 ).props( IsMandatory ).limits( 0.001, 0.1 ).help( "Number of seconds of a single step of simulation - for the best stability keep it below 0.1 (100 milliseconds)" );
        sub.describeReal( "size" ).props( IsList ).help( "The X, Y and Z dimension of the world in meters" );
        sub.describeBool( "realTime" ).def( true ).help( "If the advance of the simulation is real time or as fast as possible" );
    sub = d.describeSubgroup( "ICUB" ).props( IsMandatory ).help( "This group contains all parameters regarding the iCub robot");
        sub.describeString( "name" ).def( "icub" ).props( IsMandatory ).help( "The name of the iCub - this will also correspond to the root of all yarp port for controlling the robot" );
        sub.describeString( "position" ).props( IsList ).help( "The x, y and z position of the iCub in the world" );
    sub = d.describeSubgroup( "OBJECT" ).props( AllowMultiple ).help( "This group specifies the type and the parameters of an object to be inserted into the simulated world" );
        sub.describeString( "name" ).props( IsMandatory ).help( "The name of the object - this is necessary to access the object" );
        sub.describeEnum( "shape" ).props( IsMandatory ).values( QStringList() << "cube" << "sphere" << "cylinder" ).help( "The shape of the object to create: cube, sphere or cylinder" );
        sub.describeReal( "position" ).props( IsList ).help( "The x, y and z position of the object in the world" );
        sub.describeReal( "rotation" ).props( IsList ).help( "The three Euler angles representing the rotation of the object respect to the world" );
}

void iCubSimulator::start() {
    setStatus("Simulator Running");
}

void iCubSimulator::suspend() {
    setStatus("Simulator Suspended");
}

void iCubSimulator::step() {
    setStatus("Simulator in step-by-step mode");
}

} // end namespace farsa

#endif // FARSA_USE_YARP_AND_ICUB