From 28624394a8135eaf56556de7d6894f673646bc5c Mon Sep 17 00:00:00 2001
From: ArmarX User <armarx@kit.edu>
Date: Tue, 19 Feb 2019 18:02:02 +0100
Subject: [PATCH] added all changes for VMP
---
.../NJointTaskSpaceDMPController.ice | 3 +
.../DMPController/TaskSpaceDMPController.cpp | 33 ++--
.../DMPController/TaskSpaceDMPController.h | 9 +-
.../DMPController/NJointTSDMPController.cpp | 147 ++++++++++++------
.../DMPController/NJointTSDMPController.h | 7 +-
5 files changed, 133 insertions(+), 66 deletions(-)
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
index e4af46f9e..8388479b7 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
@@ -86,9 +86,12 @@ module armarx
void setSpeed(double times);
void setViaPoints(double canVal, Ice::DoubleSeq point);
+ void removeAllViaPoints();
void setGoals(Ice::DoubleSeq goals);
double getCanVal();
+ void resetDMP();
+ void stopDMP();
void setControllerTarget(float avoidJointLimitsKp, NJointTaskSpaceDMPControllerMode::CartesianSelection mode);
void setTorqueKp(StringFloatDictionary torqueKp);
diff --git a/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.cpp b/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.cpp
index 425a9abf8..eb748893a 100644
--- a/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.cpp
+++ b/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.cpp
@@ -78,7 +78,7 @@ void TaskSpaceDMPController::flow(double deltaT, const Eigen::Matrix4f& currentP
double tau = dmpPtr->getTemporalFactor();
double timeDuration = config.motionTimeDuration;
- canVal -= 1 / tau * deltaT * 1 / (1 + phaseStop) ;
+ canVal -= deltaT * 1 / (1 + phaseStop) ;
for (size_t i = 0; i < 7; ++i)
@@ -86,7 +86,7 @@ void TaskSpaceDMPController::flow(double deltaT, const Eigen::Matrix4f& currentP
currentState[i].vel = currentState[i].vel * config.DMPAmplitude;
}
- currentState = dmpPtr->calculateDirectlyVelocity(currentState, canVal / timeDuration, deltaT / timeDuration, targetPoseVec);
+ currentState = dmpPtr->calculateDirectlyVelocity(currentState, canVal / (tau * timeDuration), deltaT / (tau * timeDuration), targetPoseVec);
// for (size_t i = 0; i < 7; ++i)
// {
@@ -102,7 +102,7 @@ void TaskSpaceDMPController::flow(double deltaT, const Eigen::Matrix4f& currentP
linearVel << twist(0), twist(1), twist(2);
for (size_t i = 0; i < 3; i++)
{
- vel0 = currentState[i].vel / timeDuration;
+ vel0 = currentState[i].vel / (timeDuration * tau);
vel1 = config.phaseStopParas.Kpos * (targetPoseVec[i] - currentPosition(i)) - config.phaseStopParas.Dpos * linearVel(i);
targetVel(i) = mpcFactor * vel0 + (1 - mpcFactor) * vel1;
}
@@ -111,7 +111,7 @@ void TaskSpaceDMPController::flow(double deltaT, const Eigen::Matrix4f& currentP
{
for (size_t i = 0; i < 3; i++)
{
- targetVel(i) = currentState[i].vel / timeDuration;
+ targetVel(i) = currentState[i].vel / (timeDuration * tau);
}
}
@@ -167,15 +167,15 @@ void TaskSpaceDMPController::flow(double deltaT, const Eigen::Matrix4f& currentP
if (isPhaseStopControl)
{
- targetVel(3) = angularVel0.x() / timeDuration;
- targetVel(4) = angularVel0.y() / timeDuration;
- targetVel(5) = angularVel0.z() / timeDuration;
+ targetVel(3) = angularVel0.x() / (timeDuration * tau);
+ targetVel(4) = angularVel0.y() / (timeDuration * tau);
+ targetVel(5) = angularVel0.z() / (timeDuration * tau);
}
else
{
- targetVel(3) = mpcFactor * angularVel0.x() / timeDuration + (1 - mpcFactor) * angularVel1(0);
- targetVel(4) = mpcFactor * angularVel0.y() / timeDuration + (1 - mpcFactor) * angularVel1(1);
- targetVel(5) = mpcFactor * angularVel0.z() / timeDuration + (1 - mpcFactor) * angularVel1(2);
+ targetVel(3) = mpcFactor * angularVel0.x() / (timeDuration * tau) + (1 - mpcFactor) * angularVel1(0);
+ targetVel(4) = mpcFactor * angularVel0.y() / (timeDuration * tau) + (1 - mpcFactor) * angularVel1(1);
+ targetVel(5) = mpcFactor * angularVel0.z() / (timeDuration * tau) + (1 - mpcFactor) * angularVel1(2);
}
debugData.canVal = canVal;
@@ -258,15 +258,20 @@ void TaskSpaceDMPController::setViaPose(double canVal, const std::vector<double>
dmpPtr->setViaPoint(canVal, viaPoseWithQuaternion);
}
-void TaskSpaceDMPController::prepareExecution(const Eigen::Matrix4f& currentPose, const Eigen::Matrix4f& goalPose)
+void TaskSpaceDMPController::removeAllViaPoints()
+{
+ dmpPtr->removeViaPoints();
+}
+
+void TaskSpaceDMPController::prepareExecution(const Eigen::Matrix4f& currentPose, const Eigen::Matrix4f& goalPose, double tau)
{
std::vector<double> currentPoseVec = eigen4f2vec(currentPose);
std::vector<double> goalPoseVec = eigen4f2vec(goalPose);
- prepareExecution(currentPoseVec, goalPoseVec);
+ prepareExecution(currentPoseVec, goalPoseVec, tau);
}
-void TaskSpaceDMPController::prepareExecution(const std::vector<double>& currentPoseVec, const std::vector<double>& goalPoseVec)
+void TaskSpaceDMPController::prepareExecution(const std::vector<double>& currentPoseVec, const std::vector<double>& goalPoseVec, double tau)
{
ARMARX_CHECK_EQUAL(currentPoseVec.size(), 7);
ARMARX_CHECK_EQUAL(goalPoseVec.size(), 7);
@@ -278,7 +283,7 @@ void TaskSpaceDMPController::prepareExecution(const std::vector<double>& current
targetPoseVec.at(i) = currentPoseVec.at(i);
}
- dmpPtr->prepareExecution(goalPoseVec, currentState, 1, 1);
+ dmpPtr->prepareExecution(goalPoseVec, currentState, 1, tau);
this->goalPoseVec = goalPoseVec;
isDisturbance = false;
canVal = config.motionTimeDuration;
diff --git a/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.h b/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.h
index d95e1b62c..777c75da7 100644
--- a/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.h
+++ b/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.h
@@ -142,8 +142,10 @@ namespace armarx
void setViaPose(double canVal, const Eigen::Matrix4f& viaPose);
void setViaPose(double canVal, const std::vector<double>& viaPoseWithQuaternion);
- void prepareExecution(const Eigen::Matrix4f& currentPose, const Eigen::Matrix4f& goalPose);
- void prepareExecution(const std::vector<double>& currentPoseVec, const std::vector<double>& goalPoseVec);
+ void removeAllViaPoints();
+
+ void prepareExecution(const Eigen::Matrix4f& currentPose, const Eigen::Matrix4f& goalPose, double tau = 1);
+ void prepareExecution(const std::vector<double>& currentPoseVec, const std::vector<double>& goalPoseVec, double tau = 1);
void setSpeed(double times);
@@ -168,6 +170,8 @@ namespace armarx
double canVal;
bool isPhaseStopControl;
std::string dmpName;
+ TaskSpaceDMPControllerConfig config;
+
private:
@@ -178,7 +182,6 @@ namespace armarx
DMP::UMITSMPPtr dmpPtr;
DMP::Vec<DMP::DMPState > currentState;
- TaskSpaceDMPControllerConfig config;
bool isDisturbance;
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
index 37dcd2291..ec9bf0b23 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
@@ -71,19 +71,7 @@ namespace armarx
initSensorData.currentTwist.setZero();
controllerSensorData.reinitAllBuffers(initSensorData);
- targetVels.resize(6);
- NJointTSDMPControllerControlData initData;
- initData.targetTSVel.resize(6);
- for (size_t i = 0; i < 6; ++i)
- {
- initData.targetTSVel(i) = 0;
- targetVels(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);
+
debugName = cfg->debugName;
@@ -111,6 +99,12 @@ namespace armarx
jointLowLimits(i) = rn->getJointLimitLo();
jointHighLimits(i) = rn->getJointLimitHi();
}
+
+ started = false;
+
+ NJointTSDMPControllerInterfaceData initInterfaceData;
+ initInterfaceData.currentTcpPose = tcp->getPoseInRootFrame();
+ interfaceData.reinitAllBuffers(initInterfaceData);
}
std::string NJointTSDMPController::getClassName(const Ice::Current&) const
@@ -120,6 +114,11 @@ namespace armarx
void NJointTSDMPController::controllerRun()
{
+ if (!started)
+ {
+ return;
+ }
+
if (!controllerSensorData.updateReadBuffer() || !taskSpaceDMPController)
{
return;
@@ -128,6 +127,8 @@ namespace armarx
Eigen::Matrix4f currentPose = controllerSensorData.getReadBuffer().currentPose;
Eigen::VectorXf currentTwist = controllerSensorData.getReadBuffer().currentTwist;
+
+ LockGuardType guard {controllerMutex};
taskSpaceDMPController->flow(deltaT, currentPose, currentTwist);
@@ -140,6 +141,7 @@ namespace armarx
std::vector<double> targetState = taskSpaceDMPController->getTargetPose();
// ARMARX_IMPORTANT << "CanVal: " << taskSpaceDMPController->canVal;
+ // ARMARX_IMPORTANT << "currentPose: " << currentPose;
// ARMARX_IMPORTANT << "targetVels: " << targetVels;
// ARMARX_IMPORTANT << "targetPose: " << targetPose;
@@ -179,16 +181,30 @@ namespace armarx
writeControlStruct();
- NJointTSDMPControllerInterfaceData initInterfaceData;
- initInterfaceData.currentTcpPose = Eigen::Matrix4f::Identity();
- interfaceData.reinitAllBuffers(initInterfaceData);
}
void NJointTSDMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
{
Eigen::Matrix4f currentPose = tcp->getPoseInRootFrame();
+ interfaceData.getWriteBuffer().currentTcpPose = currentPose;
+ interfaceData.commitWrite();
+ if (firstRun)
+ {
+ filtered_position = currentPose.block<3, 1>(0, 3);
+
+ firstRun = false;
+ for (size_t i = 0; i < targets.size(); ++i)
+ {
+ targets.at(i)->velocity = 0;
+ }
+ return;
+ }
+ else
+ {
+ filtered_position = (1 - pos_filter_factor) * filtered_position + pos_filter_factor * currentPose.block<3, 1>(0, 3);
+ }
double deltaT = timeSinceLastIteration.toSecondsDouble();
@@ -213,25 +229,27 @@ namespace armarx
controllerSensorData.getWriteBuffer().currentTime += deltaT;
controllerSensorData.commitWrite();
- interfaceData.getWriteBuffer().currentTcpPose = currentPose;
- interfaceData.commitWrite();
+
Eigen::VectorXf targetVel = rtGetControlStruct().targetTSVel;
Eigen::Matrix4f targetPose = rtGetControlStruct().targetPose;
-
- Eigen::Matrix3f diffMat = targetPose.block<3, 3>(0, 0) * currentPose.block<3, 3>(0, 0).inverse();
- Eigen::Vector3f errorRPY = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
-
- if (firstRun)
+ if (started)
{
- filtered_position = currentPose.block<3, 1>(0, 3);
- firstRun = false;
+ targetVel = rtGetControlStruct().targetTSVel;
+ targetPose = rtGetControlStruct().targetPose;
}
else
{
- filtered_position = (1 - pos_filter_factor) * filtered_position + pos_filter_factor * currentPose.block<3, 1>(0, 3);
+ targetVel.setZero(6);
+ targetPose = currentPose;
}
+
+ ARMARX_IMPORTANT << "targetPose: " << targetPose;
+
+ Eigen::Matrix3f diffMat = targetPose.block<3, 3>(0, 0) * currentPose.block<3, 3>(0, 0).inverse();
+ Eigen::Vector3f errorRPY = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
+
Eigen::VectorXf rtTargetVel;
rtTargetVel.resize(6);
rtTargetVel.block<3, 1>(0, 0) = KpF * (targetPose.block<3, 1>(0, 3) - filtered_position) + DpF * (targetVel.block<3, 1>(0, 0) - tcptwist.block<3, 1>(0, 0));
@@ -298,17 +316,6 @@ namespace armarx
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);
@@ -325,11 +332,6 @@ namespace armarx
targets.at(i)->velocity = 0.0f;
}
- // if (qpos(i) > jointHighLimits(i) || qpos(i) < jointLowLimits(i))
- // {
- // targets.at(i)->velocity = 0;
- // }
-
}
@@ -345,17 +347,24 @@ namespace armarx
void NJointTSDMPController::setSpeed(Ice::Double times, const Ice::Current&)
{
- LockGuardType guard(controllerMutex);
+ LockGuardType guard {controllerMutex};
taskSpaceDMPController->setSpeed(times);
}
void NJointTSDMPController::setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&)
{
- LockGuardType guard(controllerMutex);
+ LockGuardType guard {controllerMutex};
ARMARX_INFO << "setting via points ";
taskSpaceDMPController->setViaPose(u, viapoint);
}
+ void NJointTSDMPController::removeAllViaPoints(const Ice::Current&)
+ {
+ LockGuardType guard {controllerMutex};
+ ARMARX_INFO << "setting via points ";
+ taskSpaceDMPController->removeAllViaPoints();
+ }
+
void NJointTSDMPController::setGoals(const Ice::DoubleSeq& goals, const Ice::Current& ice)
{
@@ -409,25 +418,47 @@ namespace armarx
writeControlStruct();
}
+ void NJointTSDMPController::resetDMP(const Ice::Current&)
+ {
+ if (started)
+ {
+ ARMARX_INFO << "Cannot reset running DMP";
+ }
+ firstRun = true;
+ }
+
+ void NJointTSDMPController::stopDMP(const Ice::Current&)
+ {
+ started = false;
+ firstRun = true;
+ }
+
void NJointTSDMPController::runDMP(const Ice::DoubleSeq& goals, double tau, const Ice::Current&)
{
+ firstRun = true;
+ while (firstRun)
+ {
+ usleep(100);
+ }
while (!interfaceData.updateReadBuffer())
{
usleep(100);
}
Eigen::Matrix4f pose = interfaceData.getReadBuffer().currentTcpPose;
-
+ ARMARX_INFO << "current pose: " << pose;
// Eigen::Matrix4f pose = tcp->getPoseInRootFrame();
- taskSpaceDMPController->prepareExecution(taskSpaceDMPController->eigen4f2vec(pose), goals);
+ LockGuardType guard {controllerMutex};
+ taskSpaceDMPController->prepareExecution(taskSpaceDMPController->eigen4f2vec(pose), goals, tau);
finished = false;
ARMARX_INFO << "run DMP";
- controllerTask->start();
-
+ started = true;
}
+
+
void NJointTSDMPController::rtPreActivateController()
{
}
@@ -482,7 +513,27 @@ namespace armarx
void NJointTSDMPController::onInitComponent()
{
ARMARX_INFO << "init ...";
+ targetVels.resize(6);
+ NJointTSDMPControllerControlData initData;
+ initData.targetTSVel.resize(6);
+ for (size_t i = 0; i < 6; ++i)
+ {
+ initData.targetTSVel(i) = 0;
+ targetVels(i) = 0;
+ initData.targetPose = tcp->getPoseInRootFrame();
+ }
+ 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);
+
+
+
controllerTask = new PeriodicTask<NJointTSDMPController>(this, &NJointTSDMPController::controllerRun, 0.3);
+ controllerTask->start();
+
+
}
void NJointTSDMPController::onDisconnectComponent()
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
index 498ec423e..e50b9890b 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
@@ -79,6 +79,7 @@ namespace armarx
void runDMP(const Ice::DoubleSeq& goals, Ice::Double tau, const Ice::Current&) override;
void setSpeed(Ice::Double times, const Ice::Current&) override;
void setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&) override;
+ void removeAllViaPoints(const Ice::Current&) override;
void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&) override;
@@ -86,9 +87,12 @@ namespace armarx
void setTorqueKp(const StringFloatDictionary& torqueKp, const Ice::Current&) override;
void setNullspaceJointVelocities(const StringFloatDictionary& nullspaceJointVelocities, const Ice::Current&) override;
+
+ void resetDMP(const Ice::Current&) override;
+ void stopDMP(const Ice::Current&) override;
double getCanVal(const Ice::Current&) override
{
- return taskSpaceDMPController->canVal;
+ return taskSpaceDMPController->canVal / cfg->timeDuration;
}
protected:
@@ -150,6 +154,7 @@ namespace armarx
// dmp parameters
bool finished;
+ bool started;
VirtualRobot::DifferentialIKPtr ik;
VirtualRobot::RobotNodePtr tcp;
--
GitLab