From 1741ed76aad755e72454d9ad4b71df75cadab33d Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Fri, 11 Jun 2021 15:54:17 +0200
Subject: [PATCH] cleanup
---
...onomicPlatformGlobalPositionController.cpp | 20 +++----------------
1 file changed, 3 insertions(+), 17 deletions(-)
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
index 249520b76..f11e5e156 100755
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
@@ -80,24 +80,10 @@ namespace armarx
return;
}
+ const float measuredOrientation = rtGetControlStruct().globalOrientation;
- const Eigen::Vector2f measuredPosition = rtGetControlStruct().globalPosition;// + relativeCurrentPosition;
- const float measuredOrientation = rtGetControlStruct().globalOrientation;//+ relativeOrientation;
-
- const float targetOrientation = rtGetControlStruct().targetOrientation;
-
- // measured and target orientation are now within bounds [-pi, pi]
-
- // in which orientation to rotate? => angleBetween is not reliable yet!
- // float angleBetween = targetOrientation - measuredOrientation;
-
- // angleBetween = simox::math::periodic_clamp(angleBetween, -M_PIf32, M_PIf32);
- // targetOrientation = measuredOrientation + angleBetween;
- // -> now angleBetween is reliable
-
- pid.update(timeSinceLastIteration.toSecondsDouble(), measuredPosition, rtGetControlStruct().target);
- //opid.update(timeSinceLastIteration.toSecondsDouble(), static_cast<double>(updatedOrientation), rtGetControlStruct().targetOrientation);
- opid.update(timeSinceLastIteration.toSecondsDouble(), static_cast<double>(measuredOrientation), targetOrientation);
+ pid.update(timeSinceLastIteration.toSecondsDouble(), rtGetControlStruct().globalPosition, rtGetControlStruct().target);
+ opid.update(timeSinceLastIteration.toSecondsDouble(), static_cast<double>(measuredOrientation), rtGetControlStruct().targetOrientation);
const Eigen::Rotation2Df global_R_local(-measuredOrientation);
--
GitLab