added getPositionDiff and getOrientationDiff

source/RobotAPI/libraries/core/CartesianPositionController.cpp

@@ -83,3 +83,19 @@ float CartesianPositionController::getOrientationError(const Eigen::Matrix4f& ta
     Eigen::AngleAxisf aa(oriDir);
     return aa.angle();
 }
+
+Eigen::Vector3f CartesianPositionController::getPositionDiff(const Eigen::Matrix4f& targetPose)
+{
+    Eigen::Vector3f targetPos = targetPose.block<3, 1>(0, 3);
+    return targetPos - tcp->getPositionInRootFrame();
+
+}
+
+Eigen::Vector3f CartesianPositionController::getOrientationDiff(const Eigen::Matrix4f& targetPose)
+{
+    Eigen::Matrix3f targetOri = targetPose.block<3, 3>(0, 0);
+    Eigen::Matrix3f tcpOri = tcp->getPoseInRootFrame().block<3, 3>(0, 0);
+    Eigen::Matrix3f oriDir = targetOri * tcpOri.inverse();
+    Eigen::AngleAxisf aa(oriDir);
+    return aa.axis() * aa.angle();
+}

source/RobotAPI/libraries/core/CartesianPositionController.h

@@ -45,6 +45,8 @@ namespace armarx
 
         float getPositionError(const Eigen::Matrix4f& targetPose);
         float getOrientationError(const Eigen::Matrix4f& targetPose);
+        Eigen::Vector3f getPositionDiff(const Eigen::Matrix4f& targetPose);
+        Eigen::Vector3f getOrientationDiff(const Eigen::Matrix4f& targetPose);
 
         //CartesianVelocityController velocityController;
         float KpPos = 1;