orientation optimization: clamping orientations before printing

source/armarx/navigation/global_planning/optimization/OrientationOptimizer.cpp

 #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";