From c5b20c5890cc2e672b16f3cb49519f9bf330fa75 Mon Sep 17 00:00:00 2001
From: Armar6 <armar6@h2t.com>
Date: Tue, 17 Apr 2018 12:08:51 +0200
Subject: [PATCH] modified dmpcontroller
---
.../DMPController/NJointCCDMPController.cpp | 14 +++++++++++++
.../DMPController/NJointCCDMPController.h | 2 ++
.../DMPController/NJointTSDMPController.cpp | 21 ++++++++++++++++---
.../DMPController/NJointTSDMPController.h | 1 +
4 files changed, 35 insertions(+), 3 deletions(-)
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.cpp
index 460e20be4..d13c9d954 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.cpp
@@ -262,6 +262,13 @@ namespace armarx
debugOutputData.getWriteBuffer().dmpTargets["dmp_pitch"] = targetState[4];
debugOutputData.getWriteBuffer().dmpTargets["dmp_yaw"] = targetState[5];
+ debugOutputData.getWriteBuffer().realTCP["real_x"] = currentPosition[0];
+ debugOutputData.getWriteBuffer().realTCP["real_y"] = currentPosition[1];
+ debugOutputData.getWriteBuffer().realTCP["real_z"] = currentPosition[2];
+ debugOutputData.getWriteBuffer().realTCP["real_roll"] = currentRPY[0];
+ debugOutputData.getWriteBuffer().realTCP["real_pitch"] = currentRPY[1];
+ debugOutputData.getWriteBuffer().realTCP["real_yaw"] = currentRPY[2];
+
debugOutputData.getWriteBuffer().mpcFactor = mpcFactor;
debugOutputData.getWriteBuffer().error = error;
debugOutputData.getWriteBuffer().phaseStop = phaseStop;
@@ -548,6 +555,13 @@ namespace armarx
datafields[pair.first] = new Variant(pair.second);
}
+ auto realTCP = debugOutputData.getUpToDateReadBuffer().realTCP;
+ for (auto& pair : realTCP)
+ {
+ datafields[pair.first] = new Variant(pair.second);
+ }
+
+
datafields["mpcFactor"] = new Variant(debugOutputData.getUpToDateReadBuffer().mpcFactor);
datafields["poseError"] = new Variant(debugOutputData.getUpToDateReadBuffer().error);
datafields["phaseStop"] = new Variant(debugOutputData.getUpToDateReadBuffer().phaseStop);
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.h
index 2247c5639..cde02bb4e 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.h
@@ -93,6 +93,8 @@ namespace armarx
{
StringFloatDictionary latestTargetVelocities;
StringFloatDictionary dmpTargets;
+ StringFloatDictionary realTCP;
+
double mpcFactor;
double error;
double phaseStop;
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
index 92df61bbc..a9d87b718 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
@@ -105,6 +105,9 @@ namespace armarx
double posError = 0;
double oriError = 0;
double deltaT = 0;
+ std::vector<double> currentposi;
+ std::vector<double> currentori;
+
if (canVal > 1e-8)
{
if (!controllerSensorData.updateReadBuffer())
@@ -115,7 +118,6 @@ namespace armarx
currentState = controllerSensorData.getReadBuffer().currentState;
- std::vector<double> currentposi;
currentposi.resize(3);
for (size_t i = 0; i < 3; ++i)
{
@@ -123,7 +125,6 @@ namespace armarx
posError += pow(currentState[i].pos - targetState[i], 2);
}
- std::vector<double> currentori;
currentori.resize(3);
for (size_t i = 0; i < 3; ++i)
{
@@ -204,6 +205,14 @@ namespace armarx
debugOutputData.getWriteBuffer().dmpTargets["dmp_pitch"] = targetState[4];
debugOutputData.getWriteBuffer().dmpTargets["dmp_yaw"] = targetState[5];
+ debugOutputData.getWriteBuffer().realTCP["real_x"] = currentposi[0];
+ debugOutputData.getWriteBuffer().realTCP["real_y"] = currentposi[1];
+ debugOutputData.getWriteBuffer().realTCP["real_z"] = currentposi[2];
+ debugOutputData.getWriteBuffer().realTCP["real_roll"] = currentori[0];
+ debugOutputData.getWriteBuffer().realTCP["real_pitch"] = currentori[1];
+ debugOutputData.getWriteBuffer().realTCP["real_yaw"] = currentori[2];
+
+
debugOutputData.getWriteBuffer().currentCanVal = canVal;
debugOutputData.getWriteBuffer().mpcFactor = mpcFactor;
debugOutputData.getWriteBuffer().error = error;
@@ -483,6 +492,12 @@ namespace armarx
datafields[pair.first] = new Variant(pair.second);
}
+ auto realTCP = debugOutputData.getUpToDateReadBuffer().realTCP;
+ for (auto& pair : realTCP)
+ {
+ datafields[pair.first] = new Variant(pair.second);
+ }
+
datafields["canVal"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentCanVal);
datafields["mpcFactor"] = new Variant(debugOutputData.getUpToDateReadBuffer().mpcFactor);
datafields["poseError"] = new Variant(debugOutputData.getUpToDateReadBuffer().error);
@@ -497,7 +512,7 @@ namespace armarx
void NJointTSDMPController::onInitComponent()
{
ARMARX_INFO << "init ...";
- controllerTask = new PeriodicTask<NJointTSDMPController>(this, &NJointTSDMPController::controllerRun, 1);
+ controllerTask = new PeriodicTask<NJointTSDMPController>(this, &NJointTSDMPController::controllerRun, 0.3);
}
void NJointTSDMPController::onDisconnectComponent()
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
index e23451a67..d472aee7a 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
@@ -93,6 +93,7 @@ namespace armarx
{
StringFloatDictionary latestTargetVelocities;
StringFloatDictionary dmpTargets;
+ StringFloatDictionary realTCP;
double currentCanVal;
double mpcFactor;
double error;
--
GitLab