enable framed pose conversion to root frame, which then is the VMP target pose

source/armarx/control/retrieve_hand/core/RetrieveHand.cpp

@@ -21,10 +21,12 @@
  */
 
 #include "RetrieveHand.h"
+
 #include <ArmarXCore/core/time/Metronome.h>
+
+#include <armarx/control/common/utils.h>
 #include <armarx/control/deprecated_njoint_mp_controller/task_space/ControllerInterface.h>
 #include <armarx/control/deprecated_njoint_mp_controller/task_space/NJointTSDMPController.h>
-#include <armarx/control/common/utils.h>
 
 namespace armarx::control::retrieve_hand::core
 {
@@ -97,15 +99,17 @@ namespace armarx::control::retrieve_hand::core
         robotReader.connect(remote.memoryNameSystem);
 
         auto now = armarx::core::time::DateTime::Now();
-        // auto r = robotReader.getSynchronizedRobot(properties.robotName, now);
-        // ARMARX_CHECK(r.get());
+        auto r = robotReader.getSynchronizedRobot(properties.robotName, now);
+        ARMARX_CHECK(r.get());
 
         std::vector<double> goals;
-        // Eigen::Matrix4f targetPoseInRoot = properties.targetPose.toRootFrame(r)->toEigen();
-        Eigen::Matrix4f targetPoseInRoot = properties.targetPose.toEigen();
+        Eigen::Matrix4f targetPoseInRoot = properties.targetPose.toRootFrame(r)->toEigen();
+        //        Eigen::Matrix4f targetPoseInRoot = properties.targetPose.toEigen();
         //        Eigen::Matrix4f currentPose = r->getRobotNodeSet(properties.robotNodeSet)->getTCP()->getPoseInRootFrame();
         goals = armarx::control::common::mat4ToDVec(targetPoseInRoot);
-        ARMARX_INFO << "Running VMP with goal \n" << VAROUT(targetPoseInRoot) << ", vec: \n" << armarx::control::common::dVecToString(goals);
+        ARMARX_INFO << "Running VMP with goal \n"
+                    << VAROUT(targetPoseInRoot) << ", vec: \n"
+                    << armarx::control::common::dVecToString(goals);
 
         // if (in.isViaPoseListSet() && in.isViaPoseCanValSet() && in.getViaPoseList().size() == in.getViaPoseCanVal().size())
         // {