From c0840fbf65a46b5f0cac9d2bc9418ca3e025f39e Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Wed, 13 Jul 2022 13:28:03 +0200
Subject: [PATCH] TrajectoryFollowingController: floating point rounding fix
---
.../TrajectoryFollowingController.cpp | 14 ++++++++------
1 file changed, 8 insertions(+), 6 deletions(-)
diff --git a/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp b/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp
index 486ba3d0..b5ef2b45 100644
--- a/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp
+++ b/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp
@@ -112,13 +112,15 @@ namespace armarx::navigation::traj_ctrl
twist.linear /= scaleMax;
twist.angular /= scaleMax;
+ constexpr float eps = 0.001;
+
// pedantic checks
- ARMARX_CHECK_LESS_EQUAL(std::abs(twist.linear.x()), params.limits.linear);
- ARMARX_CHECK_LESS_EQUAL(std::abs(twist.linear.y()), params.limits.linear);
- ARMARX_CHECK_LESS_EQUAL(std::abs(twist.linear.z()), params.limits.linear);
- ARMARX_CHECK_LESS_EQUAL(std::abs(twist.angular.x()), params.limits.angular);
- ARMARX_CHECK_LESS_EQUAL(std::abs(twist.angular.y()), params.limits.angular);
- ARMARX_CHECK_LESS_EQUAL(std::abs(twist.angular.z()), params.limits.angular);
+ ARMARX_CHECK_LESS_EQUAL(std::abs(twist.linear.x()), params.limits.linear + eps);
+ ARMARX_CHECK_LESS_EQUAL(std::abs(twist.linear.y()), params.limits.linear + eps);
+ ARMARX_CHECK_LESS_EQUAL(std::abs(twist.linear.z()), params.limits.linear + eps);
+ ARMARX_CHECK_LESS_EQUAL(std::abs(twist.angular.x()), params.limits.angular + eps);
+ ARMARX_CHECK_LESS_EQUAL(std::abs(twist.angular.y()), params.limits.angular + eps);
+ ARMARX_CHECK_LESS_EQUAL(std::abs(twist.angular.z()), params.limits.angular + eps);
return twist;
}
--
GitLab