From efa2917d9dcc6d9437c0bf695e430c0619a496aa Mon Sep 17 00:00:00 2001
From: ArmarX User <armarx@kit.edu>
Date: Wed, 2 Oct 2019 14:12:18 +0200
Subject: [PATCH] add dmp serialization
---
.../NJointTaskSpaceDMPController.ice | 3 ++
.../DMPController/NJointJSDMPController.cpp | 6 +++-
.../DMPController/NJointJSDMPController.h | 1 +
.../DMPController/NJointTSDMPController.cpp | 30 ++++++++++++-------
.../DMPController/NJointTSDMPController.h | 2 ++
5 files changed, 31 insertions(+), 11 deletions(-)
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
index 5d5a40944..ca8d23856 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
@@ -98,6 +98,9 @@ module armarx
void setControllerTarget(float avoidJointLimitsKp, NJointTaskSpaceDMPControllerMode::CartesianSelection mode);
void setTorqueKp(StringFloatDictionary torqueKp);
void setNullspaceJointVelocities(StringFloatDictionary nullspaceJointVelocities);
+ string getDMPAsString();
+ void createDMPFromString(string dmpString);
+
};
class NJointCCDMPControllerConfig extends NJointControllerConfig
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.cpp
index 85746f5c1..510557089 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.cpp
@@ -285,7 +285,6 @@ namespace armarx
ARMARX_INFO << "dim name: " << dimNames[i] << " current state: qpos: " << currentPos.pos << " orig target: " << goals[i] << " current goal: " << tjv;
}
-
}
dmpPtr->prepareExecution(goalVec, currentDMPState, 1, 1);
@@ -303,6 +302,11 @@ namespace armarx
{
}
+ // std::string NJointJSDMPController::getDMPAsString(const Ice::Current &)
+ // {
+
+ // }
+
void NJointJSDMPController::setSpeed(double times, const Ice::Current&)
{
LockGuardType guard(controllerMutex);
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.h
index 9b3e627f6..163a34bcc 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.h
@@ -52,6 +52,7 @@ namespace armarx
void runDMP(const Ice::DoubleSeq& goals, double times, const Ice::Current&) override;
void showMessages(const Ice::Current&) override;
+ // std::string getDMPAsString(const Ice::Current&) override;
protected:
void rtPreActivateController() override;
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
index e7c02120f..2f1220aa9 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
@@ -1,4 +1,6 @@
#include "NJointTSDMPController.h"
+#include <boost/archive/text_oarchive.hpp>
+#include <boost/archive/text_iarchive.hpp>
namespace armarx
{
@@ -137,9 +139,6 @@ namespace armarx
started = false;
- // NJointTSDMPControllerInterfaceData initInterfaceData;
- // initInterfaceData.currentTcpPose = tcp->getPoseInRootFrame();
- // interfaceData.reinitAllBuffers(initInterfaceData);
RTToUserData initInterfaceData;
initInterfaceData.currentTcpPose = Eigen::Matrix4f::Identity();
rt2UserData.reinitAllBuffers(initInterfaceData);
@@ -208,12 +207,9 @@ namespace armarx
debugOutputData.commitWrite();
-
getWriterControlStruct().targetTSVel = targetVels;
getWriterControlStruct().targetPose = targetPose;
writeControlStruct();
-
-
}
@@ -345,9 +341,6 @@ namespace armarx
rtDebugData.getWriteBuffer().targetJointVels = jointTargetVelocities;
rtDebugData.commitWrite();
-
-
-
}
@@ -357,7 +350,6 @@ namespace armarx
LockGuardType guard {controllerMutex};
dmpCtrl->learnDMPFromFiles(fileNames);
-
}
void NJointTSDMPController::setSpeed(Ice::Double times, const Ice::Current&)
@@ -440,6 +432,24 @@ namespace armarx
firstRun = true;
}
+ string NJointTSDMPController::getDMPAsString(const Ice::Current&)
+ {
+ std::stringstream ss;
+ boost::archive::text_oarchive oa{ss};
+ oa << dmpCtrl->getDMP().get();
+ return ss.str();
+ }
+
+ void NJointTSDMPController::createDMPFromString(const string& dmpString, const Ice::Current&)
+ {
+ std::stringstream ss;
+ ss.str() = dmpString;
+ boost::archive::text_iarchive ia{ss};
+ UMITSMP* dmpPtr;
+ ia >> dmpPtr;
+ dmpCtrl->getDMP().reset(dmpPtr);
+ }
+
VirtualRobot::IKSolver::CartesianSelection NJointTSDMPController::ModeFromIce(const NJointTaskSpaceDMPControllerMode::CartesianSelection mode)
{
if (mode == NJointTaskSpaceDMPControllerMode::CartesianSelection::ePosition)
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
index d69508151..0f09333b6 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
@@ -100,6 +100,8 @@ namespace armarx
{
return dmpCtrl->canVal;
}
+ std::string getDMPAsString(const Ice::Current&) override;
+ void createDMPFromString(const std::string& dmpString, const Ice::Current&) override;
VirtualRobot::IKSolver::CartesianSelection ModeFromIce(const NJointTaskSpaceDMPControllerMode::CartesianSelection mode);
protected:
--
GitLab