phyepuck.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 "phyepuck.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 {

// All measures have been taken on the LARAL e-puck or the official specifications. Lengths are in
// meters, weights in kilograms.
const real PhyEpuck::batteryplacex = 0.043f;
const real PhyEpuck::batteryplacey = 0.055f;
const real PhyEpuck::batteryplacez = 0.033f;
const real PhyEpuck::batterym = 0.042f;
const real PhyEpuck::batteryplacedistancefromground = 0.0015f;
const real PhyEpuck::bodyr = 0.035f;
const real PhyEpuck::bodyh = 0.021f;
const real PhyEpuck::wholebodym = 0.078f;
const real PhyEpuck::wheelr = 0.0205f;
const real PhyEpuck::wheelh = 0.003f;
const real PhyEpuck::wheelm = 0.010f;
const real PhyEpuck::axletrack = 0.053f;
const real PhyEpuck::passivewheelr = 0.005f;
const real PhyEpuck::passivewheelm = 0.005f;

PhyEpuck::PhyEpuck(World* world, QString name, const wMatrix& transformation) :
    WObject(world, name, transformation, false),
    m_body(NULL),
    m_wheels(),
    m_wheelsTransformation(),
    m_wheelJoints(),
    m_wheelsCtrl(NULL),
    m_proximityIR(NULL),
    m_groundIR(NULL),
    m_kinematicSimulation(false),
    m_leftWheelVelocity(0.0f),
    m_rightWheelVelocity(0.0f)
{
    // --- reference frame
    //  X
    //  ^
    //  |       ------
    //  |       |    |
    //  |  -------------------
    //  |  |   battery       |
    //  |  |                 |
    //  |  -------------------
    //  |       |    |
    //  |       ------
    //  |
    //  +---------------------> Y
    // The front of the robot is towards -Y (positive speeds of the wheel make the robot move towards -Y)
    // and the camera is in -Y

    // Creating a material for the e-puck
    world->materials().createMaterial("epuckMaterial");
    world->materials().setProperties("epuckMaterial", "default", 0.0f, 0.0f, 0.01f, 0.01f, true);
    world->materials().createMaterial("epuckTire");
    world->materials().setProperties("epuckTire", "default", 1.2f, 0.9f, 0.01f, 0.01f, true);

    // Now creating the body of the e-puck. It is made up of two pieces: a box modelling the battery
    // pack and a short cylinder modelling the upper part (where the electronic board is). We put the
    // frame of reference of the body (which is equal to that of the whole robot) on the ground to have
    // move stability and to simplify moving the robot (no need to apply displacements to put it on a
    // plane)
    {
        QVector<PhyObject*> bodyObjs;
        wMatrix tm = wMatrix::identity();
        // First we create the battery pack
        PhyObject* obj = new PhyBox(batteryplacex, batteryplacey, batteryplacez, world);
        tm.w_pos.z = (batteryplacez / 2.0f) + batteryplacedistancefromground;
        obj->setMatrix(tm);
        bodyObjs << obj;
        // Then we create the upper part
        obj = new PhyCylinder(bodyr, bodyh, world);
        tm = wMatrix::yaw(toRad(90.0f));
        tm.w_pos.z = batteryplacez + batteryplacedistancefromground + (bodyh / 2.0f);
        obj->setMatrix( tm );
        bodyObjs << obj;
        // Finally we create the compound object actually modelling the body
        m_body = new PhyCompoundObject(bodyObjs, world, "base");
        m_body->setMass(batterym + wholebodym);
        m_body->setMaterial("epuckMaterial");
        m_body->setOwner(this, false);
        m_body->setMatrix(transformation);
    }

    // Creating all the wheels. We first create the two motorized wheels and then the two passive ones.
    // The position of the wheels and their indexes are as follows (remember that the front of the
    // robot is towards -Y):
    //
    //  X
    //  ^
    //  |       ------
    //  |       | 0  |
    //  |  -------------------
    //  |  |     battery     |
    //  |  | 3             2 |
    //  |  -------------------
    //  |       | 1  |
    //  |       ------
    //  |
    //  +---------------------> Y

    // Creating the first motorized wheel and its joint
    {
        // The matrix is relative to the base. First creating the wheel
        wMatrix tm = wMatrix::identity();
        tm.w_pos = wVector(axletrack / 2.0f, 0.0f, wheelr);
        PhyObject* wheel = new PhyCylinder(wheelr, wheelh, world, "motorWheel");
        wheel->setMass(wheelm);
        wheel->setColor(Qt::blue);
        wheel->setMaterial("epuckTire");
        wheel->setOwner(this, false);
        wheel->setMatrix(tm * m_body->matrix());
        m_wheels.append(wheel);
        m_wheelsTransformation.append(tm);

        // Now creating the joint
        PhyJoint* joint = new PhySuspension(m_body->matrix().x_ax, wheel->matrix().w_pos, m_body->matrix().z_ax, m_body, wheel);
        joint->dofs()[0]->disableLimits();
        joint->setOwner(this, false);
        joint->updateJointInfo();
        m_wheelJoints.append(joint);
    }

    // Creating the second motorized wheel and its joint
    {
        // The matrix is relative to the base. First creating the wheel
        wMatrix tm = wMatrix::identity();
        tm.w_pos = wVector(-axletrack / 2.0f, 0.0f, wheelr);
        PhyObject* wheel = new PhyCylinder(wheelr, wheelh, world, "motorWheel");
        wheel->setMass(wheelm);
        wheel->setColor(Qt::blue);
        wheel->setMaterial("epuckTire");
        wheel->setOwner(this, false);
        wheel->setMatrix(tm * m_body->matrix());
        m_wheels.append(wheel);
        m_wheelsTransformation.append(tm);

        // Now creating the joint
        PhyJoint* joint = new PhySuspension(m_body->matrix().x_ax, wheel->matrix().w_pos, m_body->matrix().z_ax, m_body, wheel);
        joint->dofs()[0]->disableLimits();
        joint->setOwner(this, false);
        joint->updateJointInfo();
        m_wheelJoints.append(joint);
    }

    // Creating the first passive wheel and its joint
    {
        // The matrix is relative to the base. First creating the wheel
        wMatrix tm = wMatrix::identity();
        tm.w_pos = wVector(0.0f, (batteryplacey / 2.0f) - passivewheelr, passivewheelr);
        PhyObject* wheel = new PhySphere(passivewheelr, world, "passiveWheel");
        wheel->setMass(passivewheelm);
        wheel->setColor(Qt::blue);
        wheel->setMaterial("epuckTire");
        wheel->setOwner(this, false);
        wheel->setMatrix(tm * m_body->matrix());
        m_wheels.append(wheel);
        m_wheelsTransformation.append(tm);

        // Now creating the joint
        PhyJoint* joint = new PhyBallAndSocket(wheel->matrix().w_pos, m_body, wheel);
        joint->setOwner(this, false);
        joint->updateJointInfo();
        m_wheelJoints.append(joint);
    }

    // Creating the second passive wheel and its joint
    {
        // The matrix is relative to the base. First creating the wheel
        wMatrix tm = wMatrix::identity();
        tm.w_pos = wVector(0.0f, -((batteryplacey / 2.0f) - passivewheelr), passivewheelr);
        PhyObject* wheel = new PhySphere(passivewheelr, world, "passiveWheel");
        wheel->setMass(passivewheelm);
        wheel->setColor(Qt::blue);
        wheel->setMaterial("epuckTire");
        wheel->setOwner(this, false);
        wheel->setMatrix(tm * m_body->matrix());
        m_wheels.append(wheel);
        m_wheelsTransformation.append(tm);

        // Now creating the joint
        PhyJoint* joint = new PhyBallAndSocket(wheel->matrix().w_pos, m_body, wheel);
        joint->setOwner(this, false);
        joint->updateJointInfo();
        m_wheelJoints.append(joint);
    }

    // Now we can create the motor controller
    QVector<PhyDOF*> motors;
    motors << m_wheelJoints[0]->dofs()[0] << m_wheelJoints[1]->dofs()[0];
    m_wheelsCtrl = new WheelMotorController(motors, world);
    m_wheelsCtrl->setOwner(this, false);
    // The min and max speed in rad/s have been derived for the e-puck wheel considering as maximum linear
    // displacement 14 cm/s (that for the give wheel radius is approximately 1.1 ratations per second)
    const real maxAngularSpeed = 1.1 * 2.0 * PI_GRECO;
    m_wheelsCtrl->setSpeedLimits(-maxAngularSpeed, -maxAngularSpeed, maxAngularSpeed, maxAngularSpeed);

    // Connecting wheels speed signals to be able to move the robot when in kinematic
    connect(m_wheelJoints[0]->dofs()[0], SIGNAL(changedDesiredVelocity(real)), this, SLOT(setRightWheelDesideredVelocity(real)));
    connect(m_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
#endif
    // Adding the sensors. The epuck has 8 proximity infrared sensors
    for (unsigned int i = 0; i < 8; i++) {
        const double curAngle = double(i) * (360.0 / 8.0) + ((360.0 / 8.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 = batteryplacez + batteryplacedistancefromground;

        sensors.append(SingleIR(m_body, mtr, 0.01, 0.05, 10.0, 5));
    }
    m_proximityIR = new SimulatedIRProximitySensorController(world, sensors);

    // Now creating the three ground IR sensors
    sensors.clear();
    wMatrix mtr = wMatrix::yaw(deg2rad(180.0));

    // Adding the sensors. The e-puck has 3 ground infrared sensors in the front part
    mtr.w_pos = wVector(-batteryplacex / 2.0f, -batteryplacey / 2.0f, batteryplacedistancefromground);
    sensors.append(SingleIR(m_body, mtr, 0.0, mtr.w_pos.z + 0.005, 0.0, 1));
    mtr.w_pos = wVector(0.0f, -batteryplacey / 2.0f, batteryplacedistancefromground);
    sensors.append(SingleIR(m_body, mtr, 0.0, mtr.w_pos.z + 0.005, 0.0, 1));
    mtr.w_pos = wVector(batteryplacex / 2.0f, -batteryplacey / 2.0f, batteryplacedistancefromground);
    sensors.append(SingleIR(m_body, mtr, 0.0, mtr.w_pos.z + 0.005, 0.0, 1));
    m_groundIR = new SimulatedIRGroundSensorController(world, sensors);

    world->pushObject(this);
}

PhyEpuck::~PhyEpuck()
{
    foreach (PhyJoint* j, m_wheelJoints) {
        delete j;
    }
    foreach (PhyObject* w, m_wheels) {
        delete w;
    }
    delete m_wheelsCtrl;
    delete m_body;
    delete m_proximityIR;
    delete m_groundIR;
}

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

    if (m_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 = m_leftWheelVelocity * wheelr;
        const real rightSpeed = m_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(axletrack, -(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(axletrack / 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 PhyEpuck::postUpdate()
{
    // Updating sensors
    if (m_proximityIR->isEnabled()) {
        m_proximityIR->update();
    }
    if (m_groundIR->isEnabled()) {
        m_groundIR->update();
    }

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

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

void PhyEpuck::setGroundIRSensorsGraphicalProperties(bool drawSensor, bool drawRay, bool drawRealRay)
{
    m_groundIR->setGraphicalProperties(drawSensor, drawRay, drawRealRay);
}

void PhyEpuck::doKinematicSimulation(bool k)
{
    if (m_kinematicSimulation == k) {
        return;
    }

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

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

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

void PhyEpuck::setLeftWheelDesideredVelocity(real velocity)
{
    m_leftWheelVelocity = velocity;
}

void PhyEpuck::setRightWheelDesideredVelocity(real velocity)
{
    m_rightWheelVelocity = velocity;
}

void PhyEpuck::changedMatrix()
{
    wMatrix tm = matrix();
    m_body->setMatrix(tm);
    for (int i = 0; i < m_wheels.size(); i++) {
        m_wheels[i]->setMatrix(m_wheelsTransformation[i] * tm);
    }
    foreach (PhyJoint* j, m_wheelJoints) {
        j->updateJointInfo();
    }
}

} // end namespace farsa