modified jointspacedmp

source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointJointSpaceDMPController.cpp

@@ -39,8 +39,11 @@ namespace armarx
         phaseL = cfg->phaseL;
         phaseK = cfg->phaseK;
         phaseDist0 = cfg->phaseDist0;
+        phaseDist1 = cfg->phaseDist1;
         phaseKp = cfg->phaseKp;
 
+        isDisturbance = false;
+
         NJointJointSpaceDMPControllerControlData initData;
         initData.tau = 1.0;
         initData.isStart = false;
@@ -70,10 +73,32 @@ namespace armarx
                 error += pow(currentPos.pos - targetState[i], 2);
 
             }
+
+            double phaseDist;
+
+            if(isDisturbance)
+            {
+                phaseDist = phaseDist1;
+            }
+            else
+            {
+                phaseDist = phaseDist0;
+            }
+
             error = sqrt(error);
-            phaseStop = phaseL / (1 + exp(-phaseK * (error - phaseDist0)));
+            phaseStop = phaseL / (1 + exp(-phaseK * (error - phaseDist)));
             mpcFactor = 1 - (phaseStop / phaseL);
 
+            if(mpcFactor < 0.1)
+            {
+                isDisturbance = true;
+            }
+
+            if(mpcFactor > 0.9)
+            {
+                isDisturbance = false;
+            }
+
             double tau = rtGetControlStruct().tau;
             double deltaT = timeSinceLastIteration.toSecondsDouble();
             canVal -= 1 / tau * deltaT * 1 / (1 + phaseStop);
@@ -92,8 +117,8 @@ namespace armarx
                 const auto& jointName = dimNames.at(i);
                 if (targets.count(jointName) == 1)
                 {
-                    double vel0 = currentState[i].vel / timeDuration;
-                    double vel1 = phaseKp * (targetState[i] - currentPosition[i]);
+                    vel0 = currentState[i].vel / timeDuration;
+                    vel1 = phaseKp * (targetState[i] - currentPosition[i]);
                     double vel = mpcFactor * vel0 + (1 - mpcFactor) * vel1;
                     targets[jointName]->velocity = finished ? 0.0f : vel;
                 }
@@ -171,7 +196,11 @@ namespace armarx
 
     void NJointJointSpaceDMPController::showMessages(const Ice::Current &)
     {
-        ARMARX_INFO << "mpcFactor: " << mpcFactor;
+            ARMARX_INFO << "mpcFactor: " << mpcFactor;
+            ARMARX_INFO << "canVal: " << canVal;
+            ARMARX_INFO << "vel0: " << vel0;
+            ARMARX_INFO << "vel1: " << vel1;
+            ARMARX_INFO << "isDisturbance: " << isDisturbance;
     }
 
     void NJointJointSpaceDMPController::setTemporalFactor(double tau, const Ice::Current&)

source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointJointSpaceDMPController.h

@@ -91,10 +91,14 @@ namespace armarx
         double phaseL;
         double phaseK;
         double phaseDist0;
+        double phaseDist1;
         double phaseKp;
 
         double mpcFactor;
+        double vel0;
+        double vel1;
 
+        bool isDisturbance;
         std::vector<std::string> dimNames;
         DMP::Vec<DMP::DMPState> currentState;
         DMP::DVec targetState;

source/RobotAPI/interface/units/RobotUnit/NJointJointSpaceDMPController.ice

@@ -39,6 +39,7 @@ module armarx
         double phaseL = 10;
         double phaseK = 10;
         double phaseDist0 = 0.3;
+        double phaseDist1 = 0.1;
         double phaseKp = 1;
 
         double timeDuration = 10;