From bfcc7c9600d9237a89192c1b26bc1f25c4ee5b1c Mon Sep 17 00:00:00 2001
From: armar-user <armar6@kit.edu>
Date: Thu, 29 Jul 2021 14:58:28 +0200
Subject: [PATCH] Added interface function for TSImpedanceDMPController to
check if force threshold was reached
Signed-off-by: Christoph Pohl <christoph.pohl@kit.edu>
---
.../RobotUnit/NJointTaskSpaceDMPController.ice | 1 +
.../NJointPeriodicTSDMPCompliantController.cpp | 6 ++++++
.../NJointPeriodicTSDMPCompliantController.h | 1 +
.../NJointTaskSpaceImpedanceDMPController.cpp | 15 +++++++++++----
.../NJointTaskSpaceImpedanceDMPController.h | 5 +++++
5 files changed, 24 insertions(+), 4 deletions(-)
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
index 56a17ef19..84614f0d5 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
@@ -336,6 +336,7 @@ module armarx
void setUseNullSpaceJointDMP(bool enable);
bool isFinished();
+ bool isDMPRunning();
void runDMP(Ice::DoubleSeq goals);
void runDMPWithTime(Ice::DoubleSeq goals, double timeDuration);
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp
index 79a090adb..e8a401f48 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp
@@ -472,6 +472,7 @@ namespace armarx
debugRT.getWriteBuffer().targetVel = targetVel;
debugRT.getWriteBuffer().adaptK = adaptK;
debugRT.getWriteBuffer().isPhaseStop = isPhaseStop;
+ debugRT.getWriteBuffer().currentTwist = currentTwist;
rt2CtrlData.getWriteBuffer().currentPose = currentPose;
rt2CtrlData.getWriteBuffer().currentTwist = currentTwist;
@@ -673,6 +674,11 @@ namespace armarx
datafields["targetVel_y"] = new Variant(targetVel(1));
datafields["targetVel_z"] = new Variant(targetVel(2));
+ Eigen::VectorXf currentVel = debugRT.getUpToDateReadBuffer().currentTwist;
+ datafields["currentVel_x"] = new Variant(currentVel(0));
+ datafields["currentVel_y"] = new Variant(currentVel(1));
+ datafields["currentVel_z"] = new Variant(currentVel(2));
+
Eigen::Vector3f adaptK = debugRT.getUpToDateReadBuffer().adaptK;
datafields["adaptK_x"] = new Variant(adaptK(0));
datafields["adaptK_y"] = new Variant(adaptK(1));
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.h
index ebb3b05a5..477feb690 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.h
@@ -99,6 +99,7 @@ namespace armarx
Eigen::Vector3f adaptK;
Eigen::VectorXf targetVel;
Eigen::Matrix4f currentPose;
+ Eigen::VectorXf currentTwist;
bool isPhaseStop;
float manidist;
};
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
index d550b478c..0a08c5396 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
@@ -11,12 +11,14 @@ namespace armarx
const armarx::NJointControllerConfigPtr& config,
const VirtualRobot::RobotPtr&)
{
+ ARMARX_TRACE;
ARMARX_INFO << "creating impedance dmp controller";
cfg = NJointTaskSpaceImpedanceDMPControllerConfigPtr::dynamicCast(config);
+ ARMARX_CHECK_NOT_NULL(cfg);
useSynchronizedRtRobot();
rns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName);
ARMARX_CHECK_EXPRESSION(rns) << cfg->nodeSetName;
-
+ ARMARX_INFO << "1";
for (size_t i = 0; i < rns->getSize(); ++i)
{
std::string jointName = rns->getNode(i)->getName();
@@ -39,19 +41,20 @@ namespace armarx
velocitySensors.push_back(velocitySensor);
positionSensors.push_back(positionSensor);
};
-
const SensorValueBase* svlf = robotUnit->getSensorDevice(cfg->forceSensorName)->getSensorValue();
forceSensor = svlf->asA<SensorValueForceTorque>();
+ ARMARX_TRACE;
forceOffset.setZero();
filteredForce.setZero();
filteredForceInRoot.setZero();
- forceThreshold.getWriteBuffer() = cfg->forceThreshold;
- forceThreshold.commitWrite();
+ ARMARX_INFO << cfg->forceThreshold;
+ forceThreshold.reinitAllBuffers(cfg->forceThreshold);
tcp = rns->getTCP();
ik.reset(new VirtualRobot::DifferentialIK(rns, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
ik->setDampedSvdLambda(0.0001);
+ ARMARX_TRACE;
numOfJoints = targets.size();
// set DMP
TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
@@ -88,6 +91,7 @@ namespace armarx
}
defaultNullSpaceJointValues.reinitAllBuffers(nullspaceValues);
+ ARMARX_TRACE;
Eigen::Vector3f kpos(cfg->Kpos[0], cfg->Kpos[1], cfg->Kpos[2]);
Eigen::Vector3f dpos(cfg->Dpos[0], cfg->Dpos[1], cfg->Dpos[2]);
Eigen::Vector3f kori(cfg->Kori[0], cfg->Kori[1], cfg->Kori[2]);
@@ -136,6 +140,7 @@ namespace armarx
void NJointTaskSpaceImpedanceDMPController::rtPreActivateController()
{
+ ARMARX_TRACE;
NJointTaskSpaceImpedanceDMPControllerControlData initData;
initData.targetPose = tcp->getPoseInRootFrame();
initData.targetVel.resize(6);
@@ -547,6 +552,8 @@ namespace armarx
{
firstRun = true;
timeForCalibration = 0;
+ started = false;
+
while (firstRun || timeForCalibration < cfg->waitTimeForCalibration)
{
usleep(100);
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
index 3a6f685e4..58fd840fb 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
@@ -56,6 +56,11 @@ namespace armarx
return finished;
}
+ bool isDMPRunning(const Ice::Current&) override
+ {
+ return started;
+ }
+
void setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&) override;
void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&) override;
--
GitLab