worldsim/include/phyjoint.h

/********************************************************************************
 *  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  *
 ********************************************************************************/

#ifndef PHYJOINT_H
#define PHYJOINT_H

#include "worldsimconfig.h"
#include "world.h"
#include "phyobject.h"
#include "wvector.h"
#include "wmatrix.h"
#include "wquaternion.h"
#include "ownable.h"
#include <QObject>

namespace farsa {

class PhyJoint;
class PhyJointPrivate;
class PhyObjectPrivate;
class WorldPrivate;
class Motor;

class FARSA_WSIM_API PhyDOF : public QObject {
    Q_OBJECT
public:
    enum MotionMode { force, vel, pos, off };

    PhyDOF( PhyJoint* parent, wVector axis, wVector centre, bool translate = false ) {
        this->ax = axis;
        this->istranslate = translate;
        this->centrev = centre;
        limitsOn = false;
        lolimit = -3.14f;
        hilimit = +3.14f;
        parentv = parent;
        desiredPos = 0.0;
        desiredVel = 0.0;
        motionMode = off;
        positionv = 0.0;
        velocityv = 0.0;
        stiffnessv = 0.99f;
        maxvelocityv = 1.5708f; // 90 degrees/sec
        forcea = 0.0;
        x_ax = wVector(0,0,0);
        y_ax = wVector(0,0,0);
    };

    PhyJoint* joint() {
        return parentv;
    };

    const PhyJoint* joint() const {
        return parentv;
    };

    real appliedForce() const {
        return forcea;
    };

    real desiredPosition() const {
        return desiredPos;
    };

    real desiredVelocity() const {
        return desiredVel;
    };

    MotionMode motion() const {
        return motionMode;
    };

    wVector centre() const {
        return centrev;
    };
    void setCentre( const wVector& cent ) {
        this->centrev = cent;
    };

    real position() const {
        return positionv;
    };
    void setPosition( real newpos ) {
        positionv = newpos;
        emit changedPosition( newpos );
    };

    real velocity() const {
        return velocityv;
    };
    void setVelocity( real newvel ) {
        velocityv = newvel;
        emit changedVelocity( newvel );
    };

    void limits( real& lo, real& hi ) const {
        lo = lolimit;
        hi = hilimit;
    };

    real stiffness() const {
        return stiffnessv;
    };

    void setStiffness( real newstiff ) {
        //--- clamp newstiff between 0 and 0.99
        stiffnessv = ramp( 0.0f, 0.99f, newstiff );
        emit changedStiffness( stiffnessv );
    };

    void setMaxVelocity( real maxvel ) {
        maxvelocityv = maxvel;
    };
    
    real maxVelocity() const {
        return maxvelocityv;
    };
    
    wVector axis() const {
        return ax;
    };
    void setAxis( const wVector& ax ) {
        this->ax = ax;
    };

    wVector xAxis() const {
        return x_ax;
    };
    void setXAxis( const wVector& x_ax ) {
        this->x_ax = x_ax;
    };

    wVector yAxis() const {
        return y_ax;
    };
    void setYAxis( const wVector& y_ax ) {
        this->y_ax = y_ax;
    };

    bool translate() const {
        return istranslate;
    };

    bool rotate() const {
        return !istranslate;
    };

    bool isLimited() const {
        return limitsOn;
    };

    void enableLimits() {
        limitsOn = true;
    };

    void disableLimits() {
        limitsOn = false;
    };
public slots:
    void applyForce( real newforce ) {
        forcea = newforce;
        motionMode = force;
        emit appliedForce( newforce );
        return;
    };
    void setDesiredPosition( real wishpos ) {
        //--- change the current desired position to avoid to push to the limits
        real offset = (fabs(hilimit)-fabs(lolimit))*0.005;
        desiredPos = ramp( lolimit+offset, hilimit-offset, wishpos );
        //desiredPos = wishpos;
        motionMode = pos;
        emit changedDesiredPosition( wishpos );
        return;
    };
    void setDesiredVelocity( real wishvel ) {
        desiredVel = wishvel;
        motionMode = vel;
        emit changedDesiredVelocity( wishvel );
        return;
    };
    void setLimits( real lowlimit, real highlimit ) {
#ifdef FARSA_DEBUG
        if ( !istranslate && lowlimit <= -PI_GRECO ) {
            qDebug() << "DOF Lower Limit must be greater than -pi";
        }
        if ( !istranslate && highlimit >= PI_GRECO ) {
            qDebug() << "DOF Higher Limit must be lesser that pi";
        }
#endif
        lolimit = lowlimit;
        hilimit = highlimit;
        //--- change the current desired position to avoid to push to the limits
        real offset = (fabs(hilimit)-fabs(lolimit))*0.005;
        desiredPos = ramp( lolimit+offset, hilimit-offset, desiredPos );
        emit changedLimits( lolimit, hilimit );
    };

signals:
    void appliedForce( real newforce );
    void changedDesiredPosition( real wishpos );
    void changedDesiredVelocity( real wishvel );
    void changedPosition( real newpos );
    void changedVelocity( real newvel );
    void changedStiffness( real newstiff );
    void changedLimits( real lowlimit, real highlimit );
private:
    PhyJoint* parentv;
    wVector ax;
    wVector x_ax;
    wVector y_ax;
    wVector centrev;
    bool istranslate;
    real stiffnessv;
    real forcea;
    real desiredPos;
    real desiredVel;
    MotionMode motionMode;
    real positionv;
    real velocityv;
    bool limitsOn;
    real lolimit;
    real hilimit;
    real maxvelocityv;
};

class FARSA_WSIM_API PhyJoint : public Ownable {
public:
    PhyJoint( PhyObject* parent, PhyObject* child );

    virtual ~PhyJoint();
    virtual PhyObject* child() {
        return childv;
    };
    virtual const PhyObject* child() const {
        return childv;
    };
    virtual PhyObject* parent() {
        return parentv;
    };
    virtual const PhyObject* parent() const {
        return parentv;
    };
    virtual unsigned int numDofs() const {
        return dofv;
    };
    virtual QVector<PhyDOF*> dofs() {
        return dofsv;
    };
    void enable( bool b );
    bool isEnabled() const {
        return enabled;
    };
    
    virtual wVector centre() const = 0;

    virtual void updateJointInfo() = 0;

    World* world() {
        return worldv;
    };

    const World* world() const {
        return worldv;
    };

    virtual void preUpdate() {
        /* nothing to do */
    };
    virtual void postUpdate() {
        /* nothing to do */
    };

protected:
    QVector<PhyDOF*> dofsv;
    int dofv;
    bool enabled;
    PhyObject* parentv;
    PhyObject* childv;
    World* worldv;

    PhyJointPrivate* priv;
    PhyObjectPrivate* parentpriv;
    PhyObjectPrivate* childpriv;
    WorldPrivate* worldpriv;
    friend class PhyJointPrivate;
    virtual void updateJoint( real timestep ) = 0;
};

} // end namespace farsa

#endif