From 09110faf381e42f6512d4a6a288f50bf6eb08f56 Mon Sep 17 00:00:00 2001 From: Andeshog <157955984+Andeshog@users.noreply.github.com> Date: Sat, 13 Jul 2024 13:28:01 +0200 Subject: [PATCH] Enabled controller tuning of LQR and PID during runtime. --- .../dp_guidance/dp_guidance_node.py | 12 ++-- .../lqr_controller/lqr_controller.py | 25 ++++---- .../lqr_controller/lqr_controller_node.py | 57 ++++++++++++----- motion/pid_controller/config/pid_config.yaml | 6 +- .../pid_controller/pid_controller.py | 37 ++++++----- .../pid_controller/pid_controller_node.py | 64 +++++++++++++------ 6 files changed, 128 insertions(+), 73 deletions(-) diff --git a/guidance/dp_guidance/dp_guidance/dp_guidance_node.py b/guidance/dp_guidance/dp_guidance/dp_guidance_node.py index b0d754f5..317332da 100755 --- a/guidance/dp_guidance/dp_guidance/dp_guidance_node.py +++ b/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]}') diff --git a/motion/lqr_controller/lqr_controller/lqr_controller.py b/motion/lqr_controller/lqr_controller/lqr_controller.py index 7c7b6f67..d9507b67 100644 --- a/motion/lqr_controller/lqr_controller/lqr_controller.py +++ b/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 diff --git a/motion/lqr_controller/lqr_controller/lqr_controller_node.py b/motion/lqr_controller/lqr_controller/lqr_controller_node.py index 9261d41a..6ffd5420 100755 --- a/motion/lqr_controller/lqr_controller/lqr_controller_node.py +++ b/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,32 +29,49 @@ 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) @@ -60,9 +79,13 @@ 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] diff --git a/motion/pid_controller/config/pid_config.yaml b/motion/pid_controller/config/pid_config.yaml index f04befd2..f600f9f0 100644 --- a/motion/pid_controller/config/pid_config.yaml +++ b/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] \ No newline at end of file + 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] \ No newline at end of file diff --git a/motion/pid_controller/pid_controller/pid_controller.py b/motion/pid_controller/pid_controller/pid_controller.py index 1207fbb1..1bd218e1 100644 --- a/motion/pid_controller/pid_controller/pid_controller.py +++ b/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 \ No newline at end of file diff --git a/motion/pid_controller/pid_controller/pid_controller_node.py b/motion/pid_controller/pid_controller/pid_controller_node.py index 700a9d4d..dfff3a08 100755 --- a/motion/pid_controller/pid_controller/pid_controller_node.py +++ b/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]