modified the jsdmpctrl

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

@@ -71,12 +71,11 @@ namespace armarx
                 currentVelocity.push_back(currentPos.vel);
 
                 error += pow(currentPos.pos - targetState[i], 2);
-
             }
 
             double phaseDist;
 
-            if(isDisturbance)
+            if (isDisturbance)
             {
                 phaseDist = phaseDist1;
             }
@@ -89,12 +88,12 @@ namespace armarx
             phaseStop = phaseL / (1 + exp(-phaseK * (error - phaseDist)));
             mpcFactor = 1 - (phaseStop / phaseL);
 
-            if(mpcFactor < 0.1)
+            if (mpcFactor < 0.1)
             {
                 isDisturbance = true;
             }
 
-            if(mpcFactor > 0.9)
+            if (mpcFactor > 0.9)
             {
                 isDisturbance = false;
             }
@@ -117,8 +116,8 @@ namespace armarx
                 const auto& jointName = dimNames.at(i);
                 if (targets.count(jointName) == 1)
                 {
-                    vel0 = currentState[i].vel / timeDuration;
-                    vel1 = phaseKp * (targetState[i] - currentPosition[i]);
+                    double vel0 = currentState[i].vel / timeDuration;
+                    double vel1 = phaseKp * (targetState[i] - currentPosition[i]);
                     double vel = mpcFactor * vel0 + (1 - mpcFactor) * vel1;
                     targets[jointName]->velocity = finished ? 0.0f : vel;
 
@@ -126,6 +125,8 @@ namespace armarx
                 }
             }
 
+            debugOutputData.getWriteBuffer().currentCanVal = canVal;
+            debugOutputData.getWriteBuffer().mpcFactor = mpcFactor;
             debugOutputData.commitWrite();
         }
         else
@@ -198,13 +199,8 @@ namespace armarx
 
     }
 
-    void NJointJointSpaceDMPController::showMessages(const Ice::Current &)
+    void NJointJointSpaceDMPController::showMessages(const Ice::Current&)
     {
-            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&)
@@ -225,15 +221,17 @@ namespace armarx
 
     }
 
-    void NJointJointSpaceDMPController::onPublish(const SensorAndControl &, const DebugDrawerInterfacePrx &, const DebugObserverInterfacePrx &debugObs)
+    void NJointJointSpaceDMPController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
     {
         StringVariantBaseMap datafields;
         auto values = debugOutputData.getUpToDateReadBuffer().latestTargetVelocities;
-        for(auto& pair : values)
+        for (auto& pair : values)
         {
             datafields[pair.first] = new Variant(pair.second);
         }
 
+        datafields["canVal"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentCanVal);
+        datafields["mpcFactor"] = new Variant(debugOutputData.getUpToDateReadBuffer().mpcFactor);
         debugObs->setDebugChannel("latestDMPTargetVelocities", datafields);
     }
 

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

@@ -70,12 +70,14 @@ namespace armarx
         void rtPreActivateController() override;
         void rtPostDeactivateController() override;
 
-        virtual void onPublish(const SensorAndControl &, const DebugDrawerInterfacePrx &, const DebugObserverInterfacePrx &);
+        virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
     private:
 
         struct DebugBufferData
         {
             StringFloatDictionary latestTargetVelocities;
+            double currentCanVal;
+            double mpcFactor;
         };
 
         std::map<std::string, const SensorValue1DoFActuatorTorque*> torqueSensors;
@@ -106,13 +108,12 @@ namespace armarx
         double phaseKp;
 
         double mpcFactor;
-        double vel0;
-        double vel1;
 
         bool isDisturbance;
         std::vector<std::string> dimNames;
         DMP::Vec<DMP::DMPState> currentState;
         DMP::DVec targetState;
+
     };
 
 } // namespace armarx

source/RobotAPI/libraries/RobotAPINJointControllers/test/CMakeLists.txt

 
 # Libs required for the tests
-SET(LIBS ${LIBS} ArmarXCore RobotAPINJointsController)
+SET(LIBS ${LIBS} ArmarXCore RobotAPINJointControllers)
  
-armarx_add_test(RobotAPINJointsControllerTest RobotAPINJointsControllerTest.cpp "${LIBS}")
\ No newline at end of file
+armarx_add_test(RobotAPINJointsControllerTest RobotAPINJointsControllerTest.cpp "${LIBS}")