From 8a32cd172b48de32e0f10cad5b047f1b6b5b0093 Mon Sep 17 00:00:00 2001
From: Armar6 <armar6@h2t.com>
Date: Tue, 10 Apr 2018 17:49:57 +0200
Subject: [PATCH] modified jointspacedmp
---
.../NJointJointSpaceDMPController.cpp | 37 +++++++++++++++++--
.../NJointJointSpaceDMPController.h | 4 ++
.../NJointJointSpaceDMPController.ice | 1 +
3 files changed, 38 insertions(+), 4 deletions(-)
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointJointSpaceDMPController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointJointSpaceDMPController.cpp
index abe5e7eb6..7b8c35ce6 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointJointSpaceDMPController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointJointSpaceDMPController.cpp
@@ -39,8 +39,11 @@ namespace armarx
phaseL = cfg->phaseL;
phaseK = cfg->phaseK;
phaseDist0 = cfg->phaseDist0;
+ phaseDist1 = cfg->phaseDist1;
phaseKp = cfg->phaseKp;
+ isDisturbance = false;
+
NJointJointSpaceDMPControllerControlData initData;
initData.tau = 1.0;
initData.isStart = false;
@@ -70,10 +73,32 @@ namespace armarx
error += pow(currentPos.pos - targetState[i], 2);
}
+
+ double phaseDist;
+
+ if(isDisturbance)
+ {
+ phaseDist = phaseDist1;
+ }
+ else
+ {
+ phaseDist = phaseDist0;
+ }
+
error = sqrt(error);
- phaseStop = phaseL / (1 + exp(-phaseK * (error - phaseDist0)));
+ phaseStop = phaseL / (1 + exp(-phaseK * (error - phaseDist)));
mpcFactor = 1 - (phaseStop / phaseL);
+ if(mpcFactor < 0.1)
+ {
+ isDisturbance = true;
+ }
+
+ if(mpcFactor > 0.9)
+ {
+ isDisturbance = false;
+ }
+
double tau = rtGetControlStruct().tau;
double deltaT = timeSinceLastIteration.toSecondsDouble();
canVal -= 1 / tau * deltaT * 1 / (1 + phaseStop);
@@ -92,8 +117,8 @@ namespace armarx
const auto& jointName = dimNames.at(i);
if (targets.count(jointName) == 1)
{
- double vel0 = currentState[i].vel / timeDuration;
- double vel1 = phaseKp * (targetState[i] - currentPosition[i]);
+ vel0 = currentState[i].vel / timeDuration;
+ vel1 = phaseKp * (targetState[i] - currentPosition[i]);
double vel = mpcFactor * vel0 + (1 - mpcFactor) * vel1;
targets[jointName]->velocity = finished ? 0.0f : vel;
}
@@ -171,7 +196,11 @@ namespace armarx
void NJointJointSpaceDMPController::showMessages(const Ice::Current &)
{
- ARMARX_INFO << "mpcFactor: " << mpcFactor;
+ ARMARX_INFO << "mpcFactor: " << mpcFactor;
+ ARMARX_INFO << "canVal: " << canVal;
+ ARMARX_INFO << "vel0: " << vel0;
+ ARMARX_INFO << "vel1: " << vel1;
+ ARMARX_INFO << "isDisturbance: " << isDisturbance;
}
void NJointJointSpaceDMPController::setTemporalFactor(double tau, const Ice::Current&)
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointJointSpaceDMPController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointJointSpaceDMPController.h
index 04488956a..c80b80dd6 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointJointSpaceDMPController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointJointSpaceDMPController.h
@@ -91,10 +91,14 @@ namespace armarx
double phaseL;
double phaseK;
double phaseDist0;
+ double phaseDist1;
double phaseKp;
double mpcFactor;
+ double vel0;
+ double vel1;
+ bool isDisturbance;
std::vector<std::string> dimNames;
DMP::Vec<DMP::DMPState> currentState;
DMP::DVec targetState;
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointJointSpaceDMPController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointJointSpaceDMPController.ice
index 102ebe8f8..276ecb113 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointJointSpaceDMPController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointJointSpaceDMPController.ice
@@ -39,6 +39,7 @@ module armarx
double phaseL = 10;
double phaseK = 10;
double phaseDist0 = 0.3;
+ double phaseDist1 = 0.1;
double phaseKp = 1;
double timeDuration = 10;
--
GitLab