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 <Eigen/Core>
#include <Eigen/Geometry>
 
#include <VirtualRobot/LinkedCoordinate.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotConfig.h>
#include <VirtualRobot/VirtualRobot.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/observers/AbstractObjectSerializer.h>
 
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
 
 
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::RobotConstPtr& 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::RobotConstPtr& 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::RobotConstPtr sharedRobot(new RemoteRobot(referenceRobot));
        changeToGlobal(sharedRobot);
    }
 
    void
    FramedDirection::changeToGlobal(const VirtualRobot::RobotConstPtr& 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::RobotConstPtr sharedRobot(new RemoteRobot(referenceRobot));
        return toGlobal(sharedRobot);
    }
 
    FramedDirectionPtr
    FramedDirection::toGlobal(const VirtualRobot::RobotConstPtr& 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::RobotConstPtr sharedRobot(new RemoteRobot(referenceRobot));
        return toGlobalEigen(sharedRobot);
    }
 
    Eigen::Vector3f
    FramedDirection::toGlobalEigen(const VirtualRobot::RobotConstPtr& 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::RobotConstPtr sharedRobot(new RemoteRobot(referenceRobot));
        return toRootFrame(sharedRobot);
    }
 
    FramedDirectionPtr
    FramedDirection::toRootFrame(const VirtualRobot::RobotConstPtr& 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::RobotConstPtr sharedRobot(new RemoteRobot(referenceRobot));
        return toRootEigen(sharedRobot);
    }
 
    Eigen::Vector3f
    FramedDirection::toRootEigen(const VirtualRobot::RobotConstPtr& 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;
    }
 
    FramedPose::FramedPose(const Eigen::Vector3f& pos,
                           const Eigen::Quaternionf& 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::RobotConstPtr sharedRobot(new RemoteRobot(referenceRobot));
        changeFrame(sharedRobot, newFrame);
    }
 
    void
    FramedPose::changeFrame(const VirtualRobot::RobotConstPtr& 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::Isometry3f oldFrameTransformation = Eigen::Isometry3f::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 =
                Eigen::Isometry3f{referenceRobot.getRootNode()->getGlobalPose()}.inverse();
        }
 
        Eigen::Isometry3f newPose =
            (Eigen::Isometry3f{referenceRobot.getRobotNode(newFrame)->getPoseInRootFrame()}
                 .inverse() *
             oldFrameTransformation) *
            Eigen::Isometry3f{toEigen()};
 
        orientation = new Quaternion(Eigen::Matrix3f{newPose.linear()});
        Eigen::Vector3f pos = newPose.translation();
        position = new Vector3(pos);
        frame = newFrame;
        agent = referenceRobot.getName();
        init();
    }
 
    void
    FramedPose::changeToGlobal(const SharedRobotInterfacePrx& referenceRobot)
    {
        VirtualRobot::RobotConstPtr sharedRobot(new RemoteRobot(referenceRobot));
        changeToGlobal(sharedRobot);
    }
 
    void
    FramedPose::changeToGlobal(const VirtualRobot::RobotConstPtr& 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::RobotConstPtr sharedRobot(new RemoteRobot(referenceRobot));
        return toGlobal(sharedRobot);
    }
 
    FramedPosePtr
    FramedPose::toGlobal(const VirtualRobot::RobotConstPtr& 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::RobotConstPtr sharedRobot(new RemoteRobot(referenceRobot));
        return toGlobalEigen(sharedRobot);
    }
 
    Eigen::Matrix4f
    FramedPose::toGlobalEigen(const VirtualRobot::RobotConstPtr& 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::RobotConstPtr sharedRobot(new RemoteRobot(referenceRobot));
        return toRootFrame(sharedRobot);
    }
 
    FramedPosePtr
    FramedPose::toRootFrame(const VirtualRobot::RobotConstPtr& 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::RobotConstPtr sharedRobot(new RemoteRobot(referenceRobot));
        return toRootEigen(sharedRobot);
    }
 
    Eigen::Matrix4f
    FramedPose::toRootEigen(const VirtualRobot::RobotConstPtr& 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::RobotConstPtr sharedRobot(new RemoteRobot(referenceRobot));
        changeFrame(sharedRobot, newFrame);
    }
 
    void
    FramedPosition::changeFrame(const VirtualRobot::RobotConstPtr& 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::RobotConstPtr sharedRobot(new RemoteRobot(referenceRobot));
        changeToGlobal(sharedRobot);
    }
 
    void
    FramedPosition::changeToGlobal(const VirtualRobot::RobotConstPtr& 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::RobotConstPtr sharedRobot(new RemoteRobot(referenceRobot));
        return toGlobal(sharedRobot);
    }
 
    FramedPositionPtr
    FramedPosition::toGlobal(const VirtualRobot::RobotConstPtr& 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::RobotConstPtr sharedRobot(new RemoteRobot(referenceRobot));
        return toGlobalEigen(sharedRobot);
    }
 
    Eigen::Vector3f
    FramedPosition::toGlobalEigen(const VirtualRobot::RobotConstPtr& 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::RobotConstPtr sharedRobot(new RemoteRobot(referenceRobot));
        return toRootFrame(sharedRobot);
    }
 
    FramedPositionPtr
    FramedPosition::toRootFrame(const VirtualRobot::RobotConstPtr& 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::RobotConstPtr sharedRobot(new RemoteRobot(referenceRobot));
        return toRootEigen(sharedRobot);
    }
 
    Eigen::Vector3f
    FramedPosition::toRootEigen(const VirtualRobot::RobotConstPtr& 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::RobotConstPtr sharedRobot(new RemoteRobot(referenceRobot));
 
        changeFrame(sharedRobot, newFrame);
    }
 
    void
    FramedOrientation::changeFrame(const VirtualRobot::RobotConstPtr& 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::RobotConstPtr sharedRobot(new RemoteRobot(referenceRobot));
        changeToGlobal(sharedRobot);
    }
 
    void
    FramedOrientation::changeToGlobal(const VirtualRobot::RobotConstPtr& 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::RobotConstPtr sharedRobot(new RemoteRobot(referenceRobot));
        return toGlobal(sharedRobot);
    }
 
    FramedOrientationPtr
    FramedOrientation::toGlobal(const VirtualRobot::RobotConstPtr& 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::RobotConstPtr sharedRobot(new RemoteRobot(referenceRobot));
        return toGlobalEigen(sharedRobot);
    }
 
    Eigen::Matrix3f
    FramedOrientation::toGlobalEigen(const VirtualRobot::RobotConstPtr& 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::RobotConstPtr sharedRobot(new RemoteRobot(referenceRobot));
        return toRootFrame(sharedRobot);
    }
 
    FramedOrientationPtr
    FramedOrientation::toRootFrame(const VirtualRobot::RobotConstPtr& 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::RobotConstPtr sharedRobot(new RemoteRobot(referenceRobot));
        return toRootEigen(sharedRobot);
    }
 
    Eigen::Matrix3f
    FramedOrientation::toRootEigen(const VirtualRobot::RobotConstPtr& 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::RobotConstPtr sharedRobot(new RemoteRobot(referenceRobot));
        return toFrame(sharedRobot, newFrame);
    }
 
    FramedPosePtr
    FramedPose::toFrame(const VirtualRobot::RobotConstPtr& 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::RobotConstPtr sharedRobot(new RemoteRobot(referenceRobot));
        return toFrameEigen(sharedRobot, newFrame);
    }
 
    Eigen::Matrix4f
    FramedPose::toFrameEigen(const VirtualRobot::RobotConstPtr& 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);
    }
 
 
} // namespace armarx