402 task auv thrust allocation

auv_setup/config/robots/new_auv.yaml

@@ -1,4 +1,4 @@
-# This file defines parameters specific to Beluga
+# This file defines parameters specific to (Insert Name of New AUV).
 #
 # When looking at the AUV from above, the thruster placement is:   
 #
@@ -12,4 +12,22 @@
 #  | 5• |      | 2• |
 #  |    |      |    |
 #  |=4↖=|==||==|=3↗=|
-#
+#
+
+/**:
+  ros__parameters:
+    propulsion:
+      dofs:
+        num: 6
+      thrusters:
+        num: 8
+        min: -100
+        max: 100
+        configuration_matrix: #NED
+          [ 0.70711,  0.00000,  0.00000,  0.70711,  0.70711,  0.00000,  0.00000,  0.70711,  # Surge
+           -0.70711,  0.00000,  0.00000,  0.70711, -0.70711,  0.00000,  0.00000,  0.70711,  # Sway
+            0.00000,  1.00000,  1.00000,  0.00000,  0.00000,  1.00000,  1.00000,  0.00000,  # Heave
+
+            0.01414,  0.15000,  0.15000, -0.01414,  0.01414, -0.15000, -0.15000, -0.01414,  # Roll
+            0.01414, -0.31000,  0.37500,  0.01414,  0.01414,  0.31000, -0.31000,  0.01414,  # Pitch 
+           -0.41720,  0.00000,  0.00000, -0.44901,  0.44901,  0.00000,  0.00000,  0.41720 ] # Yaw

motion/thruster_allocator_auv/CMakeLists.txt

@@ -0,0 +1,50 @@
+cmake_minimum_required(VERSION 3.8)
+project(thruster_allocator_auv)
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+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(rclcpp REQUIRED)
+find_package(vortex_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(Eigen3 REQUIRED)
+
+include_directories(include)
+
+add_executable(${PROJECT_NAME}_node 
+  src/thruster_allocator_auv_node.cpp 
+  src/thruster_allocator_ros.cpp 
+  src/pseudoinverse_allocator.cpp
+)
+
+ament_target_dependencies(${PROJECT_NAME}_node 
+  rclcpp 
+  geometry_msgs 
+  vortex_msgs
+  Eigen3
+)
+
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+
+install(TARGETS
+  ${PROJECT_NAME}_node
+  DESTINATION lib/${PROJECT_NAME})
+
+# Install launch files.
+install(DIRECTORY
+  launch
+  DESTINATION share/${PROJECT_NAME}/
+)
+
+ament_package()

motion/thruster_allocator_auv/LICENSE

@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2024 Vortex NTNU
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.

motion/thruster_allocator_auv/include/thruster_allocator_auv/eigen_vector6d_typedef.hpp

@@ -0,0 +1,15 @@
+/**
+ * @file eigen_vector6d_typedef.hpp
+ * @brief Contains the typedef for a 6x1 Eigen vector.
+ */
+
+#ifndef VORTEX_EIGEN_TYPEDEFS_H
+#define VORTEX_EIGEN_TYPEDEFS_H
+
+#include <eigen3/Eigen/Dense>
+
+namespace Eigen {
+typedef Eigen::Matrix<double, 6, 1> Vector6d;
+}
+
+#endif // VORTEX_EIGEN_TYPEDEFS_H

motion/thruster_allocator_auv/include/thruster_allocator_auv/pseudoinverse_allocator.hpp

@@ -0,0 +1,39 @@
+/**
+ * @file pseudoinverse_allocator.hpp
+ * @brief Contains the PseudoinverseAllocator class, which implements the
+ * unweighted pseudoinverse-based allocator described in e.g. Fossen 2011
+ * Handbook of Marine Craft Hydrodynamics and Motion Control (chapter 12.3.2).
+ */
+
+#ifndef VORTEX_ALLOCATOR_PSEUDOINVERSE_ALLOCATOR_HPP
+#define VORTEX_ALLOCATOR_PSEUDOINVERSE_ALLOCATOR_HPP
+
+#include <eigen3/Eigen/Eigen>
+
+/**
+ * @brief The PseudoinverseAllocator class calculates the allocated thrust given
+ * the input torques using the pseudoinverse allocator.
+ */
+class PseudoinverseAllocator {
+public:
+  /**
+   * @brief Constructor for the PseudoinverseAllocator class.
+   *
+   * @param T_pinv The pseudoinverse of the thruster configuration matrix.
+   */
+  explicit PseudoinverseAllocator(const Eigen::MatrixXd &T_pinv);
+
+  /**
+   * @brief Calculates the allocated thrust given the input torques using the
+   * pseudoinverse allocator.
+   *
+   * @param tau The input torques as a vector.
+   * @return The allocated thrust as a vector.
+   */
+  Eigen::VectorXd calculate_allocated_thrust(const Eigen::VectorXd &tau);
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  Eigen::MatrixXd T_pinv;
+};
+
+#endif // VORTEX_ALLOCATOR_PSEUDOINVERSE_ALLOCATOR_HPP

motion/thruster_allocator_auv/include/thruster_allocator_auv/thruster_allocator_ros.hpp

@@ -0,0 +1,61 @@
+/**
+ * @file thruster_allocator_ros.hpp
+ * @brief Contains the ROS logic for the thruster allocator node.
+ */
+
+#ifndef VORTEX_ALLOCATOR_ROS_HPP
+#define VORTEX_ALLOCATOR_ROS_HPP
+
+#include "thruster_allocator_auv/eigen_vector6d_typedef.hpp"
+#include "thruster_allocator_auv/pseudoinverse_allocator.hpp"
+#include "thruster_allocator_auv/thruster_allocator_utils.hpp"
+#include <eigen3/Eigen/Eigen>
+#include <geometry_msgs/msg/wrench.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <vortex_msgs/msg/thruster_forces.hpp>
+
+class ThrusterAllocator : public rclcpp::Node {
+public:
+  explicit ThrusterAllocator();
+
+  /**
+   * @brief Calculates the allocated
+   * thrust based on the body frame forces. It then saturates the output vector
+   * between min and max values and publishes the thruster forces to the topic
+   * "thrust/thruster_forces".
+   */
+  void calculate_thrust_timer_cb();
+
+private:
+  Eigen::MatrixXd thrust_configuration;
+
+  /**
+   * @brief Callback function for the wrench input subscription. Extracts the
+   * surge, sway, heave, roll, pitch and yaw values from the received wrench msg
+   * and stores them in the body_frame_forces_ Eigen vector.
+   * @param msg The received geometry_msgs::msg::Wrench message.
+   */
+  void wrench_cb(const geometry_msgs::msg::Wrench &msg);
+
+  /**
+   * @brief Checks if the given Eigen vector contains any NaN or Inf values
+   * @param v The Eigen vector to check.
+   * @return True if the vector is healthy, false otherwise.
+   */
+  bool healthy_wrench(const Eigen::VectorXd &v) const;
+
+  rclcpp::Publisher<vortex_msgs::msg::ThrusterForces>::SharedPtr
+      thruster_forces_publisher_;
+  rclcpp::Subscription<geometry_msgs::msg::Wrench>::SharedPtr
+      wrench_subscriber_;
+  rclcpp::TimerBase::SharedPtr calculate_thrust_timer_;
+  size_t count_;
+  int num_dof_;
+  int num_thrusters_;
+  int min_thrust_;
+  int max_thrust_;
+  Eigen::Vector6d body_frame_forces_;
+  PseudoinverseAllocator pseudoinverse_allocator_;
+};
+
+#endif // VORTEX_ALLOCATOR_ROS_HPP

motion/thruster_allocator_auv/include/thruster_allocator_auv/thruster_allocator_utils.hpp

@@ -0,0 +1,101 @@
+/**
+ * @file thruster_allocator_utils.hpp
+ * @brief This file contains utility functions for the thruster allocator
+ * module.
+ */
+
+#ifndef VORTEX_ALLOCATOR_UTILS_HPP
+#define VORTEX_ALLOCATOR_UTILS_HPP
+
+#include <eigen3/Eigen/Eigen>
+#include <rclcpp/rclcpp.hpp>
+#include <string>
+#include <vector>
+
+#include <vortex_msgs/msg/thruster_forces.hpp>
+
+/**
+ * @brief Check if the matrix has any NaN or INF elements.
+ *
+ * @tparam Derived The type of the matrix.
+ * @param M The matrix to check.
+ * @return true if the matrix has any NaN or INF elements, false otherwise.
+ */
+template <typename Derived>
+inline bool is_invalid_matrix(const Eigen::MatrixBase<Derived> &M) {
+  bool has_nan = !(M.array() == M.array()).all();
+  bool has_inf = M.array().isInf().any();
+  return has_nan || has_inf;
+}
+
+/**
+ * @brief Calculates the right pseudoinverse of the given matrix.
+ *
+ * @param M The matrix to calculate the pseudoinverse of.
+ * @throws char* if the pseudoinverse is invalid.
+ * @return The pseudoinverse of the given matrix.
+ */
+inline Eigen::MatrixXd calculate_right_pseudoinverse(const Eigen::MatrixXd &T) {
+  Eigen::MatrixXd pseudoinverse = T.transpose() * (T * T.transpose()).inverse();
+  if (is_invalid_matrix(pseudoinverse)) {
+    throw std::runtime_error("Invalid Psuedoinverse Calculated");
+  }
+  return pseudoinverse;
+}
+
+/**
+ * @brief Saturates the values of a given Eigen vector between a minimum and
+ * maximum value.
+ *
+ * @param vec The Eigen vector to be saturated.
+ * @param min The minimum value to saturate the vector values to.
+ * @param max The maximum value to saturate the vector values to.
+ * @return True if all vector values are within the given range, false
+ * otherwise.
+ */
+inline bool saturate_vector_values(Eigen::VectorXd &vec, double min,
+                                   double max) {
+  bool all_values_in_range =
+      std::all_of(vec.begin(), vec.end(),
+                  [min, max](double val) { return val >= min && val <= max; });
+
+  std::transform(vec.begin(), vec.end(), vec.begin(), [min, max](double val) {
+    return std::min(std::max(val, min), max);
+  });
+
+  return all_values_in_range;
+}
+
+/**
+ * @brief Converts an Eigen VectorXd to a vortex_msgs::msg::ThrusterForces
+ * message.
+ *
+ * @param u The Eigen VectorXd to be converted.
+ * @param msg The vortex_msgs::msg::ThrusterForces message to store the
+ * converted values.
+ */
+inline void array_eigen_to_msg(const Eigen::VectorXd &u,
+                               vortex_msgs::msg::ThrusterForces &msg) {
+  int r = u.size();
+  std::vector<double> u_vec(r);
+  std::copy_n(u.begin(), r, u_vec.begin());
+  msg.thrust = u_vec;
+}
+
+/**
+ * @brief Converts a 1D array of doubles to a 2D Eigen matrix.
+ *
+ * @param matrix The 1D array of doubles to be converted.
+ * @param rows The number of rows in the resulting Eigen matrix.
+ * @param cols The number of columns in the resulting Eigen matrix.
+ * @return The resulting Eigen matrix.
+ */
+inline Eigen::MatrixXd
+double_array_to_eigen_matrix(const std::vector<double> &matrix, int rows,
+                             int cols) {
+  return Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                        Eigen::RowMajor>>(matrix.data(), rows,
+                                                          cols);
+}
+
+#endif // VORTEX_ALLOCATOR_UTILS_HPP

motion/thruster_allocator_auv/launch/thruster_allocator_auv.launch.py

@@ -0,0 +1,18 @@
+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():
+    thruster_allocator_auv_node = Node(
+        package='thruster_allocator_auv',
+        executable='thruster_allocator_auv_node',
+        name='thruster_allocator_auv_node',
+        parameters=[
+            os.path.join(get_package_share_directory('auv_setup'), 'config',
+                         'robots', 'new_auv.yaml')
+        ],
+        output='screen',
+    )
+    return LaunchDescription([thruster_allocator_auv_node])

motion/thruster_allocator_auv/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>thruster_allocator_auv</name>
+  <version>0.0.0</version>
+  <description>Thruster allocator for AUV</description>
+  <maintainer email="[email protected]">alice</maintainer>
+  <license>MIT</license>
+
+  <depend>rclcpp</depend>
+  <depend>geometry_msgs</depend>
+  <depend>vortex_msgs</depend>
+  <depend>eigen</depend>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

motion/thruster_allocator_auv/src/pseudoinverse_allocator.cpp

@@ -0,0 +1,10 @@
+#include "thruster_allocator_auv/pseudoinverse_allocator.hpp"
+
+PseudoinverseAllocator::PseudoinverseAllocator(const Eigen::MatrixXd &T_pinv)
+    : T_pinv(T_pinv) {}
+
+Eigen::VectorXd
+PseudoinverseAllocator::calculate_allocated_thrust(const Eigen::VectorXd &tau) {
+  Eigen::VectorXd u = T_pinv * tau;
+  return u;
+}

motion/thruster_allocator_auv/src/thruster_allocator_auv_node.cpp

@@ -0,0 +1,11 @@
+#include "thruster_allocator_auv/thruster_allocator_ros.hpp"
+#include "thruster_allocator_auv/thruster_allocator_utils.hpp"
+
+int main(int argc, char **argv) {
+  rclcpp::init(argc, argv);
+  auto allocator = std::make_shared<ThrusterAllocator>();
+  RCLCPP_INFO(allocator->get_logger(), "Thruster allocator initiated");
+  rclcpp::spin(allocator);
+  rclcpp::shutdown();
+  return 0;
+}