Merge branch 'master' of https://gitlab.com/ArmarX/RobotAPI

source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.cpp

@@ -262,6 +262,13 @@ namespace armarx
         debugOutputData.getWriteBuffer().dmpTargets["dmp_pitch"] = targetState[4];
         debugOutputData.getWriteBuffer().dmpTargets["dmp_yaw"] = targetState[5];
 
+        debugOutputData.getWriteBuffer().realTCP["real_x"] = currentPosition[0];
+        debugOutputData.getWriteBuffer().realTCP["real_y"] = currentPosition[1];
+        debugOutputData.getWriteBuffer().realTCP["real_z"] = currentPosition[2];
+        debugOutputData.getWriteBuffer().realTCP["real_roll"] = currentRPY[0];
+        debugOutputData.getWriteBuffer().realTCP["real_pitch"] = currentRPY[1];
+        debugOutputData.getWriteBuffer().realTCP["real_yaw"] = currentRPY[2];
+
         debugOutputData.getWriteBuffer().mpcFactor = mpcFactor;
         debugOutputData.getWriteBuffer().error = error;
         debugOutputData.getWriteBuffer().phaseStop = phaseStop;
@@ -548,6 +555,13 @@ namespace armarx
             datafields[pair.first] = new Variant(pair.second);
         }
 
+        auto realTCP = debugOutputData.getUpToDateReadBuffer().realTCP;
+        for (auto& pair : realTCP)
+        {
+            datafields[pair.first] = new Variant(pair.second);
+        }
+
+
         datafields["mpcFactor"] = new Variant(debugOutputData.getUpToDateReadBuffer().mpcFactor);
         datafields["poseError"] = new Variant(debugOutputData.getUpToDateReadBuffer().error);
         datafields["phaseStop"] = new Variant(debugOutputData.getUpToDateReadBuffer().phaseStop);

source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.h

@@ -93,6 +93,8 @@ namespace armarx
         {
             StringFloatDictionary latestTargetVelocities;
             StringFloatDictionary dmpTargets;
+            StringFloatDictionary realTCP;
+
             double mpcFactor;
             double error;
             double phaseStop;

source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp

@@ -105,6 +105,9 @@ namespace armarx
         double posError = 0;
         double oriError = 0;
         double deltaT = 0;
+        std::vector<double> currentposi;
+        std::vector<double> currentori;
+
         if (canVal > 1e-8)
         {
             if (!controllerSensorData.updateReadBuffer())
@@ -115,7 +118,6 @@ namespace armarx
             currentState = controllerSensorData.getReadBuffer().currentState;
 
 
-            std::vector<double> currentposi;
             currentposi.resize(3);
             for (size_t i = 0; i < 3; ++i)
             {
@@ -123,7 +125,6 @@ namespace armarx
                 posError += pow(currentState[i].pos - targetState[i], 2);
             }
 
-            std::vector<double> currentori;
             currentori.resize(3);
             for (size_t i = 0; i < 3; ++i)
             {
@@ -204,6 +205,14 @@ namespace armarx
         debugOutputData.getWriteBuffer().dmpTargets["dmp_pitch"] = targetState[4];
         debugOutputData.getWriteBuffer().dmpTargets["dmp_yaw"] = targetState[5];
 
+        debugOutputData.getWriteBuffer().realTCP["real_x"] = currentposi[0];
+        debugOutputData.getWriteBuffer().realTCP["real_y"] = currentposi[1];
+        debugOutputData.getWriteBuffer().realTCP["real_z"] = currentposi[2];
+        debugOutputData.getWriteBuffer().realTCP["real_roll"] = currentori[0];
+        debugOutputData.getWriteBuffer().realTCP["real_pitch"] = currentori[1];
+        debugOutputData.getWriteBuffer().realTCP["real_yaw"] = currentori[2];
+
+
         debugOutputData.getWriteBuffer().currentCanVal = canVal;
         debugOutputData.getWriteBuffer().mpcFactor = mpcFactor;
         debugOutputData.getWriteBuffer().error = error;
@@ -483,6 +492,12 @@ namespace armarx
             datafields[pair.first] = new Variant(pair.second);
         }
 
+        auto realTCP = debugOutputData.getUpToDateReadBuffer().realTCP;
+        for (auto& pair : realTCP)
+        {
+            datafields[pair.first] = new Variant(pair.second);
+        }
+
         datafields["canVal"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentCanVal);
         datafields["mpcFactor"] = new Variant(debugOutputData.getUpToDateReadBuffer().mpcFactor);
         datafields["poseError"] = new Variant(debugOutputData.getUpToDateReadBuffer().error);
@@ -497,7 +512,7 @@ namespace armarx
     void NJointTSDMPController::onInitComponent()
     {
         ARMARX_INFO << "init ...";
-        controllerTask = new PeriodicTask<NJointTSDMPController>(this, &NJointTSDMPController::controllerRun, 1);
+        controllerTask = new PeriodicTask<NJointTSDMPController>(this, &NJointTSDMPController::controllerRun, 0.3);
     }
 
     void NJointTSDMPController::onDisconnectComponent()

source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h

@@ -93,6 +93,7 @@ namespace armarx
         {
             StringFloatDictionary latestTargetVelocities;
             StringFloatDictionary dmpTargets;
+            StringFloatDictionary realTCP;
             double currentCanVal;
             double mpcFactor;
             double error;