diff --git a/src/subjugator/gnc/subjugator_thruster_manager/CMakeLists.txt b/src/subjugator/gnc/subjugator_thruster_manager/CMakeLists.txt index f4d45aa9e..1fa499ff2 100644 --- a/src/subjugator/gnc/subjugator_thruster_manager/CMakeLists.txt +++ b/src/subjugator/gnc/subjugator_thruster_manager/CMakeLists.txt @@ -25,11 +25,27 @@ target_include_directories(thruster_manager PRIVATE ${EIGEN3_INCLUDE_DIR} add_executable(forward_to_sim src/forward_to_sim.cpp) ament_target_dependencies(forward_to_sim rclcpp subjugator_msgs) +target_include_directories(forward_to_sim PRIVATE include) install(TARGETS thruster_manager forward_to_sim DESTINATION lib/${PROJECT_NAME}) install(DIRECTORY config launch DESTINATION share/${PROJECT_NAME}) -install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) +# include/ already contains the subjugator_thruster_manager/ subdir, so install +# its contents straight into include/ (not include/${PROJECT_NAME}, which would +# double-nest the path for downstream consumers of the exported header). +install(DIRECTORY include/ DESTINATION include) + +# Export the headers (lut.h thrust curve) so other packages (e.g. the Gazebo +# thruster plugin) can share the same force<->effort model. +ament_export_include_directories(include) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + # Unit tests for the force<->effort thrust curve (lut.h): endpoints, clamping, + # monotonicity, round-trip inversion, and forward/reverse asymmetry. + ament_add_gtest(test_lut test/test_lut.cpp) + target_include_directories(test_lut PRIVATE include) +endif() ament_package() diff --git a/src/subjugator/gnc/subjugator_thruster_manager/README.md b/src/subjugator/gnc/subjugator_thruster_manager/README.md index 658ec1f00..37f56b860 100644 --- a/src/subjugator/gnc/subjugator_thruster_manager/README.md +++ b/src/subjugator/gnc/subjugator_thruster_manager/README.md @@ -1,4 +1,32 @@ This package listens to the wrench topic and converts it to the thruster effort needed from each of the 8 thrusters on SubjuGator 9. It relies on the thruster allocation matrix configured in the config yaml file. +The desired wrench is allocated to per-thruster forces (Newtons) via the pseudo-inverse of the +allocation matrix, then converted to normalized effort `[-1, 1]` through the measured thrust curve in +`include/subjugator_thruster_manager/lut.h`. That curve is nonlinear and asymmetric (a thruster makes +more force forward than reverse), so this replaces the old single-slope `max_force_pos`/`max_force_neg` +scaling. `forward_to_sim` applies the same curve in reverse (`force_from_effort`) so the simulator and +the real vehicle share one thrust model. Saturation against `thruster_cap` is applied in force units +before the curve so the wrench direction is preserved. + +### The thrust curve (`lut.h`) + +The table is the measured T200 curve transcribed from the thruster board ("purple board") and +converted from kgf to Newtons. It holds 201 samples evenly spaced in effort from `-1.0` (full +reverse, `-39.9130655` N) to `+1.0` (full forward, `+51.4849125` N), so full forward is ~29% stronger +than full reverse. Efforts within `|effort| <= 0.07` are the deadband and produce 0 N. The properties +the rest of the system depends on (endpoints, monotonicity, deadband, force/effort round-trip, +asymmetry) are enforced by `test/test_lut.cpp`. + +Two assumptions are not yet verified and should be before trusting this on hardware: + +- **Voltage:** the samples correspond to a single supply voltage (assumed ~16V). The sub runs + ~15.5-16V, which shifts max thrust a few percent. Voltage-aware curve selection is deferred to + separate follow-up work, since it first needs the board to report pack voltage (no such telemetry + exists today). +- **Direction:** positive effort is assumed to be each thruster's stronger (forward) direction. The + T200s use mixed CW/CCW props, so confirm a positive command on every thruster lands on the strong + side of the curve, not the weak side — otherwise the asymmetry is applied backwards on those + thrusters. + Start node with `ros2 launch subjugator_thruster_manager thruster_manager.launch.py` diff --git a/src/subjugator/gnc/subjugator_thruster_manager/config/thruster_manager.yaml b/src/subjugator/gnc/subjugator_thruster_manager/config/thruster_manager.yaml index 0aeb42ce2..b1a11c2a6 100644 --- a/src/subjugator/gnc/subjugator_thruster_manager/config/thruster_manager.yaml +++ b/src/subjugator/gnc/subjugator_thruster_manager/config/thruster_manager.yaml @@ -10,6 +10,6 @@ rx: [0.0, 0.0, 0.0, 0.0, -0.14922, 0.14922, -0.14922, 0.14922 ] # roll ry: [0.0, 0.0, 0.0, 0.0, -0.230185, -0.230185, 0.230185,0.231085] # pitch rz: [0.2695308, -0.2695308, 0.337687697, -0.337687697, 0.0, 0.0, 0.0, 0.0 ] # yaw + # Per-thruster effort limit in [0, 1]. The force<->effort curve (lut.h) + # supplies the actual forward/reverse max thrust, so no max_force params here. thruster_cap: 1.0 - max_force_pos: 51.5 - max_force_neg: 40.2 diff --git a/src/subjugator/gnc/subjugator_thruster_manager/include/subjugator_thruster_manager/lut.h b/src/subjugator/gnc/subjugator_thruster_manager/include/subjugator_thruster_manager/lut.h new file mode 100644 index 000000000..64afe13a2 --- /dev/null +++ b/src/subjugator/gnc/subjugator_thruster_manager/include/subjugator_thruster_manager/lut.h @@ -0,0 +1,278 @@ +#pragma once + +#include +#include +#include +#include + +// Measured T200 thrust curve, transcribed from the thruster board ("purple +// board") data and converted from kgf to Newtons. 201 samples, evenly spaced in +// effort from -1.0 (full reverse, index 0) to +1.0 (full forward, index 200); +// step 0.01. The curve is nonlinear and asymmetric: full forward (+51.4849125 N) +// is ~29% stronger than full reverse (-39.9130655 N). Indices 93-107 +// (|effort| <= 0.07) are the deadband and produce 0 N. +// +// Assumptions worth verifying before relying on this on hardware: +// - Voltage: these samples correspond to a single supply voltage (assumed +// ~16V). The sub runs ~15.5-16V, which shifts max thrust a few percent; +// voltage-aware curve selection is tracked as separate follow-up work. +// - Direction: positive effort is assumed to be each thruster's *stronger* +// (forward) direction. T200s use mixed CW/CCW props, so confirm that a +// positive command on every thruster lands on the strong side of this curve +// and not the weak side, otherwise the asymmetry is applied backwards. +std::array const thrust_lut = { -39.9130655, + -39.7169325, + -39.422732999999994, + -38.834334, + -38.245934999999996, + -37.9517355, + -37.461403, + -37.265269999999994, + -36.7749375, + -36.3826715, + -35.892339, + -35.205873499999996, + -34.519408, + -33.8329425, + -33.34261, + -32.4600115, + -31.871612499999998, + -31.38128, + -30.498681499999996, + -29.812216, + -29.321883500000002, + -28.831550999999997, + -28.047019, + -27.654752999999996, + -27.1644205, + -26.5760215, + -26.085689, + -25.301157, + -25.006957499999995, + -24.614691499999996, + -24.0262925, + -23.339826999999996, + -23.0456275, + -22.359161999999998, + -21.966896000000002, + -21.57463, + -20.790098, + -20.397832, + -19.809433, + -19.417167, + -19.024901, + -18.240369, + -17.7500365, + -17.259704, + -16.671305, + -16.3771055, + -15.7887065, + -15.298373999999999, + -14.8080415, + -14.611908499999998, + -14.121576, + -13.729309999999998, + -13.2389775, + -12.748645, + -12.356378999999999, + -11.76798, + -11.375713999999999, + -10.983448000000001, + -10.787315, + -10.2969825, + -10.002782999999999, + -9.610517, + -9.218250999999999, + -8.825985, + -8.5317855, + -8.041452999999999, + -7.6491869999999995, + -7.256920999999999, + -7.060788, + -6.668522, + -6.3743225, + -6.0801229999999995, + -5.687856999999999, + -5.295591, + -5.0013914999999995, + -4.707191999999999, + -4.314926, + -4.118792999999999, + -3.8245934999999998, + -3.4323274999999995, + -3.138128, + -2.8439284999999996, + -2.549729, + -2.3535959999999996, + -2.0593964999999996, + -1.765197, + -1.4709975, + -1.2748645, + -0.980665, + -0.8825985, + -0.6864655000000001, + -0.4903325, + -0.392266, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.392266, + 0.4903325, + 0.784532, + 0.980665, + 1.2748645, + 1.4709975, + 1.765197, + 2.157463, + 2.4516625, + 2.8439284999999996, + 3.138128, + 3.530394, + 3.92266, + 4.314926, + 4.609125499999999, + 5.0013914999999995, + 5.4917240000000005, + 5.88399, + 6.276256, + 6.668522, + 7.060788, + 7.6491869999999995, + 8.041452999999999, + 8.5317855, + 8.9240515, + 9.316317499999998, + 9.7085835, + 10.100849499999999, + 10.787315, + 11.179580999999999, + 11.571846999999998, + 12.160245999999999, + 12.552512, + 13.0428445, + 13.631243499999998, + 14.121576, + 14.513841999999999, + 15.102241, + 15.5925735, + 16.1809725, + 16.5732385, + 17.259704, + 17.848103, + 18.436501999999997, + 18.9268345, + 19.515233499999997, + 20.103632499999996, + 20.790098, + 21.378497, + 21.770763000000002, + 22.359161999999998, + 23.339826999999996, + 23.8301595, + 24.712757999999997, + 25.301157, + 25.987622499999997, + 26.7721545, + 27.066353999999997, + 27.850885999999996, + 28.3412185, + 29.223816999999997, + 29.910282499999997, + 30.498681499999996, + 31.0870805, + 31.577413, + 32.361945, + 33.048410499999996, + 33.538743, + 34.4213415, + 35.4020065, + 36.088472, + 36.676871, + 37.461403, + 38.1478685, + 38.834334, + 39.81499899999999, + 40.6975975, + 41.678262499999995, + 42.168594999999996, + 42.953126999999995, + 44.227991499999995, + 44.4241245, + 45.6009225, + 46.1893215, + 46.9738535, + 47.464186, + 48.34678449999999, + 49.1313165, + 49.817782, + 50.406181, + 50.798446999999996, + 51.190712999999995, + 51.4849125 }; + +// Spacing between adjacent LUT samples in normalized effort units. The table +// holds 201 samples spanning effort -1.0 (full reverse) to +1.0 (full forward), +// so each step is 0.01 of effort. +inline constexpr double thrust_lut_effort_step = 2.0 / static_cast(thrust_lut.size() - 1); + +// Force (Newtons) the thruster produces at a given normalized effort in [-1, 1], +// linearly interpolating between table samples. Effort is clamped to the table's +// achievable range. +inline double force_from_effort(double effort) +{ + if (effort <= -1.0) + { + return thrust_lut.front(); + } + if (effort >= 1.0) + { + return thrust_lut.back(); + } + double const pos = (effort + 1.0) / thrust_lut_effort_step; + // Clamp the base index so lo+1 stays in range: for an effort just below 1.0 + // the division can round pos up to size()-1, which would read past the end. + std::size_t const lo = std::min(static_cast(pos), thrust_lut.size() - 2); + double const frac = pos - static_cast(lo); + return thrust_lut[lo] + frac * (thrust_lut[lo + 1] - thrust_lut[lo]); +} + +// Inverse of force_from_effort: the normalized effort in [-1, 1] that produces +// the requested force (Newtons), linearly interpolating between table samples. +// The table is monotonic non-decreasing with a flat zero band near the center, +// so any non-zero force is bracketed by the two samples around it; we bias toward +// the smallest-magnitude effort that achieves it. Out-of-range requests clamp to +// full reverse/forward. +inline double effort_from_force(double force) +{ + if (force <= thrust_lut.front()) + { + return -1.0; + } + if (force >= thrust_lut.back()) + { + return 1.0; + } + if (force == 0.0) + { + return 0.0; + } + // First sample >= force; its predecessor is < force (a non-zero force never + // lands inside the zero band), so the two straddle the target. + auto const hi = std::lower_bound(thrust_lut.begin(), thrust_lut.end(), force); + auto const lo = hi - 1; + double const frac = (force - *lo) / (*hi - *lo); + auto const idx = std::distance(thrust_lut.begin(), lo); + return -1.0 + (static_cast(idx) + frac) * thrust_lut_effort_step; +} diff --git a/src/subjugator/gnc/subjugator_thruster_manager/include/subjugator_thruster_manager/thruster_manager.h b/src/subjugator/gnc/subjugator_thruster_manager/include/subjugator_thruster_manager/thruster_manager.h index 14fcde50f..e0e496946 100644 --- a/src/subjugator/gnc/subjugator_thruster_manager/include/subjugator_thruster_manager/thruster_manager.h +++ b/src/subjugator/gnc/subjugator_thruster_manager/include/subjugator_thruster_manager/thruster_manager.h @@ -20,8 +20,8 @@ class ThrusterManager : public rclcpp::Node int const dof_ = 6; int const thruster_count_ = 8; double thruster_cap_; - double max_force_pos_; - double max_force_neg_; + double cap_force_pos_; // forward force (N) the thrusters can make at thruster_cap_ + double cap_force_neg_; // reverse force (N, negative) the thrusters can make at thruster_cap_ rclcpp::TimerBase::SharedPtr timer_; rclcpp::Publisher::SharedPtr thrust_publisher_; diff --git a/src/subjugator/gnc/subjugator_thruster_manager/package.xml b/src/subjugator/gnc/subjugator_thruster_manager/package.xml index 38209c650..43cdc6862 100644 --- a/src/subjugator/gnc/subjugator_thruster_manager/package.xml +++ b/src/subjugator/gnc/subjugator_thruster_manager/package.xml @@ -9,6 +9,7 @@ ament_cmake + ament_cmake_gtest ament_lint_auto ament_lint_common diff --git a/src/subjugator/gnc/subjugator_thruster_manager/src/forward_to_sim.cpp b/src/subjugator/gnc/subjugator_thruster_manager/src/forward_to_sim.cpp index 6e38f13ba..26096ef8f 100644 --- a/src/subjugator/gnc/subjugator_thruster_manager/src/forward_to_sim.cpp +++ b/src/subjugator/gnc/subjugator_thruster_manager/src/forward_to_sim.cpp @@ -5,17 +5,13 @@ #include "std_msgs/msg/float64.hpp" #include "subjugator_msgs/msg/thruster_efforts.hpp" +#include "subjugator_thruster_manager/lut.h" class SimulatedThrusterCmds : public rclcpp::Node { public: SimulatedThrusterCmds() : Node("simulate_thruster_cmds") { - this->declare_parameter("max_force_pos", 0.0); - max_force_pos_ = this->get_parameter("max_force_pos").as_double(); - this->declare_parameter("max_force_neg", 0.0); - max_force_neg_ = this->get_parameter("max_force_neg").as_double(); - std::vector thruster_topics = { "thruster/FLH", "thruster/FRH", "thruster/BLH", "thruster/BRH", "thruster/FLV", "thruster/FRV", "thruster/BLV", "thruster/BRV" }; @@ -32,8 +28,10 @@ class SimulatedThrusterCmds : public rclcpp::Node for (size_t i = 0; i < publishers_.size(); ++i) { std_msgs::msg::Float64 msg_thruster; - msg_thruster.data = - (thrust_values[i] > 0) ? thrust_values[i] * max_force_pos_ : thrust_values[i] * max_force_neg_; + // Convert normalized effort back to force (N) for the sim plugin + // using the same asymmetric, nonlinear curve the thruster manager + // inverts, so sim and vehicle share one thrust model. + msg_thruster.data = force_from_effort(thrust_values[i]); publishers_[i]->publish(msg_thruster); } }; @@ -43,8 +41,6 @@ class SimulatedThrusterCmds : public rclcpp::Node } private: - double max_force_pos_; - double max_force_neg_; rclcpp::Subscription::SharedPtr subscription_; std::vector::SharedPtr> publishers_; }; diff --git a/src/subjugator/gnc/subjugator_thruster_manager/src/thruster_manager.cpp b/src/subjugator/gnc/subjugator_thruster_manager/src/thruster_manager.cpp index b50188488..afe63d15f 100644 --- a/src/subjugator/gnc/subjugator_thruster_manager/src/thruster_manager.cpp +++ b/src/subjugator/gnc/subjugator_thruster_manager/src/thruster_manager.cpp @@ -1,5 +1,6 @@ #include "subjugator_thruster_manager/thruster_manager.h" +#include #include #include #include @@ -11,6 +12,7 @@ #include "geometry_msgs/msg/wrench.hpp" #include "std_msgs/msg/string.hpp" #include "subjugator_msgs/msg/thruster_efforts.hpp" +#include "subjugator_thruster_manager/lut.h" // Construct node class ThrusterManager::ThrusterManager() : Node("thruster_manager") @@ -19,10 +21,11 @@ ThrusterManager::ThrusterManager() : Node("thruster_manager") this->declare_parameter("thruster_cap", 0.0); thruster_cap_ = this->get_parameter("thruster_cap").as_double(); - this->declare_parameter("max_force_pos", 0.0); - max_force_pos_ = this->get_parameter("max_force_pos").as_double(); - this->declare_parameter("max_force_neg", 0.0); - max_force_neg_ = this->get_parameter("max_force_neg").as_double(); + // The force<->effort curve (lut.h) already encodes each direction's max + // thrust, so the cap is given in effort and converted once to the force + // limits it implies (forward and reverse, which differ). + cap_force_pos_ = force_from_effort(thruster_cap_); + cap_force_neg_ = force_from_effort(-thruster_cap_); // Create thruster allocation matrix from config file parameters tam_ = Eigen::MatrixXd::Zero(dof_, thruster_count_); @@ -63,30 +66,35 @@ void ThrusterManager::wrench_callback(geometry_msgs::msg::Wrench::SharedPtr msg) // Compute and publish thruster efforts void ThrusterManager::timer_callback() { - Eigen::VectorXd thrust_values(tam_.completeOrthogonalDecomposition().pseudoInverse() * reference_wrench_); + // Per-thruster force (Newtons) that realizes the requested wrench. + Eigen::VectorXd forces(tam_.completeOrthogonalDecomposition().pseudoInverse() * reference_wrench_); - // check that the allocated thrust is not over the thruster cap (typically 1.0), and if it is, rescale all thrusters - double biggest_thrust = 0; - bool over_thruster_cap = false; - for (int i = 0; i < thrust_values.size(); i++) + // Saturation: if any thruster needs more force than the cap permits, scale + // every force down by a single factor so the wrench direction is preserved. + // We saturate in force units (not effort) because the force->effort curve is + // nonlinear, so scaling efforts would distort the relative thrust mix. The + // forward and reverse limits differ, so each thruster is compared to the one + // matching its direction. + double worst_ratio = 1.0; + for (int i = 0; i < forces.size(); i++) { - // scale desired force to percent effort for thruster board compatibility - // Note that thrusters produce different force if spinning forward (max_force_pos_) vs spinning backwards - thrust_values[i] = - (thrust_values[i] > 0) ? thrust_values[i] / max_force_pos_ : thrust_values[i] / max_force_neg_; - // determine largest thrust magnitude - if (std::abs(thrust_values[i]) > biggest_thrust) + double const limit = (forces[i] >= 0) ? cap_force_pos_ : cap_force_neg_; + if (limit != 0.0) { - biggest_thrust = std::abs(thrust_values[i]); - if (biggest_thrust > thruster_cap_) - { - over_thruster_cap = true; - } + worst_ratio = std::max(worst_ratio, forces[i] / limit); } } - if (over_thruster_cap) + if (worst_ratio > 1.0) { - thrust_values = thrust_values * (thruster_cap_ / biggest_thrust); + forces /= worst_ratio; + } + + // Map each force to a normalized effort through the asymmetric, nonlinear LUT + // (replaces the old single-slope linear scaling) for thruster board compatibility. + Eigen::VectorXd thrust_values(forces.size()); + for (int i = 0; i < forces.size(); i++) + { + thrust_values[i] = effort_from_force(forces[i]); } auto msg = subjugator_msgs::msg::ThrusterEfforts(); diff --git a/src/subjugator/gnc/subjugator_thruster_manager/test/test_lut.cpp b/src/subjugator/gnc/subjugator_thruster_manager/test/test_lut.cpp new file mode 100644 index 000000000..1a695c9e2 --- /dev/null +++ b/src/subjugator/gnc/subjugator_thruster_manager/test/test_lut.cpp @@ -0,0 +1,143 @@ +#include + +#include + +#include "subjugator_thruster_manager/lut.h" + +// The endpoints of the measured T200 curve: full reverse is weaker than full +// forward, which is the asymmetry the whole change exists to capture. +constexpr double kFullReverse = -39.9130655; +constexpr double kFullForward = 51.4849125; + +TEST(Lut, EndpointsMatchTable) +{ + EXPECT_DOUBLE_EQ(force_from_effort(-1.0), kFullReverse); + EXPECT_DOUBLE_EQ(force_from_effort(1.0), kFullForward); +} + +TEST(Lut, ClampsOutOfRangeEffort) +{ + EXPECT_DOUBLE_EQ(force_from_effort(-2.0), kFullReverse); + EXPECT_DOUBLE_EQ(force_from_effort(5.0), kFullForward); +} + +TEST(Lut, ClampsOutOfRangeForce) +{ + EXPECT_DOUBLE_EQ(effort_from_force(kFullReverse - 100.0), -1.0); + EXPECT_DOUBLE_EQ(effort_from_force(kFullForward + 100.0), 1.0); +} + +TEST(Lut, ZeroMapsToZeroBothWays) +{ + EXPECT_DOUBLE_EQ(force_from_effort(0.0), 0.0); + EXPECT_DOUBLE_EQ(effort_from_force(0.0), 0.0); +} + +// force_from_effort must be non-decreasing across the whole effort range: more +// effort never yields less thrust. +TEST(Lut, ForceIsMonotonicInEffort) +{ + double previous = force_from_effort(-1.0); + for (int i = -100; i <= 100; ++i) + { + double const effort = static_cast(i) / 100.0; + double const force = force_from_effort(effort); + EXPECT_GE(force, previous - 1e-12) << "non-monotonic at effort " << effort; + previous = force; + } +} + +// effort_from_force inverts force_from_effort. We skip the central deadband +// (|effort| <= 0.07) where many efforts collapse to 0 N and the inverse is not +// unique by construction. +TEST(Lut, RoundTripEffortForceEffort) +{ + for (int i = -100; i <= 100; ++i) + { + double const effort = static_cast(i) / 100.0; + if (std::abs(effort) <= 0.07) + { + continue; + } + double const force = force_from_effort(effort); + double const recovered = effort_from_force(force); + EXPECT_NEAR(recovered, effort, 1e-9) << "round trip failed at effort " << effort; + } +} + +// The core asymmetry: forward thrust is stronger than reverse at equal effort +// magnitude, so the same effort magnitude produces a larger force forward. +TEST(Lut, ForwardStrongerThanReverse) +{ + for (double effort = 0.1; effort < 1.0; effort += 0.1) + { + double const forward = force_from_effort(effort); + double const reverse = force_from_effort(-effort); + EXPECT_GT(forward, std::abs(reverse)) << "forward not stronger at effort " << effort; + } +} + +// Equivalently, reaching a given force magnitude in reverse costs more effort +// than reaching it forward. Pick a force achievable in both directions. +TEST(Lut, ReverseCostsMoreEffortForSameForce) +{ + double const force = 30.0; + double const forward_effort = effort_from_force(force); + double const reverse_effort = effort_from_force(-force); + EXPECT_GT(std::abs(reverse_effort), forward_effort); +} + +// This mirrors the real vehicle/sim data flow: the thruster manager turns a +// desired per-thruster force into a normalized effort (effort_from_force) and +// publishes /thruster_efforts; the Gazebo ThrusterBridge turns that effort back +// into force (force_from_effort) and applies it to the plant. The force the sub +// actually feels must equal the force the manager commanded. Because both ends +// linearly interpolate the same table, this round trip is exact to fp precision +// for any force in the achievable range. +TEST(Lut, CommandedForceSurvivesEffortRoundTrip) +{ + for (double force = kFullReverse; force <= kFullForward; force += 0.123) + { + double const effort = effort_from_force(force); // thruster manager + double const applied = force_from_effort(effort); // sim ThrusterBridge + EXPECT_NEAR(applied, force, 1e-9) << "plant force diverged at commanded " << force << " N"; + } + // Endpoints exactly. + EXPECT_DOUBLE_EQ(force_from_effort(effort_from_force(kFullReverse)), kFullReverse); + EXPECT_DOUBLE_EQ(force_from_effort(effort_from_force(kFullForward)), kFullForward); +} + +// The documented deadband (|effort| <= 0.07 produces no thrust). The interior is +// exactly 0 N because both bracketing samples are zero; the band edges land +// between a zero and a nonzero sample, so interpolation leaves fp dust there +// rather than a literal 0. Either way a near-zero wrench produces no meaningful +// creep, and the first sample past the band does produce thrust. +TEST(Lut, DeadbandSpansDocumentedRange) +{ + for (double effort = -0.06; effort <= 0.06 + 1e-12; effort += 0.01) + { + EXPECT_DOUBLE_EQ(force_from_effort(effort), 0.0) << "expected deadband interior at effort " << effort; + } + EXPECT_NEAR(force_from_effort(-0.07), 0.0, 1e-9); + EXPECT_NEAR(force_from_effort(0.07), 0.0, 1e-9); + // First samples outside the band produce thrust. + EXPECT_LT(force_from_effort(-0.08), 0.0); + EXPECT_GT(force_from_effort(0.08), 0.0); +} + +// The asymmetry the issue exists to capture is real and substantial: full +// forward thrust exceeds full reverse by a wide margin (the measured curve is +// ~29% stronger forward), well beyond the ~20% the issue cites. A linear model +// would have this ratio at 1.0. +TEST(Lut, AsymmetryMatchesIssueClaim) +{ + double const forward = force_from_effort(1.0); + double const reverse = std::abs(force_from_effort(-1.0)); + EXPECT_GT(forward / reverse, 1.20) << "forward/reverse asymmetry weaker than documented"; +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/src/subjugator/simulation/subjugator_gazebo/CMakeLists.txt b/src/subjugator/simulation/subjugator_gazebo/CMakeLists.txt index 36c2b9f5d..043181318 100644 --- a/src/subjugator/simulation/subjugator_gazebo/CMakeLists.txt +++ b/src/subjugator/simulation/subjugator_gazebo/CMakeLists.txt @@ -10,6 +10,7 @@ find_package(sensor_msgs REQUIRED) find_package(nav_msgs REQUIRED) find_package(mil_msgs REQUIRED) find_package(subjugator_msgs REQUIRED) +find_package(subjugator_thruster_manager REQUIRED) find_package(OpenCV REQUIRED) @@ -124,7 +125,8 @@ ament_target_dependencies(DepthSensor rclcpp mil_msgs sensor_msgs) ament_target_dependencies(Hydrophone rclcpp geometry_msgs mil_msgs) -ament_target_dependencies(ThrusterBridge rclcpp subjugator_msgs geometry_msgs) +ament_target_dependencies(ThrusterBridge rclcpp subjugator_msgs geometry_msgs + subjugator_thruster_manager) ament_target_dependencies(MarbleDropper rclcpp subjugator_msgs geometry_msgs) diff --git a/src/subjugator/simulation/subjugator_gazebo/include/ThrusterBridge.hh b/src/subjugator/simulation/subjugator_gazebo/include/ThrusterBridge.hh index 322f17127..0b8026de6 100644 --- a/src/subjugator/simulation/subjugator_gazebo/include/ThrusterBridge.hh +++ b/src/subjugator/simulation/subjugator_gazebo/include/ThrusterBridge.hh @@ -50,10 +50,6 @@ class ThrusterBridge : public gz::sim::System, public gz::sim::ISystemConfigure, // Thruster efforts storage - FLV = Front Left Vertical, BRH = Back Right Horizontal // std::unordered_map thrusterEfforts; std::unordered_map publishers; - - // Thruster Force Outputs // - double max_force_pos = 1.0; - double max_force_neg = 1.0; }; } // namespace thrusterBridge diff --git a/src/subjugator/simulation/subjugator_gazebo/package.xml b/src/subjugator/simulation/subjugator_gazebo/package.xml index 02b9009d7..a0111cd94 100644 --- a/src/subjugator/simulation/subjugator_gazebo/package.xml +++ b/src/subjugator/simulation/subjugator_gazebo/package.xml @@ -20,6 +20,7 @@ std_msgs subjugator_description subjugator_msgs + subjugator_thruster_manager rclpy ament_cmake diff --git a/src/subjugator/simulation/subjugator_gazebo/src/ThrusterBridge.cc b/src/subjugator/simulation/subjugator_gazebo/src/ThrusterBridge.cc index 5d356a7ad..678e3fc4d 100644 --- a/src/subjugator/simulation/subjugator_gazebo/src/ThrusterBridge.cc +++ b/src/subjugator/simulation/subjugator_gazebo/src/ThrusterBridge.cc @@ -3,6 +3,7 @@ #include #include "gz/plugin/Register.hh" // For GZ_ADD_PLUGIN +#include "subjugator_thruster_manager/lut.h" #include @@ -41,20 +42,6 @@ void ThrusterBridge::Configure(gz::sim::Entity const &entity, std::shared_ptr(topicName); publishers[thruster] = pub; } - - // Get Thruster Force Parameters - auto parameters_client = std::make_shared(this->thrustNode, "thruster_manager"); - while (!parameters_client->wait_for_service(std::chrono::seconds(1))) - { - if (!rclcpp::ok()) - { - RCLCPP_ERROR(this->thrustNode->get_logger(), "Interrupted while waiting for the service. Exiting."); - rclcpp::shutdown(); - } - RCLCPP_INFO(this->thrustNode->get_logger(), "service not available, waiting again..."); - } - max_force_pos = parameters_client->get_parameter("max_force_pos", 1.0); - max_force_neg = parameters_client->get_parameter("max_force_neg", 1.0); } // receiveEffortCallback() - Whenever ROS2 node receives message call this // @@ -75,8 +62,11 @@ void ThrusterBridge::receiveEffortCallback(subjugator_msgs::msg::ThrusterEfforts for (auto const &[thrusterName, thrustValue] : thrusterEfforts) { gz::msgs::Double thrustMsg; - double scaledThrust = (thrustValue > 0) ? thrustValue * this->max_force_pos : thrustValue * this->max_force_neg; - thrustMsg.set_data(scaledThrust); + // Model the physical thruster: convert normalized effort to force (N) + // through the measured, nonlinear and asymmetric T200 curve so the sim + // plant matches the real vehicle. See subjugator_thruster_manager/lut.h. + double const thrust = force_from_effort(thrustValue); + thrustMsg.set_data(thrust); publishers[thrusterName].Publish(thrustMsg); } }