precommit fix. WIP: odl implementation

Author: rakeshv24

docking_control/src/mission_control.py

@@ -19,7 +19,6 @@
 import sys
 
 sys.path.insert(0, "/home/ros/ws_dock/src/underwater_docking/docking_control/src")
-from auto_dock import MPControl  # noqa: E402
 from odl_controller import ODL  # noqa: E402
 
 
@@ -319,7 +318,7 @@ def rov_pose_cb(self, pose):
             y = pose.pose.orientation.y
             z = pose.pose.orientation.z
             w = pose.pose.orientation.w
-            euler = R.from_quat([x, y, z, w]).as_euler("xyz")
+            R.from_quat([x, y, z, w]).as_euler("xyz")
             # roll = euler[0]
             # pitch = euler[1]
             # yaw = euler[2]
@@ -546,7 +545,9 @@ def auto_control(self, joy):
         else:
             x0 = self.rov_odom
 
-        xr = np.array([[-1.25, 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(
+            [[-1.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]]
+        ).T
 
         try:
             if self.mpc.gp_enabled and self.mpc.sogp_models:
@@ -558,7 +559,9 @@ def auto_control(self, joy):
                     self.timestamp,
                 )
 
-            forces, self.wrench, converge_flag = self.mpc.run_mpc(x0, xr, self.gp_residual_pred)
+            forces, self.wrench, converge_flag = self.mpc.run_mpc(
+                x0, xr, self.gp_residual_pred
+            )
             if converge_flag:
                 msg = """
                 [BlueROV2][auto_control] ROV reached dock successfully!

docking_control/src/mpc_sogp_cbf.py

@@ -1,7 +1,22 @@
 import numpy as np
 import yaml
 from acados_template import AcadosOcp, AcadosOcpSolver, AcadosModel
-from casadi import evalf, SX, mtimes, pinv, vertcat, hessian, Function, jacobian, dot, fmin, fmax, sumsqr, cos, sin, pi
+from casadi import (
+    evalf,
+    SX,
+    mtimes,
+    pinv,
+    vertcat,
+    Function,
+    jacobian,
+    dot,
+    fmin,
+    fmax,
+    sumsqr,
+    cos,
+    sin,
+    pi,
+)
 from scipy.linalg import block_diag
 import sys
 
@@ -178,13 +193,17 @@ def frustum_barrier(
 
             Args:
                 x (ca.MX): The AUV's current state vector (12x1).
-                xr (ca.MX): The AUV's reference state vector (12x1), including target position and orientation.
+                xr (ca.MX): The AUV's reference state vector (12x1), including
+                target position and orientation.
+
                 p_start (ca.MX): The start point of the frustum's axis (3x1).
                 start_radius (float): The radius of the corridor at the start point.
                 end_radius (float): The radius of the corridor at the end point.
-                orientation_weight (float): Tuning parameter to scale the orientation constraint's importance.
-                max_align_angle_rad (float): The maximum allowed angle (in radians) between the AUV's
-                                            forward vector and the target's forward vector.
+                orientation_weight (float): Tuning parameter to scale the orientation
+                constraint's importance.
+
+                max_align_angle_rad (float): The maximum allowed angle (in radians)
+                between the AUV's forward vector and the target's forward vector.
 
             Returns:
                 ca.MX: The value of the combined barrier function.
@@ -203,7 +222,7 @@ def frustum_barrier(
 
             # --- 2. Orientation Barrier (Aligns AUV frame with Target frame) ---
             # AUV's current orientation
-            phi_auv, theta_auv, psi_auv = x[3], x[4], x[5]
+            _, theta_auv, psi_auv = x[3], x[4], x[5]
 
             # AUV's forward vector (body x-axis) in the world frame
             v_auv_x = cos(psi_auv) * cos(theta_auv)
@@ -212,7 +231,7 @@ def frustum_barrier(
             v_auv = vertcat(v_auv_x, v_auv_y, v_auv_z)
 
             # Target's orientation from the reference state
-            phi_target, theta_target, psi_target = xr[3], xr[4], xr[5]
+            _, theta_target, psi_target = xr[3], xr[4], xr[5]
 
             # Target's forward vector (body x-axis) in the world frame
             v_target_orient_x = cos(psi_target) * cos(theta_target)
@@ -228,8 +247,9 @@ def frustum_barrier(
             # The cosine of the maximum allowed alignment angle
             cos_max_angle = cos(max_align_angle_rad)
 
-            # The orientation barrier is positive when the frames are aligned within the allowed cone
-            b_orientation = cos_angle_error - cos_max_angle
+            # The orientation barrier is positive when the frames are aligned within the
+            # allowed cone
+            cos_angle_error - cos_max_angle
 
             # --- 3. Combined Barrier ---
             b_combined = b_position  # + orientation_weight * b_orientation
@@ -259,7 +279,8 @@ def tube_barrier(x, p_start, p_end, tube_radius):
 
             # Calculate the projection parameter 't'.
             # This determines the closest point on the infinite line.
-            # Add a small epsilon to prevent division by zero if start and end points are the same.
+            # Add a small epsilon to prevent division by zero if start and end
+            # points are the same.
             t = dot(w, v) / (dot(v, v) + 1e-9)
 
             # Clamp 't' to the range [0, 1] to stay on the line *segment*.
@@ -363,8 +384,8 @@ def create_ocp_solver_description(self):
         self.acados_ocp.dims.N = self.horizon
         self.acados_ocp.parameter_values = np.zeros(2 * self.vehicle_state_dim)
 
-        xr = self.acados_model.p[0:self.vehicle_state_dim]
-        residual = self.acados_model.p[self.vehicle_state_dim:]
+        xr = self.acados_model.p[0 : self.vehicle_state_dim]
+        residual = self.acados_model.p[self.vehicle_state_dim :]
 
         self.acados_ocp.cost.cost_type = "NONLINEAR_LS"
         self.acados_ocp.cost.cost_type_e = "NONLINEAR_LS"
@@ -433,7 +454,7 @@ def optimize(self, x, x_ref, residual):
         x0 = x[0:12, :]
         N = self.horizon
 
-        nx = self.acados_ocp.model.x.shape[0]
+        self.acados_ocp.model.x.shape[0]
         nu = self.acados_ocp.model.u.shape[0]
 
         # set initial state constraint

docking_control/src/odl_controller.py

@@ -2,15 +2,14 @@
 import time
 import numpy as np
 import sys
-from casadi import evalf
 import yaml
-from typing import Any, Dict, List, Tuple, Optional
+from typing import Dict, List, Tuple, Optional
 import logging
 
 sys.path.insert(0, "/home/ros/ws_dock/src/underwater_docking/docking_control/src")
 
 from auv_hinsdale import AUV  # noqa: E402
-from sogp import SOGP
+from sogp import SOGP  # noqa: E402
 from mpc_sogp_cbf import MPC  # noqa: E402
 
 
@@ -100,7 +99,8 @@ def _initialize_sogp_models(self) -> None:
 
         if not isinstance(loaded_params, dict):
             logging.warning(
-                f"YAML 'hyperparameters' format is invalid (type: {type(loaded_params)}). "
+                "YAML 'hyperparameters' format is invalid "
+                f"(type: {type(loaded_params)}). "
                 "Using all default values."
             )
             loaded_params = {}
@@ -111,7 +111,8 @@ def _initialize_sogp_models(self) -> None:
             )
         else:
             logging.info(
-                "Loading SOGP hyperparameters from YAML, using defaults for any missing keys."
+                "Loading SOGP hyperparameters from YAML, using "
+                "defaults for any missing keys."
             )
 
         length_scale = loaded_params.get(
@@ -221,7 +222,6 @@ def load_yaml_params(self, filename: str):
             params = yaml.load(f.read(), Loader=yaml.SafeLoader)
         return params
 
-
     def wrap_pi2negpi(self, angle):
         """This function wraps the angle to the range -pi to pi.
 
@@ -259,14 +259,12 @@ def run_mpc(self, x0, xr, residual):
         """
 
         if self.gp_enabled and np.any(np.abs(residual) > 1e2):
-            logging.warning(
-                f"Large GP residual {residual.T}, resetting to zero."
-            )
+            logging.warning(f"Large GP residual {residual.T}, resetting to zero.")
             residual = np.zeros_like(residual)
 
         current_gp_res_for_mpc = (
-                residual if self.gp_enabled else np.zeros_like(residual)
-            )
+            residual if self.gp_enabled else np.zeros_like(residual)
+        )
 
         process_t0 = time.perf_counter()
         self.distance = np.linalg.norm(x0[0:3, :] - xr[0:3, :])