Skip to content

feat: apply nonlinear/asymmetric T200 thrust curve in manager + sim #570

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
nrebolloso wants to merge 1 commit into
base: main
Choose a base branch
from
Open

feat: apply nonlinear/asymmetric T200 thrust curve in manager + sim #570

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
18 changes: 17 additions & 1 deletion src/subjugator/gnc/subjugator_thruster_manager/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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()
28 changes: 28 additions & 0 deletions src/subjugator/gnc/subjugator_thruster_manager/README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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`
4 changes: 2 additions & 2 deletions src/subjugator/gnc/subjugator_thruster_manager/config/thruster_manager.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
278 changes: 278 additions & 0 deletions src/subjugator/gnc/subjugator_thruster_manager/include/subjugator_thruster_manager/lut.h
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,278 @@
#pragma once

#include <algorithm>
#include <array>
#include <cstddef>
#include <iterator>

// 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<double, 201> 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<double>(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<std::size_t>(pos), thrust_lut.size() - 2);
double const frac = pos - static_cast<double>(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<double>(idx) + frac) * thrust_lut_effort_step;
}
4 changes: 2 additions & 2 deletions ...or/gnc/subjugator_thruster_manager/include/subjugator_thruster_manager/thruster_manager.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<subjugator_msgs::msg::ThrusterEfforts>::SharedPtr thrust_publisher_;
Expand Down
Loading
Loading