diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
index d39e94c87ce0ea5d5ed5b21e252d31f1e2ca41f9..b00a86caf56aec6a6e3f6c59e734ded8aea893b8 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
@@ -345,10 +345,12 @@ module armarx
void setMPWeights(DoubleSeqSeq weights);
DoubleSeqSeq getMPWeights();
- void setLinearVelocityKd(Ice::FloatSeq kd);
- void setLinearVelocityKp(Ice::FloatSeq kp);
- void setAngularVelocityKd(Ice::FloatSeq kd);
- void setAngularVelocityKp(Ice::FloatSeq kp);
+ void setLinearVelocityKd(Eigen::Vector3f kd);
+ void setLinearVelocityKp(Eigen::Vector3f kp);
+ void setAngularVelocityKd(Eigen::Vector3f kd);
+ void setAngularVelocityKp(Eigen::Vector3f kp);
+ void setNullspaceVelocityKd(Eigen::VectorXf jointValues);
+ void setNullspaceVelocityKp(Eigen::VectorXf jointValues);
};
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
index 6bd72fd26162491f6128f3fddce78b7eced8c1f3..762d3f240eb99b6f37b4ecf7d47b63a55e7932f0 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
@@ -84,22 +84,21 @@ namespace armarx
Eigen::Vector3f dpos(cfg->Dpos[0], cfg->Dpos[1], cfg->Dpos[2]);
Eigen::Vector3f kori(cfg->Kori[0], cfg->Kori[1], cfg->Kori[2]);
Eigen::Vector3f dori(cfg->Dori[0], cfg->Dori[1], cfg->Dori[2]);
-
- CtrlParams initParams = {kpos, dpos, kori, dori};
- ctrlParams.reinitAllBuffers(initParams);
+ Eigen::VectorXf knull(targets.size());
+ Eigen::VectorXf dnull(targets.size());
ARMARX_CHECK_EQUAL(cfg->Knull.size(), targets.size());
ARMARX_CHECK_EQUAL(cfg->Dnull.size(), targets.size());
- knull.setZero(targets.size());
- dnull.setZero(targets.size());
-
for (size_t i = 0; i < targets.size(); ++i)
{
knull(i) = cfg->Knull.at(i);
dnull(i) = cfg->Dnull.at(i);
}
+ CtrlParams initParams = {kpos, dpos, kori, dori, knull, dnull};
+ ctrlParams.reinitAllBuffers(initParams);
+
torqueLimit = cfg->torqueLimit;
timeDuration = cfg->timeDuration;
@@ -284,6 +283,8 @@ namespace armarx
Eigen::Vector3f dpos = ctrlParams.getUpToDateReadBuffer().dpos;
Eigen::Vector3f kori = ctrlParams.getUpToDateReadBuffer().kori;
Eigen::Vector3f dori = ctrlParams.getUpToDateReadBuffer().dori;
+ Eigen::Vector3f knull = ctrlParams.getUpToDateReadBuffer().knull;
+ Eigen::Vector3f dnull = ctrlParams.getUpToDateReadBuffer().dnull;
Eigen::Vector6f jointControlWrench;
{
@@ -375,6 +376,26 @@ namespace armarx
debugOutputData.getWriteBuffer().currentPose_qz = currentQuat.z;
debugOutputData.getWriteBuffer().deltaT = deltaT;
+ debugOutputData.getWriteBuffer().currentKpos_x = kpos.x();
+ debugOutputData.getWriteBuffer().currentKpos_y = kpos.y();
+ debugOutputData.getWriteBuffer().currentKpos_z = kpos.z();
+ debugOutputData.getWriteBuffer().currentKori_x = kori.x();
+ debugOutputData.getWriteBuffer().currentKori_y = kori.y();
+ debugOutputData.getWriteBuffer().currentKori_z = kori.z();
+ debugOutputData.getWriteBuffer().currentKnull_x = knull.x();
+ debugOutputData.getWriteBuffer().currentKnull_y = knull.y();
+ debugOutputData.getWriteBuffer().currentKnull_z = knull.z();
+
+ debugOutputData.getWriteBuffer().currentDpos_x = dpos.x();
+ debugOutputData.getWriteBuffer().currentDpos_y = dpos.y();
+ debugOutputData.getWriteBuffer().currentDpos_z = dpos.z();
+ debugOutputData.getWriteBuffer().currentDori_x = dori.x();
+ debugOutputData.getWriteBuffer().currentDori_y = dori.y();
+ debugOutputData.getWriteBuffer().currentDori_z = dori.z();
+ debugOutputData.getWriteBuffer().currentDnull_x = dnull.x();
+ debugOutputData.getWriteBuffer().currentDnull_y = dnull.y();
+ debugOutputData.getWriteBuffer().currentDnull_z = dnull.z();
+
debugOutputData.commitWrite();
}
@@ -548,6 +569,26 @@ namespace armarx
datafields["currentPose_qy"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_qy);
datafields["currentPose_qz"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_qz);
+ datafields["currentKpos_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentKpos_x);
+ datafields["currentKpos_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentKpos_y);
+ datafields["currentKpos_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentKpos_z);
+ datafields["currentKori_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentKori_x);
+ datafields["currentKori_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentKori_y);
+ datafields["currentKori_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentKori_z);
+ datafields["currentKnull_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentKnull_x);
+ datafields["currentKnull_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentKnull_y);
+ datafields["currentKnull_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentKnull_z);
+
+ datafields["currentDpos_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentDpos_x);
+ datafields["currentDpos_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentDpos_y);
+ datafields["currentDpos_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentDpos_z);
+ datafields["currentDori_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentDori_x);
+ datafields["currentDori_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentDori_y);
+ datafields["currentDori_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentDori_z);
+ datafields["currentDnull_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentDnull_x);
+ datafields["currentDnull_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentDnull_y);
+ datafields["currentDnull_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentDnull_z);
+
datafields["forceDesired_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_x);
datafields["forceDesired_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_y);
datafields["forceDesired_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_z);
@@ -614,42 +655,64 @@ namespace armarx
dmpCtrl->removeAllViaPoints();
}
- void NJointTaskSpaceImpedanceDMPController::setLinearVelocityKd(const Ice::FloatSeq& kd, const Ice::Current&)
+ void NJointTaskSpaceImpedanceDMPController::setLinearVelocityKd(const Eigen::Vector3f& kd, const Ice::Current&)
{
ARMARX_CHECK_EQUAL(kd.size(), 3);
+ ARMARX_INFO << "set linear kd " << VAROUT(kd);
LockGuardType guard(controllerMutex);
- ctrlParams.getWriteBuffer().dpos << kd[0], kd[1], kd[2];
+ ctrlParams.getWriteBuffer().dpos = kd;
ctrlParams.commitWrite();
}
- void NJointTaskSpaceImpedanceDMPController::setLinearVelocityKp(const Ice::FloatSeq& kp, const Ice::Current&)
+ void NJointTaskSpaceImpedanceDMPController::setLinearVelocityKp(const Eigen::Vector3f& kp, const Ice::Current&)
{
ARMARX_CHECK_EQUAL(kp.size(), 3);
+ ARMARX_INFO << "set linear kp " << VAROUT(kp);
LockGuardType guard(controllerMutex);
- ctrlParams.getWriteBuffer().kpos << kp[0], kp[1], kp[2];
+ ctrlParams.getWriteBuffer().kpos = kp;
ctrlParams.commitWrite();
}
- void NJointTaskSpaceImpedanceDMPController::setAngularVelocityKd(const Ice::FloatSeq& kd, const Ice::Current&)
+ void NJointTaskSpaceImpedanceDMPController::setAngularVelocityKd(const Eigen::Vector3f& kd, const Ice::Current&)
{
ARMARX_CHECK_EQUAL(kd.size(), 3);
+ ARMARX_INFO << "set angular kd " << VAROUT(kd);
LockGuardType guard(controllerMutex);
- ctrlParams.getWriteBuffer().dori << kd[0], kd[1], kd[2];
+ ctrlParams.getWriteBuffer().dori = kd;
ctrlParams.commitWrite();
-
}
- void NJointTaskSpaceImpedanceDMPController::setAngularVelocityKp(const Ice::FloatSeq& kp, const Ice::Current&)
+ void NJointTaskSpaceImpedanceDMPController::setAngularVelocityKp(const Eigen::Vector3f& kp, const Ice::Current&)
{
ARMARX_CHECK_EQUAL(kp.size(), 3);
+ ARMARX_INFO << "set angular kp " << VAROUT(kp);
LockGuardType guard(controllerMutex);
- ctrlParams.getWriteBuffer().kori << kp[0], kp[1], kp[2];
+ ctrlParams.getWriteBuffer().kori = kp;
ctrlParams.commitWrite();
}
+ void NJointTaskSpaceImpedanceDMPController::setNullspaceVelocityKd(const Eigen::VectorXf& kd, const Ice::Current&)
+ {
+ ARMARX_CHECK_EQUAL(kd.size(), targets.size());
+ ARMARX_INFO << "set nullspace kd " << VAROUT(kd);
+ LockGuardType guard(controllerMutex);
+ ctrlParams.getWriteBuffer().dnull = kd;
+ ctrlParams.commitWrite();
+ }
+
+ void NJointTaskSpaceImpedanceDMPController::setNullspaceVelocityKp(const Eigen::VectorXf& kp, const Ice::Current&)
+ {
+ ARMARX_CHECK_EQUAL(kp.size(), targets.size());
+ ARMARX_INFO << "set linear kp " << VAROUT(kp);
+ LockGuardType guard(controllerMutex);
+ ctrlParams.getWriteBuffer().knull = kp;
+ ctrlParams.commitWrite();
+ }
+
+
void NJointTaskSpaceImpedanceDMPController::setDefaultNullSpaceJointValues(const Eigen::VectorXf& jointValues,
const Ice::Current&)
{
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
index ea8cef187de6296e07eeb270e4ca5ac2ad7b78f6..9ded363e9e110bf803ada28b5523057b69b3bd4f 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
@@ -76,10 +76,12 @@ namespace armarx
void removeAllViaPoints(const Ice::Current&) override;
- void setLinearVelocityKd(const Ice::FloatSeq& kd, const Ice::Current&) override;
- void setLinearVelocityKp(const Ice::FloatSeq& kp, const Ice::Current&) override;
- void setAngularVelocityKd(const Ice::FloatSeq& kd, const Ice::Current&) override;
- void setAngularVelocityKp(const Ice::FloatSeq& kp, const Ice::Current&) override;
+ void setLinearVelocityKd(const Eigen::Vector3f& kd, const Ice::Current&) override;
+ void setLinearVelocityKp(const Eigen::Vector3f& kp, const Ice::Current&) override;
+ void setAngularVelocityKd(const Eigen::Vector3f& kd, const Ice::Current&) override;
+ void setAngularVelocityKp(const Eigen::Vector3f& kp, const Ice::Current&) override;
+ void setNullspaceVelocityKd(const Eigen::VectorXf& kd, const Ice::Current&) override;
+ void setNullspaceVelocityKp(const Eigen::VectorXf& kp, const Ice::Current&) override;
void setUseNullSpaceJointDMP(bool enable, const Ice::Current&) override;
void setDefaultNullSpaceJointValues(const Eigen::VectorXf& jointValues, const Ice::Current&) override;
@@ -112,6 +114,26 @@ namespace armarx
float currentPose_qy;
float currentPose_qz;
+ float currentKpos_x;
+ float currentKpos_y;
+ float currentKpos_z;
+ float currentKori_x;
+ float currentKori_y;
+ float currentKori_z;
+ float currentKnull_x;
+ float currentKnull_y;
+ float currentKnull_z;
+
+ float currentDpos_x;
+ float currentDpos_y;
+ float currentDpos_z;
+ float currentDori_x;
+ float currentDori_y;
+ float currentDori_z;
+ float currentDnull_x;
+ float currentDnull_y;
+ float currentDnull_z;
+
StringFloatDictionary desired_torques;
StringFloatDictionary desired_nullspaceJoint;
float forceDesired_x;
@@ -123,9 +145,11 @@ namespace armarx
float deltaT;
+
+
};
- TripleBuffer<DebugBufferData> debugOutputData;
+ WriteBufferedTripleBuffer<DebugBufferData> debugOutputData;
struct NJointTaskSpaceImpedanceDMPControllerSensorData
{
@@ -134,14 +158,14 @@ namespace armarx
Eigen::Matrix4f currentPose;
Eigen::VectorXf currentTwist;
};
- TripleBuffer<NJointTaskSpaceImpedanceDMPControllerSensorData> controllerSensorData;
+ WriteBufferedTripleBuffer<NJointTaskSpaceImpedanceDMPControllerSensorData> controllerSensorData;
struct NJointTaskSpaceImpedanceDMPControllerInterfaceData
{
Eigen::Matrix4f currentTcpPose;
};
- TripleBuffer<NJointTaskSpaceImpedanceDMPControllerInterfaceData> interfaceData;
+ WriteBufferedTripleBuffer<NJointTaskSpaceImpedanceDMPControllerInterfaceData> interfaceData;
struct CtrlParams
{
@@ -149,9 +173,11 @@ namespace armarx
Eigen::Vector3f dpos;
Eigen::Vector3f kori;
Eigen::Vector3f dori;
+ Eigen::VectorXf knull;
+ Eigen::VectorXf dnull;
};
- TripleBuffer<CtrlParams> ctrlParams;
+ WriteBufferedTripleBuffer<CtrlParams> ctrlParams;
DMP::Vec<DMP::DMPState> currentJointState;
@@ -188,15 +214,15 @@ namespace armarx
// Eigen::Vector3f kori;
// Eigen::Vector3f dpos;
// Eigen::Vector3f dori;
- Eigen::VectorXf knull;
- Eigen::VectorXf dnull;
+ // Eigen::VectorXf knull;
+ // Eigen::VectorXf dnull;
int numOfJoints;
std::atomic_bool useNullSpaceJointDMP;
bool isNullSpaceJointDMPLearned;
- armarx::TripleBuffer<Eigen::VectorXf> defaultNullSpaceJointValues;
+ WriteBufferedTripleBuffer<Eigen::VectorXf> defaultNullSpaceJointValues;
std::vector<std::string> jointNames;
mutable MutexType controllerMutex;
PeriodicTask<NJointTaskSpaceImpedanceDMPController>::pointer_type controllerTask;