<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'
+      dependency_script: 'requirements.sh'

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

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.

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

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

control/velocity_controller_lqr/package.xml

@@ -0,0 +1,23 @@
+<?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_python</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_cmake_pytest</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

control/velocity_controller_lqr/requirements.txt

@@ -0,0 +1 @@
+numpy<1.25.0

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