added debug viewer for target pose

source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp

@@ -342,9 +342,7 @@ namespace armarx
 
 
 
-            debugRT.getWriteBuffer().filteredForce = filteredForceInRoot;
-            debugRT.getWriteBuffer().targetVel = targetVel;
-            debugRT.commitWrite();
+
 
         }
 
@@ -379,7 +377,8 @@ namespace armarx
         targetPose(2, 3) = targetPose(2, 3) > cfg->ws_z[0] ? targetPose(2, 3) : cfg->ws_z[0];
         targetPose(2, 3) = targetPose(2, 3) < cfg->ws_z[1] ? targetPose(2, 3) : cfg->ws_z[1];
 
-
+        debugRT.getWriteBuffer().targetPose = targetPose;
+        debugRT.commitWrite();
 
         //            Eigen::Matrix3f rotVel = VirtualRobot::MathTools::rpy2eigen3f(targetVel(3) * dTf, targetVel(4) * dTf, targetVel(5) * dTf);
         //            targetPose.block<3, 3>(0, 0) = rotVel * targetPose.block<3, 3>(0, 0);
@@ -523,18 +522,23 @@ namespace armarx
         std::string debugName = "Periodic";
         StringVariantBaseMap datafields;
 
-        Eigen::Vector3f filteredForce = debugRT.getUpToDateReadBuffer().filteredForce;
-        datafields["filtered_force_x"] = new Variant(filteredForce(0));
-        datafields["filtered_force_y"] = new Variant(filteredForce(1));
-        datafields["filtered_force_z"] = new Variant(filteredForce(2));
-
-        Eigen::VectorXf targetVel = debugRT.getUpToDateReadBuffer().targetVel;
-        datafields["targetVel_x"] = new Variant(targetVel(0));
-        datafields["targetVel_y"] = new Variant(targetVel(1));
-        datafields["targetVel_z"] = new Variant(targetVel(2));
-        datafields["targetVel_rx"] = new Variant(targetVel(3));
-        datafields["targetVel_ry"] = new Variant(targetVel(4));
-        datafields["targetVel_rz"] = new Variant(targetVel(5));
+        Eigen::Matrix4f targetPoseDebug = debugRT.getUpToDateReadBuffer().targetPose;
+        datafields["target_x"] = new Variant(targetPoseDebug(0, 3));
+        datafields["target_y"] = new Variant(targetPoseDebug(1, 3));
+        datafields["target_z"] = new Variant(targetPoseDebug(2, 3));
+
+        //        Eigen::Vector3f filteredForce = debugRT.getUpToDateReadBuffer().filteredForce;
+        //        datafields["filtered_force_x"] = new Variant(filteredForce(0));
+        //        datafields["filtered_force_y"] = new Variant(filteredForce(1));
+        //        datafields["filtered_force_z"] = new Variant(filteredForce(2));
+
+        //        Eigen::VectorXf targetVel = debugRT.getUpToDateReadBuffer().targetVel;
+        //        datafields["targetVel_x"] = new Variant(targetVel(0));
+        //        datafields["targetVel_y"] = new Variant(targetVel(1));
+        //        datafields["targetVel_z"] = new Variant(targetVel(2));
+        //        datafields["targetVel_rx"] = new Variant(targetVel(3));
+        //        datafields["targetVel_ry"] = new Variant(targetVel(4));
+        //        datafields["targetVel_rz"] = new Variant(targetVel(5));
 
         //        auto values = debugOutputData.getUpToDateReadBuffer().latestTargetVelocities;
         //        for (auto& pair : values)

source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.h

@@ -103,10 +103,10 @@ namespace armarx
 
         TripleBuffer<DebugBufferData> debugOutputData;
 
+
         struct DebugRTData
         {
-            Eigen::Vector3f filteredForce;
-            Eigen::VectorXf targetVel;
+            Eigen::Matrix4f targetPose;
         };
         TripleBuffer<DebugRTData> debugRT;