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

#ifdef FARSA_USE_YARP_AND_ICUB

#ifdef __GNUC__
    #warning ================================================================================================================================================================================================================= PENSARE A DIVIDERE wVector IN UNA CLASSE PER I VETTORI (QUARTO ELEMENTO = 0) E UNA PER I PUNTI (QUARTO ELEMENTO = 1) =================================================================================================================================================================================================================
#endif

#include "phyicubhelpers.h"
#include "phyicub.h"
#include "phybox.h"
#include "phycylinder.h"
#include "physphere.h"
#include "phyfixed.h"
#include "phyhinge.h"
#include "phyuniversal.h"
#include "phycompoundobject.h"
#include "motorcontrollers.h"
#include "wcamera.h"

#ifdef __GNUC__
#pragma GCC diagnostic ignored "-Wunused-parameter"
#endif
    #include "yarp/os/ResourceFinder.h"
    #include "yarp/os/Bottle.h"
    #include "yarp/os/Property.h"
    #include "yarp/dev/PolyDriverList.h"
    #include "yarp/dev/Wrapper.h"
    #include "yarp/os/RateThread.h"
    #include "yarp/os/Network.h"
#ifdef __GNUC__
#pragma GCC diagnostic warning "-Wunused-parameter"
#endif

using namespace yarp::os;
using namespace yarp::dev;
using namespace iCub::iKin;

#include <iostream>

namespace farsa {
    // Perhaps we should move this function and the next one into wMatrix.h and wVector.h
    std::ostream& operator<<(std::ostream &os, const wMatrix &m)
    {
        os.precision(3);
        os.setf(std::ostream::fixed);
        os.unsetf(std::ostream::scientific);
        os << std::endl
           << "\t" << m[0][0] << "\t" << m[0][1] << "\t" << m[0][2] << "\t" << m[0][3] << std::endl
           << "\t" << m[1][0] << "\t" << m[1][1] << "\t" << m[1][2] << "\t" << m[1][3] << std::endl
           << "\t" << m[2][0] << "\t" << m[2][1] << "\t" << m[2][2] << "\t" << m[2][3] << std::endl
           << "\t" << m[3][0] << "\t" << m[3][1] << "\t" << m[3][2] << "\t" << m[3][3] << std::endl;

        return os;
    }

    std::ostream& operator<<(std::ostream &os, const wVector &v)
    {
        os.precision(3);
        os.setf(std::ostream::fixed);
        os.unsetf(std::ostream::scientific);
        os << "[" << v.x << ", " << v.y << ", " << v.z << ", " << v.w << "]";

        return os;
    }

    void convertYarpMatrixToWorldMatrix(const yarp::sig::Matrix &y, wMatrix &w)
    {
        // Extremely ugly, but works for sure... Yarp matrix are in conventional
        // order, wMatrix are like openGL matrices
        w[0][0] = y[0][0];
        w[0][1] = y[1][0];
        w[0][2] = y[2][0];
        w[0][3] = y[3][0];
        w[1][0] = y[0][1];
        w[1][1] = y[1][1];
        w[1][2] = y[2][1];
        w[1][3] = y[3][1];
        w[2][0] = y[0][2];
        w[2][1] = y[1][2];
        w[2][2] = y[2][2];
        w[2][3] = y[3][2];
        w[3][0] = y[0][3];
        w[3][1] = y[1][3];
        w[3][2] = y[2][3];
        w[3][3] = y[3][3];
    }

    void convertWorldMatrixToYarpMatrix(const wMatrix &w, yarp::sig::Matrix &y)
    {
        // Extremely ugly, but works for sure... Yarp matrix are in conventional
        // order, wMatrix are like openGL matrices
        y[0][0] = w[0][0];
        y[0][1] = w[1][0];
        y[0][2] = w[2][0];
        y[0][3] = w[3][0];
        y[1][0] = w[0][1];
        y[1][1] = w[1][1];
        y[1][2] = w[2][1];
        y[1][3] = w[3][1];
        y[2][0] = w[0][2];
        y[2][1] = w[1][2];
        y[2][2] = w[2][2];
        y[2][3] = w[3][2];
        y[3][0] = w[0][3];
        y[3][1] = w[1][3];
        y[3][2] = w[2][3];
        y[3][3] = w[3][3];
    }

    DOFStatusListener::DOFStatusListener(const PhyDOF* dof, Qt::ConnectionType connType) :
        QObject(NULL),
        m_dof(dof),
        m_curPosition(dof->position()),
        m_desideredPosition(dof->desiredPosition()),
        m_velocity(dof->desiredVelocity()),
        m_motionMode(dof->motion())
    {
        // Connecting our slots to those of the PhyDOF object
        connect(m_dof, SIGNAL(changedPosition(real)), this, SLOT(setLinkAngle(real)), connType);
        connect(m_dof, SIGNAL(changedDesiredPosition(real)), this, SLOT(setDesideredPosition(real)), connType);
        connect(m_dof, SIGNAL(changedDesiredVelocity(real)), this, SLOT(setDesideredVelocity(real)), connType);
    }

    DOFStatusListener::~DOFStatusListener()
    {
        // Nothing to do here
    }

    void DOFStatusListener::update()
    {
        switch (m_motionMode) {
            case PhyDOF::force:
                {
                    // Not supported (never will)
                }
                break;
            case PhyDOF::vel:
                {
                    // When setting the new position we also have to check limits here (ramp simply
                    // clamps the position within limits)
                    real lowLimit, highLimit;
                    m_dof->limits(lowLimit, highLimit);
                    m_curPosition = ramp(lowLimit, highLimit, m_curPosition + m_velocity * m_dof->joint()->world()->timeStep());
                    m_motionMode = PhyDOF::off;
                }
                break;
            case PhyDOF::pos:
                {
                    m_curPosition = m_desideredPosition;
                    m_motionMode = PhyDOF::off;
                }
                break;
            case PhyDOF::off:
                {
                    // Nothing to do, here
                }
                break;
        }
    }

    void DOFStatusListener::setLinkAngle(real newAngle)
    {
        m_curPosition = newAngle;
    }

    void DOFStatusListener::setDesideredPosition(real desideredAngle)
    {
        m_desideredPosition = desideredAngle;
        m_motionMode = PhyDOF::pos;
    }

    void DOFStatusListener::setDesideredVelocity(real velocity)
    {
        m_velocity = velocity;
        m_motionMode = PhyDOF::vel;
    }

    PhyObjectsGroup::PhyObjectsGroup(PhyObject* headObject, World* world, QString name) :
        WObject(world, name),
        m_headObject(headObject),
        m_objects(),
        m_joints()
    {
        // Setting the transformation matrix equal to the object matrix
        tm = m_headObject->matrix();
    }

    PhyObjectsGroup::~PhyObjectsGroup()
    {
        // Deleting all joint listeners
        for (QMap<ChainIDType, QVector<DOFAndListener> >::iterator it = m_joints.begin(); it != m_joints.end(); it++) {
            for (int i = 0; i < it.value().size(); i++) {
                delete it.value()[i].listener;
            }
        }
    }

    PhyObjectsGroup::ElementIDType PhyObjectsGroup::addObject(PhyObject* obj, PhyObjectsGroup::ChainIDType chainID)
    {
        // Storing matrix for the object relative to the head object frame of reference
        wMatrix invHeadMatrix = m_headObject->matrix().inverse();
        m_objects[chainID].push_back(ObjectAndMatrix(obj, obj->matrix() * invHeadMatrix));

        return (m_objects[chainID].size() - 1);
    }

    PhyObjectsGroup::ElementIDType PhyObjectsGroup::addJointDOF(PhyJoint* joint, unsigned int dofID, PhyObjectsGroup::ChainIDType chainID)
    {
        Q_ASSERT_X(joint->numDofs() > dofID, "PhyObjectsGroup::addJointDOF", "Joint has not the given dof");
        m_joints[chainID].push_back(DOFAndListener(joint, dofID, new DOFStatusListener(joint->dofs()[0])));

        return (m_joints[chainID].size() - 1);
    }

    void PhyObjectsGroup::setKinematic(bool b)
    {
        // Calling setKinematic on all objects
        m_headObject->setKinematic(b);
        for (QMap<ChainIDType, QVector<ObjectAndMatrix> >::iterator it = m_objects.begin(); it != m_objects.end(); it++) {
            for (int i = 0; i < it.value().size(); i++) {
                it.value()[i].object->setKinematic(b);
            }
        }
    }

    void PhyObjectsGroup::setStatic(bool b)
    {
        // Calling setStatic on all objects
        m_headObject->setStatic(b);
        for (QMap<ChainIDType, QVector<ObjectAndMatrix> >::iterator it = m_objects.begin(); it != m_objects.end(); it++) {
            for (int i = 0; i < it.value().size(); i++) {
                it.value()[i].object->setStatic(b);
            }
        }
    }

    void PhyObjectsGroup::enableJoints(bool b)
    {
        // Calling enable on all joints
        for (QMap<ChainIDType, QVector<DOFAndListener> >::iterator it = m_joints.begin(); it != m_joints.end(); it++) {
            for (int i = 0; i < it.value().size(); i++) {
                it.value()[i].joint->enable(b);
            }
        }
    }

    void PhyObjectsGroup::updateRelativePositions()
    {
        // Storing matrix for all objects relative to the head object frame of reference
        wMatrix invHeadMatrix = m_headObject->matrix().inverse();
        for (QMap<ChainIDType, QVector<ObjectAndMatrix> >::iterator it = m_objects.begin(); it != m_objects.end(); it++) {
            for (int i = 0; i < it.value().size(); i++) {
                it.value()[i].matrix = it.value()[i].object->matrix() * invHeadMatrix;
            }
        }
    }

    void PhyObjectsGroup::setDOFPosition(real pos, PhyObjectsGroup::ChainIDType chainID, PhyObjectsGroup::ElementIDType dof)
    {
        // First checking the joint and its child object exist (these are all asserts for performance reasons)
        Q_ASSERT_X(m_joints.contains(chainID), "PhyObjectsGroup::setDOFPosition", "No joint chain with the given ID");
        Q_ASSERT_X(m_joints[chainID].size() > dof, "PhyObjectsGroup::setDOFPosition", "No joint with the given ID in the given chain");
        Q_ASSERT_X(m_joints[chainID][dof].joint->dofs()[m_joints[chainID][dof].dofID]->rotate(), "PhyObjectsGroup::setDOFPosition", "Linear joints are not supported");
        Q_ASSERT_X(m_objects.contains(chainID), "PhyObjectsGroup::setDOFPosition", "No object chain with the given ID");
        Q_ASSERT_X(m_objects[chainID].size() > dof, "PhyObjectsGroup::setDOFPosition", "No object with the given ID in the given chain");

        QVector<ObjectAndMatrix>& objChain = m_objects[chainID];
        QVector<DOFAndListener>& jointChain = m_joints[chainID];
        PhyDOF* phydof = jointChain[dof].joint->dofs()[m_joints[chainID][dof].dofID];

        // Computing the new matrix relative to the head object for all elements after the given joint
        for (ElementIDType i = dof; i < objChain.size(); i++) {
            const wMatrix newMatrixWorldCoordinate = objChain[i].object->matrix().rotateAround(-phydof->axis(), phydof->centre(), pos - phydof->position());
            objChain[i].matrix = newMatrixWorldCoordinate * matrix().inverse();
        }

        // Also updating joint and moving object (first objects, then joints because joints
        // need both the parent and the child in the correct position)
        for (ElementIDType i = dof; i < objChain.size(); i++) {
            objChain[i].object->setMatrix(objChain[i].matrix * matrix());
        }
        for (ElementIDType i = dof; i < jointChain.size(); i++) {
            jointChain[i].joint->updateJointInfo();
        }
    }

    void PhyObjectsGroup::updateFromDOF()
    {
        // This could be made a bit more efficient if there was a function like setDOFPosition
        // taking pointer to objects instead of their indexes. However most of the work is in
        // computing matrix multiplication, so for the moment it is ok this way

        // Here we simply cycle through the list dofs for each kinematic chain and call setDOFPosition
        for (QMap<ChainIDType, QVector<DOFAndListener> >::iterator it = m_joints.begin(); it != m_joints.end(); it++) {
            for (int i = 0; i < it.value().size(); i++) {
                it.value()[i].listener->update();
                setDOFPosition(it.value()[i].listener->position(), it.key(), i);
            }
        }
    }

    void PhyObjectsGroup::preUpdate()
    {
    }

    void PhyObjectsGroup::postUpdate()
    {
    }

    void PhyObjectsGroup::changedMatrix()
    {
        // First of all setting the transformation matrix for the head object to the
        // matrix for this object
        m_headObject->setMatrix(matrix());

        // Now setting transformation matrices for all other objects
        for (QMap<ChainIDType, QVector<ObjectAndMatrix> >::iterator it = m_objects.begin(); it != m_objects.end(); it++) {
            for (int i = 0; i < it.value().size(); i++) {
                it.value()[i].object->setMatrix(it.value()[i].matrix * matrix());
            }
        }

        // Finally updating all joints
        for (QMap<ChainIDType, QVector<DOFAndListener> >::iterator it = m_joints.begin(); it != m_joints.end(); it++) {
            for (int i = 0; i < it.value().size(); i++) {
                it.value()[i].joint->updateJointInfo();
            }
        }
    }

    KinematicLinkInfo::KinematicLinkInfo() :
        QObject(),
        m_buddies(),
        m_isMasterBuddy(true),
        m_chain(NULL),
        m_linkID(0),
        m_wObject(NULL),
        m_objectMatrix(wMatrix::identity()),
        m_axis(),
        m_bottom(),
        m_top(),
        m_height(0.0),
        m_worldTm(&s_identityMatrix),
        m_phyJoint(NULL),
        m_phyJointDOF(0),
        m_invertedJointAxis(true),
        m_nextAngle(0.0),
        m_velocity(0.0),
        m_motionMode(PhyDOF::off),
        m_linkAngleUpdated(true)
    {
    }

    KinematicLinkInfo::KinematicLinkInfo(iCub::iKin::iKinLimb* chain, unsigned int linkID) :
        QObject(),
        m_buddies(),
        m_isMasterBuddy(true),
        m_chain(chain),
        m_linkID(linkID),
        m_wObject(NULL),
        m_objectMatrix(wMatrix::identity()),
        m_axis(),
        m_bottom(),
        m_top(),
        m_height(0.0),
        m_worldTm(&s_identityMatrix),
        m_phyJoint(NULL),
        m_phyJointDOF(0),
        m_invertedJointAxis(true),
        m_nextAngle(0.0),
        m_velocity(0.0),
        m_motionMode(PhyDOF::off),
        m_linkAngleUpdated(true)
    {
        updateInformationFromiKin();
    }

    KinematicLinkInfo::KinematicLinkInfo(const KinematicLinkInfo& other) :
        QObject(),
        m_buddies(other.m_buddies),
        m_isMasterBuddy(other.m_isMasterBuddy),
        m_chain(other.m_chain),
        m_linkID(other.m_linkID),
        m_wObject(other.m_wObject),
        m_objectMatrix(other.m_objectMatrix),
        m_axis(other.m_axis),
        m_bottom(other.m_bottom),
        m_top(other.m_top),
        m_height(other.m_height),
        m_worldTm(other.m_worldTm),
        m_phyJoint(other.m_phyJoint),
        m_phyJointDOF(other.m_phyJointDOF),
        m_invertedJointAxis(other.m_invertedJointAxis),
        m_nextAngle(other.m_nextAngle),
        m_velocity(other.m_velocity),
        m_motionMode(other.m_motionMode),
        m_linkAngleUpdated(other.m_linkAngleUpdated)
    {
        // Adding the object we are copying in the list of our buddies and removing ourself
        m_buddies.insert(const_cast<KinematicLinkInfo*>(&other));
        m_buddies.remove(this);

        // Now adding ourself to our buddies' buddies
        foreach (KinematicLinkInfo* buddy, m_buddies) {
            buddy->m_buddies.insert(this);
        }
    }

    KinematicLinkInfo& KinematicLinkInfo::operator=(const KinematicLinkInfo& other)
    {
        if (&other == this) {
            return *this;
        }

        m_chain = other.m_chain;
        m_isMasterBuddy = other.m_isMasterBuddy;
        m_linkID = other.m_linkID;
        m_wObject = other.m_wObject;
        m_objectMatrix = other.m_objectMatrix;
        m_axis = other.m_axis;
        m_bottom = other.m_bottom;
        m_top = other.m_top;
        m_height = other.m_height;
        m_worldTm = other.m_worldTm;
        m_phyJoint = other.m_phyJoint;
        m_phyJointDOF = other.m_phyJointDOF;
        m_invertedJointAxis = other.m_invertedJointAxis;
        m_nextAngle = other.m_nextAngle;
        m_velocity = other.m_velocity;
        m_motionMode = other.m_motionMode;
        m_linkAngleUpdated = other.m_linkAngleUpdated;

        // Copying and updating the list of buddies
        m_buddies = other.m_buddies;

        // Adding the object we are copying in the list of our buddies and removing ourself
        m_buddies.insert(const_cast<KinematicLinkInfo*>(&other));
        m_buddies.remove(this);

        // Now adding ourself to our buddies' buddies
        foreach (KinematicLinkInfo* buddy, m_buddies) {
            buddy->m_buddies.insert(this);
        }

        return *this;
    }

    KinematicLinkInfo::~KinematicLinkInfo()
    {
        // Removing ourself from all buddies
        foreach (KinematicLinkInfo* buddy, m_buddies) {
            buddy->m_buddies.remove(this);
        }
    }

    void KinematicLinkInfo::setBuddies(QSet<KinematicLinkInfo*> buddies)
    {
        m_buddies = buddies;
        m_isMasterBuddy = true;

        // Making sure we are not in the list of our buddies (to avoid
        // infinite recursion)
        m_buddies.remove(this);

        // Setting the buddy list to all buddies
        foreach (KinematicLinkInfo *buddy, m_buddies) {
            buddy->m_buddies = m_buddies;
            buddy->m_isMasterBuddy = false;

            // Removing buddy from its list and adding ourself
            buddy->m_buddies.remove(buddy);
            buddy->m_buddies.insert(this);

            // Now syncing buddy to my values
            buddy->m_wObject = m_wObject;
            buddy->m_objectMatrix = m_objectMatrix;
            buddy->m_axis = m_axis;
            buddy->m_bottom = m_bottom;
            buddy->m_top = m_top;
            buddy->m_height = m_height;
            buddy->m_worldTm = m_worldTm;
            buddy->m_phyJoint = m_phyJoint;
            buddy->m_phyJointDOF = m_phyJointDOF;
            buddy->m_invertedJointAxis = m_invertedJointAxis;
            buddy->m_nextAngle = m_nextAngle;
            buddy->m_velocity = m_velocity;
            buddy->m_motionMode = m_motionMode;
            buddy->m_linkAngleUpdated = m_linkAngleUpdated;
        }
    }

    void KinematicLinkInfo::setWObject(WObject* wObject, const wMatrix &m)
    {
        // Calling the "NoBuddies" version for me and all buddies
        setWObject_NB(wObject, m);
        foreach (KinematicLinkInfo *buddy, m_buddies) {
            buddy->setWObject_NB(wObject, m);
        }

        // Now also updating wObject matrix
        updateMatrixFromiKin();
    }

    void KinematicLinkInfo::setWorldMatrix(const wMatrix *mtr)
    {
        // Calling the "NoBuddies" version for me and all buddies
        setWorldMatrix_NB(mtr);
        foreach (KinematicLinkInfo *buddy, m_buddies) {
            buddy->setWorldMatrix_NB(mtr);
        }

        // Now also updating wObject matrix
        updateMatrixFromiKin();
    }

    void KinematicLinkInfo::updateInformationFromiKin()
    {
        // Calling the "NoBuddies" version for me and all buddies
        updateInformationFromiKin_NB();
        foreach (KinematicLinkInfo *buddy, m_buddies) {
            buddy->updateInformationFromiKin_NB();
        }
    }

    void KinematicLinkInfo::updateMatrixFromiKin()
    {
        // Calling the "NoBuddies" version for me and all buddies
        updateMatrixFromiKin_NB();
        foreach (KinematicLinkInfo *buddy, m_buddies) {
            buddy->updateMatrixFromiKin_NB();
        }
    }

    void KinematicLinkInfo::setJoint(PhyJoint *joint, bool invertedJointAxis, unsigned int dof, bool doConnection, Qt::ConnectionType connType)
    {
        // Calling the "NoBuddies" version for me and all buddies
        setJoint_NB(joint, invertedJointAxis, dof, doConnection, connType);
        foreach (KinematicLinkInfo *buddy, m_buddies) {
            // Here we don't connect the signal, as when my setLinkAngle is
            // called, I take care of updating all buddies
            buddy->setJoint_NB(joint, invertedJointAxis, dof, false);
        }
    }

    void KinematicLinkInfo::setLinkAngle(real newAngle)
    {
        // Calling the "NoBuddies" version for me and all buddies.
        setLinkAngle_NB(newAngle);
        foreach (KinematicLinkInfo *buddy, m_buddies) {
            buddy->setLinkAngle_NB(newAngle);
        }
    }

    void KinematicLinkInfo::setDesideredPosition(real desideredAngle)
    {
        // Calling the "NoBuddies" version for me and all buddies
        setDesideredPosition_NB(desideredAngle);
        foreach (KinematicLinkInfo *buddy, m_buddies) {
            buddy->setDesideredPosition_NB(desideredAngle);
        }
    }

    void KinematicLinkInfo::setDesideredVelocity(real velocity)
    {
        // Calling the "NoBuddies" version for me and all buddies
        setDesideredVelocity_NB(velocity);
        foreach (KinematicLinkInfo *buddy, m_buddies) {
            buddy->setDesideredVelocity_NB(velocity);
        }
    }

    void KinematicLinkInfo::setiKinLinkLimits(real min, real max)
    {
        // Calling the "NoBuddies" version for me and all buddies
        setiKinLinkLimits_NB(min, max);
        foreach (KinematicLinkInfo *buddy, m_buddies) {
            buddy->setiKinLinkLimits_NB(min, max);
        }
    }

    void KinematicLinkInfo::updateLink()
    {
        // Calling the "NoBuddies" version for me and all buddies.
        updateLink_NB();
        foreach (KinematicLinkInfo *buddy, m_buddies) {
            buddy->updateLink_NB();
        }
    }

    void KinematicLinkInfo::setWObject_NB(WObject* wObject, const wMatrix &m)
    {
        m_wObject = wObject;
        m_objectMatrix = m;
    }

    void KinematicLinkInfo::setWorldMatrix_NB(const wMatrix *mtr)
    {
        if (mtr == NULL) {
            m_worldTm = &s_identityMatrix;
        } else {
            m_worldTm = mtr;
        }
    }

    void KinematicLinkInfo::updateInformationFromiKin_NB()
    {
        // Updating world matrix and other structures

        // Getting our frame of reference
        wMatrix thisMatrix;
        convertYarpMatrixToWorldMatrix(m_chain->getH(m_linkID, true), thisMatrix);

        // Getting the frame of reference for the previous link (we need it to
        // compute other stuffs)
        wMatrix prevLinkMatrix;
        if (m_linkID == 0) {
            convertYarpMatrixToWorldMatrix(m_chain->getH0(), prevLinkMatrix);
        } else {
            convertYarpMatrixToWorldMatrix(m_chain->getH(m_linkID - 1, true), prevLinkMatrix);
        }

        // Computing link information
        m_bottom = prevLinkMatrix.w_pos;
        m_top = thisMatrix.w_pos;
        m_axis = m_top - m_bottom;
        m_height = m_axis.norm();
        m_rotAxis = thisMatrix.unrotateVector(prevLinkMatrix.z_ax);
        m_rotCenter = thisMatrix.untransformVector(prevLinkMatrix.w_pos);
    }

    void KinematicLinkInfo::updateMatrixFromiKin_NB()
    {
        if ((m_isMasterBuddy) && (m_wObject != NULL)) {
            // Updating our frame of reference
            wMatrix thisMatrix;
            convertYarpMatrixToWorldMatrix(m_chain->getH(m_linkID, true), thisMatrix);
            thisMatrix = m_objectMatrix * thisMatrix * (*m_worldTm);
            m_wObject->setMatrix(thisMatrix);
        }
    }

    void KinematicLinkInfo::setJoint_NB(PhyJoint *joint, bool invertedJointAxis, unsigned int dof, bool doConnection, Qt::ConnectionType connType)
    {
        // First disconnecting signal from the previous joint dof (if we were connected)
        if (m_phyJoint != NULL) {
            disconnect(m_phyJoint->dofs()[m_phyJointDOF], SIGNAL(changedPosition(real)), this, SLOT(setLinkAngle(real)));
            disconnect(m_phyJoint->dofs()[m_phyJointDOF], SIGNAL(changedDesiredPosition(real)), this, SLOT(setDesideredPosition(real)));
            disconnect(m_phyJoint->dofs()[m_phyJointDOF], SIGNAL(changedDesiredVelocity(real)), this, SLOT(setDesideredVelocity(real)));
            disconnect(m_phyJoint->dofs()[m_phyJointDOF], SIGNAL(changedLimits(real, real)), this, SLOT(setiKinLinkLimits(real, real)));
        }

        m_phyJoint = joint;
        m_phyJointDOF = dof;
        m_invertedJointAxis = invertedJointAxis;

        if (doConnection) {
            connect(m_phyJoint->dofs()[m_phyJointDOF], SIGNAL(changedPosition(real)), this, SLOT(setLinkAngle(real)), connType);
            connect(m_phyJoint->dofs()[m_phyJointDOF], SIGNAL(changedDesiredPosition(real)), this, SLOT(setDesideredPosition(real)), connType);
            connect(m_phyJoint->dofs()[m_phyJointDOF], SIGNAL(changedDesiredVelocity(real)), this, SLOT(setDesideredVelocity(real)), connType);
            connect(m_phyJoint->dofs()[m_phyJointDOF], SIGNAL(changedLimits(real, real)), this, SLOT(setiKinLinkLimits(real, real)), connType);
        }
    }

    void KinematicLinkInfo::setLinkAngle_NB(real newAngle)
    {
        if (!m_invertedJointAxis) {
            newAngle = -newAngle;
        }
        (*(m_chain->asChain()))[m_linkID].setAng(newAngle);
        // Resetting m_nextAngle
        m_nextAngle = newAngle;
    }

    void KinematicLinkInfo::setDesideredPosition_NB(real desideredAngle)
    {
        // Saving the desidered angle and setting the motion mode
        if (m_invertedJointAxis) {
            m_nextAngle = desideredAngle;
        } else {
            m_nextAngle = -desideredAngle;
        }
        m_motionMode = PhyDOF::pos;
        m_linkAngleUpdated = false;
    }

    void KinematicLinkInfo::setDesideredVelocity_NB(real velocity)
    {
        // Checking the velocity is not grater than the highest possible velocity
        if (fabs(velocity) > fabs(m_phyJoint->dofs()[m_phyJointDOF]->maxVelocity())) {
            velocity = (velocity > 0) ? fabs(m_phyJoint->dofs()[m_phyJointDOF]->maxVelocity()) : -fabs(m_phyJoint->dofs()[m_phyJointDOF]->maxVelocity());
        }
        // Saving the desidered angle and setting the motion mode
        if (m_invertedJointAxis) {
            m_velocity = velocity;
        } else {
            m_velocity = -velocity;
        }
        m_motionMode = PhyDOF::vel;
        m_linkAngleUpdated = false;
    }

    void KinematicLinkInfo::setiKinLinkLimits_NB(real min, real max)
    {
        if (m_invertedJointAxis) {
            (*(m_chain->asChain()))[m_linkID].setMin(min);
            (*(m_chain->asChain()))[m_linkID].setMax(max);
        } else {
            (*(m_chain->asChain()))[m_linkID].setMin(-max);
            (*(m_chain->asChain()))[m_linkID].setMax(-min);
        }
    }

    void KinematicLinkInfo::updateLink_NB()
    {
        // It is ok to do this because the motor control calls setDesideredVelocity
        // at each time step (otherwise we would only do one step and then stop)
        if (m_linkAngleUpdated) {
            return;
        }

        // Here we also update the DOF velocity and position
        real velocity = 0.0;
        real position = (*(m_chain->asChain()))[m_linkID].getAng();
        switch (m_motionMode) {
            case PhyDOF::force:
                {
                    // Not supported (never will)
                }
                break;
            case PhyDOF::vel:
                {
                    // Here we have to invert the angle if the axis is not inverted to balance the
                    // inversion done in the setLinkAngle function (this is to avoid having two versions
                    // of setLinkAngle, one that inverts the angle if it has to, and another that
                    // never inverts angles). We also have to multiply velocity by the timestep to have the
                    // actual delta angle
                    const real deltaAngle = m_velocity * m_phyJoint->world()->timeStep();
                    velocity = m_velocity;
                    position = (*(m_chain->asChain()))[m_linkID].getAng() + deltaAngle;
                    if (m_invertedJointAxis) {
                        setLinkAngle_NB(position);
                    } else {
                        setLinkAngle_NB(-position);
                    }
                }
                break;
            case PhyDOF::pos:
                {
                    // Here we have to invert the angle if the axis is not inverted to balance the
                    // inversion done in the setLinkAngle function (this is to avoid having two versions
                    // of setLinkAngle, one that inverts the angle if it has to, and another that
                    // never inverts angles)
                    velocity = (m_nextAngle - (*(m_chain->asChain()))[m_linkID].getAng()) / m_phyJoint->world()->timeStep();
                    position = m_nextAngle;
                    if (m_invertedJointAxis) {
                        setLinkAngle_NB(position);
                    } else {
                        setLinkAngle_NB(-position);
                    }
                }
                break;
            case PhyDOF::off:
                {
                    // Nothing to do, here
                }
                break;
        }

        // Updating velocity and position
        m_phyJoint->dofs()[m_phyJointDOF]->setPosition(position);
        m_phyJoint->dofs()[m_phyJointDOF]->setVelocity(velocity);
        m_linkAngleUpdated = true;
    }

    const wMatrix KinematicLinkInfo::s_identityMatrix(wMatrix::identity());
} // end namespace farsa

#endif // FARSA_USE_YARP_AND_ICUB