Fix box size setting when adjusting penalties

tesseract-robotics · Nov 21, 2023 · a257232 · a257232
1 parent 806a40a
commit a257232
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 13 deletions.
diff --git a/.clang-format b/.clang-format
@@ -54,15 +54,16 @@ BreakBeforeBraces: Custom
 
 # Control of individual brace wrapping cases
 BraceWrapping: {
-    AfterClass: 'true'
-    AfterControlStatement: 'true'
-    AfterEnum : 'true'
-    AfterFunction : 'true'
-    AfterNamespace : 'true'
-    AfterStruct : 'true'
-    AfterUnion : 'true'
-    BeforeCatch : 'true'
-    BeforeElse : 'true'
-    IndentBraces : 'false'
+    AfterCaseLabel: 'true',
+    AfterClass: 'true',
+    AfterControlStatement: 'true',
+    AfterEnum : 'true',
+    AfterFunction : 'true',
+    AfterNamespace : 'true',
+    AfterStruct : 'true',
+    AfterUnion : 'true',
+    BeforeCatch : 'true',
+    BeforeElse : 'true',
+    IndentBraces : 'false',
 }
 ...
diff --git a/trajopt_optimizers/trajopt_sqp/src/trust_region_sqp_solver.cpp b/trajopt_optimizers/trajopt_sqp/src/trust_region_sqp_solver.cpp
@@ -30,7 +30,6 @@
  */
 #include <trajopt_sqp/trust_region_sqp_solver.h>
 #include <console_bridge/console.h>
-#include <iostream>
 #include <chrono>
 
 namespace trajopt_sqp
@@ -186,8 +185,7 @@ void TrustRegionSQPSolver::adjustPenalty()
     CONSOLE_BRIDGE_logInform("Not all constraints are satisfied. Increasing constraint penalties uniformly");
     results_.merit_error_coeffs *= params.merit_coeff_increase_ratio;
   }
-  results_.box_size = Eigen::VectorXd::Ones(results_.box_size.size()) *
-                      fmax(results_.box_size[0], params.min_trust_box_size / params.trust_shrink_ratio * 1.5);
+  setBoxSize(fmax(results_.box_size[0], params.min_trust_box_size / params.trust_shrink_ratio * 1.5));
 }
 
 bool TrustRegionSQPSolver::stepSQPSolver()