diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice index e80c3cf92487806e92f7ea6fa90fc64f4b96e15a..4b4211d8a5a8449e4a0fba74ffeeaeceae991f0c 100644 --- a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice +++ b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice @@ -565,6 +565,7 @@ module armarx double getCanVal(); Ice::FloatSeq getAnomalyInput(); + Ice::FloatSeq getAnomalyOutput(); }; }; diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.cpp index efb6befa7bbd81921ec4738d05f71d23bbae2a6f..c7fe811d8358c894f2d5ac1175537cba6ff53d4d 100644 --- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.cpp +++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.cpp @@ -286,12 +286,6 @@ namespace armarx velocityHorizonList.pop_front(); } - rt2UserData.getWriteBuffer().currentTcpPose = currentPose; - rt2UserData.getWriteBuffer().tcpTranslVel = currentTwist.head(3); - rt2UserData.getWriteBuffer().waitTimeForCalibration += deltaT; - - rt2UserData.commitWrite(); - Eigen::VectorXf targetVel(6); Eigen::Vector3f axis; Eigen::Vector3f forceInToolFrame; @@ -573,6 +567,14 @@ namespace armarx // targetFTInRootFrame.tail(3) = currentToolOri * targetFTInToolFrame.tail(3); } + + rt2UserData.getWriteBuffer().currentTcpPose = currentPose; + rt2UserData.getWriteBuffer().tcpTranslVel = currentTwist.head(3); + rt2UserData.getWriteBuffer().forceOutput = forceInToolFrame; + rt2UserData.getWriteBuffer().waitTimeForCalibration += deltaT; + rt2UserData.commitWrite(); + + /* -------------------------- VMP Phase Stop --------------------------------- */ bool isPhaseStop = false; @@ -795,6 +797,13 @@ namespace armarx return tvelvec; } + std::vector<float> NJointAnomalyDetectionAdaptiveWipingController::getAnomalyOutput(const Ice::Current&) + { + Eigen::Vector3f force = rt2UserData.getUpToDateReadBuffer().forceOutput; + std::vector<float> forceVec = {force(0), force(1), force(2)}; + return forceVec; + } + void NJointAnomalyDetectionAdaptiveWipingController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx& debugDrawer, const DebugObserverInterfacePrx& debugObs) { diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.h index 8940f3b0476e6d178ee4736351818373391bff31..154b2c8ce1af2afdb6caf09af548a24d3660e02e 100644 --- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.h +++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.h @@ -82,6 +82,8 @@ namespace armarx } std::vector<float> getAnomalyInput(const Ice::Current&); + std::vector<float> getAnomalyOutput(const Ice::Current&); + protected: virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&); @@ -151,6 +153,7 @@ namespace armarx { Eigen::Matrix4f currentTcpPose; Eigen::Vector3f tcpTranslVel; + Eigen::Vector3f forceOutput; float waitTimeForCalibration; }; TripleBuffer<RTToUserData> rt2UserData;