diff --git a/asv_setup/CMakeLists.txt b/asv_setup/CMakeLists.txt index 8a67f88d..d57de63a 100644 --- 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 new file mode 100644 index 00000000..7ec64e1e --- /dev/null +++ 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 new file mode 100644 index 00000000..de9e0920 --- /dev/null +++ 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 new file mode 100644 index 00000000..5cbc8f0a --- /dev/null +++ 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). \ No newline at end of file diff --git a/motion/thruster_allocator/include/thruster_allocator/allocator_ros.hpp b/motion/thruster_allocator/include/thruster_allocator/allocator_ros.hpp new file mode 100644 index 00000000..22f2d5cf --- /dev/null +++ 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 +#include + +#include +#include + +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::SharedPtr publisher_; + rclcpp::Subscription::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 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 new file mode 100644 index 00000000..5c861b0b --- /dev/null +++ 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 +#include +#include +#include + +#include + +/** + * @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 +inline bool isInvalidMatrix(const Eigen::MatrixBase &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 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 &matrix, int rows, + int cols) { + return Eigen::Map>(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 new file mode 100644 index 00000000..0d94596e --- /dev/null +++ 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 + +/** + * @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 \ No newline at end of file diff --git a/motion/thruster_allocator/launch/thruster_allocator.launch.py b/motion/thruster_allocator/launch/thruster_allocator.launch.py new file mode 100644 index 00000000..3e08ca13 --- /dev/null +++ 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 new file mode 100644 index 00000000..6f3f3a38 --- /dev/null +++ b/motion/thruster_allocator/package.xml @@ -0,0 +1,24 @@ + + + + thruster_allocator + 0.0.0 + Thurster allocator for ASV + martin + MIT + + rclcpp + geometry_msgs + vortex_msgs + eigen + + ament_cmake_gtest + ament_lint_auto + ament_lint_common + + ament_cmake + + + ament_cmake + + diff --git a/motion/thruster_allocator/src/allocator_ros.cpp b/motion/thruster_allocator/src/allocator_ros.cpp new file mode 100644 index 00000000..0bf1ba68 --- /dev/null +++ b/motion/thruster_allocator/src/allocator_ros.cpp @@ -0,0 +1,95 @@ +#include "thruster_allocator/allocator_ros.hpp" +#include "thruster_allocator/allocator_utils.hpp" +#include "thruster_allocator/pseudoinverse_allocator.hpp" +#include + +#include +#include + +using namespace std::chrono_literals; + +ThrusterAllocator::ThrusterAllocator() + : Node("thruster_allocator_node"), + pseudoinverse_allocator_(Eigen::MatrixXd::Zero(3, 4)) { + declare_parameter("propulsion.dofs.num", 3); + declare_parameter("propulsion.thrusters.num", 4); + declare_parameter("propulsion.thrusters.min", -100); + declare_parameter("propulsion.thrusters.max", 100); + declare_parameter("propulsion.thrusters.direction", std::vector{0}); + declare_parameter("propulsion.thrusters.configuration_matrix", + std::vector{0}); + + num_dof_ = get_parameter("propulsion.dofs.num").as_int(); + num_thrusters_ = get_parameter("propulsion.thrusters.num").as_int(); + min_thrust_ = get_parameter("propulsion.thrusters.min").as_int(); + max_thrust_ = get_parameter("propulsion.thrusters.max").as_int(); + direction_ = + get_parameter("propulsion.thrusters.direction").as_integer_array(); + thrust_configuration = doubleArrayToEigenMatrix( + get_parameter("propulsion.thrusters.configuration_matrix") + .as_double_array(), + num_dof_, num_thrusters_); + + subscription_ = this->create_subscription( + "thrust/wrench_input", 1, + std::bind(&ThrusterAllocator::wrench_callback, this, + std::placeholders::_1)); + + publisher_ = this->create_publisher( + "thrust/thruster_forces", 1); + + timer_ = this->create_wall_timer( + 100ms, std::bind(&ThrusterAllocator::timer_callback, this)); + + Eigen::MatrixXd thrust_configuration_pseudoinverse; + calculateRightPseudoinverse(thrust_configuration, + thrust_configuration_pseudoinverse); + + pseudoinverse_allocator_.T_pinv = thrust_configuration_pseudoinverse; + + body_frame_forces_.setZero(); +} + +void ThrusterAllocator::timer_callback() { + Eigen::VectorXd thruster_forces = + pseudoinverse_allocator_.calculateAllocatedThrust(body_frame_forces_); + + if (isInvalidMatrix(thruster_forces)) { + RCLCPP_ERROR(get_logger(), "ThrusterForces vector invalid"); + return; + } + + if (!saturateVectorValues(thruster_forces, min_thrust_, max_thrust_)) { + RCLCPP_WARN(get_logger(), "Thruster forces vector required saturation."); + } + + vortex_msgs::msg::ThrusterForces msg_out; + arrayEigenToMsg(thruster_forces, msg_out); + std::transform(msg_out.thrust.begin(), msg_out.thrust.end(), + direction_.begin(), msg_out.thrust.begin(), + std::multiplies<>()); + publisher_->publish(msg_out); +} + +void ThrusterAllocator::wrench_callback(const geometry_msgs::msg::Wrench &msg) { + Eigen::Vector3d msg_vector; + msg_vector(0) = msg.force.x; // surge + msg_vector(1) = msg.force.y; // sway + msg_vector(2) = msg.torque.z; // yaw + if (!healthyWrench(msg_vector)) { + RCLCPP_ERROR(get_logger(), "ASV wrench vector invalid, ignoring."); + return; + } + std::swap(msg_vector, body_frame_forces_); +} + +bool ThrusterAllocator::healthyWrench(const Eigen::VectorXd &v) const { + if (isInvalidMatrix(v)) + return false; + + bool within_max_thrust = std::none_of(v.begin(), v.end(), [this](double val) { + return std::abs(val) > max_thrust_; + }); + + return within_max_thrust; +} diff --git a/motion/thruster_allocator/src/pseudoinverse_allocator.cpp b/motion/thruster_allocator/src/pseudoinverse_allocator.cpp new file mode 100644 index 00000000..8b553763 --- /dev/null +++ b/motion/thruster_allocator/src/pseudoinverse_allocator.cpp @@ -0,0 +1,10 @@ +#include "thruster_allocator/pseudoinverse_allocator.hpp" + +PseudoinverseAllocator::PseudoinverseAllocator(const Eigen::MatrixXd &T_pinv) + : T_pinv(T_pinv) {} + +Eigen::VectorXd +PseudoinverseAllocator::calculateAllocatedThrust(const Eigen::VectorXd &tau) { + Eigen::VectorXd u = T_pinv * tau; + return u; +} \ No newline at end of file diff --git a/motion/thruster_allocator/src/thruster_allocator_node.cpp b/motion/thruster_allocator/src/thruster_allocator_node.cpp new file mode 100644 index 00000000..77c604b7 --- /dev/null +++ b/motion/thruster_allocator/src/thruster_allocator_node.cpp @@ -0,0 +1,11 @@ +#include "thruster_allocator/allocator_ros.hpp" +#include "thruster_allocator/allocator_utils.hpp" + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + auto allocator = std::make_shared(); + RCLCPP_INFO(allocator->get_logger(), "Thruster allocator initiated"); + rclcpp::spin(allocator); + rclcpp::shutdown(); + return 0; +}