Enabled controller tuning of LQR and PID during runtime.

guidance/dp_guidance/dp_guidance/dp_guidance_node.py

@@ -7,7 +7,7 @@
 from vortex_msgs.srv import Waypoint
 from nav_msgs.msg import Odometry
 from transforms3d.euler import quat2euler
-
+from std_msgs.msg import Float32
 from conversions import odometrymsg_to_state, state_to_odometrymsg
 from reference_filter import ReferenceFilter
 from rclpy.qos import QoSProfile, qos_profile_sensor_data, QoSReliabilityPolicy
@@ -27,7 +27,9 @@ def __init__(self):
         self.waypoint_server = self.create_service(Waypoint, 'waypoint_list', self.waypoint_callback)
         self.eta_subscriber_ = self.state_subscriber_ = self.create_subscription(Odometry, '/seapath/odom/ned', self.eta_callback, qos_profile=qos_profile)
         self.guidance_publisher = self.create_publisher(Odometry, 'guidance/dp/reference', 1)
-
+        self.yaw_publisher = self.create_publisher(Float32, 'yaw', 1)
+        self.yaw_ref_publisher = self.create_publisher(Float32, 'yaw_ref', 1)
+
         # Get parameters
         self.dt = self.get_parameter('dp_guidance.dt').get_parameter_value().double_value
 
@@ -39,8 +41,7 @@ def __init__(self):
         self.eta_received = False
         self.eta_logger = True
 
-        self.eta = np.array([0, 0, 0])
-        self.eta_ref = np.array([0, 0, 0])
+        self.yaw_ref = 0
 
         self.xd = np.zeros(9)
 
@@ -62,6 +63,7 @@ def waypoint_callback(self, request, response):
     def eta_callback(self, msg: Odometry):
         self.eta = odometrymsg_to_state(msg)[:3]
         self.eta_received = True
+        self.yaw_publisher.publish(Float32(data=self.eta[2]))
 
     def guidance_callback(self):
         if self.waypoints_received and self.eta_received:
@@ -70,7 +72,7 @@ def guidance_callback(self):
                 self.get_logger().info(f"Reference initialized at {self.xd[0:3]}")
                 self.init_pos = True
             last_waypoint = self.waypoints[-1]
-            self.eta_ref = np.array([last_waypoint.x, last_waypoint.y, self.eta[2]])
+            self.eta_ref = np.array([last_waypoint.x, last_waypoint.y, self.yaw_ref])
             x_next = self.reference_filter.step(self.eta_ref, self.xd)
             self.xd = x_next
             # self.get_logger().info(f'x_next[0]: {x_next[0]}')

motion/lqr_controller/lqr_controller/lqr_controller.py

@@ -2,34 +2,37 @@
 from scipy.linalg import solve_continuous_are
 
 class LQRController:
-    def __init__(self, M: float, D: list[float], Q: list[float], R: list[float], heading_ref: float = 0.0):
-        self.M = M
-        self.M_inv = np.linalg.inv(self.M)
-        self.D = D  
+    def __init__(self):
+        self.M = np.eye(3)
+        self.D = np.eye(3)
+        self.Q = np.eye(6)
+        self.R = np.eye(3)
 
         self.A = np.zeros((6,6))
         self.B  = np.zeros((6,3))
         self.C = np.zeros((3,6))
         self.C[:3, :3] = np.eye(3)
-
+
+    def update_parameters(self, M: float, D: list[float], Q: list[float], R: list[float]):
+        self.M = M
+        self.M_inv = np.linalg.inv(self.M)
+        self.D = D  
+
         self.Q = np.diag(Q)
-        print(f"Q: {self.Q}")
         self.R = np.diag(R)
 
-        self.linearize_model(heading_ref)
-
     def calculate_control_input(self, x, x_ref):
         tau = -self.K_LQR @ x + self.K_r @ x_ref
         return tau
 
-    def linearize_model(self, heading: float) -> tuple[np.ndarray, np.ndarray]:
-        R = np.array(
+    def calculate_model(self, heading: float) -> tuple[np.ndarray, np.ndarray]:
+        rotation_matrix = np.array(
             [[np.cos(heading), -np.sin(heading), 0],
             [np.sin(heading), np.cos(heading), 0],
             [0, 0, 1]]
         )
 
-        self.A[:3,3:] = R
+        self.A[:3,3:] = rotation_matrix
         self.A[3:, 3:] = - self.M_inv @ self.D
 
         self.B[3:,:] = self.M_inv

motion/lqr_controller/lqr_controller/lqr_controller_node.py

@@ -8,6 +8,8 @@
 from lqr_controller import LQRController
 from conversions import odometrymsg_to_state
 from time import sleep
+from std_msgs.msg import Float32
+from rcl_interfaces.msg import SetParametersResult
 
 from rclpy.qos import QoSProfile, qos_profile_sensor_data, QoSReliabilityPolicy
 
@@ -27,42 +29,63 @@ def __init__(self):
                 ('physical.damping_matrix_diag', [77.55, 162.5, 42.65])
             ])
 
-        self.state_subscriber_ = self.create_subscription(Odometry, "/sensor/seapath/odom/ned", self.state_cb, qos_profile=qos_profile)
+        self.parameters_updated = False
+
+        self.state_subscriber_ = self.create_subscription(Odometry, "/seapath/odom/ned", self.state_cb, qos_profile=qos_profile)
         self.guidance_subscriber_ = self.create_subscription(Odometry, "guidance/dp/reference", self.guidance_cb, 1)
         self.wrench_publisher_ = self.create_publisher(Wrench, "thrust/wrench_input", 1)
+
+        self.LQR_controller = LQRController()
 
-        Q = self.get_parameter('lqr_controller.Q').get_parameter_value().double_array_value
-        R = self.get_parameter('lqr_controller.R').get_parameter_value().double_array_value
-        D_diag = self.get_parameter('physical.damping_matrix_diag').get_parameter_value().double_array_value
-        M = self.get_parameter('physical.inertia_matrix').get_parameter_value().double_array_value
-
-        D = np.diag(D_diag)
-        M = np.reshape(M, (3, 3))
-
-        heading_ref = 0.0
-
-        self.LQR = LQRController(M, D, Q, R, heading_ref)
-
-        self.x_ref = [0, 0, 0]
-        self.state = [0, 0, 0, 0, 0, 0]
+        self.update_controller_parameters()
 
         self.enabled = False
 
         controller_period = 0.1
         self.controller_timer_ = self.create_timer(controller_period, self.controller_callback)
+        self.add_on_set_parameters_callback(self.parameter_callback)
 
         self.get_logger().info("lqr_controller_node started")
 
+    def update_controller_parameters(self):
+        Q = self.get_parameter('lqr_controller.Q').get_parameter_value().double_array_value
+        R = self.get_parameter('lqr_controller.R').get_parameter_value().double_array_value
+        D_diag = self.get_parameter('physical.damping_matrix_diag').get_parameter_value().double_array_value
+        M = self.get_parameter('physical.inertia_matrix').get_parameter_value().double_array_value
+
+        D = np.diag(D_diag)
+        M = np.reshape(M, (3, 3))
+
+        self.LQR_controller.update_parameters(M, D, Q, R)
+
+    def parameter_callback(self, params):
+        self.parameters_updated = True
+        for param in params:
+            if param.name == 'lqr_controller.Q':
+                self.get_logger().info(f"Updated Q to {param.value}")
+            elif param.name == 'lqr_controller.R':
+                self.get_logger().info(f"Updated R to {param.value}")
+            elif param.name == 'physical.inertia_matrix':
+                self.get_logger().info(f"Updated inertia_matrix to {param.value}")
+            elif param.name == 'physical.damping_matrix_diag':
+                self.get_logger().info(f"Updated damping_matrix_diag to {param.value}")
+
+        return SetParametersResult(successful=True)
+
     def state_cb(self, msg):
         self.state = odometrymsg_to_state(msg)
 
     def guidance_cb(self, msg):
         self.x_ref = odometrymsg_to_state(msg)[:3]
 
     def controller_callback(self):
+        if self.parameters_updated:
+            self.update_controller_parameters()
+            self.parameters_updated = False
+
         if hasattr(self, 'state') and hasattr(self, 'x_ref'):
-            self.LQR.linearize_model(self.state[2])
-            control_input = self.LQR.calculate_control_input(self.state, self.x_ref)
+            self.LQR_controller.calculate_model(self.state[2])
+            control_input = self.LQR_controller.calculate_control_input(self.state, self.x_ref)
             wrench_msg = Wrench()
             wrench_msg.force.x  = control_input[0]
             wrench_msg.force.y  = control_input[1]

motion/pid_controller/config/pid_config.yaml

@@ -1,6 +1,6 @@
 /**:
   ros__parameters:
     pid_controller:
-      Kp: [23.04, 34.56, 4.032] # Proportional costs for [x, y, heading]
-      Ki: [2.76, 4.1, 0.48]  # Integral costs for [x, y, heading]
-      Kd: [28.7648, 43.1472, 5.048384]  # Derivative costs for [x, y, heading]
+      Kp: [130., 240., 60.] # Proportional costs for [x, y, heading]
+      Ki: [15., 29., 7.]  # Integral costs for [x, y, heading]
+      Kd: [160., 300., 75.]  # Derivative costs for [x, y, heading]

motion/pid_controller/pid_controller/pid_controller.py

@@ -3,15 +3,20 @@
 import numpy as np
 
 class PID:
-    def __init__(self, Kp, Ki, Kd):
+    def __init__(self):
         self.error_sum = np.zeros(3)
 
+        self.Kp = np.eye(3)
+        self.Ki = np.eye(3)
+        self.Kd = np.eye(3)
+
+        # self.tau_max = np.array([100, 100, 100])
+
+    def update_parameters(self, Kp, Ki, Kd):
         self.Kp = np.diag(Kp)
         self.Ki = np.diag(Ki)
         self.Kd = np.diag(Kd)
 
-        self.tau_max = np.array([100, 100, 100])
-
     def calculate_control_input(self, eta, eta_d, eta_dot, dt):
         error = eta - eta_d
         self.error_sum += error * dt
@@ -25,19 +30,19 @@ def calculate_control_input(self, eta, eta_d, eta_dot, dt):
 
         tau = -(p + i + d)
 
-        if tau[0] > self.tau_max[0]:
-            tau[0] = self.tau_max[0]
-        elif tau[0] < -self.tau_max[0]:
-            tau[0] = -self.tau_max[0]
+        # if tau[0] > self.tau_max[0]:
+        #     tau[0] = self.tau_max[0]
+        # elif tau[0] < -self.tau_max[0]:
+        #     tau[0] = -self.tau_max[0]
 
-        if tau[1] > self.tau_max[1]:
-            tau[1] = self.tau_max[1]
-        elif tau[1] < -self.tau_max[1]:
-            tau[1] = -self.tau_max[1]
-
-        if tau[2] > self.tau_max[2]:
-            tau[2] = self.tau_max[2]
-        elif tau[2] < -self.tau_max[2]:
-            tau[2] = -self.tau_max[2]
+        # if tau[1] > self.tau_max[1]:
+        #     tau[1] = self.tau_max[1]
+        # elif tau[1] < -self.tau_max[1]:
+        #     tau[1] = -self.tau_max[1]
+
+        # if tau[2] > self.tau_max[2]:
+        #     tau[2] = self.tau_max[2]
+        # elif tau[2] < -self.tau_max[2]:
+        #     tau[2] = -self.tau_max[2]
 
         return tau

motion/pid_controller/pid_controller/pid_controller_node.py

@@ -8,6 +8,7 @@
 from pid_controller import PID
 from conversions import odometrymsg_to_state
 from time import sleep
+from rcl_interfaces.msg import SetParametersResult
 
 from rclpy.qos import QoSProfile, qos_profile_sensor_data, QoSReliabilityPolicy
 
@@ -24,40 +25,61 @@ def __init__(self):
                 ('pid_controller.Kp', [1.0, 1.0, 1.0]),
                 ('pid_controller.Ki', [1.0, 1.0, 1.0]),
                 ('pid_controller.Kd', [1.0, 1.0, 1.0]),
-                ('physical.inertia_matrix', [90.5, 0.0, 0.0, 0.0, 167.5, 12.25, 0.0, 12.25, 42.65])
-            ])
+                ])
 
-        self.state_subscriber_ = self.create_subscription(Odometry, "/sensor/seapath/odom/ned", self.state_cb, qos_profile=qos_profile)
+        self.parameters_updated = False
+
+        self.state_subscriber_ = self.create_subscription(Odometry, "/seapath/odom/ned", self.state_cb, qos_profile=qos_profile)
         self.guidance_subscriber_ = self.create_subscription(Odometry, "guidance/dp/reference", self.guidance_cb, 1)
         self.wrench_publisher_ = self.create_publisher(Wrench, "thrust/wrench_input", 1)
 
-        Kp = self.get_parameter('pid_controller.Kp').get_parameter_value().double_array_value
-        Ki = self.get_parameter('pid_controller.Ki').get_parameter_value().double_array_value
-        Kd = self.get_parameter('pid_controller.Kd').get_parameter_value().double_array_value
-        M = self.get_parameter('physical.inertia_matrix').get_parameter_value().double_array_value
-
-        M = np.reshape(M, (3, 3))
-        M_diag = np.diag(M)
-
         ## PID TUNING VALUES ## (OVERWRITES YAML FILE VALUES)
-        omega_n = 1.2
-        zeta = 0.75
-        Kp = M_diag * omega_n**2
-        Kd = M_diag * 2 * zeta * omega_n #- D_diag
-        Ki = omega_n/10 * Kp
+        # M = self.get_parameter('physical.inertia_matrix').get_parameter_value().double_array_value
+
+        # M = np.reshape(M, (3, 3))
+        # M_diag = np.diag(M)
+        # omega_n = 1.2
+        # zeta = 0.75
+        # Kp = M_diag * omega_n**2
+        # self.get_logger().info(f"Kp: {Kp}")
+        # Kd = M_diag * 2 * zeta * omega_n #- D_diag
+        # self.get_logger().info(f"Kd: {Kd}")
+        # Ki = omega_n/10 * Kp
+        # self.get_logger().info(f"Ki: {Ki}")
 
-        self.pid = PID(Kp, Ki, Kd)
+        self.pid_controller = PID()
 
-        self.x_ref = np.array([0, 0, 0])
-        self.state = np.array([0, 0, 0, 0, 0, 0])
+        self.update_controller_parameters()
 
         self.enabled = False
 
         self.controller_period = 0.1
         self.controller_timer_ = self.create_timer(self.controller_period, self.controller_callback)
-
+        self.add_on_set_parameters_callback(self.parameter_callback)
+
         self.get_logger().info("pid_controller_node started")
 
+    def update_controller_parameters(self):
+        Kp = self.get_parameter('pid_controller.Kp').get_parameter_value().double_array_value
+        Ki = self.get_parameter('pid_controller.Ki').get_parameter_value().double_array_value
+        Kd = self.get_parameter('pid_controller.Kd').get_parameter_value().double_array_value
+
+        self.pid_controller.update_parameters(Kp, Ki, Kd)
+
+        self.get_logger().info("Updated controller parameters")
+
+    def parameter_callback(self, params):
+        self.parameters_updated = True
+        for param in params:
+            if param.name == 'pid_controller.Kp':
+                self.get_logger().info(f"Updated Kp to {param.value}")
+            elif param.name == 'pid_controller.Ki':
+                self.get_logger().info(f"Updated Ki to {param.value}")
+            elif param.name == 'pid_controller.Kd':
+                self.get_logger().info(f"Updated Kd to {param.value}")
+
+        return SetParametersResult(successful=True)
+
     def state_cb(self, msg):
         self.state = odometrymsg_to_state(msg)
 
@@ -66,7 +88,7 @@ def guidance_cb(self, msg):
 
     def controller_callback(self):
         if hasattr(self, 'state') and hasattr(self, 'x_ref'):
-            control_input = self.pid.calculate_control_input(self.state[:3], self.x_ref, self.state[3:], self.controller_period)
+            control_input = self.pid_controller.calculate_control_input(self.state[:3], self.x_ref, self.state[3:], self.controller_period)
             # self.get_logger().info(f"Control input: {control_input}")
             wrench_msg = Wrench()
             wrench_msg.force.x  = control_input[0]