fix: load default solver options

Author: Meow404

core/BUILD.bazel

@@ -22,6 +22,13 @@ cc_library(
     ],
 )
 
+filegroup(
+    name = "default_solver_options",
+    srcs = [
+        "configs/solver_options_default.yaml",
+    ],
+)
+
 cc_library(
     name = "c3",
     srcs = [
@@ -39,7 +46,9 @@ cc_library(
         "c3.h",
         "c3_miqp.h",
         "c3_qp.h",
-        "configs/solve_options_default.hpp"
+    ],
+    data = [
+        ":default_solver_options",
     ],
     copts = ["-fopenmp"],
     linkopts = ["-fopenmp"],

core/c3.cc

@@ -4,11 +4,11 @@
 #include <iostream>
 
 #include <Eigen/Core>
+#include <drake/common/find_runfiles.h>
 #include <omp.h>
 
 #include "lcs.h"
 #include "solver_options_io.h"
-#include "configs/solve_options_default.hpp"
 
 #include "drake/common/text_logging.h"
 #include "drake/solvers/mathematical_program.h"
@@ -154,8 +154,24 @@ C3::C3(const LCS& lcs, const CostMatrices& costs,
   }
 
   // Set default solver options
+  SetDefaultSolverOptions();
+}
+
+void C3::SetDefaultSolverOptions() {
+  // Set default solver options
+  auto main_runfile =
+      drake::FindRunfile("_main/core/configs/solver_options_default.yaml");
+  auto external_runfile =
+      drake::FindRunfile("c3/core/configs/solver_options_default.yaml");
+  if (main_runfile.abspath.empty() && external_runfile.abspath.empty()) {
+    throw std::runtime_error(fmt::format(
+        "Could not find the default solver options YAML file. {}, {}",
+        main_runfile.error, external_runfile.error));
+  }
   drake::solvers::SolverOptions solver_options =
-      drake::yaml::LoadYamlString<c3::SolverOptionsFromYaml>(default_solver_options)
+      drake::yaml::LoadYamlFile<c3::SolverOptionsFromYaml>(
+          main_runfile.abspath.empty() ? external_runfile.abspath
+                                       : main_runfile.abspath)
           .GetAsSolverOptions(drake::solvers::OsqpSolver::id());
   SetSolverOptions(solver_options);
 }

core/c3.h

@@ -164,7 +164,8 @@ class C3 {
    * @param options The C3Options object containing configuration values.
    * @return CostMatrices The initialized cost matrices.
    */
-  static CostMatrices CreateCostMatricesFromC3Options(const C3Options& options, int N);
+  static CostMatrices CreateCostMatricesFromC3Options(const C3Options& options,
+                                                      int N);
 
   /**
    * @brief Get a vector of user defined linear constraints.
@@ -228,6 +229,12 @@ class C3 {
    */
   void ScaleLCS();
 
+  /*!
+   * Set the default solver options for the MathematicalProgram
+   * This is called in the constructor, and can be overridden by the user
+   */
+  void SetDefaultSolverOptions();
+
   /*!
    * Solve the projection problem for all time-steps
    * @param U Similarity cost weights for the projection optimization

core/configs/solve_options_default.hpp

@@ -8,6 +8,7 @@ cc_library(
     visibility = ["//visibility:public"],
     deps = [
         ":framework",
+        ":options",
         ":c3_controller",
         ":lcs_simulator",
         ":lcs_factory_system",