<feat> implemented Liner Quadratic Regulator

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()

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

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])

control/velocity_controller_lqr/package.xml

@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>velocity_controller_lqr</name>
+  <version>1.0.0</version>
+  <description>Velocity controller package for the AUV Orca</description>
+  <maintainer email="cyprian.github@gmail.com">cyprian</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>rclpy</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>python3-control</depend>
+  <depend>vortex_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+  <test_depend>ament_cmake_pytest</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

control/velocity_controller_lqr/scripts/velocity_controller_lqr_node.py

@@ -0,0 +1,146 @@
+#!/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 velocity_controller_lqr.velocity_controller_lqr_lib import LQRController
+from vortex_msgs.msg import LOSGuidance
+
+
+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
+        )
+        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)
+
+        # 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 = 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
+
+    # ---------------------------------------------------------------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(
+            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 = LinearQuadraticRegulator()
+    rclpy.spin(node)
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()
+
+# Anders er goated

control/velocity_controller_lqr/tests/test_velocity_controller_lqr.py

@@ -0,0 +1,70 @@
+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)
+
+
+class TestVelocityController:
+    def test_placeholder(self):
+        assert (
+            controller is not None
+        )  # Simple test to ensure the controller initializes
+
+    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, 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")
+
+    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