From d6a472bdbc064565c32276e4bab4e550a04a8f19 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Fri, 7 Jan 2022 09:54:36 +0100
Subject: [PATCH] orientation optimization: clamping orientations before
printing
---
.../global_planning/optimization/OrientationOptimizer.cpp | 7 +++++++
1 file changed, 7 insertions(+)
diff --git a/source/armarx/navigation/global_planning/optimization/OrientationOptimizer.cpp b/source/armarx/navigation/global_planning/optimization/OrientationOptimizer.cpp
index 30f75542..eb98cee4 100644
--- a/source/armarx/navigation/global_planning/optimization/OrientationOptimizer.cpp
+++ b/source/armarx/navigation/global_planning/optimization/OrientationOptimizer.cpp
@@ -1,5 +1,6 @@
#include "OrientationOptimizer.h"
+#include <algorithm>
#include <cmath>
#include <range/v3/range/conversion.hpp>
@@ -16,6 +17,7 @@
#include <SimoxUtility/math/convert/mat4f_to_xyyaw.h>
#include <SimoxUtility/math/convert/rpy_to_mat3f.h>
+#include <SimoxUtility/math/periodic/periodic_clamp.h>
#include <SimoxUtility/math/periodic/periodic_diff.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
@@ -134,6 +136,11 @@ namespace armarx::navigation::global_planning::optimization
std::cout << summary.FullReport() << "\n";
// std::cout << summary.BriefReport() << "\n";
+ const auto clampInPlace = [](auto& val)
+ { val = simox::math::periodic_clamp(val, -M_PI, M_PI); };
+
+ std::for_each(orientations.begin(), orientations.end(), clampInPlace);
+
ARMARX_INFO << "orientations after: " << orientations;
ARMARX_INFO << "Optimization: " << summary.iterations.size() << " iterations";
--
GitLab