Merge branch 'master' of https://gitlab.com/ArmarX/RobotAPI

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;

source/RobotAPI/libraries/core/CartesianVelocityController.cpp

@@ -33,7 +33,7 @@ using namespace armarx;
 CartesianVelocityController::CartesianVelocityController(const VirtualRobot::RobotNodeSetPtr& rns, const VirtualRobot::RobotNodePtr& tcp)
     : rns(rns)
 {
-    ARMARX_IMPORTANT << VAROUT(rns->getRobot()->getRootNode());
+    //ARMARX_IMPORTANT << VAROUT(rns->getRobot()->getRootNode());
     ik.reset(new VirtualRobot::DifferentialIK(rns, rns->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
     this->tcp = tcp ? tcp : rns->getTCP();