From d82aa7877c3d7256546022b659f999de0d178cba Mon Sep 17 00:00:00 2001
From: zhou <you.zhou@kit.edu>
Date: Tue, 10 Sep 2019 09:32:22 +0200
Subject: [PATCH] modified the NJointAnomalyDetectionAdaptiveWipingController
---
.../NJointTaskSpaceDMPController.ice | 1 +
...omalyDetectionAdaptiveWipingController.cpp | 21 +++++++++++++------
...AnomalyDetectionAdaptiveWipingController.h | 3 +++
3 files changed, 19 insertions(+), 6 deletions(-)
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
index e80c3cf92..4b4211d8a 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 efb6befa7..c7fe811d8 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 8940f3b04..154b2c8ce 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;
--
GitLab