diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
index 850ab231f12ca392810c78c0af412454c8a4a28f..ea8580ace2601cc93d498f4fab9319e1af0615bb 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
@@ -100,9 +100,12 @@ module armarx
double phaseKpPos = 1;
double phaseKpOri = 0.1;
+
// misc
Ice::DoubleSeq timeDurations;
Ice::StringSeq dmpTypes;
+ Ice::DoubleSeq amplitudes;
+
double posToOriRatio = 100;
// velocity controller configuration
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.cpp
index d13c9d954d32b5a7a3b78f98543cdc4595c4c352..c00253ed7a2524d5e6d8a591754f458ef2649168 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.cpp
@@ -72,6 +72,7 @@ namespace armarx
// initialize tcp position and orientation
Eigen::Matrix4f pose = tcp->getPoseInRootFrame();
+ oldPose = pose;
tcpPosition(0) = pose(0, 3);
tcpPosition(1) = pose(1, 3);
tcpPosition(2) = pose(2, 3);
@@ -103,6 +104,8 @@ namespace armarx
reinitTripleBuffer(initData);
learnedDMP.clear();
+
+ amplitudes = cfg->amplitudes;
}
std::string NJointCCDMPController::getClassName(const Ice::Current&) const
@@ -149,7 +152,7 @@ namespace armarx
phaseDist = phaseDist0;
}
- double phaseStop = 0;//phaseL / (1 + exp(-phaseK * (error - phaseDist)));
+ double phaseStop = phaseL / (1 + exp(-phaseK * (error - phaseDist)));
double mpcFactor = 1 - (phaseStop / phaseL);
@@ -165,25 +168,20 @@ namespace armarx
// 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];
+ double amplitude = 1.0;
if (dmpType == "Periodic")
{
if (canVals[i] <= 1e-8)
{
canVals[i] = timeDuration;
}
+ amplitude = amplitudes[i];
}
if (canVals[i] > 1e-8)
@@ -197,37 +195,25 @@ namespace armarx
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;
- }
+ finished = false;
- 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];
- 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;
- }
+ ARMARX_INFO << "targetSubMat: " << targetSubMat;
+
+ targetPose = targetPose * targetSubMat;
}
+ ARMARX_INFO << "targetPose: " << targetPose;
+
Eigen::Vector3f targetRPY = VirtualRobot::MathTools::eigen4f2rpy(targetPose);
targetState[0] = targetPose(0, 3);
targetState[1] = targetPose(1, 3);
@@ -236,18 +222,24 @@ namespace armarx
targetState[4] = targetRPY(1);
targetState[5] = targetRPY(2);
+ Eigen::Vector3f oldRPY = VirtualRobot::MathTools::eigen4f2rpy(oldPose);
+
+
for (size_t i = 0; i < 3; i++)
{
+ double vel0 = (targetState[i] - oldPose(i, 3)) / deltaT;
double vel1 = phaseKpPos * (targetState[i] - currentPose(i, 3));
- targetVels[i] = mpcFactor * vel0[i] + (1 - mpcFactor) * vel1;
+ targetVels[i] = mpcFactor * vel0 + (1 - mpcFactor) * vel1;
}
for (size_t i = 0; i < 3; i++)
{
+ double vel0 = (targetState[i + 3] - oldRPY(i)) / deltaT;
double vel1 = phaseKpOri * (targetState[i + 3] - currentRPY(i));
- targetVels[i + 3] = mpcFactor * vel0[i + 3] + (1 - mpcFactor) * vel1;
+ targetVels[i + 3] = mpcFactor * vel0 + (1 - mpcFactor) * vel1;
}
+ oldPose = targetPose;
debugOutputData.getWriteBuffer().latestTargetVelocities["x_vel"] = targetVels[0];
debugOutputData.getWriteBuffer().latestTargetVelocities["y_vel"] = targetVels[1];
@@ -576,7 +568,7 @@ namespace armarx
void NJointCCDMPController::onInitComponent()
{
ARMARX_INFO << "init ...";
- controllerTask = new PeriodicTask<NJointCCDMPController>(this, &NJointCCDMPController::controllerRun, 0.5);
+ controllerTask = new PeriodicTask<NJointCCDMPController>(this, &NJointCCDMPController::controllerRun, 0.3);
}
void NJointCCDMPController::onDisconnectComponent()
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.h
index cde02bb4e350de36c37c022e9904597f6811e8c9..7dc42afd1e8c99b9a5d269727cec270753abfc5c 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.h
@@ -128,6 +128,7 @@ namespace armarx
std::vector<double> canVals;
std::vector<double> timeDurations;
std::vector<std::string > dmpTypes;
+ std::vector<double> amplitudes;
std::vector<Vec<DMPState > > currentStates;
std::vector<DVec > targetSubStates;
@@ -156,6 +157,8 @@ namespace armarx
Eigen::Vector3f tcpPosition;
Eigen::Vector3f tcpOrientation;
+ Eigen::Matrix4f oldPose;
+
DMP::DVec targetState;
mutable MutexType controllerMutex;
PeriodicTask<NJointCCDMPController>::pointer_type controllerTask;
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
index a9d87b71878959e6dc716a1ccb99c748c5f30521..28f315c41525c72ba1a89caf313162603acd9d2c 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
@@ -64,11 +64,7 @@ namespace armarx
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;
@@ -90,6 +86,7 @@ namespace armarx
initData.torqueKd.resize(tcpController->rns->getSize(), 0);
initData.mode = ModeFromIce(cfg->mode);
reinitTripleBuffer(initData);
+
}
std::string NJointTSDMPController::getClassName(const Ice::Current&) const
@@ -107,6 +104,8 @@ namespace armarx
double deltaT = 0;
std::vector<double> currentposi;
std::vector<double> currentori;
+ currentposi.resize(3);
+ currentori.resize(3);
if (canVal > 1e-8)
{
@@ -118,14 +117,12 @@ namespace armarx
currentState = controllerSensorData.getReadBuffer().currentState;
- currentposi.resize(3);
for (size_t i = 0; i < 3; ++i)
{
currentposi[i] = currentState[i].pos;
posError += pow(currentState[i].pos - targetState[i], 2);
}
- currentori.resize(3);
for (size_t i = 0; i < 3; ++i)
{
currentori[i] = currentState[i + 3].pos;
@@ -169,6 +166,7 @@ namespace armarx
double vel0, vel1;
+
for (size_t i = 0; i < 3; i++)
{
vel0 = currentState[i].vel / timeDuration;
@@ -181,6 +179,7 @@ namespace armarx
vel1 = phaseKpOri * (targetState[i + 3] - currentori[i]);
targetVels[i + 3] = mpcFactor * vel0 + (1 - mpcFactor) * vel1;
}
+
}
else
{
@@ -190,14 +189,24 @@ namespace armarx
{
targetVels[i] = 0;
}
+
+ for (size_t i = 0; i < 3; ++i)
+ {
+ currentposi[i] = currentState[i].pos;
+ }
+
+ for (size_t i = 0; i < 3; ++i)
+ {
+ currentori[i] = currentState[i + 3].pos;
+ }
}
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().latestTargetVelocities["angle_x"] = targetVels[3];
+ debugOutputData.getWriteBuffer().latestTargetVelocities["angle_y"] = targetVels[4];
+ debugOutputData.getWriteBuffer().latestTargetVelocities["angle_z"] = targetVels[5];
debugOutputData.getWriteBuffer().dmpTargets["dmp_x"] = targetState[0];
debugOutputData.getWriteBuffer().dmpTargets["dmp_y"] = targetState[1];
debugOutputData.getWriteBuffer().dmpTargets["dmp_z"] = targetState[2];
@@ -435,6 +444,15 @@ namespace armarx
void NJointTSDMPController::runDMP(const Ice::DoubleSeq& goals, double tau, const Ice::Current&)
{
+ 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);
+
+ ARMARX_INFO << "tcpPosition: " << tcpPosition;
+ ARMARX_INFO << "tcpOrientation: " << tcpOrientation;
+
currentState.clear();
targetState.clear();
for (size_t i = 0; i < 3; i++)