RemoteRobot.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2011-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
 * @author
 * @date
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
#include "RemoteRobot.h"
 
#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/XML/RobotIO.h>
#include <VirtualRobot/RobotConfig.h>
#include <VirtualRobot/RobotFactory.h>
#include <VirtualRobot/Nodes/RobotNodeFixedFactory.h>
#include <VirtualRobot/Nodes/RobotNodePrismaticFactory.h>
#include <VirtualRobot/Nodes/RobotNodeRevoluteFactory.h>
#include <VirtualRobot/CollisionDetection/CollisionChecker.h>
 
 
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <RobotAPI/libraries/core/FramedPose.h>
#include <ArmarXCore/core/logging/Logging.h>
 
#include <SimoxUtility/algorithm/string/string_tools.h>
 
#include <Eigen/Geometry>
 
//#define DMES(Obj) cout << format("[%1%|%2%|%3%] %4%") % __FILE__ % __LINE__ % __func__ % Obj;
#define DMES(Obj) ;
 
using namespace VirtualRobot;
using namespace Eigen;
 
namespace armarx
{
 
    std::recursive_mutex RemoteRobot::m;
 
    RemoteRobot::RemoteRobot(SharedRobotInterfacePrx robot) :
        Robot(),
        _robot(robot)
    {
        _robot->ref();
    }
 
    RemoteRobot::~RemoteRobot()
    {
        try
        {
            _robot->unref();
        }
        catch (std::exception& e)
        {
            ARMARX_DEBUG_S << "Unref of SharedRobot failed: " << e.what();
        }
        catch (...)
        {
            ARMARX_DEBUG_S << "Unref of SharedRobot failed: reason unknown";
        }
 
    }
 
    RobotNodePtr RemoteRobot::getRootNode() const
    {
        // lazy initialization needed since shared_from_this() must not be called in constructor
        if (!_root)
        {
            _root = RemoteRobot::createRemoteRobotNode(_robot->getRootNode(), shared_from_this());
        }
        return _root;
    }
 
    bool RemoteRobot::hasRobotNode(const std::string& robotNodeName) const
    {
        if (_cachedNodes.find(name) == _cachedNodes.end())
        {
            return _robot->hasRobotNode(robotNodeName);
        }
        else
        {
            return true;
        }
    }
 
 
    bool RemoteRobot::hasRobotNode(RobotNodePtr robotNode) const
    {
        return this->hasRobotNode(robotNode->getName());
 
        /*
         * This just does not work. because you cannot tell wheter RemoteRobotNode<RobotNodeRevolute> or another type is used
         * perhaps you can infer the actual RobotNodeType somehow. Until now we just check for names which is
         * much faster!
         *
        if  ( (_cachedNodes.find(name)==_cachedNodes.end()) || _robot->hasRobotNode(robotNode->getName())) {
            shared_ptr<RemoteRobotNode> remoteNode(dynamic_pointer_cast<RemoteRobotNode>(robotNode));
            if (! remoteNode) return false;
 
            SharedRobotNodeInterfacePrx sharedNode = remoteNode->getSharedNode();
            SharedRobotNodeInterfacePrx otherSharedNode = dynamic_pointer_cast<RemoteRobotNode>(this->getRobotNode(robotNodeName))->getSharedNode();
            if (sharedNode == otherSharedNode)
                return true;
        }
 
        return false;
        */
    }
 
 
    RobotNodePtr RemoteRobot::getRobotNode(const std::string& robotNodeName) const
    {
        DMES((format("Node: %1%") % robotNodeName));
        auto it = _cachedNodes.find(robotNodeName);
        if (it == _cachedNodes.end() || it->second == NULL)
        {
            DMES("No cache hit");
            _cachedNodes[robotNodeName] = RemoteRobot::createRemoteRobotNode(_robot->getRobotNode(robotNodeName),
                                          shared_from_this());
            return _cachedNodes[robotNodeName];
        }
        else
        {
            DMES("Cache hit");
            return it->second;
        }
    }
 
    void RemoteRobot::getRobotNodes(std::vector< RobotNodePtr >& storeNodes, bool clearVector) const
    {
        if (clearVector)
        {
            storeNodes.clear();
        }
 
        NameList nodes = _robot->getRobotNodes();
        for (std::string const& name : nodes)
        {
            storeNodes.push_back(this->getRobotNode(name));
        }
    }
 
    bool RemoteRobot::hasRobotNodeSet(const std::string& name) const
    {
        return _robot->hasRobotNodeSet(name);
    }
 
    RobotNodeSetPtr RemoteRobot::getRobotNodeSet(const std::string& nodeSetName) const
    {
        std::vector<RobotNodePtr> storeNodes;
        RobotNodeSetInfoPtr info = _robot->getRobotNodeSet(nodeSetName);
        return RobotNodeSet::createRobotNodeSet(
                   shared_from_this(), nodeSetName, info->names, info->rootName, info->tcpName);
    }
 
 
    void RemoteRobot::getRobotNodeSets(std::vector<RobotNodeSetPtr>& storeNodeSet) const
    {
        NameList sets = _robot->getRobotNodeSets();
 
        for (std::string const& name : sets)
        {
            storeNodeSet.push_back(this->getRobotNodeSet(name));
        }
    }
 
    Matrix4f RemoteRobot::getGlobalPose() const
    {
        PosePtr p = PosePtr::dynamicCast(_robot->getGlobalPose());
        return p->toEigen(); // convert to eigen first
    }
 
    float RemoteRobot::getScaling()
    {
        return _robot->getScaling();
    }
 
    SharedRobotInterfacePrx RemoteRobot::getSharedRobot() const
    {
        return this->_robot;
    }
 
    std::string RemoteRobot::getName() const
    {
        return _robot->getName();
    }
 
    VirtualRobot::RobotNodePtr RemoteRobot::createLocalNode(SharedRobotNodeInterfacePrx remoteNode, std::vector<VirtualRobot::RobotNodePtr>& allNodes, std::map< VirtualRobot::RobotNodePtr, std::vector<std::string> >& childrenMap, RobotPtr robo)
    {
        std::scoped_lock cloneLock(m);
        static int nonameCounter = 0;
 
        if (!remoteNode || !robo)
        {
            ARMARX_ERROR_S << " NULL data ";
            return VirtualRobot::RobotNodePtr();
        }
 
        VirtualRobot::RobotNodeFactoryPtr revoluteNodeFactory = VirtualRobot::RobotNodeFactory::fromName(VirtualRobot::RobotNodeRevoluteFactory::getName(), NULL);
        VirtualRobot::RobotNodeFactoryPtr fixedNodeFactory = VirtualRobot::RobotNodeFactory::fromName(VirtualRobot::RobotNodeFixedFactory::getName(), NULL);
        VirtualRobot::RobotNodeFactoryPtr prismaticNodeFactory = VirtualRobot::RobotNodeFactory::fromName(VirtualRobot::RobotNodePrismaticFactory::getName(), NULL);
 
        Eigen::Vector3f idVec3 = Eigen::Vector3f::Zero();
        std::string name = remoteNode->getName();
 
        if (name.empty())
        {
            ARMARX_LOG_S << "Node without name!!!";
            std::stringstream ss;
            ss << "robot_node_" << nonameCounter;
            nonameCounter++;
            name = ss.str();
        }
 
        VirtualRobot::RobotNodePtr result;
        PosePtr lTbase = PosePtr::dynamicCast(remoteNode->getLocalTransformation());
        Eigen::Matrix4f localTransform = lTbase->toEigen();
 
        //float jv = remoteNode->getJointValue();
        float jvLo = remoteNode->getJointLimitLow();
        float jvHi = remoteNode->getJointLimitHigh();
        float jointOffset = 0;//remoteNode->getJointOffset();
 
        JointType jt = remoteNode->getType();
 
        SceneObject::Physics physics;
        physics.localCoM = Vector3Ptr::dynamicCast(remoteNode->getCoM())->toEigen();
        std::vector<float> inertia = remoteNode->getInertia();
        for (int i = 0; i < 3; i++)
            for (int j = 0; j < 3; j++)
            {
                physics.inertiaMatrix(i, j) = inertia.at(i * 3 + j);
            }
        physics.massKg = remoteNode->getMass();
 
        switch (jt)
        {
            case ePrismatic:
            {
                Vector3Ptr axisBase = Vector3Ptr::dynamicCast(remoteNode->getJointTranslationDirection());
                Eigen::Vector3f axis = axisBase->toEigen();
                // convert axis to local coord system
                Eigen::Vector4f result4f = Eigen::Vector4f::Zero();
                result4f.segment(0, 3) = axis;
                PosePtr gp = PosePtr::dynamicCast(remoteNode->getGlobalPose());
                result4f = gp->toEigen().inverse() * result4f;
                axis = result4f.block(0, 0, 3, 1);
 
                result = prismaticNodeFactory->createRobotNode(robo, name, VirtualRobot::VisualizationNodePtr(), VirtualRobot::CollisionModelPtr(),
                         jvLo, jvHi, jointOffset, localTransform, idVec3, axis, physics);
            }
            break;
 
            case eFixed:
                result = fixedNodeFactory->createRobotNode(robo, name, VirtualRobot::VisualizationNodePtr(), VirtualRobot::CollisionModelPtr(), 0,
                         0, 0, localTransform, idVec3, idVec3, physics);
                break;
 
            case eRevolute:
            {
                Vector3Ptr axisBase = Vector3Ptr::dynamicCast(remoteNode->getJointRotationAxis());
                Eigen::Vector3f axis = axisBase->toEigen();
                // convert axis to local coord system
                Eigen::Vector4f result4f = Eigen::Vector4f::Zero();
                result4f.segment(0, 3) = axis;
                PosePtr gp = PosePtr::dynamicCast(remoteNode->getGlobalPose());
                result4f = gp->toEigen().inverse() * result4f;
                axis = result4f.block(0, 0, 3, 1);
 
                result = revoluteNodeFactory->createRobotNode(robo, name, VirtualRobot::VisualizationNodePtr(), VirtualRobot::CollisionModelPtr(),
                         jvLo, jvHi, jointOffset, localTransform, axis, idVec3, physics);
            }
            break;
 
            default:
                ARMARX_ERROR_S << "JointType nyi...";
                return VirtualRobot::RobotNodePtr();
                break;
        }
 
        robo->registerRobotNode(result);
        allNodes.push_back(result);
 
 
        // setup joint->nextNodes children
        std::vector<std::string> childrenBase = remoteNode->getChildren();
        std::vector<std::string> children;
 
        // check for RobotNodes (sensors do also register as children!)
        for (size_t i = 0; i < childrenBase.size(); i++)
        {
            if (_robot->hasRobotNode(childrenBase[i]))
            {
                SharedRobotNodeInterfacePrx rnRemote = _robot->getRobotNode(childrenBase[i]);
                VirtualRobot::RobotNodePtr localNode = createLocalNode(rnRemote, allNodes, childrenMap, robo);
 
                if (!localNode)
                {
                    ARMARX_ERROR_S << "Could not create local node: " << children[i];
                    continue;
                }
 
                children.push_back(childrenBase[i]);
            }
        }
 
        childrenMap[result] = children;
        return result;
    }
 
    VirtualRobot::RobotPtr RemoteRobot::createLocalClone()
    {
        std::scoped_lock cloneLock(m);
        std::string robotType = getName();
        std::string robotName = getName();
        VirtualRobot::RobotPtr robo(new VirtualRobot::LocalRobot(robotName, robotType));
 
        //RobotNodePtr
        SharedRobotNodeInterfacePrx root = _robot->getRootNode();
 
        std::vector<VirtualRobot::RobotNodePtr> allNodes;
        std::map< VirtualRobot::RobotNodePtr, std::vector<std::string> > childrenMap;
 
        VirtualRobot::RobotNodePtr rootLocal = createLocalNode(root, allNodes, childrenMap, robo);
 
        bool res = VirtualRobot::RobotFactory::initializeRobot(robo, allNodes, childrenMap, rootLocal);
 
        if (!res)
        {
            ARMARX_ERROR_S << "Failed to initialize local robot...";
            return VirtualRobot::RobotPtr();
        }
 
        // clone rns
        std::vector<std::string> rns = _robot->getRobotNodeSets();
 
        for (size_t i = 0; i < rns.size(); i++)
        {
            RobotNodeSetInfoPtr rnsInfo = _robot->getRobotNodeSet(rns[i]);
            RobotNodeSet::createRobotNodeSet(robo, rnsInfo->name, rnsInfo->names, rnsInfo->rootName, rnsInfo->tcpName, true);
        }
 
        //ARMARX_IMPORTANT_S << "RemoteRobot local clone end" << flush;
        auto pose = PosePtr::dynamicCast(_robot->getGlobalPose());
        robo->setGlobalPose(pose->toEigen());
        return robo;
    }
 
    VirtualRobot::RobotPtr RemoteRobot::createLocalClone(RobotStateComponentInterfacePrx robotStatePrx, const std::string& filename, const Ice::StringSeq packages, VirtualRobot::RobotIO::RobotDescription loadMode)
    {
        return createLocalClone(robotStatePrx->getSynchronizedRobot(), filename, robotStatePrx->getScaling(), packages, loadMode);
    }
 
    RobotPtr RemoteRobot::createLocalClone(SharedRobotInterfacePrx sharedRobotPrx, std::string filename, float scaling, const Ice::StringSeq packages, VirtualRobot::RobotIO::RobotDescription loadMode)
    {
        RobotPtr result;
 
        std::scoped_lock cloneLock(m);
        ARMARX_VERBOSE_S << "Creating local clone of remote robot (filename:" << filename << ")";
 
 
        if (!sharedRobotPrx)
        {
            ARMARX_ERROR_S << "NULL sharedRobotPrx. Aborting...";
            return result;
        }
 
        if (filename.empty())
        {
            std::shared_ptr<RemoteRobot> rob(new RemoteRobot(sharedRobotPrx));
            result = rob->createLocalClone();
 
            if (!result)
            {
                ARMARX_ERROR_S << "Could not create local clone. Aborting...";
                return result;
            }
        }
        else
        {
            Ice::StringSeq includePaths;
            for (const std::string& projectName : packages)
            {
                if (projectName.empty())
                {
                    continue;
                }
 
                CMakePackageFinder project(projectName);
 
                auto pathsString = project.getDataDir();
                ARMARX_DEBUG_S << "Data paths of ArmarX package " << projectName << ": " << pathsString;
                Ice::StringSeq projectIncludePaths = simox::alg::split(pathsString, ";,");
                ARMARX_DEBUG_S << "Result: Data paths of ArmarX package " << projectName << ": " << projectIncludePaths;
                includePaths.insert(includePaths.end(), projectIncludePaths.begin(), projectIncludePaths.end());
 
            }
 
 
            if (!ArmarXDataPath::getAbsolutePath(filename, filename, includePaths))
            {
                ARMARX_ERROR_S << "Could find robot file " << filename;
                return result;
            }
            result = RobotIO::loadRobot(filename, loadMode);
 
            if (!result)
            {
                ARMARX_ERROR_S << "Could not load robot file " << filename << ". Aborting...";
                return result;
            }
        }
 
        if (result && scaling != 1.0f)
        {
            ARMARX_INFO_S << "Scaling robot to " << scaling;
            result = result->clone(result->getName(), result->getCollisionChecker(), scaling);
        }
 
        synchronizeLocalClone(result, sharedRobotPrx);
        return result;
    }
 
    RobotPtr RemoteRobot::createLocalCloneFromFile(RobotStateComponentInterfacePrx robotStatePrx, RobotIO::RobotDescription loadMode)
    {
        return createLocalClone(robotStatePrx, robotStatePrx->getRobotFilename(), robotStatePrx->getArmarXPackages(), loadMode);
    }
 
 
 
    bool RemoteRobot::synchronizeLocalClone(VirtualRobot::RobotPtr robot, RobotStateComponentInterfacePrx robotStatePrx)
    {
        return synchronizeLocalClone(robot, robotStatePrx->getSynchronizedRobot());
    }
 
    bool RemoteRobot::synchronizeLocalClone(VirtualRobot::RobotPtr robot, SharedRobotInterfacePrx sharedRobotPrx)
    {
        if (!robot)
        {
            ARMARX_ERROR << "Robot is NULL! Aborting...";
            return false;
        }
        if (!sharedRobotPrx)
        {
            ARMARX_ERROR_S << "shared robot prx is NULL! Aborting...";
            return false;
        }
        PoseBasePtr globalPose;
        RobotConfigPtr c(new RobotConfig(robot, "synchronizeLocalClone"));
        NameValueMap jv = sharedRobotPrx->getConfigAndPose(globalPose);
 
        for (NameValueMap::const_iterator it = jv.begin(); it != jv.end(); it++)
        {
            // joint values
            const std::string& jointName = it->first;
            float jointAngle = it->second;
 
            if (!c->setConfig(jointName, jointAngle))
            {
                ARMARX_WARNING << deactivateSpam(10, jointName) << "Joint not known in local copy:" << jointName << ". Skipping...";
            }
        }
 
        robot->setConfig(c);
        auto pose = PosePtr::dynamicCast(globalPose);
        robot->setGlobalPose(pose->toEigen());
        return true;
    }
 
    bool RemoteRobot::synchronizeLocalCloneToTimestamp(RobotPtr robot, RobotStateComponentInterfacePrx robotStatePrx, Ice::Long timestamp)
    {
        ARMARX_CHECK_EXPRESSION(robotStatePrx);
 
        RobotConfigPtr c(new RobotConfig(robot, "synchronizeLocalClone"));
        RobotStateConfig state = robotStatePrx->getRobotStateAtTimestamp(timestamp);
 
        return synchronizeLocalCloneToState(robot, state);
    }
 
    bool RemoteRobot::synchronizeLocalCloneToState(RobotPtr robot, const RobotStateConfig& state)
    {
        ARMARX_CHECK_EXPRESSION(robot);
 
        RobotConfigPtr c(new RobotConfig(robot, "synchronizeLocalClone"));
        if (state.jointMap.empty())
        {
            return false;
        }
 
        for (NameValueMap::const_iterator it = state.jointMap.begin(); it != state.jointMap.end(); it++)
        {
            // joint values
            const std::string& jointName = it->first;
            float jointAngle = it->second;
 
            if (!c->setConfig(jointName, jointAngle))
            {
                ARMARX_WARNING << deactivateSpam(10, jointName) << "Joint not known in local copy:" << jointName << ". Skipping...";
            }
        }
 
        robot->setConfig(c);
        auto pose = PosePtr::dynamicCast(state.globalPose);
        robot->setGlobalPose(pose->toEigen());
 
        return true;
    }
 
 
 
    // Private (unused methods)
 
    bool RemoteRobot::hasEndEffector(const std::string& endEffectorName) const
    {
        return false;
    }
 
    EndEffectorPtr RemoteRobot::getEndEffector(const std::string& endEffectorName) const
    {
        return EndEffectorPtr();
    }
 
    void RemoteRobot::getEndEffectors(std::vector<EndEffectorPtr>& storeEEF) const {}
    void RemoteRobot::setRootNode(RobotNodePtr node) {}
    void RemoteRobot::registerRobotNode(RobotNodePtr node) {}
    void RemoteRobot::deregisterRobotNode(RobotNodePtr node) {}
    void RemoteRobot::registerRobotNodeSet(RobotNodeSetPtr nodeSet) {}
    void RemoteRobot::deregisterRobotNodeSet(RobotNodeSetPtr nodeSet) {}
    void RemoteRobot::registerEndEffector(EndEffectorPtr endEffector) {}
 
    void RemoteRobot::setGlobalPose(const Eigen::Matrix4f& globalPose, bool applyValues)
    {
        if (_robot)
        {
            _robot->setGlobalPose(new Pose(globalPose));
        }
    }
 
    CollisionCheckerPtr RemoteRobotNode_getGlobalCollisionChecker()
    {
        return VirtualRobot::CollisionChecker::getGlobalCollisionChecker();
    }
 
 
}