diff --git a/VirtualRobot/Robot.cpp b/VirtualRobot/Robot.cpp
index 080bf27a5f5930f8a04bd75421f99627027f3fd9..297b3938778b3600e81c5ec2156f73ecd07229b6 100644
--- a/VirtualRobot/Robot.cpp
+++ b/VirtualRobot/Robot.cpp
@@ -647,7 +647,7 @@ namespace VirtualRobot
     {
         setGlobalPose(globalPose, true);
     }
-
+    
     VirtualRobot::CollisionCheckerPtr Robot::getCollisionChecker()
     {
         std::vector<RobotNodePtr> robotNodes = this->getRobotNodes();
@@ -826,16 +826,15 @@ namespace VirtualRobot
         return result;
     }
 
-    void Robot::setGlobalPoseForRobotNode(
-            const RobotNodePtr& node, const Eigen::Matrix4f& globalPoseNode)
+    Eigen::Matrix4f Robot::getGlobalPoseForRobotNode(
+            const RobotNodePtr& node, const Eigen::Matrix4f& globalPoseNode) const
     {
         THROW_VR_EXCEPTION_IF(!node, "NULL node");
-
+        
         if (math::Helpers::Orientation(globalPoseNode).isIdentity())
         {
             // set position to avoid accumulating rounding errors when using rotation
-            setGlobalPositionForRobotNode(node, math::Helpers::Position(globalPoseNode));
-            return;
+            return getGlobalPositionForRobotNode(node, math::Helpers::Position(globalPoseNode));
         }
         
         // get transformation from current to wanted tcp pose
@@ -843,27 +842,40 @@ namespace VirtualRobot
 
         // apply transformation to current global pose of robot
         tf = tf * getGlobalPose();
-        
+
         // re-orthogonolize to keep it a valid transformation matrix
         math::Helpers::Orientation(tf) = math::Helpers::Orthogonalize(math::Helpers::Orientation(tf));
         
-        // set t
-        setGlobalPose(tf);
+        // return tf
+        return tf;
     }
     
-    void Robot::setGlobalPositionForRobotNode(
-            const RobotNodePtr& node, const Eigen::Vector3f& globalPositionNode)
+    void Robot::setGlobalPoseForRobotNode(
+            const RobotNodePtr& node, const Eigen::Matrix4f& globalPoseNode)
     {
         THROW_VR_EXCEPTION_IF(!node, "NULL node");
         
+        setGlobalPose(getGlobalPoseForRobotNode(node, globalPoseNode));
+    }
+    
+    Eigen::Matrix4f Robot::getGlobalPositionForRobotNode(
+            const RobotNodePtr& node, const Eigen::Vector3f& globalPositionNode) const
+    {
         // get translation from current to wanted tcp pose
         const Eigen::Vector3f translation = globalPositionNode - node->getGlobalPosition();
 
         // apply translation to current global position of robot
         const Eigen::Vector3f robotPosition = getGlobalPosition() + translation;
         
-        setGlobalPose(math::Helpers::Pose(robotPosition));
+        return math::Helpers::Pose(robotPosition);
     }
+    
+    void Robot::setGlobalPositionForRobotNode(
+            const RobotNodePtr& node, const Eigen::Vector3f& globalPositionNode)
+    {
+        setGlobalPose(getGlobalPositionForRobotNode(node, globalPositionNode));
+    }
+    
 
     VirtualRobot::RobotPtr Robot::clone(const std::string& name, CollisionCheckerPtr collisionChecker, float scaling)
     {
diff --git a/VirtualRobot/Robot.h b/VirtualRobot/Robot.h
index 91a3a07a9840bf24d5b231c37994ca48e4d9b9fd..d20618969423acfc9abfa280daf7900ae364696a 100644
--- a/VirtualRobot/Robot.h
+++ b/VirtualRobot/Robot.h
@@ -197,38 +197,33 @@ namespace VirtualRobot
         */
         int getNumFaces(bool collisionModel = false) override;
 
-        /*!
-            Set the global pose of this robot
-        */
+        //! Set the global pose of this robot
         virtual void setGlobalPose(const Eigen::Matrix4f& globalPose, bool applyValues) = 0;
         void setGlobalPose(const Eigen::Matrix4f& globalPose) override;
 
-        /*!
-            Set the global pose of this robot so that the RobotNode node is at pose globalPoseNode.
-        */
+        //! Get the global pose of this robot so that the RobotNode node is at pose globalPoseNode.
+        virtual Eigen::Matrix4f getGlobalPoseForRobotNode(const RobotNodePtr& node, const Eigen::Matrix4f& globalPoseNode) const;
+        //! Set the global pose of this robot so that the RobotNode node is at pose globalPoseNode.
         virtual void setGlobalPoseForRobotNode(const RobotNodePtr& node, const Eigen::Matrix4f& globalPoseNode);
         
-        /*!
-            Set the global position of this robot so that the RobotNode node is at position globalPoseNode
-        */
+        //! Get the global position of this robot so that the RobotNode node is at position globalPoseNode
+        virtual Eigen::Matrix4f getGlobalPositionForRobotNode(const RobotNodePtr& node, const Eigen::Vector3f& globalPositionNode) const;
+        //! Set the global position of this robot so that the RobotNode node is at position globalPoseNode
         virtual void setGlobalPositionForRobotNode(const RobotNodePtr& node, const Eigen::Vector3f& globalPositionNode);
 
         //virtual Eigen::Matrix4f getGlobalPose() = 0;
 
-        /*!
-            Return center of mass of this robot in local coordinate frame. All RobotNodes of this robot are considered according to their mass.
-        */
+        
+        //! Return center of mass of this robot in local coordinate frame. All RobotNodes of this robot are considered according to their mass.
         Eigen::Vector3f getCoMLocal() override;
-        /*!
-            Return Center of Mass of this robot in global coordinates. All RobotNodes of this robot are considered according to their mass.
-        */
+
+        //! Return Center of Mass of this robot in global coordinates. All RobotNodes of this robot are considered according to their mass.
         Eigen::Vector3f getCoMGlobal() override;
 
-        /*!
-            Return accumulated mass of this robot.
-        */
+        //! Return accumulated mass of this robot.
         virtual float getMass();
 
+        
         /*!
             Extract a sub kinematic from this robot and create a new robot instance.
             \param startJoint The kinematic starts with this RobotNode
@@ -251,14 +246,10 @@ namespace VirtualRobot
         virtual RobotPtr clone(const std::string& name, CollisionCheckerPtr collisionChecker = CollisionCheckerPtr(), float scaling = 1.0f);
         virtual RobotPtr clone();
 
-        /*!
-            Just storing the filename.
-        */
+        //! Just storing the filename.
         virtual void setFilename(const std::string& filename);
 
-        /*!
-            Retrieve the stored filename.
-        */
+        //! Retrieve the stored filename.
         virtual std::string getFilename();
 
         /*!