FramedPose.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 <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
 
#include <ArmarXCore/observers/AbstractObjectSerializer.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
 
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/LinkedCoordinate.h>
#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/RobotConfig.h>
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
 
 
using namespace Eigen;
 
template class ::IceInternal::Handle<::armarx::FramedPose>;
template class ::IceInternal::Handle<::armarx::FramedPosition>;
template class ::IceInternal::Handle<::armarx::FramedDirection>;
template class ::IceInternal::Handle<::armarx::FramedOrientation>;
 
namespace armarx
{
 
 
    FramedDirection::FramedDirection()
        = default;
 
    FramedDirection::FramedDirection(const FramedDirection& source) :
        IceUtil::Shared(source),
        armarx::Serializable(source),
        Vector3Base(source),
        FramedDirectionBase(source),
        Vector3(source)
    {
    }
 
    FramedDirection::FramedDirection(const Eigen::Vector3f& vec, const std::string& frame, const std::string& agent) :
        Vector3(vec)
    {
        this->frame = frame;
        this->agent = agent;
 
    }
 
    FramedDirection::FramedDirection(Ice::Float x, ::Ice::Float y, ::Ice::Float z, const std::string& frame, const std::string& agent) :
        Vector3(x, y, z)
    {
        this->frame = frame;
        this->agent = agent;
    }
 
    std::string FramedDirection::getFrame() const
    {
        return frame;
    }
 
    FramedDirectionPtr FramedDirection::ChangeFrame(const VirtualRobot::RobotPtr& robot, const FramedDirection& framedVec, const std::string& newFrame)
    {
        ARMARX_CHECK_NOT_NULL(robot);
        return ChangeFrame(*robot, framedVec, newFrame);
    }
 
    FramedDirectionPtr FramedDirection::ChangeFrame(const VirtualRobot::Robot& robot, const FramedDirection& framedVec, const std::string& newFrame)
    {
        if (framedVec.getFrame() == newFrame)
        {
            return FramedDirectionPtr::dynamicCast(framedVec.clone());
        }
 
        if (!robot.hasRobotNode(newFrame))
        {
            throw LocalException() << "The requested frame '" << newFrame << "' does not exists in the robot " << robot.getName();
        }
 
        Eigen::Matrix4f rotToNewFrame = __GetRotationBetweenFrames(framedVec.frame, newFrame, robot);
 
        Eigen::Vector3f vecOldFrame = framedVec.Vector3::toEigen();
 
        Eigen::Vector3f newVec = rotToNewFrame.block(0, 0, 3, 3).inverse() * vecOldFrame;
 
        FramedDirectionPtr result = new FramedDirection();
        result->x = newVec(0);
        result->y = newVec(1);
        result->z = newVec(2);
        result->frame = newFrame;
        result->agent = robot.getName();
        return result;
    }
 
    void FramedDirection::changeFrame(const VirtualRobot::RobotPtr& robot, const std::string& newFrame)
    {
        ARMARX_CHECK_NOT_NULL(robot);
        changeFrame(*robot, newFrame);
    }
 
    void FramedDirection::changeFrame(const VirtualRobot::Robot& robot, const std::string& newFrame)
    {
        if (frame == "")
        {
            frame = GlobalFrame;
        }
 
 
        if (getFrame() == newFrame)
        {
            return;
        }
 
        if (newFrame == GlobalFrame)
        {
            changeToGlobal(robot);
            return;
        }
 
        if (!robot.hasRobotNode(newFrame))
        {
            throw LocalException() << "The requested frame '" << newFrame << "' does not exist in the robot " << robot.getName();
        }
 
        if (frame != GlobalFrame && !robot.hasRobotNode(frame))
        {
            throw LocalException() << "The current frame '" << frame << "' does not exist in the robot " << robot.getName();
        }
 
 
        Eigen::Matrix4f rotToNewFrame = __GetRotationBetweenFrames(frame, newFrame, robot);
 
        Eigen::Vector3f vecOldFrame = Vector3::toEigen();
 
        Eigen::Vector3f newVec = rotToNewFrame.block(0, 0, 3, 3).inverse() * vecOldFrame;
 
 
        x = newVec(0);
        y = newVec(1);
        z = newVec(2);
        frame = newFrame;
        agent = robot.getName();
    }
 
    void FramedDirection::changeToGlobal(const SharedRobotInterfacePrx& referenceRobot)
    {
        VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
        changeToGlobal(sharedRobot);
    }
 
    void FramedDirection::changeToGlobal(const VirtualRobot::RobotPtr& referenceRobot)
    {
        ARMARX_CHECK_NOT_NULL(referenceRobot);
        changeToGlobal(*referenceRobot);
    }
 
    void FramedDirection::changeToGlobal(const VirtualRobot::Robot& referenceRobot)
    {
        if (frame == GlobalFrame)
        {
            return;
        }
 
        changeFrame(referenceRobot, referenceRobot.getRootNode()->getName());
        Eigen::Vector3f pos = referenceRobot.getRootNode()->getGlobalPose().block<3, 3>(0, 0) * toEigen();
        x = pos[0];
        y = pos[1];
        z = pos[2];
        frame = GlobalFrame;
        agent = "";
    }
 
    FramedDirectionPtr FramedDirection::toGlobal(const SharedRobotInterfacePrx& referenceRobot) const
    {
        VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
        return toGlobal(sharedRobot);
    }
    FramedDirectionPtr FramedDirection::toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const
    {
        ARMARX_CHECK_NOT_NULL(referenceRobot);
        return toGlobal(*referenceRobot);
    }
    FramedDirectionPtr FramedDirection::toGlobal(const VirtualRobot::Robot& referenceRobot) const
    {
        FramedDirectionPtr newDirection = FramedDirectionPtr::dynamicCast(this->clone());
        newDirection->changeToGlobal(referenceRobot);
        return newDirection;
    }
 
    Eigen::Vector3f FramedDirection::toGlobalEigen(const SharedRobotInterfacePrx& referenceRobot) const
    {
        VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
        return toGlobalEigen(sharedRobot);
    }
    Eigen::Vector3f FramedDirection::toGlobalEigen(const VirtualRobot::RobotPtr& referenceRobot) const
    {
        ARMARX_CHECK_NOT_NULL(referenceRobot);
        return toGlobalEigen(*referenceRobot);
    }
    Eigen::Vector3f FramedDirection::toGlobalEigen(const VirtualRobot::Robot& referenceRobot) const
    {
        FramedDirection newDirection(toEigen(), frame, agent);
        newDirection.changeToGlobal(referenceRobot);
        return newDirection.toEigen();
    }
 
    FramedDirectionPtr FramedDirection::toRootFrame(const SharedRobotInterfacePrx& referenceRobot) const
    {
        VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
        return toRootFrame(sharedRobot);
    }
    FramedDirectionPtr FramedDirection::toRootFrame(const VirtualRobot::RobotPtr& referenceRobot) const
    {
        ARMARX_CHECK_NOT_NULL(referenceRobot);
        return toRootFrame(*referenceRobot);
    }
    FramedDirectionPtr FramedDirection::toRootFrame(const VirtualRobot::Robot& referenceRobot) const
    {
        FramedDirectionPtr newDirection = FramedDirectionPtr::dynamicCast(this->clone());
        newDirection->changeFrame(referenceRobot, referenceRobot.getRootNode()->getName());
        return newDirection;
    }
 
    Eigen::Vector3f FramedDirection::toRootEigen(const SharedRobotInterfacePrx& referenceRobot) const
    {
        VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
        return toRootEigen(sharedRobot);
    }
    Eigen::Vector3f FramedDirection::toRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const
    {
        ARMARX_CHECK_NOT_NULL(referenceRobot);
        return toRootEigen(*referenceRobot);
    }
    Eigen::Vector3f FramedDirection::toRootEigen(const VirtualRobot::Robot& referenceRobot) const
    {
        FramedDirection newDirection(toEigen(), frame, agent);
        newDirection.changeFrame(referenceRobot, referenceRobot.getRootNode()->getName());
        return newDirection.toEigen();
    }
 
    std::string FramedDirection::output(const Ice::Current& c) const
    {
        std::stringstream s;
        s << toEigen().format(ArmarXEigenFormat) << std::endl << "frame: " << getFrame() << (agent.empty() ? "" : (" agent: " + agent));
        return s.str();
    }
 
    Eigen::Matrix4f FramedDirection::__GetRotationBetweenFrames(const std::string& oldFrame, const std::string& newFrame, const VirtualRobot::Robot& robotState)
    {
        Eigen::Matrix4f toNewFrame;
 
        if (oldFrame == GlobalFrame)
        {
            auto node = robotState.getRobotNode(newFrame);
            ARMARX_CHECK_EXPRESSION(node) << newFrame;
            toNewFrame = node->getGlobalPose();
        }
        else
        {
            auto node = robotState.getRobotNode(oldFrame);
            ARMARX_CHECK_EXPRESSION(node) << "old frame: " + oldFrame + ValueToString(robotState.getConfig()->getRobotNodeJointValueMap());
            auto newNode = robotState.getRobotNode(newFrame);
            ARMARX_CHECK_EXPRESSION(newNode) << newFrame;
            toNewFrame = node->getTransformationTo(newNode);
        }
 
        toNewFrame(0, 3) = 0;
        toNewFrame(1, 3) = 0;
        toNewFrame(2, 3) = 0;
 
        return toNewFrame;
    }
 
 
    void FramedDirection::serialize(const ObjectSerializerBasePtr& serializer, const Ice::Current&) const
    {
        AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
        Vector3::serialize(serializer);
        obj->setString("frame", frame);
        obj->setString("agent", agent);
 
    }
 
    void FramedDirection::deserialize(const ObjectSerializerBasePtr& serializer, const Ice::Current&)
    {
        AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
        Vector3::deserialize(serializer);
        frame = obj->getString("frame");
 
        if (obj->hasElement("agent"))
        {
            agent = obj->getString("agent");
        }
    }
 
    Ice::ObjectPtr FramedDirection::ice_clone() const
    {
        return this->clone();
    }
 
    VariantDataClassPtr FramedDirection::clone(const Ice::Current& c) const
    {
        return new FramedDirection(*this);
    }
 
    VariantTypeId FramedDirection::getType(const Ice::Current& c) const
    {
        return VariantType::FramedDirection;
    }
 
    bool FramedDirection::validate(const Ice::Current& c)
    {
        return true;
    }
 
    std::ostream& operator<<(std::ostream& stream, const FramedDirection& rhs)
    {
        stream << "FramedDirection: " << std::endl << rhs.output() << std::endl;
        return stream;
    }
 
 
    FramedPose::FramedPose() :
        Pose()
    {
        frame = "";
    }
 
    FramedPose::FramedPose(const FramedPose& pose) :
        IceUtil::Shared(pose),
        armarx::Serializable(pose),
        armarx::VariantDataClass(pose),
        PoseBase(pose),
        FramedPoseBase(pose),
        Pose(pose)
    {
 
    }
 
    FramedPose::FramedPose(const Eigen::Matrix3f& m, const Eigen::Vector3f& v, const std::string& s, const std::string& agent) :
        Pose(m, v)
    {
        frame = s;
        this->agent = agent;
    }
 
    FramedPose::FramedPose(const Eigen::Matrix4f& m, const std::string& s, const std::string& agent) :
        Pose(m)
    {
        frame = s;
        this->agent = agent;
    }
 
    FramedPose::FramedPose(const armarx::Vector3BasePtr pos, const armarx::QuaternionBasePtr ori, const std::string& frame, const std::string& agent) :
        Pose(pos, ori)
    {
        this->frame = frame;
        this->agent = agent;
    }
 
    std::string FramedPose::getFrame() const
    {
        return frame;
    }
 
    std::string FramedPose::output(const Ice::Current& c) const
    {
        std::stringstream s;
        const Eigen::IOFormat ArmarXEigenFormat(Eigen::StreamPrecision, 4, " ", "\n", "", "");
        s << toEigen().format(ArmarXEigenFormat) << std::endl << "frame: " << getFrame() << (agent.empty() ? "" : (" agent: " + agent));
        return s.str();
    }
 
    void FramedPose::changeFrame(const SharedRobotInterfacePrx& referenceRobot, const std::string& newFrame)
    {
        if (newFrame == frame)
        {
            return;
        }
 
        VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
        changeFrame(sharedRobot, newFrame);
    }
 
    void FramedPose::changeFrame(const VirtualRobot::RobotPtr& referenceRobot, const std::string& newFrame)
    {
        ARMARX_CHECK_NOT_NULL(referenceRobot);
        changeFrame(*referenceRobot, newFrame);
    }
 
    void FramedPose::changeFrame(const VirtualRobot::Robot& referenceRobot, const std::string& newFrame)
    {
        if (newFrame == frame)
        {
            return;
        }
 
        if (newFrame == GlobalFrame)
        {
            changeToGlobal(referenceRobot);
            return;
        }
 
        Eigen::Matrix4f oldFrameTransformation = Eigen::Matrix4f::Identity();
 
        if (frame != GlobalFrame and !referenceRobot.hasRobotNode(frame))
        {
            throw LocalException() << "The current frame '" << frame << "' does not exists in the robot " << referenceRobot.getName();
        }
        if (!referenceRobot.hasRobotNode(newFrame))
        {
            throw LocalException() << "The requested frame '" << newFrame << "' does not exists in the robot " << referenceRobot.getName();
        }
 
        if (frame != GlobalFrame)
        {
            oldFrameTransformation = referenceRobot.getRobotNode(frame)->getPoseInRootFrame();
        }
        else
        {
            oldFrameTransformation = referenceRobot.getRootNode()->getGlobalPose().inverse();
        }
 
        Eigen::Matrix4f newPose =
            (referenceRobot.getRobotNode(newFrame)->getPoseInRootFrame().inverse() * oldFrameTransformation) * toEigen();
 
        orientation = new Quaternion(newPose);
        Eigen::Vector3f pos = newPose.block<3, 1>(0, 3);
        position = new Vector3(pos);
        frame = newFrame;
        agent = referenceRobot.getName();
        init();
    }
 
    void FramedPose::changeToGlobal(const SharedRobotInterfacePrx& referenceRobot)
    {
        VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
        changeToGlobal(sharedRobot);
 
    }
    void FramedPose::changeToGlobal(const VirtualRobot::RobotPtr& referenceRobot)
    {
        ARMARX_CHECK_NOT_NULL(referenceRobot);
        changeToGlobal(*referenceRobot);
    }
    void FramedPose::changeToGlobal(const VirtualRobot::Robot& referenceRobot)
    {
        if (frame == GlobalFrame)
        {
            return;
        }
 
        changeFrame(referenceRobot, referenceRobot.getRootNode()->getName());
        Eigen::Matrix4f global = referenceRobot.getRootNode()->getGlobalPose();
        const Eigen::Matrix4f pose = global * toEigen();
        position->x = pose(0, 3);
        position->y = pose(1, 3);
        position->z = pose(2, 3);
        Eigen::Quaternionf q(pose.block<3, 3>(0, 0));
        orientation->qw = q.w();
        orientation->qx = q.x();
        orientation->qy = q.y();
        orientation->qz = q.z();
        frame = GlobalFrame;
        agent = "";
    }
 
    FramedPosePtr FramedPose::toGlobal(const SharedRobotInterfacePrx& referenceRobot) const
    {
        VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
        return toGlobal(sharedRobot);
    }
    FramedPosePtr FramedPose::toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const
    {
        ARMARX_CHECK_NOT_NULL(referenceRobot);
        return toGlobal(*referenceRobot);
    }
    FramedPosePtr FramedPose::toGlobal(const VirtualRobot::Robot& referenceRobot) const
    {
        FramedPosePtr newPose = FramedPosePtr::dynamicCast(this->clone());
        newPose->changeToGlobal(referenceRobot);
        return newPose;
    }
 
    Eigen::Matrix4f FramedPose::toGlobalEigen(const SharedRobotInterfacePrx& referenceRobot) const
    {
        VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
        return toGlobalEigen(sharedRobot);
    }
    Eigen::Matrix4f FramedPose::toGlobalEigen(const VirtualRobot::RobotPtr& referenceRobot) const
    {
        ARMARX_CHECK_NOT_NULL(referenceRobot);
        return toGlobalEigen(*referenceRobot);
    }
    Eigen::Matrix4f FramedPose::toGlobalEigen(const VirtualRobot::Robot& referenceRobot) const
    {
        FramedPose newPose(toEigen(), frame, agent);
        newPose.changeToGlobal(referenceRobot);
        return newPose.toEigen();
    }
 
    FramedPosePtr FramedPose::toRootFrame(const SharedRobotInterfacePrx& referenceRobot) const
    {
        VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
        return toRootFrame(sharedRobot);
    }
    FramedPosePtr FramedPose::toRootFrame(const VirtualRobot::RobotPtr& referenceRobot) const
    {
        ARMARX_CHECK_NOT_NULL(referenceRobot);
        return toRootFrame(*referenceRobot);
    }
    FramedPosePtr FramedPose::toRootFrame(const VirtualRobot::Robot& referenceRobot) const
    {
        FramedPosePtr newPose = FramedPosePtr::dynamicCast(this->clone());
        newPose->changeFrame(referenceRobot, referenceRobot.getRootNode()->getName());
        return newPose;
    }
 
    Eigen::Matrix4f FramedPose::toRootEigen(const SharedRobotInterfacePrx& referenceRobot) const
    {
        VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
        return toRootEigen(sharedRobot);
    }
    Eigen::Matrix4f FramedPose::toRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const
    {
        ARMARX_CHECK_NOT_NULL(referenceRobot);
        return toRootEigen(*referenceRobot);
    }
    Eigen::Matrix4f FramedPose::toRootEigen(const VirtualRobot::Robot& referenceRobot) const
    {
        FramedPose newPose(toEigen(), frame, agent);
        newPose.changeFrame(referenceRobot, referenceRobot.getRootNode()->getName());
        return newPose.toEigen();
    }
 
 
    FramedPositionPtr FramedPose::getPosition() const
    {
        FramedPositionPtr result = new FramedPosition(Vector3Ptr::dynamicCast(position)->toEigen(), frame, agent);
        return result;
    }
 
    FramedOrientationPtr FramedPose::getOrientation() const
    {
        FramedOrientationPtr result = new FramedOrientation(QuaternionPtr::dynamicCast(this->orientation)->toEigen(), frame, agent);
        return result;
    }
 
    void FramedPose::serialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c) const
    {
        AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
 
        Pose::serialize(obj, c);
        obj->setString("frame", frame);
        obj->setString("agent", agent);
    }
 
    void FramedPose::deserialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c)
    {
        AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
 
        Pose::deserialize(obj);
        frame = obj->getString("frame");
 
        if (obj->hasElement("agent"))
        {
            agent = obj->getString("agent");
        }
    }
 
    FramedPosition::FramedPosition() :
        Vector3()
    {
        frame = "";
    }
 
    FramedPosition::FramedPosition(const Eigen::Vector3f& v, const std::string& s, const std::string& agent) :
        Vector3(v)
    {
        frame = s;
        this->agent = agent;
    }
 
    FramedPosition::FramedPosition(const Eigen::Matrix4f& m, const std::string& s, const std::string& agent) :
        Vector3(m)
    {
        frame = s;
        this->agent = agent;
    }
 
    // this doesnt work for unknown reasons
    //    FramedPosition::FramedPosition(const Vector3BasePtr pos, const std::string &frame )
    //    {
    //        this->x = pos->x;
    //        this->y = pos->y;
    //        this->z = pos->z;
    //        this->frame = frame;
    //    }
 
    std::string FramedPosition::getFrame() const
    {
        return this->frame;
    }
 
    void FramedPosition::changeFrame(const SharedRobotInterfacePrx& referenceRobot, const std::string& newFrame)
    {
        if (newFrame == frame)
        {
            return;
        }
 
        if (!referenceRobot->hasRobotNode(newFrame))
        {
            throw LocalException() << "The requested frame '" << newFrame << "' does not exists in the robot " << referenceRobot->getName();
        }
 
        VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
        changeFrame(sharedRobot, newFrame);
    }
 
    void FramedPosition::changeFrame(const VirtualRobot::RobotPtr& referenceRobot, const std::string& newFrame)
    {
        ARMARX_CHECK_NOT_NULL(referenceRobot);
        changeFrame(*referenceRobot, newFrame);
    }
 
    void FramedPosition::changeFrame(const VirtualRobot::Robot& referenceRobot, const std::string& newFrame)
    {
        if (newFrame == frame)
        {
            return;
        }
 
        if (newFrame == GlobalFrame)
        {
            changeToGlobal(referenceRobot);
            return;
        }
 
        if (newFrame.empty())
        {
            ARMARX_WARNING_S << "Frame Conversion: Frame is empty, always set frame names! ...assuming GlobalFrame";
            changeToGlobal(referenceRobot);
            return;
        }
 
        Eigen::Matrix4f oldFrameTransformation = Eigen::Matrix4f::Identity();
 
        if (!referenceRobot.hasRobotNode(newFrame))
        {
            throw LocalException() << "The requested frame '" << newFrame << "' does not exists in the robot " << referenceRobot.getName();
        }
 
        if (frame != GlobalFrame)
        {
            oldFrameTransformation = referenceRobot.getRobotNode(frame)->getPoseInRootFrame();
        }
        else
        {
            oldFrameTransformation = referenceRobot.getRootNode()->getGlobalPose().inverse();
        }
 
        Eigen::Matrix4f oldPose = Eigen::Matrix4f::Identity();
        oldPose.block<3, 1>(0, 3) = toEigen();
        Eigen::Matrix4f newPose =
            (referenceRobot.getRobotNode(newFrame)->getPoseInRootFrame().inverse() * oldFrameTransformation) * oldPose;
 
        Eigen::Vector3f pos = newPose.block<3, 1>(0, 3);
        x = pos[0];
        y = pos[1];
        z = pos[2];
        agent = referenceRobot.getName();
        frame = newFrame;
    }
 
    void FramedPosition::changeToGlobal(const SharedRobotInterfacePrx& referenceRobot)
    {
        VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
        changeToGlobal(sharedRobot);
    }
    void FramedPosition::changeToGlobal(const VirtualRobot::RobotPtr& referenceRobot)
    {
        ARMARX_CHECK_NOT_NULL(referenceRobot);
        changeToGlobal(*referenceRobot);
    }
    void FramedPosition::changeToGlobal(const VirtualRobot::Robot& referenceRobot)
    {
        if (frame == GlobalFrame)
        {
            return;
        }
 
        changeFrame(referenceRobot, referenceRobot.getRootNode()->getName());
        Eigen::Vector3f pos = referenceRobot.getRootNode()->getGlobalPose().block<3, 3>(0, 0) * toEigen()
                              + referenceRobot.getRootNode()->getGlobalPose().block<3, 1>(0, 3);
        x = pos[0];
        y = pos[1];
        z = pos[2];
        frame = GlobalFrame;
        agent = "";
    }
 
    FramedPositionPtr FramedPosition::toGlobal(const SharedRobotInterfacePrx& referenceRobot) const
    {
        VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
        return toGlobal(sharedRobot);
    }
    FramedPositionPtr FramedPosition::toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const
    {
        ARMARX_CHECK_NOT_NULL(referenceRobot);
        return toGlobal(*referenceRobot);
    }
    FramedPositionPtr FramedPosition::toGlobal(const VirtualRobot::Robot& referenceRobot) const
    {
        FramedPositionPtr newPosition = FramedPositionPtr::dynamicCast(this->clone());
        newPosition->changeToGlobal(referenceRobot);
        return newPosition;
    }
 
    Eigen::Vector3f FramedPosition::toGlobalEigen(const SharedRobotInterfacePrx& referenceRobot) const
    {
        VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
        return toGlobalEigen(sharedRobot);
    }
    Eigen::Vector3f FramedPosition::toGlobalEigen(const VirtualRobot::RobotPtr& referenceRobot) const
    {
        ARMARX_CHECK_NOT_NULL(referenceRobot);
        return toGlobalEigen(*referenceRobot);
    }
    Eigen::Vector3f FramedPosition::toGlobalEigen(const VirtualRobot::Robot& referenceRobot) const
    {
        FramedPosition newPosition(toEigen(), frame, agent);
        newPosition.changeToGlobal(referenceRobot);
        return newPosition.toEigen();
    }
 
    FramedPositionPtr FramedPosition::toRootFrame(const SharedRobotInterfacePrx& referenceRobot) const
    {
        VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
        return toRootFrame(sharedRobot);
    }
    FramedPositionPtr FramedPosition::toRootFrame(const VirtualRobot::RobotPtr& referenceRobot) const
    {
        ARMARX_CHECK_NOT_NULL(referenceRobot);
        return toRootFrame(*referenceRobot);
    }
    FramedPositionPtr FramedPosition::toRootFrame(const VirtualRobot::Robot& referenceRobot) const
    {
        FramedPositionPtr newPosition = FramedPositionPtr::dynamicCast(this->clone());
        newPosition->changeFrame(referenceRobot, referenceRobot.getRootNode()->getName());
        return newPosition;
    }
 
    Eigen::Vector3f FramedPosition::toRootEigen(const SharedRobotInterfacePrx& referenceRobot) const
    {
        VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
        return toRootEigen(sharedRobot);
    }
    Eigen::Vector3f FramedPosition::toRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const
    {
        ARMARX_CHECK_NOT_NULL(referenceRobot);
        return toRootEigen(*referenceRobot);
    }
    Eigen::Vector3f FramedPosition::toRootEigen(const VirtualRobot::Robot& referenceRobot) const
    {
        FramedPosition newPosition(toEigen(), frame, agent);
        newPosition.changeFrame(referenceRobot, referenceRobot.getRootNode()->getName());
        return newPosition.toEigen();
    }
 
    std::string FramedPosition::output(const Ice::Current& c) const
    {
        std::stringstream s;
        s << toEigen().format(ArmarXEigenFormat) << std::endl << "frame: " << getFrame() << (agent.empty() ? "" : (" agent: " + agent));
        return s.str();
    }
 
    void FramedPosition::serialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c) const
    {
        AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
 
        Vector3::serialize(obj, c);
        obj->setString("frame", frame);
        obj->setString("agent", agent);
    }
 
    void FramedPosition::deserialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c)
    {
        AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
 
        Vector3::deserialize(obj);
        frame = obj->getString("frame");
 
        if (obj->hasElement("agent"))
        {
            agent = obj->getString("agent");
        }
    }
 
    FramedPosition::FramedPosition(const FramedPosition& other) :
            Shared(other),
            armarx::Serializable(other),
            Vector3Base(other.x, other.y, other.z),
            FramedPositionBase(other.x, other.y, other.z, other.frame, other.agent),
            Vector3(other.x, other.y, other.z)
    {
    }
 
    FramedPosition& FramedPosition::operator=(const FramedPosition& other)
    {
        x = other.x;
        y = other.y;
        z = other.z;
        frame = other.frame;
        agent = other.agent;
        return *this;
    }
 
    Ice::ObjectPtr FramedPosition::ice_clone() const
    {
        return this->clone();
    }
 
    VariantDataClassPtr FramedPosition::clone(const Ice::Current& c) const
    {
        return new FramedPosition(*this);
    }
 
    VariantTypeId FramedPosition::getType(const Ice::Current& c) const
    {
        return VariantType::FramedPosition;
    }
 
    bool FramedPosition::validate(const Ice::Current& c)
    {
        return true;
    }
 
    std::ostream& operator<<(std::ostream& stream, const FramedPosition& rhs)
    {
        stream << "FramedPosition: " << std::endl << rhs.output() << std::endl;
        return stream;
    }
 
 
    FramedOrientation::FramedOrientation() :
        Quaternion()
    {
        frame = "";
    }
 
    FramedOrientation::FramedOrientation(const Eigen::Matrix3f& m, const std::string& s, const std::string& agent) :
        Quaternion(m)
    {
        frame = s;
        this->agent = agent;
    }
 
    FramedOrientation::FramedOrientation(const Quaternionf& q, const std::string& frame, const std::string& agent):
        Quaternion(q)
    {
        this->frame = frame;
        this->agent = agent;
    }
 
    FramedOrientation::FramedOrientation(const Eigen::Matrix4f& m, const std::string& s, const std::string& agent) :
        Quaternion(m)
    {
        frame = s;
        this->agent = agent;
    }
 
    // this doesnt work for an unknown reason
    //    FramedOrientation::FramedOrientation(const QuaternionBasePtr ori, const std::string &frame )
    //    {
    //        Matrix3f rot;
    //        rot = Quaternionf(ori->qw, ori->qx, ori->qy, ori->qz);
    //        FramedOrientation(rot, frame);
    //    }
 
    std::string FramedOrientation::getFrame() const
    {
        return this->frame;
    }
 
    std::string FramedOrientation::output(const Ice::Current& c) const
    {
        std::stringstream s;
        s << toEigen()/*.format(ArmarXEigenFormat)*/ << std::endl << "frame: " << getFrame() << (agent.empty() ? "" : (" agent: " + agent));
        return s.str();
    }
 
    void FramedOrientation::changeFrame(const SharedRobotInterfacePrx& referenceRobot, const std::string& newFrame)
    {
        if (newFrame == frame)
        {
            return;
        }
 
        VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
 
        changeFrame(sharedRobot, newFrame);
    }
 
    void FramedOrientation::changeFrame(const VirtualRobot::RobotPtr& referenceRobot, const std::string& newFrame)
    {
        ARMARX_CHECK_NOT_NULL(referenceRobot);
        changeFrame(*referenceRobot, newFrame);
    }
 
    void FramedOrientation::changeFrame(const VirtualRobot::Robot& referenceRobot, const std::string& newFrame)
    {
        if (newFrame == frame)
        {
            return;
        }
 
        if (newFrame == GlobalFrame)
        {
            changeToGlobal(referenceRobot);
            return;
        }
 
        Eigen::Matrix4f oldFrameTransformation = Eigen::Matrix4f::Identity();
 
        if (!referenceRobot.hasRobotNode(newFrame))
        {
            throw LocalException() << "The requested frame '" << newFrame << "' does not exists in the robot " << referenceRobot.getName();
        }
 
        if (frame != GlobalFrame)
        {
            oldFrameTransformation = referenceRobot.getRobotNode(frame)->getPoseInRootFrame();
        }
        else
        {
            oldFrameTransformation = referenceRobot.getRootNode()->getGlobalPose().inverse();
        }
 
        Eigen::Matrix4f oldPose = Eigen::Matrix4f::Identity();
        oldPose.block<3, 3>(0, 0) = toEigen();
 
        Eigen::Matrix4f newPose =
            (referenceRobot.getRobotNode(newFrame)->getPoseInRootFrame().inverse() * oldFrameTransformation) * oldPose;
 
        Eigen::Quaternionf quat(newPose.block<3, 3>(0, 0));
        qw = quat.w();
        qx = quat.x();
        qy = quat.y();
        qz = quat.z();
        agent = referenceRobot.getName();
        frame = newFrame;
    }
 
    void FramedOrientation::changeToGlobal(const SharedRobotInterfacePrx& referenceRobot)
    {
        VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
        changeToGlobal(sharedRobot);
    }
 
    void FramedOrientation::changeToGlobal(const VirtualRobot::RobotPtr& referenceRobot)
    {
        ARMARX_CHECK_NOT_NULL(referenceRobot);
        changeToGlobal(*referenceRobot);
    }
    void FramedOrientation::changeToGlobal(const VirtualRobot::Robot& referenceRobot)
    {
        if (frame == GlobalFrame)
        {
            return;
        }
 
        changeFrame(referenceRobot, referenceRobot.getRootNode()->getName());
        Eigen::Matrix3f rot = referenceRobot.getRootNode()->getGlobalPose().block<3, 3>(0, 0) * toEigen();
        Eigen::Quaternionf quat(rot);
        qw = quat.w();
        qx = quat.x();
        qy = quat.y();
        qz = quat.z();
        frame = GlobalFrame;
        agent = "";
    }
 
    FramedOrientationPtr FramedOrientation::toGlobal(const SharedRobotInterfacePrx& referenceRobot) const
    {
        VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
        return toGlobal(sharedRobot);
    }
    FramedOrientationPtr FramedOrientation::toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const
    {
        ARMARX_CHECK_NOT_NULL(referenceRobot);
        return toGlobal(*referenceRobot);
    }
    FramedOrientationPtr FramedOrientation::toGlobal(const VirtualRobot::Robot& referenceRobot) const
    {
        FramedOrientationPtr newOrientation = FramedOrientationPtr::dynamicCast(this->clone());
        newOrientation->changeToGlobal(referenceRobot);
        return newOrientation;
    }
 
    Eigen::Matrix3f FramedOrientation::toGlobalEigen(const SharedRobotInterfacePrx& referenceRobot) const
    {
        VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
        return toGlobalEigen(sharedRobot);
    }
    Eigen::Matrix3f FramedOrientation::toGlobalEigen(const VirtualRobot::RobotPtr& referenceRobot) const
    {
        ARMARX_CHECK_NOT_NULL(referenceRobot);
        return toGlobalEigen(*referenceRobot);
    }
    Eigen::Matrix3f FramedOrientation::toGlobalEigen(const VirtualRobot::Robot& referenceRobot) const
    {
        FramedOrientation newOrientation(toEigen(), frame, agent);
        newOrientation.changeToGlobal(referenceRobot);
        return newOrientation.toEigen();
    }
 
    FramedOrientationPtr FramedOrientation::toRootFrame(const SharedRobotInterfacePrx& referenceRobot) const
    {
        VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
        return toRootFrame(sharedRobot);
    }
    FramedOrientationPtr FramedOrientation::toRootFrame(const VirtualRobot::RobotPtr& referenceRobot) const
    {
        ARMARX_CHECK_NOT_NULL(referenceRobot);
        return toRootFrame(*referenceRobot);
    }
    FramedOrientationPtr FramedOrientation::toRootFrame(const VirtualRobot::Robot& referenceRobot) const
    {
        FramedOrientationPtr newOrientation = FramedOrientationPtr::dynamicCast(this->clone());
        newOrientation->changeFrame(referenceRobot, referenceRobot.getRootNode()->getName());
        return newOrientation;
    }
 
    Eigen::Matrix3f FramedOrientation::toRootEigen(const SharedRobotInterfacePrx& referenceRobot) const
    {
        VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
        return toRootEigen(sharedRobot);
    }
    Eigen::Matrix3f FramedOrientation::toRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const
    {
        ARMARX_CHECK_NOT_NULL(referenceRobot);
        return toRootEigen(*referenceRobot);
    }
    Eigen::Matrix3f FramedOrientation::toRootEigen(const VirtualRobot::Robot& referenceRobot) const
    {
        FramedOrientation newOrientation(toEigen(), frame, agent);
        newOrientation.changeFrame(referenceRobot, referenceRobot.getRootNode()->getName());
        return newOrientation.toEigen();
    }
 
    void FramedOrientation::serialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c) const
    {
        AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
 
        Quaternion::serialize(obj, c);
        obj->setString("frame", frame);
        obj->setString("agent", agent);
    }
 
    void FramedOrientation::deserialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c)
    {
        AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
 
        Quaternion::deserialize(obj);
        frame = obj->getString("frame");
 
        if (obj->hasElement("agent"))
        {
            agent = obj->getString("agent");
        }
    }
 
    Ice::ObjectPtr FramedOrientation::ice_clone() const
    {
        return this->clone();
    }
 
    VariantDataClassPtr FramedOrientation::clone(const Ice::Current& c) const
    {
        return new FramedOrientation(*this);
    }
 
    VariantTypeId FramedOrientation::getType(const Ice::Current& c) const
    {
        return VariantType::FramedOrientation;
    }
 
    bool FramedOrientation::validate(const Ice::Current& c)
    {
        return true;
    }
 
    std::ostream& operator<<(std::ostream& stream, const FramedOrientation& rhs)
    {
        stream << "FramedOrientation: " << std::endl << rhs.output() << std::endl;
        return stream;
    }
 
 
    VirtualRobot::LinkedCoordinate FramedPose::createLinkedCoordinate(const VirtualRobot::RobotPtr& virtualRobot, const FramedPositionPtr& position, const FramedOrientationPtr& orientation)
    {
        VirtualRobot::LinkedCoordinate c(virtualRobot);
        std::string frame;
 
        if (position)
        {
            frame = position->getFrame();
 
            if (orientation)
            {
                if (!frame.empty() && frame != orientation->getFrame())
                {
                    throw armarx::UserException("Error: frames mismatch");
                }
            }
        }
        else
        {
            if (!orientation)
            {
                armarx::UserException("createLinkedCoordinate: orientation and position are both NULL");
            }
            else
            {
                frame = orientation->getFrame();
            }
        }
 
        Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();
 
        if (orientation)
        {
            pose.block<3, 3>(0, 0) = orientation->toEigen();
        }
 
        if (position)
        {
            pose.block<3, 1>(0, 3) = position->toEigen();
        }
 
        c.set(frame, pose);
 
        return c;
    }
 
    FramedPosePtr FramedPose::toFrame(const SharedRobotInterfacePrx& referenceRobot, const std::string& newFrame) const
    {
        VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
        return toFrame(sharedRobot, newFrame);
    }
 
    FramedPosePtr FramedPose::toFrame(const VirtualRobot::RobotPtr& referenceRobot, const std::string& newFrame) const
    {
        FramedPosePtr newPose = FramedPosePtr::dynamicCast(this->clone());
        newPose->changeFrame(referenceRobot, newFrame);
        return newPose;
    }
 
    Eigen::Matrix4f
    FramedPose::toFrameEigen(const SharedRobotInterfacePrx& referenceRobot, const std::string& newFrame) const
    {
        VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
        return toFrameEigen(sharedRobot, newFrame);
    }
 
    Eigen::Matrix4f
    FramedPose::toFrameEigen(const VirtualRobot::RobotPtr& referenceRobot, const std::string& newFrame) const
    {
        return toFrame(referenceRobot, newFrame)->toEigen();
    }
 
    Ice::ObjectPtr FramedPose::ice_clone() const
    {
        return this->clone();
    }
 
    VariantDataClassPtr FramedPose::clone(const Ice::Current& c) const
    {
        return new FramedPose(*this);
    }
 
    VariantTypeId FramedPose::getType(const Ice::Current& c) const
    {
        return VariantType::FramedPose;
    }
 
    bool FramedPose::validate(const Ice::Current& c)
    {
        return true;
    }
 
    std::ostream& operator<<(std::ostream& stream, const FramedPose& rhs)
    {
        stream << "FramedPose: " << std::endl << rhs.output() << std::endl;
        return stream;
    }
 
    bool operator==(const FramedPose& pose1, const FramedPose& pose2)
    {
        if (pose1.frame != pose2.frame || pose1.agent != pose2.agent) return false;
        return (pose1.toEigen().isApprox(pose2.toEigen()));
    }
 
    bool operator!=(const FramedPose& pose1, const FramedPose& pose2)
    {
        return !(pose1 == pose2);
    }
 
 
}