motorcontrollers.cpp

/********************************************************************************
 *  WorldSim -- library for robot simulations                                   *
 *  Copyright (C) 2008-2011 Gianluca Massera <emmegian@yahoo.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  *
 ********************************************************************************/

#include "worldsimconfig.h"
#include "motorcontrollers.h"
#include "phyjoint.h"
#include "world.h"
#include "phymarxbot.h"
#include "phyfixed.h"
#include <cmath>
#include <QSet>

#ifdef FARSA_USE_YARP_AND_ICUB

#ifdef __GNUC__
#pragma GCC diagnostic ignored "-Wunused-parameter"
#endif
    #include <yarp/os/Searchable.h>
#ifdef __GNUC__
#pragma GCC diagnostic warning "-Wunused-parameter"
#endif

using namespace yarp::dev;
using namespace yarp::os;
#endif

namespace farsa {

wPID::wPID() {
    p_gain = 0;
    i_gain = 0;
    d_gain = 0;
    acc_ff = 0;
    fri_ff = 0;
    vel_ff = 0;
    bias = 0;
    accel_r = 0;
    setpt = 0;
    min = 0;
    max = 0;
    slew = 0;
    this_target = 0;
    next_target = 0;
    integral = 0;
    last_error = 0;
    last_output = 0;
}

double wPID::pidloop( double PV ) {
    double     this_error;
    double     this_output;
    double     accel;
    double     deriv;
    double     friction;
    /* the desired PV for this iteration is the value     */
    /* calculated as next_target during the last loop     */
    this_target = next_target;
    /* test for acceleration, compute new target PV for   */
    /* the next pass through the loop                     */
    if ( accel_r > 0 && this_target != setpt) {
        if ( this_target < setpt ) {
            next_target += accel_r;
            if ( next_target > setpt ) {
                next_target = setpt;
            }
        } else { /* this_target > setpoint */
            next_target -= accel_r;
            if ( next_target < setpt ) {
                next_target = setpt;
            }
        }
    } else {
        next_target = setpt;
    }
    /* acceleration is the difference between the PV      */
    /* target on this pass and the next pass through the  */
    /* loop                                               */
    accel = next_target - this_target;
    /* the error for the current pass is the difference   */
    /* between the current target and the current PV      */
    this_error = this_target - PV;
    /* the derivative is the difference between the error */
    /* for the current pass and the previous pass         */
    deriv = this_error - last_error;
    /* a very simple determination of whether there is    */
    /* special friction to be overcome on the next pass,  */
    /* if the current PV is 0 and the target for the next */
    /* pass is not 0, stiction could be a problem         */
    friction = ( PV == 0.0 && next_target != 0.0 );
    /* the new error is added to the integral             */
    integral += this_target - PV;
    /* now that all of the variable terms have been       */
    /* computed they can be multiplied by the appropriate */
    /* coefficients and the resulting products summed     */
    this_output = p_gain * this_error
                + i_gain * integral
                + d_gain * deriv
                + acc_ff * accel
                + vel_ff * next_target
                + fri_ff * friction
                + bias;
    last_error   = this_error;
    /* check for slew rate limiting on the output change  */
    if ( 0 != slew ) {
        if ( this_output - last_output > slew ) {
            this_output = last_output + slew;
        } else if ( last_output - this_output > slew ) {
            this_output = last_output - slew;
        }
    }
    /* now check the output value for absolute limits     */
    if ( this_output < min ) {
        this_output = min;
    } else if ( this_output > max ) {
        this_output = max;
    }
    /* store the new output value to be used as the old   */
    /* output value on the next loop pass                 */
    return last_output = this_output;
}

void wPID::reset() {
    // Resetting status variables
    this_target = 0;
    next_target = 0;
    integral = 0;
    last_error = 0;
    last_output = 0;
}

MotorController::MotorController( World* world ) {
    enabled = true;
    worldv = world;
}

MotorController::~MotorController() {
}


MultiMotorController::MultiMotorController( QVector<PhyDOF*> dofs, World* world ) :
    MotorController(world),
#ifdef FARSA_USE_YARP_AND_ICUB
    IPositionControl(),
    IVelocityControl(),
#endif
    motorsv(dofs)
{
    //--- common control variable initialization
    controlModes.fill( 1, motorsv.size() );
    refsacc.fill( 1.0, motorsv.size() );
    //--- IPositionControl variable initialization
    refs.fill( 0.0, motorsv.size() );
    refsvel.fill( 1.0, motorsv.size() );
    xfx0.fill( 0.0, motorsv.size() );
    x0.fill( 0.0, motorsv.size() );
    dx0.fill( 0.0, motorsv.size() );
    t.fill( 0.0, motorsv.size() );
    T.fill( 0.0, motorsv.size() );
    stops.fill( true, motorsv.size() );
    lastErrors.fill( 0.0, motorsv.size() );
    lastIs.fill( 0.0, motorsv.size() );
    lastTorques.fill( 0.0, motorsv.size() );
    // create PIDs with default values
    pos_pids.resize( motorsv.size() );
    for( int i=0; i<motorsv.size(); i++ ) {
        pos_pids[i] = new wPID();
        pos_pids[i]->p_gain = 0.8;
        pos_pids[i]->i_gain = 0.05;
        pos_pids[i]->d_gain = 0.0;
        pos_pids[i]->accel_r = 0;
        pos_pids[i]->min = -PI_GRECO/2.0;
        pos_pids[i]->max = +PI_GRECO/2.0;
    }
    //--- IVelocityControl variable initialization
    desiredVel.fill( 0.0, motorsv.size() );
    // create PIDs with default values
    vel_pids.resize( motorsv.size() );
    for( int i=0; i<motorsv.size(); i++ ) {
        vel_pids[i] = new wPID();
        vel_pids[i]->p_gain = 0.8;
        vel_pids[i]->i_gain = 0.1;
        vel_pids[i]->d_gain = 0.0;
        //vel_pids[i]->accel_r = 3;
        //vel_pids[i]->acc_ff = 0.3;
        vel_pids[i]->min = -toRad( 120 ); // rads / second
        vel_pids[i]->max = +toRad( 120 ); // rads / second
    }
    //--- IControlLimits variable initialization
    loLimits.resize( motorsv.size() );
    hiLimits.resize( motorsv.size() );
    for( int i=0; i<motorsv.size(); i++ ) {
        real fmin, fmax;
        motorsv[i]->limits( fmin, fmax );
        loLimits[i] = fmin;
        hiLimits[i] = fmax;
    }
}

MultiMotorController::~MultiMotorController() {
    for( int i=0; i<motorsv.size(); i++ ) {
        delete (vel_pids[i]);
        delete (pos_pids[i]);
    }
}

void MultiMotorController::update() {
    real accum, /*wishangle,*/ wishvel;
    for( int i=0; i<motorsv.size(); i++ ) {
        switch( controlModes[i] ) {
        case 1: /* Position Controller */
            if ( ! stops[i] ) {
                real tFracT = t[i] / T[i];
                real tFracT3= tFracT * tFracT * tFracT;
                real tFracT4= tFracT * tFracT3;
                real tFracT5= tFracT * tFracT4;

                //--- code from p5f code from iCub/src/controller4Dc/Code/trajectory.c
                //float xfx0 = _xf[jj] - _x0[jj];  --- already computed when positionMove has been called
                //float dx0  = _dx0[jj];           --- computed when positionMove has been called
                accum = dx0[i] * tFracT;
                accum = accum + tFracT3 * (10.0*xfx0[i] - 6.0*dx0[i]);
                accum = accum - tFracT4 * (15.0*xfx0[i] - 8.0*dx0[i]);
                accum = accum + tFracT5 * (6.0*xfx0[i] - 3.0*dx0[i]);
                //--- in p5f code are missing x0 because it's added inside step_trajectory
                accum += x0[i];
            } else {
                accum = refs[i];
            }
            //--- accum is the angle at which the joint must be at current time
            pos_pids[i]->setpt = accum;
//          wishangle = pos_pids[i]->pidloop( fromRawPosition(i) );
            motorsv[i]->setDesiredPosition( toRawPosition(accum, i) /*wishangle*/ );
            //--- advance t
            t[i] += world()->timeStep();
            //--- stop if necessary
            stops[i] = ( t[i] / T[i] > 1.0 );
        break;
        case 2: /* Velocity Controller */
            //--- the velocity at which the joint must be at current time
            vel_pids[i]->setpt = desiredVel[i];
            wishvel = vel_pids[i]->pidloop( motorsv[i]->velocity() );
            motorsv[i]->setDesiredVelocity( wishvel );
//          motorsv[i]->setDesiredVelocity(desiredVel[i]); // This disables the PID
            break;
        default:
            break;
        }
    }
    return;
}

#ifdef FARSA_USE_YARP_AND_ICUB

bool MultiMotorController::open( yarp::os::Searchable & ) {
    return true;
}

bool MultiMotorController::close( ) {
    return true;
}

bool MultiMotorController::configure( yarp::os::Searchable & ) {
    return true;
}

#endif

bool MultiMotorController::getAxes( int *ax ) {
    *ax = motorsv.size();
    return true;
}

bool MultiMotorController::setPositionMode() {
    //controlMode = 1;
    return true;
}

bool MultiMotorController::positionMove(int j, double ref) {
    if ( j < 0 && j >= motorsv.size() ) {
        return false;
    }
    if ( motorsv[j]->translate() ) {
        refs[j] = ref;
    } else {
        refs[j] = toRad( ref );
    }
    if ( motorsv[j]->isLimited() ) {
        //--- check the limits
        //--- stay too near to the limits may create oscillation of the joint
        //--- about 0.5% of the range of the joint
        real offset = (fabs(hiLimits[j])-fabs(loLimits[j]))*0.005;
        refs[j] = ramp( loLimits[j]+offset, hiLimits[j]-offset, refs[j] );
    }
    //--- calculate xfx0, dx0, T
    x0[j] = fromRawPosition(j);
    xfx0[j] = refs[j] - x0[j];
    dx0[j] = motorsv[j]->velocity();
    T[j] = fabs( xfx0[j] ) / refsvel[j];
    //--- reset t to zero
    t[j] = 0.0;
    // if T[j] is less that 10 ms, then it simply stay there
    stops[j] = ( T[j] < 0.010 );
    controlModes[j] = 1;
    return true;
}

bool MultiMotorController::positionMove(const double *newrefs) {
    for( int i=0; i<refs.size(); i++ ) {
        if ( motorsv[i]->translate() ) {
            refs[i] = newrefs[i];
        } else {
            refs[i] = toRad( newrefs[i] );
        }
        if ( motorsv[i]->isLimited() ) {
            //--- check the limits
            //--- stay too near to the limits may create oscillation of the joint
            //--- about 0.5% of the range of the joint
            real offset = (fabs(hiLimits[i])-fabs(loLimits[i]))*0.005;
            refs[i] = ramp( loLimits[i]+offset, hiLimits[i]-offset, refs[i] );
        }
        //--- calculate xfx0, dx0, T
        x0[i] = fromRawPosition( i );
        xfx0[i] = refs[i] - x0[i];
        dx0[i] = motorsv[i]->velocity();
        T[i] = fabs( xfx0[i] ) / refsvel[i];
        //--- reset t to zero
        t[i] = 0.0;
        // if T[j] is less that 10 ms, then it simply stay there
        stops[i] = ( T[i] < 0.010 );
        controlModes[i] = 1;
    }
    return true;
}

bool MultiMotorController::relativeMove(int j, double delta) {
    if ( j < 0 && j >= motorsv.size() ) {
        return false;
    }
    if ( motorsv[j]->translate() ) {
        refs[j] += delta;
    } else {
        refs[j] += toRad( delta );
    }
    if ( motorsv[j]->isLimited() ) {
        //--- check the limits
        //--- stay too near to the limits may create oscillation of the joint
        //--- about 0.5% of the range of the joint
        real offset = (fabs(hiLimits[j])-fabs(loLimits[j]))*0.005;
        refs[j] = ramp( loLimits[j]+offset, hiLimits[j]-offset, refs[j] );
    }
    //--- calculate xfx0, dx0, T
    x0[j] = fromRawPosition(j);
    xfx0[j] = refs[j] - x0[j];
    dx0[j] = motorsv[j]->velocity();
    T[j] = fabs( xfx0[j] ) / refsvel[j];
    //--- reset t to zero
    t[j] = 0.0;
    stops[j] = false;
    controlModes[j] = 1;
    return true;
}

bool MultiMotorController::relativeMove(const double *deltas) {
    for( int i=0; i<refs.size(); i++ ) {
        if ( motorsv[i]->translate() ) {
            refs[i] += deltas[i];
        } else {
            refs[i] += toRad( deltas[i] );
        }
        if ( motorsv[i]->isLimited() ) {
            //--- check the limits
            //--- stay too near to the limits may create oscillation of the joint
            //--- about 0.5% of the range of the joint
            real offset = (fabs(hiLimits[i])-fabs(loLimits[i]))*0.005;
            refs[i] = ramp( loLimits[i]+offset, hiLimits[i]-offset, refs[i] );
        }
        //--- calculate xfx0, dx0, T
        x0[i] = fromRawPosition( i );
        xfx0[i] = refs[i] - x0[i];
        dx0[i] = motorsv[i]->velocity();
        T[i] = fabs( xfx0[i] ) / refsvel[i];
        //--- reset t to zero
        t[i] = 0.0;
        stops[i] = false;
        controlModes[i] = 1;
    }
    return true;
}

bool MultiMotorController::checkMotionDone(int j, bool *flag) {
    if ( j < 0 && j >= motorsv.size() ) {
        return false;
    }
    //--- I should check the actual position or just if stops[j] is true ??
    *flag = ( fabs(fromRawPosition(j) - refs[j]) < 0.01745329 );
    //*flag = stops[j];
    return true;
}

bool MultiMotorController::checkMotionDone(bool *flag) {
    bool ends = true;
    for( int i=0; i<refs.size(); i++ ) {
        //--- I should check the actual position or just if stops[j] is true ??
        ends &= ( fabs(fromRawPosition(i) - refs[i]) < 0.01745329 );
        //ends &= stops[i];
        if ( !ends ) break;
    }
    *flag = ends;
    return true;
}

bool MultiMotorController::setRefSpeed(int j, double sp) {
    if ( j < 0 && j >= motorsv.size() ) {
        return false;
    }
    if ( motorsv[j]->translate() ) {
        refsvel[j] = sp;
    } else {
        refsvel[j] = toRad(sp);
    }
    motorsv[j]->setMaxVelocity( refsvel[j] );
    return true;
}

bool MultiMotorController::setRefSpeeds(const double *spds) {
    for( int i=0; i<refs.size(); i++ ) {
        if ( motorsv[i]->translate() ) {
            refsvel[i] = spds[i];
        } else {
            refsvel[i] = toRad(spds[i]);
        }
        motorsv[i]->setMaxVelocity( refsvel[i] );
    }
    return true;
}

bool MultiMotorController::setRefAcceleration(int j, double acc) {
    if ( j < 0 && j >= motorsv.size() ) {
        return false;
    }
    if ( motorsv[j]->translate() ) {
        refsacc[j] = acc;
    } else {
        refsacc[j] = toRad(acc);
    }
    return true;
}

bool MultiMotorController::setRefAccelerations(const double *accs) {
    for( int i=0; i<refs.size(); i++ ) {
        if ( motorsv[i]->translate() ) {
            refsacc[i] = accs[i];
        } else {
            refsacc[i] = toRad(accs[i]);
        }
    }
    return true;
}

bool MultiMotorController::getRefSpeed(int j, double *ref) {
    if ( j < 0 && j >= motorsv.size() ) {
        return false;
    }
    if ( motorsv[j]->translate() ) {
        *ref = refsvel[j];
    } else {
        *ref = toDegree(refsvel[j]);
    }
    return true;
}

bool MultiMotorController::getRefSpeeds(double *spds) {
    for( int i=0; i<refsvel.size(); i++ ) {
        if ( motorsv[i]->translate() ) {
            spds[i] = refsvel[i];
        } else {
            spds[i] = toDegree(refsvel[i]);
        }
    }
    return true;
}

bool MultiMotorController::getRefAcceleration(int j, double *acc) {
    if ( j < 0 && j >= motorsv.size() ) {
        return false;
    }
    if ( motorsv[j]->translate() ) {
        *acc = refsacc[j];
    } else {
        *acc = toDegree(refsacc[j]);
    }
    return true;
}

bool MultiMotorController::getRefAccelerations(double *accs) {
    for( int i=0; i<refsvel.size(); i++ ) {
        if ( motorsv[i]->translate() ) {
            accs[i] = refsacc[i];
        } else {
            accs[i] = toDegree(refsacc[i]);
        }
    }
    return true;
}

bool MultiMotorController::stop(int j) {
    if ( j < 0 && j >= motorsv.size() ) {
        return false;
    }
    refs[j] = fromRawPosition(j);
    stops[j] = true;
    desiredVel[j] = 0;
    motorsv[j]->setVelocity(0.0);
    controlModes[j] = 1;
    vel_pids[j]->reset();
    return true;
}

bool MultiMotorController::stop() {
    for( int i=0; i<motorsv.size(); i++ ) {
        refs[i] = fromRawPosition(i);
        stops[i] = true;
        desiredVel[i] = 0;
        motorsv[i]->setVelocity(0.0);
        controlModes[i] = 1;
        vel_pids[i]->reset();
    }
    return true;
}

bool MultiMotorController::setVelocityMode() {
    //controlMode = 2;
    return true;
}

bool MultiMotorController::velocityMove(int j, double sp) {
    if ( j < 0 && j >= motorsv.size() ) {
        return false;
    }
    controlModes[j] = 2;
    desiredVel[j] = toRad(sp);
    return true;
}

bool MultiMotorController::velocityMove(const double *sp) {
    for( int i=0; i<desiredVel.size(); i++ ) {
        desiredVel[i] = toRad(sp[i]);
        controlModes[i] = 2;
    }
    return true;
}

bool MultiMotorController::setPositionMode( int j ) {
    if ( j < 0 && j >= motorsv.size() ) {
        return false;
    }
    //--- set the position to actual position and stop the joint
    refs[j] = fromRawPosition(j);
    stops[j] = true;
    controlModes[j] = 1;
    return true;
}

bool MultiMotorController::setVelocityMode(int j ) {
    if ( j < 0 && j >= motorsv.size() ) {
        return false;
    }
    //--- set the desired velocity to zero and stop the joint
    desiredVel[j] = 0;
    stops[j] = true;
    controlModes[j] = 2;
    return true;
}

bool MultiMotorController::setTorqueMode( int j ) {
    if ( j < 0 && j >= motorsv.size() ) {
        return false;
    }
    //--- torque mode not implemented yet
    return false;
}

bool MultiMotorController::getControlMode( int j, int* mode ) {
    if ( j < 0 && j >= motorsv.size() ) {
        return false;
    }
    *mode = controlModes[j];
    return true;
}

bool MultiMotorController::getControlModes( int* modes ) {
    for( int i=0; i<motorsv.size(); i++ ) {
        modes[i] = controlModes[i];
    }
    return true;
}

bool MultiMotorController::setOpenLoopMode( int ) {
    return false;
}

bool MultiMotorController::resetEncoder( int ) {
    return true;
}

bool MultiMotorController::resetEncoders() {
    return true;
}

bool MultiMotorController::setEncoder( int, double ) {
    return true;
}

bool MultiMotorController::setEncoders( const double* ) {
    return true;
}

bool MultiMotorController::getEncoder( int j, double *v ) {
    if ( j < 0 && j >= motorsv.size() ) {
        return false;
    }
    if ( motorsv[j]->translate() ) {
        (*v) = fromRawPosition(j);
    } else {
        (*v) = toDegree( fromRawPosition(j) );
    }
    return true;
}

bool MultiMotorController::getEncoders( double *encs ) {
    for( int i=0; i<motorsv.size(); i++ ) {
        if ( motorsv[i]->translate() ) {
            encs[i] = fromRawPosition(i);
        } else {
            encs[i] = toDegree( fromRawPosition(i) );
        }
    }
    return true;
}

bool MultiMotorController::getEncoderSpeed( int j, double *sp ) {
    if ( j < 0 && j >= motorsv.size() ) {
        return false;
    }
    if ( motorsv[j]->translate() ) {
        (*sp) = motorsv[j]->velocity();
    } else {
        (*sp) = toDegree(motorsv[j]->velocity());
    }
    return true;
}

bool MultiMotorController::getEncoderSpeeds( double *spds ) {
    for( int i=0; i<motorsv.size(); i++ ) {
        if ( motorsv[i]->translate() ) {
            spds[i] = motorsv[i]->velocity();
        } else {
            spds[i] = toDegree(motorsv[i]->velocity());
        }
    }
    return true;
}

bool MultiMotorController::getEncoderAcceleration( int, double * ) {
    return false;
}

bool MultiMotorController::getEncoderAccelerations( double * ) {
    return false;
}

bool MultiMotorController::setLimits( int axis, double min, double max ) {
    if ( axis < 0 && axis >= motorsv.size() ) {
        return false;
    }
    if ( motorsv[axis]->translate() ) {
        loLimits[axis] = min;
        hiLimits[axis] = max;
    } else {
        loLimits[axis] = toRad( min );
        hiLimits[axis] = toRad( max );
    }
    return true;
}

bool MultiMotorController::getLimits( int axis, double *min, double *max ) {
    if ( axis < 0 && axis >= motorsv.size() ) {
        return false;
    }
    if ( motorsv[axis]->translate() ) {
        *min = loLimits[axis];
        *max = hiLimits[axis];
    } else {
        *min = toDegree( loLimits[axis] );
        *max = toDegree( hiLimits[axis] );
    }
    return true;
}

bool MultiMotorController::setLimitsRaw( int axis, double min, double max ) {
    if ( axis < 0 && axis >= motorsv.size() ) {
        return false;
    }
    if ( motorsv[axis]->translate() ) {
        motorsv[axis]->setLimits( min, max );
    } else {
        motorsv[axis]->setLimits( toRad(min), toRad(max) );
    }
    return true;
}

bool MultiMotorController::getLimitsRaw( int axis, double *min, double *max ) {
    if ( axis < 0 && axis >= motorsv.size() ) {
        return false;
    }
    real fmin, fmax;
    motorsv[axis]->limits( fmin, fmax );
    if ( motorsv[axis]->translate() ) {
        *min = fmin;
        *max = fmax;
    } else {
        *min = toDegree( fmin );
        *max = toDegree( fmax );
    }
    return true;
}

bool MultiMotorController::setEnableLimitsRaw( int axis, bool enable ) {
    if ( axis < 0 && axis >= motorsv.size() ) {
        return false;
    }
    if ( enable ) {
        motorsv[axis]->enableLimits();
    } else {
        motorsv[axis]->disableLimits();
    }
    return true;
}

real MultiMotorController::fromRawPosition( int i ) {
    real rmin, rmax;
    motorsv[i]->limits( rmin, rmax );
    return linearMap( motorsv[i]->position(), rmin, rmax, loLimits[i], hiLimits[i] );
}

real MultiMotorController::toRawPosition( real norawpos, int i ) {
    real rmin, rmax;
    motorsv[i]->limits( rmin, rmax );
    return linearMap( norawpos, loLimits[i], hiLimits[i], rmin, rmax );
}

void wheeledRobotsComputeKinematicMovement(wMatrix &mtr, real leftWheelVelocity, real rightWheelVelocity, real wheelr, real axletrack, real timestep)
{
    const wMatrix origMtr = mtr;

    // 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 * 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 * timestep;
        const wVector centerOfRotation = origMtr.transformVector(C - wVector(axletrack / 2.0, 0.0, 0.0));
        const wVector axisOfRotation = origMtr.z_ax;

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

WheelMotorController::WheelMotorController( QVector<PhyDOF*> wheels, World* world ) :
    MotorController(world),
    motorsv(wheels) {
    desiredVel.fill( 0.0, motorsv.size() );
    minVel.fill( -1.0, motorsv.size() );
    maxVel.fill( 1.0, motorsv.size() );
}

WheelMotorController::~WheelMotorController() {
    /* nothing to do */
}

void WheelMotorController::update() {
    for( int i=0; i<motorsv.size(); i++ ) {
        motorsv[i]->setDesiredVelocity( desiredVel[i] );
    }
}

void WheelMotorController::setSpeeds( QVector<double> speeds ) {
    for( int i=0; i<qMin(speeds.size(), desiredVel.size()); i++ ) {
        if ( speeds[i] < minVel[i] ) {
            desiredVel[i] = minVel[i];
        } else if ( speeds[i] > maxVel[i] ) {
            desiredVel[i] = maxVel[i];
        } else {
            desiredVel[i] = speeds[i];
        }
    }
}

void WheelMotorController::setSpeeds( double sp1, double sp2 ) {
    if ( sp1 < minVel[0] ) {
        desiredVel[0] = minVel[0];
    } else if ( sp1 > maxVel[0] ) {
        desiredVel[0] = maxVel[0];
    } else {
        desiredVel[0] = sp1;
    }

    if ( sp2 < minVel[1] ) {
        desiredVel[1] = minVel[1];
    } else if ( sp2 > maxVel[1] ) {
        desiredVel[1] = maxVel[1];
    } else {
        desiredVel[1] = sp2;
    }
}

void WheelMotorController::getSpeeds( QVector<double>& speeds ) {
    speeds.clear();
    for( int i=0; i<motorsv.size(); i++ ) {
        speeds.append(motorsv[i]->velocity());
    }
}

void WheelMotorController::getSpeeds( double& sp1, double& sp2 ) {
    sp1 = motorsv[0]->velocity();
    sp2 = motorsv[1]->velocity();
}

void WheelMotorController::setSpeedLimits( QVector<double> minSpeeds, QVector<double> maxSpeeds ) {
    minVel = minSpeeds;
    maxVel = maxSpeeds;
}

void WheelMotorController::setSpeedLimits( double minSpeed1, double minSpeed2, double maxSpeed1, double maxSpeed2 ) {
    minVel[0] = minSpeed1;
    minVel[1] = minSpeed2;
    maxVel[0] = maxSpeed1;
    maxVel[1] = maxSpeed2;
}

void WheelMotorController::getSpeedLimits( QVector<double>& minSpeeds, QVector<double>& maxSpeeds ) {
    minSpeeds = minVel;
    maxSpeeds = maxVel;
}

void WheelMotorController::getSpeedLimits( double& minSpeed1, double& minSpeed2, double& maxSpeed1, double& maxSpeed2 ) {
    minSpeed1 = minVel[0];
    minSpeed2 = minVel[1];
    maxSpeed1 = maxVel[0];
    maxSpeed2 = maxVel[1];
}

MarXbotAttachmentDeviceMotorController::MarXbotAttachmentDeviceMotorController(PhyMarXbot* robot) :
    MotorController(robot->world()),
    m_robot(robot),
    m_status(Open),
    m_desiredStatus(Open),
    m_joint(NULL),
    m_attachedRobot(NULL),
    m_otherAttachedRobots()
{
}

MarXbotAttachmentDeviceMotorController::~MarXbotAttachmentDeviceMotorController()
{
    // Destroying the joint
    delete m_joint;
}

void MarXbotAttachmentDeviceMotorController::update()
{
    // If the desired status is equal to the current status, doing nothing
    if (m_desiredStatus == m_status) {
        return;
    }

    // Updating the status
    const Status previousStatus = m_status;
    m_status = m_desiredStatus;
    switch (m_status) {
        case Open:
            {
                // Here we simply have to detach
                delete m_joint;
                m_joint = NULL;

                // Telling the other robot we are no longer attached
                if (m_attachedRobot != NULL) {
                    int i = m_attachedRobot->attachmentDeviceController()->m_otherAttachedRobots.indexOf(m_robot);
                    if (i != -1) {
                        m_attachedRobot->attachmentDeviceController()->m_otherAttachedRobots.remove(i);
                    }

                    m_attachedRobot = NULL;
                }
            }
            break;
        case HalfClosed:
            {
                if (previousStatus == Open) {
                    // Trying to attach
                    m_attachedRobot = tryToAttach();
                } else {
                    // If I was attached, the status was Closed for sure, so we have to change the kind
                    // of joint
                    if (m_attachedRobot != NULL) {
                        delete m_joint;
                    }
                }

                if (m_attachedRobot != NULL) {
                    // Found a robot to which we can attach, creating the joint (a hinge). The parent
                    // here is the other robot because this way it is easier to specify the axis and
                    // center
                    m_joint = new PhyHinge(wVector(1.0, 0.0, 0.0), wVector(0.0, 0.0, 0.0), 0.0, m_attachedRobot->turret(), m_robot->attachmentDevice());
                    m_joint->dofs()[0]->disableLimits();
                    m_joint->dofs()[0]->switchOff(); // This way the joint rotates freely

                    // Telling the other robot we are now attached (if we weren't already)
                    if (previousStatus == Open) {
                        m_attachedRobot->attachmentDeviceController()->m_otherAttachedRobots.append(m_robot);
                    }
                }
            }
            break;
        case Closed:
            {
                if (previousStatus == Open) {
                    // Trying to attach
                    m_attachedRobot = tryToAttach();
                } else {
                    // If I was attached, the status was HalfClosed for sure, so we have to change the kind
                    // of joint
                    if (m_attachedRobot != NULL) {
                        delete m_joint;
                    }
                }

                if (m_attachedRobot != NULL) {
                    // Found a robot to which we can attach, creating the joint (a fixed joint)
                    m_joint = new PhyFixed(m_robot->attachmentDevice(), m_attachedRobot->turret());

                    // Telling the other robot we are now attached (if we weren't already)
                    if (previousStatus == Open) {
                        m_attachedRobot->attachmentDeviceController()->m_otherAttachedRobots.append(m_robot);
                    }
                }
            }
            break;
    }
}

bool MarXbotAttachmentDeviceMotorController::attachmentDeviceEnabled() const
{
    return m_robot->attachmentDeviceEnabled();
}

void MarXbotAttachmentDeviceMotorController::setMaxVelocity(double speed)
{
    if (attachmentDeviceEnabled()) {
        m_robot->attachmentDeviceJoint()->dofs()[0]->setMaxVelocity(speed);
    }
}

double MarXbotAttachmentDeviceMotorController::getMaxVelocity() const
{
    if (attachmentDeviceEnabled()) {
        return m_robot->attachmentDeviceJoint()->dofs()[0]->maxVelocity();
    } else {
        return 0.0;
    }
}

void MarXbotAttachmentDeviceMotorController::setDesiredPosition(double pos)
{
    if (attachmentDeviceEnabled() && (!attachedToRobot())) {
        m_robot->attachmentDeviceJoint()->dofs()[0]->setDesiredPosition(pos);
    }
}

double MarXbotAttachmentDeviceMotorController::getDesiredPosition() const
{
    if (attachmentDeviceEnabled()) {
        return m_robot->attachmentDeviceJoint()->dofs()[0]->desiredPosition();
    } else {
        return 0.0;
    }
}

double MarXbotAttachmentDeviceMotorController::getPosition() const
{
    if (attachmentDeviceEnabled()) {
        return m_robot->attachmentDeviceJoint()->dofs()[0]->position();
    } else {
        return 0.0;
    }
}

void MarXbotAttachmentDeviceMotorController::setDesiredVelocity(double vel)
{
    if (attachmentDeviceEnabled() && (!attachedToRobot())) {
        m_robot->attachmentDeviceJoint()->dofs()[0]->setDesiredVelocity(vel);
    }
}

double MarXbotAttachmentDeviceMotorController::getDesiredVelocity() const
{
    if (attachmentDeviceEnabled()) {
        return m_robot->attachmentDeviceJoint()->dofs()[0]->desiredVelocity();
    } else {
        return 0.0;
    }
}

double MarXbotAttachmentDeviceMotorController::getVelocity() const
{
    if (attachmentDeviceEnabled()) {
        return m_robot->attachmentDeviceJoint()->dofs()[0]->velocity();
    } else {
        return 0.0;
    }
}

void MarXbotAttachmentDeviceMotorController::setDesiredStatus(Status status)
{
    if (attachmentDeviceEnabled()) {
        m_desiredStatus = status;
    }
}

PhyMarXbot* MarXbotAttachmentDeviceMotorController::tryToAttach() const
{
    // Checking the collisions of the attachment device
    const contactVec contacts = m_robot->world()->contacts()[m_robot->attachmentDevice()];

    // Now for each contact checking whether it is a turret of another PhyMarXbot and not
    // the attachment device. We create two sets: one contains robots whose attachment
    // device collides with our attachment device (we surely won't attach to these robots),
    // the other the robots whose turret collides with our attachment device (we could attach
    // to these robots)
    QSet<PhyMarXbot*> discardedRobots;
    QSet<PhyMarXbot*> candidateRobots;
    foreach (const Contact& c, contacts) {
        PhyMarXbot* otherRobot = dynamic_cast<PhyMarXbot*>(c.collide->owner());
        if ((otherRobot != NULL) && (otherRobot->attachmentDeviceEnabled()) && (!discardedRobots.contains(otherRobot))) {
            // Checking that the contact is the turret and not the attachment device
            if (c.collide == otherRobot->turret()) {
                // We have a candidate robot for attachment!
                candidateRobots.insert(otherRobot);
            } else if (c.collide == otherRobot->attachmentDevice()) {
                // We have to discard this robot
                discardedRobots.insert(otherRobot);
                candidateRobots.remove(otherRobot);
            }
        }
    }

    // Now we have a set of candidates. In practice we should always have at most one candidate due to
    // physical constraints. In case we have more than one, we simply return the first. If we have none,
    // we return NULL
    if (candidateRobots.isEmpty()) {
        return NULL;
    } else {
        return *(candidateRobots.begin());
    }
}

void MarXbotAttachmentDeviceMotorController::attachmentDeviceAboutToBeDestroyed()
{
    // Deleting the joint
    delete m_joint;
    m_status = Open;
    m_desiredStatus = Open;
    m_joint = NULL;

    // Telling the other robot we are no longer attached
    if (m_attachedRobot != NULL) {
        int i = m_attachedRobot->attachmentDeviceController()->m_otherAttachedRobots.indexOf(m_robot);
        if (i != -1) {
            m_attachedRobot->attachmentDeviceController()->m_otherAttachedRobots.remove(i);
        }
        m_attachedRobot = NULL;
    }

    // Also detachting robots attached to us
    foreach (PhyMarXbot* other, m_otherAttachedRobots) {
        // Destroying their joint and setting the robot to which they are attached to NULL
        delete other->attachmentDeviceController()->m_joint;
        other->attachmentDeviceController()->m_joint = NULL;
        other->attachmentDeviceController()->m_attachedRobot = NULL;
    }
}

} // end namespace farsa