Passed tests!

StanfordASL · Feb 11, 2024 · d4b5427 · d4b5427
1 parent 9e4529d
commit d4b5427
Showing 1 changed file with 6 additions and 6 deletions.
diff --git a/ff_control/scripts/opt_ctrl_py_node b/ff_control/scripts/opt_ctrl_py_node
@@ -125,9 +125,14 @@ class ThrusterOptControlNode(TrinaryThrusterController):
             if not self._close_controller:
                 pos_err = np.abs(state_des[:2] - curr_state[:2])
                 # state_err_vec = np.abs(np.append(pos_err, th_err))
-                # if np.all(state_err_vec < self._readjust_controller_bounds):
                 if np.all(pos_err < self._readjust_controller_bounds):
                     self.set_close_hyperparameters()
+            else:
+                pos_err = np.abs(state_des[:2] - curr_state[:2])
+                # state_err_vec = np.abs(np.append(pos_err, th_err))
+                if np.any(pos_err > self._readjust_controller_bounds):
+                    self.set_default_hyperparameters()
+
 
             self.lbw[:len(curr_state)] = curr_state
             self.ubw[:len(curr_state)] = curr_state
@@ -475,11 +480,6 @@ class ThrusterOptControlNode(TrinaryThrusterController):
         kin = rclpy.Parameter('opt_gain_kinput', rclpy.Parameter.Type.DOUBLE, 3.5)
         kvelo = rclpy.Parameter('opt_gain_kvelo', rclpy.Parameter.Type.DOUBLE, 20.5)
 
-        # kth = rclpy.Parameter('opt_gain_kth', rclpy.Parameter.Type.DOUBLE, 50.0)
-        # kpos = rclpy.Parameter('opt_gain_kpos', rclpy.Parameter.Type.DOUBLE, 2.5)
-        # kin = rclpy.Parameter('opt_gain_kinput', rclpy.Parameter.Type.DOUBLE, 3.0)
-        # kvelo = rclpy.Parameter('opt_gain_kvelo', rclpy.Parameter.Type.DOUBLE, 40.0)
-
         self.set_parameters([horizon, freq, kth, kpos, kin, kvelo])
         self.w0, self.cont_nlp_solver, self.lbw, self.ubw, self.lbg, self.ubg = self.init_solver()