From 02dbc915f54878ecf7366ef73ae831aab4960bc0 Mon Sep 17 00:00:00 2001
From: Armar6 <armar6@h2t.com>
Date: Tue, 10 Apr 2018 15:42:58 +0200
Subject: [PATCH] fixed rtrun problem
---
.../NJointJointSpaceDMPController.cpp | 18 +++++++++++++++++-
.../NJointJointSpaceDMPController.h | 4 ++++
.../NJointJointSpaceDMPController.ice | 1 +
3 files changed, 22 insertions(+), 1 deletion(-)
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointJointSpaceDMPController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointJointSpaceDMPController.cpp
index d7073ab3f..abe5e7eb6 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointJointSpaceDMPController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointJointSpaceDMPController.cpp
@@ -72,7 +72,7 @@ namespace armarx
}
error = sqrt(error);
phaseStop = phaseL / (1 + exp(-phaseK * (error - phaseDist0)));
- double mpcFactor = 1 - (phaseStop / phaseL);
+ mpcFactor = 1 - (phaseStop / phaseL);
double tau = rtGetControlStruct().tau;
double deltaT = timeSinceLastIteration.toSecondsDouble();
@@ -99,6 +99,17 @@ namespace armarx
}
}
}
+ else
+ {
+ for (size_t i = 0; i < dimNames.size(); ++i)
+ {
+ const auto& jointName = dimNames.at(i);
+ if (targets.count(jointName) == 1)
+ {
+ targets[jointName]->velocity = 0.0f;
+ }
+ }
+ }
}
void NJointJointSpaceDMPController::learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&)
@@ -158,6 +169,11 @@ namespace armarx
}
+ void NJointJointSpaceDMPController::showMessages(const Ice::Current &)
+ {
+ ARMARX_INFO << "mpcFactor: " << mpcFactor;
+ }
+
void NJointJointSpaceDMPController::setTemporalFactor(double tau, const Ice::Current&)
{
this->tau = tau;
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointJointSpaceDMPController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointJointSpaceDMPController.h
index 7151002d7..04488956a 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointJointSpaceDMPController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointJointSpaceDMPController.h
@@ -64,6 +64,8 @@ namespace armarx
void runDMP(const Ice::DoubleSeq& goals, double tau, const Ice::Current&);
+ void showMessages(const Ice::Current&);
+
protected:
void rtPreActivateController() override;
void rtPostDeactivateController() override;
@@ -91,6 +93,8 @@ namespace armarx
double phaseDist0;
double phaseKp;
+ double mpcFactor;
+
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 356cd9ba1..102ebe8f8 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointJointSpaceDMPController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointJointSpaceDMPController.ice
@@ -51,6 +51,7 @@ module armarx
bool isFinished();
void runDMP(Ice::DoubleSeq goals, double tau);
void setTemporalFactor(double tau);
+ void showMessages();
};
};
#endif
--
GitLab