worldsim/src/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 "mathutils.h"

namespace farsa {

PhyMarXbot::PhyMarXbot( World* w, QString name, const wMatrix& basetm ) : WObject( w, name, basetm, false ) {
    // --- reference frame
    //  X
    //  ^
    //  |       ------
    //  | ______|____|_________
    //  | |_|_| track |_|_|_|_|
    //  |  -------------------
    //  |  |   battery       |
    //  |  -------------------
    //  | |_|_| track |_|_|_|_|
    //  |       |    |
    //  |       ------
    //  |
    //   ---------------------> Y
    
    //measures and masses in MKS
    // --- chassis containing the battery
    real basex = 0.034f, basey = 0.143f, basez = 0.048f, basem = 0.4f;
    // --- cylinder body supporting infrared and other modules and turrets
    real bodyr = 0.085f, bodyh = 0.0055f, bodym = 0.02f;
    // --- inner part of the continous tracks (where the rubber tracks move on)
    real axledistance = 0.104f, trackradius = 0.022f, trackheight = 0.0295f,trackm = 0.05f;
    // --- the height of the treads on tracks and wheels
    real treaddepth = 0.004f;
    // --- external wheels (the radius is excluding the tread height)
    real wheelr = 0.027f, wheelh = 0.0215f, wheelm = 0.02f;
    // --- height of the attachment ring
    real attachringh = 0.0285f, attachringm = 0.08f;
    // --- module with 12 RGB LEDs
    real ledsh = 0.010f, ledsradius = 0.080f, ledsm = 0.03f;

    // --- create a material for marXbot
    w->materials().createMaterial( "marXbotMaterial" );
    w->materials().setProperties( "marXbotMaterial", "default", 0.0f, 0.0f, 0.01f, 0.01f, true );
    w->materials().createMaterial( "marXbotTire" );
    w->materials().setProperties( "marXbotTire", "default", 1.2f, 0.9f, 0.01f, 0.01f, true );

    // --- create the body of the base of the marxbot
    QVector<PhyObject*> objs;
    wMatrix tm = wMatrix::identity();
    // battery container
    PhyObject* 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;
    // merge all togheter
    base = new PhyCompoundObject( objs, w );
    base->setMaterial("marXbotMaterial");
    base->setOwner( this, false );
    base->setMatrix( basetm );
    
    // --- 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 );
        } else {
            // the four sphere supporting the track
            wtm.w_pos[2] = treaddepth;
            awheel = new PhySphere( treaddepth, w );
        }
        wheelstm.append( wtm );
        awheel->setMass( wheelm );
        awheel->setColor( Qt::blue );
        awheel->setMaterial( "marXbotTire" );
        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 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 );

    // --- create the attachment module with the 12 RGB LEDs part
    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;
    // merge all toghether
    attachring = new PhyCompoundObject( objs, w );
    attachring->setMaterial("marXbotMaterial");
    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 );

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

#ifdef __GNUC__
    #warning QUI DECIDERE PER I RANGE/APERTURE E LE POSIZIONI (SPECIE PER I GROUND). INOLTRE IL CENTRO DI MASSA DEL ROBOT È SUL TAVOLO, SOTTO LA BASE: NON È TROPPO BASSO?
#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);
    
    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;
}

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

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

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::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