use framed pose for retrieve hand target pose

source/armarx/control/retrieve_hand/aron/RetrieveHandParams.xml

 <?xml version="1.0" encoding="UTF-8" ?>
 <AronTypeDefinition>
-<!--    <AronIncludes>
-        <Include include="<armarx/control/common/mp/aron/MPConfig.xml>" autoinclude="true" />
-    </AronIncludes>-->
+    <AronIncludes>
+        <Include include="RobotAPI/libraries/aron/common/aron/framed.xml" />
+    </AronIncludes>
     <GenerateTypes>
         <Object name="::armarx::control::retrieve_hand::skills::RetrieveHandParams">
 
             <ObjectChild key="robotName">
-                <String />
+                <String default="Armar7"/>
             </ObjectChild>
 
             <ObjectChild key="targetPose">
-                <Matrix4f />
+                <armarx::arondto::FramedPositionAndOrientation />
             </ObjectChild>
 
             <ObjectChild key="robotNodeSet">
-                <String />
+                <String default="LeftArm"/>
             </ObjectChild>
 
             <ObjectChild key="timeDurationInSec">
-                <Double />
+                <Double default="1.0" />
             </ObjectChild>
 
             <ObjectChild key="kpLinear">
-                <Float />
+                <Float default="10.0" />
             </ObjectChild>
 
             <ObjectChild key="kpAngular">
-                <Float />
+                <Float default="8.0" />
             </ObjectChild>
 
             <ObjectChild key="kdLinear">
-                <Float />
+                <Float default="1.0" />
             </ObjectChild>
 
             <ObjectChild key="kdAngular">
-                <Float />
+                <Float default="1.0" />
             </ObjectChild>
 
             <ObjectChild key="fileNames">

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

@@ -97,11 +97,15 @@ 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());
 
         std::vector<double> goals;
+        // 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(properties.targetPose);
-        ARMARX_INFO << "Running VMP with goal " << VAROUT(properties.targetPose) << ", vec: " << armarx::control::common::dVecToString(goals);
+        goals = armarx::control::common::mat4ToDVec(targetPoseInRoot);
+        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())
         // {

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

@@ -58,7 +58,8 @@ namespace armarx::control::retrieve_hand::core
             std::string robotName;
             std::string robotNodeSet;
             double timeDurationInSec;
-            Eigen::Matrix4f targetPose;
+            // Eigen::Matrix4f targetPoseInRoot;
+            armarx::FramedPose targetPose;
             float kpLinear = 10.0f;
             float kpAngular = 10.0f;
             float kdLinear = 1.0f;

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

 #include "RetrieveHand.h"
 
+#include <ArmarXCore/core/logging/Logging.h>
+
 #include <armarx/control/retrieve_hand/constants/constants.h>
 #include <armarx/control/deprecated_njoint_mp_controller/task_space/ControllerInterface.h>
 
@@ -10,6 +12,14 @@ namespace armarx::control::retrieve_hand::skills
     RetrieveHand::GetSkillDescription()
     {
         ParamType defaultParams;
+        defaultParams.targetPose.header.agent = "Armar7";
+        defaultParams.targetPose.position = Eigen::Matrix<float, 3, 1> {-203.5f, 269.00f, 1130.0f};
+        defaultParams.targetPose.orientation = Eigen::Quaternion<float> {
+            0.741636, -0.6635227, 0.05756, -0.079999};
+        // ARMARX_INFO << VAROUT(defaultParams.targetPose);
+        // ARMARX_INFO << VAROUT(defaultParams.targetPose.position) << VAROUT(defaultParams.targetPose.orientation);
+        // ARMARX_INFO << VAROUT(defaultParams.targetPose.header.frame) << VAROUT(defaultParams.targetPose.header.agent);
+
         // defaultParams.exampleStringParam = "exampleDefaultValue";
 
         auto skillId = armarx::skills::SkillID{.skillName = armarx::control::retrieve_hand::constants::SKILL_NAME};
@@ -51,11 +61,17 @@ namespace armarx::control::retrieve_hand::skills
                 .memoryNameSystem = remote.memoryNameSystem,
                 .robotUnit = remote.robotUnit};
 
+        armarx::FramedPose targetPose {
+                in.parameters.targetPose.position,
+                in.parameters.targetPose.orientation,
+                in.parameters.targetPose.header.frame,
+                in.parameters.targetPose.header.agent
+        };
         core::RetrieveHand::Properties props{
                 .robotName = in.parameters.robotName,
                 .robotNodeSet = in.parameters.robotNodeSet,
                 .timeDurationInSec = in.parameters.timeDurationInSec,
-                .targetPose = in.parameters.targetPose,
+                .targetPose = targetPose,
                 .kpLinear = in.parameters.kpLinear,
                 .kpAngular = in.parameters.kpAngular,
                 .kdLinear = in.parameters.kdLinear,