phymarxbot.cpp

/********************************************************************************
 *  WorldSim -- library for robot simulations                                   *
 *  Copyright (C) 2012-2013                                                     *
 *  Gianluca Massera <emmegian@yahoo.it>                                        *
 *  Fabrizio Papi <erkito87@gmail.com>                                          *
 *                                                                              *
 *  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 "phymarxbot.h"
#include "phybox.h"
#include "phycylinder.h"
#include "phyfixed.h"
#include "phyballandsocket.h"
#include "physphere.h"
#include "physuspension.h"
#include "phycompoundobject.h"
#include "phycone.h"
#include "phyhinge.h"
#include "mathutils.h"
#include "graphicalmarkers.h"
#include <cmath>

namespace farsa {

const real PhyMarXbot::basex = 0.034f;
const real PhyMarXbot::basey = 0.143f;
const real PhyMarXbot::basez = 0.048f;
const real PhyMarXbot::basem = 0.4f;
const real PhyMarXbot::bodyr = 0.085f;
const real PhyMarXbot::bodyh = 0.0055f;
const real PhyMarXbot::bodym = 0.02f;
const real PhyMarXbot::axledistance = 0.104f;
const real PhyMarXbot::trackradius = 0.022f;
const real PhyMarXbot::trackheight = 0.0295f;
const real PhyMarXbot::trackm = 0.05f;
const real PhyMarXbot::treaddepth = 0.004f;
const real PhyMarXbot::wheelr = 0.027f;
const real PhyMarXbot::wheelh = 0.0215f;
const real PhyMarXbot::wheelm = 0.02f;
const real PhyMarXbot::turreth = 0.0385f;
const real PhyMarXbot::turretm = 0.08f;
const real PhyMarXbot::attachdevr = bodyr;
const real PhyMarXbot::attachdevx = turreth * 0.8;
const real PhyMarXbot::attachdevy = turreth * 0.4;
const real PhyMarXbot::attachdevz = turreth * 0.2;
const real PhyMarXbot::attachdevm = 0.03;

PhyMarXbot::PhyMarXbot( World* w, QString name, const wMatrix& basetm ) : WObject( w, name, basetm, false ) {
    kinematicSimulation = false;
    frontMarker = NULL;

    // --- reference frame
    //  X
    //  ^
    //  |       ------
    //  | ______|____|_________
    //  | |_|_| track |_|_|_|_|
    //  |  -------------------
    //  |  |   battery       |
    //  |  -------------------
    //  | |_|_| track |_|_|_|_|
    //  |       |    |
    //  |       ------
    //  |
    //   ---------------------> Y
    // The front of the robot is towards -Y (positive speeds of the wheel make the robot move towards -Y)

    // --- create material for the marXbot
    //     introducing a semplification for the collision: two robot collide only with the attachring
    w->materials().createMaterial( "marXbotLowerBody" );
    w->materials().setElasticity( "marXbotLowerBody", "default", 0.01f );
    w->materials().setSoftness( "marXbotLowerBody", "default", 0.01f );
    w->materials().createMaterial( "marXbotUpperBody" );
    w->materials().enableCollision( "marXbotLowerBody", "marXbotLowerBody", false );
    w->materials().enableCollision( "marXbotLowerBody", "marXbotUpperBody", false );

    QVector<PhyObject*> objs;
    PhyObject* obj;
    wMatrix tm = wMatrix::identity();
    // --- create the body of the base of the marxbot
    obj = new PhyBox( basex+2.0f*trackheight, axledistance, basez+trackradius*2.0f, w );
    obj->setMass( basem+bodym );
    tm = wMatrix::identity();
    tm.w_pos[2] = (basez+trackradius*2.0f)/2.0f + treaddepth;
    obj->setMatrix( tm );
    objs << obj;
    // merge all togheter. Using a PhyCompoundObject even if there is only one object allows
    // to displace the transformation matrix
    basev = new PhyCompoundObject( objs, w, "base" );
    basev->setMatrix( basetm );
    basev->setMaterial("marXbotLowerBody");
    basev->setUseColorTextureOfOwner( false );
    basev->setOwner( this, false );

    // --- create the two external wheels
    //     they are motorized and they move the robot
    //     and create four spheres for supporting the tracks
    // NOTE: the tracks are not simulated, they are just rigid bodies and
    //       the four sphere move freely in order to approximate the real movement
    // --- position of wheels and their IDs in the vector
    //          +-----+
    //       |3||     ||2|
    //     |1|  |     |  |0|
    //       |5||     ||4|
    //          +-----+
    //  --------------------> X
    // arrays of X and Y offsets for calculate the positions
    double xoffs[6] = { 1, -1,  1, -1,  1, -1 };
    double yoffs[6] = { 0,  0,  1,  1, -1, -1 };
    PhyObject* awheel;
    PhyJoint* joint;
    for( int i=0; i<6; i++ ) {
        // --- relative to base
        wMatrix wtm = wMatrix::identity();
        wtm.w_pos = wVector(
            xoffs[i] * (basex + trackheight)/2.0f + (yoffs[i]==0 ? xoffs[i]*(trackheight/2.0+wheelh/2.0f+0.006f) : 0.0f),
            yoffs[i] * (axledistance/2.0f),
            0.0f );
        if ( i<2 ) {
            // the two motorized wheels
            wtm.w_pos[2] = wheelr;
            awheel = new PhyCylinder( wheelr, wheelh, w, "motorWheel" );
        } else {
            // the four sphere supporting the track
            wtm.w_pos[2] = treaddepth-0.003f;
            awheel = new PhySphere( treaddepth, w, "passiveWheel" );
        }
        wheelstm.append( wtm );
        awheel->setMass( wheelm );
        awheel->setMaterial( "marXbotLowerBody" );
        awheel->setUseColorTextureOfOwner( false );
        awheel->setOwner( this, false );
        awheel->setMatrix( wtm * basetm );
        wheels.append( awheel );

        //--- create the joints
        if ( i<2 ) {
            // the two motorized wheels
            joint = new PhySuspension(
                basev->matrix().x_ax, // rotation axis
                wheels[i]->matrix().w_pos,
                basev->matrix().z_ax, // suspension 'vertical' axis
                basev, wheels[i] );
            joint->dofs()[0]->disableLimits();
        } else {
            // the four sphere supporting the track
            joint = new PhyBallAndSocket(
                wheels[i]->matrix().w_pos,
                basev, wheels[i] );
        }
        joint->setOwner( this, false );
        joint->updateJointInfo();
        wheelJoints.append( joint );
    }

    // Creating the turret
    turretv = new PhyCylinder(bodyr, turreth, w);
    turretv->setMass(turretm);
    turrettm = wMatrix::yaw(toRad(-90.0f));
    turrettm.w_pos.z = 0.005f + basez + bodyh + turreth + 0.0015f;
    turretv->setMatrix(turrettm * basetm);
    turretv->setMaterial("marXbotUpperBody");
    turretv->setOwner(this, false);

    // --- create the joint for sensing the force
    forcesensor = new PhyFixed( basev, turretv, w );
    forcesensor->setOwner( this, false );

    // By default the attachement device is disabled
    attachdev = NULL;
    attachdevjoint = NULL;
    // We always create the attachment device controller, it will however not work unless the attachment
    // device is enabled
    attachdevCtrl = new MarXbotAttachmentDeviceMotorController(this);
    attachdevCtrl->setOwner( this, false );

    // create the motor controller
    QVector<PhyDOF*> motors;
    motors << wheelJoints[0]->dofs()[0] << wheelJoints[1]->dofs()[0];
    wheelsCtrl = new WheelMotorController( motors, w );
    wheelsCtrl->setOwner( this, false );
#ifdef __GNUC__
    #warning QUI DECIDERE PER LE VELOCITÀ MASSIME E MINIME DELLE RUOTE
#endif
    wheelsCtrl->setSpeedLimits( -10.0, -10.0, 10.0, 10.0 );

    // Connecting wheels speed signals to be able to move the robot when in kinematic
    connect(wheelJoints[0]->dofs()[0], SIGNAL(changedDesiredVelocity(real)), this, SLOT(setRightWheelDesideredVelocity(real)));
    connect(wheelJoints[1]->dofs()[0], SIGNAL(changedDesiredVelocity(real)), this, SLOT(setLeftWheelDesideredVelocity(real)));

    // Creating the proximity IR sensors
    QVector<SingleIR> sensors;

#ifdef __GNUC__
    #warning QUI DECIDERE PER I RANGE/APERTURE E LE POSIZIONI (SPECIE PER I GROUND)
#endif
    // Adding the sensors. The marxbot has 24 proximity infrared sensors
    for (unsigned int i = 0; i < 24; i++) {
        const double curAngle = double(i) * (360.0 / 24.0) + ((360.0 / 24.0) / 2.0);
        const double radius = bodyr;

        wMatrix mtr = wMatrix::yaw(deg2rad(curAngle + 90.0)) * wMatrix::pitch(deg2rad(90.0f));
        mtr.w_pos.x = radius * cos(deg2rad(curAngle));
        mtr.w_pos.y = radius * sin(deg2rad(curAngle));
        mtr.w_pos.z = 0.005f + basez + bodyh + turreth / 2.0f + 0.0015f;

        sensors.append(SingleIR(basev, mtr, 0.02, 0.1, 10.0, 5));
    }
    proximityIR = new SimulatedIRProximitySensorController(world(), sensors);

    // Now creating the ground IR sensors below the battery pack
    sensors.clear();
    wMatrix mtr = wMatrix::yaw(deg2rad(180.0));
    const double distFromBorder = 0.003;

    // Adding the sensors. The marxbot has 4 ground infrared sensors below the battery pack
    mtr.w_pos = wVector(basex / 2.0f + trackheight - distFromBorder, axledistance / 2.0f - distFromBorder, treaddepth);
    sensors.append(SingleIR(basev, mtr, 0.0, mtr.w_pos.z + 0.005, 0.0, 1));
    mtr.w_pos = wVector(basex / 2.0f + trackheight - distFromBorder, -axledistance / 2.0f + distFromBorder, treaddepth);
    sensors.append(SingleIR(basev, mtr, 0.0, mtr.w_pos.z + 0.005, 0.0, 1));
    mtr.w_pos = wVector(-basex / 2.0f - trackheight + distFromBorder, axledistance / 2.0f - distFromBorder, treaddepth);
    sensors.append(SingleIR(basev, mtr, 0.0, mtr.w_pos.z + 0.005, 0.0, 1));
    mtr.w_pos = wVector(-basex / 2.0f - trackheight + distFromBorder, -axledistance / 2.0f + distFromBorder, treaddepth);
    sensors.append(SingleIR(basev, mtr, 0.0, mtr.w_pos.z + 0.005, 0.0, 1));
    groundBottomIR = new SimulatedIRGroundSensorController(world(), sensors);

    // Creating the ground IR sensors below the base
    sensors.clear();

    // Adding the sensors. The marxbot has 8 ground infrared sensors below the base
    for (unsigned int i = 0; i < 8; i++) {
        const double curAngle = double(i) * (360.0 / 8.0);
        const double radius = bodyr - 0.003;

        wMatrix mtr = wMatrix::yaw(deg2rad(180.0));
        mtr.w_pos.x = radius * cos(deg2rad(curAngle));
        mtr.w_pos.y = radius * sin(deg2rad(curAngle));
        mtr.w_pos.z = 0.005f + basez + bodyh + turreth / 2.0f + 0.0015f;

        sensors.append(SingleIR(basev, mtr, 0.0, mtr.w_pos.z + 0.005, 0.0, 1));
    }
    groundAroundIR = new SimulatedIRGroundSensorController(world(), sensors);

    // Creating the traction sensor
    tractionSensor = new TractionSensorController(world(), *forcesensor);

    w->pushObject( this );
}

PhyMarXbot::~PhyMarXbot() {
    // First of all signalling we are about to delete the attachment device (and all the rest)
    attachdevCtrl->attachmentDeviceAboutToBeDestroyed();

    delete wheelsCtrl;
    delete attachdevCtrl;
    delete proximityIR;
    delete groundBottomIR;
    delete groundAroundIR;
    foreach( PhyJoint* susp, wheelJoints ) {
        delete susp;
    }
    foreach( PhyObject* w, wheels ) {
        delete w;
    }
    delete forcesensor;
    delete attachdevjoint;
    delete turretv;
    delete basev;
    delete attachdev;
}

void PhyMarXbot::preUpdate()
{
    // Updating motors
    if (wheelsCtrl->isEnabled()) {
        wheelsCtrl->update();
    }
    if (attachdevCtrl->isEnabled()) {
        attachdevCtrl->update();
    }

    if (kinematicSimulation) {
        // In kinematic mode, we have to manually move the robot depending on the wheel velocities
        wMatrix mtr = matrix();
        wheeledRobotsComputeKinematicMovement(mtr, leftWheelVelocity, rightWheelVelocity, wheelr, wheelstm[0].w_pos.x - wheelstm[1].w_pos.x, world()->timeStep());

        // This will also update the position of all pieces
        setMatrix(mtr);
    }
}

void PhyMarXbot::postUpdate()
{
    // Updating sensors
    if (proximityIR->isEnabled()) {
        proximityIR->update();
    }
    if (groundBottomIR->isEnabled()) {
        groundBottomIR->update();
    }
    if (groundAroundIR->isEnabled()) {
        groundAroundIR->update();
    }

    // Updating the transformation matrix of the robot. It is coincident with the matrix of the base
    tm = basev->matrix();
}

void PhyMarXbot::setProximityIRSensorsGraphicalProperties(bool drawSensor, bool drawRay, bool drawRealRay)
{
    proximityIR->setGraphicalProperties(drawSensor, drawRay, drawRealRay);
}

void PhyMarXbot::setGroundBottomIRSensorsGraphicalProperties(bool drawSensor, bool drawRay, bool drawRealRay)
{
    groundBottomIR->setGraphicalProperties(drawSensor, drawRay, drawRealRay);
}

void PhyMarXbot::setGroundAroundIRSensorsGraphicalProperties(bool drawSensor, bool drawRay, bool drawRealRay)
{
    groundAroundIR->setGraphicalProperties(drawSensor, drawRay, drawRealRay);
}

void PhyMarXbot::setDrawFrontMarker(bool drawMarker)
{
    if (getDrawFrontMarker() == drawMarker) {
        return;
    }

    if (drawMarker) {
        frontMarker = new PlanarArrowGraphicalMarker(PhyMarXbot::bodyr, PhyMarXbot::bodyr / 6.0f, PhyMarXbot::bodyr / 4.0f, 0.7f, world());
        frontMarker->setUseColorTextureOfOwner(false);
        frontMarker->setColor(Qt::green);
        frontMarker->setTexture("");

        wMatrix displacement = wMatrix::roll(-PI_GRECO / 2.0f);
        displacement.w_pos = wVector(0.0, 0.0, basez + trackradius * 2.0f + treaddepth + turreth - 0.010f + 0.0001f);
        frontMarker->attachToObject(this, true, displacement);
    } else {
        delete frontMarker;
        frontMarker = NULL;
    }
}

bool PhyMarXbot::getDrawFrontMarker() const
{
    return (frontMarker != NULL);
}

void PhyMarXbot::enableAttachmentDevice(bool enable)
{
    if (attachmentDeviceEnabled() == enable) {
        return;
    }

    if (enable) {
        // This little cube shows the position of the attachment fingers
        attachdev = new PhyBox(attachdevx, attachdevy, attachdevz, world());
        attachdev->setMass(attachdevm);
        attachdevtm = turretv->matrix();
        attachdevtm.w_pos.y -= attachdevr;
        attachdev->setMatrix(attachdevtm);
        attachdev->setOwner(this, false);

        // Creating the joint for the attachment device
        attachdevjoint = new PhyHinge(wVector(1.0, 0.0, 0.0), wVector(0.0, 0.0, 0.0), 0.0, turretv, attachdev, world());
        attachdevjoint->dofs()[0]->disableLimits();
        attachdevjoint->dofs()[0]->setDesiredPosition(0.0); // This way the attachdev doesn't rotate
        attachdevjoint->dofs()[0]->setStiffness(0.1); // This way the attachdev motor is not very strong
        attachdevjoint->setOwner(this, false);
    } else {
        // Notifying the attachment device controller
        attachdevCtrl->attachmentDeviceAboutToBeDestroyed();
        // Destroying the joint and the attachment device
        delete attachdevjoint;
        attachdevjoint = NULL;
        delete attachdev;
        attachdev = NULL;
    }
}

void PhyMarXbot::resetAttachmentDevice()
{
    // If the attachment device is enabled, we simply disable and then re-enable it
    if (attachmentDeviceEnabled()) {
        enableAttachmentDevice(false);
        enableAttachmentDevice(true);
    }
}

void PhyMarXbot::doKinematicSimulation(bool k)
{
    if (kinematicSimulation == k) {
        return;
    }

    kinematicSimulation = k;
    if (kinematicSimulation) {
        // First disabling all joints...
        for (int i = 0; i < wheelJoints.size(); i++) {
            wheelJoints[i]->enable(false);
        }
        forcesensor->enable(false);
        if (attachdevjoint != NULL) {
            attachdevjoint->enable(false);
        }

        // ... then setting all objects to kinematic behaviour
        basev->setKinematic(true, true);
        for (int i = 0; i < wheels.size(); i++) {
            wheels[i]->setKinematic(true, true);
        }
        turretv->setKinematic(true, true);
        if (attachdev != NULL) {
            attachdev->setKinematic(true, true);
        }
    } else {
        // First setting all objects to dynamic behaviour...
        basev->setKinematic(false);
        for (int i = 0; i < wheels.size(); i++) {
            wheels[i]->setKinematic(false);
        }
        turretv->setKinematic(false);
        if (attachdev != NULL) {
            attachdev->setKinematic(false);
        }

        // ... then enabling all joints (if the corresponding part is enabled)
        for (int i = 0; i < wheelJoints.size(); i++) {
            wheelJoints[i]->enable(true);
            wheelJoints[i]->updateJointInfo();
        }
        forcesensor->enable(true);
        forcesensor->updateJointInfo();
        if (attachdevjoint != NULL) {
            attachdevjoint->enable(true);
            attachdevjoint->updateJointInfo();
        }
    }
}

void PhyMarXbot::setLeftWheelDesideredVelocity(real velocity)
{
    leftWheelVelocity = velocity;
}

void PhyMarXbot::setRightWheelDesideredVelocity(real velocity)
{
    rightWheelVelocity = velocity;
}

void PhyMarXbot::changedMatrix() {
    wMatrix tm = matrix();
    basev->setMatrix( tm );
    for( int i=0; i<wheels.size(); i++ ) {
        wheels[i]->setMatrix( wheelstm[i] * tm );
    }
    foreach( PhyJoint* ajoint, wheelJoints ) {
        ajoint->updateJointInfo();
    }
    turretv->setMatrix( turrettm * tm );
    forcesensor->updateJointInfo();
    if ((attachdev != NULL) && (attachdevjoint != NULL)) {
        attachdev->setMatrix( attachdevtm * tm );
        attachdevjoint->updateJointInfo();
    }
}

namespace previous {
    const real PhyMarXbot::basex = 0.034f;
    const real PhyMarXbot::basey = 0.143f;
    const real PhyMarXbot::basez = 0.048f;
    const real PhyMarXbot::basem = 0.4f;
    const real PhyMarXbot::bodyr = 0.085f;
    const real PhyMarXbot::bodyh = 0.0055f;
    const real PhyMarXbot::bodym = 0.02f;
    const real PhyMarXbot::axledistance = 0.104f;
    const real PhyMarXbot::trackradius = 0.022f;
    const real PhyMarXbot::trackheight = 0.0295f;
    const real PhyMarXbot::trackm = 0.05f;
    const real PhyMarXbot::treaddepth = 0.004f;
    const real PhyMarXbot::wheelr = 0.027f;
    const real PhyMarXbot::wheelh = 0.0215f;
    const real PhyMarXbot::wheelm = 0.02f;
    const real PhyMarXbot::attachringh = 0.0285f;
    const real PhyMarXbot::attachringm = 0.08f;
    const real PhyMarXbot::ledsh = 0.010f;
    const real PhyMarXbot::ledsradius = 0.080f;
    const real PhyMarXbot::ledsm = 0.03f;

    PhyMarXbot::PhyMarXbot( World* w, QString name, const wMatrix& basetm ) : WObject( w, name, basetm, false ) {
        kinematicSimulation = false;

        // --- reference frame
        //  X
        //  ^
        //  |       ------
        //  | ______|____|_________
        //  | |_|_| track |_|_|_|_|
        //  |  -------------------
        //  |  |   battery       |
        //  |  -------------------
        //  | |_|_| track |_|_|_|_|
        //  |       |    |
        //  |       ------
        //  |
        //   ---------------------> Y
        // The front of the robot is towards -Y (positive speeds of the wheel make the robot move towards -Y)

        // --- create material for the marXbot
        //     introducing a semplification for the collision: two robot collide only with the attachring
        w->materials().createMaterial( "marXbotLowerBody" );
        w->materials().setElasticity( "marXbotLowerBody", "default", 0.01f );
        w->materials().setSoftness( "marXbotLowerBody", "default", 0.01f );
        w->materials().createMaterial( "marXbotUpperBody" );
        w->materials().enableCollision( "marXbotLowerBody", "marXbotLowerBody", false );
        w->materials().enableCollision( "marXbotLowerBody", "marXbotUpperBody", false );

        QVector<PhyObject*> objs;
        PhyObject* obj;
        // --- create the body of the base of the marxbot
    #ifdef MARXBOT_DETAILED
        wMatrix tm = wMatrix::identity();
        // battery container
        obj = new PhyBox( basex, basey, basez, w );
        tm.w_pos[2] = basez/2.0f + 0.015f;
        obj->setMass( basem );
        obj->setMatrix( tm );
        objs << obj;
        // cylinder body
        obj = new PhyCylinder( bodyr, bodyh, w );
        obj->setMass( bodym );
        tm = wMatrix::yaw( toRad(90.0f) );
        tm.w_pos[2] = 0.015f + basez + bodyh/2.0f;
        obj->setMatrix( tm );
        objs << obj;
        // right track
        obj = new PhyBox( trackheight, axledistance, trackradius*2.0f, w );
        tm = wMatrix::identity();
        tm.w_pos[0] = (basex + trackheight)/2.0f;
        tm.w_pos[2] = trackradius + treaddepth;
        obj->setMass( trackm );
        obj->setMatrix( tm );
        objs << obj;
        obj = new PhyCylinder( trackradius, trackheight, w );
        obj->setMass( wheelm );
        tm.w_pos[1] = axledistance/2.0;
        obj->setMatrix( tm );
        objs << obj;
        obj = new PhyCylinder( trackradius, trackheight, w );
        obj->setMass( wheelm );
        tm.w_pos[1] = -axledistance/2.0;
        obj->setMatrix( tm );
        objs << obj;
        // left track
        obj = new PhyBox( trackheight, axledistance, trackradius*2.0f, w );
        tm = wMatrix::identity();
        tm.w_pos[0] = -(basex + trackheight)/2.0f;
        tm.w_pos[2] = trackradius + treaddepth;
        obj->setMass( trackm );
        obj->setMatrix( tm );
        objs << obj;
        obj = new PhyCylinder( trackradius, trackheight, w );
        obj->setMass( wheelm );
        tm.w_pos[1] = axledistance/2.0;
        obj->setMatrix( tm );
        objs << obj;
        obj = new PhyCylinder( trackradius, trackheight, w );
        obj->setMass( wheelm );
        tm.w_pos[1] = -axledistance/2.0;
        obj->setMatrix( tm );
        objs << obj;
    #else
        // simplified version of the marxbot physic
        obj = new PhyBox( basex+2.0f*trackheight, axledistance, basez+trackradius*2.0f, w );
        obj->setMass( basem+bodym );
        tm = wMatrix::identity();
        tm.w_pos[2] = (basez+trackradius*2.0f)/2.0f + treaddepth;
        obj->setMatrix( tm );
        objs << obj;
    #endif
        // merge all togheter
        base = new PhyCompoundObject( objs, w, "base" );
        base->setMatrix( basetm );
        base->setMaterial("marXbotLowerBody");
        base->setOwner( this, false );

        // --- create the two external wheels
        //     they are motorized and they move the robot
        //     and create four spheres for supporting the tracks
        // NOTE: the tracks are not simulated, they are just rigid bodies and
        //       the four sphere move freely in order to approximate the real movement
        // --- position of wheels and their IDs in the vector
        //          +-----+
        //       |3||     ||2|
        //     |1|  |     |  |0|
        //       |5||     ||4|
        //          +-----+
        //  --------------------> X
        // arrays of X and Y offsets for calculate the positions
        double xoffs[6] = { 1, -1,  1, -1,  1, -1 };
        double yoffs[6] = { 0,  0,  1,  1, -1, -1 };
        PhyObject* awheel;
        PhyJoint* joint;
        for( int i=0; i<6; i++ ) {
            // --- relative to base
            wMatrix wtm = wMatrix::identity();
            wtm.w_pos = wVector(
                xoffs[i] * (basex + trackheight)/2.0f + (yoffs[i]==0 ? xoffs[i]*(trackheight/2.0+wheelh/2.0f+0.006f) : 0.0f),
                yoffs[i] * (axledistance/2.0f),
                0.0f );
            if ( i<2 ) {
                // the two motorized wheels
                wtm.w_pos[2] = wheelr;
                awheel = new PhyCylinder( wheelr, wheelh, w, "motorWheel" );
            } else {
                // the four sphere supporting the track
                wtm.w_pos[2] = treaddepth-0.003f;
                awheel = new PhySphere( treaddepth, w, "passiveWheel" );
            }
            wheelstm.append( wtm );
            awheel->setMass( wheelm );
            awheel->setColor( Qt::blue );
            awheel->setMaterial( "marXbotLowerBody" );
            awheel->setOwner( this, false );
            awheel->setMatrix( wtm * basetm );
            wheels.append( awheel );

            //--- create the joints
            if ( i<2 ) {
                // the two motorized wheels
                joint = new PhySuspension(
                    base->matrix().x_ax, // rotation axis
                    wheels[i]->matrix().w_pos,
                    base->matrix().z_ax, // suspension 'vertical' axis
                    base, wheels[i] );
                joint->dofs()[0]->disableLimits();
            } else {
                // the four sphere supporting the track
                joint = new PhyBallAndSocket(
                    wheels[i]->matrix().w_pos,
                    base, wheels[i] );
            }
            joint->setOwner( this, false );
            joint->updateJointInfo();
            wheelJoints.append( joint );
        }

        // --- create the attachment module with the 12 RGB LEDs part
    #ifdef MARXBOT_DETAILED
        objs.clear();
        obj = new PhyCone( bodyr, attachringh+0.010f, w );
        obj->setMass( attachringm );
        tm = wMatrix::identity();
        tm.w_pos[0] = 0.005f;
        obj->setMatrix( tm );
        objs << obj;
        obj = new PhyCone( bodyr, attachringh+0.010f, w );
        obj->setMass( attachringm );
        tm = wMatrix::roll( toRad(180.0f) );
        tm.w_pos[0] = -0.005f;
        obj->setMatrix( tm );
        objs << obj;
        obj = new PhyCylinder( ledsradius, ledsh, w );
        obj->setMass( ledsm );
        obj->setTexture( "marXbot_12leds" );
        obj->setUseColorTextureOfOwner( false );
        tm.w_pos[0] = attachringh/2.0f + ledsh/2.0f + 0.0015f;
        obj->setMatrix( tm );
        objs << obj;
    #else
        objs.clear();
        obj = new PhyCylinder( bodyr, attachringh+ledsh, w );
        obj->setMass( attachringm );
        tm = wMatrix::identity();
        tm.w_pos[0] = attachringh/2.0f-0.010f;
        obj->setMatrix( tm );
        objs << obj;
    #endif
        // merge all toghether
        attachring = new PhyCompoundObject( objs, w );
        attachring->setMaterial("marXbotUpperBody");
        attachring->setOwner( this, false );
        attachring->setUseColorTextureOfOwner( false );
        attachringtm = wMatrix::yaw( toRad(-90.0f) );
        attachringtm.w_pos[2] = 0.015f + basez + bodyh + attachringh/2.0f + 0.0015f;
        attachring->setMatrix( attachringtm * basetm );
        // --- create the joint for sensing the force
        forcesensor = new PhyFixed( base, attachring, w );
        forcesensor->setOwner( this, false );

        // create the motor controller
        QVector<PhyDOF*> motors;
        motors << wheelJoints[0]->dofs()[0] << wheelJoints[1]->dofs()[0];
        wheelsCtrl = new WheelMotorController( motors, w );
        wheelsCtrl->setOwner( this, false );
    #ifdef __GNUC__
        #warning QUI DECIDERE PER LE VELOCITÀ MASSIME E MINIME DELLE RUOTE
    #endif
        wheelsCtrl->setSpeedLimits( -10.0, -10.0, 10.0, 10.0 );

        // Connecting wheels speed signals to be able to move the robot when in kinematic
        connect(wheelJoints[0]->dofs()[0], SIGNAL(changedDesiredVelocity(real)), this, SLOT(setRightWheelDesideredVelocity(real)));
        connect(wheelJoints[1]->dofs()[0], SIGNAL(changedDesiredVelocity(real)), this, SLOT(setLeftWheelDesideredVelocity(real)));

        // Creating the proximity IR sensors
        QVector<SingleIR> sensors;

    #ifdef __GNUC__
        #warning QUI DECIDERE PER I RANGE/APERTURE E LE POSIZIONI (SPECIE PER I GROUND)
    #endif
        // Adding the sensors. The marxbot has 24 proximity infrared sensors
        for (unsigned int i = 0; i < 24; i++) {
            const double curAngle = double(i) * (360.0 / 24.0) + ((360.0 / 24.0) / 2.0);
            const double radius = bodyr;

            wMatrix mtr = wMatrix::yaw(deg2rad(curAngle + 90.0)) * wMatrix::pitch(deg2rad(90.0f));
            mtr.w_pos.x = radius * cos(deg2rad(curAngle));
            mtr.w_pos.y = radius * sin(deg2rad(curAngle));
            mtr.w_pos.z = 0.015f + basez + bodyh/2.0f;

            sensors.append(SingleIR(base, mtr, 0.02, 0.1, 10.0, 5));
        }
        proximityIR = new SimulatedIRProximitySensorController(world(), sensors);

        // Now creating the ground IR sensors below the battery pack
        sensors.clear();
        wMatrix mtr = wMatrix::yaw(deg2rad(180.0));
        const double distFromBorder = 0.003;

        // Adding the sensors. The marxbot has 4 ground infrared sensors below the battery pack
        mtr.w_pos = wVector(basex / 2.0f - distFromBorder, basey / 2.0f - distFromBorder, 0.015f);
        sensors.append(SingleIR(base, mtr, 0.0, mtr.w_pos.z + 0.005, 0.0, 1));
        mtr.w_pos = wVector(basex / 2.0f - distFromBorder, -basey / 2.0f + distFromBorder, 0.015f);
        sensors.append(SingleIR(base, mtr, 0.0, mtr.w_pos.z + 0.005, 0.0, 1));
        mtr.w_pos = wVector(-basex / 2.0f + distFromBorder, basey / 2.0f - distFromBorder, 0.015f);
        sensors.append(SingleIR(base, mtr, 0.0, mtr.w_pos.z + 0.005, 0.0, 1));
        mtr.w_pos = wVector(-basex / 2.0f + distFromBorder, -basey / 2.0f + distFromBorder, 0.015f);
        sensors.append(SingleIR(base, mtr, 0.0, mtr.w_pos.z + 0.005, 0.0, 1));
        groundBottomIR = new SimulatedIRGroundSensorController(world(), sensors);

        // Creating the ground IR sensors below the base
        sensors.clear();

        // Adding the sensors. The marxbot has 8 ground infrared sensors below the base
        for (unsigned int i = 0; i < 8; i++) {
            const double curAngle = double(i) * (360.0 / 8.0);
            const double radius = bodyr - 0.003;

            wMatrix mtr = wMatrix::yaw(deg2rad(180.0));
            mtr.w_pos.x = radius * cos(deg2rad(curAngle));
            mtr.w_pos.y = radius * sin(deg2rad(curAngle));
            mtr.w_pos.z = 0.015f + basez;

            sensors.append(SingleIR(base, mtr, 0.0, mtr.w_pos.z + 0.005, 0.0, 1));
        }
        groundAroundIR = new SimulatedIRGroundSensorController(world(), sensors);

        // Creating the traction sensor
        tractionSensor = new TractionSensorController(world(), *forcesensor);

        w->pushObject( this );
    }

    PhyMarXbot::~PhyMarXbot() {
        foreach( PhyJoint* susp, wheelJoints ) {
            delete susp;
        }
        foreach( PhyObject* w, wheels ) {
            delete w;
        }
        delete wheelsCtrl;
        delete forcesensor;
        delete attachring;
        delete base;
        delete proximityIR;
        delete groundBottomIR;
        delete groundAroundIR;
    }

    void PhyMarXbot::preUpdate()
    {
        // Updating motors
        if (wheelsCtrl->isEnabled()) {
            wheelsCtrl->update();
        }

        if (kinematicSimulation) {
            // In kinematic mode, we have to manually move the robot depending on the wheel velocities
            wMatrix mtr = matrix();
            wheeledRobotsComputeKinematicMovement(mtr, leftWheelVelocity, rightWheelVelocity, wheelr, wheelstm[0].w_pos.x - wheelstm[1].w_pos.x, world()->timeStep());

            // This will also update the position of all pieces
            setMatrix(mtr);
        }
    }

    void PhyMarXbot::postUpdate()
    {
        // Updating sensors
        if (proximityIR->isEnabled()) {
            proximityIR->update();
        }
        if (groundBottomIR->isEnabled()) {
            groundBottomIR->update();
        }
        if (groundAroundIR->isEnabled()) {
            groundAroundIR->update();
        }

        // Updating the transformation matrix of the robot. It is coincident with the matrix of the base
        tm = base->matrix();
    }

    void PhyMarXbot::setProximityIRSensorsGraphicalProperties(bool drawSensor, bool drawRay, bool drawRealRay)
    {
        proximityIR->setGraphicalProperties(drawSensor, drawRay, drawRealRay);
    }

    void PhyMarXbot::setGroundBottomIRSensorsGraphicalProperties(bool drawSensor, bool drawRay, bool drawRealRay)
    {
        groundBottomIR->setGraphicalProperties(drawSensor, drawRay, drawRealRay);
    }

    void PhyMarXbot::setGroundAroundIRSensorsGraphicalProperties(bool drawSensor, bool drawRay, bool drawRealRay)
    {
        groundAroundIR->setGraphicalProperties(drawSensor, drawRay, drawRealRay);
    }

    void PhyMarXbot::doKinematicSimulation(bool k)
    {
        if (kinematicSimulation == k) {
            return;
        }

        kinematicSimulation = k;
        if (kinematicSimulation) {
            // First disabling all joints...
            for (int i = 0; i < wheelJoints.size(); i++) {
                wheelJoints[i]->enable(false);
            }
            forcesensor->enable(false);

            // ... then setting all objects to kinematic behaviour
            base->setKinematic(true, true);
            for (int i = 0; i < wheels.size(); i++) {
                wheels[i]->setKinematic(true, true);
            }
            attachring->setKinematic(true, true);
        } else {
            // First setting all objects to dynamic behaviour...
            base->setKinematic(false);
            for (int i = 0; i < wheels.size(); i++) {
                wheels[i]->setKinematic(false);
            }
            attachring->setKinematic(false);

            // ... then enabling all joints (if the corresponding part is enabled)
            for (int i = 0; i < wheelJoints.size(); i++) {
                wheelJoints[i]->enable(true);
                wheelJoints[i]->updateJointInfo();
            }
            forcesensor->enable(true);
            forcesensor->updateJointInfo();
        }
    }

    void PhyMarXbot::setLeftWheelDesideredVelocity(real velocity)
    {
        leftWheelVelocity = velocity;
    }

    void PhyMarXbot::setRightWheelDesideredVelocity(real velocity)
    {
        rightWheelVelocity = velocity;
    }

    void PhyMarXbot::changedMatrix() {
        wMatrix tm = matrix();
        base->setMatrix( tm );
        for( int i=0; i<wheels.size(); i++ ) {
            wheels[i]->setMatrix( wheelstm[i] * tm );
        }
        foreach( PhyJoint* ajoint, wheelJoints ) {
            ajoint->updateJointInfo();
        }
        attachring->setMatrix( attachringtm * tm );
        forcesensor->updateJointInfo();
    }
}

} // end namespace farsa