From 6d555579a161145f5405eaf560a8f2e7a87588ba Mon Sep 17 00:00:00 2001 From: Martin Huynh Date: Sun, 7 Apr 2024 12:03:18 +0200 Subject: [PATCH] Removed old lqr_controller --- .../lqr_controller/lqr_controller/__init__.py | 0 .../lqr_controller/asv_model.py | 72 -------------- .../lqr_controller/lqr_controller.py | 43 -------- .../lqr_controller/lqr_controller_node.py | 98 ------------------- 4 files changed, 213 deletions(-) delete mode 100644 motion/lqr_controller/lqr_controller/__init__.py delete mode 100644 motion/lqr_controller/lqr_controller/asv_model.py delete mode 100644 motion/lqr_controller/lqr_controller/lqr_controller.py delete mode 100644 motion/lqr_controller/lqr_controller/lqr_controller_node.py diff --git a/motion/lqr_controller/lqr_controller/__init__.py b/motion/lqr_controller/lqr_controller/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/motion/lqr_controller/lqr_controller/asv_model.py b/motion/lqr_controller/lqr_controller/asv_model.py deleted file mode 100644 index 09231ce1..00000000 --- a/motion/lqr_controller/lqr_controller/asv_model.py +++ /dev/null @@ -1,72 +0,0 @@ -import numpy as np - -class ASV(): - - def __init__(self, M: np.ndarray, D: np.ndarray): - """ - Initialize some system matrices - """ - self.M = M - self.M_inv = np.linalg.inv(self.M) - self.D = D - - def linearize_model(self, heading: float) -> tuple[np.ndarray, np.ndarray]: - """ - Get a linearization about some heading - """ - J = np.array( - [[np.cos(heading), -np.sin(heading), 0], - [np.sin(heading), np.cos(heading), 0], - [0, 0, 1]] - ) - - A = np.zeros((6,6)) - - A[:3,3:] = J - A[3:, 3:] = - self.M_inv @ self.D - - B = np.zeros((6,3)) - B[3:,:] = self.M_inv - - return A, B - - def RK4_integration_step(self, x: np.ndarray, u: np.ndarray, dt: float) -> np.ndarray: - """ - Perform an integration step using the Runge-Kutta 4 integrator - """ - k1 = self.state_dot(x, u) - k2 = self.state_dot(x+dt/2*k1, u) - k3 = self.state_dot(x+dt/2*k2, u) - k4 = self.state_dot(x+dt*k3, u) - - x_next = x + dt/6*(k1+2*k2+2*k3+k4) - - - return x_next - - def state_dot(self, state: np.ndarray, tau_actuation: np.ndarray, V_current: np.ndarray = np.zeros(2)) -> np.ndarray: - """ - Calculate the derivative of the state using the non-linear kinematics - """ - heading = state[2] - - J = np.array( - [[np.cos(heading), -np.sin(heading), 0], - [np.sin(heading), np.cos(heading), 0], - [0, 0, 1]] - ) - - A = np.zeros((6,6)) - - A[:3,3:] = J - A[3:, 3:] = - self.M_inv @ self.D - - B = np.zeros((6,3)) - B[3:,:] = self.M_inv - - x_dot = A @ state + B @ tau_actuation - x_dot[0:2] += V_current # add current drift term at velocity level - - return x_dot - - diff --git a/motion/lqr_controller/lqr_controller/lqr_controller.py b/motion/lqr_controller/lqr_controller/lqr_controller.py deleted file mode 100644 index 2d02230d..00000000 --- a/motion/lqr_controller/lqr_controller/lqr_controller.py +++ /dev/null @@ -1,43 +0,0 @@ -import numpy as np -import matplotlib.pyplot as plt -from scipy.linalg import solve_continuous_are -from lqr_controller.asv_model import ASV - -def ssa(angle): - return (angle + np.pi) % (2 * np.pi) - np.pi - -class LQRController: - - def __init__(self, m: float, D: list[float], Q: list[float], R: list[float]): - self.heading_ref = 50*np.pi/180 - - self.M = np.diag([m, m, m]) - self.M_inv = np.linalg.inv(self.M) - self.D = np.diag(D) - - self.asv = ASV(self.M, self.D) - - self.linearize_model(self.heading_ref) - - C = np.zeros((3,6)) - C[:3, :3] = np.eye(3) - - self.Q = np.diag(Q) - self.R = np.diag(R) - self.P = solve_continuous_are(self.A, self.B, self.Q, self.R) - self.K_LQR = np.dot(np.dot(np.linalg.inv(self.R), self.B.T), self.P) - self.K_r = np.linalg.inv(C@np.linalg.inv(self.B @ self.K_LQR - self.A) @ self.B) - - def linearize_model(self, heading: float): - self.A, self.B = self.asv.linearize_model(heading) - - def calculate_control_input(self, x, x_ref, K_LQR, K_r): - # print("x:", x) - # print("x_ref:", x) - # print("K_LQR:", K_LQR) - # print("K_r:", K_r) - u = -K_LQR @ x + K_r @ x_ref - return u - - - \ No newline at end of file diff --git a/motion/lqr_controller/lqr_controller/lqr_controller_node.py b/motion/lqr_controller/lqr_controller/lqr_controller_node.py deleted file mode 100644 index 5f180f37..00000000 --- a/motion/lqr_controller/lqr_controller/lqr_controller_node.py +++ /dev/null @@ -1,98 +0,0 @@ -import rclpy -import numpy as np -from rclpy.node import Node -from geometry_msgs.msg import Wrench -from nav_msgs.msg import Odometry -from transforms3d.euler import quat2euler -from lqr_controller.lqr_controller import LQRController - -class LQRControllerNode(Node): - def __init__(self): - super().__init__("lqr_controller_node") - - # Load parameters from lqr_config.yaml - self.declare_parameters( - namespace='', - parameters=[ - ('lqr_controller.mass', 1.0), - ('lqr_controller.D', [0.0, 0.0, 0.0]), - ('lqr_controller.Q', [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]), - ('lqr_controller.R', [0.0, 0.0, 0.0]) - ]) - - self.wrench_publisher_ = self.create_publisher(Wrench, "thrust/wrench_input", 1) - self.state_subscriber_ = self.create_subscription(Odometry, "/sensor/seapath/odometry/ned", self.state_cb, 1) - self.guidance_subscriber_ = self.create_subscription(Odometry, "controller/lqr/reference", self.guidance_cb, 1) - - m = self.get_parameter('lqr_controller.mass').get_parameter_value().double_value - D = self.get_parameter('lqr_controller.D').get_parameter_value().double_array_value - 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 - - self.get_logger().info(f"Mass: {m}") - self.get_logger().info(f"D: {D}") - self.get_logger().info(f"Q: {Q}") - self.get_logger().info(f"R: {R}") - - self.lqr_controller = LQRController(m, D, Q, R) - - # Using x, y, yaw as reference (1x3) - self.x_ref = [0, 0, 0] - self.state = [0, 0, 0, 0, 0, 0] - - self.get_logger().info("lqr_controller_node started") - - def odometrymsg_to_state(self, msg): - x = msg.pose.pose.position.x - y = msg.pose.pose.position.y - orientation_q = msg.pose.pose.orientation - orientation_list = [ - orientation_q.w, orientation_q.x, orientation_q.y, orientation_q.z - ] - - # Convert quaternion to Euler angles, extract yaw - yaw = quat2euler(orientation_list)[2] - - vx = msg.twist.twist.linear.x - vy = msg.twist.twist.linear.y - vyaw = msg.twist.twist.angular.z - - state = np.array([x, y, yaw, vx, vy, vyaw]) - return state - - def guidance_cb(self, msg): - self.x_ref = self.odometrymsg_to_state(msg)[:3] - - #wrench = self.run_lqr_to_wrench() - #self.wrench_publisher_.publish(wrench) - - def state_cb(self, msg): - self.state = self.odometrymsg_to_state(msg) - - wrench = self.run_lqr_to_wrench() - self.wrench_publisher_.publish(wrench) - - def run_lqr_to_wrench(self): - self.lqr_controller.linearize_model(self.state[2]) - u = self.lqr_controller.calculate_control_input(self.state, self.x_ref, self.lqr_controller.K_LQR, self.lqr_controller.K_r) - - wrench = Wrench() - wrench.force.x = u[0] - wrench.force.y = u[1] - wrench.force.z = 0.0 - wrench.torque.x = 0.0 - wrench.torque.y = 0.0 - wrench.torque.z = u[2] - - return wrench - - -def main(args=None): - rclpy.init(args=args) - node = LQRControllerNode() - rclpy.spin(node) - node.destroy_node() - rclpy.shutdown() - -if __name__ == "__main__": - main()