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"
#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::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 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" );
    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, "motorWheel" );
        } else {
            // the four sphere supporting the track
            wtm.w_pos[2] = treaddepth;
            awheel = new PhySphere( treaddepth, w, "passiveWheel" );
        }
        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 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 );

    // 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();

        // To compute the actual movement of the robot I assume the movement is on the plane on which
        // the two wheels lie. This means that I assume that both wheels are in contact with a plane
        // at every time. Then I compute the instant centre of rotation and move the robot rotating
        // it around the axis passing through the instant center of rotation and perpendicular to the
        // plane on which the wheels lie (i.e. around the local XY plane)

        // First of all computing the linear speed of the two wheels
        const real leftSpeed = leftWheelVelocity * wheelr;
        const real rightSpeed = rightWheelVelocity * wheelr;

        // If the speed of the two wheels is very similar, simply moving the robot forward (doing the
        // computations would probably lead to invalid matrices)
        if (fabs(leftSpeed - rightSpeed) < 0.00001f) {
            // The front of the robot is towards -y
            mtr.w_pos += -mtr.y_ax.scale(rightSpeed * world()->timeStep());
        } else {
            // The first thing to do is to compute the instant centre of rotation. We do the
            // calculation in the robot local frame of reference. We proceed as follows (with
            // uppercase letters we indicate vectors and points):
            //
            // +------------------------->
            // |                       X axis
            // |    ^
            // |    |\
            // |    | \A
            // |    |  \
            // |   L|   ^
            // |    |   |\
            // |    |   | \
            // |    |  R|  \
            // |    |   |   \
            // |   O+--->----+C
            // |      D
            // |
            // |
            // v Y axis (the robot moves forward towards -Y)
            //
            // In the picture L and R are the velocity vectors of the two wheels, D is the vector
            // going from the center of the left wheel to the center of the right wheel, A is the
            // vector going from the tip of L to the tip of R and C is the instant centre of
            // rotation. D is fixed an parallel to the X axis, while L and R are always parallel
            // to the Y axis. Also, for the moment, we take the frame of reference origin at O.
            // The construction shown in the picture allows to find the instant center of rotation
            // for any L and R except when they are equal. In this case C is "at infinite" and the
            // robot moves forward of backward without rotation. All the vectors lie on the local
            // XY plane. To find C we proceed in the following way. First of all we note that
            // A = R - L. If a and b are two real numbers, then we can say that C is at the
            // instersection of L + aA with bD, that is we need a and b such that:
            //  L + aA = bD.
            // If we take the cross product on both sides we get:
            //  (L + aA) x D = bD x D => (L + aA) x D = 0
            // This means that we need to find a such that L + aA and D are parallel. As D is parallel
            // to the X axis, its Y component is 0, so we can impose that L + aA has a 0 Y component,
            // too, that is:
            //  (Ly) + a(Ay) = 0 => a = -(Ly) / (Ay)
            // Once we have a, we can easily compute C:
            //  C = L + aA
            // In the following code we use the same names we have used in the description above. Also
            // note that the actual origin of the frame of reference is not in O: it is in the middle
            // between the left and right wheel (we need to take this into account when computing the
            // point around which the robot rotates)
            const wVector L(0.0, -leftSpeed, 0.0);
            const wVector A(wheelstm[0].w_pos.x - wheelstm[1].w_pos.x, -(rightSpeed - leftSpeed), 0.0);
            const real a = -(L.y / A.y);
            const wVector C = L + A.scale(a);

            // Now we have to compute the angular velocity of the robot. This is simply equal to v/r where
            // v is the linear velocity at a point that is distant r from C. We use the velocity of one of
            // the wheel: which weels depends on its distance from C. We take the wheel which is furthest from
            // C to avoid having r = 0 (which would lead to an invalid value of the angular velocity). Distances
            // are signed (they are taken along the X axis)
            const real distLeftWheel = -C.x;
            const real distRightWheel = -(C.x - A.x); // A.x is equal to D.x
            const real angularVelocity = (fabs(distLeftWheel) > fabs(distRightWheel)) ? (-leftSpeed / distLeftWheel) : (-rightSpeed / distRightWheel);

            // The angular displacement is simply the angular velocity multiplied by the timeStep. Here we
            // also compute the center of rotation in world coordinates (we also need to compute it in the frame
            // of reference centered between the wheels, not in O) and the axis of rotation (that is simply the
            // z axis of the robot in world coordinates)
            const real angularDisplacement = angularVelocity * world()->timeStep();
            const wVector centerOfRotation = matrix().transformVector(C - wVector((wheelstm[0].w_pos.x - wheelstm[1].w_pos.x) / 2.0, 0.0, 0.0));
            const wVector axisOfRotation = matrix().z_ax;

            // Rotating the robot transformation matrix to obtain the new position
            mtr = mtr.rotateAround(axisOfRotation, centerOfRotation, angularDisplacement);
        }

        // 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);
        for (int i = 0; i < wheels.size(); i++) {
            wheels[i]->setKinematic(true);
        }
        attachring->setKinematic(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