From a5a19406d281c8507c644d86a7676dca485dd074 Mon Sep 17 00:00:00 2001 From: Martin Huynh Date: Mon, 9 Oct 2023 17:31:28 +0200 Subject: [PATCH] Added matrix and wrench health cheks --- motion/thruster_allocator/CMakeLists.txt | 24 ++++++++++- .../thruster_allocator/allocator_ros.hpp | 12 ++++-- .../thruster_allocator/allocator_utils.hpp | 8 +++- .../thruster_allocator/src/allocator_ros.cpp | 42 ++++++++++++++++--- motion/thruster_allocator/src/main.cpp | 5 ++- .../src/pseudoinverse_allocator.cpp | 2 +- 6 files changed, 78 insertions(+), 15 deletions(-) diff --git a/motion/thruster_allocator/CMakeLists.txt b/motion/thruster_allocator/CMakeLists.txt index 8be7e00f..b59fb750 100644 --- a/motion/thruster_allocator/CMakeLists.txt +++ b/motion/thruster_allocator/CMakeLists.txt @@ -1,6 +1,11 @@ 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() @@ -11,9 +16,24 @@ find_package(rclcpp REQUIRED) find_package(vortex_msgs REQUIRED) find_package(geometry_msgs REQUIRED) -add_executable(thruster_allocator_node src/main.cpp src/allocator_ros.cpp src/pseudoinverse_allocator.cpp) +include_directories(include) + +add_executable(thruster_allocator_node + src/main.cpp + src/allocator_ros.cpp + src/pseudoinverse_allocator.cpp + ) -ament_target_dependencies(thruster_allocator_node rclcpp geometry_msgs vortex_msgs) +ament_target_dependencies(thruster_allocator_node + rclcpp + geometry_msgs + vortex_msgs + ) + +install( + DIRECTORY include/ + DESTINATION include +) install(TARGETS thruster_allocator_node diff --git a/motion/thruster_allocator/include/thruster_allocator/allocator_ros.hpp b/motion/thruster_allocator/include/thruster_allocator/allocator_ros.hpp index 7080edc6..53ea2d4a 100644 --- a/motion/thruster_allocator/include/thruster_allocator/allocator_ros.hpp +++ b/motion/thruster_allocator/include/thruster_allocator/allocator_ros.hpp @@ -13,15 +13,19 @@ using namespace std::chrono_literals; class Allocator : public rclcpp::Node { public: explicit Allocator(); + void spinOnce(); private: Eigen::MatrixXd thrust_configuration = - (Eigen::MatrixXd(3, 4) << 0.70711, 0.70711, 0.70711, 0.70711, -0.70711, - 0.70711, -0.70711, 0.70711, 0.27738, 0.27738, -0.27738, -0.27738) - .finished(); // Hardcoded thruster config matrix for T_pinv + (Eigen::MatrixXd(3, 4) << + 0.70711, 0.70711, 0.70711, 0.70711, + -0.70711, 0.70711, -0.70711, 0.70711, + 0.27738, 0.27738, -0.27738, -0.27738) + .finished(); + // Hardcoded thruster config matrix for T_pinv - void spinOnce(); void wrench_callback(const geometry_msgs::msg::Wrench &msg); + bool healthyWrench(const Eigen::VectorXd &v) const; Eigen::VectorXd wrenchMsgToEigen(const geometry_msgs::msg::Wrench &msg) const; Eigen::VectorXd wrenchMsgToEigen(const float force_x, const float force_y, const float torque) const; diff --git a/motion/thruster_allocator/include/thruster_allocator/allocator_utils.hpp b/motion/thruster_allocator/include/thruster_allocator/allocator_utils.hpp index d8965157..019a8628 100644 --- a/motion/thruster_allocator/include/thruster_allocator/allocator_utils.hpp +++ b/motion/thruster_allocator/include/thruster_allocator/allocator_utils.hpp @@ -21,12 +21,18 @@ inline void printMatrix(std::string name, const Eigen::MatrixXd &X) { RCLCPP_INFO_STREAM(rclcpp::get_logger("rclcpp"), ss.str()); } +inline void printVector(std::string name, const Eigen::VectorXd &X) { + std::stringstream ss; + ss << std::endl << name << " = " << std::endl << X; + RCLCPP_INFO_STREAM(rclcpp::get_logger("rclcpp"), ss.str()); +} + // Calculate the pseudoinverse matrix of the matrix X. // Return false if the calculations fails. inline bool calculatePseudoinverse(const Eigen::MatrixXd &X, Eigen::MatrixXd *X_pinv) { Eigen::MatrixXd pseudoinverse = X.transpose() * (X * X.transpose()).inverse(); - + if (isInvalidMatrix(pseudoinverse)) { return false; } diff --git a/motion/thruster_allocator/src/allocator_ros.cpp b/motion/thruster_allocator/src/allocator_ros.cpp index 133f49ad..61728abb 100644 --- a/motion/thruster_allocator/src/allocator_ros.cpp +++ b/motion/thruster_allocator/src/allocator_ros.cpp @@ -1,6 +1,6 @@ -#include "../include/thruster_allocator/allocator_ros.hpp" -#include "../include/thruster_allocator/allocator_utils.hpp" -#include "../include/thruster_allocator/pseudoinverse_allocator.hpp" +#include "thruster_allocator/allocator_ros.hpp" +#include "thruster_allocator/allocator_utils.hpp" +#include "thruster_allocator/pseudoinverse_allocator.hpp" #include "vortex_msgs/msg/thruster_forces.hpp" Allocator::Allocator() : Node("thrust_allocator_node") { @@ -28,26 +28,44 @@ void Allocator::spinOnce() { const Eigen::VectorXd body_frame_forces = wrenchMsgToEigen( body_frame_force_x, body_frame_force_y, body_frame_torque); + + // printMatrix("T_pinv", m_pseudoinverse_allocator->T_pinv); + // u vector Eigen::VectorXd thruster_forces = m_pseudoinverse_allocator->calculateAllocatedThrust(body_frame_forces); // TODO: Legg til isValicMatrix sjekk og clampVectorValues (saturateVector) + if(isInvalidMatrix(thruster_forces)){ + RCLCPP_ERROR(get_logger(), "ThrusterForces vector invalid"); + return; + } + vortex_msgs::msg::ThrusterForces msg_out; + //printVector("thruster_forces", thruster_forces); arrayEigenToMsg(thruster_forces, &msg_out); for (int i = 0; i < 4; i++) // 4 thrusters msg_out.thrust[i] *= m_direction[i]; + //for (double d : msg_out.thrust){ + // RCLCPP_INFO(this->get_logger(), "msg_out-thrust: '%f'", d); + //} + publisher_->publish(msg_out); } void Allocator::wrench_callback(const geometry_msgs::msg::Wrench &msg) { - // const Eigen::VectorXd body_frame_forces = wrenchMsgToEigen(msg); // Wrench + const Eigen::VectorXd body_frame_forces = wrenchMsgToEigen(msg); // Wrench // health check + if (!healthyWrench(body_frame_forces)) { + RCLCPP_ERROR(get_logger(), "ASV wrench vector invalid, ignoring."); + return; + } + body_frame_force_x = msg.force.x; body_frame_force_y = msg.force.y; body_frame_torque = msg.torque.z; - RCLCPP_INFO(this->get_logger(), "I heard: '%f'", msg.force.x); + //RCLCPP_INFO(this->get_logger(), "I heard: '%f'", msg.force.x); } Eigen::VectorXd @@ -68,6 +86,20 @@ Eigen::VectorXd Allocator::wrenchMsgToEigen(const float force_x, body_frame_forces(2) = torque; // yaw return body_frame_forces; } + + bool Allocator::healthyWrench(const Eigen::VectorXd &v) const { + // Check for NaN/Inf + if (isInvalidMatrix(v)) + return false; + + // Check reasonableness + for (unsigned i = 0; i < v.size(); ++i) + if (std::abs(v[i]) > 100) //c_force_range_limit + return false; + + return true; + } + // void Allocator::timer_callback() { // auto message = geometry_msgs::msg::Wrench(); // message.force.x = 1; diff --git a/motion/thruster_allocator/src/main.cpp b/motion/thruster_allocator/src/main.cpp index 6a8905d9..5fa35176 100644 --- a/motion/thruster_allocator/src/main.cpp +++ b/motion/thruster_allocator/src/main.cpp @@ -1,5 +1,5 @@ -#include "../include/thruster_allocator/allocator_ros.hpp" -#include "../include/thruster_allocator/allocator_utils.hpp" +#include "thruster_allocator/allocator_ros.hpp" +#include "thruster_allocator/allocator_utils.hpp" int main(int argc, char **argv) { @@ -8,6 +8,7 @@ int main(int argc, char **argv) { rclcpp::Rate loop_rate(10); while (rclcpp::ok()) { + allocator->spinOnce(); rclcpp::spin_some(allocator); loop_rate.sleep(); } diff --git a/motion/thruster_allocator/src/pseudoinverse_allocator.cpp b/motion/thruster_allocator/src/pseudoinverse_allocator.cpp index 896df6e7..8b553763 100644 --- a/motion/thruster_allocator/src/pseudoinverse_allocator.cpp +++ b/motion/thruster_allocator/src/pseudoinverse_allocator.cpp @@ -1,4 +1,4 @@ -#include "../include/thruster_allocator/pseudoinverse_allocator.hpp" +#include "thruster_allocator/pseudoinverse_allocator.hpp" PseudoinverseAllocator::PseudoinverseAllocator(const Eigen::MatrixXd &T_pinv) : T_pinv(T_pinv) {}