diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice index 7ace18c19116679537e6dfdd65b62adedc855b77..8c649d9a730f40156508c14798cbb5b141221332 100644 --- a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice +++ b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice @@ -82,6 +82,7 @@ module armarx void learnDMPFromFiles(Ice::StringSeq trajfiles); bool isFinished(); void runDMP(Ice::DoubleSeq goals, double tau); + void runDMPWithTime(Ice::DoubleSeq goals, double time); void setSpeed(double times); void setViaPoints(double canVal, Ice::DoubleSeq point); diff --git a/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.h b/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.h index 1ec911ecdbb123469ed95d8129d038cab84d57db..f532fd0e2e2175982fc5c878ce7d1415e0199ba1 100644 --- a/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.h +++ b/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.h @@ -200,6 +200,7 @@ namespace armarx bool isPhaseStopControl; std::string dmpName; DMP::UMITSMPPtr dmpPtr; + TaskSpaceDMPControllerConfig config; private: @@ -210,7 +211,6 @@ namespace armarx DMP::DVec targetPoseVec; DMP::Vec<DMP::DMPState > currentState; - TaskSpaceDMPControllerConfig config; bool paused; bool isDisturbance; diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp index 76025d7683569dbd12a996521ee9928288a49256..e1a5fe8ac8e3afb4c81cb2005efd347d7849809d 100644 --- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp +++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp @@ -495,6 +495,31 @@ namespace armarx started = true; } + void NJointTSDMPController::runDMPWithTime(const Ice::DoubleSeq& goals, Ice::Double timeDuration, const Ice::Current&) + { + ARMARX_INFO << "------dmp controller: " << VAROUT(goals); + firstRun = true; + while (firstRun) + { + usleep(100); + } + while (!rt2UserData.updateReadBuffer()) + { + usleep(100); + } + + Eigen::Matrix4f pose = rt2UserData.getReadBuffer().currentTcpPose; + + LockGuardType guard {controllerMutex}; + dmpCtrl->config.motionTimeDuration = timeDuration; + dmpCtrl->prepareExecution(dmpCtrl->eigen4f2vec(pose), goals); + + finished = false; + + ARMARX_INFO << "run DMP"; + started = true; + } + diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h index 5cc152b6604ec34c4147b4611738cea476c3f27a..c48ac700c06800bd4c19c2509b27244ef71899ad 100644 --- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h +++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h @@ -79,6 +79,8 @@ namespace armarx } void runDMP(const Ice::DoubleSeq& goals, Ice::Double tau, const Ice::Current&) override; + void runDMPWithTime(const Ice::DoubleSeq& goals, Ice::Double timeDuration, const Ice::Current&) override; + void setSpeed(Ice::Double times, const Ice::Current&) override; void setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&) override; void removeAllViaPoints(const Ice::Current&) override; diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp index e56bb103a8da8ba235d4a38f331fc964f03a54df..08141bbf4af075c7edc4aa305dfdee2d3586ea03 100644 --- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp +++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp @@ -439,9 +439,10 @@ namespace armarx } Eigen::Matrix4f pose = interfaceData.getUpToDateReadBuffer().currentTcpPose; + dmpCtrl->config.motionTimeDuration = timeDuration; + dmpCtrl->prepareExecution(dmpCtrl->eigen4f2vec(pose), goals); - dmpCtrl->canVal = timeDuration; finished = false; if (isNullSpaceJointDMPLearned && useNullSpaceJointDMP)