From 1a5754e5323a0c095b56977fa7d895c3ffa07c4e Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Fri, 11 Jun 2021 15:51:41 +0200
Subject: [PATCH] bugfix: platform position controller is turning into wrong
direction
Tested: from orientation -1.3 to 1.5
---
...onomicPlatformGlobalPositionController.cpp | 34 +++++++++++--------
1 file changed, 20 insertions(+), 14 deletions(-)
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
index bc246422b..249520b76 100755
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
@@ -22,6 +22,8 @@
* GNU General Public License
*/
#include "NJointHolonomicPlatformGlobalPositionController.h"
+#include <cmath>
+#include <SimoxUtility/math/periodic/periodic_clamp.h>
namespace armarx
{
@@ -34,7 +36,7 @@ namespace armarx
const NJointHolonomicPlatformGlobalPositionControllerConfigPtr& cfg,
const VirtualRobot::RobotPtr&) :
pid(cfg->p, cfg->i, cfg->d, cfg->maxVelocity, cfg->maxAcceleration),
- opid(cfg->p_rot, cfg->i_rot, cfg->d_rot, cfg->maxRotationVelocity, cfg->maxRotationAcceleration)
+ opid(cfg->p_rot, cfg->i_rot, cfg->d_rot, cfg->maxRotationVelocity, cfg->maxRotationAcceleration, true)
{
const SensorValueBase* sv = useSensorValue(cfg->platformName);
@@ -78,24 +80,28 @@ namespace armarx
return;
}
- float relativeOrientation = currentOrientation - rtGetControlStruct().startOrientation;
- Eigen::Vector2f relativeCurrentPosition = currentPosition - rtGetControlStruct().startPosition;
- Eigen::Vector2f updatedPosition = rtGetControlStruct().globalPosition;// + relativeCurrentPosition;
- float updatedOrientation = rtGetControlStruct().globalOrientation;//+ relativeOrientation;
+ const Eigen::Vector2f measuredPosition = rtGetControlStruct().globalPosition;// + relativeCurrentPosition;
+ const float measuredOrientation = rtGetControlStruct().globalOrientation;//+ relativeOrientation;
- float relativeGlobalOrientation = rtGetControlStruct().globalOrientation - getWriterControlStruct().startOrientation;
- relativeGlobalOrientation = std::atan2(std::sin(relativeGlobalOrientation), std::cos(relativeGlobalOrientation));
+ const float targetOrientation = rtGetControlStruct().targetOrientation;
- float relativeTargetOrientation = rtGetControlStruct().targetOrientation - getWriterControlStruct().startOrientation;
- relativeTargetOrientation = std::atan2(std::sin(relativeTargetOrientation), std::cos(relativeTargetOrientation));
+ // measured and target orientation are now within bounds [-pi, pi]
+ // in which orientation to rotate? => angleBetween is not reliable yet!
+ // float angleBetween = targetOrientation - measuredOrientation;
- pid.update(timeSinceLastIteration.toSecondsDouble(), updatedPosition, rtGetControlStruct().target);
+ // 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>(relativeGlobalOrientation), relativeTargetOrientation);
+ opid.update(timeSinceLastIteration.toSecondsDouble(), static_cast<double>(measuredOrientation), targetOrientation);
+
+ const Eigen::Rotation2Df global_R_local(-measuredOrientation);
- Eigen::Vector2f velocities = Eigen::Rotation2Df(-updatedOrientation) * Eigen::Vector2f(pid.getControlValue()[0], pid.getControlValue()[1]);
+ Eigen::Vector2f velocities = global_R_local * Eigen::Vector2f(pid.getControlValue()[0], pid.getControlValue()[1]);
target->velocityX = velocities.x();
target->velocityY = velocities.y();
target->velocityRotation = static_cast<float>(opid.getControlValue());
@@ -112,7 +118,7 @@ namespace armarx
std::lock_guard<std::recursive_mutex> lock(controlDataMutex);
getWriterControlStruct().target << x, y;
- getWriterControlStruct().targetOrientation = std::atan2(std::sin(yaw), std::cos(yaw));
+ getWriterControlStruct().targetOrientation = simox::math::periodic_clamp(yaw, -M_PIf32, M_PIf32);
getWriterControlStruct().translationAccuracy = translationAccuracy;
getWriterControlStruct().rotationAccuracy = rotationAccuracy;
getWriterControlStruct().newTargetSet = true;
@@ -125,7 +131,7 @@ namespace armarx
// ..todo: check if norm is too large
getWriterControlStruct().globalPosition << currentPose.x, currentPose.y;
- getWriterControlStruct().globalOrientation = currentPose.rotationAroundZ;
+ getWriterControlStruct().globalOrientation = simox::math::periodic_clamp(currentPose.rotationAroundZ, -M_PIf32, M_PIf32);
getWriterControlStruct().startPosition = currentPosition;
getWriterControlStruct().startOrientation = currentOrientation;
--
GitLab