diff --git a/source/armarx/navigation/global_planning/optimization/OrientationOptimizer.cpp b/source/armarx/navigation/global_planning/optimization/OrientationOptimizer.cpp
index 30f755429bd5df2e4ef8578aa67791d35c0acebc..eb98cee4bb3139c5dcf846e7dd19fbde5a07af11 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";