<feat> implemented Liner Quadratic Regulator

.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'
+      download_requirements: true

control/velocity_controller_lqr/CMakeLists.txt

@@ -0,0 +1,40 @@
+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
+  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,29 @@
+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
+    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

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",
+        name="velocity_controller_lqr_node",
+        output="screen",
+        parameters=[parameter_file],
+    )
+
+    return LaunchDescription([velocity_controller_node])

control/velocity_controller_lqr/package.xml

@@ -0,0 +1,25 @@
+<?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="[email protected]">cyprian</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclpy</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>vortex_msgs</depend>
+  <depend>python-control-pip</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,189 @@
+#!/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_topic", "/nucleus/odom")
+        self.declare_parameter("guidance_topic", "/guidance/los")
+        self.declare_parameter("thrust_topic", "/thrust/wrench_input")
+
+        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(
+            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.control_loop)
+
+        # ROS2 SHENNANIGANS with parameters
+        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)
+
+        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 = 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]."
+
+        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(
+            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.coriolis_matrix = 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 control_loop(self):  # The LQR controller function
+        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])
+
+        # 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⣤⣥⣩⣶⣟⣻⠧⣻⠶⢤⢰⡱⣬⣤⣌⣑⠞⠲⠓⠭⡀⠀⠀⠀
+# ⠀⠐⣿⣿⣿⢟⡋⢈⢤⣤⣷⢿⣿⠒⢜⣁⡱⡧⢿⣹⣷⣿⡿⣷⠌⣢⣟⢱⢽⡨⢊⠴⡉⢉⡿⣯⢿⣏⢹⠏⣯⣩⣙⠾⢿⣳⣶⢻⣟⣿⠧⢀⠋⠟⣿⡧⠠⠄⡤⠈⢠⠀⠀
+# ⠀⠀⠘⠻⠿⠶⠶⠿⠛⣹⣟⡞⠸⣬⠐⠙⢍⢉⣔⠪⠟⡛⠫⢵⣾⣣⣼⣽⢈⠔⡅⣜⡽⢯⢞⡕⡠⠓⣡⢚⣷⣷⣿⣳⡄⠢⠉⠛⢿⣲⢿⣶⢿⣬⣾⣛⠳⣼⡮⠳⡂⠒⠀
+# ⠀⠀⠀⠀⠀⠀⠀⠀⢠⠏⡁⢉⣀⣑⣆⡐⠊⣅⡕⢦⣀⣱⡶⢫⣨⢟⠽⠕⣇⢶⣵⣋⢝⣉⣋⠜⠉⠉⡯⠛⣿⣿⣿⣾⣳⠠⠤⠪⠁⠊⠉⠻⣟⣾⣿⣿⣟⣧⣧⢸⠂⠠⢠
+# ⠀⠀⠀⠀⠀⠀⠀⣠⣾⢞⢉⠿⢿⣟⡓⠖⢾⡏⢠⣾⣿⠛⣰⠾⢓⡵⣺⢺⣼⡫⢪⡱⣉⠷⢗⡀⠤⠆⡻⣛⠿⣻⣿⢶⣊⡄⠀⠀⠀⠀⠄⢀⠀⠉⠿⣿⡿⣿⣛⡁⢍⣀⡌
+# ⠀⠀⠀⠀⠀⠀⣠⣛⢓⠗⠀⠀⠠⣈⠉⢀⢬⣵⡿⢋⣴⣞⣵⣼⣭⢁⠕⢿⢋⣞⢟⣕⡩⣔⠃⠀⠀⡀⣛⣤⢿⣷⢻⣿⣿⣽⣮⡙⠆⠀⠤⢓⡙⣆⠀⠀⠘⠙⢯⣛⣶⠋⠁
+# ⠀⠀⠀⠀⠀⢠⢋⢿⣼⣶⣶⣤⠒⢉⠠⣪⣮⠟⣡⡾⠹⡿⣿⣿⠝⢊⣐⢺⡜⣫⡞⢭⡕⠋⣐⠒⠀⠡⠏⠉⠘⠛⠚⡻⣯⠋⠛⢅⠐⢄⠀⣸⡃⠛⠀⡀⡀⠀⠈⠙⡟⠀⠀
+# ⠀⠀⠀⠀⣠⢫⠎⠙⠿⣿⠛⡡⠔⠕⣴⡿⡁⡮⡷⡶⢟⣿⢎⡵⡠⢞⠱⢈⣼⠁⠄⠇⡄⣢⠒⠀⡎⠀⡇⠒⠐⠐⠐⢚⠐⢷⣔⢖⢊⡈⠒⠗⠠⠘⠈⡁⢈⣠⣤⠾⠀⠀⠀
+# ⠀⠀⠀⣰⢳⠏⢀⢔⢤⠶⠪⣠⣭⣾⣿⠗⢘⣷⣼⠛⠛⢛⡝⣜⢑⣤⣾⠿⣿⣿⢽⣿⠿⠶⢴⣯⣄⡄⣇⣀⣀⡀⠄⠠⠆⣀⡨⢽⣵⣕⣄⣀⣰⣥⣶⣾⡿⠟⠉⠀⠀⠀⠀
+# ⠀⠀⡰⣱⢋⠴⣩⠔⠻⣴⡾⢷⣿⡿⠃⠰⢿⣿⣿⣿⣶⣬⣧⣼⡿⠛⠁⡢⠒⠈⢈⡉⠿⠚⢼⣿⣿⣿⡆⠋⠉⠢⠀⢀⣀⣡⣴⡶⣿⣿⣿⠿⠻⠛⠋⠁⠀⠀⠀⠀⠀⠀⠀
+# ⠀⡼⣳⣯⣱⣪⣲⣫⠻⣯⢟⠽⠋⠀⠀⠀⠀⠈⠙⠻⢻⡳⡩⢇⢀⠸⢢⣤⣴⣁⡀⠊⡀⠠⠂⢉⣫⡭⣁⣀⣠⣴⣶⢿⣿⣿⣿⡿⠞⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
+# ⣼⣟⡿⣿⣿⢝⣫⣶⡿⠣⠉⠀⠀⠀⠀⠀⠀⠀⣠⣖⠕⡩⢂⢕⠡⠒⠸⣿⣿⣿⡿⢂⢀⠀⣜⡿⣵⣶⣾⣿⡿⠯⠟⠋⠉⠋⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
+# ⢸⢿⣷⣷⣷⣿⠛⠋⠁⠀⠀⠀⠀⠀⠀⢀⣴⡺⠟⣑⣿⡺⢑⡴⠂⠊⠀⢀⡁⣍⣢⣼⣺⣽⣶⡿⠿⠏⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
+# ⠀⠈⠙⠻⠝⠀⠀⠀⠀⠀⠀⠀⠀⠀⣰⡿⡋⢰⠕⠋⡿⠉⢁⡈⢕⣲⣼⣒⣯⣷⣿⠿⠟⠋⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
+# ⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣀⣴⣿⣷⣧⡎⣠⡤⠥⣰⢬⣵⣮⣽⣿⡿⠟⠛⠉⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
+# ⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣷⣮⣟⡯⣓⣦⣿⣮⣿⣿⣿⠿⠛⠋⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
+# ⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠛⠿⣿⣿⣿⣿⡿⠟⠛⠉⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
+# ⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠈⠛⠉⠀⠀