Applied chrstrom saturateVectorValues() fix

vortexntnu · Nov 12, 2023 · 236579e · 236579e
1 parent 5b08048
commit 236579e
Showing 1 changed file with 8 additions and 10 deletions.
diff --git a/motion/thruster_allocator/include/thruster_allocator/allocator_utils.hpp b/motion/thruster_allocator/include/thruster_allocator/allocator_utils.hpp
@@ -70,17 +70,15 @@ inline void calculateRightPseudoinverse(const Eigen::MatrixXd &M,
  * otherwise.
  */
 inline bool saturateVectorValues(Eigen::VectorXd &vec, double min, double max) {
-  bool vector_in_range = std::all_of(vec.begin(), vec.end(), [&](double &val) {
-    if (val > max) {
-      val = max;
-      return false;
-    } else if (val < min) {
-      val = min;
-      return false;
-    }
-    return true;
+  bool all_values_in_range =
+      std::all_of(vec.begin(), vec.end(),
+                  [min, max](double val) { return val >= min && val <= max; });
+
+  std::transform(vec.begin(), vec.end(), vec.begin(), [min, max](double val) {
+    return std::min(std::max(val, min), max);
   });
-  return vector_in_range;
+
+  return all_values_in_range;
 }
 
 /**