From 23a8730914d79038762e2ccbf693b6fcc5f29e03 Mon Sep 17 00:00:00 2001
From: "Christian R. G. Dreher" <c.dreher@kit.edu>
Date: Wed, 24 Feb 2021 12:14:51 +0100
Subject: [PATCH] fix: Clamp input orientation to -pi,pi.
---
.../ObstacleAvoidingPlatformUnit.cpp | 4 +++-
1 file changed, 3 insertions(+), 1 deletion(-)
diff --git a/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.cpp b/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.cpp
index 0d9a7c50c..dcf0ebd84 100644
--- a/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.cpp
+++ b/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.cpp
@@ -160,10 +160,12 @@ armarx::ObstacleAvoidingPlatformUnit::moveTo(
const float ori_reached_threshold,
const Ice::Current&)
{
+ using namespace simox::math;
+
std::scoped_lock l{m_control_data.mutex};
m_control_data.target_pos = Eigen::Vector2f{target_pos_x, target_pos_y};
- m_control_data.target_ori = target_ori;
+ m_control_data.target_ori = periodic_clamp<float>(target_ori, -M_PI, M_PI);
m_control_data.pos_reached_threshold = pos_reached_threshold;
m_control_data.ori_reached_threshold = ori_reached_threshold;
--
GitLab