Thrust allocation ros2 (#99)

* Created Wrench publisher, force = (1,1,1) * Impl. thrust allocation, listening wrench_input * Committing clang-format changes * Committing clang-format changes * Added matrix and wrench health cheks * Committing clang-format changes * Cleanup, add launch file, thrust saturation * Committing clang-format changes * Cleanup: removed unnecessary functions and variables, added pass by reference, use ROS2 timer_callback, added doxygen documentation * Committing clang-format changes * Add runtime_error for calculateRightPseudoinverse and renamed Allocator to ThrusterAllocator * Removed TODO comment * Committing clang-format changes * Comments/doxygen cleanup, fixed healthyWrench() logic * Committing clang-format changes * Renamed node to thruster_allocator_node * Added thruster allocator tests (not working) * Committing clang-format changes * Updated freya.yaml with wildcards * Update thruster_allocator_launch.py so that it takes in params from freya.yaml * Update CMakeLists.txt with installation of config * Added get ros parameters from YAML wildcards * Committing clang-format changes * Fix: No raw loops * Committing clang-format changes * Removed non-working tests * rearranged doxygen comments * fixed include statements and removed non-existent tests from cmake * Committing clang-format changes --------- Co-authored-by: Clang Robot <[email protected]> Co-authored-by: Aldokan <[email protected]> Co-authored-by: alekskl01 <[email protected]> Co-authored-by: Aleksander Klund <[email protected]>
vortexntnu · Nov 7, 2023 · d573379 · d573379
1 parent 432f7a5
commit d573379
Show file tree

Hide file tree

Showing 12 changed files with 490 additions and 1 deletion.
diff --git a/asv_setup/CMakeLists.txt b/asv_setup/CMakeLists.txt
@@ -5,11 +5,18 @@ 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)
 
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  set(ament_cmake_copyright_FOUND TRUE)
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
 # Install launch files.
 install(DIRECTORY
+  config
   launch
   DESTINATION share/${PROJECT_NAME}/
 )

diff --git a/asv_setup/config/robots/freya.yaml b/asv_setup/config/robots/freya.yaml
@@ -0,0 +1,52 @@
+# This file defines parameters specific to Freya
+#
+# When looking at the ASV from above, the thruster placement is:   
+#    /\          /\
+#   /  \  front /  \
+#  /    \      /    \
+#  |=3↗=|      |=0↖=|
+#  |    |      |    |
+#  |    |      |    |
+#  |    |======|    |
+#  |    |      |    |
+#  |    |      |    |
+#  |    |      |    |
+#  |=2↖=|==||==|=1↗=|
+#
+/**:
+  ros__parameters:
+    physical:
+      mass_kg: 0 
+      displacement_m3: 0
+      buoyancy: 0   # kg
+      center_of_mass: [0, 0, 0]        # mm (x,y,z)
+      center_of_buoyancy: [0, 0, 0]   # mm (x,y,z)
+
+
+    propulsion:
+      dofs:
+        num: 3
+        which:
+          surge: true
+          sway:  true
+          yaw:   true
+      thrusters:
+        num: 4
+        min: -100
+        max: 100
+        configuration_matrix: #NED
+          [0.70711, 0.70711, 0.70711, 0.70711, 
+          -0.70711, 0.70711, -0.70711, 0.70711, 
+          0.27738, 0.27738, -0.27738, -0.27738]
+
+        rate_of_change:
+          max: 0 # Maximum rate of change in newton per second for a thruster
+        thruster_to_pin_map: [1, 3, 2, 0] # I.e. if thruster_to_pin = [1, 3, 2, 0] then thruster 0 is pin 1 etc..
+        direction: [1, 1, 1, 1] # Disclose during thruster mapping
+        offset: [0, 0, 0, 0] # Disclose during thruster mapping
+      command:
+        wrench:
+          max:     [150.0, 150.0, 150.0, 150.0, 150.0, 150.0]
+          scaling: [ 1.0,  1.0,  1.0, 1.0, 1.0, 1.0]
+
+    computer: "pc-debug"
diff --git a/motion/thruster_allocator/CMakeLists.txt b/motion/thruster_allocator/CMakeLists.txt
@@ -0,0 +1,50 @@
+cmake_minimum_required(VERSION 3.8)
+project(thruster_allocator)
+
+# 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_node.cpp 
+  src/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()
diff --git a/motion/thruster_allocator/README.md b/motion/thruster_allocator/README.md
@@ -0,0 +1,5 @@
+## Thruster allocator
+
+This package takes a thrust vector and calculates the corresponding thrust required from each individual thruster. The resulting calculation is published as a vortex_msgs ThrusterForces.
+
+The calculation itself is based on the 'unweighted pseudoinverse-based allocator' as described in Fossen (2011): Handbook of Marine Craft Hydrodynamics and Motion Control (chapter 12.3.2).
diff --git a/motion/thruster_allocator/include/thruster_allocator/allocator_ros.hpp b/motion/thruster_allocator/include/thruster_allocator/allocator_ros.hpp
@@ -0,0 +1,62 @@
+/**
+ * @file allocator_ros.hpp
+ * @brief ThrusterAllocator class, which
+ * allocates thrust to the ASV's thrusters based on the desired body frame
+ * forces.
+ */
+
+#ifndef VORTEX_ALLOCATOR_ALLOCATOR_ROS_HPP
+#define VORTEX_ALLOCATOR_ALLOCATOR_ROS_HPP
+
+#include "thruster_allocator/allocator_utils.hpp"
+#include "thruster_allocator/pseudoinverse_allocator.hpp"
+#include <eigen3/Eigen/Eigen>
+#include <rclcpp/rclcpp.hpp>
+
+#include <geometry_msgs/msg/wrench.hpp>
+#include <vortex_msgs/msg/thruster_forces.hpp>
+
+using namespace std::chrono_literals;
+
+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 timer_callback();
+
+private:
+  Eigen::MatrixXd thrust_configuration;
+  /**
+   * @brief Callback function for the wrench input subscription. Extracts the
+   * surge, sway 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_callback(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 healthyWrench(const Eigen::VectorXd &v) const;
+  rclcpp::Publisher<vortex_msgs::msg::ThrusterForces>::SharedPtr publisher_;
+  rclcpp::Subscription<geometry_msgs::msg::Wrench>::SharedPtr subscription_;
+  rclcpp::TimerBase::SharedPtr timer_;
+  size_t count_;
+  int num_dof_;
+  int num_thrusters_;
+  int min_thrust_;
+  int max_thrust_;
+  Eigen::Vector3d body_frame_forces_;
+  std::vector<int64_t> direction_;
+  PseudoinverseAllocator pseudoinverse_allocator_;
+};
+
+#endif // VORTEX_ALLOCATOR_ALLOCATOR_ROS_HPP
diff --git a/motion/thruster_allocator/include/thruster_allocator/allocator_utils.hpp b/motion/thruster_allocator/include/thruster_allocator/allocator_utils.hpp
@@ -0,0 +1,118 @@
+/**
+ * @file allocator_utils.hpp
+ * @brief This file contains utility functions for the thruster allocator
+ * module.
+ */
+
+#ifndef VORTEX_ALLOCATOR_ALLOCATOR_UTILS_HPP
+#define VORTEX_ALLOCATOR_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 isInvalidMatrix(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 Returns a string stream containing the matrix with the given name.
+ *
+ * @param name The name of the matrix.
+ * @param M The matrix to print.
+ * @return std::stringstream The string stream containing the matrix.
+ */
+inline std::stringstream printMatrix(std::string name,
+                                     const Eigen::MatrixXd &M) {
+  std::stringstream ss;
+  ss << std::endl << name << " = " << std::endl << M;
+  return ss;
+}
+
+/**
+ * @brief Calculates the right pseudoinverse of the given matrix.
+ *
+ * @param M The matrix to calculate the pseudoinverse of.
+ * @param M_pinv The resulting pseudoinverse matrix.
+ * @throws char* if the pseudoinverse is invalid.
+ */
+inline void calculateRightPseudoinverse(const Eigen::MatrixXd &M,
+                                        Eigen::MatrixXd &M_pinv) {
+  Eigen::MatrixXd pseudoinverse = M.transpose() * (M * M.transpose()).inverse();
+  // pseudoinverse.completeOrthogonalDecomposition().pseudoInverse();
+  if (isInvalidMatrix(pseudoinverse)) {
+    throw std::runtime_error("Invalid Psuedoinverse Calculated");
+  }
+  M_pinv = 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 saturateVectorValues(Eigen::VectorXd &vec, double min, double max) {
+  bool vector_in_range = std::all_of(vec.begin(), vec.end(), [&](double &val) {
+    if (val > max) {
+      val = max;
+      return false;
+    } else if (val < min) {
+      val = min;
+      return false;
+    }
+    return true;
+  });
+  return vector_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 arrayEigenToMsg(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
+doubleArrayToEigenMatrix(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_ALLOCATOR_UTILS_HPP
diff --git a/motion/thruster_allocator/include/thruster_allocator/pseudoinverse_allocator.hpp b/motion/thruster_allocator/include/thruster_allocator/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 calculateAllocatedThrust(const Eigen::VectorXd &tau);
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  Eigen::MatrixXd T_pinv;
+};
+
+#endif // VORTEX_ALLOCATOR_PSEUDOINVERSE_ALLOCATOR_HPP
diff --git a/motion/thruster_allocator/launch/thruster_allocator.launch.py b/motion/thruster_allocator/launch/thruster_allocator.launch.py
@@ -0,0 +1,16 @@
+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():
+    trushter_allocator_node = Node(
+            package='thruster_allocator',
+            executable='thruster_allocator_node',
+            name='thruster_allocator_node',
+            parameters=[os.path.join(get_package_share_directory('asv_setup'),'config','robots','freya.yaml')],
+            output='screen',
+        )
+    return LaunchDescription([
+        trushter_allocator_node
+    ])
diff --git a/motion/thruster_allocator/package.xml b/motion/thruster_allocator/package.xml
@@ -0,0 +1,24 @@
+<?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</name>
+  <version>0.0.0</version>
+  <description>Thurster allocator for ASV</description>
+  <maintainer email="[email protected]">martin</maintainer>
+  <license>MIT</license>
+
+  <depend>rclcpp</depend>
+  <depend>geometry_msgs</depend>
+  <depend>vortex_msgs</depend>
+  <depend>eigen</depend>
+
+  <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>