From 117dbb67336567ea4f328eb825e1a7d914ce13e5 Mon Sep 17 00:00:00 2001
From: zhou <you.zhou@kit.edu>
Date: Wed, 11 Apr 2018 18:34:30 +0200
Subject: [PATCH] added a new jointdmpctrl with tripple-buffered control thread
---
.../NJointJointSpaceDMPController.ice | 1 +
.../RobotAPINJointControllers/CMakeLists.txt | 2 +
.../DMPController/NJointJSDMPController.cpp | 271 ++++++++++++++++++
.../DMPController/NJointJSDMPController.h | 127 ++++++++
.../NJointJointSpaceDMPController.cpp | 2 +-
5 files changed, 402 insertions(+), 1 deletion(-)
create mode 100644 source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.cpp
create mode 100644 source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.h
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointJointSpaceDMPController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointJointSpaceDMPController.ice
index 276ecb113..947a7dbbe 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointJointSpaceDMPController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointJointSpaceDMPController.ice
@@ -54,5 +54,6 @@ module armarx
void setTemporalFactor(double tau);
void showMessages();
};
+
};
#endif
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt b/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt
index da765662b..76755149a 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt
@@ -39,11 +39,13 @@ if (DMP_FOUND)
list(APPEND LIB_HEADERS
DMPController/NJointJointSpaceDMPController.h
DMPController/NJointTaskSpaceDMPController.h
+ DMPController/NJointJSDMPController.h
)
list(APPEND LIB_FILES
DMPController/NJointJointSpaceDMPController.cpp
DMPController/NJointTaskSpaceDMPController.cpp
+ DMPController/NJointJSDMPController.cpp
)
list(APPEND LIBS ${DMP_LIBRARIES})
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.cpp
new file mode 100644
index 000000000..6837a99be
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.cpp
@@ -0,0 +1,271 @@
+#include "NJointJSDMPController.h"
+
+
+
+namespace armarx
+{
+
+ NJointControllerRegistration<NJointJSDMPController> registrationControllerNJointJSDMPController("NJointJSDMPController");
+
+ std::string NJointJSDMPController::getClassName(const Ice::Current&) const
+ {
+ return "NJointJSDMPController";
+ }
+
+ NJointJSDMPController::NJointJSDMPController(armarx::NJointControllerDescriptionProviderInterfacePtr prov, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
+ {
+ NJointJointSpaceDMPControllerConfigPtr cfg = NJointJointSpaceDMPControllerConfigPtr::dynamicCast(config);
+ ARMARX_CHECK_EXPRESSION_W_HINT(cfg, "Needed type: NJointJointSpaceDMPControllerConfigPtr");
+
+ for (std::string jointName : cfg->jointNames)
+ {
+ ControlTargetBase* ct = prov->getControlTarget(jointName, ControlModes::Velocity1DoF);
+ const SensorValueBase* sv = prov->getSensorValue(jointName);
+ targets.insert(std::make_pair(jointName, ct->asA<ControlTarget1DoFActuatorVelocity>()));
+ positionSensors.insert(std::make_pair(jointName, sv->asA<SensorValue1DoFActuatorPosition>()));
+ torqueSensors.insert(std::make_pair(jointName, sv->asA<SensorValue1DoFActuatorTorque>()));
+ gravityTorqueSensors.insert(std::make_pair(jointName, sv->asA<SensorValue1DoFGravityTorque>()));
+ velocitySensors.insert(std::make_pair(jointName, sv->asA<SensorValue1DoFActuatorVelocity>()));
+ }
+ if (cfg->jointNames.size() == 0)
+ {
+ ARMARX_ERROR << "cfg->jointNames.size() == 0";
+ }
+
+ dmpPtr.reset(new DMP::UMIDMP(cfg->kernelSize, cfg->DMPKd, cfg->baseMode, cfg->tau));
+ timeDuration = cfg->timeDuration;
+ phaseL = cfg->phaseL;
+ phaseK = cfg->phaseK;
+ phaseDist0 = cfg->phaseDist0;
+ phaseDist1 = cfg->phaseDist1;
+ phaseKp = cfg->phaseKp;
+ dimNames = cfg->jointNames;
+
+ targetVels.resize(cfg->jointNames.size());
+ NJointJSDMPControllerControlData initData;
+ initData.targetJointVels.resize(cfg->jointNames.size());
+ for (size_t i = 0; i < cfg->jointNames.size(); ++i)
+ {
+ initData.targetJointVels[i] = 0;
+ }
+
+ reinitTripleBuffer(initData);
+
+
+ NJointJSDMPControllerSensorData initSensorData;
+ initSensorData.deltaT = 0;
+ initSensorData.currentState.resize(cfg->jointNames.size());
+ controllerSensorData.reinitAllBuffers(initSensorData);
+
+
+ deltaT = 0;
+ }
+
+ void NJointJSDMPController::controllerRun()
+ {
+ if (canVal > 1e-8)
+ {
+ currentState = controllerSensorData.getUpToDateReadBuffer().currentState;
+ double deltaT = controllerSensorData.getUpToDateReadBuffer().deltaT;
+ double phaseStop = 0;
+ double error = 0;
+ std::vector<double> currentPosition;
+ currentPosition.resize(dimNames.size());
+
+ for (size_t i = 0; i < currentState.size(); i++)
+ {
+ DMP::DMPState currentPos = currentState[i];
+ currentPosition[i] = currentPos.pos;
+ error += pow(currentPos.pos - targetState[i], 2);
+ }
+
+ double phaseDist;
+
+ if (isDisturbance)
+ {
+ phaseDist = phaseDist1;
+ }
+ else
+ {
+ phaseDist = phaseDist0;
+ }
+
+ error = sqrt(error);
+ phaseStop = phaseL / (1 + exp(-phaseK * (error - phaseDist)));
+ double mpcFactor = 1 - (phaseStop / phaseL);
+
+ if (mpcFactor < 0.1)
+ {
+ isDisturbance = true;
+ }
+
+ if (mpcFactor > 0.9)
+ {
+ isDisturbance = false;
+ }
+
+ canVal -= 1 / tau * deltaT * 1 / (1 + phaseStop);
+ double dmpDeltaT = deltaT / timeDuration;
+ dmpPtr->setTemporalFactor(tau);
+
+ currentState = dmpPtr->calculateDirectlyVelocity(currentState, canVal / timeDuration, dmpDeltaT, targetState);
+
+ for (size_t i = 0; i < currentState.size(); ++i)
+ {
+ double vel0 = currentState[i].vel / timeDuration;
+ double vel1 = phaseKp * (targetState[i] - currentPosition[i]);
+ double vel = mpcFactor * vel0 + (1 - mpcFactor) * vel1;
+ targetVels[i] = finished ? 0.0f : vel;
+
+ debugOutputData.getWriteBuffer().latestTargetVelocities[dimNames[i]] = vel;
+ }
+
+ debugOutputData.getWriteBuffer().currentCanVal = canVal;
+ debugOutputData.getWriteBuffer().mpcFactor = deltaT;
+ debugOutputData.commitWrite();
+
+ }
+ else
+ {
+ finished = true;
+ for (size_t i = 0; i < dimNames.size(); ++i)
+ {
+ targetVels[i] = 0;
+ }
+ }
+
+ LockGuardType guard {controlDataMutex};
+ getWriterControlStruct().targetJointVels = targetVels;
+ writeControlStruct();
+ }
+
+ void NJointJSDMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ {
+ for (size_t i = 0; i < dimNames.size(); i++)
+ {
+ const auto& jointName = dimNames.at(i);
+ DMP::DMPState currentPos;
+ currentPos.pos = (positionSensors.count(jointName) == 1) ? positionSensors[jointName]->position : 0.0f;
+ currentPos.vel = (velocitySensors.count(jointName) == 1) ? velocitySensors[jointName]->velocity : 0.0f;
+ currentPos.vel *= timeDuration;
+ controllerSensorData.getWriteBuffer().currentState[i] = currentPos;
+ }
+ controllerSensorData.getWriteBuffer().deltaT = timeSinceLastIteration.toSecondsDouble();
+ controllerSensorData.commitWrite();
+
+ std::vector<double> targetJointVels = rtGetControlStruct().targetJointVels;
+ for (size_t i = 0; i < dimNames.size(); ++i)
+ {
+ targets[dimNames[i]]->velocity = targetJointVels[i];
+ }
+
+
+ }
+
+ void NJointJSDMPController::learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&)
+ {
+ DMP::Vec<DMP::SampledTrajectoryV2 > trajs;
+
+ DMP::DVec ratios;
+ for (size_t i = 0; i < fileNames.size(); ++i)
+ {
+ DMP::SampledTrajectoryV2 traj;
+ traj.readFromCSVFile(fileNames.at(i));
+ traj = DMP::SampledTrajectoryV2::normalizeTimestamps(traj, 0, 1);
+ trajs.push_back(traj);
+
+ if (i == 0)
+ {
+ ratios.push_back(1.0);
+ }
+ else
+ {
+ ratios.push_back(0.0);
+ }
+ }
+ dmpPtr->learnFromTrajectories(trajs);
+ dmpPtr->setOneStepMPC(true);
+ dmpPtr->styleParas = dmpPtr->getStyleParasWithRatio(ratios);
+
+ ARMARX_INFO << "Learned DMP ... ";
+ }
+
+ void NJointJSDMPController::runDMP(const Ice::DoubleSeq& goals, double tau, const Ice::Current&)
+ {
+ targetState.clear();
+ targetState.resize(dimNames.size());
+ currentState.clear();
+ currentState.resize(dimNames.size());
+ for (size_t i = 0; i < dimNames.size(); i++)
+ {
+ const auto& jointName = dimNames.at(i);
+ DMP::DMPState currentPos;
+ currentPos.pos = (positionSensors.count(jointName) == 1) ? positionSensors[jointName]->position : 0.0f;
+ currentPos.vel = (velocitySensors.count(jointName) == 1) ? velocitySensors[jointName]->velocity : 0.0f;
+
+ currentState[i] = currentPos;
+ targetState.push_back(currentPos.pos);
+ }
+ dmpPtr->prepareExecution(goals, currentState, 1, tau);
+
+ this->goals = goals;
+ this->tau = tau;
+ canVal = timeDuration;
+ finished = false;
+ isDisturbance = false;
+
+ ARMARX_INFO << "run DMP...";
+ controllerTask->start();
+ }
+
+ void NJointJSDMPController::showMessages(const Ice::Current&)
+ {
+ }
+
+ void NJointJSDMPController::setTemporalFactor(double tau, const Ice::Current&)
+ {
+ LockGuardType guard(controllerMutex);
+ this->tau = tau;
+ }
+
+ void NJointJSDMPController::rtPreActivateController()
+ {
+ }
+
+ void NJointJSDMPController::rtPostDeactivateController()
+ {
+
+ }
+
+ void NJointJSDMPController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
+ {
+ StringVariantBaseMap datafields;
+ auto values = debugOutputData.getUpToDateReadBuffer().latestTargetVelocities;
+ for (auto& pair : values)
+ {
+ datafields[pair.first] = new Variant(pair.second);
+ }
+
+ datafields["canVal"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentCanVal);
+ datafields["mpcFactor"] = new Variant(debugOutputData.getUpToDateReadBuffer().mpcFactor);
+ debugObs->setDebugChannel("latestDMPTargetVelocities", datafields);
+ }
+
+
+ void NJointJSDMPController::onInitComponent()
+ {
+ ARMARX_INFO << "init ...";
+ controllerTask = new PeriodicTask<NJointJSDMPController>(this, &NJointJSDMPController::controllerRun, 0.2);
+ }
+
+ void NJointJSDMPController::onDisconnectComponent()
+ {
+ controllerTask->stop();
+ ARMARX_INFO << "stopped ...";
+
+ }
+
+
+
+
+}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.h
new file mode 100644
index 000000000..625058be3
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.h
@@ -0,0 +1,127 @@
+
+#pragma once
+
+
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
+#include <VirtualRobot/Robot.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
+#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
+#include <dmp/representation/dmp/umidmp.h>
+#include <ArmarXCore/core/services/tasks/RunningTask.h>
+#include <RobotAPI/interface/units/RobotUnit/NJointJointSpaceDMPController.h>
+
+namespace armarx
+{
+
+ TYPEDEF_PTRS_HANDLE(NJointJSDMPController);
+
+ TYPEDEF_PTRS_HANDLE(NJointJSDMPControllerControlData);
+ class NJointJSDMPControllerControlData
+ {
+ public:
+ std::vector<double> targetJointVels;
+ };
+
+
+ // class SimplePID
+ // {
+ // public:
+ // float Kp = 0, Kd = 0;
+ // float lastError = 0;
+ // float update(float dt, float error);
+ // };
+
+ class NJointJSDMPController :
+ public NJointControllerWithTripleBuffer<NJointJSDMPControllerControlData>,
+ public NJointJointSpaceDMPControllerInterface
+ {
+ public:
+ using ConfigPtrT = NJointJointSpaceDMPControllerConfigPtr;
+ NJointJSDMPController(NJointControllerDescriptionProviderInterfacePtr prov, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
+
+ // NJointControllerInterface interface
+ std::string getClassName(const Ice::Current&) const override;
+
+ // NJointController interface
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+
+ bool isFinished(const Ice::Current&)
+ {
+ return finished;
+ }
+
+ void learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&);
+ void setTemporalFactor(double tau, const Ice::Current&);
+
+ void runDMP(const Ice::DoubleSeq& goals, double tau, const Ice::Current&);
+
+ void showMessages(const Ice::Current&);
+
+ protected:
+ void rtPreActivateController() override;
+ void rtPostDeactivateController() override;
+
+ virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
+ private:
+
+ struct DebugBufferData
+ {
+ StringFloatDictionary latestTargetVelocities;
+ double currentCanVal;
+ double mpcFactor;
+ };
+ TripleBuffer<DebugBufferData> debugOutputData;
+
+
+ struct NJointJSDMPControllerSensorData
+ {
+ double deltaT;
+ DMP::Vec<DMP::DMPState> currentState;
+ };
+ TripleBuffer<NJointJSDMPControllerSensorData> controllerSensorData;
+
+
+ std::map<std::string, const SensorValue1DoFActuatorTorque*> torqueSensors;
+ std::map<std::string, const SensorValue1DoFGravityTorque*> gravityTorqueSensors;
+ std::map<std::string, const SensorValue1DoFActuatorPosition*> positionSensors;
+ std::map<std::string, const SensorValue1DoFActuatorVelocity*> velocitySensors;
+ std::map<std::string, ControlTarget1DoFActuatorVelocity*> targets;
+
+ IceUtil::Time last;
+
+ std::vector<double> goals;
+ DMP::UMIDMPPtr dmpPtr;
+ bool DMPAsForwardControl;
+ double timeDuration;
+ DMP::Vec<DMP::DMPState> currentState;
+ double canVal;
+ double deltaT;
+
+ double tau;
+ double finished;
+
+ // phaseStop parameters
+ double phaseL;
+ double phaseK;
+ double phaseDist0;
+ double phaseDist1;
+ double phaseKp;
+
+ bool isDisturbance;
+ std::vector<std::string> dimNames;
+ DMP::DVec targetState;
+ std::vector<double> targetVels;
+
+ mutable MutexType controllerMutex;
+ PeriodicTask<NJointJSDMPController>::pointer_type controllerTask;
+ // ManagedIceObject interface
+ protected:
+ void controllerRun();
+ void onInitComponent();
+ void onDisconnectComponent();
+
+ };
+
+} // namespace armarx
+
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJointSpaceDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJointSpaceDMPController.cpp
index e6882de92..737502f40 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJointSpaceDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJointSpaceDMPController.cpp
@@ -52,7 +52,7 @@ namespace armarx
void NJointJointSpaceDMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
{
- if (rtGetControlStruct().isStart)
+ if (rtGetControlStruct().isStart && !finished)
{
currentState.clear();
double phaseStop = 0;
--
GitLab