finished coding njointccdmpcontroller

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

@@ -68,6 +68,7 @@ namespace armarx
         phaseKpOri = cfg->phaseKpOri;
 
         posToOriRatio = cfg->posToOriRatio;
+        tau = cfg->tau;
 
         // initialize tcp position and orientation
         Eigen::Matrix4f pose = tcp->getPoseInRootFrame();
@@ -100,6 +101,8 @@ namespace armarx
         initData.torqueKd.resize(tcpController->rns->getSize(), 0);
         initData.mode = ModeFromIce(cfg->mode);
         reinitTripleBuffer(initData);
+
+        learnedDMP.clear();
     }
 
     std::string NJointCCDMPController::getClassName(const Ice::Current&) const
@@ -113,7 +116,9 @@ namespace armarx
         {
             return;
         }
+
         double deltaT = controllerSensorData.getReadBuffer().deltaT;
+
         Eigen::Matrix4f currentPose = controllerSensorData.getReadBuffer().currentPose;
         Eigen::Vector3f currentPosition;
         currentPosition << currentPose(0, 3), currentPose(1, 3), currentPose(2, 3);
@@ -277,14 +282,12 @@ namespace armarx
 
     void NJointCCDMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
     {
-        double deltaT = sensorValuesTimestamp.toSecondsDouble();
+        double deltaT = timeSinceLastIteration.toSecondsDouble();
         controllerSensorData.getWriteBuffer().currentPose = tcp->getPoseInRootFrame();
         controllerSensorData.getWriteBuffer().deltaT = deltaT;
         controllerSensorData.getWriteBuffer().currentTime += deltaT;
 
         controllerSensorData.commitWrite();
-
-
         // cartesian vel controller
         Eigen::VectorXf x;
         auto mode = rtGetControlStruct().mode;
@@ -430,6 +433,8 @@ namespace armarx
 
         dmpPtrList[dmpId]->prepareExecution(goals, currentState, 1,  tau);
         dmpPtrList[dmpId]->setTemporalFactor(tau);
+
+        learnedDMP.push_back(dmpId);
         ARMARX_INFO << "Learned DMP ... ";
     }
 
@@ -498,7 +503,16 @@ namespace armarx
 
         finished = false;
 
-
+        if (dmpTypes.size() != cfg->dmpNum ||
+            dmpPtrList.size() != cfg->dmpNum ||
+            learnedDMP.size() != cfg->dmpNum ||
+            canVals.size() != cfg->dmpNum ||
+            currentStates.size() != cfg->dmpNum ||
+            targetSubStates.size() != cfg->dmpNum)
+        {
+            ARMARX_ERROR << "Error: cannot run CCDMP controller. The reason is that some parameters have different sizes";
+            return;
+        }
         ARMARX_INFO << "run DMP";
         controllerTask->start();
 

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

@@ -148,6 +148,7 @@ namespace armarx
 
         std::vector<float> targetVels;
 
+        std::vector<int> learnedDMP;
         NJointCCDMPControllerConfigPtr cfg;
         VirtualRobot::RobotNodePtr tcp;
         Eigen::Vector3f tcpPosition;

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

@@ -63,10 +63,16 @@ namespace armarx
 
     void NJointJSDMPController::controllerRun()
     {
+        if (!controllerSensorData.updateReadBuffer())
+        {
+            return;
+        }
+
+        currentState = controllerSensorData.getUpToDateReadBuffer().currentState;
+        double deltaT = controllerSensorData.getUpToDateReadBuffer().deltaT;
         if (canVal > 1e-8)
         {
-            currentState = controllerSensorData.getUpToDateReadBuffer().currentState;
-            double deltaT = controllerSensorData.getUpToDateReadBuffer().deltaT;
+
             double phaseStop = 0;
             double error = 0;
             std::vector<double> currentPosition;