Pose convenience functions

source/RobotAPI/libraries/core/FramedPose.cpp

@@ -132,9 +132,21 @@ namespace armarx
     }
     FramedDirectionPtr FramedDirection::toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const
     {
-        FramedDirectionPtr newPos = FramedDirectionPtr::dynamicCast(this->clone());
-        newPos->changeToGlobal(referenceRobot);
-        return newPos;
+        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
+    {
+        FramedDirection newDirection(toEigen(), frame, agent);
+        newDirection.changeToGlobal(referenceRobot);
+        return newDirection.toEigen();
     }
 
     FramedDirectionPtr FramedDirection::toRootFrame(const SharedRobotInterfacePrx& referenceRobot) const
@@ -144,9 +156,9 @@ namespace armarx
     }
     FramedDirectionPtr FramedDirection::toRootFrame(const VirtualRobot::RobotPtr& referenceRobot) const
     {
-        FramedDirectionPtr newDir = FramedDirectionPtr::dynamicCast(this->clone());
-        newDir->changeFrame(referenceRobot, referenceRobot->getRootNode()->getName());
-        return newDir;
+        FramedDirectionPtr newDirection = FramedDirectionPtr::dynamicCast(this->clone());
+        newDirection->changeFrame(referenceRobot, referenceRobot->getRootNode()->getName());
+        return newDirection;
     }
 
     Eigen::Vector3f FramedDirection::toRootEigen(const SharedRobotInterfacePrx& referenceRobot) const
@@ -156,9 +168,9 @@ namespace armarx
     }
     Eigen::Vector3f FramedDirection::toRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const
     {
-        FramedDirection newDir(toEigen(), frame, agent);
-        newDir.changeFrame(referenceRobot, referenceRobot->getRootNode()->getName());
-        return newDir.toEigen();
+        FramedDirection newDirection(toEigen(), frame, agent);
+        newDirection.changeFrame(referenceRobot, referenceRobot->getRootNode()->getName());
+        return newDirection.toEigen();
     }
 
     string FramedDirection::output(const Ice::Current& c) const
@@ -351,6 +363,18 @@ namespace armarx
         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
+    {
+        FramedPose newPose(toEigen(), frame, agent);
+        newPose.changeToGlobal(referenceRobot);
+        return newPose.toEigen();
+    }
+
     FramedPosePtr FramedPose::toRootFrame(const SharedRobotInterfacePrx& referenceRobot) const
     {
         VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
@@ -531,9 +555,21 @@ namespace armarx
     }
     FramedPositionPtr FramedPosition::toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const
     {
-        FramedPositionPtr newPos = FramedPositionPtr::dynamicCast(this->clone());
-        newPos->changeToGlobal(referenceRobot);
-        return newPos;
+        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
+    {
+        FramedPosition newPosition(toEigen(), frame, agent);
+        newPosition.changeToGlobal(referenceRobot);
+        return newPosition.toEigen();
     }
 
     FramedPositionPtr FramedPosition::toRootFrame(const SharedRobotInterfacePrx& referenceRobot) const
@@ -716,9 +752,21 @@ namespace armarx
     }
     FramedOrientationPtr FramedOrientation::toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const
     {
-        FramedOrientationPtr newPos = FramedOrientationPtr::dynamicCast(this->clone());
-        newPos->changeToGlobal(referenceRobot);
-        return newPos;
+        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
+    {
+        FramedOrientation newOrientation(toEigen(), frame, agent);
+        newOrientation.changeToGlobal(referenceRobot);
+        return newOrientation.toEigen();
     }
 
     FramedOrientationPtr FramedOrientation::toRootFrame(const SharedRobotInterfacePrx& referenceRobot) const

source/RobotAPI/libraries/core/FramedPose.h

@@ -94,6 +94,8 @@ namespace armarx
         void changeToGlobal(const VirtualRobot::RobotPtr& referenceRobot);
         FramedDirectionPtr toGlobal(const SharedRobotInterfacePrx& referenceRobot) const;
         FramedDirectionPtr toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const;
+        Eigen::Vector3f toGlobalEigen(const SharedRobotInterfacePrx& referenceRobot) const;
+        Eigen::Vector3f toGlobalEigen(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;
@@ -158,6 +160,8 @@ namespace armarx
         void changeToGlobal(const VirtualRobot::RobotPtr& referenceRobot);
         FramedPositionPtr toGlobal(const SharedRobotInterfacePrx& referenceRobot) const;
         FramedPositionPtr toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const;
+        Eigen::Vector3f toGlobalEigen(const SharedRobotInterfacePrx& referenceRobot) const;
+        Eigen::Vector3f toGlobalEigen(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;
@@ -239,6 +243,8 @@ namespace armarx
         void changeToGlobal(const VirtualRobot::RobotPtr& referenceRobot);
         FramedOrientationPtr toGlobal(const SharedRobotInterfacePrx& referenceRobot) const;
         FramedOrientationPtr toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const;
+        Eigen::Matrix3f toGlobalEigen(const SharedRobotInterfacePrx& referenceRobot) const;
+        Eigen::Matrix3f toGlobalEigen(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;
@@ -308,6 +314,8 @@ namespace armarx
         void changeToGlobal(const VirtualRobot::RobotPtr& referenceRobot);
         FramedPosePtr toGlobal(const SharedRobotInterfacePrx& referenceRobot) const;
         FramedPosePtr toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const;
+        Eigen::Matrix4f toGlobalEigen(const SharedRobotInterfacePrx& referenceRobot) const;
+        Eigen::Matrix4f toGlobalEigen(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;