NJointTaskSpaceImpedanceDMPController

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()

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;
 
         };