fixed rtrun problem

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

@@ -72,7 +72,7 @@ namespace armarx
             }
             error = sqrt(error);
             phaseStop = phaseL / (1 + exp(-phaseK * (error - phaseDist0)));
-            double mpcFactor = 1 - (phaseStop / phaseL);
+            mpcFactor = 1 - (phaseStop / phaseL);
 
             double tau = rtGetControlStruct().tau;
             double deltaT = timeSinceLastIteration.toSecondsDouble();
@@ -99,6 +99,17 @@ namespace armarx
                 }
             }
         }
+        else
+        {
+            for (size_t i = 0; i < dimNames.size(); ++i)
+            {
+                const auto& jointName = dimNames.at(i);
+                if (targets.count(jointName) == 1)
+                {
+                    targets[jointName]->velocity =  0.0f;
+                }
+            }
+        }
     }
 
     void NJointJointSpaceDMPController::learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&)
@@ -158,6 +169,11 @@ namespace armarx
 
     }
 
+    void NJointJointSpaceDMPController::showMessages(const Ice::Current &)
+    {
+        ARMARX_INFO << "mpcFactor: " << mpcFactor;
+    }
+
     void NJointJointSpaceDMPController::setTemporalFactor(double tau, const Ice::Current&)
     {
         this->tau = tau;

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

@@ -64,6 +64,8 @@ namespace armarx
 
         void runDMP(const Ice::DoubleSeq&  goals, double tau, const Ice::Current&);
 
+        void showMessages(const Ice::Current&);
+
     protected:
         void rtPreActivateController() override;
         void rtPostDeactivateController() override;
@@ -91,6 +93,8 @@ namespace armarx
         double phaseDist0;
         double phaseKp;
 
+        double mpcFactor;
+
         std::vector<std::string> dimNames;
         DMP::Vec<DMP::DMPState> currentState;
         DMP::DVec targetState;

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

@@ -51,6 +51,7 @@ module armarx
         bool isFinished();
         void runDMP(Ice::DoubleSeq goals, double tau);
         void setTemporalFactor(double tau);
+        void showMessages();
     };
 };
 #endif