diff --git a/source/RobotAPI/libraries/core/FramedPose.cpp b/source/RobotAPI/libraries/core/FramedPose.cpp
index 4b22c5cff5fd77119a7ee3f1841ea811a9488730..a3a92fc860965272aee0f2e1723868aab86848e3 100644
--- a/source/RobotAPI/libraries/core/FramedPose.cpp
+++ b/source/RobotAPI/libraries/core/FramedPose.cpp
@@ -34,10 +34,11 @@ namespace armarx
 
     }
 
-    FramedDirection::FramedDirection(Ice::Float x, ::Ice::Float y, ::Ice::Float z, const std::string& frame) :
+    FramedDirection::FramedDirection(Ice::Float x, ::Ice::Float y, ::Ice::Float z, const std::string& frame, const string& agent) :
         Vector3(x, y, z)
     {
         this->frame = frame;
+        this->agent = agent;
     }
 
     string FramedDirection::getFrame() const
@@ -45,7 +46,7 @@ namespace armarx
         return frame;
     }
 
-    FramedDirectionPtr FramedDirection::ChangeFrame(const VirtualRobot::RobotPtr robot, const FramedDirection& framedVec, const string& newFrame)
+    FramedDirectionPtr FramedDirection::ChangeFrame(const VirtualRobot::RobotPtr& robot, const FramedDirection& framedVec, const string& newFrame)
     {
         if (framedVec.getFrame() == newFrame)
         {
@@ -71,7 +72,7 @@ namespace armarx
         return result;
     }
 
-    void FramedDirection::changeFrame(const VirtualRobot::RobotPtr robot, const string& newFrame)
+    void FramedDirection::changeFrame(const VirtualRobot::RobotPtr& robot, const string& newFrame)
     {
         if (getFrame() == newFrame)
         {
@@ -129,7 +130,6 @@ namespace armarx
         VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
         return toGlobal(sharedRobot);
     }
-
     FramedDirectionPtr FramedDirection::toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const
     {
         FramedDirectionPtr newPos = FramedDirectionPtr::dynamicCast(this->clone());
@@ -137,6 +137,30 @@ namespace armarx
         return newPos;
     }
 
+    FramedDirectionPtr FramedDirection::toRootFrame(const SharedRobotInterfacePrx& referenceRobot) const
+    {
+        VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
+        return toRootFrame(sharedRobot);
+    }
+    FramedDirectionPtr FramedDirection::toRootFrame(const VirtualRobot::RobotPtr& referenceRobot) const
+    {
+        FramedDirectionPtr newDir = FramedDirectionPtr::dynamicCast(this->clone());
+        newDir->changeFrame(referenceRobot, referenceRobot->getRootNode()->getName());
+        return newDir;
+    }
+
+    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
+    {
+        FramedDirection newDir(toEigen(), frame, agent);
+        newDir.changeFrame(referenceRobot, referenceRobot->getRootNode()->getName());
+        return newDir.toEigen();
+    }
+
     string FramedDirection::output(const Ice::Current& c) const
     {
         std::stringstream s;
@@ -320,7 +344,6 @@ namespace armarx
         VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
         return toGlobal(sharedRobot);
     }
-
     FramedPosePtr FramedPose::toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const
     {
         FramedPosePtr newPose = FramedPosePtr::dynamicCast(this->clone());
@@ -328,6 +351,30 @@ namespace armarx
         return newPose;
     }
 
+    FramedPosePtr FramedPose::toRootFrame(const SharedRobotInterfacePrx& referenceRobot) const
+    {
+        VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
+        return toRootFrame(sharedRobot);
+    }
+    FramedPosePtr FramedPose::toRootFrame(const VirtualRobot::RobotPtr& 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
+    {
+        FramedPose newPose(toEigen(), frame, agent);
+        newPose.changeFrame(referenceRobot, referenceRobot->getRootNode()->getName());
+        return newPose.toEigen();
+    }
+
 
     FramedPositionPtr FramedPose::getPosition() const
     {
@@ -482,7 +529,6 @@ namespace armarx
         VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
         return toGlobal(sharedRobot);
     }
-
     FramedPositionPtr FramedPosition::toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const
     {
         FramedPositionPtr newPos = FramedPositionPtr::dynamicCast(this->clone());
@@ -490,6 +536,30 @@ namespace armarx
         return newPos;
     }
 
+    FramedPositionPtr FramedPosition::toRootFrame(const SharedRobotInterfacePrx& referenceRobot) const
+    {
+        VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
+        return toRootFrame(sharedRobot);
+    }
+    FramedPositionPtr FramedPosition::toRootFrame(const VirtualRobot::RobotPtr& 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
+    {
+        FramedPosition newPosition(toEigen(), frame, agent);
+        newPosition.changeFrame(referenceRobot, referenceRobot->getRootNode()->getName());
+        return newPosition.toEigen();
+    }
+
     string FramedPosition::output(const Ice::Current& c) const
     {
         std::stringstream s;
@@ -644,7 +714,6 @@ namespace armarx
         VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
         return toGlobal(sharedRobot);
     }
-
     FramedOrientationPtr FramedOrientation::toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const
     {
         FramedOrientationPtr newPos = FramedOrientationPtr::dynamicCast(this->clone());
@@ -652,6 +721,30 @@ namespace armarx
         return newPos;
     }
 
+    FramedOrientationPtr FramedOrientation::toRootFrame(const SharedRobotInterfacePrx& referenceRobot) const
+    {
+        VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
+        return toRootFrame(sharedRobot);
+    }
+    FramedOrientationPtr FramedOrientation::toRootFrame(const VirtualRobot::RobotPtr& 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
+    {
+        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);
diff --git a/source/RobotAPI/libraries/core/FramedPose.h b/source/RobotAPI/libraries/core/FramedPose.h
index fb60bb5e3c3eceb404bde7d9b8b94b23851f386f..a1800b4951588ee9d5fd7ce62ecaa6c042f3e91f 100644
--- a/source/RobotAPI/libraries/core/FramedPose.h
+++ b/source/RobotAPI/libraries/core/FramedPose.h
@@ -84,16 +84,20 @@ namespace armarx
         FramedDirection();
         FramedDirection(const FramedDirection& source);
         FramedDirection(const Eigen::Vector3f& vec, const std::string& frame, const std::string& agent);
-        FramedDirection(Ice::Float x, ::Ice::Float y, ::Ice::Float z, const std::string& frame);
+        FramedDirection(Ice::Float x, ::Ice::Float y, ::Ice::Float z, const std::string& frame, const std::string& agent);
 
         std::string getFrame() const;
-        static FramedDirectionPtr ChangeFrame(const VirtualRobot::RobotPtr robot, const FramedDirection& framedVec, const std::string& newFrame);
-        void changeFrame(const VirtualRobot::RobotPtr robot, const std::string& newFrame);
+        static FramedDirectionPtr ChangeFrame(const VirtualRobot::RobotPtr& robot, const FramedDirection& framedVec, const std::string& newFrame);
+        void changeFrame(const VirtualRobot::RobotPtr& robot, const std::string& newFrame);
 
         void changeToGlobal(const SharedRobotInterfacePrx& referenceRobot);
         void changeToGlobal(const VirtualRobot::RobotPtr& referenceRobot);
         FramedDirectionPtr toGlobal(const SharedRobotInterfacePrx& referenceRobot) const;
         FramedDirectionPtr toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const;
+        FramedDirectionPtr toRootFrame(const SharedRobotInterfacePrx& referenceRobot) const;
+        FramedDirectionPtr toRootFrame(const VirtualRobot::RobotPtr& referenceRobot) const;
+        Eigen::Vector3f toRootEigen(const SharedRobotInterfacePrx& referenceRobot) const;
+        Eigen::Vector3f toRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const;
 
         // inherited from VariantDataClass
         Ice::ObjectPtr ice_clone() const
@@ -123,8 +127,8 @@ namespace armarx
     public: // serialization
         virtual void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current()) const;
         virtual void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current());
-    private:
 
+    private:
         static Eigen::Matrix4f __GetRotationBetweenFrames(const std::string& oldFrame, const std::string& newFrame, VirtualRobot::RobotPtr robotState);
     };
 
@@ -154,7 +158,10 @@ namespace armarx
         void changeToGlobal(const VirtualRobot::RobotPtr& referenceRobot);
         FramedPositionPtr toGlobal(const SharedRobotInterfacePrx& referenceRobot) const;
         FramedPositionPtr toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const;
-
+        FramedPositionPtr toRootFrame(const SharedRobotInterfacePrx& referenceRobot) const;
+        FramedPositionPtr toRootFrame(const VirtualRobot::RobotPtr& referenceRobot) const;
+        Eigen::Vector3f toRootEigen(const SharedRobotInterfacePrx& referenceRobot) const;
+        Eigen::Vector3f toRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const;
 
         // inherited from VariantDataClass
         Ice::ObjectPtr ice_clone() const
@@ -232,6 +239,10 @@ namespace armarx
         void changeToGlobal(const VirtualRobot::RobotPtr& referenceRobot);
         FramedOrientationPtr toGlobal(const SharedRobotInterfacePrx& referenceRobot) const;
         FramedOrientationPtr toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const;
+        FramedOrientationPtr toRootFrame(const SharedRobotInterfacePrx& referenceRobot) const;
+        FramedOrientationPtr toRootFrame(const VirtualRobot::RobotPtr& referenceRobot) const;
+        Eigen::Matrix3f toRootEigen(const SharedRobotInterfacePrx& referenceRobot) const;
+        Eigen::Matrix3f toRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const;
 
         friend std::ostream& operator<<(std::ostream& stream, const FramedOrientation& rhs)
         {
@@ -297,6 +308,10 @@ namespace armarx
         void changeToGlobal(const VirtualRobot::RobotPtr& referenceRobot);
         FramedPosePtr toGlobal(const SharedRobotInterfacePrx& referenceRobot) const;
         FramedPosePtr toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const;
+        FramedPosePtr toRootFrame(const SharedRobotInterfacePrx& referenceRobot) const;
+        FramedPosePtr toRootFrame(const VirtualRobot::RobotPtr& referenceRobot) const;
+        Eigen::Matrix4f toRootEigen(const SharedRobotInterfacePrx& referenceRobot) const;
+        Eigen::Matrix4f toRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const;
 
         friend std::ostream& operator<<(std::ostream& stream, const FramedPose& rhs)
         {