RemoteRobotNode.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 <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/VirtualRobot.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/interface/core/BasicTypes.h>
 
#include <RobotAPI/libraries/core/FramedPose.h>
 
#include "RemoteRobot.h"
 
namespace armarx
{
 
    using namespace std;
    using namespace VirtualRobot;
    using namespace Eigen;
 
    RobotNodePtr
    RemoteRobot::createRemoteRobotNode(SharedRobotNodeInterfacePrx node, RobotPtr robot)
    {
        switch (node->getType())
        {
            case ePrismatic:
                return RobotNodePtr(new RemoteRobotNode<RobotNodePrismatic>(node, robot));
 
            case eRevolute:
                return RobotNodePtr(new RemoteRobotNode<RobotNodeRevolute>(node, robot));
 
            case eFixed:
                return RobotNodePtr(new RemoteRobotNode<RobotNodeFixed>(node, robot));
 
            default:
                break; // thow an exception
        }
 
        return RobotNodePtr();
    }
 
    template <>
    void
    RemoteRobotNodeInitializer<VirtualRobot::RobotNodeRevolute>::initialize(
        RemoteRobotNode<VirtualRobot::RobotNodeRevolute>* remoteNode)
    {
        // set rotation axis
        remoteNode->jointRotationAxis =
            remoteNode->getGlobalPose().block<3, 3>(0, 0).inverse() *
            Vector3Ptr::dynamicCast(remoteNode->_node->getJointRotationAxis())->toEigen();
    }
 
    template <>
    void
    RemoteRobotNodeInitializer<VirtualRobot::RobotNodePrismatic>::initialize(
        RemoteRobotNode<VirtualRobot::RobotNodePrismatic>* remoteNode)
    {
        // set translation direction
        remoteNode->jointTranslationDirection =
            remoteNode->getGlobalPose().block<3, 3>(0, 0).inverse() *
            Vector3Ptr::dynamicCast(remoteNode->_node->getJointTranslationDirection())->toEigen();
    }
 
    template <>
    void
    RemoteRobotNodeInitializer<VirtualRobot::RobotNodeFixed>::initialize(
        RemoteRobotNode<VirtualRobot::RobotNodeFixed>* remoteNode)
    {
        // nothing to do for fixed joints
    }
 
    template <class RobotNodeType>
    RemoteRobotNode<RobotNodeType>::~RemoteRobotNode()
    {
        try
        {
            _node->unref();
        }
        catch (std::exception& e)
        {
            ARMARX_DEBUG_S << "Unref of SharedRobotNode failed: " << e.what();
        }
        catch (...)
        {
            ARMARX_DEBUG_S << "Unref of SharedRobotNode failed: reason unknown";
        }
    }
 
    template <class RobotNodeType>
    float
    RemoteRobotNode<RobotNodeType>::getJointValue() const
    {
        return _node->getJointValue();
    }
 
    template <class RobotNodeType>
    float
    RemoteRobotNode<RobotNodeType>::getJointLimitHi() const
    {
        return _node->getJointLimitHigh();
    }
 
    template <class RobotNodeType>
    float
    RemoteRobotNode<RobotNodeType>::getJointLimitLo() const
    {
        return _node->getJointLimitLow();
    }
 
    /*
    template<class RobotNodeType>
    Eigen::Matrix4f RemoteRobotNode<RobotNodeType>::getPostJointTransformation(){
        return PosePtr::dynamicCast(_node->getPostJointTransformation())->toEigen();
    }*/
 
    template <class RobotNodeType>
    Eigen::Matrix4f
    RemoteRobotNode<RobotNodeType>::getLocalTransformation()
    {
        return PosePtr::dynamicCast(_node->getLocalTransformation())->toEigen();
    }
 
    template <class RobotNodeType>
    Eigen::Matrix4f
    RemoteRobotNode<RobotNodeType>::getGlobalPose() const
    {
        return FramedPosePtr::dynamicCast(_node->getGlobalPose())->toEigen();
    }
 
    template <class RobotNodeType>
    Eigen::Matrix4f
    RemoteRobotNode<RobotNodeType>::getPoseInRootFrame() const
    {
        return FramedPosePtr::dynamicCast(_node->getPoseInRootFrame())->toEigen();
    }
 
    template <class RobotNodeType>
    Eigen::Vector3f
    RemoteRobotNode<RobotNodeType>::getPositionInRootFrame() const
    {
        Vector3Ptr pos = Vector3Ptr::dynamicCast(_node->getPoseInRootFrame()->position);
        ARMARX_CHECK_EXPRESSION(pos);
        return pos->toEigen();
    }
 
    template <class RobotNodeType>
    bool
    RemoteRobotNode<RobotNodeType>::hasChildNode(const RobotNodePtr child, bool recursive) const
    {
        return false;
    }
 
    template <class RobotNodeType>
    bool
    RemoteRobotNode<RobotNodeType>::hasChildNode(const std::string& child, bool recursive) const
    {
        return _node->hasChild(child, recursive);
    }
 
    template <class RobotNodeType>
    void
    RemoteRobotNode<RobotNodeType>::setJointLimits(float lo, float hi)
    {
    }
 
    template <class RobotNodeType>
    vector<RobotNodePtr>
    RemoteRobotNode<RobotNodeType>::getAllParents(RobotNodeSetPtr rns)
    {
        NameList nodes = _node->getAllParents(rns->getName());
        vector<RobotNodePtr> result;
        RobotPtr robot = this->robot.lock();
 
        for (std::string const& name : nodes)
        {
            result.push_back(robot->getRobotNode(name));
        }
        return result;
    }
 
    template <class RobotNodeType>
    SceneObjectPtr
    RemoteRobotNode<RobotNodeType>::getParent()
    {
        return this->robot.lock()->getRobotNode(_node->getParent());
    }
 
    /*
    template<class RobotNodeType>
    void RemoteRobotNode<RobotNodeType>::setPostJointTransformation(const Eigen::Matrix4f &trafo){
    }*/
    template <class RobotNodeType>
    void
    RemoteRobotNode<RobotNodeType>::setLocalTransformation(const Eigen::Matrix4f& trafo)
    {
    }
 
    template <class RobotNodeType>
    std::vector<std::string>
    RemoteRobotNode<RobotNodeType>::getChildrenNames() const
    {
        return _node->getChildren();
    }
 
    template <class RobotNodeType>
    std::string
    RemoteRobotNode<RobotNodeType>::getParentName() const
    {
        return _node->getParent();
    }
 
    template <class RobotNodeType>
    std::vector<SceneObjectPtr>
    RemoteRobotNode<RobotNodeType>::getChildren() const
    {
        NameList nodes = _node->getChildren();
        vector<SceneObjectPtr> result;
        for (string const& name : nodes)
        {
            result.push_back(this->robot.lock()->getRobotNode(name));
        }
        return result;
    }
 
    template <class RobotNodeType>
    void
    RemoteRobotNode<RobotNodeType>::updateTransformationMatrices()
    {
    }
 
    template <class RobotNodeType>
    void
    RemoteRobotNode<RobotNodeType>::updateTransformationMatrices(const Eigen::Matrix4f& globalPose)
    {
    }
 
    template <class RobotNodeType>
    void
    RemoteRobotNode<RobotNodeType>::addChildNode(RobotNodePtr child)
    {
    }
 
    template <class RobotNodeType>
    bool
    RemoteRobotNode<RobotNodeType>::initialize(RobotNodePtr parent, bool initializeChildren)
    {
        return false;
    }
 
    template <class RobotNodeType>
    void
    RemoteRobotNode<RobotNodeType>::reset()
    {
    }
 
    template <class RobotNodeType>
    void
    RemoteRobotNode<RobotNodeType>::setGlobalPose(const Eigen::Matrix4f& pose)
    {
    }
 
    template <class RobotNodeType>
    void
    RemoteRobotNode<RobotNodeType>::setJointValue(float q,
                                                  bool updateTransformations,
                                                  bool clampToLimits)
    {
    }
 
} // namespace armarx