From 8eb05a3cfa1aee652409ecd3840dd70fdaae489e Mon Sep 17 00:00:00 2001
From: armar6-user <armar6-user@kit.edu>
Date: Wed, 4 Apr 2018 16:50:53 +0200
Subject: [PATCH] added getPositionDiff and getOrientationDiff
---
.../core/CartesianPositionController.cpp | 16 ++++++++++++++++
.../libraries/core/CartesianPositionController.h | 2 ++
2 files changed, 18 insertions(+)
diff --git a/source/RobotAPI/libraries/core/CartesianPositionController.cpp b/source/RobotAPI/libraries/core/CartesianPositionController.cpp
index 8caa1a53b..1c49e462d 100644
--- a/source/RobotAPI/libraries/core/CartesianPositionController.cpp
+++ b/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();
+}
diff --git a/source/RobotAPI/libraries/core/CartesianPositionController.h b/source/RobotAPI/libraries/core/CartesianPositionController.h
index c0d8642ee..dd592515b 100644
--- a/source/RobotAPI/libraries/core/CartesianPositionController.h
+++ b/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;
--
GitLab