Fix solver failure issue in mpccbf test files (#10)

car_racing/control/control.py

@@ -524,7 +524,7 @@ def mpccbf(
     # slack variables for control barrier functions
     cbf_slack = opti.variable(len(obs_infos), mpc_cbf_param.num_horizon + 1)
     # obstacle avoidance
-    safety_margin = 0.15
+    safety_margin = 0.2
     degree = 6  # 2, 4, 6, 8
     for count, obs_name in enumerate(obs_infos):
         obs_traj = obs_infos[obs_name]
@@ -593,12 +593,17 @@ def mpccbf(
     option = {"verbose": False, "ipopt.print_level": 0, "print_time": 0}
     opti.minimize(cost)
     opti.solver("ipopt", option)
-    sol = opti.solve()
+    try:
+        sol = opti.solve()
+        x_pred = sol.value(xvar).T
+        u_pred = sol.value(uvar).T
+    except RuntimeError:
+        print("solver failed.")
+        x_pred = opti.debug.value(xvar).T
+        u_pred = opti.debug.value(uvar).T
     end_timer = datetime.datetime.now()
     solver_time = (end_timer - start_timer).total_seconds()
     print("solver time: {}".format(solver_time))
-    x_pred = sol.value(xvar).T
-    u_pred = sol.value(uvar).T
     return u_pred[0, :]
 
 

car_racing/tests/mpccbf_test.py

@@ -16,6 +16,8 @@ def racing(args):
         mpc_cbf_param = base.MPCCBFRacingParam(vt=0.8)
         ego.set_state_curvilinear(np.zeros((X_DIM,)))
         ego.set_state_global(np.zeros((X_DIM,)))
+        if args["zero_noise"]:
+            ego.set_zero_noise()
         ego.start_logging()
         ego.set_ctrl_policy(offboard.MPCCBFRacing(mpc_cbf_param, ego.system_param))
         ego.ctrl_policy.set_timestep(0.1)
@@ -59,5 +61,6 @@ def racing(args):
     parser.add_argument("--plotting", action="store_true")
     parser.add_argument("--animation", action="store_true")
     parser.add_argument("--track-layout", type=str)
+    parser.add_argument("--zero-noise", action="store_true")
     args = vars(parser.parse_args())
     racing(args)

car_racing/utils/base.py

@@ -279,7 +279,7 @@ def __init__(
         vt=0.6,
         eyt=0.0,
         num_horizon=10,
-        alpha=0.6,
+        alpha=0.8,
     ):
         self.matrix_A = matrix_A
         self.matrix_B = matrix_B