diff --git a/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt b/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
index 3615120f6c99c496a64c9a7ee17c7bbb2b70dc31..317729db5c2481f35c7e1ddd722cd88b84f99566 100644
--- a/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
+++ b/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
@@ -124,13 +124,16 @@ if (DMP_FOUND)
message(STATUS "DMP libraries is ${DMP_LIBRARIES}")
include_directories(${DMP_INCLUDE_DIRS})
- link_directories(${DMP_LIB_DIRS})
+# link_directories(${DMP_LIB_DIRS})
list(APPEND LIB_HEADERS
NJointControllers/NJointJointSpaceDMPController.h
+ NJointControllers/NJointTaskSpaceDMPController.h
)
list(APPEND LIB_FILES
- NJointControllers/NJointJointSpaceDMPController.cpp)
+ NJointControllers/NJointJointSpaceDMPController.cpp
+ NJointControllers/NJointTaskSpaceDMPController.cpp
+ )
list(APPEND LIBS ${DMP_LIBRARIES})
endif ()
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointDMPCartesianVelocityController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointDMPCartesianVelocityController.cpp
deleted file mode 100644
index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..0000000000000000000000000000000000000000
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointDMPCartesianVelocityController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointDMPCartesianVelocityController.h
deleted file mode 100644
index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..0000000000000000000000000000000000000000
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointJointSpaceDMPController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointJointSpaceDMPController.cpp
index 299888c1b3020199403bd17a42d8d256de9f21e3..d7073ab3f84bab8eff8f2ed3850d6ab488cca5c6 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointJointSpaceDMPController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointJointSpaceDMPController.cpp
@@ -25,17 +25,21 @@ namespace armarx
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";
}
- DMPAsForwardControl = cfg->DMPAsForwardControl;
- dmpPtr.reset(new DMP::UMIDMP(cfg->kernelSize, cfg->Kd, cfg->baseMode, cfg->tau));
+ dmpPtr.reset(new DMP::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;
+ phaseKp = cfg->phaseKp;
NJointJointSpaceDMPControllerControlData initData;
initData.tau = 1.0;
@@ -48,23 +52,36 @@ namespace armarx
if (rtGetControlStruct().isStart)
{
currentState.clear();
+ double phaseStop = 0;
+ double error = 0;
+ std::vector<double> currentPosition;
+ std::vector<double> currentVelocity;
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;
currentState.push_back(currentPos);
+ currentPosition.push_back(currentPos.pos);
+ currentVelocity.push_back(currentPos.vel);
+
+ error += pow(currentPos.pos - targetState[i], 2);
+
}
+ error = sqrt(error);
+ phaseStop = phaseL / (1 + exp(-phaseK * (error - phaseDist0)));
+ double mpcFactor = 1 - (phaseStop / phaseL);
double tau = rtGetControlStruct().tau;
double deltaT = timeSinceLastIteration.toSecondsDouble();
- canVal -= 1 / tau * deltaT * 1 ;
+ canVal -= 1 / tau * deltaT * 1 / (1 + phaseStop);
double dmpDeltaT = deltaT / timeDuration;
dmpPtr->setTemporalFactor(tau);
- DMP::DVec targetState;
- currentState = dmpPtr->calculateNextPointWithEulerMethod(currentState, canVal / timeDuration, dmpDeltaT, targetState, 1.0);
+ currentState = dmpPtr->calculateDirectlyVelocity(currentState, canVal / timeDuration, dmpDeltaT, targetState);
+
if (canVal < 1e-8)
{
finished = true;
@@ -75,7 +92,10 @@ namespace armarx
const auto& jointName = dimNames.at(i);
if (targets.count(jointName) == 1)
{
- targets[jointName]->velocity = finished ? 0.0f : currentState[i].vel;
+ double vel0 = currentState[i].vel / timeDuration;
+ double vel1 = phaseKp * (targetState[i] - currentPosition[i]);
+ double vel = mpcFactor * vel0 + (1 - mpcFactor) * vel1;
+ targets[jointName]->velocity = finished ? 0.0f : vel;
}
}
}
@@ -115,6 +135,7 @@ namespace armarx
void NJointJointSpaceDMPController::runDMP(const Ice::DoubleSeq& goals, double tau, const Ice::Current&)
{
currentState.clear();
+ targetState.clear();
for (size_t i = 0; i < dimNames.size(); i++)
{
const auto& jointName = dimNames.at(i);
@@ -122,6 +143,7 @@ namespace armarx
currentPos.pos = (positionSensors.count(jointName) == 1) ? positionSensors[jointName]->position : 0.0f;
currentPos.vel = (velocitySensors.count(jointName) == 1) ? velocitySensors[jointName]->velocity : 0.0f;
currentState.push_back(currentPos);
+ targetState.push_back(currentPos.pos);
}
dmpPtr->prepareExecution(goals, currentState, 1, tau);
finished = false;
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointJointSpaceDMPController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointJointSpaceDMPController.h
index 89d1395dcb27ca170b25d9631e1a9cfc8da07d48..7151002d7db3a35ffc3946d1eaaea7f1547ac02a 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointJointSpaceDMPController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointJointSpaceDMPController.h
@@ -84,8 +84,16 @@ namespace armarx
double tau;
double finished;
+
+ // phaseStop parameters
+ double phaseL;
+ double phaseK;
+ double phaseDist0;
+ double phaseKp;
+
std::vector<std::string> dimNames;
DMP::Vec<DMP::DMPState> currentState;
+ DMP::DVec targetState;
};
} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceDMPController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceDMPController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c6a70da0cb1323f893b3cc1d23f7e9b75cb0e443
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceDMPController.cpp
@@ -0,0 +1,375 @@
+#include "NJointTaskSpaceDMPController.h"
+
+namespace armarx
+{
+ NJointControllerRegistration<NJointTaskSpaceDMPController> registrationControllerNJointTaskSpaceDMPController("NJointTaskSpaceDMPController");
+
+ NJointTaskSpaceDMPController::NJointTaskSpaceDMPController(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());
+
+
+ NJointTaskSpaceDMPControllerControlData initData;
+ 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);
+
+ // set DMP
+ dmpPtr.reset(new UMIDMP(cfg->kernelSize, cfg->DMPKd, cfg->baseMode, cfg->tau));
+ timeDuration = cfg->timeDuration;
+ canVal = timeDuration;
+ finished = false;
+
+ initData.dmpPtr = dmpPtr;
+ initData.isStart = false;
+ reinitTripleBuffer(initData);
+
+ phaseL = cfg->phaseL;
+ phaseK = cfg->phaseK;
+ phaseDist0 = cfg->phaseDist0;
+ phaseKp = cfg->phaseKp;
+
+ // 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);
+ }
+
+ std::string NJointTaskSpaceDMPController::getClassName(const Ice::Current&) const
+ {
+ return "NJointTaskSpaceDMPController";
+ }
+
+ void NJointTaskSpaceDMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ {
+ if (rtGetControlStruct().isStart)
+ {
+ UMIDMPPtr dmpPtr = rtGetControlStruct().dmpPtr;
+
+ // DMP controller
+ 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();
+ double dmpDeltaT = deltaT / timeDuration;
+
+ 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;
+
+ double phaseStop = 0;
+ double error = 0;
+ currentState.clear();
+ for (size_t i = 0; i < 3; i++)
+ {
+ DMP::DMPState currentPos;
+ currentPos.pos = tcpPosition(i);
+ currentPos.vel = posVel(i) * timeDuration;
+ currentState.push_back(currentPos);
+
+ error += pow(currentPos.pos - targetState[i], 2);
+ }
+
+ for (size_t i = 0; i < 3; i++)
+ {
+ DMP::DMPState currentPos;
+ currentPos.pos = tcpOrientation(i);
+ currentPos.vel = oriVel(i) * timeDuration;
+ currentState.push_back(currentPos);
+
+ error += pow(currentPos.pos - targetState[i + 3], 2);
+ }
+
+ error = sqrt(error);
+ phaseStop = phaseL / (1 + exp(-phaseK * (error - phaseDist0)));
+ double mpcFactor = 1 - (phaseStop / phaseL);
+
+ double tau = dmpPtr->getTemporalFactor();
+ canVal -= 1 / tau * deltaT * 1 / (1 + phaseStop) ;
+ currentState = dmpPtr->calculateDirectlyVelocity(currentState, canVal / timeDuration, dmpDeltaT, targetState);
+
+ if (canVal < 1e-8)
+ {
+ finished = true;
+ }
+
+ // cartesian velocity controller
+ Eigen::VectorXf x;
+ auto mode = rtGetControlStruct().mode;
+
+ double vel0, vel1;
+ if (mode == VirtualRobot::IKSolver::All)
+ {
+ x.resize(6);
+
+ for (size_t i = 0; i < 3; i++)
+ {
+ vel0 = currentState[i].vel / timeDuration;
+ vel1 = phaseKp * (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]);
+ x(i + 3) = mpcFactor * vel0 + (1 - mpcFactor) * vel1;
+ }
+ }
+ else if (mode == VirtualRobot::IKSolver::Position)
+ {
+ x.resize(3);
+
+ for (size_t i = 0; i < 3; i++)
+ {
+ vel0 = currentState[i].vel / timeDuration;
+ vel1 = phaseKp * (targetState[i] - tcpPosition[i]);
+ x(i) = mpcFactor * vel0 + (1 - mpcFactor) * vel1;
+ }
+ }
+ else if (mode == VirtualRobot::IKSolver::Orientation)
+ {
+ x.resize(3);
+
+ for (size_t i = 0; i < 3; i++)
+ {
+ vel0 = currentState[i + 3].vel / timeDuration;
+ vel1 = phaseKp * (targetState[i + 3] - tcpOrientation[i]);
+ x(i) = mpcFactor * vel0 + (1 - mpcFactor) * vel1;
+ }
+ }
+ 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(timeSinceLastIteration.toSecondsDouble(), torqueSensors.at(i)->torque - gravityTorqueSensors.at(i)->gravityTorque);
+ //jnv(i) += rtGetControlStruct().torqueKp.at(i) * (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 NJointTaskSpaceDMPController::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 NJointTaskSpaceDMPController::setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&)
+ {
+ ViaPoint viaPoint(u, viapoint);
+ dmpPtr->setViaPoint(viaPoint);
+
+ LockGuardType guard {controlDataMutex};
+ getWriterControlStruct().dmpPtr = dmpPtr;
+ writeControlStruct();
+ }
+
+ void NJointTaskSpaceDMPController::setGoals(const Ice::DoubleSeq& goals, const Ice::Current& ice)
+ {
+ setViaPoints(dmpPtr->getUMin(), goals, ice);
+ }
+
+ void NJointTaskSpaceDMPController::setControllerTarget(Ice::Float avoidJointLimitsKp, NJointTaskSpaceDMPControllerMode::CartesianSelection mode, const Ice::Current&)
+ {
+ LockGuardType guard {controlDataMutex};
+ getWriterControlStruct().avoidJointLimitsKp = avoidJointLimitsKp;
+ getWriterControlStruct().mode = ModeFromIce(mode);
+ writeControlStruct();
+ }
+
+ VirtualRobot::IKSolver::CartesianSelection NJointTaskSpaceDMPController::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 NJointTaskSpaceDMPController::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 NJointTaskSpaceDMPController::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 NJointTaskSpaceDMPController::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);
+
+ LockGuardType guard {controlDataMutex};
+ getWriterControlStruct().dmpPtr = dmpPtr;
+ getWriterControlStruct().isStart = true;
+ writeControlStruct();
+
+ }
+
+ void NJointTaskSpaceDMPController::setTemporalFactor(double tau, const Ice::Current&)
+ {
+ dmpPtr->setTemporalFactor(tau);
+ LockGuardType guard {controlDataMutex};
+ getWriterControlStruct().dmpPtr = dmpPtr;
+ writeControlStruct();
+ }
+
+ void NJointTaskSpaceDMPController::rtPreActivateController()
+ {
+ }
+
+ void NJointTaskSpaceDMPController::rtPostDeactivateController()
+ {
+
+ }
+
+}
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceDMPController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceDMPController.h
new file mode 100644
index 0000000000000000000000000000000000000000..7a0b4c2aabc8b6a5974cca219ad0d292cc000435
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceDMPController.h
@@ -0,0 +1,126 @@
+
+#pragma once
+
+
+#include "NJointController.h"
+#include <VirtualRobot/Robot.h>
+#include "../RobotUnit.h"
+#include "../ControlTargets/ControlTarget1DoFActuator.h"
+#include "../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(NJointTaskSpaceDMPController);
+ TYPEDEF_PTRS_HANDLE(NJointTaskSpaceDMPControllerControlData);
+
+ typedef std::pair<double, DVec > ViaPoint;
+ typedef std::vector<ViaPoint > ViaPointsSet;
+ class NJointTaskSpaceDMPControllerControlData
+ {
+ public:
+ // dmp control data
+ UMIDMPPtr dmpPtr;
+ bool isStart;
+
+ // 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 NJointTaskSpaceDMPController :
+ public NJointControllerWithTripleBuffer<NJointTaskSpaceDMPControllerControlData>,
+ public NJointTaskSpaceDMPControllerInterface
+ {
+ public:
+ using ConfigPtrT = NJointTaskSpaceDMPControllerConfigPtr;
+ NJointTaskSpaceDMPController(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);
+
+ // NJointTaskSpaceDMPControllerInterface 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);
+
+ private:
+ 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;
+
+ // phaseStop parameters
+ double phaseL;
+ double phaseK;
+ double phaseDist0;
+ double phaseKp;
+
+
+ NJointTaskSpaceDMPControllerConfigPtr cfg;
+ VirtualRobot::RobotNodePtr tcp;
+ Eigen::Vector3f tcpPosition;
+ Eigen::Vector3f tcpOrientation;
+ DMP::Vec<DMP::DMPState> currentState;
+ DMP::DVec targetState;
+
+
+ };
+
+} // namespace armarx
+
diff --git a/source/RobotAPI/interface/CMakeLists.txt b/source/RobotAPI/interface/CMakeLists.txt
index 8bc308172ea96dae58ca1f38d680cdb57b75c441..2ea2965e5fc0a6c23787684003fdaf66677d335a 100644
--- a/source/RobotAPI/interface/CMakeLists.txt
+++ b/source/RobotAPI/interface/CMakeLists.txt
@@ -48,6 +48,7 @@ set(SLICE_FILES
units/RobotUnit/RobotUnitInterface.ice
units/RobotUnit/NJointJointSpaceDMPController.ice
+ units/RobotUnit/NJointTaskSpaceDMPController.ice
components/ViewSelectionInterface.ice
components/TrajectoryPlayerInterface.ice
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointJointSpaceDMPController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointJointSpaceDMPController.ice
index 4faa50476deb917d8321f2fbf9e8423aeb7fc50d..356cd9ba185bee800a404116dd6123de1c1f2c5b 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointJointSpaceDMPController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointJointSpaceDMPController.ice
@@ -20,8 +20,8 @@
* GNU General Public License
*/
-#ifndef _ARMARX_ROBOTAPI_NJointTrajectoryControllerInterface_SLICE_
-#define _ARMARX_ROBOTAPI_NJointTrajectoryControllerInterface_SLICE_
+#ifndef _ARMARX_ROBOTAPI_NJointJointSpaceDMPControllerInterface_SLICE_
+#define _ARMARX_ROBOTAPI_NJointJointSpaceDMPControllerInterface_SLICE_
#include <RobotAPI/interface/units/RobotUnit/NJointController.ice>
#include <RobotAPI/interface/core/Trajectory.ice>
@@ -31,13 +31,17 @@ module armarx
class NJointJointSpaceDMPControllerConfig extends NJointControllerConfig
{
Ice::StringSeq jointNames;
- float Kd = 20;
+ float DMPKd = 20;
int kernelSize = 100;
double tau = 1;
int baseMode = 1;
+ double phaseL = 10;
+ double phaseK = 10;
+ double phaseDist0 = 0.3;
+ double phaseKp = 1;
+
double timeDuration = 10;
- bool DMPAsForwardControl = true;
};
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
new file mode 100644
index 0000000000000000000000000000000000000000..d9d4c42d0bfc6fdff332b6eb0f979d7c33cb27c4
--- /dev/null
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
@@ -0,0 +1,81 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::NJointControllerInterface
+ * @author Mirko Waechter ( mirko dot waechter at kit dot edu )
+ * @date 2017
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#ifndef _ARMARX_ROBOTAPI_NJointTaskSpaceControllerInterface_SLICE_
+#define _ARMARX_ROBOTAPI_NJointTaskSpaceControllerInterface_SLICE_
+
+#include <RobotAPI/interface/units/RobotUnit/NJointController.ice>
+#include <RobotAPI/interface/core/Trajectory.ice>
+
+module armarx
+{
+ module NJointTaskSpaceDMPControllerMode
+ {
+ enum CartesianSelection
+ {
+ ePosition = 7,
+ eOrientation = 8,
+ eAll = 15
+ };
+ };
+
+
+ class NJointTaskSpaceDMPControllerConfig extends NJointControllerConfig
+ {
+
+ // dmp configuration
+ float DMPKd = 20;
+ int kernelSize = 100;
+ double tau = 1;
+ int baseMode = 1;
+
+ double phaseL = 10;
+ double phaseK = 10;
+ double phaseDist0 = 0.3;
+ double phaseKp = 1;
+
+ double timeDuration = 10;
+
+ // velocity controller configuration
+ string nodeSetName = "";
+ string tcpName = "";
+ NJointTaskSpaceDMPControllerMode::CartesianSelection mode = NJointTaskSpaceDMPControllerMode::eAll;
+
+ };
+
+
+ interface NJointTaskSpaceDMPControllerInterface extends NJointControllerInterface
+ {
+ void learnDMPFromFiles(Ice::StringSeq trajfiles);
+ bool isFinished();
+ void runDMP(Ice::DoubleSeq goals, double tau);
+
+ void setTemporalFactor(double tau);
+ void setViaPoints(double canVal, Ice::DoubleSeq point);
+ void setGoals(Ice::DoubleSeq goals);
+
+ void setControllerTarget(float avoidJointLimitsKp, NJointTaskSpaceDMPControllerMode::CartesianSelection mode);
+ void setTorqueKp(StringFloatDictionary torqueKp);
+ void setNullspaceJointVelocities(StringFloatDictionary nullspaceJointVelocities);
+ };
+};
+#endif