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;
 
    /** @brief RemoteRobotNodeInitializer is used to initialize the robot node for a given node type.
     *  For each robot type to be supported make a specialization of the initialize function.
     * Currently supports: RobotNodeRevolute, RobotNodePrismatic, RobotNodeFixed. Node type specific
     * initializations go here.
     */
    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();
 
 
    /** @brief Mimics the behaviour of robot nodes while redirecting everything to Ice proxies.
     * @tparam VirtualRobotNodeType Must be a descendant of VirtualRobot::RobotNode
     * @details This class is for <b> internal use only</b> as classes cannot be referenced!
     */
    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:
        ///////////////////////// SETUP ////////////////////////////////////
        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;
    };
 
    /** @brief Mimics the behaviour of the VirtualRobot::Robot class while redirecting everything to an Ice proxy.
     * @details For a description of the  members please refer to the Simox documentation (VirtualRobot::Robot).
     * Note that not the complete interface has been made available yet. These functions are marked as protected.
     */
    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;
 
        /**
         *
         * @return Global pose of the robot
         */
        Eigen::Matrix4f getGlobalPose() const override;
 
        float getScaling();
 
 
 
        /// Use this method to share the robot instance over Ice.
        SharedRobotInterfacePrx getSharedRobot() const;
 
        std::string getName() const override;
 
        /*!
              Creates a local robot that is synchronized once but not updated any more. You can use the synchronizeLocalClone method to update the joint values.
              Note that only the kinematic structure is cloned, but the visualization files, collision models, end effectors, etc are not copied.
              This means you can use this model for kinematic computations (e.g. coordinate transformations) but not for advanced features like collision detection.
              In order to get a fully featured robot model you can pass a filename to VirtualRobot::Robot,
              but then you need to make sure that the loaded model is identical to the model of the remote robot (otherwise errors will occur).
              In the packages parameter you can pass in ArmarX packages, in which the robot file model might be in.
              The loadMode specifies in which mode the model should be loaded. Refer to simox for more information (only matters if filename was passed in).
              @see createLocalCloneFromFile(), synchronizeLocalClone()
            */
        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);
 
        /**
         * @brief This is a convenience function for createLocalClone, which automatically gets the filename from the RobotStateComponent,
         * loads robot from the file and syncs it once.
         * @param robotStatePrx Proxy to the RobotStateComponent
         * @param loadMode The loadMode specifies in which mode the model should be loaded. Refer to simox for more information.
         * @return new robot clone
         * @see createLocalClone(), synchronizeLocalClone()
         */
        static VirtualRobot::RobotPtr createLocalCloneFromFile(RobotStateComponentInterfacePrx robotStatePrx, VirtualRobot::RobotIO::RobotDescription loadMode = VirtualRobot::RobotIO::eFull);
 
 
        /*!
                Use this method to synchronize (i.e. copy the joint values) from the remote robot to the local clone.
                The local clone must have the identical structure as the remote robot model, otherwise an error will be reported.
                This is the fastest way to get update a local robot.
              */
        static bool synchronizeLocalClone(VirtualRobot::RobotPtr robot, RobotStateComponentInterfacePrx robotStatePrx);
 
        static bool synchronizeLocalClone(VirtualRobot::RobotPtr robot, SharedRobotInterfacePrx sharedRobotPrx);
 
        /**
         * @brief Synchronizes a local robot to a robot state at timestamp.
         * @param robot Local robot that should be updated
         * @param robotStatePrx Proxy to the RobotStateComponent
         * @param timestamp Timestamp to which the local robot should be sychronized. Must be in range of the state history of the RobotStateComponent.
         * @return True if successfully synced.
         */
        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
 
        //! Clones the structure of this remote robot to a local instance
        VirtualRobot::RobotPtr createLocalClone();
 
    protected:
 
        /// Not implemented yet
        bool hasEndEffector(const std::string& endEffectorName) const override;
        /// Not implemented yet
        VirtualRobot::EndEffectorPtr getEndEffector(const std::string& endEffectorName) const override;
        /// Not implemented yet
        void getEndEffectors(std::vector<VirtualRobot::EndEffectorPtr>& storeEEF) const override;
 
        /// Not implemented yet
        void setRootNode(VirtualRobot::RobotNodePtr node) override;
        /// Not implemented yet
        void registerRobotNode(VirtualRobot::RobotNodePtr node) override;
        /// Not implemented yet
        void deregisterRobotNode(VirtualRobot::RobotNodePtr node) override;
        /// Not implemented yet
        void registerRobotNodeSet(VirtualRobot::RobotNodeSetPtr nodeSet) override;
        /// Not implemented yet
        void deregisterRobotNodeSet(VirtualRobot::RobotNodeSetPtr nodeSet) override;
        /// Not implemented yet
        void registerEndEffector(VirtualRobot::EndEffectorPtr endEffector) override;
        /**
         * @brief Sets the global pose of the robot.
         * @param globalPose new global pose
         * @param applyValues No effect. Will be always applied.
         */
        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>;
}