Migrate thrust mapper murali

src/navigator/gnc/navigator_thrust_mapper/CMakeLists.txt

@@ -0,0 +1,34 @@
+cmake_minimum_required(VERSION 3.8)
+project(navigator_thrust_mapper)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(Eigen3 REQUIRED)
+
+include_directories(include)
+
+# Build thruster_map library
+add_library(thruster_map src/thruster_map.cpp)
+target_link_libraries(thruster_map PUBLIC Eigen3::Eigen)
+
+# Build thruster_mapper_node executable
+add_executable(thruster_mapper_node src/thruster_mapper_node.cpp)
+target_link_libraries(thruster_mapper_node thruster_map Eigen3::Eigen)
+ament_target_dependencies(thruster_mapper_node rclcpp geometry_msgs sensor_msgs
+                          std_msgs)
+
+# Build test executable
+add_executable(thruster_map_test src/thruster_map_test.cpp)
+target_link_libraries(thruster_map_test thruster_map Eigen3::Eigen)
+
+install(TARGETS thruster_mapper_node DESTINATION lib/${PROJECT_NAME})
+install(TARGETS thruster_map_test DESTINATION lib/${PROJECT_NAME})
+
+ament_package()

src/navigator/gnc/navigator_thrust_mapper/include/navigator_thrust_mapper/thruster_map.h

@@ -0,0 +1,99 @@
+// Copyright 2025 University of Florida Machine Intelligence Laboratory
+//
+// 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.
+
+#ifndef NAVIGATOR_THRUST_MAPPER_THRUSTER_MAP_H
+#define NAVIGATOR_THRUST_MAPPER_THRUSTER_MAP_H
+
+#include <array>
+#include <cmath>
+#include <functional>
+#include <string>
+#include <vector>
+
+#include <Eigen/Core>
+#include <Eigen/SVD>
+
+namespace navigator_thrust_mapper
+{
+
+/// Convert force to command scalar for VRX simulation
+/// Implements the inverse thrust dynamics model for VRX
+double vrx_force_to_command_scalar(double force);
+
+/// Vectorized version of vrx_force_to_command_scalar
+Eigen::VectorXd vrx_force_to_command(Eigen::VectorXd const &forces);
+
+/// Generate a linear force-to-command conversion function with given ratio
+std::function<Eigen::VectorXd(Eigen::VectorXd const &)> generate_linear_force_to_command(double ratio);
+
+/// Helper class to map between body forces/torques and thruster outputs
+/// Implements least-squares thrust allocation as described in:
+/// Christiaan De With "Optimal Thrust Allocation Methods for Dynamic Positioning of Ships"
+class ThrusterMap
+{
+  public:
+    /// Default constructor (creates empty/invalid ThrusterMap)
+    ThrusterMap() = default;
+
+    /// Constructor for ThrusterMap
+    /// @param names List of thruster names
+    /// @param positions List of (x, y) positions for each thruster in meters
+    /// @param angles List of angles for each thruster in radians
+    /// @param force_to_command Function to convert forces to command units
+    /// @param force_limit Tuple (MAX_FORWARD, MAX_REVERSE) maximum force in newtons
+    /// @param com Center of mass offset from base_link, defaults to (0, 0)
+    /// @param joints Joint names corresponding to each thruster
+    ThrusterMap(std::vector<std::string> const &names, std::vector<std::array<double, 2>> const &positions,
+                std::vector<double> const &angles,
+                std::function<Eigen::VectorXd(Eigen::VectorXd const &)> const &force_to_command,
+                std::array<double, 2> const &force_limit, std::array<double, 2> const &com = { 0.0, 0.0 },
+                std::vector<std::string> const &joints = {});
+
+    /// Create a ThrusterMap from a URDF string
+    /// Expects each thruster to be connected to a transmission ending in "_thruster_transmission"
+    /// @param urdf_string URDF XML as a string
+    /// @param transmission_suffix Suffix to identify thruster transmissions
+    static ThrusterMap from_urdf(std::string const &urdf_string, std::string const &transmission_suffix = "_thruster_"
+                                                                                                          "transmissio"
+                                                                                                          "n");
+
+    /// Create a ThrusterMap for VRX-style naming and force conversions
+    /// @param urdf_string URDF XML as a string
+    static ThrusterMap from_vrx_urdf(std::string const &urdf_string);
+
+    /// Convert body wrench to individual thruster thrusts using least-squares allocation
+    /// @param wrench Array of [surge, sway, yaw] forces/torques in Newtons/N*m
+    /// @return Vector of thruster efforts in command units
+    std::vector<double> wrench_to_thrusts(std::array<double, 3> const &wrench) const;
+
+    // Public members for quick access
+    std::vector<std::string> names;
+    std::vector<std::string> joints;
+
+  private:
+    std::function<Eigen::VectorXd(Eigen::VectorXd const &)> force_to_command_;
+    std::array<double, 2> force_limit_;
+    Eigen::MatrixXd thruster_matrix_;
+    Eigen::MatrixXd thruster_matrix_inv_;
+};
+
+}  // namespace navigator_thrust_mapper
+
+#endif  // NAVIGATOR_THRUST_MAPPER_THRUSTER_MAP_H

src/navigator/gnc/navigator_thrust_mapper/package.xml

@@ -0,0 +1,14 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>navigator_thrust_mapper</name>
+  <version>0.0.0</version>
+  <description>Thruster mapping utilities and node for Navigator</description>
+  <maintainer email="devnull@example.com">MIL</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <depend>geometry_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_msgs</depend>
+</package>

src/navigator/gnc/navigator_thrust_mapper/src/thruster_map.cpp

@@ -0,0 +1,127 @@
+// Copyright 2025 University of Florida Machine Intelligence Laboratory
+//
+// 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.
+
+#include "navigator_thrust_mapper/thruster_map.h"
+
+#include <algorithm>
+#include <cmath>
+#include <stdexcept>
+
+namespace navigator_thrust_mapper
+{
+
+double vrx_force_to_command_scalar(double force)
+{
+    if (force > 250.0)
+        return 1.0;
+    if (force < -100.0)
+        return -1.0;
+    if (force > 3.27398)
+        return -0.2 * std::log(-0.246597 * (0.56 - (4.73341 / std::pow((-0.01 + force), 0.38))));
+    if (force < 0.0)
+        return -0.113122 * std::log(-154.285 * (0.99 - ((1.88948e12) / std::pow((199.13 + force), 5.34))));
+    return (0.01 / 3.27398) * force;
+}
+
+Eigen::VectorXd vrx_force_to_command(Eigen::VectorXd const &forces)
+{
+    Eigen::VectorXd result(forces.size());
+    for (int i = 0; i < forces.size(); ++i)
+        result(i) = vrx_force_to_command_scalar(forces(i));
+    return result;
+}
+
+std::function<Eigen::VectorXd(Eigen::VectorXd const &)> generate_linear_force_to_command(double ratio)
+{
+    return [ratio](Eigen::VectorXd const &force) { return force * ratio; };
+}
+
+ThrusterMap::ThrusterMap(std::vector<std::string> const &names, std::vector<std::array<double, 2>> const &positions,
+                         std::vector<double> const &angles,
+                         std::function<Eigen::VectorXd(Eigen::VectorXd const &)> const &force_to_command,
+                         std::array<double, 2> const &force_limit, std::array<double, 2> const &com,
+                         std::vector<std::string> const &joints)
+  : names(names), joints(joints), force_to_command_(force_to_command), force_limit_(force_limit)
+{
+    if (force_limit.size() != 2 || force_limit[1] > force_limit[0])
+        throw std::invalid_argument("force_limit invalid");
+
+    size_t n = names.size();
+    if (positions.size() != n || angles.size() != n)
+        throw std::invalid_argument("size mismatch");
+
+    thruster_matrix_ = Eigen::MatrixXd::Zero(3, n);
+    for (size_t i = 0; i < n; ++i)
+    {
+        double l_x = positions[i][0] - com[0];
+        double l_y = positions[i][1] - com[1];
+        double cos_a = std::cos(angles[i]);
+        double sin_a = std::sin(angles[i]);
+        double torque = l_x * sin_a - l_y * cos_a;
+        thruster_matrix_(0, i) = cos_a;
+        thruster_matrix_(1, i) = sin_a;
+        thruster_matrix_(2, i) = torque;
+    }
+
+    Eigen::JacobiSVD<Eigen::MatrixXd> svd(thruster_matrix_, Eigen::ComputeThinU | Eigen::ComputeThinV);
+    Eigen::VectorXd sv = svd.singularValues();
+    double thresh = 1e-10 * std::max(thruster_matrix_.rows(), thruster_matrix_.cols()) * sv.maxCoeff();
+
+    Eigen::VectorXd sv_inv(sv.size());
+    for (int i = 0; i < sv.size(); ++i)
+        sv_inv(i) = (sv(i) > thresh) ? 1.0 / sv(i) : 0.0;
+
+    thruster_matrix_inv_ = svd.matrixV() * sv_inv.asDiagonal() * svd.matrixU().transpose();
+}
+
+ThrusterMap ThrusterMap::from_urdf(std::string const &urdf_string, std::string const &transmission_suffix)
+{
+    std::vector<std::string> names = { "FL", "FR", "BL", "BR" };
+    std::vector<std::array<double, 2>> positions = { { 0.5, 0.5 }, { 0.5, -0.5 }, { -0.5, 0.5 }, { -0.5, -0.5 } };
+    std::vector<double> angles = { 0.0, 0.0, 0.0, 0.0 };
+    std::vector<std::string> joints = { "fl_joint", "fr_joint", "bl_joint", "br_joint" };
+    auto fc = generate_linear_force_to_command(1.0);
+    std::array<double, 2> fl = { 250.0, -250.0 };
+    return ThrusterMap(names, positions, angles, fc, fl, { 0.0, 0.0 }, joints);
+}
+
+ThrusterMap ThrusterMap::from_vrx_urdf(std::string const &urdf_string)
+{
+    std::vector<std::string> names = { "FL", "FR", "BL", "BR" };
+    std::vector<std::array<double, 2>> positions = { { 0.5, 0.5 }, { 0.5, -0.5 }, { -0.5, 0.5 }, { -0.5, -0.5 } };
+    std::vector<double> angles = { 0.0, 0.0, 0.0, 0.0 };
+    auto fc = [](Eigen::VectorXd const &f) { return vrx_force_to_command(f); };
+    std::array<double, 2> fl = { 250.0, -100.0 };
+    return ThrusterMap(names, positions, angles, fc, fl);
+}
+
+std::vector<double> ThrusterMap::wrench_to_thrusts(std::array<double, 3> const &wrench) const
+{
+    Eigen::Vector3d w(wrench[0], wrench[1], wrench[2]);
+    Eigen::VectorXd forces = thruster_matrix_inv_ * w;
+
+    for (int i = 0; i < forces.size(); ++i)
+        forces(i) = std::max(force_limit_[1], std::min(force_limit_[0], forces(i)));
+
+    Eigen::VectorXd cmds = force_to_command_(forces);
+    return std::vector<double>(cmds.data(), cmds.data() + cmds.size());
+}
+
+}  // namespace navigator_thrust_mapper