add solver tolerances to WBC

dfki-ric-underactuated-lab · Feb 26, 2025 · 3dd7ed2 · 3dd7ed2
1 parent ce49746
commit 3dd7ed2
Show file tree

Hide file tree

Showing 5 changed files with 49 additions and 10 deletions.
diff --git a/docker/Dockerfile b/docker/Dockerfile
@@ -133,7 +133,7 @@ RUN git clone --recursive https://github.com/fmtlib/fmt.git && cd fmt && git che
 # install arc-opt
 RUN apt-get update && apt-get -y install git cmake build-essential libboost-system-dev libboost-program-options-dev libboost-thread-dev libboost-test-dev pkg-config libeigen3-dev libboost-filesystem-dev
 RUN apt-get update && apt-get -y install liburdfdom-headers-dev liburdfdom-dev
-RUN git clone --branch icra_2025 https://github.com/ARC-OPT/wbc.git && cd wbc
+RUN git clone --branch icra_2025_rev https://github.com/FranekStark/wbc.git && cd wbc
 RUN git clone --branch v2.6.8 --recurse-submodules https://github.com/stack-of-tasks/pinocchio.git && cd pinocchio && mkdir build && cd build && cmake .. -DBUILD_PYTHON_INTERFACE=OFF -DBUILD_UNIT_TESTS=OFF -DCMAKE_BUILD_TYPE=Release && make -j$(nproc) && make install
 RUN git clone https://github.com/coin-or/qpOASES.git -b releases/3.2.0 && cd qpOASES &&  mkdir patches && cp ../wbc/patches/qpOASES.patch patches && git apply patches/qpOASES.patch && mkdir build && cd build && cmake .. -DCMAKE_BUILD_TYPE=Release && make -j$(nproc) && make install
 RUN git clone --recurse-submodules https://github.com/stack-of-tasks/eiquadprog.git -b v1.2.5 && cd eiquadprog && cp ../wbc/patches/eiquadprog.patch . && git apply eiquadprog.patch && mkdir build && cd build && cmake .. -DCMAKE_BUILD_TYPE=Release && make -j$(nproc) && make install

diff --git a/ws/src/controllers/include/mit_controller/wbc_arc_opt.hpp b/ws/src/controllers/include/mit_controller/wbc_arc_opt.hpp
@@ -74,7 +74,9 @@ class WBCArcOPT : public WBCInterface<JointTorqueVelocityPositionCommands> {
             const Eigen::Matrix<double, 3, 1>& feet_pose_p_gain,
             const Eigen::Matrix<double, 3, 1>& feet_pose_d_gain,
             const Eigen::Matrix<double, 6, 1>& com_pose_saturation,
-            const Eigen::Matrix<double, 3, 1>& feet_pose_saturation);
+            const Eigen::Matrix<double, 3, 1>& feet_pose_saturation,
+            double solver_tolerances
+          );
 
   void UpdateState(const StateInterface& quad_state) override;
   void UpdateModel(const ModelInterface& quad_model) override;

diff --git a/ws/src/controllers/src/mit_controller/wbc_arc_opt.cpp b/ws/src/controllers/src/mit_controller/wbc_arc_opt.cpp
@@ -24,7 +24,9 @@ WBCArcOPT::WBCArcOPT(std::unique_ptr<StateInterface> state,
                      const Eigen::Matrix<double, 3, 1>& feet_pose_p_gain,
                      const Eigen::Matrix<double, 3, 1>& feet_pose_d_gain,
                      const Eigen::Matrix<double, 6, 1>& com_pose_saturation,
-                     const Eigen::Matrix<double, 3, 1>& feet_pose_saturation)
+                     const Eigen::Matrix<double, 3, 1>& feet_pose_saturation,
+                     double solver_tolerances
+                    )
     : quad_state_(std::move(state)),
       feet_names_(feet_names),
       wbc_robot_model_(std::make_shared<wbc::RobotModelPinocchio>()),
@@ -47,23 +49,52 @@ WBCArcOPT::WBCArcOPT(std::unique_ptr<StateInterface> state,
     throw std::runtime_error("Error in configuring wbc_robot_model");
   }
 
+  if(solver_tolerances < 0){
+    throw std::runtime_error("Pleae specify valid solver tolerances!");
+  }
+
   // Solver
   if (solver == "EiquadprogSolver") {
-    wbc_solver_ = std::make_shared<wbc::EiquadprogSolver>();
+    auto solver = std::make_shared<wbc::EiquadprogSolver>();
+    // No options
+    wbc_solver_ = solver;
   } else if (solver == "ProxQPSolver") {
-    wbc_solver_ = std::make_shared<wbc::ProxQPSolver>();
+    auto solver = std::make_shared<wbc::ProxQPSolver>();
+    proxsuite::proxqp::Settings<double> solver_opts;
+    solver_opts.eps_abs = solver_tolerances;
+    solver_opts.eps_duality_gap_abs = solver_tolerances;
+    solver->setOptions(solver_opts);
+    wbc_solver_ = solver;
   } else if (solver == "QPOasesSolver") {
-    wbc_solver_ = std::make_shared<wbc::QPOASESSolver>();
+    auto solver = std::make_shared<wbc::QPOASESSolver>();
+    qpOASES::Options solver_opts;
+    solver_opts.setToMPC();
+    solver_opts.terminationTolerance = solver_tolerances;
+    solver_opts.printLevel = qpOASES::PL_NONE;
+    solver->setOptions(solver_opts);
+    wbc_solver_ = solver;
   } else if (solver == "OSQPSolver") {
-    wbc_solver_ = std::make_shared<wbc::OsqpSolver>();
+    auto solver = std::make_shared<wbc::OsqpSolver>();
+    solver->solver.settings()->setAbsoluteTolerance(solver_tolerances);
+    solver->solver.settings()->setPrimalInfeasibilityTolerance(solver_tolerances);
+    solver->solver.settings()->setVerbosity(false);
+    wbc_solver_ = solver;
   } else if (solver == "QPSwiftSolver") {
+    // Does not work in our case
     wbc_solver_ = std::make_shared<wbc::QPSwiftSolver>();
   } else if (solver == "HPIPMSolver") {
-    wbc_solver_ = std::make_shared<wbc::HPIPMSolver>();
+    auto solver = std::make_shared<wbc::HPIPMSolver>();
+    double tolerance = solver_tolerances;
+    std::array<std::string, 4> fields = {"tol_dual_gap", "tol_eq", "tol_stat", "tol_comp"};
+    for(auto field : fields){
+      solver->setOptions(field, tolerance);
+    }
+    wbc_solver_ = solver;
   } else {
     throw std::runtime_error(fmt::format("Unknown wbc solver {0}", solver));
   }
 
+
   // Setup scene
   if (scene == "AccelerationSceneReducedTSID") {
     wbc_scene_ = std::make_unique<wbc::AccelerationSceneReducedTSID>(wbc_robot_model_, wbc_solver_, WBC_CYCLE_DT);

diff --git a/ws/src/controllers/src/mit_controller_node.cpp b/ws/src/controllers/src/mit_controller_node.cpp
@@ -65,6 +65,7 @@ MITController::MITController(const std::string &nodeName)
   this->declare_parameter<int>("mpc_warm_start", 1);
   this->declare_parameter<double>("mpc_solver_tolerances", -1.0);
   this->declare_parameter<std::string>("mpc_osqp_linsys_solver", "qdldl");
+  this->declare_parameter<double>("wbc_solver_tolerances", -1.0);
 
   leg_control_mode_ = static_cast<LEGControlMode>(this->get_parameter("leg_control_mode").as_int());
   early_contact_detection_ = this->get_parameter("early_contact_detection").as_bool();
@@ -575,15 +576,18 @@ MITController::MITController(const std::string &nodeName)
         as_eigen_vector<3>(this->get_parameter("wbc.arc_opt.feet_pose_Kp").as_double_array()),
         as_eigen_vector<3>(this->get_parameter("wbc.arc_opt.feet_pose_Kd").as_double_array()),
         as_eigen_vector<6>(this->get_parameter("wbc.arc_opt.com_pose_saturation").as_double_array()),
-        as_eigen_vector<3>(this->get_parameter("wbc.arc_opt.feet_pose_saturation").as_double_array())));
+        as_eigen_vector<3>(this->get_parameter("wbc.arc_opt.feet_pose_saturation").as_double_array()),
+        this->get_parameter("wbc_solver_tolerances").as_double()
+      ));
   } else {
     wbc_ptr = reinterpret_cast<WBCType *>(new InverseDynamics(
         std::make_unique<QuadModelSymbolic>(quad_model_),
         std::make_unique<QuadState>(quad_state_),
         this->get_parameter("wbc.inverse_dynamics.foot_position_based_on_target_height").as_bool(),
         this->get_parameter("wbc.inverse_dynamics.foot_position_based_on_target_orientation").as_bool(),
         this->get_parameter("wbc.inverse_dynamics.transformation_filter_size").as_int(),
-        this->get_parameter("wbc.inverse_dynamics.target_velocity_blend").as_double()));
+        this->get_parameter("wbc.inverse_dynamics.target_velocity_blend").as_double())
+      );
   }
   wbc_ = std::unique_ptr<WBCType>(wbc_ptr);
 

diff --git a/ws/src/solver_experiments/solver_experiments/solver_experiments.py b/ws/src/solver_experiments/solver_experiments/solver_experiments.py
@@ -327,6 +327,8 @@ def run_controller(
                 "-p",
                 "mpc_solver_tolerances:=" + str(solver_tolerance),
                 "-p",
+                "wbc_solver_tolerances:=" + str(solver_tolerance),
+                "-p",
                 "simple_gait_sequencer.gait:=" + gs_gait,
                 "-p",
                 "use_sim_time:=True",