diff --git a/.github/workflows/source-build.yaml b/.github/workflows/source-build.yaml index 802f5363..7b73ca40 100644 --- a/.github/workflows/source-build.yaml +++ b/.github/workflows/source-build.yaml @@ -12,5 +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' + dependency_script: 'requirements.sh' diff --git a/control/velocity_controller_lqr/CMakeLists.txt b/control/velocity_controller_lqr/CMakeLists.txt new file mode 100644 index 00000000..05f93727 --- /dev/null +++ b/control/velocity_controller_lqr/CMakeLists.txt @@ -0,0 +1,37 @@ +cmake_minimum_required(VERSION 3.8) +project(velocity_controller_lqr) + +find_package(ament_cmake_python REQUIRED) +find_package(rclpy REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(vortex_msgs REQUIRED) + +install(DIRECTORY + launch + config + DESTINATION share/${PROJECT_NAME} +) + +ament_python_install_package(${PROJECT_NAME}) + +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/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 new file mode 100644 index 00000000..d21b1739 --- /dev/null +++ b/control/velocity_controller_lqr/config/param_velocity_controller_lqr.yaml @@ -0,0 +1,28 @@ +velocity_controller_lqr_node: + ros__parameters: + + #topic parameters + odom_topic: /nucleus/odom + guidance_topic: /guidance/los + thrust_topic: /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 + + i_weight: 0.5 + + 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 new file mode 100644 index 00000000..716bfdc9 --- /dev/null +++ b/control/velocity_controller_lqr/launch/velocity_controller_lqr.launch.py @@ -0,0 +1,33 @@ +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. + """ + parameter_file = os.path.join( + get_package_share_directory("velocity_controller_lqr"), + "config", + "param_velocity_controller_lqr.yaml", + ) + + velocity_controller_node = Node( + package="velocity_controller_lqr", + executable="velocity_controller_lqr_node.py", + 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..252dc84f --- /dev/null +++ b/control/velocity_controller_lqr/package.xml @@ -0,0 +1,23 @@ + + + + velocity_controller_lqr + 1.0.0 + Velocity controller package for the AUV Orca + cyprian + MIT + + ament_cmake_python + + rclpy + geometry_msgs + nav_msgs + vortex_msgs + python-control-pip + + ament_cmake_pytest + + + ament_cmake + + 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 new file mode 100644 index 00000000..a616acf3 --- /dev/null +++ b/control/velocity_controller_lqr/scripts/velocity_controller_lqr_node.py @@ -0,0 +1,211 @@ +#!/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 rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy +from velocity_controller_lqr.velocity_controller_lqr_lib import ( + GuidanceValues, + LQRController, + State, +) +from vortex_msgs.msg import LOSGuidance + + +class LinearQuadraticRegulator(Node): + def __init__(self): + super().__init__("velocity_controller_lqr_node") + + odom_topic, guidance_topic, thrust_topic = self.get_topics() + + best_effort_qos = QoSProfile( + reliability=ReliabilityPolicy.BEST_EFFORT, + history=HistoryPolicy.KEEP_LAST, + depth=1, + ) + + reliable_qos = QoSProfile( + reliability=ReliabilityPolicy.RELIABLE, + history=HistoryPolicy.KEEP_LAST, + depth=10, + ) + + # ---------------------------- SUBSCRIBERS --------------------------- + self.nucleus_subscriber = self.create_subscription( + Odometry, odom_topic, self.odom_callback, qos_profile=best_effort_qos + ) + self.guidance_subscriber = self.create_subscription( + LOSGuidance, + guidance_topic, + self.guidance_callback, + qos_profile=best_effort_qos, + ) + + # ---------------------------- PUBLISHERS ---------------------------- + self.publisherLQR = self.create_publisher(Wrench, thrust_topic, reliable_qos) + + # ------------------------------ TIMERS ------------------------------ + self.control_timer = self.create_timer(0.1, self.control_loop) + + # ------------------ ROS2 PARAMETERS AND CONTROLLER ------------------ + parameters, inertia_matrix = self.get_parameters() + self.controller = LQRController(parameters, inertia_matrix) + + # ---------------- 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) + + 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) + + 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 + + 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 + + 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 = [ + 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 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.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, + ) + + 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-------------------------------------------------------------- + + def control_loop(self): + msg = Wrench() + + 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]) + + self.publisherLQR.publish(msg) + + +# ---------------------------------------------------------------------Main Function----------------------------------------------------------------- + + +def main(args=None): + rclpy.init(args=args) + node = LinearQuadraticRegulator() + rclpy.spin(node) + rclpy.shutdown() + + +if __name__ == "__main__": + main() + +# ⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀ +# ⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⣀⣠⢴⣤⣠⣤⢤⣂⣔⣲⣒⣖⡺⢯⣝⡿⣿⣿⣿⣷⣶⣶⣢⢦⣤⣄⣀⣀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀ +# ⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣰⣯⣿⣾⣿⣶⣺⣯⡿⣓⣽⢞⡸⣻⢏⣋⠌⣛⣭⣿⢟⣿⣛⣿⢷⣿⣿⣿⡟⣿⣻⣵⣲⢢⣄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀ +# ⠀⠀⠀⠀⠀⠀⢀⣀⣤⡴⠲⠶⢦⠤⢤⡤⠿⠿⠿⠿⣿⣽⣿⣽⣷⣿⢽⣾⣷⣭⡞⣩⡐⠏⡋⣽⡬⣭⠏⢍⣞⢿⣽⣿⣷⢿⣿⣿⡿⠾⣿⢶⡶⣤⣀⠀⠀⠀⠀⠀⠀⠀⠀ +# ⠀⠀⠀⠀⣤⣖⠯⡙⢢⡁⠄⢢⡤⢠⢀⣸⠀⣄⡠⣀⣀⣦⡄⣠⢀⡃⠽⣽⠬⠍⣿⣿⣤⣥⣷⣿⣿⣿⣾⡍⠛⢌⠈⢫⣍⢋⣍⡁⠹⢍⠈⣳⢎⠴⠟⠻⢧⣄⠀⠀⠀⠀⠀ +# ⠀⠀⣠⣾⣿⣿⣿⣯⡔⠆⠠⠈⣿⣿⠾⡾⠿⣶⠿⡟⣻⡛⣭⢙⠍⢩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 new file mode 100644 index 00000000..3eedce10 --- /dev/null +++ b/control/velocity_controller_lqr/tests/test_velocity_controller_lqr.py @@ -0,0 +1,71 @@ +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]]) +) + + +class TestVelocityController: + def test_placeholder(self): + assert controller is not None + + def test_ssa(self): + print("Commencing ssa test: \n") + + assert LQRController.ssa(np.pi + 1) == -np.pi + 1 + + assert LQRController.ssa(-np.pi - 1) == (np.pi - 1) + + print("SSA test passed") + + def test_quaternion_to_euler_angle(self): + print("Commencing quaternion to euler angle test: \n") + + 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") + + # 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 + saturated_value, windup = 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) + assert saturated_value == 3.0 + assert windup == 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 + + 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_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/__init__.py b/control/velocity_controller_lqr/velocity_controller_lqr/__init__.py new file mode 100644 index 00000000..e69de29b 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..c4255f42 --- /dev/null +++ b/control/velocity_controller_lqr/velocity_controller_lqr/velocity_controller_lqr_lib.py @@ -0,0 +1,336 @@ +#!/usr/bin/env python3 +from dataclasses import dataclass + +import numpy as np + +import control as ct + + +@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 + + +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: + """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: + phi: float: roll angle + theta: float: pitch angle + psi: float: yaw angle + + """ + y_square = y * y + + t0 = +2.0 * (w * x + y * z) + t1 = +1.0 - 2.0 * (x * x + y_square) + 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 + theta = np.arcsin(t2) + + t3 = +2.0 * (w * z + x * y) + t4 = +1.0 - 2.0 * (y_square + z * z) + psi = np.arctan2(t3, t4) + + return phi, theta, psi + + @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 + + """ + 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. + + Args: + value: float: value to be saturated + windup: bool: windup variable + limit: float: limit for saturation + + Returns: + (value, windup): tuple: saturated value and windup flag + + """ + if abs(value) > limit: + windup = True + value = limit * value / abs(value) + else: + windup = False + + 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. + + Args: + k_i : float: integral gain + error: float: error value + integral_sum: float: integral sum + windup: bool: windup variable + + + + Returns: + value: float: saturated value + + """ + if windup: + integral_sum += 0.0 + else: + integral_sum += k_i * error + + return integral_sum + + @staticmethod + def calculate_coriolis_matrix( + 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_vel: float: sway velocity in m/s + heave_vel: float: heave velocity in m/s + + Returns: + C: np.array: 3x3 Coriolis Matrix + + """ + return np.array( + [ + [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 set_params(self, parameters): + 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 + + 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 + + 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 + + def update_error(self, guidance_values: GuidanceValues, states: State) -> np.array: + """Updates the error values for the LQR controller. + + 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.surge - states.surge + ) # Surge error 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 + 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, + ] + ) + return state_error + + def saturate_input(self, u: np.array) -> np.array: + """Saturates the control input within the limits, and sets the windup flag. + + 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 + ) + 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) + 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, + ) + + state_error = self.update_error(guidance_values, states) + + u = self.saturate_input(-lqr_gain @ state_error) + return u + + +# ---------------------------------------------------------------Main Function--------------------------------------------------------------- + + +# .--------------. +# .---' o . `---. +# .-' . O . . `-. +# .-' @@@@@@ . `-. +# .'@@ @@@@@@@@@@@ @@@@@@@ . `. +# .'@@@ @@@@@@@@@@@@@@ @@@@@@@@@ `. +# /@@@ o @@@@@@@@@@@@@@ @@@@@@@@@ O \ +# / @@@@@@@@@@@@@@ @ @@@@@@@@@ @@ . \ +# /@ o @@@@@@@@@@@ . @@ @@@@@@@@@@@ @@ \ +# /@@@ . @@@@@@ o @ @@@@@@@@@@@@@ o @@@@ \ +# /@@@@@ @ . @@@@@@@@@@@@@@ @@@@@ \ +# |@@@@@ O `.-./ . . @@@@@@@@@@@@@ @@@ | +# / @@@@@ --`-' o @@@@@@@@@@@ @@@ . \ +# |@ @@@@ . @ @ ` @ @@ . @@@@@@ | +# | @@ o @@ . @@@@@@ | +# | . @ @ @ o @@ o @@@@@@. | +# \ @ @ @ .-. @@@@ @@@ / +# | @ @ @ `-' . @@@@ . . | +# \ . o @ @@@@ . @@ . . / +# \ @@@ @@@@@@ . o / +# \ @@@@@ @@\@@ / O . / +# \ o @@@ \ \ / __ . . .--. / +# \ . . \.-.--- `--' / +# `. `-' . .' +# `. o / | ` O . .' +# `-. / | o .-' +# `-. . . .-' +# `---. . .---' +# `--------------' diff --git a/requirements.sh b/requirements.sh new file mode 100644 index 00000000..376146dd --- /dev/null +++ b/requirements.sh @@ -0,0 +1,21 @@ +#!/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 +pip3 install "numpy<1.25.0" + +echo "Finished installing extra dependencies."