hpipm_mode can be defined in configuration file

ethz-adrl · Mar 28, 2022 · ef096b9 · ef096b9
1 parent c9ebddf
commit ef096b9
Show file tree

Hide file tree

Showing 4 changed files with 31 additions and 7 deletions.
diff --git a/ct_optcon/examples/nlocSolver_ObstacleConstraint.info b/ct_optcon/examples/nlocSolver_ObstacleConstraint.info
@@ -37,5 +37,6 @@ ilqr
     {
         lqoc_debug_print        false
         num_lqoc_iterations  	50
+        hpipm_mode_str          SPEED
     }
 }
diff --git a/ct_optcon/include/ct/optcon/solver/NLOptConSettings.hpp b/ct_optcon/include/ct/optcon/solver/NLOptConSettings.hpp
@@ -24,9 +24,9 @@ struct LineSearchSettings
     //! types of backtracking line-search
     enum TYPE
     {
-        NONE = 0,      //! take full-step updates
-        SIMPLE,        //! simple backtracking using cost or merit function
-        ARMIJO,        //! backtracking including riccati matrix measure
+        NONE = 0,   //! take full-step updates
+        SIMPLE,     //! simple backtracking using cost or merit function
+        ARMIJO,     //! backtracking including riccati matrix measure
         GOLDSTEIN,  //! backtracking including riccati matrix measure and defects
         NUM_TYPES
     };
@@ -154,16 +154,20 @@ struct LineSearchSettings
 struct LQOCSolverSettings
 {
 public:
-    LQOCSolverSettings() : lqoc_debug_print(false), num_lqoc_iterations(10) {}
+    LQOCSolverSettings() : lqoc_debug_print(false), num_lqoc_iterations(10), hpipm_mode_str("SPEED") {}
 
     bool lqoc_debug_print;
-    int num_lqoc_iterations;  //! number of allowed sub-iterations of LQOC solver per NLOC main iteration
+    int num_lqoc_iterations;     //! number of allowed sub-iterations of LQOC solver per NLOC main iteration
+    std::string hpipm_mode_str;  // HPIPM specific : SPEED_ABS, SPEED, ROBUST or BALANCED.
+    // Stored as string and not as hpipm_mode enum because HPIPM is not a compulsory dependency.
+    //(see https://github.com/giaf/hpipm/blob/f38a216c5a9f916682cd543f8992a594323ad202/ocp_qp/x_ocp_qp_ipm.c#L69)
 
     void print() const
     {
         std::cout << "======================= LQOCSolverSettings =====================" << std::endl;
         std::cout << "num_lqoc_iterations: \t" << num_lqoc_iterations << std::endl;
         std::cout << "lqoc_debug_print: \t" << lqoc_debug_print << std::endl;
+        std::cout << "hpipm_mode_str: \t" << hpipm_mode_str << std::endl;
     }
 
     void load(const std::string& filename, bool verbose = true, const std::string& ns = "lqoc_solver_settings")
@@ -186,6 +190,12 @@ struct LQOCSolverSettings
         } catch (...)
         {
         }
+        try
+        {
+            hpipm_mode_str = pt.get<std::string>(ns + ".hpipm_mode_str");
+        } catch (...)
+        {
+        }
     }
 };
 

diff --git a/ct_optcon/include/ct/optcon/solver/lqp/HPIPMInterface-impl.hpp b/ct_optcon/include/ct/optcon/solver/lqp/HPIPMInterface-impl.hpp
@@ -139,6 +139,16 @@ void HPIPMInterface<STATE_DIM, CONTROL_DIM>::initializeAndAllocate()
     ipm_arg_mem_ = malloc(ipm_arg_size);
     ::d_ocp_qp_ipm_arg_create(&dim_, &arg_, ipm_arg_mem_);
 
+    if (stringToHpipmMode.find(settings_.lqoc_solver_settings.hpipm_mode_str) != stringToHpipmMode.end())
+        mode_ = stringToHpipmMode[settings_.lqoc_solver_settings.hpipm_mode_str];
+    else
+    {
+        std::cout << "Invalid hpipm_mode_str specified in config, should be one of the following:" << std::endl;
+        for (auto it = stringToHpipmMode.begin(); it != stringToHpipmMode.end(); it++)
+            std::cout << it->first << std::endl;
+        exit(-1);
+    }
+
     ::d_ocp_qp_ipm_arg_set_default(mode_, &arg_);
     ::d_ocp_qp_ipm_arg_set_iter_max(&settings_.lqoc_solver_settings.num_lqoc_iterations, &arg_);
 

diff --git a/ct_optcon/include/ct/optcon/solver/lqp/HPIPMInterface.hpp b/ct_optcon/include/ct/optcon/solver/lqp/HPIPMInterface.hpp
@@ -255,8 +255,11 @@ class HPIPMInterface : public LQOCSolver<STATE_DIM, CONTROL_DIM>
     struct d_ocp_qp_ipm_ws workspace_;
     int hpipm_status_;  // status code after solving
 
-    // todo make this a setting
-    ::hpipm_mode mode_ = ::hpipm_mode::SPEED;  // ROBUST/BALANCED; see also hpipm_common.h
+    // hpipm mode SPEED_ABS, SPEED, ROBUST or BALANCED
+    ::hpipm_mode mode_;  // see also hpipm_common.h
+
+    std::map<std::string, ::hpipm_mode> stringToHpipmMode = {{"SPEED_ABS", ::hpipm_mode::SPEED_ABS},
+        {"SPEED", ::hpipm_mode::SPEED}, {"ROBUST", ::hpipm_mode::ROBUST}, {"BALANCE", ::hpipm_mode::BALANCE}};
 };
 
 }  // namespace optcon