diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
index 751814b46d3f9f24e2087ac0c19b6cba422f2bea..dfb40405adbc4a8613233833a252145fbc8d2c57 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
@@ -129,7 +129,7 @@ namespace armarx
return;
}
- double deltaT = controllerSensorData.getReadBuffer().deltaT;
+ double deltaT = 0.001; //controllerSensorData.getReadBuffer().deltaT;
Eigen::Matrix4f currentPose = controllerSensorData.getReadBuffer().currentPose;
Eigen::VectorXf currentTwist = controllerSensorData.getReadBuffer().currentTwist;
@@ -162,6 +162,9 @@ namespace armarx
getWriterControlStruct().desiredNullSpaceJointValues = desiredNullSpaceJointValues;
getWriterControlStruct().targetVel = dmpCtrl->getTargetVelocity();
getWriterControlStruct().targetPose = dmpCtrl->getTargetPoseMat();
+ getWriterControlStruct().canVal = dmpCtrl->canVal;
+ getWriterControlStruct().mpcFactor = dmpCtrl->debugData.mpcFactor;
+
writeControlStruct();
}
@@ -204,7 +207,7 @@ namespace armarx
currentTCPLinearVelocity << 0.001 * currentTwist(0), 0.001 * currentTwist(1), 0.001 * currentTwist(2);
Eigen::Vector3f currentTCPPosition = currentPose.block<3, 1>(0, 3);
Eigen::Vector3f desiredPosition = targetPose.block<3, 1>(0, 3);
- Eigen::Vector3f tcpDesiredForce = 0.001 * kpos.cwiseProduct(desiredPosition - currentTCPPosition) + dpos.cwiseProduct(targetTCPLinearVelocity - currentTCPLinearVelocity);
+ Eigen::Vector3f tcpDesiredForce = 0.001 * kpos.cwiseProduct(desiredPosition - currentTCPPosition) + dpos.cwiseProduct(- currentTCPLinearVelocity);
Eigen::Vector3f currentTCPAngularVelocity;
currentTCPAngularVelocity << currentTwist(3), currentTwist(4), currentTwist(5);
@@ -222,6 +225,10 @@ namespace armarx
Eigen::MatrixXf jtpinv = ik->computePseudoInverseJacobianMatrix(jacobi.transpose(), 2.0);
Eigen::VectorXf jointDesiredTorques = jacobi.transpose() * jointControlWrench + (I - jacobi.transpose() * jtpinv) * nullspaceTorque;
+
+
+
+
// torque limit
ARMARX_CHECK_EXPRESSION(!targets.empty());
ARMARX_CHECK_LESS(targets.size(), 1000);
@@ -248,7 +255,17 @@ namespace armarx
}
}
- debugOutputData.getWriteBuffer().currentCanVal = dmpCtrl->debugData.canVal;
+
+ debugOutputData.getWriteBuffer().forceDesired_x = jointControlWrench(0);
+ debugOutputData.getWriteBuffer().forceDesired_y = jointControlWrench(1);
+ debugOutputData.getWriteBuffer().forceDesired_z = jointControlWrench(2);
+ debugOutputData.getWriteBuffer().forceDesired_rx = jointControlWrench(3);
+ debugOutputData.getWriteBuffer().forceDesired_ry = jointControlWrench(4);
+ debugOutputData.getWriteBuffer().forceDesired_rz = jointControlWrench(5);
+
+ debugOutputData.getWriteBuffer().currentCanVal = rtGetControlStruct().canVal;
+ debugOutputData.getWriteBuffer().mpcfactor = rtGetControlStruct().mpcFactor;
+
debugOutputData.getWriteBuffer().targetPose_x = targetPose(0, 3);
debugOutputData.getWriteBuffer().targetPose_y = targetPose(1, 3);
debugOutputData.getWriteBuffer().targetPose_z = targetPose(2, 3);
@@ -258,10 +275,6 @@ namespace armarx
debugOutputData.getWriteBuffer().targetPose_qy = targetQuat.y;
debugOutputData.getWriteBuffer().targetPose_qz = targetQuat.z;
-
-
-
-
debugOutputData.getWriteBuffer().currentPose_x = currentPose(0, 3);
debugOutputData.getWriteBuffer().currentPose_y = currentPose(1, 3);
debugOutputData.getWriteBuffer().currentPose_z = currentPose(2, 3);
@@ -270,6 +283,7 @@ namespace armarx
debugOutputData.getWriteBuffer().currentPose_qx = currentQuat.x;
debugOutputData.getWriteBuffer().currentPose_qy = currentQuat.y;
debugOutputData.getWriteBuffer().currentPose_qz = currentQuat.z;
+ debugOutputData.getWriteBuffer().deltaT = deltaT;
debugOutputData.commitWrite();
@@ -358,17 +372,17 @@ namespace armarx
auto values = debugOutputData.getUpToDateReadBuffer().desired_torques;
for (auto& pair : values)
{
- datafields[pair.first] = new Variant(pair.second);
+ datafields["torqueDesired_" + pair.first] = new Variant(pair.second);
}
auto values_null = debugOutputData.getUpToDateReadBuffer().desired_nullspaceJoint;
for (auto& pair : values_null)
{
- datafields[pair.first] = new Variant(pair.second);
+ datafields["nullspaceDesired_" + pair.first] = new Variant(pair.second);
}
datafields["canVal"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentCanVal);
-
+ datafields["mpcfactor"] = new Variant(debugOutputData.getUpToDateReadBuffer().mpcfactor);
datafields["targetPose_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_x);
datafields["targetPose_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_y);
datafields["targetPose_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_z);
@@ -385,6 +399,14 @@ namespace armarx
datafields["currentPose_qy"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_qy);
datafields["currentPose_qz"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_qz);
+ 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);
+ datafields["forceDesired_rx"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_rx);
+ datafields["forceDesired_ry"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_ry);
+ datafields["forceDesired_rz"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_rz);
+
+ datafields["deltaT"] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaT);
std::string channelName = cfg->nodeSetName + "_TaskSpaceImpedanceControl";
debugObs->setDebugChannel(channelName, datafields);
@@ -393,7 +415,7 @@ namespace armarx
void NJointTaskSpaceImpedanceDMPController::onInitComponent()
{
ARMARX_INFO << "init ...";
- controllerTask = new PeriodicTask<NJointTaskSpaceImpedanceDMPController>(this, &NJointTaskSpaceImpedanceDMPController::controllerRun, 0.3);
+ controllerTask = new PeriodicTask<NJointTaskSpaceImpedanceDMPController>(this, &NJointTaskSpaceImpedanceDMPController::controllerRun, 1);
}
void NJointTaskSpaceImpedanceDMPController::onDisconnectComponent()
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
index ff12ea42b68a0461179d6e6a4aa62470723c7c71..27737f7269d18c8ad20024613f8b23c09ab2c331 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
@@ -27,6 +27,8 @@ namespace armarx
Eigen::VectorXf targetVel;
Eigen::Matrix4f targetPose;
Eigen::VectorXf desiredNullSpaceJointValues;
+ double canVal;
+ double mpcFactor;
};
@@ -78,6 +80,7 @@ namespace armarx
struct DebugBufferData
{
double currentCanVal;
+ double mpcfactor;
float targetPose_x;
float targetPose_y;
float targetPose_z;
@@ -96,6 +99,14 @@ namespace armarx
StringFloatDictionary desired_torques;
StringFloatDictionary desired_nullspaceJoint;
+ float forceDesired_x;
+ float forceDesired_y;
+ float forceDesired_z;
+ float forceDesired_rx;
+ float forceDesired_ry;
+ float forceDesired_rz;
+
+ float deltaT;
};