From 18542f8ee69ab085998cc56170929fa49c75bc1f Mon Sep 17 00:00:00 2001 From: Cyprian Date: Sun, 10 Nov 2024 17:31:07 +0100 Subject: [PATCH 01/17] make Andreas happy with precomitt hok --- .../velocity_controller_lqr/CMakeLists.txt | 45 ++++ .../config/param_velocity_controller_lqr.yaml | 24 ++ .../launch/velocity_controller_lqr.launch.py | 35 +++ control/velocity_controller_lqr/package.xml | 26 ++ .../scripts/velocity_controller_lqr_node.py | 132 +++++++++ .../tests/test_velocity_controller_lqr.py | 67 +++++ .../velocity_controller_lqr/__init__.py | 1 + .../velocity_controller_lqr_lib.py | 255 ++++++++++++++++++ 8 files changed, 585 insertions(+) create mode 100644 control/velocity_controller_lqr/CMakeLists.txt create mode 100644 control/velocity_controller_lqr/config/param_velocity_controller_lqr.yaml create mode 100644 control/velocity_controller_lqr/launch/velocity_controller_lqr.launch.py create mode 100644 control/velocity_controller_lqr/package.xml create mode 100644 control/velocity_controller_lqr/scripts/velocity_controller_lqr_node.py create mode 100644 control/velocity_controller_lqr/tests/test_velocity_controller_lqr.py create mode 100644 control/velocity_controller_lqr/velocity_controller_lqr/__init__.py create mode 100644 control/velocity_controller_lqr/velocity_controller_lqr/velocity_controller_lqr_lib.py diff --git a/control/velocity_controller_lqr/CMakeLists.txt b/control/velocity_controller_lqr/CMakeLists.txt new file mode 100644 index 00000000..59fef0e8 --- /dev/null +++ b/control/velocity_controller_lqr/CMakeLists.txt @@ -0,0 +1,45 @@ +cmake_minimum_required(VERSION 3.8) +project(velocity_controller_lqr) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) +find_package(rclpy REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(vortex_msgs REQUIRED) + +install(DIRECTORY + launch + config + DESTINATION share/${PROJECT_NAME} +) + +ament_python_install_package(${PROJECT_NAME}) + +#install python scripts +install(PROGRAMS + scripts/velocity_controller_lqr_node.py + DESTINATION lib/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_cmake_pytest REQUIRED) + set(_pytest_tests + tests/test_velocity_controller_lqr.py + ) + foreach(_test_path ${_pytest_tests}) + get_filename_component(_test_name ${_test_path} NAME_WE) + ament_add_pytest_test(${_test_name} ${_test_path} + APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR} + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} + ) + endforeach() +endif() + +ament_package() diff --git a/control/velocity_controller_lqr/config/param_velocity_controller_lqr.yaml b/control/velocity_controller_lqr/config/param_velocity_controller_lqr.yaml new file mode 100644 index 00000000..ad90fc6a --- /dev/null +++ b/control/velocity_controller_lqr/config/param_velocity_controller_lqr.yaml @@ -0,0 +1,24 @@ +velocity_controller_lqr_node: + ros__parameters: + + #Path parameters + odom_path: /nucleus/odom + guidance_path: /guidance/los + thrust_path: /thrust/wrench_input + + #LQR parameters + + Q_surge: 75 + Q_pitch: 175 + Q_yaw: 175 + + R_surge: 0.3 + R_pitch: 0.4 + R_yaw: 0.4 + + I_surge: 0.3 + I_pitch: 0.4 + I_yaw: 0.3 + + #Clamp parameter + max_force: 99.5 \ No newline at end of file diff --git a/control/velocity_controller_lqr/launch/velocity_controller_lqr.launch.py b/control/velocity_controller_lqr/launch/velocity_controller_lqr.launch.py new file mode 100644 index 00000000..570058bd --- /dev/null +++ b/control/velocity_controller_lqr/launch/velocity_controller_lqr.launch.py @@ -0,0 +1,35 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description() -> LaunchDescription: + """ + Generates a launch description for the velocity_controller_lqr node. + + This function creates a ROS 2 launch description that includes the + velocity_controller_lqr node. The node is configured to use the + parameters specified in the 'params_velocity_controller_lqr.yaml' file. + + Returns: + LaunchDescription: A ROS 2 launch description containing the + velocity_controller_lqr node. + """ + # Define the path to the parameter file + parameter_file = os.path.join( + get_package_share_directory("velocity_controller_lqr"), + "config", + "param_velocity_controller_lqr.yaml" + ) + + # Define the node + velocity_controller_node = Node( + package="velocity_controller_lqr", + executable="velocity_controller_lqr_node.py", # Ensure this matches your Python file name + name="velocity_controller_lqr_node", + output="screen", + parameters=[parameter_file], + ) + + return LaunchDescription([velocity_controller_node]) diff --git a/control/velocity_controller_lqr/package.xml b/control/velocity_controller_lqr/package.xml new file mode 100644 index 00000000..ed5074d0 --- /dev/null +++ b/control/velocity_controller_lqr/package.xml @@ -0,0 +1,26 @@ + + + + velocity_controller_lqr + 1.0.0 + Velocity controller package for the AUV Orca + cyprian + MIT + + ament_cmake + + rclcpp + rclpy + geometry_msgs + nav_msgs + python3-control + vortex_msgs + + ament_lint_auto + ament_lint_common + ament_cmake_pytest + + + ament_cmake + + diff --git a/control/velocity_controller_lqr/scripts/velocity_controller_lqr_node.py b/control/velocity_controller_lqr/scripts/velocity_controller_lqr_node.py new file mode 100644 index 00000000..bf34177e --- /dev/null +++ b/control/velocity_controller_lqr/scripts/velocity_controller_lqr_node.py @@ -0,0 +1,132 @@ +#!/usr/bin/env python3 +import numpy as np +import rclpy +from geometry_msgs.msg import Wrench +from nav_msgs.msg import Odometry +from rclpy.node import Node +from std_msgs.msg import Float32MultiArray +from velocity_controller_lqr.velocity_controller_lqr_lib import LQR_controller +from vortex_msgs.msg import LOSGuidance + + +class VelocityLQRNode(Node): + def __init__(self): + super().__init__("velocity_controller_lqr_node") + + self.declare_parameter("odom_path", "/nucleus/odom") + self.declare_parameter("guidance_path", "/guidance/los") + self.declare_parameter("thrust_path", "/thrust/wrench_input") + + odom_path = self.get_parameter("odom_path").value + guidance_path = self.get_parameter("guidance_path").value + thrust_path = self.get_parameter("thrust_path").value + + # SUBSRCIBERS ----------------------------------- + self.nucleus_subscriber = self.create_subscription( + Odometry, odom_path, self.nucleus_callback, 20 + ) + self.guidance_subscriber = self.create_subscription( + LOSGuidance, guidance_path, self.guidance_callback, 20 + ) + + # PUBLISHERS ------------------------------------ + self.publisherLQR = self.create_publisher( + Wrench, thrust_path, 10 + ) + + # TIMERS ---------------------------------------- + self.control_timer = self.create_timer( + 0.1, self.controller + ) + self.state_timer = self.create_timer( + 0.1, self.publish_states + ) + + # ROS2 SHENNANIGANS with parameters + self.declare_parameter("max_force", 99.5) + max_force = self.get_parameter("max_force").value + + self.declare_parameter("Q_surge", 75) + self.declare_parameter("Q_pitch", 175) + self.declare_parameter("Q_yaw", 175) + + self.declare_parameter("R_surge", 0.3) + self.declare_parameter("R_pitch", 0.4) + self.declare_parameter("R_yaw", 0.4) + + self.declare_parameter("I_surge", 0.3) + self.declare_parameter("I_pitch", 0.4) + self.declare_parameter("I_yaw", 0.3) + + Q_surge = self.get_parameter("Q_surge").value + Q_pitch = self.get_parameter("Q_pitch").value + Q_yaw = self.get_parameter("Q_yaw").value + + R_surge = self.get_parameter("R_surge").value + R_pitch = self.get_parameter("R_pitch").value + R_yaw = self.get_parameter("R_yaw").value + + I_surge = self.get_parameter("I_surge").value + I_pitch = self.get_parameter("I_pitch").value + I_yaw = self.get_parameter("I_yaw").value + + self.controller = LQR_controller(Q_surge, Q_pitch, Q_yaw, R_surge, R_pitch, R_yaw, I_surge, I_pitch, I_yaw, max_force) + + # Only keeping variables that need to be updated inside of a callback + self.C = np.zeros((3, 3)) # Initialize Coriolis matrix + self.states = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) # State vector 1. surge, 2. pitch, 3. yaw, 4. integral_surge, 5. integral_pitch, 6. integral_yaw + self.guidance_values = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) # Array to store guidance values + + # ---------------------------------------------------------------Callback Functions--------------------------------------------------------------- + + def nucleus_callback(self, msg: Odometry): # callback function to read data from nucleus + dummy, self.states[1], self.states[2] = LQR_controller.quaternion_to_euler_angle( + msg.pose.pose.orientation.w, + msg.pose.pose.orientation.x, + msg.pose.pose.orientation.y, + msg.pose.pose.orientation.z, + ) + + self.states[0] = msg.twist.twist.linear.x + + self.C = self.controller.calculate_coriolis_matrix( + msg.twist.twist.angular.y, + msg.twist.twist.angular.z, + msg.twist.twist.linear.y, + msg.twist.twist.linear.z, + ) # coriolis matrix + + def guidance_callback( + self, msg: LOSGuidance + ): # Function to read data from guidance + self.guidance_values[0] = msg.surge + self.guidance_values[1] = msg.pitch + self.guidance_values[2] = msg.yaw + + # ---------------------------------------------------------------Publisher Functions--------------------------------------------------------------- + + def controller(self): # The LQR controller function + msg = Wrench() + + u = self.controller.calculate_lqr_u(self.C, self.states, self.guidance_values) + msg.force.x = u[0] + msg.torque.y = u[1] + msg.torque.z = u[2] + + self.get_logger().info(f"Input values: {msg.force.x}, {msg.torque.y}, {msg.torque.z}") + + # Publish the control input + self.publisherLQR.publish(msg) + +# ---------------------------------------------------------------Main Function--------------------------------------------------------------- + +def main(args=None): # main function + rclpy.init(args=args) + node = VelocityLQRNode() + rclpy.spin(node) + rclpy.shutdown() + +if __name__ == "__main__": + main() + +# Anders er goated \ No newline at end of file diff --git a/control/velocity_controller_lqr/tests/test_velocity_controller_lqr.py b/control/velocity_controller_lqr/tests/test_velocity_controller_lqr.py new file mode 100644 index 00000000..8698680c --- /dev/null +++ b/control/velocity_controller_lqr/tests/test_velocity_controller_lqr.py @@ -0,0 +1,67 @@ +import numpy as np +import pytest +import rclpy +from velocity_controller_lqr.velocity_controller_lqr import LQR_controller + +controller = LQR_controller(0, 0, 0, 0, 0, 0, 0, 0, 0, 0) + +class TestVelocityController: + + + def test_placeholder(self): + assert controller is not None # Simple test to ensure the controller initializes + + def test_ssa(self,capsys): + print("Commencing ssa test: \n") + + assert controller.ssa(np.pi + 1) == -np.pi + 1 + + assert controller.ssa(-np.pi - 1) == (np.pi - 1) + + print("SSA test passed") + + captured = capsys.readouterr() + + def test_quaternion_to_euler_angle(self): + print("Commencing quaternion to euler angle test: \n") + + roll, pitch, yaw = controller.quaternion_to_euler_angle(0.5, 0.5, 0.5, 0.5) + assert roll == np.pi / 2 + assert pitch == 0 + assert yaw == np.pi / 2 + + print("Quaternion to euler angle test passed") + + def test_saturate(self): + + print("Commencing saturate test: \n") + + windup = False + assert controller.saturate(10, windup, 5) == 5 + + windup = False + assert controller.saturate(-10, False, 5) == -5 + + windup = True + assert controller.saturate(10, False, 15) == 10 + + windup = True + assert controller.saturate(-10, False, 15) == -10 + + print("Saturate test passed") + + def test_anti_windup(self): + + print("Commencing anti windup test: \n") + + windup = True + assert controller.anti_windup(10, 5, 10, windup) == 10 + + windup = False + assert controller.anti_windup(1, 5, 10, windup) == 15 + + print("Anti windup test passed") + + def test_final(self): + print("¯\_(ツ)_/¯ ehh good enough pass anyway") + pass \ No newline at end of file diff --git a/control/velocity_controller_lqr/velocity_controller_lqr/__init__.py b/control/velocity_controller_lqr/velocity_controller_lqr/__init__.py new file mode 100644 index 00000000..79b530b8 --- /dev/null +++ b/control/velocity_controller_lqr/velocity_controller_lqr/__init__.py @@ -0,0 +1 @@ +from velocity_controller_lqr.velocity_controller_lqr_lib import LQR_controller \ No newline at end of file diff --git a/control/velocity_controller_lqr/velocity_controller_lqr/velocity_controller_lqr_lib.py b/control/velocity_controller_lqr/velocity_controller_lqr/velocity_controller_lqr_lib.py new file mode 100644 index 00000000..038dc72c --- /dev/null +++ b/control/velocity_controller_lqr/velocity_controller_lqr/velocity_controller_lqr_lib.py @@ -0,0 +1,255 @@ +#!/usr/bin/env python3 +import math as math + +import control as ct +import numpy as np + + +class LQR_controller(): + def __init__(self, Q_surge, Q_pitch, Q_yaw, R_surge, R_pitch, R_yaw, I_surge, I_pitch, I_yaw, max_force): + + self.integral_error_surge = 0.0 + self.integral_error_pitch = 0.0 + self.integral_error_yaw = 0.0 + + self.surge_windup = False # Windup variable + self.pitch_windup = False # Windup variable + self.yaw_windup = False # Windup variable + + self.max_force = max_force # Maximum force that can be applied + + self.I_surge = I_surge # Integral gain for surge + self.I_pitch = I_pitch # Integral gain for pitch + self.I_yaw = I_yaw # Integral gain for yaw + + self.Q_surge = Q_surge # Surge cost + self.Q_pitch = Q_pitch # Pitch cost + self.Q_yaw = Q_yaw # Yaw cost + + self.R_surge = R_surge # Surge control cost + self.R_pitch = R_pitch # Pitch control cost + self.R_yaw = R_yaw # Yaw control cost + + # VARIABLES + self.M_inv = np.linalg.inv(np.array( + [[30, 0.6, 0], + [0.6, 1.629, 0], + [0, 0, 1.769]] + )) # mass matrix + + self.Q = np.block( + [ + [np.diag([self.Q_surge, self.Q_pitch, self.Q_yaw]), np.zeros((3, 3))], + [np.zeros((3, 3)), np.diag([0.5, 0.5, 0.5])] + ] + ) # Augmented state cost matrix + self.R = np.diag([self.R_surge, self.R_pitch, self.R_yaw]) # control cost matrix + + @staticmethod + def quaternion_to_euler_angle(w: float, x: float, y: float, z: float) -> tuple: + """ + Function to convert quaternion to euler angles. + + Args: + w: float: w component of quaternion + x: float: x component of quaternion + y: float: y component of quaternion + z: float: z component of quaternion + + Returns: + X: float: roll angle + Y: float: pitch angle + Z: float: yaw angle + + """ + y_square = y * y + + t0 = +2.0 * (w * x + y * z) + t1 = +1.0 - 2.0 * (x * x + y_square) + X = np.arctan2(t0, t1) + + t2 = +2.0 * (w * y - z * x) + t2 = +1.0 if t2 > +1.0 else t2 + t2 = -1.0 if t2 < -1.0 else t2 + Y = np.arcsin(t2) + + t3 = +2.0 * (w * z + x * y) + t4 = +1.0 - 2.0 * (y_square + z * z) + Z = np.arctan2(t3, t4) + + return X, Y, Z + + @staticmethod + def ssa(angle: float) -> float: + """ + Function to convert the angle to the smallest signed angle + + Args: + angle: float: angle in radians + + Returns: + angle: float: angle in radians + + """ + if angle > np.pi: + angle -= 2 * np.pi + elif angle < -np.pi: + angle += 2 * np.pi + return angle + + def saturate(self, value: float, windup: bool, limit: float) -> float: + """ + Function to saturate the value within the limits, and set the windup flag + + Args: + value: float: value to be saturated + windup: bool: windup variable + limit: float: limit for saturation + + Returns: + value: float: saturated value + + """ + if abs(value) > limit: + windup = True + value = limit * value / abs(value) + else: + windup = False + + return value + + @staticmethod + def anti_windup(K_i: float, error: float, integral_sum: float, windup: bool) -> float: + """ + Function to saturate the integral value within the limits + + Args: + value: float: value to be saturated + limit: float: limit for saturation + + Returns: + value: float: saturated value + + """ + if windup: + integral_sum += 0.0 + else: + integral_sum += K_i * error + + return integral_sum + + def calculate_coriolis_matrix(self, pitch_rate: float, yaw_rate: float, sway: float, heave: float) -> np.array: + """ + Calculates the 3x3 coriolis matrix + + Parameters: + pitch_rate: float: pitch rate in rad/s + yaw_rate: float: yaw rate in rad/s + sway: float: sway velocity in m/s + heave: float: heave velocity in m/s + + Returns: + C: np.array: 3x3 Coriolis Matrix + + """ + return np.array( + [ + [0.2, -30 * sway * 0.01, -30 * heave * 0.01], + [30 * sway * 0.01, 0, 1.629 * pitch_rate], + [30 * heave * 0.01, 1.769 * yaw_rate, 0], + ] + ) + + def calculate_lqr_u(self, C: np.array, states: np.array, guidance_values: np.array) -> np.array: + """ + Calculates the control input using the control library in python + + Parameters: + C: np.array: 3x3 Coriolis Matrix + states: np.array: 6x1 state vector + guidance_values: np.array: 6x1 guidance vector + + Returns: + u: np.array: 3x1 control input + + """ + + A = self.M_inv @ C + B = self.M_inv + + # Augment the A and B matrices for integrators for surge, pitch, and yaw + self.A_aug = np.block( + [[A, np.zeros((3, 3))], + [-np.eye(3), np.zeros((3, 3))]] + ) # Integrators added for surge, pitch, and yaw + self.B_aug = np.block( + [[B], + [np.zeros((3, 3))]] + ) # Control input does not affect integrators directly + + # CT LQR controller from control library python + K, S, E = ct.lqr( + self.A_aug, self.B_aug, self.Q, self.R + ) + + # Calculate the control input + surge_error = ( + (guidance_values[0] - states[0]) + ) # Surge error no need for angle wrapping + pitch_error = self.ssa( + guidance_values[1] - states[1] + ) # Apply ssa to pitch error + yaw_error = self.ssa( + guidance_values[2] - states[2] + ) # Apply ssa to yaw error + + # Update the integrators for surge, pitch, and yaw + self.integral_error_surge = self.anti_windup(self.I_surge, surge_error, self.integral_error_surge, self.surge_windup) + self.integral_error_pitch = self.anti_windup(self.I_pitch, pitch_error, self.integral_error_pitch, self.pitch_windup) + self.integral_error_yaw = self.anti_windup(self.I_yaw, yaw_error, self.integral_error_yaw, self.yaw_windup) + + state_error = np.array([-surge_error,-pitch_error,-yaw_error, self.integral_error_surge, self.integral_error_pitch, self.integral_error_yaw]) + + # Augmented state vector including integrators + u = -K @ state_error + + force_x = self.saturate(u[0], self.surge_windup, self.max_force) + torque_y = self.saturate(u[1], self.pitch_windup, self.max_force) + torque_z = self.saturate(u[2], self.yaw_windup, self.max_force) + + u = np.array([force_x, torque_y, torque_z]) + + return u + +# ---------------------------------------------------------------Main Function--------------------------------------------------------------- + + +# .--------------. +# .---' o . `---. +# .-' . O . . `-. +# .-' @@@@@@ . `-. +# .'@@ @@@@@@@@@@@ @@@@@@@ . `. +# .'@@@ @@@@@@@@@@@@@@ @@@@@@@@@ `. +# /@@@ o @@@@@@@@@@@@@@ @@@@@@@@@ O \ +# / @@@@@@@@@@@@@@ @ @@@@@@@@@ @@ . \ +# /@ o @@@@@@@@@@@ . @@ @@@@@@@@@@@ @@ \ +# /@@@ . @@@@@@ o @ @@@@@@@@@@@@@ o @@@@ \ +# /@@@@@ @ . @@@@@@@@@@@@@@ @@@@@ \ +# |@@@@@ O `.-./ . . @@@@@@@@@@@@@ @@@ | +# / @@@@@ --`-' o @@@@@@@@@@@ @@@ . \ +# |@ @@@@ . @ @ ` @ @@ . @@@@@@ | +# | @@ o @@ . @@@@@@ | +# | . @ @ @ o @@ o @@@@@@. | +# \ @ @ @ .-. @@@@ @@@ / +# | @ @ @ `-' . @@@@ . . | +# \ . o @ @@@@ . @@ . . / +# \ @@@ @@@@@@ . o / +# \ @@@@@ @@\@@ / O . / +# \ o @@@ \ \ / __ . . .--. / +# \ . . \.-.--- `--' / +# `. `-' . .' +# `. o / | ` O . .' +# `-. / | o .-' +# `-. . . .-' +# `---. . .---' +# `--------------' \ No newline at end of file From 4819b06896eac971212e215ade29c80946321edd Mon Sep 17 00:00:00 2001 From: Cyprian Date: Sun, 10 Nov 2024 17:31:32 +0100 Subject: [PATCH 02/17] make Andreas happy with precomitt hok --- .../config/param_velocity_controller_lqr.yaml | 22 +- .../launch/velocity_controller_lqr.launch.py | 6 +- .../scripts/velocity_controller_lqr_node.py | 124 +++++----- .../tests/test_velocity_controller_lqr.py | 64 +++--- .../velocity_controller_lqr/__init__.py | 2 +- .../velocity_controller_lqr_lib.py | 211 ++++++++++-------- 6 files changed, 234 insertions(+), 195 deletions(-) diff --git a/control/velocity_controller_lqr/config/param_velocity_controller_lqr.yaml b/control/velocity_controller_lqr/config/param_velocity_controller_lqr.yaml index ad90fc6a..6d5787b4 100644 --- a/control/velocity_controller_lqr/config/param_velocity_controller_lqr.yaml +++ b/control/velocity_controller_lqr/config/param_velocity_controller_lqr.yaml @@ -8,17 +8,17 @@ velocity_controller_lqr_node: #LQR parameters - Q_surge: 75 - Q_pitch: 175 - Q_yaw: 175 + q_surge: 75 + q_pitch: 175 + q_yaw: 175 - R_surge: 0.3 - R_pitch: 0.4 - R_yaw: 0.4 - - I_surge: 0.3 - I_pitch: 0.4 - I_yaw: 0.3 + r_surge: 0.3 + r_pitch: 0.4 + r_yaw: 0.4 + + i_surge: 0.3 + i_pitch: 0.4 + i_yaw: 0.3 #Clamp parameter - max_force: 99.5 \ No newline at end of file + max_force: 99.5 diff --git a/control/velocity_controller_lqr/launch/velocity_controller_lqr.launch.py b/control/velocity_controller_lqr/launch/velocity_controller_lqr.launch.py index 570058bd..387918f7 100644 --- a/control/velocity_controller_lqr/launch/velocity_controller_lqr.launch.py +++ b/control/velocity_controller_lqr/launch/velocity_controller_lqr.launch.py @@ -1,12 +1,12 @@ import os + from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch_ros.actions import Node def generate_launch_description() -> LaunchDescription: - """ - Generates a launch description for the velocity_controller_lqr node. + """Generates a launch description for the velocity_controller_lqr node. This function creates a ROS 2 launch description that includes the velocity_controller_lqr node. The node is configured to use the @@ -20,7 +20,7 @@ def generate_launch_description() -> LaunchDescription: parameter_file = os.path.join( get_package_share_directory("velocity_controller_lqr"), "config", - "param_velocity_controller_lqr.yaml" + "param_velocity_controller_lqr.yaml", ) # Define the node diff --git a/control/velocity_controller_lqr/scripts/velocity_controller_lqr_node.py b/control/velocity_controller_lqr/scripts/velocity_controller_lqr_node.py index bf34177e..8b85cd8f 100644 --- a/control/velocity_controller_lqr/scripts/velocity_controller_lqr_node.py +++ b/control/velocity_controller_lqr/scripts/velocity_controller_lqr_node.py @@ -4,23 +4,22 @@ from geometry_msgs.msg import Wrench from nav_msgs.msg import Odometry from rclpy.node import Node -from std_msgs.msg import Float32MultiArray -from velocity_controller_lqr.velocity_controller_lqr_lib import LQR_controller +from velocity_controller_lqr.velocity_controller_lqr_lib import LQRController from vortex_msgs.msg import LOSGuidance -class VelocityLQRNode(Node): +class LinearQuadraticRegulator(Node): def __init__(self): super().__init__("velocity_controller_lqr_node") - + self.declare_parameter("odom_path", "/nucleus/odom") self.declare_parameter("guidance_path", "/guidance/los") self.declare_parameter("thrust_path", "/thrust/wrench_input") - + odom_path = self.get_parameter("odom_path").value guidance_path = self.get_parameter("guidance_path").value thrust_path = self.get_parameter("thrust_path").value - + # SUBSRCIBERS ----------------------------------- self.nucleus_subscriber = self.create_subscription( Odometry, odom_path, self.nucleus_callback, 20 @@ -28,59 +27,69 @@ def __init__(self): self.guidance_subscriber = self.create_subscription( LOSGuidance, guidance_path, self.guidance_callback, 20 ) - + # PUBLISHERS ------------------------------------ - self.publisherLQR = self.create_publisher( - Wrench, thrust_path, 10 - ) + self.publisherLQR = self.create_publisher(Wrench, thrust_path, 10) # TIMERS ---------------------------------------- - self.control_timer = self.create_timer( - 0.1, self.controller - ) - self.state_timer = self.create_timer( - 0.1, self.publish_states - ) - + self.control_timer = self.create_timer(0.1, self.controller) + # ROS2 SHENNANIGANS with parameters self.declare_parameter("max_force", 99.5) max_force = self.get_parameter("max_force").value - - self.declare_parameter("Q_surge", 75) - self.declare_parameter("Q_pitch", 175) - self.declare_parameter("Q_yaw", 175) - - self.declare_parameter("R_surge", 0.3) - self.declare_parameter("R_pitch", 0.4) - self.declare_parameter("R_yaw", 0.4) - - self.declare_parameter("I_surge", 0.3) - self.declare_parameter("I_pitch", 0.4) - self.declare_parameter("I_yaw", 0.3) - - Q_surge = self.get_parameter("Q_surge").value - Q_pitch = self.get_parameter("Q_pitch").value - Q_yaw = self.get_parameter("Q_yaw").value - - R_surge = self.get_parameter("R_surge").value - R_pitch = self.get_parameter("R_pitch").value - R_yaw = self.get_parameter("R_yaw").value - - I_surge = self.get_parameter("I_surge").value - I_pitch = self.get_parameter("I_pitch").value - I_yaw = self.get_parameter("I_yaw").value - - self.controller = LQR_controller(Q_surge, Q_pitch, Q_yaw, R_surge, R_pitch, R_yaw, I_surge, I_pitch, I_yaw, max_force) - + + self.declare_parameter("q_surge", 75) + self.declare_parameter("q_pitch", 175) + self.declare_parameter("q_yaw", 175) + + self.declare_parameter("r_surge", 0.3) + self.declare_parameter("r_pitch", 0.4) + self.declare_parameter("r_yaw", 0.4) + + self.declare_parameter("i_surge", 0.3) + self.declare_parameter("i_pitch", 0.4) + self.declare_parameter("i_yaw", 0.3) + + q_surge = self.get_parameter("q_surge").value + q_pitch = self.get_parameter("q_pitch").value + q_yaw = self.get_parameter("q_yaw").value + + r_surge = self.get_parameter("r_surge").value + r_pitch = self.get_parameter("r_pitch").value + r_yaw = self.get_parameter("r_yaw").value + + i_surge = self.get_parameter("i_surge").value + i_pitch = self.get_parameter("i_pitch").value + i_yaw = self.get_parameter("i_yaw").value + + self.controller = LQRController( + q_surge, + q_pitch, + q_yaw, + r_surge, + r_pitch, + r_yaw, + i_surge, + i_pitch, + i_yaw, + max_force, + ) + # Only keeping variables that need to be updated inside of a callback self.C = np.zeros((3, 3)) # Initialize Coriolis matrix - self.states = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) # State vector 1. surge, 2. pitch, 3. yaw, 4. integral_surge, 5. integral_pitch, 6. integral_yaw - self.guidance_values = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) # Array to store guidance values + self.states = np.array( + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + ) # State vector 1. surge, 2. pitch, 3. yaw, 4. integral_surge, 5. integral_pitch, 6. integral_yaw + self.guidance_values = np.array( + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + ) # Array to store guidance values # ---------------------------------------------------------------Callback Functions--------------------------------------------------------------- - def nucleus_callback(self, msg: Odometry): # callback function to read data from nucleus - dummy, self.states[1], self.states[2] = LQR_controller.quaternion_to_euler_angle( + def nucleus_callback( + self, msg: Odometry + ): # callback function to read data from nucleus + dummy, self.states[1], self.states[2] = LQRController.quaternion_to_euler_angle( msg.pose.pose.orientation.w, msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, @@ -107,26 +116,31 @@ def guidance_callback( def controller(self): # The LQR controller function msg = Wrench() - + u = self.controller.calculate_lqr_u(self.C, self.states, self.guidance_values) msg.force.x = u[0] msg.torque.y = u[1] msg.torque.z = u[2] - - self.get_logger().info(f"Input values: {msg.force.x}, {msg.torque.y}, {msg.torque.z}") - + + self.get_logger().info( + f"Input values: {msg.force.x}, {msg.torque.y}, {msg.torque.z}" + ) + # Publish the control input self.publisherLQR.publish(msg) + # ---------------------------------------------------------------Main Function--------------------------------------------------------------- + def main(args=None): # main function rclpy.init(args=args) - node = VelocityLQRNode() + node = LinearQuadraticRegulator() rclpy.spin(node) rclpy.shutdown() + if __name__ == "__main__": main() - -# Anders er goated \ No newline at end of file + +# Anders er goated diff --git a/control/velocity_controller_lqr/tests/test_velocity_controller_lqr.py b/control/velocity_controller_lqr/tests/test_velocity_controller_lqr.py index 8698680c..62cb0081 100644 --- a/control/velocity_controller_lqr/tests/test_velocity_controller_lqr.py +++ b/control/velocity_controller_lqr/tests/test_velocity_controller_lqr.py @@ -1,47 +1,44 @@ import numpy as np -import pytest -import rclpy -from velocity_controller_lqr.velocity_controller_lqr import LQR_controller +from velocity_controller_lqr.velocity_controller_lqr_lib import LQRController + +controller = LQRController(0, 0, 0, 0, 0, 0, 0, 0, 0, 0) -controller = LQR_controller(0, 0, 0, 0, 0, 0, 0, 0, 0, 0) class TestVelocityController: - - def test_placeholder(self): - assert controller is not None # Simple test to ensure the controller initializes - - def test_ssa(self,capsys): + assert ( + controller is not None + ) # Simple test to ensure the controller initializes + + def test_ssa(self): print("Commencing ssa test: \n") - - assert controller.ssa(np.pi + 1) == -np.pi + 1 - - assert controller.ssa(-np.pi - 1) == (np.pi - 1) - + + assert LQRController.ssa(np.pi + 1) == -np.pi + 1 + + assert LQRController.ssa(-np.pi - 1) == (np.pi - 1) + print("SSA test passed") - - captured = capsys.readouterr() - + def test_quaternion_to_euler_angle(self): print("Commencing quaternion to euler angle test: \n") - - roll, pitch, yaw = controller.quaternion_to_euler_angle(0.5, 0.5, 0.5, 0.5) + + roll, pitch, yaw = LQRController.quaternion_to_euler_angle(0.5, 0.5, 0.5, 0.5) assert roll == np.pi / 2 assert pitch == 0 assert yaw == np.pi / 2 - + print("Quaternion to euler angle test passed") - + def test_saturate(self): - print("Commencing saturate test: \n") - - windup = False - assert controller.saturate(10, windup, 5) == 5 - + + self.windup = False + assert controller.saturate(10, self.windup, 5) == 5 + assert self.windup == True + windup = False assert controller.saturate(-10, False, 5) == -5 - + windup = True assert controller.saturate(10, False, 15) == 10 @@ -49,19 +46,18 @@ def test_saturate(self): assert controller.saturate(-10, False, 15) == -10 print("Saturate test passed") - + def test_anti_windup(self): - print("Commencing anti windup test: \n") - + windup = True assert controller.anti_windup(10, 5, 10, windup) == 10 - + windup = False assert controller.anti_windup(1, 5, 10, windup) == 15 - + print("Anti windup test passed") - + def test_final(self): print("¯\_(ツ)_/¯ ehh good enough pass anyway") - pass \ No newline at end of file + pass diff --git a/control/velocity_controller_lqr/velocity_controller_lqr/__init__.py b/control/velocity_controller_lqr/velocity_controller_lqr/__init__.py index 79b530b8..08509a28 100644 --- a/control/velocity_controller_lqr/velocity_controller_lqr/__init__.py +++ b/control/velocity_controller_lqr/velocity_controller_lqr/__init__.py @@ -1 +1 @@ -from velocity_controller_lqr.velocity_controller_lqr_lib import LQR_controller \ No newline at end of file +from velocity_controller_lqr.velocity_controller_lqr_lib import LQRController diff --git a/control/velocity_controller_lqr/velocity_controller_lqr/velocity_controller_lqr_lib.py b/control/velocity_controller_lqr/velocity_controller_lqr/velocity_controller_lqr_lib.py index 038dc72c..08870c69 100644 --- a/control/velocity_controller_lqr/velocity_controller_lqr/velocity_controller_lqr_lib.py +++ b/control/velocity_controller_lqr/velocity_controller_lqr/velocity_controller_lqr_lib.py @@ -5,9 +5,20 @@ import numpy as np -class LQR_controller(): - def __init__(self, Q_surge, Q_pitch, Q_yaw, R_surge, R_pitch, R_yaw, I_surge, I_pitch, I_yaw, max_force): - +class LQRController: + def __init__( + self, + q_surge, + q_pitch, + q_yaw, + r_surge, + r_pitch, + r_yaw, + i_surge, + i_pitch, + i_yaw, + max_force, + ): self.integral_error_surge = 0.0 self.integral_error_pitch = 0.0 self.integral_error_yaw = 0.0 @@ -15,40 +26,39 @@ def __init__(self, Q_surge, Q_pitch, Q_yaw, R_surge, R_pitch, R_yaw, I_surge, I_ self.surge_windup = False # Windup variable self.pitch_windup = False # Windup variable self.yaw_windup = False # Windup variable - + self.max_force = max_force # Maximum force that can be applied - - self.I_surge = I_surge # Integral gain for surge - self.I_pitch = I_pitch # Integral gain for pitch - self.I_yaw = I_yaw # Integral gain for yaw - - self.Q_surge = Q_surge # Surge cost - self.Q_pitch = Q_pitch # Pitch cost - self.Q_yaw = Q_yaw # Yaw cost - - self.R_surge = R_surge # Surge control cost - self.R_pitch = R_pitch # Pitch control cost - self.R_yaw = R_yaw # Yaw control cost - + + self.i_surge = i_surge # Integral gain for surge + self.i_pitch = i_pitch # Integral gain for pitch + self.i_yaw = i_yaw # Integral gain for yaw + + self.q_surge = q_surge # Surge cost + self.q_pitch = q_pitch # Pitch cost + self.q_yaw = q_yaw # Yaw cost + + self.r_surge = r_surge # Surge control cost + self.r_pitch = r_pitch # Pitch control cost + self.r_yaw = r_yaw # Yaw control cost + # VARIABLES - self.M_inv = np.linalg.inv(np.array( - [[30, 0.6, 0], - [0.6, 1.629, 0], - [0, 0, 1.769]] - )) # mass matrix - - self.Q = np.block( + self.inertia_matrix_inv = np.linalg.inv( + np.array([[30, 0.6, 0], [0.6, 1.629, 0], [0, 0, 1.769]]) + ) # mass matrix + + self.state_weight_matrix = np.block( [ - [np.diag([self.Q_surge, self.Q_pitch, self.Q_yaw]), np.zeros((3, 3))], - [np.zeros((3, 3)), np.diag([0.5, 0.5, 0.5])] + [np.diag([self.q_surge, self.q_pitch, self.q_yaw]), np.zeros((3, 3))], + [np.zeros((3, 3)), np.diag([0.5, 0.5, 0.5])], ] ) # Augmented state cost matrix - self.R = np.diag([self.R_surge, self.R_pitch, self.R_yaw]) # control cost matrix - + self.input_weight_matrix = np.diag( + [self.r_surge, self.r_pitch, self.r_yaw] + ) # control cost matrix + @staticmethod def quaternion_to_euler_angle(w: float, x: float, y: float, z: float) -> tuple: - """ - Function to convert quaternion to euler angles. + """Function to convert quaternion to euler angles. Args: w: float: w component of quaternion @@ -66,23 +76,22 @@ def quaternion_to_euler_angle(w: float, x: float, y: float, z: float) -> tuple: t0 = +2.0 * (w * x + y * z) t1 = +1.0 - 2.0 * (x * x + y_square) - X = np.arctan2(t0, t1) + phi = np.arctan2(t0, t1) t2 = +2.0 * (w * y - z * x) t2 = +1.0 if t2 > +1.0 else t2 t2 = -1.0 if t2 < -1.0 else t2 - Y = np.arcsin(t2) + theta = np.arcsin(t2) t3 = +2.0 * (w * z + x * y) t4 = +1.0 - 2.0 * (y_square + z * z) - Z = np.arctan2(t3, t4) + psi = np.arctan2(t3, t4) - return X, Y, Z + return phi, theta, psi @staticmethod def ssa(angle: float) -> float: - """ - Function to convert the angle to the smallest signed angle + """Function to convert the angle to the smallest signed angle. Args: angle: float: angle in radians @@ -96,10 +105,9 @@ def ssa(angle: float) -> float: elif angle < -np.pi: angle += 2 * np.pi return angle - + def saturate(self, value: float, windup: bool, limit: float) -> float: - """ - Function to saturate the value within the limits, and set the windup flag + """Function to saturate the value within the limits, and set the windup flag. Args: value: float: value to be saturated @@ -115,17 +123,22 @@ def saturate(self, value: float, windup: bool, limit: float) -> float: value = limit * value / abs(value) else: windup = False - - return value + + return value, windup @staticmethod - def anti_windup(K_i: float, error: float, integral_sum: float, windup: bool) -> float: - """ - Function to saturate the integral value within the limits + def anti_windup( + k_i: float, error: float, integral_sum: float, windup: bool + ) -> float: + """Function to saturate the integral value within the limits. Args: - value: float: value to be saturated - limit: float: limit for saturation + k_i : float: integral gain + error: float: error value + integral_sum: float: integral sum + windup: bool: windup variable + + Returns: value: float: saturated value @@ -134,14 +147,15 @@ def anti_windup(K_i: float, error: float, integral_sum: float, windup: bool) -> if windup: integral_sum += 0.0 else: - integral_sum += K_i * error - + integral_sum += k_i * error + return integral_sum - def calculate_coriolis_matrix(self, pitch_rate: float, yaw_rate: float, sway: float, heave: float) -> np.array: - """ - Calculates the 3x3 coriolis matrix - + def calculate_coriolis_matrix( + self, pitch_rate: float, yaw_rate: float, sway: float, heave: float + ) -> np.array: + """Calculates the 3x3 coriolis matrix. + Parameters: pitch_rate: float: pitch rate in rad/s yaw_rate: float: yaw rate in rad/s @@ -150,7 +164,7 @@ def calculate_coriolis_matrix(self, pitch_rate: float, yaw_rate: float, sway: fl Returns: C: np.array: 3x3 Coriolis Matrix - + """ return np.array( [ @@ -159,71 +173,86 @@ def calculate_coriolis_matrix(self, pitch_rate: float, yaw_rate: float, sway: fl [30 * heave * 0.01, 1.769 * yaw_rate, 0], ] ) - - def calculate_lqr_u(self, C: np.array, states: np.array, guidance_values: np.array) -> np.array: - """ - Calculates the control input using the control library in python - + + def calculate_lqr_u( + self, coriolis_matrix: np.array, states: np.array, guidance_values: np.array + ) -> np.array: + """Calculates the control input using the control library in python. + Parameters: C: np.array: 3x3 Coriolis Matrix states: np.array: 6x1 state vector guidance_values: np.array: 6x1 guidance vector - + Returns: u: np.array: 3x1 control input - + """ - - A = self.M_inv @ C - B = self.M_inv + system_matrix = self.inertia_matrix_inv @ coriolis_matrix + input_matrix = self.inertia_matrix_inv # Augment the A and B matrices for integrators for surge, pitch, and yaw - self.A_aug = np.block( - [[A, np.zeros((3, 3))], - [-np.eye(3), np.zeros((3, 3))]] + self.augmented_system_matrix = np.block( + [[system_matrix, np.zeros((3, 3))], [-np.eye(3), np.zeros((3, 3))]] ) # Integrators added for surge, pitch, and yaw - self.B_aug = np.block( - [[B], - [np.zeros((3, 3))]] + self.augmented_input_matrix = np.block( + [[input_matrix], [np.zeros((3, 3))]] ) # Control input does not affect integrators directly # CT LQR controller from control library python - K, S, E = ct.lqr( - self.A_aug, self.B_aug, self.Q, self.R + lqr_gain, ricatti_s, ricatti_e = ct.lqr( + self.augmented_system_matrix, + self.augmented_input_matrix, + self.state_weight_matrix, + self.input_weight_matrix, ) - + # Calculate the control input surge_error = ( - (guidance_values[0] - states[0]) + guidance_values[0] - states[0] ) # Surge error no need for angle wrapping pitch_error = self.ssa( guidance_values[1] - states[1] ) # Apply ssa to pitch error - yaw_error = self.ssa( - guidance_values[2] - states[2] - ) # Apply ssa to yaw error + yaw_error = self.ssa(guidance_values[2] - states[2]) # Apply ssa to yaw error # Update the integrators for surge, pitch, and yaw - self.integral_error_surge = self.anti_windup(self.I_surge, surge_error, self.integral_error_surge, self.surge_windup) - self.integral_error_pitch = self.anti_windup(self.I_pitch, pitch_error, self.integral_error_pitch, self.pitch_windup) - self.integral_error_yaw = self.anti_windup(self.I_yaw, yaw_error, self.integral_error_yaw, self.yaw_windup) - - state_error = np.array([-surge_error,-pitch_error,-yaw_error, self.integral_error_surge, self.integral_error_pitch, self.integral_error_yaw]) + self.integral_error_surge = self.anti_windup( + self.i_surge, surge_error, self.integral_error_surge, self.surge_windup + ) + self.integral_error_pitch = self.anti_windup( + self.i_pitch, pitch_error, self.integral_error_pitch, self.pitch_windup + ) + self.integral_error_yaw = self.anti_windup( + self.i_yaw, yaw_error, self.integral_error_yaw, self.yaw_windup + ) + + state_error = np.array( + [ + -surge_error, + -pitch_error, + -yaw_error, + self.integral_error_surge, + self.integral_error_pitch, + self.integral_error_yaw, + ] + ) # Augmented state vector including integrators - u = -K @ state_error - - force_x = self.saturate(u[0], self.surge_windup, self.max_force) - torque_y = self.saturate(u[1], self.pitch_windup, self.max_force) - torque_z = self.saturate(u[2], self.yaw_windup, self.max_force) - + u = -lqr_gain @ state_error + + self.surge_windup, force_x = self.saturate(u[0], self.surge_windup, self.max_force) + self.pitch_windup, torque_y = self.saturate(u[1], self.pitch_windup, self.max_force) + self.yaw_windup, torque_z = self.saturate(u[2], self.yaw_windup, self.max_force) + u = np.array([force_x, torque_y, torque_z]) - + return u - + + # ---------------------------------------------------------------Main Function--------------------------------------------------------------- - + # .--------------. # .---' o . `---. # .-' . O . . `-. @@ -252,4 +281,4 @@ def calculate_lqr_u(self, C: np.array, states: np.array, guidance_values: np.arr # `-. / | o .-' # `-. . . .-' # `---. . .---' -# `--------------' \ No newline at end of file +# `--------------' From d7ccc7984aa0a575206ee28e8749cc1cdf02ea70 Mon Sep 17 00:00:00 2001 From: Cyprian Date: Sun, 10 Nov 2024 17:56:53 +0100 Subject: [PATCH 03/17] Your pipeline is green --- .../tests/test_velocity_controller_lqr.py | 31 ++++++++++++------- .../velocity_controller_lqr/__init__.py | 1 - .../velocity_controller_lqr_lib.py | 11 +++++-- 3 files changed, 27 insertions(+), 16 deletions(-) diff --git a/control/velocity_controller_lqr/tests/test_velocity_controller_lqr.py b/control/velocity_controller_lqr/tests/test_velocity_controller_lqr.py index 62cb0081..511d6d22 100644 --- a/control/velocity_controller_lqr/tests/test_velocity_controller_lqr.py +++ b/control/velocity_controller_lqr/tests/test_velocity_controller_lqr.py @@ -32,18 +32,25 @@ def test_quaternion_to_euler_angle(self): def test_saturate(self): print("Commencing saturate test: \n") - self.windup = False - assert controller.saturate(10, self.windup, 5) == 5 - assert self.windup == True - - windup = False - assert controller.saturate(-10, False, 5) == -5 - - windup = True - assert controller.saturate(10, False, 15) == 10 - - windup = True - assert controller.saturate(-10, False, 15) == -10 + # Test case 1: Saturation occurs, so windup should be True + saturated_value, windup = controller.saturate(10, False, 5) + assert saturated_value == 5.0 + assert windup == True + + # Test case 2: Saturation occurs with negative limit, so windup should be True + saturated_value, windup = controller.saturate(-10, False, 5) + assert saturated_value == -5.0 + assert windup == True + + # Test case 3: No saturation, so windup should be False + saturated_value, windup = controller.saturate(3, True, 5) + assert saturated_value == 3.0 + assert windup == False + + # Test case 4: No saturation with negative value, so windup should be False + saturated_value, windup = controller.saturate(-3, True, 5) + assert saturated_value == -3.0 + assert windup == False print("Saturate test passed") diff --git a/control/velocity_controller_lqr/velocity_controller_lqr/__init__.py b/control/velocity_controller_lqr/velocity_controller_lqr/__init__.py index 08509a28..e69de29b 100644 --- a/control/velocity_controller_lqr/velocity_controller_lqr/__init__.py +++ b/control/velocity_controller_lqr/velocity_controller_lqr/__init__.py @@ -1 +0,0 @@ -from velocity_controller_lqr.velocity_controller_lqr_lib import LQRController diff --git a/control/velocity_controller_lqr/velocity_controller_lqr/velocity_controller_lqr_lib.py b/control/velocity_controller_lqr/velocity_controller_lqr/velocity_controller_lqr_lib.py index 08870c69..76ee15ed 100644 --- a/control/velocity_controller_lqr/velocity_controller_lqr/velocity_controller_lqr_lib.py +++ b/control/velocity_controller_lqr/velocity_controller_lqr/velocity_controller_lqr_lib.py @@ -1,9 +1,10 @@ #!/usr/bin/env python3 import math as math -import control as ct import numpy as np +import control as ct + class LQRController: def __init__( @@ -241,8 +242,12 @@ def calculate_lqr_u( # Augmented state vector including integrators u = -lqr_gain @ state_error - self.surge_windup, force_x = self.saturate(u[0], self.surge_windup, self.max_force) - self.pitch_windup, torque_y = self.saturate(u[1], self.pitch_windup, self.max_force) + self.surge_windup, force_x = self.saturate( + u[0], self.surge_windup, self.max_force + ) + self.pitch_windup, torque_y = self.saturate( + u[1], self.pitch_windup, self.max_force + ) self.yaw_windup, torque_z = self.saturate(u[2], self.yaw_windup, self.max_force) u = np.array([force_x, torque_y, torque_z]) From 9a171f18ffab29b4aac4b2beb737459f4f5d7ed0 Mon Sep 17 00:00:00 2001 From: Cyprian Date: Sun, 10 Nov 2024 18:50:50 +0100 Subject: [PATCH 04/17] refactor(velocity_controller): removed unnecessary dependency --- control/velocity_controller_lqr/package.xml | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/control/velocity_controller_lqr/package.xml b/control/velocity_controller_lqr/package.xml index ed5074d0..8562c7df 100644 --- a/control/velocity_controller_lqr/package.xml +++ b/control/velocity_controller_lqr/package.xml @@ -13,9 +13,11 @@ rclpy geometry_msgs nav_msgs - python3-control vortex_msgs + control + + ament_lint_auto ament_lint_common ament_cmake_pytest From 8f578c5eb73e8c12b2a9ec888cdd5c61beb9b71d Mon Sep 17 00:00:00 2001 From: Cyprian Date: Fri, 15 Nov 2024 15:38:03 +0100 Subject: [PATCH 05/17] : pull request v1.0 comitted --- .github/workflows/source-build.yaml | 1 + .../velocity_controller_lqr/CMakeLists.txt | 5 - .../config/param_velocity_controller_lqr.yaml | 13 +- .../launch/velocity_controller_lqr.launch.py | 2 +- control/velocity_controller_lqr/package.xml | 5 +- .../scripts/velocity_controller_lqr_node.py | 119 ++++++++++++------ .../velocity_controller_lqr_lib.py | 53 ++++---- requirements.sh | 20 +++ 8 files changed, 136 insertions(+), 82 deletions(-) create mode 100644 requirements.sh diff --git a/.github/workflows/source-build.yaml b/.github/workflows/source-build.yaml index 802f5363..8c59afad 100644 --- a/.github/workflows/source-build.yaml +++ b/.github/workflows/source-build.yaml @@ -14,3 +14,4 @@ jobs: os_name: 'ubuntu-22.04' ref: ${{ github.ref_name }} vcs_repo_file_url: 'https://raw.githubusercontent.com/vortexntnu/vortex-auv/main/ros2.repos' + download_requirements: true diff --git a/control/velocity_controller_lqr/CMakeLists.txt b/control/velocity_controller_lqr/CMakeLists.txt index 59fef0e8..a12bc847 100644 --- a/control/velocity_controller_lqr/CMakeLists.txt +++ b/control/velocity_controller_lqr/CMakeLists.txt @@ -1,12 +1,7 @@ cmake_minimum_required(VERSION 3.8) project(velocity_controller_lqr) -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - # find dependencies -find_package(ament_cmake REQUIRED) find_package(ament_cmake_python REQUIRED) find_package(rclpy REQUIRED) find_package(nav_msgs REQUIRED) diff --git a/control/velocity_controller_lqr/config/param_velocity_controller_lqr.yaml b/control/velocity_controller_lqr/config/param_velocity_controller_lqr.yaml index 6d5787b4..98711ab9 100644 --- a/control/velocity_controller_lqr/config/param_velocity_controller_lqr.yaml +++ b/control/velocity_controller_lqr/config/param_velocity_controller_lqr.yaml @@ -1,10 +1,10 @@ velocity_controller_lqr_node: ros__parameters: - #Path parameters - odom_path: /nucleus/odom - guidance_path: /guidance/los - thrust_path: /thrust/wrench_input + #topic parameters + odom_topic: /nucleus/odom + guidance_topic: /guidance/los + thrust_topic: /thrust/wrench_input #LQR parameters @@ -20,5 +20,10 @@ velocity_controller_lqr_node: i_pitch: 0.4 i_yaw: 0.3 + i_weight: 0.5 + + #Inertia matrix + inertia_matrix: [30.0, 0.6, 0.0, 0.6, 1.629, 0.0, 0.0, 0.0, 1.729] + #Clamp parameter max_force: 99.5 diff --git a/control/velocity_controller_lqr/launch/velocity_controller_lqr.launch.py b/control/velocity_controller_lqr/launch/velocity_controller_lqr.launch.py index 387918f7..c35cb141 100644 --- a/control/velocity_controller_lqr/launch/velocity_controller_lqr.launch.py +++ b/control/velocity_controller_lqr/launch/velocity_controller_lqr.launch.py @@ -26,7 +26,7 @@ def generate_launch_description() -> LaunchDescription: # Define the node velocity_controller_node = Node( package="velocity_controller_lqr", - executable="velocity_controller_lqr_node.py", # Ensure this matches your Python file name + executable="velocity_controller_lqr_node.py", name="velocity_controller_lqr_node", output="screen", parameters=[parameter_file], diff --git a/control/velocity_controller_lqr/package.xml b/control/velocity_controller_lqr/package.xml index 8562c7df..02080efd 100644 --- a/control/velocity_controller_lqr/package.xml +++ b/control/velocity_controller_lqr/package.xml @@ -9,14 +9,11 @@ ament_cmake - rclcpp rclpy geometry_msgs nav_msgs vortex_msgs - - control - + python-control-pip ament_lint_auto ament_lint_common diff --git a/control/velocity_controller_lqr/scripts/velocity_controller_lqr_node.py b/control/velocity_controller_lqr/scripts/velocity_controller_lqr_node.py index 8b85cd8f..7744b2e9 100644 --- a/control/velocity_controller_lqr/scripts/velocity_controller_lqr_node.py +++ b/control/velocity_controller_lqr/scripts/velocity_controller_lqr_node.py @@ -12,13 +12,13 @@ class LinearQuadraticRegulator(Node): def __init__(self): super().__init__("velocity_controller_lqr_node") - self.declare_parameter("odom_path", "/nucleus/odom") - self.declare_parameter("guidance_path", "/guidance/los") - self.declare_parameter("thrust_path", "/thrust/wrench_input") + self.declare_parameter("odom_topic", "/nucleus/odom") + self.declare_parameter("guidance_topic", "/guidance/los") + self.declare_parameter("thrust_topic", "/thrust/wrench_input") - odom_path = self.get_parameter("odom_path").value - guidance_path = self.get_parameter("guidance_path").value - thrust_path = self.get_parameter("thrust_path").value + odom_path = self.get_parameter("odom_topic").value + guidance_path = self.get_parameter("guidance_topic").value + thrust_path = self.get_parameter("thrust_topic").value # SUBSRCIBERS ----------------------------------- self.nucleus_subscriber = self.create_subscription( @@ -32,12 +32,22 @@ def __init__(self): self.publisherLQR = self.create_publisher(Wrench, thrust_path, 10) # TIMERS ---------------------------------------- - self.control_timer = self.create_timer(0.1, self.controller) + self.control_timer = self.create_timer(0.1, self.control_loop) # ROS2 SHENNANIGANS with parameters - self.declare_parameter("max_force", 99.5) - max_force = self.get_parameter("max_force").value + parameters, inertia_matrix = self.get_parameters() + self.controller = LQRController(parameters, inertia_matrix) + # Only keeping variables that need to be updated inside of a callback + self.coriolis_matrix = np.zeros((3, 3)) # Initialize Coriolis matrix + self.states = np.array( + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + ) # State vector 1. surge, 2. pitch, 3. yaw, 4. integral_surge, 5. integral_pitch, 6. integral_yaw + self.guidance_values = np.array( + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + ) # Array to store guidance values + + def get_parameters(self): self.declare_parameter("q_surge", 75) self.declare_parameter("q_pitch", 175) self.declare_parameter("q_yaw", 175) @@ -50,6 +60,13 @@ def __init__(self): self.declare_parameter("i_pitch", 0.4) self.declare_parameter("i_yaw", 0.3) + self.declare_parameter("i_weight", 0.5) + + self.declare_parameter("max_force", 99.5) + self.declare_parameter( + "inertia_matrix", [30.0, 0.6, 0.0, 0.6, 1.629, 0.0, 0.0, 0.0, 1.729] + ) + q_surge = self.get_parameter("q_surge").value q_pitch = self.get_parameter("q_pitch").value q_yaw = self.get_parameter("q_yaw").value @@ -62,27 +79,33 @@ def __init__(self): i_pitch = self.get_parameter("i_pitch").value i_yaw = self.get_parameter("i_yaw").value - self.controller = LQRController( - q_surge, - q_pitch, - q_yaw, - r_surge, - r_pitch, - r_yaw, - i_surge, - i_pitch, - i_yaw, - max_force, + i_weight = self.get_parameter("i_weight").value + max_force = self.get_parameter("max_force").value + + inertia_matrix_flat = self.get_parameter("inertia_matrix").value + inertia_matrix = np.array(inertia_matrix_flat).reshape((3, 3)) + + parameters = np.array( + [ + q_surge, + q_pitch, + q_yaw, + r_surge, + r_pitch, + r_yaw, + i_surge, + i_pitch, + i_yaw, + i_weight, + max_force, + ] ) - # Only keeping variables that need to be updated inside of a callback - self.C = np.zeros((3, 3)) # Initialize Coriolis matrix - self.states = np.array( - [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - ) # State vector 1. surge, 2. pitch, 3. yaw, 4. integral_surge, 5. integral_pitch, 6. integral_yaw - self.guidance_values = np.array( - [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - ) # Array to store guidance values + for i in range(len(parameters - 2)): + assert parameters[i] > 0, f"Parameter {i} is negative" + assert 0 <= parameters[-1] <= 99.9, "Max force must be in the range [0, 99.9]." + + return parameters, inertia_matrix # ---------------------------------------------------------------Callback Functions--------------------------------------------------------------- @@ -98,7 +121,7 @@ def nucleus_callback( self.states[0] = msg.twist.twist.linear.x - self.C = self.controller.calculate_coriolis_matrix( + self.coriolis_matrix = self.controller.calculate_coriolis_matrix( msg.twist.twist.angular.y, msg.twist.twist.angular.z, msg.twist.twist.linear.y, @@ -114,17 +137,15 @@ def guidance_callback( # ---------------------------------------------------------------Publisher Functions--------------------------------------------------------------- - def controller(self): # The LQR controller function + def control_loop(self): # The LQR controller function msg = Wrench() - u = self.controller.calculate_lqr_u(self.C, self.states, self.guidance_values) - msg.force.x = u[0] - msg.torque.y = u[1] - msg.torque.z = u[2] - - self.get_logger().info( - f"Input values: {msg.force.x}, {msg.torque.y}, {msg.torque.z}" + u = self.controller.calculate_lqr_u( + self.coriolis_matrix, self.states, self.guidance_values ) + msg.force.x = float(u[0]) + msg.torque.y = float(u[1]) + msg.torque.z = float(u[2]) # Publish the control input self.publisherLQR.publish(msg) @@ -143,4 +164,26 @@ def main(args=None): # main function if __name__ == "__main__": main() -# Anders er goated +# ⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀ +# ⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⣀⣠⢴⣤⣠⣤⢤⣂⣔⣲⣒⣖⡺⢯⣝⡿⣿⣿⣿⣷⣶⣶⣢⢦⣤⣄⣀⣀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀ +# ⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣰⣯⣿⣾⣿⣶⣺⣯⡿⣓⣽⢞⡸⣻⢏⣋⠌⣛⣭⣿⢟⣿⣛⣿⢷⣿⣿⣿⡟⣿⣻⣵⣲⢢⣄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀ +# ⠀⠀⠀⠀⠀⠀⢀⣀⣤⡴⠲⠶⢦⠤⢤⡤⠿⠿⠿⠿⣿⣽⣿⣽⣷⣿⢽⣾⣷⣭⡞⣩⡐⠏⡋⣽⡬⣭⠏⢍⣞⢿⣽⣿⣷⢿⣿⣿⡿⠾⣿⢶⡶⣤⣀⠀⠀⠀⠀⠀⠀⠀⠀ +# ⠀⠀⠀⠀⣤⣖⠯⡙⢢⡁⠄⢢⡤⢠⢀⣸⠀⣄⡠⣀⣀⣦⡄⣠⢀⡃⠽⣽⠬⠍⣿⣿⣤⣥⣷⣿⣿⣿⣾⡍⠛⢌⠈⢫⣍⢋⣍⡁⠹⢍⠈⣳⢎⠴⠟⠻⢧⣄⠀⠀⠀⠀⠀ +# ⠀⠀⣠⣾⣿⣿⣿⣯⡔⠆⠠⠈⣿⣿⠾⡾⠿⣶⠿⡟⣻⡛⣭⢙⠍⢩ANDERS ER GOATED⣤⣥⣩⣶⣟⣻⠧⣻⠶⢤⢰⡱⣬⣤⣌⣑⠞⠲⠓⠭⡀⠀⠀⠀ +# ⠀⠐⣿⣿⣿⢟⡋⢈⢤⣤⣷⢿⣿⠒⢜⣁⡱⡧⢿⣹⣷⣿⡿⣷⠌⣢⣟⢱⢽⡨⢊⠴⡉⢉⡿⣯⢿⣏⢹⠏⣯⣩⣙⠾⢿⣳⣶⢻⣟⣿⠧⢀⠋⠟⣿⡧⠠⠄⡤⠈⢠⠀⠀ +# ⠀⠀⠘⠻⠿⠶⠶⠿⠛⣹⣟⡞⠸⣬⠐⠙⢍⢉⣔⠪⠟⡛⠫⢵⣾⣣⣼⣽⢈⠔⡅⣜⡽⢯⢞⡕⡠⠓⣡⢚⣷⣷⣿⣳⡄⠢⠉⠛⢿⣲⢿⣶⢿⣬⣾⣛⠳⣼⡮⠳⡂⠒⠀ +# ⠀⠀⠀⠀⠀⠀⠀⠀⢠⠏⡁⢉⣀⣑⣆⡐⠊⣅⡕⢦⣀⣱⡶⢫⣨⢟⠽⠕⣇⢶⣵⣋⢝⣉⣋⠜⠉⠉⡯⠛⣿⣿⣿⣾⣳⠠⠤⠪⠁⠊⠉⠻⣟⣾⣿⣿⣟⣧⣧⢸⠂⠠⢠ +# ⠀⠀⠀⠀⠀⠀⠀⣠⣾⢞⢉⠿⢿⣟⡓⠖⢾⡏⢠⣾⣿⠛⣰⠾⢓⡵⣺⢺⣼⡫⢪⡱⣉⠷⢗⡀⠤⠆⡻⣛⠿⣻⣿⢶⣊⡄⠀⠀⠀⠀⠄⢀⠀⠉⠿⣿⡿⣿⣛⡁⢍⣀⡌ +# ⠀⠀⠀⠀⠀⠀⣠⣛⢓⠗⠀⠀⠠⣈⠉⢀⢬⣵⡿⢋⣴⣞⣵⣼⣭⢁⠕⢿⢋⣞⢟⣕⡩⣔⠃⠀⠀⡀⣛⣤⢿⣷⢻⣿⣿⣽⣮⡙⠆⠀⠤⢓⡙⣆⠀⠀⠘⠙⢯⣛⣶⠋⠁ +# ⠀⠀⠀⠀⠀⢠⢋⢿⣼⣶⣶⣤⠒⢉⠠⣪⣮⠟⣡⡾⠹⡿⣿⣿⠝⢊⣐⢺⡜⣫⡞⢭⡕⠋⣐⠒⠀⠡⠏⠉⠘⠛⠚⡻⣯⠋⠛⢅⠐⢄⠀⣸⡃⠛⠀⡀⡀⠀⠈⠙⡟⠀⠀ +# ⠀⠀⠀⠀⣠⢫⠎⠙⠿⣿⠛⡡⠔⠕⣴⡿⡁⡮⡷⡶⢟⣿⢎⡵⡠⢞⠱⢈⣼⠁⠄⠇⡄⣢⠒⠀⡎⠀⡇⠒⠐⠐⠐⢚⠐⢷⣔⢖⢊⡈⠒⠗⠠⠘⠈⡁⢈⣠⣤⠾⠀⠀⠀ +# ⠀⠀⠀⣰⢳⠏⢀⢔⢤⠶⠪⣠⣭⣾⣿⠗⢘⣷⣼⠛⠛⢛⡝⣜⢑⣤⣾⠿⣿⣿⢽⣿⠿⠶⢴⣯⣄⡄⣇⣀⣀⡀⠄⠠⠆⣀⡨⢽⣵⣕⣄⣀⣰⣥⣶⣾⡿⠟⠉⠀⠀⠀⠀ +# ⠀⠀⡰⣱⢋⠴⣩⠔⠻⣴⡾⢷⣿⡿⠃⠰⢿⣿⣿⣿⣶⣬⣧⣼⡿⠛⠁⡢⠒⠈⢈⡉⠿⠚⢼⣿⣿⣿⡆⠋⠉⠢⠀⢀⣀⣡⣴⡶⣿⣿⣿⠿⠻⠛⠋⠁⠀⠀⠀⠀⠀⠀⠀ +# ⠀⡼⣳⣯⣱⣪⣲⣫⠻⣯⢟⠽⠋⠀⠀⠀⠀⠈⠙⠻⢻⡳⡩⢇⢀⠸⢢⣤⣴⣁⡀⠊⡀⠠⠂⢉⣫⡭⣁⣀⣠⣴⣶⢿⣿⣿⣿⡿⠞⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀ +# ⣼⣟⡿⣿⣿⢝⣫⣶⡿⠣⠉⠀⠀⠀⠀⠀⠀⠀⣠⣖⠕⡩⢂⢕⠡⠒⠸⣿⣿⣿⡿⢂⢀⠀⣜⡿⣵⣶⣾⣿⡿⠯⠟⠋⠉⠋⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀ +# ⢸⢿⣷⣷⣷⣿⠛⠋⠁⠀⠀⠀⠀⠀⠀⢀⣴⡺⠟⣑⣿⡺⢑⡴⠂⠊⠀⢀⡁⣍⣢⣼⣺⣽⣶⡿⠿⠏⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀ +# ⠀⠈⠙⠻⠝⠀⠀⠀⠀⠀⠀⠀⠀⠀⣰⡿⡋⢰⠕⠋⡿⠉⢁⡈⢕⣲⣼⣒⣯⣷⣿⠿⠟⠋⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀ +# ⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣀⣴⣿⣷⣧⡎⣠⡤⠥⣰⢬⣵⣮⣽⣿⡿⠟⠛⠉⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀ +# ⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣷⣮⣟⡯⣓⣦⣿⣮⣿⣿⣿⠿⠛⠋⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀ +# ⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠛⠿⣿⣿⣿⣿⡿⠟⠛⠉⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀ +# ⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠈⠛⠉⠀⠀ diff --git a/control/velocity_controller_lqr/velocity_controller_lqr/velocity_controller_lqr_lib.py b/control/velocity_controller_lqr/velocity_controller_lqr/velocity_controller_lqr_lib.py index 76ee15ed..5305a08f 100644 --- a/control/velocity_controller_lqr/velocity_controller_lqr/velocity_controller_lqr_lib.py +++ b/control/velocity_controller_lqr/velocity_controller_lqr/velocity_controller_lqr_lib.py @@ -1,25 +1,11 @@ #!/usr/bin/env python3 -import math as math - import numpy as np import control as ct class LQRController: - def __init__( - self, - q_surge, - q_pitch, - q_yaw, - r_surge, - r_pitch, - r_yaw, - i_surge, - i_pitch, - i_yaw, - max_force, - ): + def __init__(self, parameters, inertia_matrix): self.integral_error_surge = 0.0 self.integral_error_pitch = 0.0 self.integral_error_yaw = 0.0 @@ -28,29 +14,36 @@ def __init__( self.pitch_windup = False # Windup variable self.yaw_windup = False # Windup variable - self.max_force = max_force # Maximum force that can be applied + self.q_surge = parameters[0] # Surge cost + self.q_pitch = parameters[1] # Pitch cost + self.q_yaw = parameters[2] # Yaw cost + + self.r_surge = parameters[3] # Surge control cost + self.r_pitch = parameters[4] # Pitch control cost + self.r_yaw = parameters[5] # Yaw control cost - self.i_surge = i_surge # Integral gain for surge - self.i_pitch = i_pitch # Integral gain for pitch - self.i_yaw = i_yaw # Integral gain for yaw + self.i_surge = parameters[6] # Integral gain for surge + self.i_pitch = parameters[7] # Integral gain for pitch + self.i_yaw = parameters[8] # Integral gain for yaw - self.q_surge = q_surge # Surge cost - self.q_pitch = q_pitch # Pitch cost - self.q_yaw = q_yaw # Yaw cost + self.i_weight = parameters[9] # Weight for integral gain - self.r_surge = r_surge # Surge control cost - self.r_pitch = r_pitch # Pitch control cost - self.r_yaw = r_yaw # Yaw control cost + self.max_force = parameters[10] # Maximum force that can be applied + + self.inertia_matrix_inv = inertia_matrix # Inverse of the inertia matrix # VARIABLES self.inertia_matrix_inv = np.linalg.inv( - np.array([[30, 0.6, 0], [0.6, 1.629, 0], [0, 0, 1.769]]) - ) # mass matrix + inertia_matrix + ) # Inverse of the inertia matrix self.state_weight_matrix = np.block( [ [np.diag([self.q_surge, self.q_pitch, self.q_yaw]), np.zeros((3, 3))], - [np.zeros((3, 3)), np.diag([0.5, 0.5, 0.5])], + [ + np.zeros((3, 3)), + np.diag([self.i_weight, self.i_weight, self.i_weight]), + ], ] ) # Augmented state cost matrix self.input_weight_matrix = np.diag( @@ -107,7 +100,7 @@ def ssa(angle: float) -> float: angle += 2 * np.pi return angle - def saturate(self, value: float, windup: bool, limit: float) -> float: + def saturate(self, value: float, windup: bool, limit: float) -> tuple: """Function to saturate the value within the limits, and set the windup flag. Args: @@ -116,7 +109,7 @@ def saturate(self, value: float, windup: bool, limit: float) -> float: limit: float: limit for saturation Returns: - value: float: saturated value + (value, windup): tuple: saturated value and windup flag """ if abs(value) > limit: diff --git a/requirements.sh b/requirements.sh new file mode 100644 index 00000000..762c71ec --- /dev/null +++ b/requirements.sh @@ -0,0 +1,20 @@ +#!/bin/bash +# requirements.sh +# This script installs additional dependencies that are not managed by rosdep. +# It can install both Python libraries (using pip) and C++ libraries (using apt-get). + +# Exit immediately if any command fails to prevent the script from continuing after an error. +set -e + +echo "Starting manual installation of extra dependencies..." + +# ---- PYTHON DEPENDENCIES ---- +# Upgrade pip to ensure compatibility with the latest packages +pip3 install --upgrade pip + +# Install Python packages with specified versions that are not handled by rosdep. +# Specify versions using `==` for consistent builds. +# Example: `pip3 install ==` +pip3 install control==0.10.1 + +echo "Finished installing extra dependencies." From 5f23a5562316fab6716af0b2b5a1cfb0c197fdcd Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud Date: Fri, 15 Nov 2024 15:48:37 +0100 Subject: [PATCH 06/17] =?UTF-8?q?=F0=9F=90=9B=20fix(workflows):=20updated?= =?UTF-8?q?=20source=20build=20to=20use=20correct=20ref?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .github/workflows/source-build.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/source-build.yaml b/.github/workflows/source-build.yaml index 8c59afad..ab5ec431 100644 --- a/.github/workflows/source-build.yaml +++ b/.github/workflows/source-build.yaml @@ -12,6 +12,6 @@ jobs: with: ros_distro: 'humble' os_name: 'ubuntu-22.04' - ref: ${{ github.ref_name }} + ref: ${{ github.ref }} vcs_repo_file_url: 'https://raw.githubusercontent.com/vortexntnu/vortex-auv/main/ros2.repos' download_requirements: true From 0a94dd698a22787be449723dcdcd3dcac9d80136 Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud Date: Fri, 15 Nov 2024 16:09:55 +0100 Subject: [PATCH 07/17] =?UTF-8?q?=F0=9F=94=A7=20feat(requirements.sh):=20s?= =?UTF-8?q?pecified=20numpy=20version=20to=20be=20less=20than=201.25.0=20t?= =?UTF-8?q?o=20support=20older=20python=20libraries?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- requirements.sh | 1 + 1 file changed, 1 insertion(+) diff --git a/requirements.sh b/requirements.sh index 762c71ec..07e952d9 100644 --- a/requirements.sh +++ b/requirements.sh @@ -16,5 +16,6 @@ pip3 install --upgrade pip # Specify versions using `==` for consistent builds. # Example: `pip3 install ==` pip3 install control==0.10.1 +pip3 install numpy<=1.25.0 echo "Finished installing extra dependencies." From df25b13e9054f1bc89f4a23b9bb7a53c4c095b37 Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud Date: Fri, 15 Nov 2024 16:18:42 +0100 Subject: [PATCH 08/17] =?UTF-8?q?=F0=9F=90=9B=20fix:=20ensure=20NumPy=20ve?= =?UTF-8?q?rsion=20is=20constrained=20to=20less=20than=201.25.0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- requirements.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/requirements.sh b/requirements.sh index 07e952d9..376146dd 100644 --- a/requirements.sh +++ b/requirements.sh @@ -16,6 +16,6 @@ pip3 install --upgrade pip # Specify versions using `==` for consistent builds. # Example: `pip3 install ==` pip3 install control==0.10.1 -pip3 install numpy<=1.25.0 +pip3 install "numpy<1.25.0" echo "Finished installing extra dependencies." From 5d3b4ee34b5d9132017b1a77fde58dbe5e57e4c5 Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud Date: Sat, 16 Nov 2024 11:47:11 +0100 Subject: [PATCH 09/17] =?UTF-8?q?=F0=9F=9A=91=20fix(workflows):=20fixed=20?= =?UTF-8?q?source=20builds=20input=20for=20dependency=20script?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .github/workflows/source-build.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/source-build.yaml b/.github/workflows/source-build.yaml index ab5ec431..7b73ca40 100644 --- a/.github/workflows/source-build.yaml +++ b/.github/workflows/source-build.yaml @@ -14,4 +14,4 @@ jobs: os_name: 'ubuntu-22.04' ref: ${{ github.ref }} vcs_repo_file_url: 'https://raw.githubusercontent.com/vortexntnu/vortex-auv/main/ros2.repos' - download_requirements: true + dependency_script: 'requirements.sh' From a957ff1b513ad59d89c460bf055fb51fa49702ee Mon Sep 17 00:00:00 2001 From: Cyprian Date: Sun, 17 Nov 2024 15:39:01 +0100 Subject: [PATCH 10/17] =?UTF-8?q?=E2=9C=A8=20feat:=20fixed=20anders=20klag?= =?UTF-8?q?erud=20bug=20=F0=9F=90=9B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../velocity_controller_lqr/CMakeLists.txt | 3 - control/velocity_controller_lqr/README.txt | 1 + .../config/param_velocity_controller_lqr.yaml | 1 - .../launch/velocity_controller_lqr.launch.py | 2 - control/velocity_controller_lqr/package.xml | 4 +- .../velocity_controller_lqr/requirements.txt | 1 + .../scripts/__init__.py | 0 .../scripts/velocity_controller_lqr_node.py | 140 ++++++----- .../tests/test_velocity_controller_lqr.py | 23 +- .../velocity_controller_lqr_lib.py | 228 +++++++++++------- 10 files changed, 237 insertions(+), 166 deletions(-) create mode 100644 control/velocity_controller_lqr/README.txt create mode 100644 control/velocity_controller_lqr/requirements.txt create mode 100644 control/velocity_controller_lqr/scripts/__init__.py diff --git a/control/velocity_controller_lqr/CMakeLists.txt b/control/velocity_controller_lqr/CMakeLists.txt index a12bc847..05f93727 100644 --- a/control/velocity_controller_lqr/CMakeLists.txt +++ b/control/velocity_controller_lqr/CMakeLists.txt @@ -1,12 +1,10 @@ cmake_minimum_required(VERSION 3.8) project(velocity_controller_lqr) -# find dependencies find_package(ament_cmake_python REQUIRED) find_package(rclpy REQUIRED) find_package(nav_msgs REQUIRED) find_package(geometry_msgs REQUIRED) -find_package(rclcpp REQUIRED) find_package(vortex_msgs REQUIRED) install(DIRECTORY @@ -17,7 +15,6 @@ install(DIRECTORY ament_python_install_package(${PROJECT_NAME}) -#install python scripts install(PROGRAMS scripts/velocity_controller_lqr_node.py DESTINATION lib/${PROJECT_NAME} diff --git a/control/velocity_controller_lqr/README.txt b/control/velocity_controller_lqr/README.txt new file mode 100644 index 00000000..c49e405e --- /dev/null +++ b/control/velocity_controller_lqr/README.txt @@ -0,0 +1 @@ +This package contains the velocity controller for the AUV Orca. The controller utilizes an LQR optimal controller (imported from the python control library), and controls pitch, yaw and surge. The controller is meant to traverse larger distances. diff --git a/control/velocity_controller_lqr/config/param_velocity_controller_lqr.yaml b/control/velocity_controller_lqr/config/param_velocity_controller_lqr.yaml index 98711ab9..d21b1739 100644 --- a/control/velocity_controller_lqr/config/param_velocity_controller_lqr.yaml +++ b/control/velocity_controller_lqr/config/param_velocity_controller_lqr.yaml @@ -22,7 +22,6 @@ velocity_controller_lqr_node: i_weight: 0.5 - #Inertia matrix inertia_matrix: [30.0, 0.6, 0.0, 0.6, 1.629, 0.0, 0.0, 0.0, 1.729] #Clamp parameter diff --git a/control/velocity_controller_lqr/launch/velocity_controller_lqr.launch.py b/control/velocity_controller_lqr/launch/velocity_controller_lqr.launch.py index c35cb141..716bfdc9 100644 --- a/control/velocity_controller_lqr/launch/velocity_controller_lqr.launch.py +++ b/control/velocity_controller_lqr/launch/velocity_controller_lqr.launch.py @@ -16,14 +16,12 @@ def generate_launch_description() -> LaunchDescription: LaunchDescription: A ROS 2 launch description containing the velocity_controller_lqr node. """ - # Define the path to the parameter file parameter_file = os.path.join( get_package_share_directory("velocity_controller_lqr"), "config", "param_velocity_controller_lqr.yaml", ) - # Define the node velocity_controller_node = Node( package="velocity_controller_lqr", executable="velocity_controller_lqr_node.py", diff --git a/control/velocity_controller_lqr/package.xml b/control/velocity_controller_lqr/package.xml index 02080efd..252dc84f 100644 --- a/control/velocity_controller_lqr/package.xml +++ b/control/velocity_controller_lqr/package.xml @@ -7,7 +7,7 @@ cyprian MIT - ament_cmake + ament_cmake_python rclpy geometry_msgs @@ -15,8 +15,6 @@ vortex_msgs python-control-pip - ament_lint_auto - ament_lint_common ament_cmake_pytest diff --git a/control/velocity_controller_lqr/requirements.txt b/control/velocity_controller_lqr/requirements.txt new file mode 100644 index 00000000..2c2b91d3 --- /dev/null +++ b/control/velocity_controller_lqr/requirements.txt @@ -0,0 +1 @@ +numpy<1.25.0 diff --git a/control/velocity_controller_lqr/scripts/__init__.py b/control/velocity_controller_lqr/scripts/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/control/velocity_controller_lqr/scripts/velocity_controller_lqr_node.py b/control/velocity_controller_lqr/scripts/velocity_controller_lqr_node.py index 7744b2e9..a616acf3 100644 --- a/control/velocity_controller_lqr/scripts/velocity_controller_lqr_node.py +++ b/control/velocity_controller_lqr/scripts/velocity_controller_lqr_node.py @@ -4,7 +4,12 @@ from geometry_msgs.msg import Wrench from nav_msgs.msg import Odometry from rclpy.node import Node -from velocity_controller_lqr.velocity_controller_lqr_lib import LQRController +from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy +from velocity_controller_lqr.velocity_controller_lqr_lib import ( + GuidanceValues, + LQRController, + State, +) from vortex_msgs.msg import LOSGuidance @@ -12,42 +17,70 @@ class LinearQuadraticRegulator(Node): def __init__(self): super().__init__("velocity_controller_lqr_node") - self.declare_parameter("odom_topic", "/nucleus/odom") - self.declare_parameter("guidance_topic", "/guidance/los") - self.declare_parameter("thrust_topic", "/thrust/wrench_input") + odom_topic, guidance_topic, thrust_topic = self.get_topics() - odom_path = self.get_parameter("odom_topic").value - guidance_path = self.get_parameter("guidance_topic").value - thrust_path = self.get_parameter("thrust_topic").value + best_effort_qos = QoSProfile( + reliability=ReliabilityPolicy.BEST_EFFORT, + history=HistoryPolicy.KEEP_LAST, + depth=1, + ) - # SUBSRCIBERS ----------------------------------- + reliable_qos = QoSProfile( + reliability=ReliabilityPolicy.RELIABLE, + history=HistoryPolicy.KEEP_LAST, + depth=10, + ) + + # ---------------------------- SUBSCRIBERS --------------------------- self.nucleus_subscriber = self.create_subscription( - Odometry, odom_path, self.nucleus_callback, 20 + Odometry, odom_topic, self.odom_callback, qos_profile=best_effort_qos ) self.guidance_subscriber = self.create_subscription( - LOSGuidance, guidance_path, self.guidance_callback, 20 + LOSGuidance, + guidance_topic, + self.guidance_callback, + qos_profile=best_effort_qos, ) - # PUBLISHERS ------------------------------------ - self.publisherLQR = self.create_publisher(Wrench, thrust_path, 10) + # ---------------------------- PUBLISHERS ---------------------------- + self.publisherLQR = self.create_publisher(Wrench, thrust_topic, reliable_qos) - # TIMERS ---------------------------------------- + # ------------------------------ TIMERS ------------------------------ self.control_timer = self.create_timer(0.1, self.control_loop) - # ROS2 SHENNANIGANS with parameters + # ------------------ ROS2 PARAMETERS AND CONTROLLER ------------------ parameters, inertia_matrix = self.get_parameters() self.controller = LQRController(parameters, inertia_matrix) - # Only keeping variables that need to be updated inside of a callback - self.coriolis_matrix = np.zeros((3, 3)) # Initialize Coriolis matrix - self.states = np.array( - [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - ) # State vector 1. surge, 2. pitch, 3. yaw, 4. integral_surge, 5. integral_pitch, 6. integral_yaw - self.guidance_values = np.array( - [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - ) # Array to store guidance values + # ---------------- CALLBACK VARIABLES INITIALIZATION ----------------- + self.coriolis_matrix = np.zeros((3, 3)) + self.states = State() + self.guidance_values = GuidanceValues() + + def get_topics(self): + """Get the topics from the parameter server. + + Returns: + odom_topic: str: The topic for accessing the odometry data from the parameter file + guidance_topic: str: The topic for accessing the guidance data the parameter file + thrust_topic: str: The topic for accessing the thrust data from the parameter file + """ + self.declare_parameter("odom_topic", "/nucleus/odom") + self.declare_parameter("guidance_topic", "/guidance/los") + self.declare_parameter("thrust_topic", "/thrust/wrench_input") + + odom_topic = self.get_parameter("odom_topic").value + guidance_topic = self.get_parameter("guidance_topic").value + thrust_topic = self.get_parameter("thrust_topic").value + + return odom_topic, guidance_topic, thrust_topic def get_parameters(self): + """Get the parameters from the parameter server. + + Returns: + parameters: LQRParams: The parameters for the LQR controller + """ self.declare_parameter("q_surge", 75) self.declare_parameter("q_pitch", 175) self.declare_parameter("q_yaw", 175) @@ -85,59 +118,49 @@ def get_parameters(self): inertia_matrix_flat = self.get_parameter("inertia_matrix").value inertia_matrix = np.array(inertia_matrix_flat).reshape((3, 3)) - parameters = np.array( - [ - q_surge, - q_pitch, - q_yaw, - r_surge, - r_pitch, - r_yaw, - i_surge, - i_pitch, - i_yaw, - i_weight, - max_force, - ] - ) - - for i in range(len(parameters - 2)): - assert parameters[i] > 0, f"Parameter {i} is negative" - assert 0 <= parameters[-1] <= 99.9, "Max force must be in the range [0, 99.9]." + parameters = [ + q_surge, + q_pitch, + q_yaw, + r_surge, + r_pitch, + r_yaw, + i_surge, + i_pitch, + i_yaw, + i_weight, + max_force, + ] return parameters, inertia_matrix # ---------------------------------------------------------------Callback Functions--------------------------------------------------------------- - def nucleus_callback( - self, msg: Odometry - ): # callback function to read data from nucleus - dummy, self.states[1], self.states[2] = LQRController.quaternion_to_euler_angle( + def odom_callback(self, msg: Odometry): + _, self.states.yaw, self.states.pitch = LQRController.quaternion_to_euler_angle( msg.pose.pose.orientation.w, msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, ) - self.states[0] = msg.twist.twist.linear.x + self.states.surge = msg.twist.twist.linear.x - self.coriolis_matrix = self.controller.calculate_coriolis_matrix( + self.coriolis_matrix = LQRController.calculate_coriolis_matrix( msg.twist.twist.angular.y, msg.twist.twist.angular.z, msg.twist.twist.linear.y, msg.twist.twist.linear.z, - ) # coriolis matrix + ) - def guidance_callback( - self, msg: LOSGuidance - ): # Function to read data from guidance - self.guidance_values[0] = msg.surge - self.guidance_values[1] = msg.pitch - self.guidance_values[2] = msg.yaw + def guidance_callback(self, msg: LOSGuidance): + self.guidance_values.surge = msg.surge + self.guidance_values.pitch = msg.pitch + self.guidance_values.yaw = msg.yaw - # ---------------------------------------------------------------Publisher Functions--------------------------------------------------------------- + # ---------------------------------------------------------------Publisher Functions-------------------------------------------------------------- - def control_loop(self): # The LQR controller function + def control_loop(self): msg = Wrench() u = self.controller.calculate_lqr_u( @@ -147,14 +170,13 @@ def control_loop(self): # The LQR controller function msg.torque.y = float(u[1]) msg.torque.z = float(u[2]) - # Publish the control input self.publisherLQR.publish(msg) -# ---------------------------------------------------------------Main Function--------------------------------------------------------------- +# ---------------------------------------------------------------------Main Function----------------------------------------------------------------- -def main(args=None): # main function +def main(args=None): rclpy.init(args=args) node = LinearQuadraticRegulator() rclpy.spin(node) diff --git a/control/velocity_controller_lqr/tests/test_velocity_controller_lqr.py b/control/velocity_controller_lqr/tests/test_velocity_controller_lqr.py index 511d6d22..3eedce10 100644 --- a/control/velocity_controller_lqr/tests/test_velocity_controller_lqr.py +++ b/control/velocity_controller_lqr/tests/test_velocity_controller_lqr.py @@ -1,14 +1,14 @@ import numpy as np from velocity_controller_lqr.velocity_controller_lqr_lib import LQRController -controller = LQRController(0, 0, 0, 0, 0, 0, 0, 0, 0, 0) +controller = LQRController( + [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]) +) class TestVelocityController: def test_placeholder(self): - assert ( - controller is not None - ) # Simple test to ensure the controller initializes + assert controller is not None def test_ssa(self): print("Commencing ssa test: \n") @@ -32,22 +32,22 @@ def test_quaternion_to_euler_angle(self): def test_saturate(self): print("Commencing saturate test: \n") - # Test case 1: Saturation occurs, so windup should be True + # Test case 1: Saturation occurs saturated_value, windup = controller.saturate(10, False, 5) assert saturated_value == 5.0 assert windup == True - # Test case 2: Saturation occurs with negative limit, so windup should be True + # Test case 2: Saturation occurs with negative limit saturated_value, windup = controller.saturate(-10, False, 5) assert saturated_value == -5.0 assert windup == True - # Test case 3: No saturation, so windup should be False + # Test case 3: No saturation saturated_value, windup = controller.saturate(3, True, 5) assert saturated_value == 3.0 assert windup == False - # Test case 4: No saturation with negative value, so windup should be False + # Test case 4: No saturation with negative value saturated_value, windup = controller.saturate(-3, True, 5) assert saturated_value == -3.0 assert windup == False @@ -65,6 +65,7 @@ def test_anti_windup(self): print("Anti windup test passed") - def test_final(self): - print("¯\_(ツ)_/¯ ehh good enough pass anyway") - pass + def test_max_force(self): + assert ( + 0 <= controller.max_force <= 99.9 + ), "Max force must be in the range [0, 99.9]." diff --git a/control/velocity_controller_lqr/velocity_controller_lqr/velocity_controller_lqr_lib.py b/control/velocity_controller_lqr/velocity_controller_lqr/velocity_controller_lqr_lib.py index 5305a08f..c4255f42 100644 --- a/control/velocity_controller_lqr/velocity_controller_lqr/velocity_controller_lqr_lib.py +++ b/control/velocity_controller_lqr/velocity_controller_lqr/velocity_controller_lqr_lib.py @@ -1,54 +1,57 @@ #!/usr/bin/env python3 +from dataclasses import dataclass + import numpy as np import control as ct -class LQRController: - def __init__(self, parameters, inertia_matrix): - self.integral_error_surge = 0.0 - self.integral_error_pitch = 0.0 - self.integral_error_yaw = 0.0 - - self.surge_windup = False # Windup variable - self.pitch_windup = False # Windup variable - self.yaw_windup = False # Windup variable - - self.q_surge = parameters[0] # Surge cost - self.q_pitch = parameters[1] # Pitch cost - self.q_yaw = parameters[2] # Yaw cost - - self.r_surge = parameters[3] # Surge control cost - self.r_pitch = parameters[4] # Pitch control cost - self.r_yaw = parameters[5] # Yaw control cost +@dataclass +class State: + """Dataclass to store the state values for the LQR controller. + + Attributes: + surge: float: Surge state value + pitch: float: Pitch state value + yaw: float: Yaw state value + integral_surge: float: Surge integral state value + integral_pitch: float: Pitch integral state value + integral_yaw: float: Yaw integral state value + """ + + surge: float = 0.0 + pitch: float = 0.0 + yaw: float = 0.0 + integral_surge: float = 0.0 + integral_pitch: float = 0.0 + integral_yaw: float = 0.0 + + +@dataclass +class GuidanceValues: + """Dataclass to store the guidance values for the LQR controller. + + Attributes: + surge: float: Surge guidance value + pitch: float: Pitch guidance value + yaw: float: Yaw guidance value + integral_surge: float: Surge integral guidance value + integral_pitch: float: Pitch integral guidance value + integral_yaw: float: Yaw integral guidance value + """ + + surge: float = 0.0 + pitch: float = 0.0 + yaw: float = 0.0 + integral_surge: float = 0.0 + integral_pitch: float = 0.0 + integral_yaw: float = 0.0 - self.i_surge = parameters[6] # Integral gain for surge - self.i_pitch = parameters[7] # Integral gain for pitch - self.i_yaw = parameters[8] # Integral gain for yaw - self.i_weight = parameters[9] # Weight for integral gain - - self.max_force = parameters[10] # Maximum force that can be applied - - self.inertia_matrix_inv = inertia_matrix # Inverse of the inertia matrix - - # VARIABLES - self.inertia_matrix_inv = np.linalg.inv( - inertia_matrix - ) # Inverse of the inertia matrix - - self.state_weight_matrix = np.block( - [ - [np.diag([self.q_surge, self.q_pitch, self.q_yaw]), np.zeros((3, 3))], - [ - np.zeros((3, 3)), - np.diag([self.i_weight, self.i_weight, self.i_weight]), - ], - ] - ) # Augmented state cost matrix - self.input_weight_matrix = np.diag( - [self.r_surge, self.r_pitch, self.r_yaw] - ) # control cost matrix +class LQRController: + def __init__(self, parameters, inertia_matrix): + self.set_params(parameters) + self.set_matrices(inertia_matrix) @staticmethod def quaternion_to_euler_angle(w: float, x: float, y: float, z: float) -> tuple: @@ -61,9 +64,9 @@ def quaternion_to_euler_angle(w: float, x: float, y: float, z: float) -> tuple: z: float: z component of quaternion Returns: - X: float: roll angle - Y: float: pitch angle - Z: float: yaw angle + phi: float: roll angle + theta: float: pitch angle + psi: float: yaw angle """ y_square = y * y @@ -94,11 +97,7 @@ def ssa(angle: float) -> float: angle: float: angle in radians """ - if angle > np.pi: - angle -= 2 * np.pi - elif angle < -np.pi: - angle += 2 * np.pi - return angle + return (angle + np.pi) % (2 * np.pi) - np.pi def saturate(self, value: float, windup: bool, limit: float) -> tuple: """Function to saturate the value within the limits, and set the windup flag. @@ -145,16 +144,17 @@ def anti_windup( return integral_sum + @staticmethod def calculate_coriolis_matrix( - self, pitch_rate: float, yaw_rate: float, sway: float, heave: float + pitch_rate: float, yaw_rate: float, sway_vel: float, heave_vel: float ) -> np.array: """Calculates the 3x3 coriolis matrix. Parameters: pitch_rate: float: pitch rate in rad/s yaw_rate: float: yaw rate in rad/s - sway: float: sway velocity in m/s - heave: float: heave velocity in m/s + sway_vel: float: sway velocity in m/s + heave_vel: float: heave velocity in m/s Returns: C: np.array: 3x3 Coriolis Matrix @@ -162,55 +162,77 @@ def calculate_coriolis_matrix( """ return np.array( [ - [0.2, -30 * sway * 0.01, -30 * heave * 0.01], - [30 * sway * 0.01, 0, 1.629 * pitch_rate], - [30 * heave * 0.01, 1.769 * yaw_rate, 0], + [0.2, -30 * sway_vel * 0.01, -30 * heave_vel * 0.01], + [30 * sway_vel * 0.01, 0, 1.629 * pitch_rate], + [30 * heave_vel * 0.01, 1.769 * yaw_rate, 0], ] ) - def calculate_lqr_u( - self, coriolis_matrix: np.array, states: np.array, guidance_values: np.array - ) -> np.array: - """Calculates the control input using the control library in python. + def set_params(self, parameters): + self.integral_error_surge = 0.0 + self.integral_error_pitch = 0.0 + self.integral_error_yaw = 0.0 - Parameters: - C: np.array: 3x3 Coriolis Matrix - states: np.array: 6x1 state vector - guidance_values: np.array: 6x1 guidance vector + self.surge_windup = False + self.pitch_windup = False + self.yaw_windup = False - Returns: - u: np.array: 3x1 control input + self.q_surge = parameters[0] # Surge LQR cost + self.q_pitch = parameters[1] # Pitch LQR cost + self.q_yaw = parameters[2] # Yaw LQR cost - """ + self.r_surge = parameters[3] # Surge LQR input cost + self.r_pitch = parameters[4] # Pitch LQR input cost + self.r_yaw = parameters[5] # Yaw input LQR cost + + self.i_surge = parameters[6] # Integral gain for surge + self.i_pitch = parameters[7] # Integral gain for pitch + self.i_yaw = parameters[8] # Integral gain for yaw + self.i_weight = parameters[9] # Weight for integral gain + self.max_force = parameters[10] + + def set_matrices(self, inertia_matrix): + self.inertia_matrix_inv = np.linalg.inv(inertia_matrix) + self.state_weight_matrix = np.block( + [ + [np.diag([self.q_surge, self.q_pitch, self.q_yaw]), np.zeros((3, 3))], + [ + np.zeros((3, 3)), + np.diag([self.i_weight, self.i_weight, self.i_weight]), + ], + ] + ) + + self.input_weight_matrix = np.diag([self.r_surge, self.r_pitch, self.r_yaw]) + + def update_augmented_matrices(self, coriolis_matrix: np.array) -> None: system_matrix = self.inertia_matrix_inv @ coriolis_matrix input_matrix = self.inertia_matrix_inv - # Augment the A and B matrices for integrators for surge, pitch, and yaw self.augmented_system_matrix = np.block( [[system_matrix, np.zeros((3, 3))], [-np.eye(3), np.zeros((3, 3))]] - ) # Integrators added for surge, pitch, and yaw + ) self.augmented_input_matrix = np.block( [[input_matrix], [np.zeros((3, 3))]] ) # Control input does not affect integrators directly - # CT LQR controller from control library python - lqr_gain, ricatti_s, ricatti_e = ct.lqr( - self.augmented_system_matrix, - self.augmented_input_matrix, - self.state_weight_matrix, - self.input_weight_matrix, - ) + def update_error(self, guidance_values: GuidanceValues, states: State) -> np.array: + """Updates the error values for the LQR controller. - # Calculate the control input + Args: + guidance_values: GuidanceValues: 6x1 dataclass containing the guidance values + states: State: 6x1 dataclass containing the state values + + Returns: + state_error: np.array: 6x1 array of the state errors + """ surge_error = ( - guidance_values[0] - states[0] + guidance_values.surge - states.surge ) # Surge error no need for angle wrapping - pitch_error = self.ssa( - guidance_values[1] - states[1] - ) # Apply ssa to pitch error - yaw_error = self.ssa(guidance_values[2] - states[2]) # Apply ssa to yaw error + pitch_error = self.ssa(guidance_values.yaw - states.yaw) + yaw_error = self.ssa(guidance_values.pitch - states.pitch) - # Update the integrators for surge, pitch, and yaw + # Update the integrator sums self.integral_error_surge = self.anti_windup( self.i_surge, surge_error, self.integral_error_surge, self.surge_windup ) @@ -231,10 +253,17 @@ def calculate_lqr_u( self.integral_error_yaw, ] ) + return state_error + + def saturate_input(self, u: np.array) -> np.array: + """Saturates the control input within the limits, and sets the windup flag. - # Augmented state vector including integrators - u = -lqr_gain @ state_error + Args: + u: np.array: 3x1 control input + Returns: + u: np.array: 3x1 saturated control input + """ self.surge_windup, force_x = self.saturate( u[0], self.surge_windup, self.max_force ) @@ -242,9 +271,34 @@ def calculate_lqr_u( u[1], self.pitch_windup, self.max_force ) self.yaw_windup, torque_z = self.saturate(u[2], self.yaw_windup, self.max_force) + return [force_x, torque_y, torque_z] + + def calculate_lqr_u( + self, coriolis_matrix: np.array, states: State, guidance_values: GuidanceValues + ) -> np.array: + """Calculates the control input using the control library in python. + + Parameters: + C: np.array: 3x3 Coriolis Matrix + states: State: 6x1 dataclass containing the state values + guidance_values: GuidanceValues: 6x1 dataclass containing the guidance values + + Returns: + u: np.array: 3x1 control input + + """ + self.update_augmented_matrices(coriolis_matrix) + + lqr_gain, _, _ = ct.lqr( + self.augmented_system_matrix, + self.augmented_input_matrix, + self.state_weight_matrix, + self.input_weight_matrix, + ) - u = np.array([force_x, torque_y, torque_z]) + state_error = self.update_error(guidance_values, states) + u = self.saturate_input(-lqr_gain @ state_error) return u From a553dda6de422162ac40573eb348a9344eab53a0 Mon Sep 17 00:00:00 2001 From: Cyprian Date: Mon, 6 Jan 2025 12:14:20 +0100 Subject: [PATCH 11/17] shedding rust after winter break --- .../scripts/velocity_controller_lqr_node.py | 17 ++++++++-- .../velocity_controller_lqr_lib.py | 32 +++++++++---------- 2 files changed, 29 insertions(+), 20 deletions(-) diff --git a/control/velocity_controller_lqr/scripts/velocity_controller_lqr_node.py b/control/velocity_controller_lqr/scripts/velocity_controller_lqr_node.py index a616acf3..3307d82b 100644 --- a/control/velocity_controller_lqr/scripts/velocity_controller_lqr_node.py +++ b/control/velocity_controller_lqr/scripts/velocity_controller_lqr_node.py @@ -3,6 +3,7 @@ import rclpy from geometry_msgs.msg import Wrench from nav_msgs.msg import Odometry +from rclpy.executors import MultiThreadedExecutor from rclpy.node import Node from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy from velocity_controller_lqr.velocity_controller_lqr_lib import ( @@ -178,9 +179,19 @@ def control_loop(self): def main(args=None): rclpy.init(args=args) - node = LinearQuadraticRegulator() - rclpy.spin(node) - rclpy.shutdown() + lqr_node = LinearQuadraticRegulator() + executor = MultiThreadedExecutor() + executor.add_node(lqr_node) + + try: + executor.spin() + except KeyboardInterrupt: + pass + finally: + executor.shutdown() + lqr_node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() if __name__ == "__main__": diff --git a/control/velocity_controller_lqr/velocity_controller_lqr/velocity_controller_lqr_lib.py b/control/velocity_controller_lqr/velocity_controller_lqr/velocity_controller_lqr_lib.py index c4255f42..dd05c2ce 100644 --- a/control/velocity_controller_lqr/velocity_controller_lqr/velocity_controller_lqr_lib.py +++ b/control/velocity_controller_lqr/velocity_controller_lqr/velocity_controller_lqr_lib.py @@ -177,18 +177,18 @@ def set_params(self, parameters): self.pitch_windup = False self.yaw_windup = False - self.q_surge = parameters[0] # Surge LQR cost - self.q_pitch = parameters[1] # Pitch LQR cost - self.q_yaw = parameters[2] # Yaw LQR cost - - self.r_surge = parameters[3] # Surge LQR input cost - self.r_pitch = parameters[4] # Pitch LQR input cost - self.r_yaw = parameters[5] # Yaw input LQR cost - - self.i_surge = parameters[6] # Integral gain for surge - self.i_pitch = parameters[7] # Integral gain for pitch - self.i_yaw = parameters[8] # Integral gain for yaw - self.i_weight = parameters[9] # Weight for integral gain + self.q_surge = parameters[0] + self.q_pitch = parameters[1] + self.q_yaw = parameters[2] + + self.r_surge = parameters[3] + self.r_pitch = parameters[4] + self.r_yaw = parameters[5] + + self.i_surge = parameters[6] + self.i_pitch = parameters[7] + self.i_yaw = parameters[8] + self.i_weight = parameters[9] self.max_force = parameters[10] def set_matrices(self, inertia_matrix): @@ -212,9 +212,7 @@ def update_augmented_matrices(self, coriolis_matrix: np.array) -> None: self.augmented_system_matrix = np.block( [[system_matrix, np.zeros((3, 3))], [-np.eye(3), np.zeros((3, 3))]] ) - self.augmented_input_matrix = np.block( - [[input_matrix], [np.zeros((3, 3))]] - ) # Control input does not affect integrators directly + self.augmented_input_matrix = np.block([[input_matrix], [np.zeros((3, 3))]]) def update_error(self, guidance_values: GuidanceValues, states: State) -> np.array: """Updates the error values for the LQR controller. @@ -228,11 +226,11 @@ def update_error(self, guidance_values: GuidanceValues, states: State) -> np.arr """ surge_error = ( guidance_values.surge - states.surge - ) # Surge error no need for angle wrapping + ) # Surge error isn't an angle, no need for angle wrapping pitch_error = self.ssa(guidance_values.yaw - states.yaw) yaw_error = self.ssa(guidance_values.pitch - states.pitch) - # Update the integrator sums + # Update the running integrator sums self.integral_error_surge = self.anti_windup( self.i_surge, surge_error, self.integral_error_surge, self.surge_windup ) From 1d55485c5d89f71b39744c3bac449116d78eaa32 Mon Sep 17 00:00:00 2001 From: Cyprian Date: Mon, 6 Jan 2025 14:43:36 +0100 Subject: [PATCH 12/17] dataclasses. --- .../config/param_velocity_controller_lqr.yaml | 32 +++--- .../scripts/velocity_controller_lqr_node.py | 101 +++++++++--------- .../velocity_controller_lqr_lib.py | 97 ++++++++++++----- 3 files changed, 136 insertions(+), 94 deletions(-) diff --git a/control/velocity_controller_lqr/config/param_velocity_controller_lqr.yaml b/control/velocity_controller_lqr/config/param_velocity_controller_lqr.yaml index d21b1739..c57dfb5f 100644 --- a/control/velocity_controller_lqr/config/param_velocity_controller_lqr.yaml +++ b/control/velocity_controller_lqr/config/param_velocity_controller_lqr.yaml @@ -1,26 +1,26 @@ velocity_controller_lqr_node: ros__parameters: + dt: 0.1 - #topic parameters - odom_topic: /nucleus/odom - guidance_topic: /guidance/los - thrust_topic: /thrust/wrench_input + topics: + odom_topic: /nucleus/odom + guidance_topic: /guidance/los + thrust_topic: /thrust/wrench_input - #LQR parameters + LQR_params: + q_surge: 75 + q_pitch: 175 + q_yaw: 175 - q_surge: 75 - q_pitch: 175 - q_yaw: 175 + r_surge: 0.3 + r_pitch: 0.4 + r_yaw: 0.4 - r_surge: 0.3 - r_pitch: 0.4 - r_yaw: 0.4 + i_surge: 0.3 + i_pitch: 0.4 + i_yaw: 0.3 - i_surge: 0.3 - i_pitch: 0.4 - i_yaw: 0.3 - - i_weight: 0.5 + i_weight: 0.5 inertia_matrix: [30.0, 0.6, 0.0, 0.6, 1.629, 0.0, 0.0, 0.0, 1.729] diff --git a/control/velocity_controller_lqr/scripts/velocity_controller_lqr_node.py b/control/velocity_controller_lqr/scripts/velocity_controller_lqr_node.py index 3307d82b..f7c7ff08 100644 --- a/control/velocity_controller_lqr/scripts/velocity_controller_lqr_node.py +++ b/control/velocity_controller_lqr/scripts/velocity_controller_lqr_node.py @@ -9,6 +9,7 @@ from velocity_controller_lqr.velocity_controller_lqr_lib import ( GuidanceValues, LQRController, + LQRParameters, State, ) from vortex_msgs.msg import LOSGuidance @@ -47,11 +48,14 @@ def __init__(self): self.publisherLQR = self.create_publisher(Wrench, thrust_topic, reliable_qos) # ------------------------------ TIMERS ------------------------------ - self.control_timer = self.create_timer(0.1, self.control_loop) + self.declare_parameter("dt", 0.1) + dt = self.get_parameter("dt").value + self.control_timer = self.create_timer(dt, self.control_loop) # ------------------ ROS2 PARAMETERS AND CONTROLLER ------------------ - parameters, inertia_matrix = self.get_parameters() - self.controller = LQRController(parameters, inertia_matrix) + self.lqr_params = LQRParameters() + inertia_matrix = self.get_parameters() + self.controller = LQRController(self.lqr_params, inertia_matrix) # ---------------- CALLBACK VARIABLES INITIALIZATION ----------------- self.coriolis_matrix = np.zeros((3, 3)) @@ -66,78 +70,69 @@ def get_topics(self): guidance_topic: str: The topic for accessing the guidance data the parameter file thrust_topic: str: The topic for accessing the thrust data from the parameter file """ - self.declare_parameter("odom_topic", "/nucleus/odom") - self.declare_parameter("guidance_topic", "/guidance/los") - self.declare_parameter("thrust_topic", "/thrust/wrench_input") + self.declare_parameter("topics.odom_topic", "/nucleus/odom") + self.declare_parameter("topics.guidance_topic", "/guidance/los") + self.declare_parameter("topics.thrust_topic", "/thrust/wrench_input") - odom_topic = self.get_parameter("odom_topic").value - guidance_topic = self.get_parameter("guidance_topic").value - thrust_topic = self.get_parameter("thrust_topic").value + odom_topic = self.get_parameter("topics.odom_topic").value + guidance_topic = self.get_parameter("topics.guidance_topic").value + thrust_topic = self.get_parameter("topics.thrust_topic").value return odom_topic, guidance_topic, thrust_topic def get_parameters(self): - """Get the parameters from the parameter server. + """Updates the LQR_params in the LQR_parameters Dataclass, and gets the inertia matrix from config. Returns: - parameters: LQRParams: The parameters for the LQR controller + inertia_matrix: np.array: The inertia matrix of the AUV """ - self.declare_parameter("q_surge", 75) - self.declare_parameter("q_pitch", 175) - self.declare_parameter("q_yaw", 175) + self.declare_parameter("LQR_params.q_surge", 75) + self.declare_parameter("LQR_params.q_pitch", 175) + self.declare_parameter("LQR_params.q_yaw", 175) - self.declare_parameter("r_surge", 0.3) - self.declare_parameter("r_pitch", 0.4) - self.declare_parameter("r_yaw", 0.4) + self.declare_parameter("LQR_params.r_surge", 0.3) + self.declare_parameter("LQR_params.r_pitch", 0.4) + self.declare_parameter("LQR_params.r_yaw", 0.4) - self.declare_parameter("i_surge", 0.3) - self.declare_parameter("i_pitch", 0.4) - self.declare_parameter("i_yaw", 0.3) + self.declare_parameter("LQR_params.i_surge", 0.3) + self.declare_parameter("LQR_params.i_pitch", 0.4) + self.declare_parameter("LQR_params.i_yaw", 0.3) - self.declare_parameter("i_weight", 0.5) + self.declare_parameter("LQR_params.i_weight", 0.5) self.declare_parameter("max_force", 99.5) self.declare_parameter( "inertia_matrix", [30.0, 0.6, 0.0, 0.6, 1.629, 0.0, 0.0, 0.0, 1.729] ) - q_surge = self.get_parameter("q_surge").value - q_pitch = self.get_parameter("q_pitch").value - q_yaw = self.get_parameter("q_yaw").value + self.lqr_params.q_surge = self.get_parameter("LQR_params.q_surge").value + self.lqr_params.q_pitch = self.get_parameter("LQR_params.q_pitch").value + self.lqr_params.q_yaw = self.get_parameter("LQR_params.q_yaw").value - r_surge = self.get_parameter("r_surge").value - r_pitch = self.get_parameter("r_pitch").value - r_yaw = self.get_parameter("r_yaw").value + self.lqr_params.r_surge = self.get_parameter("LQR_params.r_surge").value + self.lqr_params.r_pitch = self.get_parameter("LQR_params.r_pitch").value + self.lqr_params.r_yaw = self.get_parameter("LQR_params.r_yaw").value - i_surge = self.get_parameter("i_surge").value - i_pitch = self.get_parameter("i_pitch").value - i_yaw = self.get_parameter("i_yaw").value + self.lqr_params.i_surge = self.get_parameter("LQR_params.i_surge").value + self.lqr_params.i_pitch = self.get_parameter("LQR_params.i_pitch").value + self.lqr_params.i_yaw = self.get_parameter("LQR_params.i_yaw").value - i_weight = self.get_parameter("i_weight").value - max_force = self.get_parameter("max_force").value + self.lqr_params.i_weight = self.get_parameter("LQR_params.i_weight").value + self.lqr_params.max_force = self.get_parameter("max_force").value inertia_matrix_flat = self.get_parameter("inertia_matrix").value inertia_matrix = np.array(inertia_matrix_flat).reshape((3, 3)) - parameters = [ - q_surge, - q_pitch, - q_yaw, - r_surge, - r_pitch, - r_yaw, - i_surge, - i_pitch, - i_yaw, - i_weight, - max_force, - ] + return inertia_matrix - return parameters, inertia_matrix - - # ---------------------------------------------------------------Callback Functions--------------------------------------------------------------- + # ---------------------------------------------------------------CALLBACK FUNCTIONS--------------------------------------------------------------- def odom_callback(self, msg: Odometry): + """Callback function for the odometry data. + + Parameters: Odometry: msg: The odometry data from the AUV. + + """ _, self.states.yaw, self.states.pitch = LQRController.quaternion_to_euler_angle( msg.pose.pose.orientation.w, msg.pose.pose.orientation.x, @@ -155,13 +150,19 @@ def odom_callback(self, msg: Odometry): ) def guidance_callback(self, msg: LOSGuidance): + """Callback function for the guidance data. + + Parameters: LOSGuidance: msg: The guidance data from the LOS guidance system. + + """ self.guidance_values.surge = msg.surge self.guidance_values.pitch = msg.pitch self.guidance_values.yaw = msg.yaw - # ---------------------------------------------------------------Publisher Functions-------------------------------------------------------------- + # ---------------------------------------------------------------PUBLISHER FUNCTIONS-------------------------------------------------------------- def control_loop(self): + """The control loop that calculates the input for the LQR controller.""" msg = Wrench() u = self.controller.calculate_lqr_u( @@ -174,7 +175,7 @@ def control_loop(self): self.publisherLQR.publish(msg) -# ---------------------------------------------------------------------Main Function----------------------------------------------------------------- +# ----------------------------------------------------------------------MAIN FUNCTION----------------------------------------------------------------- def main(args=None): diff --git a/control/velocity_controller_lqr/velocity_controller_lqr/velocity_controller_lqr_lib.py b/control/velocity_controller_lqr/velocity_controller_lqr/velocity_controller_lqr_lib.py index dd05c2ce..b3aab790 100644 --- a/control/velocity_controller_lqr/velocity_controller_lqr/velocity_controller_lqr_lib.py +++ b/control/velocity_controller_lqr/velocity_controller_lqr/velocity_controller_lqr_lib.py @@ -1,4 +1,3 @@ -#!/usr/bin/env python3 from dataclasses import dataclass import numpy as np @@ -11,7 +10,7 @@ class State: """Dataclass to store the state values for the LQR controller. Attributes: - surge: float: Surge state value + surge: float Surge state value pitch: float: Pitch state value yaw: float: Yaw state value integral_surge: float: Surge integral state value @@ -48,8 +47,39 @@ class GuidanceValues: integral_yaw: float = 0.0 +@dataclass +class LQRParameters: + """Dataclass to store the parameters for the LQR controller. + + Attributes: + q_surge: float: Surge state weight + q_pitch: float: Pitch state weight + q_yaw: float: Yaw state weight + r_surge: float: Surge input weight + r_pitch: float: Pitch input weight + r_yaw: float: Yaw input weight + i_surge: float: Surge integral weight + i_pitch: float: Pitch integral weight + i_yaw: float: Yaw integral weight + i_weight: float: Integral weight + max_force: float: Maximum force + """ + + q_surge: float = 0.0 + q_pitch: float = 0.0 + q_yaw: float = 0.0 + r_surge: float = 0.0 + r_pitch: float = 0.0 + r_yaw: float = 0.0 + i_surge: float = 0.0 + i_pitch: float = 0.0 + i_yaw: float = 0.0 + i_weight: float = 0.0 + max_force: float = 0.0 + + class LQRController: - def __init__(self, parameters, inertia_matrix): + def __init__(self, parameters: LQRParameters, inertia_matrix: np.array) -> None: self.set_params(parameters) self.set_matrices(inertia_matrix) @@ -57,7 +87,7 @@ def __init__(self, parameters, inertia_matrix): def quaternion_to_euler_angle(w: float, x: float, y: float, z: float) -> tuple: """Function to convert quaternion to euler angles. - Args: + Parameters: w: float: w component of quaternion x: float: x component of quaternion y: float: y component of quaternion @@ -90,7 +120,7 @@ def quaternion_to_euler_angle(w: float, x: float, y: float, z: float) -> tuple: def ssa(angle: float) -> float: """Function to convert the angle to the smallest signed angle. - Args: + Parameters: angle: float: angle in radians Returns: @@ -102,7 +132,7 @@ def ssa(angle: float) -> float: def saturate(self, value: float, windup: bool, limit: float) -> tuple: """Function to saturate the value within the limits, and set the windup flag. - Args: + Parameters: value: float: value to be saturated windup: bool: windup variable limit: float: limit for saturation @@ -125,7 +155,7 @@ def anti_windup( ) -> float: """Function to saturate the integral value within the limits. - Args: + Parameters: k_i : float: integral gain error: float: error value integral_sum: float: integral sum @@ -137,9 +167,7 @@ def anti_windup( value: float: saturated value """ - if windup: - integral_sum += 0.0 - else: + if not windup: integral_sum += k_i * error return integral_sum @@ -168,7 +196,13 @@ def calculate_coriolis_matrix( ] ) - def set_params(self, parameters): + def set_params(self, parameters: LQRParameters) -> None: + """Sets the parameters for the LQR controller. + + Parameters: + parameters: LQRParameters: dataclass containing the parameters for the LQR controller + + """ self.integral_error_surge = 0.0 self.integral_error_pitch = 0.0 self.integral_error_yaw = 0.0 @@ -177,21 +211,26 @@ def set_params(self, parameters): self.pitch_windup = False self.yaw_windup = False - self.q_surge = parameters[0] - self.q_pitch = parameters[1] - self.q_yaw = parameters[2] + self.q_surge = parameters.q_surge + self.q_pitch = parameters.q_pitch + self.q_yaw = parameters.q_yaw + + self.r_surge = parameters.r_surge + self.r_pitch = parameters.r_pitch + self.r_yaw = parameters.r_yaw - self.r_surge = parameters[3] - self.r_pitch = parameters[4] - self.r_yaw = parameters[5] + self.i_surge = parameters.i_surge + self.i_pitch = parameters.i_pitch + self.i_yaw = parameters.i_yaw + self.i_weight = parameters.i_weight + self.max_force = parameters.max_force - self.i_surge = parameters[6] - self.i_pitch = parameters[7] - self.i_yaw = parameters[8] - self.i_weight = parameters[9] - self.max_force = parameters[10] + def set_matrices(self, inertia_matrix: np.array) -> None: + """Adjusts the matrices for the LQR controller. - def set_matrices(self, inertia_matrix): + Parameters: + inertia_matrix: np.array: 3x3 inertia matrix + """ self.inertia_matrix_inv = np.linalg.inv(inertia_matrix) self.state_weight_matrix = np.block( [ @@ -206,6 +245,11 @@ def set_matrices(self, inertia_matrix): self.input_weight_matrix = np.diag([self.r_surge, self.r_pitch, self.r_yaw]) def update_augmented_matrices(self, coriolis_matrix: np.array) -> None: + """Updates the augmented matrices for the LQR controller. + + Parameters: + coriolis_matrix: np.array: 3x3 Coriolis Matrix + """ system_matrix = self.inertia_matrix_inv @ coriolis_matrix input_matrix = self.inertia_matrix_inv @@ -217,7 +261,7 @@ def update_augmented_matrices(self, coriolis_matrix: np.array) -> None: def update_error(self, guidance_values: GuidanceValues, states: State) -> np.array: """Updates the error values for the LQR controller. - Args: + Parameters: guidance_values: GuidanceValues: 6x1 dataclass containing the guidance values states: State: 6x1 dataclass containing the state values @@ -256,7 +300,7 @@ def update_error(self, guidance_values: GuidanceValues, states: State) -> np.arr def saturate_input(self, u: np.array) -> np.array: """Saturates the control input within the limits, and sets the windup flag. - Args: + Parameters: u: np.array: 3x1 control input Returns: @@ -300,9 +344,6 @@ def calculate_lqr_u( return u -# ---------------------------------------------------------------Main Function--------------------------------------------------------------- - - # .--------------. # .---' o . `---. # .-' . O . . `-. From 3adf035fb471fea236e98b58454e19063f21f5dd Mon Sep 17 00:00:00 2001 From: Cyprian Date: Mon, 6 Jan 2025 14:56:49 +0100 Subject: [PATCH 13/17] now pipeline should be green --- .../tests/test_velocity_controller_lqr.py | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/control/velocity_controller_lqr/tests/test_velocity_controller_lqr.py b/control/velocity_controller_lqr/tests/test_velocity_controller_lqr.py index 3eedce10..1e5f651d 100644 --- a/control/velocity_controller_lqr/tests/test_velocity_controller_lqr.py +++ b/control/velocity_controller_lqr/tests/test_velocity_controller_lqr.py @@ -1,10 +1,12 @@ import numpy as np -from velocity_controller_lqr.velocity_controller_lqr_lib import LQRController - -controller = LQRController( - [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]) +from velocity_controller_lqr.velocity_controller_lqr_lib import ( + LQRController, + LQRParameters, ) +lqr_params = LQRParameters() +controller = LQRController(lqr_params, np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]])) + class TestVelocityController: def test_placeholder(self): From 819fc53ae42909d86bf1371dda63b7f741265838 Mon Sep 17 00:00:00 2001 From: Cyprian Date: Tue, 7 Jan 2025 13:04:57 +0100 Subject: [PATCH 14/17] Made anders happy, implemented all his ditry wishes. --- .../config/param_velocity_controller_lqr.yaml | 5 +- control/velocity_controller_lqr/package.xml | 1 + .../scripts/velocity_controller_lqr_node.py | 132 ++++++++++++++++-- .../velocity_controller_lqr_lib.py | 25 +++- 4 files changed, 142 insertions(+), 21 deletions(-) diff --git a/control/velocity_controller_lqr/config/param_velocity_controller_lqr.yaml b/control/velocity_controller_lqr/config/param_velocity_controller_lqr.yaml index c57dfb5f..d1db393e 100644 --- a/control/velocity_controller_lqr/config/param_velocity_controller_lqr.yaml +++ b/control/velocity_controller_lqr/config/param_velocity_controller_lqr.yaml @@ -3,9 +3,12 @@ velocity_controller_lqr_node: dt: 0.1 topics: - odom_topic: /nucleus/odom + odom_topic: /orca/odom + twist_topic: /dvl/twist + pose_topic: /dvl/pose guidance_topic: /guidance/los thrust_topic: /thrust/wrench_input + softwareoperation_topic: /softwareOperationMode LQR_params: q_surge: 75 diff --git a/control/velocity_controller_lqr/package.xml b/control/velocity_controller_lqr/package.xml index 252dc84f..a27ed477 100644 --- a/control/velocity_controller_lqr/package.xml +++ b/control/velocity_controller_lqr/package.xml @@ -14,6 +14,7 @@ nav_msgs vortex_msgs python-control-pip + std_msgs ament_cmake_pytest diff --git a/control/velocity_controller_lqr/scripts/velocity_controller_lqr_node.py b/control/velocity_controller_lqr/scripts/velocity_controller_lqr_node.py index f7c7ff08..97884a76 100644 --- a/control/velocity_controller_lqr/scripts/velocity_controller_lqr_node.py +++ b/control/velocity_controller_lqr/scripts/velocity_controller_lqr_node.py @@ -1,11 +1,15 @@ #!/usr/bin/env python3 import numpy as np import rclpy -from geometry_msgs.msg import Wrench -from nav_msgs.msg import Odometry +from geometry_msgs.msg import ( + PoseWithCovarianceStamped, + TwistWithCovarianceStamped, + Wrench, +) from rclpy.executors import MultiThreadedExecutor from rclpy.node import Node from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy +from std_msgs.msg import String from velocity_controller_lqr.velocity_controller_lqr_lib import ( GuidanceValues, LQRController, @@ -19,7 +23,14 @@ class LinearQuadraticRegulator(Node): def __init__(self): super().__init__("velocity_controller_lqr_node") - odom_topic, guidance_topic, thrust_topic = self.get_topics() + ( + odom_topic, + pose_topic, + twist_topic, + guidance_topic, + thrust_topic, + softwareoperation_topic, + ) = self.get_topics() best_effort_qos = QoSProfile( reliability=ReliabilityPolicy.BEST_EFFORT, @@ -34,9 +45,31 @@ def __init__(self): ) # ---------------------------- SUBSCRIBERS --------------------------- - self.nucleus_subscriber = self.create_subscription( - Odometry, odom_topic, self.odom_callback, qos_profile=best_effort_qos + # self.nucleus_subscriber = self.create_subscription( + # Odometry, odom_topic, self.odom_callback, qos_profile=best_effort_qos + # ) + + self.pose_subscriber = self.create_subscription( + PoseWithCovarianceStamped, + pose_topic, + self.pose_callback, + qos_profile=best_effort_qos, + ) + + self.twist_subscriber = self.create_subscription( + TwistWithCovarianceStamped, + twist_topic, + self.twist_callback, + qos_profile=best_effort_qos, + ) + + self.operationmode_subscriber = self.create_subscription( + String, + softwareoperation_topic, + self.operation_callback, + qos_profile=best_effort_qos, ) + self.guidance_subscriber = self.create_subscription( LOSGuidance, guidance_topic, @@ -62,23 +95,45 @@ def __init__(self): self.states = State() self.guidance_values = GuidanceValues() + self.guidance_values.surge = 0.2 + self.guidance_values.pitch = np.pi / 4 + def get_topics(self): """Get the topics from the parameter server. Returns: odom_topic: str: The topic for accessing the odometry data from the parameter file + twist_topic: str: The topic for accessing the twist data from the parameter file + pose_topic: str: The topic for accessing the pose data from the parameter file guidance_topic: str: The topic for accessing the guidance data the parameter file thrust_topic: str: The topic for accessing the thrust data from the parameter file """ self.declare_parameter("topics.odom_topic", "/nucleus/odom") + self.declare_parameter("topics.pose_topic", "/dvl/pose") + self.declare_parameter("topics.twist_topic", "/dvl/twist") self.declare_parameter("topics.guidance_topic", "/guidance/los") self.declare_parameter("topics.thrust_topic", "/thrust/wrench_input") + self.declare_parameter( + "topics.softwareoperation_topic", "/softwareOperationMode" + ) odom_topic = self.get_parameter("topics.odom_topic").value + pose_topic = self.get_parameter("topics.pose_topic").value + twist_topic = self.get_parameter("topics.twist_topic").value guidance_topic = self.get_parameter("topics.guidance_topic").value thrust_topic = self.get_parameter("topics.thrust_topic").value - - return odom_topic, guidance_topic, thrust_topic + softwareoperation_topic = self.get_parameter( + "topics.softwareoperation_topic" + ).value + + return ( + odom_topic, + pose_topic, + twist_topic, + guidance_topic, + thrust_topic, + softwareoperation_topic, + ) def get_parameters(self): """Updates the LQR_params in the LQR_parameters Dataclass, and gets the inertia matrix from config. @@ -127,19 +182,62 @@ def get_parameters(self): # ---------------------------------------------------------------CALLBACK FUNCTIONS--------------------------------------------------------------- - def odom_callback(self, msg: Odometry): - """Callback function for the odometry data. + # def odom_callback(self, msg: Odometry): + # """Callback function for the odometry data. - Parameters: Odometry: msg: The odometry data from the AUV. + # Parameters: Odometry: msg: The odometry data from the AUV. + + # """ + # _, self.states.pitch, self.states.yaw = LQRController.quaternion_to_euler_angle( + # msg.pose.pose.orientation.w, + # msg.pose.pose.orientation.x, + # msg.pose.pose.orientation.y, + # msg.pose.pose.orientation.z, + # ) + + # self.states.surge = msg.twist.twist.linear.x + + # self.coriolis_matrix = LQRController.calculate_coriolis_matrix( + # msg.twist.twist.angular.y, + # msg.twist.twist.angular.z, + # msg.twist.twist.linear.y, + # msg.twist.twist.linear.z, + # ) + + # ----------------------------------------------------DVL CALLBACK (TEMP WHILE TALHA FIXES ODOM)--------------------------------------------------- + + def pose_callback(self, msg: PoseWithCovarianceStamped): + """Callback function for the pose data from DVL. + + Parameters: msg: PoseWithCovarianceStamped The pose data from the DVL. """ - _, self.states.yaw, self.states.pitch = LQRController.quaternion_to_euler_angle( + _, self.states.pitch, self.states.yaw = LQRController.quaternion_to_euler_angle( msg.pose.pose.orientation.w, msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, ) + if self.controller.operation_mode == "autonomous mode": + self.get_logger().info( + f"roll: {_} pitch: {self.states.pitch}, yaw: {self.states.yaw}" + ) + + def operation_callback(self, msg: String): + """Callback function for the operation mode data. + + Parameters: String: msg: The operation mode data from the AUV. + + """ + self.controller.operation_mode = msg.data + + def twist_callback(self, msg: TwistWithCovarianceStamped): + """Callback function for the Twist data from DVL. + + Parameters: msg: TwistWithCovarianceStamped The twist data from the DVL. + + """ self.states.surge = msg.twist.twist.linear.x self.coriolis_matrix = LQRController.calculate_coriolis_matrix( @@ -149,6 +247,9 @@ def odom_callback(self, msg: Odometry): msg.twist.twist.linear.z, ) + if self.controller.operation_mode == "autonomous mode": + self.get_logger().info(f"twist_callback: {msg.twist.twist.linear.x}") + def guidance_callback(self, msg: LOSGuidance): """Callback function for the guidance data. @@ -159,7 +260,7 @@ def guidance_callback(self, msg: LOSGuidance): self.guidance_values.pitch = msg.pitch self.guidance_values.yaw = msg.yaw - # ---------------------------------------------------------------PUBLISHER FUNCTIONS-------------------------------------------------------------- + # ---------------------------------------------------------------PUBLISHER FUNCTIONS------------------------------------------------------------- def control_loop(self): """The control loop that calculates the input for the LQR controller.""" @@ -172,10 +273,13 @@ def control_loop(self): msg.torque.y = float(u[1]) msg.torque.z = float(u[2]) - self.publisherLQR.publish(msg) + if self.controller.operation_mode == "autonomous mode": + self.publisherLQR.publish(msg) + else: + self.controller.reset_controller() -# ----------------------------------------------------------------------MAIN FUNCTION----------------------------------------------------------------- +# ----------------------------------------------------------------------MAIN FUNCTION---------------------------------------------------------------- def main(args=None): diff --git a/control/velocity_controller_lqr/velocity_controller_lqr/velocity_controller_lqr_lib.py b/control/velocity_controller_lqr/velocity_controller_lqr/velocity_controller_lqr_lib.py index b3aab790..fabb5cd1 100644 --- a/control/velocity_controller_lqr/velocity_controller_lqr/velocity_controller_lqr_lib.py +++ b/control/velocity_controller_lqr/velocity_controller_lqr/velocity_controller_lqr_lib.py @@ -76,6 +76,7 @@ class LQRParameters: i_yaw: float = 0.0 i_weight: float = 0.0 max_force: float = 0.0 + operation_mode: str = "xbox mode" class LQRController: @@ -143,7 +144,7 @@ def saturate(self, value: float, windup: bool, limit: float) -> tuple: """ if abs(value) > limit: windup = True - value = limit * value / abs(value) + value = limit * (value / abs(value)) else: windup = False @@ -161,8 +162,6 @@ def anti_windup( integral_sum: float: integral sum windup: bool: windup variable - - Returns: value: float: saturated value @@ -225,6 +224,8 @@ def set_params(self, parameters: LQRParameters) -> None: self.i_weight = parameters.i_weight self.max_force = parameters.max_force + self.operation_mode = parameters.operation_mode + def set_matrices(self, inertia_matrix: np.array) -> None: """Adjusts the matrices for the LQR controller. @@ -271,8 +272,8 @@ def update_error(self, guidance_values: GuidanceValues, states: State) -> np.arr surge_error = ( guidance_values.surge - states.surge ) # Surge error isn't an angle, no need for angle wrapping - pitch_error = self.ssa(guidance_values.yaw - states.yaw) - yaw_error = self.ssa(guidance_values.pitch - states.pitch) + pitch_error = self.ssa(guidance_values.pitch - states.pitch) + yaw_error = self.ssa(guidance_values.yaw - states.yaw) # Update the running integrator sums self.integral_error_surge = self.anti_windup( @@ -313,6 +314,7 @@ def saturate_input(self, u: np.array) -> np.array: u[1], self.pitch_windup, self.max_force ) self.yaw_windup, torque_z = self.saturate(u[2], self.yaw_windup, self.max_force) + return [force_x, torque_y, torque_z] def calculate_lqr_u( @@ -340,9 +342,20 @@ def calculate_lqr_u( state_error = self.update_error(guidance_values, states) - u = self.saturate_input(-lqr_gain @ state_error) + u = self.saturate_input(lqr_gain @ state_error) + u = -lqr_gain @ state_error return u + def reset_controller(self) -> None: + """Resets the controller to the initial state.""" + self.integral_error_surge = 0.0 + self.integral_error_pitch = 0.0 + self.integral_error_yaw = 0.0 + + self.surge_windup = False + self.pitch_windup = False + self.yaw_windup = False + # .--------------. # .---' o . `---. From 08747e50b6598e161a86399fbf9e6106da1ff1b8 Mon Sep 17 00:00:00 2001 From: Cyprian Date: Tue, 7 Jan 2025 15:58:27 +0100 Subject: [PATCH 15/17] fixed anders' saturate mistake --- .../scripts/velocity_controller_lqr_node.py | 48 ++----------------- .../velocity_controller_lqr_lib.py | 5 +- 2 files changed, 7 insertions(+), 46 deletions(-) diff --git a/control/velocity_controller_lqr/scripts/velocity_controller_lqr_node.py b/control/velocity_controller_lqr/scripts/velocity_controller_lqr_node.py index 97884a76..88926134 100644 --- a/control/velocity_controller_lqr/scripts/velocity_controller_lqr_node.py +++ b/control/velocity_controller_lqr/scripts/velocity_controller_lqr_node.py @@ -9,7 +9,7 @@ from rclpy.executors import MultiThreadedExecutor from rclpy.node import Node from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy -from std_msgs.msg import String +from std_msgs.msg import Float32MultiArray, String from velocity_controller_lqr.velocity_controller_lqr_lib import ( GuidanceValues, LQRController, @@ -24,7 +24,6 @@ def __init__(self): super().__init__("velocity_controller_lqr_node") ( - odom_topic, pose_topic, twist_topic, guidance_topic, @@ -45,9 +44,6 @@ def __init__(self): ) # ---------------------------- SUBSCRIBERS --------------------------- - # self.nucleus_subscriber = self.create_subscription( - # Odometry, odom_topic, self.odom_callback, qos_profile=best_effort_qos - # ) self.pose_subscriber = self.create_subscription( PoseWithCovarianceStamped, @@ -79,6 +75,9 @@ def __init__(self): # ---------------------------- PUBLISHERS ---------------------------- self.publisherLQR = self.create_publisher(Wrench, thrust_topic, reliable_qos) + self.publisherStates = self.create_publisher( + Float32MultiArray, "velocity/state", reliable_qos + ) # ------------------------------ TIMERS ------------------------------ self.declare_parameter("dt", 0.1) @@ -95,9 +94,6 @@ def __init__(self): self.states = State() self.guidance_values = GuidanceValues() - self.guidance_values.surge = 0.2 - self.guidance_values.pitch = np.pi / 4 - def get_topics(self): """Get the topics from the parameter server. @@ -108,7 +104,6 @@ def get_topics(self): guidance_topic: str: The topic for accessing the guidance data the parameter file thrust_topic: str: The topic for accessing the thrust data from the parameter file """ - self.declare_parameter("topics.odom_topic", "/nucleus/odom") self.declare_parameter("topics.pose_topic", "/dvl/pose") self.declare_parameter("topics.twist_topic", "/dvl/twist") self.declare_parameter("topics.guidance_topic", "/guidance/los") @@ -117,7 +112,6 @@ def get_topics(self): "topics.softwareoperation_topic", "/softwareOperationMode" ) - odom_topic = self.get_parameter("topics.odom_topic").value pose_topic = self.get_parameter("topics.pose_topic").value twist_topic = self.get_parameter("topics.twist_topic").value guidance_topic = self.get_parameter("topics.guidance_topic").value @@ -127,7 +121,6 @@ def get_topics(self): ).value return ( - odom_topic, pose_topic, twist_topic, guidance_topic, @@ -182,30 +175,6 @@ def get_parameters(self): # ---------------------------------------------------------------CALLBACK FUNCTIONS--------------------------------------------------------------- - # def odom_callback(self, msg: Odometry): - # """Callback function for the odometry data. - - # Parameters: Odometry: msg: The odometry data from the AUV. - - # """ - # _, self.states.pitch, self.states.yaw = LQRController.quaternion_to_euler_angle( - # msg.pose.pose.orientation.w, - # msg.pose.pose.orientation.x, - # msg.pose.pose.orientation.y, - # msg.pose.pose.orientation.z, - # ) - - # self.states.surge = msg.twist.twist.linear.x - - # self.coriolis_matrix = LQRController.calculate_coriolis_matrix( - # msg.twist.twist.angular.y, - # msg.twist.twist.angular.z, - # msg.twist.twist.linear.y, - # msg.twist.twist.linear.z, - # ) - - # ----------------------------------------------------DVL CALLBACK (TEMP WHILE TALHA FIXES ODOM)--------------------------------------------------- - def pose_callback(self, msg: PoseWithCovarianceStamped): """Callback function for the pose data from DVL. @@ -219,11 +188,6 @@ def pose_callback(self, msg: PoseWithCovarianceStamped): msg.pose.pose.orientation.z, ) - if self.controller.operation_mode == "autonomous mode": - self.get_logger().info( - f"roll: {_} pitch: {self.states.pitch}, yaw: {self.states.yaw}" - ) - def operation_callback(self, msg: String): """Callback function for the operation mode data. @@ -247,9 +211,6 @@ def twist_callback(self, msg: TwistWithCovarianceStamped): msg.twist.twist.linear.z, ) - if self.controller.operation_mode == "autonomous mode": - self.get_logger().info(f"twist_callback: {msg.twist.twist.linear.x}") - def guidance_callback(self, msg: LOSGuidance): """Callback function for the guidance data. @@ -275,6 +236,7 @@ def control_loop(self): if self.controller.operation_mode == "autonomous mode": self.publisherLQR.publish(msg) + else: self.controller.reset_controller() diff --git a/control/velocity_controller_lqr/velocity_controller_lqr/velocity_controller_lqr_lib.py b/control/velocity_controller_lqr/velocity_controller_lqr/velocity_controller_lqr_lib.py index fabb5cd1..c46146d7 100644 --- a/control/velocity_controller_lqr/velocity_controller_lqr/velocity_controller_lqr_lib.py +++ b/control/velocity_controller_lqr/velocity_controller_lqr/velocity_controller_lqr_lib.py @@ -148,7 +148,7 @@ def saturate(self, value: float, windup: bool, limit: float) -> tuple: else: windup = False - return value, windup + return windup, value @staticmethod def anti_windup( @@ -341,9 +341,8 @@ def calculate_lqr_u( ) state_error = self.update_error(guidance_values, states) + u = self.saturate_input(-lqr_gain @ state_error) - u = self.saturate_input(lqr_gain @ state_error) - u = -lqr_gain @ state_error return u def reset_controller(self) -> None: From 7554010d774b1c25bcb6d290586a2879938e2e63 Mon Sep 17 00:00:00 2001 From: Cyprian Date: Tue, 7 Jan 2025 16:33:12 +0100 Subject: [PATCH 16/17] fixed test --- .../tests/test_velocity_controller_lqr.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/control/velocity_controller_lqr/tests/test_velocity_controller_lqr.py b/control/velocity_controller_lqr/tests/test_velocity_controller_lqr.py index 1e5f651d..2a7ce4b8 100644 --- a/control/velocity_controller_lqr/tests/test_velocity_controller_lqr.py +++ b/control/velocity_controller_lqr/tests/test_velocity_controller_lqr.py @@ -35,22 +35,22 @@ def test_saturate(self): print("Commencing saturate test: \n") # Test case 1: Saturation occurs - saturated_value, windup = controller.saturate(10, False, 5) + windup, saturated_value = controller.saturate(10, False, 5) assert saturated_value == 5.0 assert windup == True # Test case 2: Saturation occurs with negative limit - saturated_value, windup = controller.saturate(-10, False, 5) + windup, saturated_value = controller.saturate(-10, False, 5) assert saturated_value == -5.0 assert windup == True # Test case 3: No saturation - saturated_value, windup = controller.saturate(3, True, 5) + windup, saturated_value = controller.saturate(3, True, 5) assert saturated_value == 3.0 assert windup == False # Test case 4: No saturation with negative value - saturated_value, windup = controller.saturate(-3, True, 5) + windup, saturated_value = controller.saturate(-3, True, 5) assert saturated_value == -3.0 assert windup == False From 73663fc243ff603eeb576f60ae762873547ab8f6 Mon Sep 17 00:00:00 2001 From: Cyprian Date: Wed, 8 Jan 2025 13:46:23 +0100 Subject: [PATCH 17/17] the comitt to comitt them all --- .../config/param_velocity_controller_lqr.yaml | 1 + .../scripts/__init__.py | 0 .../scripts/velocity_controller_lqr_node.py | 36 ++++++++++++++----- .../velocity_controller_lqr_lib.py | 2 ++ 4 files changed, 31 insertions(+), 8 deletions(-) delete mode 100644 control/velocity_controller_lqr/scripts/__init__.py diff --git a/control/velocity_controller_lqr/config/param_velocity_controller_lqr.yaml b/control/velocity_controller_lqr/config/param_velocity_controller_lqr.yaml index d1db393e..8208a93b 100644 --- a/control/velocity_controller_lqr/config/param_velocity_controller_lqr.yaml +++ b/control/velocity_controller_lqr/config/param_velocity_controller_lqr.yaml @@ -9,6 +9,7 @@ velocity_controller_lqr_node: guidance_topic: /guidance/los thrust_topic: /thrust/wrench_input softwareoperation_topic: /softwareOperationMode + killswitch_topic: /softwareKillSwitch LQR_params: q_surge: 75 diff --git a/control/velocity_controller_lqr/scripts/__init__.py b/control/velocity_controller_lqr/scripts/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/control/velocity_controller_lqr/scripts/velocity_controller_lqr_node.py b/control/velocity_controller_lqr/scripts/velocity_controller_lqr_node.py index 88926134..a5314207 100644 --- a/control/velocity_controller_lqr/scripts/velocity_controller_lqr_node.py +++ b/control/velocity_controller_lqr/scripts/velocity_controller_lqr_node.py @@ -9,7 +9,7 @@ from rclpy.executors import MultiThreadedExecutor from rclpy.node import Node from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy -from std_msgs.msg import Float32MultiArray, String +from std_msgs.msg import Bool, String from velocity_controller_lqr.velocity_controller_lqr_lib import ( GuidanceValues, LQRController, @@ -29,6 +29,7 @@ def __init__(self): guidance_topic, thrust_topic, softwareoperation_topic, + killswitch_topic, ) = self.get_topics() best_effort_qos = QoSProfile( @@ -65,6 +66,12 @@ def __init__(self): self.operation_callback, qos_profile=best_effort_qos, ) + self.killswitch_subscriber = self.create_subscription( + Bool, + killswitch_topic, + self.killswitch_callback, + qos_profile=best_effort_qos, + ) self.guidance_subscriber = self.create_subscription( LOSGuidance, @@ -75,9 +82,6 @@ def __init__(self): # ---------------------------- PUBLISHERS ---------------------------- self.publisherLQR = self.create_publisher(Wrench, thrust_topic, reliable_qos) - self.publisherStates = self.create_publisher( - Float32MultiArray, "velocity/state", reliable_qos - ) # ------------------------------ TIMERS ------------------------------ self.declare_parameter("dt", 0.1) @@ -111,6 +115,7 @@ def get_topics(self): self.declare_parameter( "topics.softwareoperation_topic", "/softwareOperationMode" ) + self.declare_parameter("topics.killswitch_topic", "/softwareKillSwitch") pose_topic = self.get_parameter("topics.pose_topic").value twist_topic = self.get_parameter("topics.twist_topic").value @@ -119,6 +124,7 @@ def get_topics(self): softwareoperation_topic = self.get_parameter( "topics.softwareoperation_topic" ).value + killswitch_topic = self.get_parameter("topics.killswitch_topic").value return ( pose_topic, @@ -126,6 +132,7 @@ def get_topics(self): guidance_topic, thrust_topic, softwareoperation_topic, + killswitch_topic, ) def get_parameters(self): @@ -221,6 +228,18 @@ def guidance_callback(self, msg: LOSGuidance): self.guidance_values.pitch = msg.pitch self.guidance_values.yaw = msg.yaw + def killswitch_callback(self, msg: Bool): + """Callback function for the killswitch data. + + Parameters: String: msg: The killswitch data from the AUV. + + """ + if msg.data == True: + self.controller.reset_controller() + self.controller.killswitch = True + else: + self.controller.killswitch = False + # ---------------------------------------------------------------PUBLISHER FUNCTIONS------------------------------------------------------------- def control_loop(self): @@ -234,11 +253,12 @@ def control_loop(self): msg.torque.y = float(u[1]) msg.torque.z = float(u[2]) - if self.controller.operation_mode == "autonomous mode": - self.publisherLQR.publish(msg) + if self.controller.killswitch == False: + if self.controller.operation_mode == "autonomous mode": + self.publisherLQR.publish(msg) - else: - self.controller.reset_controller() + else: + self.controller.reset_controller() # ----------------------------------------------------------------------MAIN FUNCTION---------------------------------------------------------------- diff --git a/control/velocity_controller_lqr/velocity_controller_lqr/velocity_controller_lqr_lib.py b/control/velocity_controller_lqr/velocity_controller_lqr/velocity_controller_lqr_lib.py index c46146d7..af0528ee 100644 --- a/control/velocity_controller_lqr/velocity_controller_lqr/velocity_controller_lqr_lib.py +++ b/control/velocity_controller_lqr/velocity_controller_lqr/velocity_controller_lqr_lib.py @@ -77,6 +77,7 @@ class LQRParameters: i_weight: float = 0.0 max_force: float = 0.0 operation_mode: str = "xbox mode" + killswitch: bool = True class LQRController: @@ -225,6 +226,7 @@ def set_params(self, parameters: LQRParameters) -> None: self.max_force = parameters.max_force self.operation_mode = parameters.operation_mode + self.killswitch = parameters.killswitch def set_matrices(self, inertia_matrix: np.array) -> None: """Adjusts the matrices for the LQR controller.