add debug info in retrieve hand skill

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

@@ -40,6 +40,7 @@ namespace armarx::control::retrieve_hand::core
     bool
     RetrieveHand::_runTSVMPController()
     {
+        ARMARX_IMPORTANT << "Creating TSVMP Controller for Retrieve hand skill";
         double phaseL = 100;
         double phaseK = 1000;
         double phaseDist0 = 50;
@@ -96,13 +97,11 @@ 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 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);
 
         // if (in.isViaPoseListSet() && in.isViaPoseCanValSet() && in.getViaPoseList().size() == in.getViaPoseCanVal().size())
         // {
@@ -111,18 +110,21 @@ namespace armarx::control::retrieve_hand::core
         //         dmpController->setViaPoints(in.getViaPoseCanVal().at(i), Helpers::pose2dvec(in.getViaPoseList().at(i)->toEigen()));
         //     }
         // }
-        //
+
         dmpController->activateController();
 
         dmpController->runDMP(goals, 1.0);
 
         const auto metronomeTargetPeriod = armarx::Duration::MilliSeconds(10);
         armarx::Metronome loopScheduler(metronomeTargetPeriod);
+        ARMARX_INFO << "VMP controller started";
         while (running.load() && not dmpController->isFinished())
         {
+            // ARMARX_INFO << VAROUT(dmpController->getCanVal());
             loopScheduler.waitForNextTick();
         }
 
+        ARMARX_DEBUG << "VMP Controller finished, try to delete controller";
         dmpController->deactivateController();
         while (dmpController->isControllerActive())
         {
@@ -131,6 +133,7 @@ namespace armarx::control::retrieve_hand::core
         dmpController->deleteController();
 
         running.store(false);
+        ARMARX_INFO << "Retrieve hand skill finished";
         return true;
     }