diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointJointSpaceDMPController.ice b/source/RobotAPI/interface/units/DMPController/NJointJointSpaceDMPController.ice
similarity index 100%
rename from source/RobotAPI/interface/units/RobotUnit/NJointJointSpaceDMPController.ice
rename to source/RobotAPI/interface/units/DMPController/NJointJointSpaceDMPController.ice
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice b/source/RobotAPI/interface/units/DMPController/NJointTaskSpaceDMPController.ice
similarity index 63%
rename from source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
rename to source/RobotAPI/interface/units/DMPController/NJointTaskSpaceDMPController.ice
index d5fc252cc50d8fbfa34e885da5cda920e4c82ecd..1ea4be9012b590015af944a8e71ecfd708413297 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
+++ b/source/RobotAPI/interface/units/DMPController/NJointTaskSpaceDMPController.ice
@@ -52,10 +52,13 @@ module armarx
double phaseK = 10;
double phaseDist0 = 50;
double phaseDist1 = 10;
- double phaseKp = 1;
+ double phaseKpPos = 1;
+ double phaseKpOri = 0.1;
double timeDuration = 10;
+ double posToOriRatio = 100;
+
// velocity controller configuration
string nodeSetName = "";
string tcpName = "";
@@ -78,5 +81,48 @@ module armarx
void setTorqueKp(StringFloatDictionary torqueKp);
void setNullspaceJointVelocities(StringFloatDictionary nullspaceJointVelocities);
};
+
+ class NJointCCDMPControllerConfig extends NJointControllerConfig
+ {
+
+ // dmp configuration
+ int kernelSize = 100;
+ double tau = 1;
+ int baseMode = 1;
+ int dmpNum = 2;
+
+ // phaseStop technique
+ double phaseL = 10;
+ double phaseK = 10;
+ double phaseDist0 = 50;
+ double phaseDist1 = 10;
+ double phaseKpPos = 1;
+ double phaseKpOri = 0.1;
+
+ // misc
+ double timeDuration = 10;
+ double posToOriRatio = 100;
+
+ // velocity controller configuration
+ string nodeSetName = "";
+ string tcpName = "";
+ NJointTaskSpaceDMPControllerMode::CartesianSelection mode = NJointTaskSpaceDMPControllerMode::eAll;
+
+ };
+
+ interface NJointCCDMPControllerInterface extends NJointControllerInterface
+ {
+ void learnDMPFromFiles(int dmpId, Ice::StringSeq trajfiles);
+ bool isFinished();
+ void runDMP();
+
+ void setTemporalFactor(double tau);
+ void setViaPoints(int dmpId, double canVal, Ice::DoubleSeq point);
+ void setGoals(int dmpId, Ice::DoubleSeq goals);
+
+ void setControllerTarget(float avoidJointLimitsKp, NJointTaskSpaceDMPControllerMode::CartesianSelection mode);
+ void setTorqueKp(StringFloatDictionary torqueKp);
+ void setNullspaceJointVelocities(StringFloatDictionary nullspaceJointVelocities);
+ };
};
#endif
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.h
index 625058be3fa1a6d22047392e6cd4dce582590afc..16b456700cc05f175718e381eb4e7683378f3d8f 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.h
@@ -9,7 +9,7 @@
#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>
+#include <RobotAPI/interface/units/DMPController/NJointJointSpaceDMPController.h>
namespace armarx
{
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJointSpaceDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJointSpaceDMPController.cpp
index 737502f40ce26d64f24f8740ed7031bedcb23de1..0626c1976c4bfe8f6b5de2eed7ba0298006a65f2 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJointSpaceDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJointSpaceDMPController.cpp
@@ -121,7 +121,11 @@ namespace armarx
double vel = mpcFactor * vel0 + (1 - mpcFactor) * vel1;
targets[jointName]->velocity = finished ? 0.0f : vel;
+ std::string targetVelstr = jointName + "_targetvel";
+ std::string targetStatestr = jointName + "_dmpTarget";
debugOutputData.getWriteBuffer().latestTargetVelocities[jointName] = vel;
+ debugOutputData.getWriteBuffer().dmpTargetState[jointName] = targetState[i];
+
}
}
@@ -166,8 +170,6 @@ namespace armarx
}
dmpPtr->learnFromTrajectories(trajs);
dmpPtr->setOneStepMPC(true);
-
-
dmpPtr->styleParas = dmpPtr->getStyleParasWithRatio(ratios);
ARMARX_INFO << "Learned DMP ... ";
@@ -230,6 +232,12 @@ namespace armarx
datafields[pair.first] = new Variant(pair.second);
}
+ auto valuesst = debugOutputData.getUpToDateReadBuffer().dmpTargetState;
+ for (auto& pair : valuesst)
+ {
+ 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);
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJointSpaceDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJointSpaceDMPController.h
index 15e69e65aa13cd065d82fe79630372a5aa4dcb25..9d7e6a53edc234f521373d4bb7cf3f0106df8f1a 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJointSpaceDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJointSpaceDMPController.h
@@ -8,7 +8,7 @@
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
#include <dmp/representation/dmp/umidmp.h>
-#include <RobotAPI/interface/units/RobotUnit/NJointJointSpaceDMPController.h>
+#include <RobotAPI/interface/units/DMPController/NJointJointSpaceDMPController.h>
namespace armarx
{
@@ -69,6 +69,7 @@ namespace armarx
struct DebugBufferData
{
StringFloatDictionary latestTargetVelocities;
+ StringFloatDictionary dmpTargetState;
double currentCanVal;
double mpcFactor;
};
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9480a8c0364636520ff5bc9e0c6a3c7e41a3b0d0
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
@@ -0,0 +1,504 @@
+#include "NJointTSDMPController.h"
+
+namespace armarx
+{
+ NJointControllerRegistration<NJointTSDMPController> registrationControllerNJointTSDMPController("NJointTSDMPController");
+
+ NJointTSDMPController::NJointTSDMPController(armarx::NJointControllerDescriptionProviderInterfacePtr prov, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
+ {
+ cfg = NJointTaskSpaceDMPControllerConfigPtr::dynamicCast(config);
+ ARMARX_CHECK_EXPRESSION(prov);
+ RobotUnitPtr robotUnit = RobotUnitPtr::dynamicCast(prov);
+ ARMARX_CHECK_EXPRESSION(robotUnit);
+ ARMARX_CHECK_EXPRESSION(!cfg->nodeSetName.empty());
+ VirtualRobot::RobotNodeSetPtr rns = robotUnit->getRtRobot()->getRobotNodeSet(cfg->nodeSetName);
+
+ ARMARX_CHECK_EXPRESSION_W_HINT(rns, cfg->nodeSetName);
+ for (size_t i = 0; i < rns->getSize(); ++i)
+ {
+ std::string jointName = rns->getNode(i)->getName();
+ ControlTargetBase* ct = prov->getControlTarget(jointName, ControlModes::Velocity1DoF);
+ const SensorValueBase* sv = prov->getSensorValue(jointName);
+ targets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
+
+ const SensorValue1DoFActuatorTorque* torqueSensor = sv->asA<SensorValue1DoFActuatorTorque>();
+ const SensorValue1DoFGravityTorque* gravityTorqueSensor = sv->asA<SensorValue1DoFGravityTorque>();
+ if (!torqueSensor)
+ {
+ ARMARX_WARNING << "No Torque sensor available for " << jointName;
+ }
+ if (!gravityTorqueSensor)
+ {
+ ARMARX_WARNING << "No Gravity Torque sensor available for " << jointName;
+ }
+
+ torqueSensors.push_back(torqueSensor);
+ gravityTorqueSensors.push_back(gravityTorqueSensor);
+ };
+
+ tcp = (cfg->tcpName.empty()) ? rns->getTCP() : robotUnit->getRtRobot()->getRobotNode(cfg->tcpName);
+ ARMARX_CHECK_EXPRESSION_W_HINT(tcp, cfg->tcpName);
+
+ // set tcp controller
+ tcpController.reset(new CartesianVelocityController(rns, tcp));
+ nodeSetName = cfg->nodeSetName;
+ torquePIDs.resize(tcpController->rns->getSize(), pidController());
+
+
+
+ // set DMP
+ isDisturbance = false;
+
+ dmpPtr.reset(new UMIDMP(cfg->kernelSize, cfg->DMPKd, cfg->baseMode, cfg->tau));
+ timeDuration = cfg->timeDuration;
+ canVal = timeDuration;
+ finished = false;
+
+ phaseL = cfg->phaseL;
+ phaseK = cfg->phaseK;
+ phaseDist0 = cfg->phaseDist0;
+ phaseDist1 = cfg->phaseDist1;
+ phaseKpPos = cfg->phaseKpPos;
+ phaseKpOri = cfg->phaseKpOri;
+
+ posToOriRatio = cfg->posToOriRatio;
+
+ // initialize tcp position and orientation
+ Eigen::Matrix4f pose = tcp->getPoseInRootFrame();
+ tcpPosition(0) = pose(0, 3);
+ tcpPosition(1) = pose(1, 3);
+ tcpPosition(2) = pose(2, 3);
+ tcpOrientation = VirtualRobot::MathTools::eigen4f2rpy(pose);
+
+
+ NJointTSDMPControllerSensorData initSensorData;
+ initSensorData.deltaT = 0;
+ initSensorData.currentTime = 0;
+ initSensorData.currentState.resize(6);
+ controllerSensorData.reinitAllBuffers(initSensorData);
+
+
+ targetVels.resize(6);
+ NJointTSDMPControllerControlData initData;
+ initData.targetTSVel.resize(6);
+ for (size_t i = 0; i < 6; ++i)
+ {
+ initData.targetTSVel[i] = 0;
+ }
+ initData.nullspaceJointVelocities.resize(tcpController->rns->getSize(), 0);
+ initData.torqueKp.resize(tcpController->rns->getSize(), 0);
+ initData.torqueKd.resize(tcpController->rns->getSize(), 0);
+ initData.mode = ModeFromIce(cfg->mode);
+ reinitTripleBuffer(initData);
+ }
+
+ std::string NJointTSDMPController::getClassName(const Ice::Current&) const
+ {
+ return "NJointTSDMPController";
+ }
+
+ void NJointTSDMPController::controllerRun()
+ {
+ double mpcFactor = 1;
+ double error = 0;
+ double phaseStop = 0;
+ double posError = 0;
+ double oriError = 0;
+ double deltaT = 0;
+ if (canVal > 1e-8)
+ {
+ if (!controllerSensorData.updateReadBuffer())
+ {
+ return;
+ }
+ deltaT = controllerSensorData.getReadBuffer().deltaT;
+ currentState = controllerSensorData.getReadBuffer().currentState;
+
+
+ for (size_t i = 0; i < 3; ++i)
+ {
+ posError += pow(currentState[i].pos - targetState[i], 2);
+ }
+ for (size_t i = 0; i < 3; ++i)
+ {
+ oriError += pow(currentState[i + 3].pos - targetState[i + 3], 2);
+ }
+
+ posError = sqrt(posError);
+ oriError = sqrt(oriError);
+
+ error = posError + posToOriRatio * oriError;
+ double dmpDeltaT = deltaT / timeDuration;
+
+ double phaseDist;
+ if (isDisturbance)
+ {
+ phaseDist = phaseDist1;
+ }
+ else
+ {
+ phaseDist = phaseDist0;
+ }
+
+ phaseStop = 0;//phaseL / (1 + exp(-phaseK * (error - phaseDist)));
+ mpcFactor = 1 - (phaseStop / phaseL);
+
+ if (mpcFactor < 0.1)
+ {
+ isDisturbance = true;
+ }
+
+ if (mpcFactor > 0.9)
+ {
+ isDisturbance = false;
+ }
+
+
+ double tau = dmpPtr->getTemporalFactor();
+ canVal -= 1 / tau * deltaT * 1 / (1 + phaseStop) ;
+ currentState = dmpPtr->calculateDirectlyVelocity(currentState, canVal / timeDuration, dmpDeltaT, targetState);
+
+
+
+ double vel0, vel1;
+ for (size_t i = 0; i < 3; i++)
+ {
+ vel0 = currentState[i].vel / timeDuration;
+ vel1 = phaseKpPos * (targetState[i] - tcpPosition[i]);
+ targetVels[i] = mpcFactor * vel0 + (1 - mpcFactor) * vel1;
+ }
+ for (size_t i = 0; i < 3; i++)
+ {
+ vel0 = currentState[i + 3].vel / timeDuration;
+ vel1 = phaseKpOri * (targetState[i + 3] - tcpOrientation[i]);
+ targetVels[i + 3] = mpcFactor * vel0 + (1 - mpcFactor) * vel1;
+ }
+ }
+ else
+ {
+ finished = true;
+
+ for (size_t i = 0; i < targetVels.size(); ++i)
+ {
+ targetVels[i] = 0;
+ }
+ }
+
+ debugOutputData.getWriteBuffer().latestTargetVelocities["x_vel"] = targetVels[0];
+ debugOutputData.getWriteBuffer().latestTargetVelocities["y_vel"] = targetVels[1];
+ debugOutputData.getWriteBuffer().latestTargetVelocities["z_vel"] = targetVels[2];
+ debugOutputData.getWriteBuffer().latestTargetVelocities["roll_vel"] = targetVels[3];
+ debugOutputData.getWriteBuffer().latestTargetVelocities["pitch_vel"] = targetVels[4];
+ debugOutputData.getWriteBuffer().latestTargetVelocities["yaw_vel"] = targetVels[5];
+ debugOutputData.getWriteBuffer().dmpTargets["dmp_x"] = targetState[0];
+ debugOutputData.getWriteBuffer().dmpTargets["dmp_y"] = targetState[1];
+ debugOutputData.getWriteBuffer().dmpTargets["dmp_z"] = targetState[2];
+ debugOutputData.getWriteBuffer().dmpTargets["dmp_roll"] = targetState[3];
+ debugOutputData.getWriteBuffer().dmpTargets["dmp_pitch"] = targetState[4];
+ debugOutputData.getWriteBuffer().dmpTargets["dmp_yaw"] = targetState[5];
+
+ debugOutputData.getWriteBuffer().currentCanVal = canVal;
+ debugOutputData.getWriteBuffer().mpcFactor = mpcFactor;
+ debugOutputData.getWriteBuffer().error = error;
+ debugOutputData.getWriteBuffer().phaseStop = phaseStop;
+ debugOutputData.getWriteBuffer().posError = posError;
+ debugOutputData.getWriteBuffer().oriError = oriError;
+ debugOutputData.getWriteBuffer().deltaT = deltaT;
+
+
+ debugOutputData.commitWrite();
+
+ LockGuardType guard {controlDataMutex};
+ getWriterControlStruct().targetTSVel = targetVels;
+ writeControlStruct();
+ }
+
+
+ void NJointTSDMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ {
+ Eigen::Matrix4f pose = tcp->getPoseInRootFrame();
+ Eigen::Vector3f currPosition;
+ Eigen::Vector3f currOrientation = VirtualRobot::MathTools::eigen4f2rpy(pose);
+ currPosition(0) = pose(0, 3);
+ currPosition(1) = pose(1, 3);
+ currPosition(2) = pose(2, 3);
+
+ double deltaT = timeSinceLastIteration.toSecondsDouble();
+
+ Eigen::Vector3f posVel;
+ Eigen::Vector3f oriVel;
+
+ if (deltaT == 0)
+ {
+ posVel << 0, 0 , 0;
+ oriVel << 0, 0 , 0;
+ }
+ else
+ {
+ posVel = (currPosition - tcpPosition) / deltaT;
+ oriVel = (currOrientation - tcpOrientation) / deltaT;
+ }
+
+ tcpPosition = currPosition;
+ tcpOrientation = currOrientation;
+
+ for (size_t i = 0; i < 3; i++)
+ {
+ DMP::DMPState currentPos;
+ currentPos.pos = tcpPosition(i);
+ currentPos.vel = posVel(i) * timeDuration;
+ controllerSensorData.getWriteBuffer().currentState[i] = currentPos;
+ }
+
+ for (size_t i = 0; i < 3; i++)
+ {
+ DMP::DMPState currentPos;
+ currentPos.pos = tcpOrientation(i);
+ currentPos.vel = oriVel(i) * timeDuration;
+ controllerSensorData.getWriteBuffer().currentState[i + 3] = currentPos;
+ }
+
+ controllerSensorData.getWriteBuffer().deltaT = deltaT;
+ controllerSensorData.getWriteBuffer().currentTime += deltaT;
+
+ controllerSensorData.commitWrite();
+
+
+ // cartesian vel controller
+ Eigen::VectorXf x;
+ auto mode = rtGetControlStruct().mode;
+
+ if (mode == VirtualRobot::IKSolver::All)
+ {
+ x.resize(6);
+ for (size_t i = 0; i < 6; i++)
+ {
+ x(i) = rtGetControlStruct().targetTSVel[i];
+ }
+ }
+ else if (mode == VirtualRobot::IKSolver::Position)
+ {
+ x.resize(3);
+ for (size_t i = 0; i < 3; i++)
+ {
+ x(i) = rtGetControlStruct().targetTSVel[i];
+ }
+
+ }
+ else if (mode == VirtualRobot::IKSolver::Orientation)
+ {
+ x.resize(3);
+ for (size_t i = 0; i < 3; i++)
+ {
+ x(i) = rtGetControlStruct().targetTSVel[i + 3];
+ }
+ }
+ else
+ {
+ // No mode has been set
+ return;
+ }
+
+ Eigen::VectorXf jnv = Eigen::VectorXf::Zero(tcpController->rns->getSize());
+ float jointLimitAvoidanceKp = rtGetControlStruct().avoidJointLimitsKp;
+ if (jointLimitAvoidanceKp > 0)
+ {
+ jnv += jointLimitAvoidanceKp * tcpController->calculateJointLimitAvoidance();
+ }
+ for (size_t i = 0; i < tcpController->rns->getSize(); i++)
+ {
+ jnv(i) += rtGetControlStruct().nullspaceJointVelocities.at(i);
+ if (torqueSensors.at(i) && gravityTorqueSensors.at(i) && rtGetControlStruct().torqueKp.at(i) != 0)
+ {
+ torquePIDs.at(i).Kp = rtGetControlStruct().torqueKp.at(i);
+ torquePIDs.at(i).Kd = rtGetControlStruct().torqueKd.at(i);
+ jnv(i) += torquePIDs.at(i).update(deltaT, torqueSensors.at(i)->torque - gravityTorqueSensors.at(i)->gravityTorque);
+ }
+ else
+ {
+ torquePIDs.at(i).lastError = 0;
+ }
+ }
+
+ Eigen::VectorXf jointTargetVelocities = tcpController->calculate(x, jnv, mode);
+ for (size_t i = 0; i < targets.size(); ++i)
+ {
+ targets.at(i)->velocity = jointTargetVelocities(i);
+ }
+ }
+
+
+ void NJointTSDMPController::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 NJointTSDMPController::setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&)
+ {
+ ViaPoint viaPoint(u, viapoint);
+
+ LockGuardType guard(controllerMutex);
+ dmpPtr->setViaPoint(viaPoint);
+ }
+
+ void NJointTSDMPController::setGoals(const Ice::DoubleSeq& goals, const Ice::Current& ice)
+ {
+ setViaPoints(dmpPtr->getUMin(), goals, ice);
+ }
+
+ void NJointTSDMPController::setControllerTarget(Ice::Float avoidJointLimitsKp, NJointTaskSpaceDMPControllerMode::CartesianSelection mode, const Ice::Current&)
+ {
+ LockGuardType guard {controlDataMutex};
+ getWriterControlStruct().avoidJointLimitsKp = avoidJointLimitsKp;
+ getWriterControlStruct().mode = ModeFromIce(mode);
+ writeControlStruct();
+ }
+
+ VirtualRobot::IKSolver::CartesianSelection NJointTSDMPController::ModeFromIce(const NJointTaskSpaceDMPControllerMode::CartesianSelection mode)
+ {
+ if (mode == NJointTaskSpaceDMPControllerMode::CartesianSelection::ePosition)
+ {
+ return VirtualRobot::IKSolver::CartesianSelection::Position;
+ }
+ if (mode == NJointTaskSpaceDMPControllerMode::CartesianSelection::eOrientation)
+ {
+ return VirtualRobot::IKSolver::CartesianSelection::Orientation;
+ }
+ if (mode == NJointTaskSpaceDMPControllerMode::CartesianSelection::eAll)
+ {
+ return VirtualRobot::IKSolver::CartesianSelection::All;
+ }
+ ARMARX_ERROR_S << "invalid mode " << mode;
+ return (VirtualRobot::IKSolver::CartesianSelection)0;
+ }
+
+
+ void NJointTSDMPController::setTorqueKp(const StringFloatDictionary& torqueKp, const Ice::Current&)
+ {
+ LockGuardType guard {controlDataMutex};
+ for (size_t i = 0; i < tcpController->rns->getSize(); i++)
+ {
+ getWriterControlStruct().torqueKp.at(i) = torqueKp.at(tcpController->rns->getNode(i)->getName());
+ }
+ writeControlStruct();
+ }
+
+ void NJointTSDMPController::setNullspaceJointVelocities(const StringFloatDictionary& nullspaceJointVelocities, const Ice::Current&)
+ {
+ LockGuardType guard {controlDataMutex};
+ for (size_t i = 0; i < tcpController->rns->getSize(); i++)
+ {
+ getWriterControlStruct().nullspaceJointVelocities.at(i) = nullspaceJointVelocities.at(tcpController->rns->getNode(i)->getName());
+ }
+ writeControlStruct();
+ }
+
+ void NJointTSDMPController::runDMP(const Ice::DoubleSeq& goals, double tau, const Ice::Current&)
+ {
+ currentState.clear();
+ targetState.clear();
+ for (size_t i = 0; i < 3; i++)
+ {
+ DMP::DMPState currentPos;
+ currentPos.pos = tcpPosition(i);
+ currentPos.vel = 0;
+ currentState.push_back(currentPos);
+ targetState.push_back(currentPos.pos);
+ }
+ for (size_t i = 0; i < 3; i++)
+ {
+ DMP::DMPState currentPos;
+ currentPos.pos = tcpOrientation(i);
+ currentPos.vel = 0;
+ currentState.push_back(currentPos);
+ targetState.push_back(currentPos.pos);
+ }
+ dmpPtr->prepareExecution(goals, currentState, 1, tau);
+ finished = false;
+ dmpPtr->setTemporalFactor(tau);
+
+ ARMARX_INFO << "run DMP";
+ controllerTask->start();
+
+ }
+
+ void NJointTSDMPController::setTemporalFactor(double tau, const Ice::Current&)
+ {
+ LockGuardType guard(controllerMutex);
+ dmpPtr->setTemporalFactor(tau);
+ }
+
+ void NJointTSDMPController::rtPreActivateController()
+ {
+ }
+
+ void NJointTSDMPController::rtPostDeactivateController()
+ {
+
+ }
+
+ void NJointTSDMPController::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);
+ }
+
+ auto dmpTargets = debugOutputData.getUpToDateReadBuffer().dmpTargets;
+ for (auto& pair : dmpTargets)
+ {
+ 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);
+ datafields["phaseStop"] = new Variant(debugOutputData.getUpToDateReadBuffer().phaseStop);
+ datafields["posError"] = new Variant(debugOutputData.getUpToDateReadBuffer().posError);
+ datafields["oriError"] = new Variant(debugOutputData.getUpToDateReadBuffer().oriError);
+ datafields["deltaT"] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaT);
+
+ debugObs->setDebugChannel("DMPController", datafields);
+ }
+
+ void NJointTSDMPController::onInitComponent()
+ {
+ ARMARX_INFO << "init ...";
+ controllerTask = new PeriodicTask<NJointTSDMPController>(this, &NJointTSDMPController::controllerRun, 1);
+ }
+
+ void NJointTSDMPController::onDisconnectComponent()
+ {
+ controllerTask->stop();
+ ARMARX_INFO << "stopped ...";
+ }
+
+
+
+}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
new file mode 100644
index 0000000000000000000000000000000000000000..ec8d216b992dd30b13edfa6f49b41ad987f897b3
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
@@ -0,0 +1,158 @@
+
+#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 <VirtualRobot/IK/DifferentialIK.h>
+#include <dmp/representation/dmp/umidmp.h>
+#include <RobotAPI/interface/units/DMPController/NJointTaskSpaceDMPController.h>
+#include <RobotAPI/libraries/core/CartesianVelocityController.h>
+
+
+using namespace DMP;
+namespace armarx
+{
+
+
+ TYPEDEF_PTRS_HANDLE(NJointTSDMPController);
+ TYPEDEF_PTRS_HANDLE(NJointTSDMPControllerControlData);
+
+ typedef std::pair<double, DVec > ViaPoint;
+ typedef std::vector<ViaPoint > ViaPointsSet;
+ class NJointTSDMPControllerControlData
+ {
+ public:
+ std::vector<float> targetTSVel;
+ // cartesian velocity control data
+ std::vector<float> nullspaceJointVelocities;
+ float avoidJointLimitsKp = 0;
+ std::vector<float> torqueKp;
+ std::vector<float> torqueKd;
+ VirtualRobot::IKSolver::CartesianSelection mode = VirtualRobot::IKSolver::All;
+ };
+
+ class pidController
+ {
+ public:
+ float Kp = 0, Kd = 0;
+ float lastError = 0;
+ float update(float dt, float error)
+ {
+ float derivative = (error - lastError) / dt;
+ float retVal = Kp * error + Kd * derivative;
+ lastError = error;
+ return retVal;
+ }
+ };
+
+ class NJointTSDMPController :
+ public NJointControllerWithTripleBuffer<NJointTSDMPControllerControlData>,
+ public NJointTaskSpaceDMPControllerInterface
+ {
+ public:
+ using ConfigPtrT = NJointTaskSpaceDMPControllerConfigPtr;
+ NJointTSDMPController(NJointControllerDescriptionProviderInterfacePtr prov, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
+
+ // NJointControllerInterface interface
+ std::string getClassName(const Ice::Current&) const;
+
+ // NJointController interface
+
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
+
+ // NJointTSDMPControllerInterface interface
+ void learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&);
+ bool isFinished(const Ice::Current&)
+ {
+ return finished;
+ }
+
+ void runDMP(const Ice::DoubleSeq& goals, Ice::Double tau, const Ice::Current&);
+ void setTemporalFactor(Ice::Double tau, const Ice::Current&);
+ void setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&);
+ void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&);
+
+ void setControllerTarget(Ice::Float avoidJointLimitsKp, NJointTaskSpaceDMPControllerMode::CartesianSelection mode, const Ice::Current&);
+ void setTorqueKp(const StringFloatDictionary& torqueKp, const Ice::Current&);
+ void setNullspaceJointVelocities(const StringFloatDictionary& nullspaceJointVelocities, const Ice::Current&);
+ protected:
+ void rtPreActivateController() override;
+ void rtPostDeactivateController() override;
+ VirtualRobot::IKSolver::CartesianSelection ModeFromIce(const NJointTaskSpaceDMPControllerMode::CartesianSelection mode);
+ virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
+
+ void onInitComponent();
+ void onDisconnectComponent();
+ void controllerRun();
+
+ private:
+ struct DebugBufferData
+ {
+ StringFloatDictionary latestTargetVelocities;
+ StringFloatDictionary dmpTargets;
+ double currentCanVal;
+ double mpcFactor;
+ double error;
+ double phaseStop;
+ double posError;
+ double oriError;
+ double deltaT;
+ };
+
+ TripleBuffer<DebugBufferData> debugOutputData;
+
+ struct NJointTSDMPControllerSensorData
+ {
+ double currentTime;
+ double deltaT;
+ DMP::Vec<DMP::DMPState> currentState;
+ };
+ TripleBuffer<NJointTSDMPControllerSensorData> controllerSensorData;
+
+
+ std::vector<const SensorValue1DoFActuatorTorque*> torqueSensors;
+ std::vector<const SensorValue1DoFGravityTorque*> gravityTorqueSensors;
+ std::vector<ControlTarget1DoFActuatorVelocity*> targets;
+
+ // velocity ik controller parameters
+ std::vector<pidController> torquePIDs;
+ CartesianVelocityControllerPtr tcpController;
+ std::string nodeSetName;
+
+ // dmp parameters
+ UMIDMPPtr dmpPtr;
+ double timeDuration;
+ double canVal;
+ bool finished;
+ double tau;
+ ViaPointsSet viaPoints;
+ bool isDisturbance;
+
+
+ // phaseStop parameters
+ double phaseL;
+ double phaseK;
+ double phaseDist0;
+ double phaseDist1;
+ double phaseKpPos;
+ double phaseKpOri;
+
+ double posToOriRatio;
+
+ std::vector<float> targetVels;
+
+ NJointTaskSpaceDMPControllerConfigPtr cfg;
+ VirtualRobot::RobotNodePtr tcp;
+ Eigen::Vector3f tcpPosition;
+ Eigen::Vector3f tcpOrientation;
+ DMP::Vec<DMP::DMPState> currentState;
+ DMP::DVec targetState;
+ mutable MutexType controllerMutex;
+ PeriodicTask<NJointTSDMPController>::pointer_type controllerTask;
+ };
+
+} // namespace armarx
+
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceDMPController.cpp
index a798bf35c117202ed3b04435467eb420a659d783..f7a2de6fee95c7a188cd816158d88836c974e9c2 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceDMPController.cpp
@@ -67,7 +67,8 @@ namespace armarx
phaseK = cfg->phaseK;
phaseDist0 = cfg->phaseDist0;
phaseDist1 = cfg->phaseDist1;
- phaseKp = cfg->phaseKp;
+ phaseKpPos = cfg->phaseKpPos;
+ phaseKpOri = cfg->phaseKpOri;
// initialize tcp position and orientation
Eigen::Matrix4f pose = tcp->getPoseInRootFrame();
@@ -76,6 +77,8 @@ namespace armarx
tcpPosition(2) = pose(2, 3);
tcpOrientation = VirtualRobot::MathTools::eigen4f2rpy(pose);
+
+ posToOriRatio = cfg->posToOriRatio;
}
std::string NJointTaskSpaceDMPController::getClassName(const Ice::Current&) const
@@ -85,7 +88,7 @@ namespace armarx
void NJointTaskSpaceDMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
{
- if (rtGetControlStruct().isStart)
+ if (rtGetControlStruct().isStart && !finished)
{
UMIDMPPtr dmpPtr = rtGetControlStruct().dmpPtr;
@@ -120,6 +123,9 @@ namespace armarx
double phaseStop = 0;
double error = 0;
currentState.clear();
+
+ double posError = 0;
+ double oriError = 0;
for (size_t i = 0; i < 3; i++)
{
DMP::DMPState currentPos;
@@ -127,7 +133,7 @@ namespace armarx
currentPos.vel = posVel(i) * timeDuration;
currentState.push_back(currentPos);
- error += pow(currentPos.pos - targetState[i], 2);
+ posError += pow(currentPos.pos - targetState[i], 2);
}
for (size_t i = 0; i < 3; i++)
@@ -137,10 +143,11 @@ namespace armarx
currentPos.vel = oriVel(i) * timeDuration;
currentState.push_back(currentPos);
- error += pow(currentPos.pos - targetState[i + 3], 2);
+ oriError += pow(currentPos.pos - targetState[i + 3], 2);
}
- error = sqrt(error);
+
+ error = sqrt(posError + posToOriRatio * oriError);
double phaseDist;
if (isDisturbance)
@@ -187,14 +194,14 @@ namespace armarx
for (size_t i = 0; i < 3; i++)
{
vel0 = currentState[i].vel / timeDuration;
- vel1 = phaseKp * (targetState[i] - tcpPosition[i]);
+ vel1 = phaseKpPos * (targetState[i] - tcpPosition[i]);
x(i) = mpcFactor * vel0 + (1 - mpcFactor) * vel1;
}
for (size_t i = 0; i < 3; i++)
{
vel0 = currentState[i + 3].vel / timeDuration;
- vel1 = phaseKp * (targetState[i + 3] - tcpOrientation[i]);
+ vel1 = phaseKpOri * (targetState[i + 3] - tcpOrientation[i]);
x(i + 3) = mpcFactor * vel0 + (1 - mpcFactor) * vel1;
}
@@ -216,7 +223,7 @@ namespace armarx
for (size_t i = 0; i < 3; i++)
{
vel0 = currentState[i].vel / timeDuration;
- vel1 = phaseKp * (targetState[i] - tcpPosition[i]);
+ vel1 = phaseKpPos * (targetState[i] - tcpPosition[i]);
x(i) = mpcFactor * vel0 + (1 - mpcFactor) * vel1;
}
@@ -234,7 +241,7 @@ namespace armarx
for (size_t i = 0; i < 3; i++)
{
vel0 = currentState[i + 3].vel / timeDuration;
- vel1 = phaseKp * (targetState[i + 3] - tcpOrientation[i]);
+ vel1 = phaseKpOri * (targetState[i + 3] - tcpOrientation[i]);
x(i) = mpcFactor * vel0 + (1 - mpcFactor) * vel1;
}
@@ -281,6 +288,13 @@ namespace armarx
targets.at(i)->velocity = jointTargetVelocities(i);
}
}
+ else
+ {
+ for (size_t i = 0; i < targets.size(); ++i)
+ {
+ targets.at(i)->velocity = 0;
+ }
+ }
}
@@ -354,7 +368,7 @@ namespace armarx
return (VirtualRobot::IKSolver::CartesianSelection)0;
}
- void NJointTaskSpaceDMPController::onPublish(const SensorAndControl &, const DebugDrawerInterfacePrx &, const DebugObserverInterfacePrx &debugObs)
+ void NJointTaskSpaceDMPController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
{
StringVariantBaseMap datafields;
auto values = debugOutputData.getUpToDateReadBuffer().latestTargetVelocities;
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceDMPController.h
index d133d81bb3955192594d5216ebce264d66c603b6..d45c8ce03449c3a2c1694269563a4618a657220c 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceDMPController.h
@@ -8,7 +8,7 @@
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
#include <VirtualRobot/IK/DifferentialIK.h>
#include <dmp/representation/dmp/umidmp.h>
-#include <RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.h>
+#include <RobotAPI/interface/units/DMPController/NJointTaskSpaceDMPController.h>
#include <RobotAPI/libraries/core/CartesianVelocityController.h>
@@ -120,7 +120,9 @@ namespace armarx
double phaseK;
double phaseDist0;
double phaseDist1;
- double phaseKp;
+ double phaseKpPos;
+ double phaseKpOri;
+ double posToOriRatio;
bool isDisturbance;