RemoteRobot.h

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX 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, see <http://www.gnu.org/licenses/>.
 *
 * @package    RobotAPI::
 * @author     (stefan dot ulbrich at kit dot edu)
 * @date
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
#pragma once
 
#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/Nodes/RobotNode.h>
#include <RobotAPI/interface/core/RobotState.h>
#include <VirtualRobot/Nodes/RobotNodeRevolute.h>
#include <VirtualRobot/Nodes/RobotNodePrismatic.h>
#include <VirtualRobot/Nodes/RobotNodeFixed.h>
#include <VirtualRobot/XML/RobotIO.h>
 
#include <mutex>
 
namespace armarx
{
    // forward declaration of RemoteRobotNode
    template<class VirtualRobotNodeType> class RemoteRobotNode;
 
    template<typename VirtualRobotNodeType>
    struct RemoteRobotNodeInitializer
    {
        static void initialize(RemoteRobotNode<VirtualRobotNodeType>* remoteNode);
    };
 
    // specializations
    template<>
    void RemoteRobotNodeInitializer<VirtualRobot::RobotNodeRevolute>::initialize(RemoteRobotNode<VirtualRobot::RobotNodeRevolute>* remoteNode);
    template<>
    void RemoteRobotNodeInitializer<VirtualRobot::RobotNodePrismatic>::initialize(RemoteRobotNode<VirtualRobot::RobotNodePrismatic>* remoteNode);
    template<>
    void RemoteRobotNodeInitializer<VirtualRobot::RobotNodeFixed>::initialize(RemoteRobotNode<VirtualRobot::RobotNodeFixed>* remoteNode);
 
    VirtualRobot::CollisionCheckerPtr RemoteRobotNode_getGlobalCollisionChecker();
 
 
    template<class VirtualRobotNodeType>
    class RemoteRobotNode :
        public VirtualRobotNodeType
    {
        friend struct RemoteRobotNodeInitializer<VirtualRobotNodeType>;
 
    public:
        RemoteRobotNode(SharedRobotNodeInterfacePrx node, VirtualRobot::RobotPtr vr) :
            _node(node)
        {
            _node->ref();
            this->name = _node->getName();
            this->robot = vr;
            _node->getJointValueOffest();
            setJointLimits(_node->getJointLimitLow(), _node->getJointLimitHigh());
 
            this->collisionChecker = RemoteRobotNode_getGlobalCollisionChecker();
 
            RemoteRobotNodeInitializer<VirtualRobotNodeType>::initialize(this);
        }
 
        ~RemoteRobotNode() override;
 
        float getJointValue() const override;
        virtual float getJointLimitHi() const;
        virtual float getJointLimitLo() const;
 
        Eigen::Matrix4f getLocalTransformation() override;
 
        Eigen::Matrix4f getGlobalPose() const override;
        Eigen::Matrix4f getPoseInRootFrame() const override;
        Eigen::Vector3f getPositionInRootFrame() const override;
        virtual bool hasChildNode(const std::string& child, bool recursive = false) const;
 
        std::vector<VirtualRobot::RobotNodePtr> getAllParents(VirtualRobot::RobotNodeSetPtr rns) override;
        virtual VirtualRobot::SceneObjectPtr getParent();
        inline SharedRobotNodeInterfacePrx getSharedRobotNode()
        {
            return _node;
        }
 
        virtual std::vector<std::string> getChildrenNames() const;
 
    protected:
        void setJointLimits(float lo, float hi) override;
        //virtual void setPostJointTransformation(const Eigen::Matrix4f &trafo);
        virtual void setLocalTransformation(const Eigen::Matrix4f& trafo);
 
        virtual std::string getParentName() const;
        std::vector< VirtualRobot::SceneObjectPtr> getChildren() const override;
 
        void updateTransformationMatrices() override;
        void updateTransformationMatrices(const Eigen::Matrix4f& globalPose) override;
 
 
        virtual bool hasChildNode(const VirtualRobot::RobotNodePtr child, bool recursive = false) const;
        virtual void addChildNode(VirtualRobot::RobotNodePtr child);
        virtual bool initialize(VirtualRobot::RobotNodePtr parent, bool initializeChildren = false);
        virtual void reset();
        void setGlobalPose(const Eigen::Matrix4f& pose) override;
        virtual void setJointValue(float q, bool updateTransformations = true, bool clampToLimits = true);
 
        SharedRobotNodeInterfacePrx _node;
    };
 
    class RemoteRobot : public VirtualRobot::Robot
    {
    public:
        RemoteRobot(SharedRobotInterfacePrx robot);
 
        ~RemoteRobot() override;
 
        VirtualRobot::RobotNodePtr getRootNode() const override;
 
        bool hasRobotNode(const std::string& robotNodeName) const override;
        bool hasRobotNode(VirtualRobot::RobotNodePtr) const override;
 
        VirtualRobot::RobotNodePtr getRobotNode(const std::string& robotNodeName) const override;
        void getRobotNodes(std::vector< VirtualRobot::RobotNodePtr >& storeNodes, bool clearVector = true) const override;
 
        bool hasRobotNodeSet(const std::string& name) const override;
        VirtualRobot::RobotNodeSetPtr getRobotNodeSet(const std::string& nodeSetName) const override;
        void getRobotNodeSets(std::vector<VirtualRobot::RobotNodeSetPtr>& storeNodeSet) const override;
 
        Eigen::Matrix4f getGlobalPose() const override;
 
        float getScaling();
 
 
 
        SharedRobotInterfacePrx getSharedRobot() const;
 
        std::string getName() const override;
 
        static VirtualRobot::RobotPtr createLocalClone(RobotStateComponentInterfacePrx robotStatePrx, const std::string& filename = std::string(), const Ice::StringSeq packages = Ice::StringSeq(), VirtualRobot::RobotIO::RobotDescription loadMode = VirtualRobot::RobotIO::eFull);
 
        static VirtualRobot::RobotPtr createLocalClone(SharedRobotInterfacePrx sharedRobotPrx, std::string filename = std::string(), float scaling = 1.0f, const Ice::StringSeq packages = Ice::StringSeq(), VirtualRobot::RobotIO::RobotDescription loadMode = VirtualRobot::RobotIO::eFull);
 
        static VirtualRobot::RobotPtr createLocalCloneFromFile(RobotStateComponentInterfacePrx robotStatePrx, VirtualRobot::RobotIO::RobotDescription loadMode = VirtualRobot::RobotIO::eFull);
 
 
        static bool synchronizeLocalClone(VirtualRobot::RobotPtr robot, RobotStateComponentInterfacePrx robotStatePrx);
 
        static bool synchronizeLocalClone(VirtualRobot::RobotPtr robot, SharedRobotInterfacePrx sharedRobotPrx);
 
        static bool synchronizeLocalCloneToTimestamp(VirtualRobot::RobotPtr robot, RobotStateComponentInterfacePrx robotStatePrx, Ice::Long timestamp);
 
        static bool synchronizeLocalCloneToState(VirtualRobot::RobotPtr robot, RobotStateConfig const& state);
 
        // VirtualRobot::RobotPtr getRobotPtr() { return shared_from_this();} // only for debugging
 
        VirtualRobot::RobotPtr createLocalClone();
 
    protected:
 
        bool hasEndEffector(const std::string& endEffectorName) const override;
        VirtualRobot::EndEffectorPtr getEndEffector(const std::string& endEffectorName) const override;
        void getEndEffectors(std::vector<VirtualRobot::EndEffectorPtr>& storeEEF) const override;
 
        void setRootNode(VirtualRobot::RobotNodePtr node) override;
        void registerRobotNode(VirtualRobot::RobotNodePtr node) override;
        void deregisterRobotNode(VirtualRobot::RobotNodePtr node) override;
        void registerRobotNodeSet(VirtualRobot::RobotNodeSetPtr nodeSet) override;
        void deregisterRobotNodeSet(VirtualRobot::RobotNodeSetPtr nodeSet) override;
        void registerEndEffector(VirtualRobot::EndEffectorPtr endEffector) override;
        void setGlobalPose(const Eigen::Matrix4f& globalPose, bool applyValues = true) override;
 
        VirtualRobot::RobotNodePtr createLocalNode(SharedRobotNodeInterfacePrx remoteNode, std::vector<VirtualRobot::RobotNodePtr>& allNodes, std::map<VirtualRobot::RobotNodePtr, std::vector<std::string> >& childrenMap, VirtualRobot::RobotPtr robo);
 
    protected:
        SharedRobotInterfacePrx _robot;
        mutable std::map<std::string, VirtualRobot::RobotNodePtr> _cachedNodes;
        mutable VirtualRobot::RobotNodePtr _root;
 
        static std::recursive_mutex m;
 
        static VirtualRobot::RobotNodePtr createRemoteRobotNode(SharedRobotNodeInterfacePrx, VirtualRobot::RobotPtr);
    };
 
    using RemoteRobotPtr = std::shared_ptr<RemoteRobot>;
}