(??) Add debug data

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

@@ -451,6 +451,8 @@ namespace armarx
         debugOutputData.getWriteBuffer().currentDnull_y = dnull.y();
         debugOutputData.getWriteBuffer().currentDnull_z = dnull.z();
 
+        debugOutputData.getWriteBuffer().filteredForceInRoot = filteredForceInRoot;
+
         debugOutputData.commitWrite();
 
     }
@@ -641,6 +643,10 @@ namespace armarx
         datafields["forceDesired_ry"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_ry);
         datafields["forceDesired_rz"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_rz);
 
+        datafields["filteredForceInRoot_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().filteredForceInRoot[0]);
+        datafields["filteredForceInRoot_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().filteredForceInRoot[1]);
+        datafields["filteredForceInRoot_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().filteredForceInRoot[2]);
+
         datafields["deltaT"] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaT);
 
         std::string channelName = cfg->nodeSetName + "_TaskSpaceImpedanceControl";

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

@@ -162,6 +162,8 @@ namespace armarx
             float forceDesired_ry;
             float forceDesired_rz;
 
+            Eigen::Vector3f filteredForceInRoot;
+
             float deltaT;