Add error methods to CompositeDiffIK::Target

source/RobotAPI/libraries/diffik/CompositeDiffIK.h

@@ -22,6 +22,8 @@
  */
 
 #pragma once
+#include <SimoxUtility/math/distance/delta_angle.h>
+
 #include <VirtualRobot/Nodes/RobotNode.h>
 #include <VirtualRobot/RobotNodeSet.h>
 #include <VirtualRobot/IK/DifferentialIK.h>
@@ -146,6 +148,19 @@ namespace armarx
             float maxOriError = 0.05f;
 
             std::vector<TargetStep> ikSteps;
+
+            Eigen::Matrix4f getPoseError() const
+            {
+                return tcp->getPoseInRootFrame().inverse() * target;
+            }
+            Eigen::Vector3f getPosError() const
+            {
+                return (tcp->getPoseInRootFrame().inverse() * target).topRightCorner<3, 1>();
+            }
+            auto getOriError() const
+            {
+                return simox::math::delta_angle(tcp->getPoseInRootFrame(), target);
+            }
         };
         typedef std::shared_ptr<Target> TargetPtr;