From e3740b4ca914c50be2f373a8e4278d7ed26ebd50 Mon Sep 17 00:00:00 2001
From: robdekon_h2t <robdekon@kit.edu>
Date: Wed, 28 Jul 2021 16:41:54 +0200
Subject: [PATCH] Fixed unused setForceThreshold interface funktion in
TaskSpaceImpedanceDMPController
---
.../units/RobotUnit/NJointTaskSpaceDMPController.ice | 2 +-
.../NJointTaskSpaceImpedanceDMPController.cpp | 5 +++--
.../DMPController/NJointTaskSpaceImpedanceDMPController.h | 8 +++++---
3 files changed, 9 insertions(+), 6 deletions(-)
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
index a1ba8214c..56a17ef19 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
@@ -362,7 +362,7 @@ module armarx
void enableForceStop();
void disableForceStop();
- void setForceThreshold(float forceThreshold);
+ void setForceThreshold(Eigen::Vector3f forceThreshold);
};
class NJointTaskSpaceAdaptiveDMPControllerConfig extends NJointControllerConfig
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
index 5fc97fcf5..712029cc7 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
@@ -46,7 +46,8 @@ namespace armarx
forceOffset.setZero();
filteredForce.setZero();
filteredForceInRoot.setZero();
-
+ forceThreshold.getWriteBuffer() = cfg->forceThreshold;
+ forceThreshold.commitWrite();
tcp = rns->getTCP();
ik.reset(new VirtualRobot::DifferentialIK(rns, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
ik->setDampedSvdLambda(0.0001);
@@ -292,7 +293,7 @@ namespace armarx
for (size_t i = 0; i < 3; ++i)
{
- if (fabs(filteredForceInRoot[i]) > cfg->forceThreshold[i])
+ if (fabs(filteredForceInRoot[i]) > forceThreshold.getUpToDateReadBuffer()[i])
{
stopPose = currentPose;
targetVel.setZero(6);
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
index c0f8fea95..3a6f685e4 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
@@ -94,9 +94,11 @@ namespace armarx
{
useForceStop = false;
}
- void setForceThreshold(float forceThreshold, const Ice::Current&) override
+
+ void setForceThreshold(const Eigen::Vector3f& f, const Ice::Current& current) override
{
- this->forceThreshold = forceThreshold;
+ forceThreshold.getWriteBuffer() = f;
+ forceThreshold.commitWrite();
}
protected:
virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) override;
@@ -248,7 +250,7 @@ namespace armarx
Eigen::Vector3f filteredForce;
Eigen::Vector3f forceOffset;
Eigen::Vector3f filteredForceInRoot;
- std::atomic<float> forceThreshold;
+ WriteBufferedTripleBuffer<Eigen::Vector3f> forceThreshold;
std::atomic<bool> useForceStop;
std::atomic<float> timeForCalibration;
const SensorValueForceTorque* forceSensor;
--
GitLab