controller init

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

@@ -162,6 +162,10 @@ namespace armarx
         getWriterControlStruct().targetPose = targetPose;
 
         writeControlStruct();
+
+        NJointTSDMPControllerInterfaceData initInterfaceData;
+        initInterfaceData.currentTcpPose = Eigen::Matrix4f::Identity();
+        interfaceData.reinitAllBuffers(initInterfaceData);
     }
 
 
@@ -187,6 +191,8 @@ namespace armarx
         controllerSensorData.getWriteBuffer().currentTime += deltaT;
         controllerSensorData.commitWrite();
 
+        interfaceData.getWriteBuffer().currentTcpPose = currentPose;
+        interfaceData.commitWrite();
 
         Eigen::VectorXf targetVel = rtGetControlStruct().targetTSVel;
         Eigen::Matrix4f targetPose = rtGetControlStruct().targetPose;
@@ -358,7 +364,14 @@ namespace armarx
 
     void NJointTSDMPController::runDMP(const Ice::DoubleSeq&  goals, double tau, const Ice::Current&)
     {
-        Eigen::Matrix4f pose = tcp->getPoseInRootFrame();
+        while (!interfaceData.updateReadBuffer())
+        {
+            usleep(100);
+        }
+
+        Eigen::Matrix4f pose = interfaceData.getReadBuffer().currentTcpPose;
+
+        //        Eigen::Matrix4f pose = tcp->getPoseInRootFrame();
         taskSpaceDMPController->prepareExecution(taskSpaceDMPController->eigen4f2vec(pose), goals);
         finished = false;
 

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

@@ -127,6 +127,12 @@ namespace armarx
         };
         TripleBuffer<NJointTSDMPControllerSensorData> controllerSensorData;
 
+        struct NJointTSDMPControllerInterfaceData
+        {
+            Eigen::Matrix4f currentTcpPose;
+        };
+
+        TripleBuffer<NJointTSDMPControllerInterfaceData> interfaceData;
 
         std::vector<const SensorValue1DoFActuatorTorque*> torqueSensors;
         std::vector<const SensorValue1DoFGravityTorque*> gravityTorqueSensors;