Add RobotStateComponentPlugin::transformFromTo

source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.cpp

@@ -20,6 +20,7 @@
  *             GNU General Public License
  */
 #include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
 
 #include "RobotStateComponentPlugin.h"
 
@@ -272,6 +273,17 @@ namespace armarx::plugins
     {
         return _robotStateComponentName;
     }
+
+    Eigen::Matrix4f RobotStateComponentPlugin::transformFromTo(
+        const std::string& from,
+        const std::string& to,
+        const VirtualRobot::RobotPtr& rob)
+    {
+        ARMARX_CHECK_NOT_NULL(rob);
+        armarx::FramedPose fp{Eigen::Matrix4f::Identity(), from, rob.full->getName()};
+        fp.changeFrame(rob, to);
+        return fp.toEigen();
+    }
 }
 
 namespace armarx

source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h

@@ -88,9 +88,14 @@ namespace armarx::plugins
         RobotData getRobotData(const std::string& id) const;
         void setRobotRNSAndNode(const std::string& id, const std::string& rnsName, const std::string& nodeName);
 
+        //querry
+    public:
         const RobotStateComponentInterfacePrx& getRobotStateComponent() const;
-
         const RobotNameHelper& getRobotNameHelper() const;
+        Eigen::Matrix4f transformFromTo(const std::string& from,
+                                        const std::string& to,
+                                        const VirtualRobot::RobotPtr& rob);
+
         //sync
     public:
         bool synchronizeLocalClone(const VirtualRobot::RobotPtr& robot) const;