Move logging to debug instead of info

Author: Levi-Armstrong

trajopt_ext/osqp_eigen/CMakeLists.txt

@@ -28,10 +28,12 @@ if(NOT ${OsqpEigen_FOUND} AND NOT TRAJOPT_OSQP_DISABLED)
     SOURCE_DIR ${CMAKE_BINARY_DIR}-src
     BINARY_DIR ${CMAKE_BINARY_DIR}-build
     CMAKE_CACHE_ARGS
-      -DCMAKE_INSTALL_PREFIX:STRING=${CMAKE_INSTALL_PREFIX}
       -DCMAKE_BUILD_TYPE:STRING=Release
+      -DCMAKE_INSTALL_PREFIX:STRING=${CMAKE_INSTALL_PREFIX}
       -DCMAKE_TOOLCHAIN_FILE:STRING=${CMAKE_TOOLCHAIN_FILE}
-      -DVCPKG_TARGET_TRIPLET:STRING=${VCPKG_TARGET_TRIPLET})
+      -DVCPKG_TARGET_TRIPLET:STRING=${VCPKG_TARGET_TRIPLET}
+      -DOSQP_EIGEN_DEBUG_OUTPUT=OFF
+    )
 
   if(TRAJOPT_PACKAGE)
     find_package(ros_industrial_cmake_boilerplate REQUIRED)

trajopt_optimizers/trajopt_sqp/package.xml

@@ -19,6 +19,7 @@
   <depend>trajopt_ifopt</depend>
   <depend>trajopt_common</depend>
   <depend>tesseract_common</depend>
+  <depend>tesseract_state_solver</depend>
   <depend>tesseract_visualization</depend>
 
   <test_depend>gtest</test_depend>

trajopt_optimizers/trajopt_sqp/src/trust_region_sqp_solver.cpp

@@ -105,14 +105,14 @@ void TrustRegionSQPSolver::solve(const QPProblem::Ptr& qp_problem)
       const double elapsed_time = std::chrono::duration<double, std::milli>(Clock::now() - start_time).count() / 1000.0;
       if (elapsed_time > params.max_time)
       {
-        CONSOLE_BRIDGE_logInform("Elapsed time %f has exceeded max time %f", elapsed_time, params.max_time);
+        CONSOLE_BRIDGE_logDebug("Elapsed time %f has exceeded max time %f", elapsed_time, params.max_time);
         status_ = SQPStatus::OPT_TIME_LIMIT;
         break;
       }
 
       if (results_.overall_iteration >= params.max_iterations)
       {
-        CONSOLE_BRIDGE_logInform("Iteration limit");
+        CONSOLE_BRIDGE_logDebug("Iteration limit");
         status_ = SQPStatus::ITERATION_LIMIT;
         break;
       }
@@ -146,7 +146,7 @@ void TrustRegionSQPSolver::solve(const QPProblem::Ptr& qp_problem)
   if (status_ == SQPStatus::RUNNING)
   {
     status_ = SQPStatus::PENALTY_ITERATION_LIMIT;
-    CONSOLE_BRIDGE_logInform("Penalty iteration limit, optimization couldn't satisfy all constraints");
+    CONSOLE_BRIDGE_logDebug("Penalty iteration limit, optimization couldn't satisfy all constraints");
   }
 
   // Final Cleanup
@@ -161,13 +161,13 @@ bool TrustRegionSQPSolver::verifySQPSolverConvergence()
   // Check if constraints are satisfied
   if (results_.best_constraint_violations.size() == 0)
   {
-    CONSOLE_BRIDGE_logInform("Optimization has converged and there are no constraints");
+    CONSOLE_BRIDGE_logDebug("Optimization has converged and there are no constraints");
     return true;
   }
 
   if (results_.best_constraint_violations.maxCoeff() < params.cnt_tolerance)
   {
-    CONSOLE_BRIDGE_logInform("woo-hoo! constraints are satisfied (to tolerance %.2e)", params.cnt_tolerance);
+    CONSOLE_BRIDGE_logDebug("woo-hoo! constraints are satisfied (to tolerance %.2e)", params.cnt_tolerance);
     return true;
   }
 
@@ -183,14 +183,14 @@ void TrustRegionSQPSolver::adjustPenalty()
     {
       if (results_.best_constraint_violations[idx] > params.cnt_tolerance)
       {
-        CONSOLE_BRIDGE_logInform("Not all constraints are satisfied. Increasing constraint penalties for %d", idx);
+        CONSOLE_BRIDGE_logDebug("Not all constraints are satisfied. Increasing constraint penalties for %d", idx);
         results_.merit_error_coeffs[idx] *= params.merit_coeff_increase_ratio;
       }
     }
   }
   else
   {
-    CONSOLE_BRIDGE_logInform("Not all constraints are satisfied. Increasing constraint penalties uniformly");
+    CONSOLE_BRIDGE_logDebug("Not all constraints are satisfied. Increasing constraint penalties uniformly");
     results_.merit_error_coeffs *= params.merit_coeff_increase_ratio;
   }
   setBoxSize(fmax(results_.box_size[0], params.min_trust_box_size / params.trust_shrink_ratio * 1.5));
@@ -222,7 +222,7 @@ bool TrustRegionSQPSolver::stepSQPSolver()
 
   if (results_.box_size.maxCoeff() < params.min_trust_box_size)
   {
-    CONSOLE_BRIDGE_logInform("Converged because trust region is tiny");
+    CONSOLE_BRIDGE_logDebug("Converged because trust region is tiny");
     status_ = SQPStatus::NLP_CONVERGED;
     return true;
   }

trajopt_sco/src/optimizers.cpp

@@ -924,18 +924,19 @@ OptStatus BasicTrustRegionSQP::optimize()
         {
           if (results_.cnt_viols[idx] > param_.cnt_tolerance)
           {
-            LOG_INFO("Not all constraints are satisfied. Increasing constraint penalties for %s", CSTR(cnt_names[idx]));
+            LOG_DEBUG("Not all constraints are satisfied. Increasing constraint penalties for %s",
+                      CSTR(cnt_names[idx]));
             merit_error_coeffs[idx] *= param_.merit_coeff_increase_ratio;
           }
         }
       }
       else
       {
-        LOG_INFO("Not all constraints are satisfied. Increasing constraint penalties uniformly");
+        LOG_DEBUG("Not all constraints are satisfied. Increasing constraint penalties uniformly");
         for (auto& merit_error_coeff : merit_error_coeffs)
           merit_error_coeff *= param_.merit_coeff_increase_ratio;
       }
-      LOG_INFO("New merit_error_coeffs: %s", CSTR(merit_error_coeffs));
+      LOG_DEBUG("New merit_error_coeffs: %s", CSTR(merit_error_coeffs));
       param_.trust_box_size = fmax(param_.trust_box_size, param_.min_trust_box_size / param_.trust_shrink_ratio * 1.5);
     }
   } /* merit adjustment loop */