lint fix

Robotic-Decision-Making-Lab · Oct 25, 2024 · 2d38613 · 2d38613
1 parent 20f4a5b
commit 2d38613
Show file tree

Hide file tree

Showing 3 changed files with 18 additions and 12 deletions.
diff --git a/docking_control/src/auto_dock.py b/docking_control/src/auto_dock.py
@@ -1,7 +1,6 @@
 import os
 import time
 import numpy as np
-import rospy
 import sys
 
 sys.path.insert(0, "/home/ros/ws_dock/src/underwater_docking/docking_control/src")
@@ -165,17 +164,22 @@ def run_mpc(self, x0, xr):
         # xr[5, :] += np.pi
         xr[3:6, :] = self.wrap_pi2negpi(xr[3:6, :])
 
-        x0[6:12, :] = np.clip(x0[6:12, 0], self.mpc.xmin[6:12], self.mpc.xmax[6:12]).reshape(-1, 1)
+        x0[6:12, :] = np.clip(
+            x0[6:12, 0], self.mpc.xmin[6:12], self.mpc.xmax[6:12]
+        ).reshape(-1, 1)
 
         self.yaw_diff = abs((((x0[5, :] - xr[5, :]) + np.pi) % (2 * np.pi)) - np.pi)[0]
-        self.ang_diff = np.linalg.norm((((x0[3:6, :] - xr[3:6, :]) + np.pi) % (2 * np.pi)) - np.pi)
+        self.ang_diff = np.linalg.norm(
+            (((x0[3:6, :] - xr[3:6, :]) + np.pi) % (2 * np.pi)) - np.pi
+        )
 
         if self.distance <= self.tolerance and self.yaw_diff <= self.yaw_tolerance:
-        # if self.distance < self.tolerance and self.ang_diff < self.yaw_tolerance:
+            # if self.distance < self.tolerance and self.ang_diff < self.yaw_tolerance:
             return np.zeros((8, 1)), np.zeros((6, 1)), True
 
         else:
-            # nu_w = self.compute_wave_particle_vel(x0[0:6, :], self.t_span[self.time_id])
+            # nu_w = self.compute_wave_particle_vel(x0[0:6, :],
+            #                                       self.t_span[self.time_id])
             # rospy.logwarn(f"Wave Info: {nu_w}")
             # self.auv.nu_w = nu_w
             x0[3:5, :] = 0.0

diff --git a/docking_control/src/mission_control.py b/docking_control/src/mission_control.py
@@ -262,7 +262,7 @@ def rov_vel_cb(self, vel):
             self.rov_twist[5][0] = -vel.twist.angular.z
             self.rov_twist[3][0] = 0.0
             self.rov_twist[4][0] = 0.0
-            # self.rov_twist[5][0] = 0.0
+            # self.rov_twist[5][0] = 0.
             # print(self.rov_twist)
         except Exception:
             rospy.logerr_throttle(
@@ -437,12 +437,12 @@ def auto_control(self, joy):
             # self.rov_odom = np.vstack((self.rov_pose, self.rov_twist))
             x0 = self.rov_odom
 
-        # x0 = np.array([[0.0, 5.0, 5.0, 0.0, 0.0, 0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]]).T
-        # xr = np.array([[2.0, 5.0, 5.0, 0.0, 0.0, 0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]]).T
-        # xr = np.array([[x0[0, 0], 5.0, 5.0, 0.0, 0.0, 0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]]).T
+        # x0 = np.array([[0., 5., 5., 0., 0., 0, 0., 0., 0., 0., 0., 0.]]).T
+        # xr = np.array([[2., 5., 5., 0., 0., 0, 0., 0., 0., 0., 0., 0.]]).T
+        # xr = np.array([[x0[0, 0], 5., 5., 0., 0., 0, 0., 0., 0., 0., 0., 0.]]).T
         xr = np.array([[-5.0, 5.0, 5.0, 0.0, 0.0, 0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]]).T
-        # xr = np.array([[x0[0, 0], 0.0, 0.0, 0.0, 0.0, 0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]]).T
-        # xr = np.array([[0.0, 0.0, 0.0, 0.0, 0.0, 0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]]).T
+        # xr = np.array([[x0[0, 0], 0., 0., 0., 0., 0, 0., 0., 0., 0., 0., 0.]]).T
+        # xr = np.array([[0., 0., 0., 0., 0., 0, 0., 0., 0., 0., 0., 0.]]).T
 
         try:
             forces, wrench, converge_flag = self.mpc.run_mpc(x0, xr)

diff --git a/docking_control/src/mpc_external_cost.py b/docking_control/src/mpc_external_cost.py
@@ -174,7 +174,9 @@ def reset(self):
         self.flag = False
 
     def warm_start(self, x0):
-        # self.previous_control = self.acados_ocp_solver.solve_for_x0(x0, fail_on_nonzero_status=False)
+        # self.previous_control = self.acados_ocp_solver.solve_for_x0(
+        #     x0, fail_on_nonzero_status=False
+        # )
         # self.previous_control = np.array([self.previous_control]).T
         self.acados_ocp_solver.set(0, "lbx", x0)
         self.acados_ocp_solver.set(0, "ubx", x0)