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 <Eigen/Geometry>
 
#include <SimoxUtility/algorithm/string/string_tools.h>
#include <VirtualRobot/CollisionDetection/CollisionChecker.h>
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/Nodes/RobotNodeFixed.h>
#include <VirtualRobot/Nodes/RobotNodeFixedFactory.h>
#include <VirtualRobot/Nodes/RobotNodePrismatic.h>
#include <VirtualRobot/Nodes/RobotNodePrismaticFactory.h>
#include <VirtualRobot/Nodes/RobotNodeRevolute.h>
#include <VirtualRobot/Nodes/RobotNodeRevoluteFactory.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotConfig.h>
#include <VirtualRobot/RobotFactory.h>
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/XML/RobotIO.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
 
#include <RobotAPI/interface/core/RobotState.h>
#include <RobotAPI/libraries/core/FramedPose.h>
 
//#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();
    }
 
 
} // namespace armarx