diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp index 0a08c5396283856d5e5ccc5e8ebfdbb9cb59500c..1730ea67e98907735b71c42798cbec2f3e5d0b63 100644 --- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp +++ b/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"; diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h index 58fd840fb1121b4a159c185abb2e72fd7470f082..5361dec0f37c7269c86432c6dff64c95504cca3f 100644 --- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h +++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h @@ -162,6 +162,8 @@ namespace armarx float forceDesired_ry; float forceDesired_rz; + Eigen::Vector3f filteredForceInRoot; + float deltaT;