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

#ifdef FARSA_USE_YARP_AND_ICUB

#ifndef PHYICUBHELPERS_H
#define PHYICUBHELPERS_H

#include "worldsimconfig.h"
#include "wvector.h"
#include "wmatrix.h"
#include "wquaternion.h"
#include "world.h"
#include "yarpobject.h"
#include "wmesh.h"
#include "phyhinge.h"
#include "phyobject.h"
#include <QVector>
#include <QSet>
#include <QMap>
#include <QString>
#include <string>
#include <typeinfo>

// All this stuff is to get rid of annoying warnings...
#ifdef __GNUC__
#if __GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ > 5)
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-parameter"
#else
#pragma GCC diagnostic ignored "-Wunused-parameter"
#endif
#endif
    #include "yarp/dev/CartesianControl.h"
    #include "yarp/os/ConnectionReader.h"
    #include "yarp/os/PortReader.h"
    #include "iCub/iKin/iKinHlp.h"
    #include "iCub/iKin/iKinSlv.h"
#ifdef __GNUC__
#if __GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ > 5)
#pragma GCC diagnostic pop
#else
#pragma GCC diagnostic warning "-Wunused-parameter"
#endif
#endif

namespace farsa {
    class PhyiCub;

    FARSA_WSIM_API std::ostream& operator<<(std::ostream &os, const wMatrix &m);

    FARSA_WSIM_API std::ostream& operator<<(std::ostream &os, const wVector &v);

    FARSA_WSIM_API void convertYarpMatrixToWorldMatrix(const yarp::sig::Matrix &y, wMatrix &w);

    FARSA_WSIM_API void convertWorldMatrixToYarpMatrix(const wMatrix &w, yarp::sig::Matrix &y);

    class FARSA_WSIM_API DOFStatusListener : public QObject
    {
        Q_OBJECT

    public:
        DOFStatusListener(const PhyDOF* dof, Qt::ConnectionType connType = Qt::DirectConnection);

        ~DOFStatusListener();

        void update();

        real position() const
        {
            return m_curPosition;
        }

    private slots:
        void setLinkAngle(real newAngle);

        void setDesideredPosition(real desideredAngle);

        void setDesideredVelocity(real velocity);

    protected:
        const PhyDOF* const m_dof;

        real m_curPosition;

        real m_desideredPosition;

        real m_velocity;

        PhyDOF::MotionMode m_motionMode;
    };

    class FARSA_WSIM_API PhyObjectsGroup : public WObject
    {
        Q_OBJECT

    public:
        typedef int ChainIDType;

        typedef int ElementIDType;

    public:
        PhyObjectsGroup(PhyObject* headObject, World* world, QString name = "unamed");

        ~PhyObjectsGroup();

        ElementIDType addObject(PhyObject* obj, ChainIDType chainID = ChainIDType());

        ElementIDType addJointDOF(PhyJoint* joint, unsigned int dofID, ChainIDType chainID = ChainIDType());

        void setKinematic(bool b);

        void setStatic(bool b);

        void enableJoints(bool b);

        void updateRelativePositions();

        void resetObjectPositions()
        {
            changedMatrix();
        }

        void setDOFPosition(real pos, ChainIDType chainID, ElementIDType dof);

        void updateFromDOF();

        virtual void preUpdate();

        virtual void postUpdate();

        PhyObject* getHeadObject() const
        {
            return m_headObject;
        }

    private:
        virtual void changedMatrix();

        PhyObject *const m_headObject;

        struct ObjectAndMatrix
        {
            ObjectAndMatrix() :
                object(NULL),
                matrix()
            {
            }

            ObjectAndMatrix(PhyObject* obj, wMatrix mtr) :
                object(obj),
                matrix(mtr)
            {
            }

            PhyObject* object;
            wMatrix matrix;
        };

        QMap<ChainIDType, QVector<ObjectAndMatrix> > m_objects;

        struct DOFAndListener
        {
            DOFAndListener() :
                joint(NULL),
                dofID(0),
                listener(NULL)
            {
            }

            DOFAndListener(PhyJoint* j, unsigned int d, DOFStatusListener* l) :
                joint(j),
                dofID(d),
                listener(l)
            {
            }

            PhyJoint* joint;
            unsigned int dofID;
            DOFStatusListener* listener;
        };

        QMap<ChainIDType, QVector<DOFAndListener> > m_joints;
    };

    class FARSA_WSIM_API KinematicLinkInfo : public QObject
    {
        Q_OBJECT

    public:
        KinematicLinkInfo();

        KinematicLinkInfo(iCub::iKin::iKinLimb* chain, unsigned int linkID);

        KinematicLinkInfo(const KinematicLinkInfo& other);

        KinematicLinkInfo& operator=(const KinematicLinkInfo& other);

        virtual ~KinematicLinkInfo();

        void setBuddies(QSet<KinematicLinkInfo*> buddies);

        void setWObject(WObject* wObject, const wMatrix &m = s_identityMatrix);

        WObject* getWObject() const
        {
            return m_wObject;
        }

        const wMatrix& getWObjectMatrix() const
        {
            return m_objectMatrix;
        }

        const wVector& getAxis() const
        {
            return m_axis;
        }

        wVector getAxisWorld() const
        {
            return m_worldTm->rotateVector(m_axis);
        }

        const wVector& getBottom() const
        {
            return m_bottom;
        }

        wVector getBottomWorld() const
        {
            return m_worldTm->transformVector(m_bottom);
        }

        const wVector& getTop() const
        {
            return m_top;
        }

        wVector getTopWorld() const
        {
            return m_worldTm->transformVector(m_top);
        }

        real getHeight() const
        {
            return m_height;
        }

        const wVector& getRotAxis() const
        {
            return m_rotAxis;
        }

        wVector getRotAxisWObject() const
        {
            return m_objectMatrix.unrotateVector(m_rotAxis);
        }

        const wVector& getRotCenter() const
        {
            return m_rotCenter;
        }

        wVector getRotCenterWObject() const
        {
            return m_objectMatrix.untransformVector(m_rotCenter);
        }

        void setWorldMatrix(const wMatrix *mtr);

        const wMatrix& getWorldMatrix() const
        {
            return *m_worldTm;
        }

        yarp::sig::Matrix getYarpMatrix() const
        {
            return m_chain->getH(m_linkID, true);
        }

        wMatrix getYarpMatrixAswMatrix() const
        {
            wMatrix mtr;
            convertYarpMatrixToWorldMatrix(m_chain->getH(m_linkID, true), mtr);
            return mtr;
        }

        const iCub::iKin::iKinLink& getiKinLink() const
        {
            return (*(m_chain->asChain()))[m_linkID];
        }

        void updateInformationFromiKin();

        void updateMatrixFromiKin();

        void setJoint(PhyJoint *joint, bool invertedJointAxis = true, unsigned int dof = 0, bool doConnection = true, Qt::ConnectionType connType = Qt::DirectConnection);

        PhyJoint* getJoint() const
        {
            return m_phyJoint;
        }

        unsigned int getJointDOF() const
        {
            return m_phyJointDOF;
        }

    public slots:
        void setLinkAngle(real newAngle);

        void setDesideredPosition(real desideredAngle);

        void setDesideredVelocity(real velocity);

        void setiKinLinkLimits(real min, real max);

        void updateLink();

    private:
        void setWObject_NB(WObject* wObject, const wMatrix &m = s_identityMatrix);

        void setWorldMatrix_NB(const wMatrix *mtr);

        void updateInformationFromiKin_NB();

        void updateMatrixFromiKin_NB();

        void setJoint_NB(PhyJoint *joint, bool invertedJointAxis = true, unsigned int dof = 0, bool doConnection = true, Qt::ConnectionType connType = Qt::DirectConnection);

        void setLinkAngle_NB(real newAngle);

        void setDesideredPosition_NB(real desideredAngle);

        void setDesideredVelocity_NB(real velocity);

        void setiKinLinkLimits_NB(real min, real max);

        void updateLink_NB();

        mutable QSet<KinematicLinkInfo*> m_buddies;

        bool m_isMasterBuddy;

        iCub::iKin::iKinLimb* m_chain;

        unsigned int m_linkID;

        WObject* m_wObject;

        wMatrix m_objectMatrix;

        wVector m_axis;

        wVector m_bottom;

        wVector m_top;

        real m_height;

        wVector m_rotAxis;

        wVector m_rotCenter;

        const wMatrix* m_worldTm;

        PhyJoint* m_phyJoint;

        unsigned int m_phyJointDOF;

        bool m_invertedJointAxis;

        real m_nextAngle;

        real m_velocity;

        PhyDOF::MotionMode m_motionMode;

        bool m_linkAngleUpdated;

        static const wMatrix s_identityMatrix;
    };

    template <class iKinLimb_t>
    class FARSA_WSIM_TEMPLATE KinematicLimb : public iKinLimb_t
    {
    public:
        typedef iKinLimb_t iKinLimb;

    public:
        KinematicLimb() :
            iKinLimb(),
            m_links(),
            m_rootObject(NULL)
        {
            initLinkInfoList();
        }

        KinematicLimb(const std::string &type) :
            iKinLimb(type),
            m_links(),
            m_rootObject(NULL)
        {
            initLinkInfoList();
        }

        ~KinematicLimb()
        {
        }

        const KinematicLinkInfo& getLinkInfo(int i) const
        {
            return m_links[i];
        }

        KinematicLinkInfo& getLinkInfo(int i)
        {
            return m_links[i];
        }

        void setWorldMatrix(const wMatrix *mtr)
        {
            for (int i = 0; i < m_links.size(); i++) {
                m_links[i].setWorldMatrix(mtr);
            }
        }

        const wMatrix& getWorldMatrix() const
        {
            return m_links[0].getWorldMatrix();
        }

        void updateInformationFromiKin()
        {
            for (int i = 0; i < m_links.size(); i++) {
                m_links[i].updateInformationFromiKin();
            }
        }

        void updateMatrixFromiKin(int updateObjMatrixFromLink = 0, int updateObjMatrixUpToLink = -1)
        {
            if (updateObjMatrixFromLink < 0) {
                updateObjMatrixFromLink = 0;
            }
            if ((updateObjMatrixUpToLink < 0) || (updateObjMatrixUpToLink >= m_links.size())) {
                updateObjMatrixUpToLink = m_links.size() - 1;
            }
            // Updating links
            //for (int i = 0; i < m_links.size(); i++) {
            for (int i = updateObjMatrixFromLink; i <= updateObjMatrixUpToLink; i++) {
                m_links[i].updateLink();
            }

            // Now updating matrices
            for (int i = updateObjMatrixFromLink; i <= updateObjMatrixUpToLink; i++) {
                m_links[i].updateMatrixFromiKin();
            }
        }

        void setRootObject(WObject* rootObject)
        {
            m_rootObject = rootObject;
        }

        WObject* getRootObject() const
        {
            return m_rootObject;
        }

        PhyJoint* createJoint(int linkID, bool invertAxis = true, bool doConnection = true, Qt::ConnectionType connType = Qt::DirectConnection, unsigned int dof = 0)
        {
            // First getting axis and objects
            const wVector axis = invertAxis ? -getLinkInfo(linkID).getRotAxisWObject() : getLinkInfo(linkID).getRotAxisWObject();
            PhyObject* parent = dynamic_cast<PhyObject*>(getLinkInfo(linkID).getWObject());
            if (parent == NULL) {
                // Let's try to convert to PhyObjectsGroup
                PhyObjectsGroup* grp = dynamic_cast<PhyObjectsGroup*>(getLinkInfo(linkID).getWObject());
                if (grp != NULL) {
                    // If we can convert parent object to PhyObjectsGroup, we use its head object
                    parent = grp->getHeadObject();
                }
            }
            WObject* const childWObject = (linkID <= 0) ? m_rootObject : getLinkInfo(linkID - 1).getWObject();
            PhyObject* child = dynamic_cast<PhyObject*>(childWObject);
            if ((child == NULL) && (typeid(childWObject) == typeid(PhyObjectsGroup*))) {
                // If we can convert child object to PhyObjectsGroup, we use its head object
                child = (dynamic_cast<PhyObjectsGroup*>(childWObject))->getHeadObject();
            }

            Q_ASSERT_X((parent != NULL), "PhyObjectsGroup::createJoint", "This is going to crash, parent object in joint is NULL");

            // Now creating joint and putting it in the link info object
            PhyJoint* joint = new PhyHinge(axis, getLinkInfo(linkID).getRotCenterWObject(), parent, child);
            getLinkInfo(linkID).setJoint(joint, invertAxis, dof, doConnection, connType);

            return joint;
        }

    private:
        void initLinkInfoList()
        {
            // Adding all links
            for (unsigned int i = 0; i < this->getN(); i++) {
                m_links.push_back(KinematicLinkInfo(this, i));
                this->releaseLink(i); // We always work as if all links are released
            }
        }

        QVector<KinematicLinkInfo> m_links;

        WObject *m_rootObject;
    };
} // end namespace farsa

#endif

#endif // FARSA_USE_YARP_AND_ICUB