Skip to content
Snippets Groups Projects
Commit 3ac0d8da authored by You Zhou's avatar You Zhou
Browse files

modified dmpcontroller

parent c5b20c58
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
+ 3
− 0
View file @ 3ac0d8da
......@@ -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
......
source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.cpp
+ 26
− 34
View file @ 3ac0d8da
......@@ -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()
......
source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.h
+ 3
− 0
View file @ 3ac0d8da
......@@ -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;
......
source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
+ 28
− 10
View file @ 3ac0d8da
......@@ -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++)
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment