diff --git a/source/RobotAPI/interface/units/DMPController/NJointJointSpaceDMPController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointJointSpaceDMPController.ice
similarity index 100%
rename from source/RobotAPI/interface/units/DMPController/NJointJointSpaceDMPController.ice
rename to source/RobotAPI/interface/units/RobotUnit/NJointJointSpaceDMPController.ice
diff --git a/source/RobotAPI/interface/units/DMPController/NJointTaskSpaceDMPController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
similarity index 96%
rename from source/RobotAPI/interface/units/DMPController/NJointTaskSpaceDMPController.ice
rename to source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
index 1ea4be9012b590015af944a8e71ecfd708413297..850ab231f12ca392810c78c0af412454c8a4a28f 100644
--- a/source/RobotAPI/interface/units/DMPController/NJointTaskSpaceDMPController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
@@ -86,6 +86,7 @@ module armarx
{
// dmp configuration
+ float DMPKd = 20;
int kernelSize = 100;
double tau = 1;
int baseMode = 1;
@@ -100,7 +101,8 @@ module armarx
double phaseKpOri = 0.1;
// misc
- double timeDuration = 10;
+ Ice::DoubleSeq timeDurations;
+ Ice::StringSeq dmpTypes;
double posToOriRatio = 100;
// velocity controller configuration
@@ -116,7 +118,7 @@ module armarx
bool isFinished();
void runDMP();
- void setTemporalFactor(double tau);
+ void setTemporalFactor(int dmpId, double tau);
void setViaPoints(int dmpId, double canVal, Ice::DoubleSeq point);
void setGoals(int dmpId, Ice::DoubleSeq goals);
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt b/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt
index 76755149a0b2879963a7faaff337d778e495ced9..ef4b0ffffec80572ab5e6b23a380a61e24e28c84 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt
@@ -40,12 +40,17 @@ if (DMP_FOUND)
DMPController/NJointJointSpaceDMPController.h
DMPController/NJointTaskSpaceDMPController.h
DMPController/NJointJSDMPController.h
+ DMPController/NJointTSDMPController.h
+ DMPController/NJointCCDMPController.h
)
list(APPEND LIB_FILES
DMPController/NJointJointSpaceDMPController.cpp
DMPController/NJointTaskSpaceDMPController.cpp
DMPController/NJointJSDMPController.cpp
+ DMPController/NJointTSDMPController.cpp
+ DMPController/NJointCCDMPController.cpp
+
)
list(APPEND LIBS ${DMP_LIBRARIES})
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..34c384badfbc7b1a56d372a4216a84b40aedffcd
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.cpp
@@ -0,0 +1,562 @@
+#include "NJointCCDMPController.h"
+
+namespace armarx
+{
+ NJointControllerRegistration<NJointCCDMPController> registrationControllerNJointCCDMPController("NJointCCDMPController");
+
+ NJointCCDMPController::NJointCCDMPController(armarx::NJointControllerDescriptionProviderInterfacePtr prov, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
+ {
+ cfg = NJointCCDMPControllerConfigPtr::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;
+
+ dmpPtrList.resize(cfg->dmpNum);
+ canVals.resize(cfg->dmpNum);
+ timeDurations = cfg->timeDurations;
+ dmpTypes = cfg->dmpTypes;
+ for (size_t i = 0; i < dmpPtrList.size(); ++i)
+ {
+ dmpPtrList[i].reset(new UMIDMP(cfg->kernelSize, cfg->DMPKd, cfg->baseMode, cfg->tau));
+ canVals[i] = timeDurations[i];
+ }
+ 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);
+
+
+ NJointCCDMPControllerSensorData initSensorData;
+ initSensorData.deltaT = 0;
+ initSensorData.currentTime = 0;
+ initSensorData.currentPose = pose;
+ controllerSensorData.reinitAllBuffers(initSensorData);
+
+
+ currentStates.resize(cfg->dmpNum);
+ targetSubStates.resize(cfg->dmpNum);
+ targetState.resize(6);
+
+ targetVels.resize(6);
+ NJointCCDMPControllerControlData 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 NJointCCDMPController::getClassName(const Ice::Current&) const
+ {
+ return "NJointCCDMPController";
+ }
+
+ void NJointCCDMPController::controllerRun()
+ {
+ if (!controllerSensorData.updateReadBuffer())
+ {
+ return;
+ }
+ double deltaT = controllerSensorData.getReadBuffer().deltaT;
+ Eigen::Matrix4f currentPose = controllerSensorData.getReadBuffer().currentPose;
+ Eigen::Vector3f currentPosition;
+ currentPosition << currentPose(0, 3), currentPose(1, 3), currentPose(2, 3);
+ Eigen::Vector3f currentRPY = VirtualRobot::MathTools::eigen4f2rpy(currentPose);
+
+ double posError = 0;
+ double oriError = 0;
+ for (size_t i = 0; i < 3; ++i)
+ {
+ posError += pow(currentPosition(i) - targetState[i], 2);
+ }
+ for (size_t i = 0; i < 3; ++i)
+ {
+ oriError += pow(currentRPY(i) - targetState[i + 3], 2);
+ }
+
+ posError = sqrt(posError);
+ oriError = sqrt(oriError);
+ double error = posError + posToOriRatio * oriError;
+
+ double phaseDist;
+ if (isDisturbance)
+ {
+ phaseDist = phaseDist1;
+ }
+ else
+ {
+ phaseDist = phaseDist0;
+ }
+
+ double phaseStop = 0;//phaseL / (1 + exp(-phaseK * (error - phaseDist)));
+ double mpcFactor = 1 - (phaseStop / phaseL);
+
+
+ if (mpcFactor < 0.1)
+ {
+ isDisturbance = true;
+ }
+
+ if (mpcFactor > 0.9)
+ {
+ isDisturbance = false;
+ }
+
+ // run DMP one after another
+ Eigen::Matrix4f targetPose = Eigen::Matrix4f::Identity();
+ std::vector<double> vel0;
+ vel0.resize(6);
+ for (size_t i = 0; i < vel0.size(); ++i)
+ {
+ vel0[i] = 0;
+ }
+
+ bool finished = true;
+ for (size_t i = 0; i < cfg->dmpNum; ++i)
+ {
+ double timeDuration = timeDurations[i];
+ std::string dmpType = dmpTypes[i];
+
+ if (dmpType == "Periodic")
+ {
+ if (canVals[i] <= 1e-8)
+ {
+ canVals[i] = timeDuration;
+ }
+ }
+
+ if (canVals[i] > 1e-8)
+ {
+ DMP::UMIDMPPtr dmpPtr = dmpPtrList[i];
+ double dmpDeltaT = deltaT / timeDuration;
+ double tau = dmpPtr->getTemporalFactor();
+ canVals[i] -= 1 / tau * deltaT * 1 / (1 + phaseStop) ;
+
+
+ currentStates[i] = dmpPtr->calculateDirectlyVelocity
+ (currentStates[i], canVals[i] / timeDurations[i], dmpDeltaT, targetSubStates[i]);
+
+ for (size_t j = 0; j < vel0.size(); ++j)
+ {
+ vel0[j] += currentStates[i][j].vel / timeDuration;
+ }
+
+
+ Eigen::Matrix4f targetSubMat = VirtualRobot::MathTools::rpy2eigen4f(targetSubStates[i][3], targetSubStates[i][4], targetSubStates[i][5]);
+ targetSubMat(0, 3) = targetSubStates[i][0];
+ targetSubMat(1, 3) = targetSubStates[i][1];
+ targetSubMat(2, 3) = targetSubStates[i][2];
+
+ targetPose = targetPose * targetSubMat;
+
+ finished = false;
+ }
+ else
+ {
+ for (size_t j = 0; j < vel0.size(); ++j)
+ {
+ vel0[j] += 0;
+ }
+
+ Eigen::Matrix4f targetSubMat = VirtualRobot::MathTools::rpy2eigen4f(targetSubStates[i][3], targetSubStates[i][4], targetSubStates[i][5]);
+ targetSubMat(0, 3) = targetSubStates[i][0];
+ targetSubMat(1, 3) = targetSubStates[i][1];
+ targetSubMat(2, 3) = targetSubStates[i][2];
+ targetPose = targetPose * targetSubMat;
+ }
+
+ }
+
+ Eigen::Vector3f targetRPY = VirtualRobot::MathTools::eigen4f2rpy(targetPose);
+ targetState[0] = targetPose(0, 3);
+ targetState[1] = targetPose(1, 3);
+ targetState[2] = targetPose(2, 3);
+ targetState[3] = targetRPY(0);
+ targetState[4] = targetRPY(1);
+ targetState[5] = targetRPY(2);
+
+ for (size_t i = 0; i < 3; i++)
+ {
+ double vel1 = phaseKpPos * (targetState[i] - currentPose(i, 3));
+ targetVels[i] = mpcFactor * vel0[i] + (1 - mpcFactor) * vel1;
+ }
+
+ for (size_t i = 0; i < 3; i++)
+ {
+ double vel1 = phaseKpOri * (targetState[i + 3] - currentRPY(i));
+ targetVels[i + 3] = mpcFactor * vel0[i + 3] + (1 - mpcFactor) * vel1;
+ }
+
+
+ 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().mpcFactor = mpcFactor;
+ debugOutputData.getWriteBuffer().error = error;
+ debugOutputData.getWriteBuffer().phaseStop = phaseStop;
+ debugOutputData.getWriteBuffer().posError = posError;
+ debugOutputData.getWriteBuffer().oriError = oriError;
+ debugOutputData.getWriteBuffer().deltaT = deltaT;
+
+ debugOutputData.getWriteBuffer().canVal0 = canVals[0];
+
+
+ debugOutputData.commitWrite();
+
+ LockGuardType guard {controlDataMutex};
+ getWriterControlStruct().targetTSVel = targetVels;
+ writeControlStruct();
+ }
+
+
+ void NJointCCDMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ {
+ double deltaT = sensorValuesTimestamp.toSecondsDouble();
+ controllerSensorData.getWriteBuffer().currentPose = tcp->getPoseInRootFrame();
+ 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 NJointCCDMPController::learnDMPFromFiles(int dmpId, const Ice::StringSeq& fileNames, const Ice::Current&)
+ {
+ DMP::Vec<DMP::SampledTrajectoryV2 > trajs;
+
+ DMP::DVec ratios;
+ DMP::DVec goals;
+ DMP::Vec<DMP::DMPState > starts;
+
+ 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)
+ {
+ goals.resize(traj.dim());
+ starts.resize(traj.dim());
+ for (size_t j = 0; j < goals.size(); ++j)
+ {
+ goals[j] = traj.rbegin()->getPosition(j);
+ starts[j].pos = traj.begin()->getPosition(j);
+ starts[j].vel = traj.begin()->getDeriv(j, 1) * timeDurations[dmpId];
+ }
+ }
+
+ if (i == 0)
+ {
+ ratios.push_back(1.0);
+ }
+ else
+ {
+ ratios.push_back(0.0);
+ }
+
+
+ }
+
+ dmpPtrList[dmpId]->learnFromTrajectories(trajs);
+ dmpPtrList[dmpId]->setOneStepMPC(true);
+ dmpPtrList[dmpId]->styleParas = dmpPtrList[dmpId]->getStyleParasWithRatio(ratios);
+
+
+ DMP::Vec<DMP::DMPState > currentState;
+ if (dmpId == 0)
+ {
+ for (size_t i = 0; i < 3; i++)
+ {
+ DMP::DMPState currentPos;
+ currentPos.pos = tcpPosition(i);
+ currentPos.vel = 0;
+ currentState.push_back(currentPos);
+ }
+ for (size_t i = 0; i < 3; i++)
+ {
+ DMP::DMPState currentPos;
+ currentPos.pos = tcpOrientation(i);
+ currentPos.vel = 0;
+ currentState.push_back(currentPos);
+ }
+ }
+ else
+ {
+ currentState = starts;
+
+
+ }
+ for (size_t i = 0; i < 3; i++)
+ {
+ targetState[i] = tcpPosition(i);
+ }
+ for (size_t i = 0; i < 3; i++)
+ {
+ targetState[i + 3] = tcpOrientation(i);
+ }
+
+ currentStates[dmpId] = currentState;
+
+ dmpPtrList[dmpId]->prepareExecution(goals, currentState, 1, tau);
+ dmpPtrList[dmpId]->setTemporalFactor(tau);
+ ARMARX_INFO << "Learned DMP ... ";
+ }
+
+ void NJointCCDMPController::setViaPoints(int dmpId, Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&)
+ {
+ ViaPoint viaPoint(u, viapoint);
+
+ LockGuardType guard(controllerMutex);
+ dmpPtrList[dmpId]->setViaPoint(viaPoint);
+ }
+
+ void NJointCCDMPController::setGoals(int dmpId, const Ice::DoubleSeq& goals, const Ice::Current& ice)
+ {
+ setViaPoints(dmpId, dmpPtrList[dmpId]->getUMin(), goals, ice);
+ }
+
+ void NJointCCDMPController::setControllerTarget(Ice::Float avoidJointLimitsKp, NJointTaskSpaceDMPControllerMode::CartesianSelection mode, const Ice::Current&)
+ {
+ LockGuardType guard {controlDataMutex};
+ getWriterControlStruct().avoidJointLimitsKp = avoidJointLimitsKp;
+ getWriterControlStruct().mode = ModeFromIce(mode);
+ writeControlStruct();
+ }
+
+ VirtualRobot::IKSolver::CartesianSelection NJointCCDMPController::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 NJointCCDMPController::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 NJointCCDMPController::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 NJointCCDMPController::runDMP(const Ice::Current&)
+ {
+
+ finished = false;
+
+
+ ARMARX_INFO << "run DMP";
+ controllerTask->start();
+
+ }
+
+ void NJointCCDMPController::setTemporalFactor(int dmpId, double tau, const Ice::Current&)
+ {
+ LockGuardType guard(controllerMutex);
+ dmpPtrList[dmpId]->setTemporalFactor(tau);
+ }
+
+ void NJointCCDMPController::rtPreActivateController()
+ {
+ }
+
+ void NJointCCDMPController::rtPostDeactivateController()
+ {
+
+ }
+
+ void NJointCCDMPController::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["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);
+ datafields["canVal0"] = new Variant(debugOutputData.getUpToDateReadBuffer().canVal0);
+
+ debugObs->setDebugChannel("DMPController", datafields);
+ }
+
+ void NJointCCDMPController::onInitComponent()
+ {
+ ARMARX_INFO << "init ...";
+ controllerTask = new PeriodicTask<NJointCCDMPController>(this, &NJointCCDMPController::controllerRun, 0.5);
+ }
+
+ void NJointCCDMPController::onDisconnectComponent()
+ {
+ controllerTask->stop();
+ ARMARX_INFO << "stopped ...";
+ }
+
+
+
+}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.h
new file mode 100644
index 0000000000000000000000000000000000000000..80ecfc3a8b1783a7066895b6d26f7e8321760185
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.h
@@ -0,0 +1,162 @@
+
+#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/RobotUnit/NJointTaskSpaceDMPController.h>
+#include <RobotAPI/libraries/core/CartesianVelocityController.h>
+
+
+using namespace DMP;
+namespace armarx
+{
+
+
+ TYPEDEF_PTRS_HANDLE(NJointCCDMPController);
+ TYPEDEF_PTRS_HANDLE(NJointCCDMPControllerControlData);
+
+ typedef std::pair<double, DVec > ViaPoint;
+ typedef std::vector<ViaPoint > ViaPointsSet;
+ class NJointCCDMPControllerControlData
+ {
+ 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 NJointCCDMPController :
+ public NJointControllerWithTripleBuffer<NJointCCDMPControllerControlData>,
+ public NJointCCDMPControllerInterface
+ {
+ public:
+ using ConfigPtrT = NJointCCDMPControllerConfigPtr;
+ NJointCCDMPController(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);
+
+ // NJointCCDMPControllerInterface interface
+ void learnDMPFromFiles(int dmpId, const Ice::StringSeq& fileNames, const Ice::Current&);
+ bool isFinished(const Ice::Current&)
+ {
+ return finished;
+ }
+
+ void runDMP(const Ice::Current&);
+ void setTemporalFactor(int dmpId, Ice::Double tau, const Ice::Current&);
+ void setViaPoints(int dmpId, Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&);
+ void setGoals(int dmpId, 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 mpcFactor;
+ double error;
+ double phaseStop;
+ double posError;
+ double oriError;
+ double deltaT;
+ double canVal0;
+ };
+
+ TripleBuffer<DebugBufferData> debugOutputData;
+
+ struct NJointCCDMPControllerSensorData
+ {
+ double currentTime;
+ double deltaT;
+ Eigen::Matrix4f currentPose;
+ };
+ TripleBuffer<NJointCCDMPControllerSensorData> 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
+ std::vector<UMIDMPPtr > dmpPtrList;
+ std::vector<double> canVals;
+ std::vector<double> timeDurations;
+ std::vector<std::string > dmpTypes;
+
+ std::vector<Vec<DMPState > > currentStates;
+ std::vector<DVec > targetSubStates;
+
+ 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;
+
+ NJointCCDMPControllerConfigPtr cfg;
+ VirtualRobot::RobotNodePtr tcp;
+ Eigen::Vector3f tcpPosition;
+ Eigen::Vector3f tcpOrientation;
+
+ DMP::DVec targetState;
+ mutable MutexType controllerMutex;
+ PeriodicTask<NJointCCDMPController>::pointer_type controllerTask;
+ };
+
+} // namespace armarx
+
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.h
index 16b456700cc05f175718e381eb4e7683378f3d8f..625058be3fa1a6d22047392e6cd4dce582590afc 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/DMPController/NJointJointSpaceDMPController.h>
+#include <RobotAPI/interface/units/RobotUnit/NJointJointSpaceDMPController.h>
namespace armarx
{
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJointSpaceDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJointSpaceDMPController.h
index 9d7e6a53edc234f521373d4bb7cf3f0106df8f1a..173c7cfb146f2117b2c6a8df498baffa2eb21477 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/DMPController/NJointJointSpaceDMPController.h>
+#include <RobotAPI/interface/units/RobotUnit/NJointJointSpaceDMPController.h>
namespace armarx
{
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
index 9480a8c0364636520ff5bc9e0c6a3c7e41a3b0d0..92df61bbc9a916d76e403329d823c70f7ed3cd6a 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
@@ -115,12 +115,19 @@ namespace armarx
currentState = controllerSensorData.getReadBuffer().currentState;
+ std::vector<double> currentposi;
+ currentposi.resize(3);
for (size_t i = 0; i < 3; ++i)
{
+ currentposi[i] = currentState[i].pos;
posError += pow(currentState[i].pos - targetState[i], 2);
}
+
+ std::vector<double> currentori;
+ currentori.resize(3);
for (size_t i = 0; i < 3; ++i)
{
+ currentori[i] = currentState[i + 3].pos;
oriError += pow(currentState[i + 3].pos - targetState[i + 3], 2);
}
@@ -140,7 +147,7 @@ namespace armarx
phaseDist = phaseDist0;
}
- phaseStop = 0;//phaseL / (1 + exp(-phaseK * (error - phaseDist)));
+ phaseStop = phaseL / (1 + exp(-phaseK * (error - phaseDist)));
mpcFactor = 1 - (phaseStop / phaseL);
if (mpcFactor < 0.1)
@@ -164,13 +171,13 @@ namespace armarx
for (size_t i = 0; i < 3; i++)
{
vel0 = currentState[i].vel / timeDuration;
- vel1 = phaseKpPos * (targetState[i] - tcpPosition[i]);
+ vel1 = phaseKpPos * (targetState[i] - currentposi[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]);
+ vel1 = phaseKpOri * (targetState[i + 3] - currentori[i]);
targetVels[i + 3] = mpcFactor * vel0 + (1 - mpcFactor) * vel1;
}
}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
index ec8d216b992dd30b13edfa6f49b41ad987f897b3..e23451a67396f9e3303b760b25fb70aed9d2269f 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.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/DMPController/NJointTaskSpaceDMPController.h>
+#include <RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.h>
#include <RobotAPI/libraries/core/CartesianVelocityController.h>
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceDMPController.h
index d45c8ce03449c3a2c1694269563a4618a657220c..b5f3fa66e39c20002747e7e02372e7f45c5aed86 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/DMPController/NJointTaskSpaceDMPController.h>
+#include <RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.h>
#include <RobotAPI/libraries/core/CartesianVelocityController.h>