diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice index 5d5a4094495f9079593f5483bf096f170f2636f8..ca8d238562c20ff53d018223aa6f68d37cba466f 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 85746f5c1a71fe02e92b1469c117f8eecf6a33a9..510557089132b513c82763a2cbe2fb75564d3052 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 9b3e627f6f9927a436b07c1ca2ad5130bf73510e..163a34bcc837aced61655825e4117335c6c05592 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 e7c02120faefb847b6a98c9ae2e66437ce33c835..2f1220aa904a41a4cdb86f6a2c34e9e5b9906290 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 d69508151583aa3d02e323f2f44291dc159dcbb5..0f09333b6bc841d94c9e4a0b6fedfdd30c4b678a 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: