580 task thrust allocator auv by Q3rkses · Pull Request #661 · vortexntnu/vortex-auv

Q3rkses · 2026-02-01T23:02:39Z

Refactored the thrust allocator module to be easier to build upon. Added more documentation in the readme file and some basic unit testing of the thrust allocator functionality. Added a new solver using CasADi and Fossen's formulation of the QP thrust allocation problem. Whenever i find time again i will add more comprehensible unit testing.

Lastly it would be nice to have simulator testing and perform some analysis on computation time vs accuracy tradeoff for both pseudoinverse and the QP solver but that is outside of the scope of this PR.

…or6d struct to vortex utils

…t each solver is more independent and will do perform own calculations

… factory class and descriptions for them

…s before. Will add tests whenever i find time again

… now), will clean up the code and fix warm start for next time

…ench in order to keep feasability and warm starting the solver after initializing

…s node tests fail :(

motion/thrust_allocator_auv/.gitignore

motion/thrust_allocator_auv/LICENSE

motion/thrust_allocator_auv/include/thrust_allocator_auv/allocator_config.hpp

…ubious and not testing the expected functionality. These are mostly tests for moments as sometimes the desired wrench and actual wrench will not physically be able to match given the constraints.

Andeshog · 2026-02-05T15:32:16Z

motion/thrust_allocator_auv/include/thrust_allocator_auv/allocator.hpp

+/**
+ * @brief The Allocator class structure that the solvers will inherit and
+ * override.
+ */
+class Allocator {


Consider naming it IAllocator or similar to make it clear it is an interface class

motion/thrust_allocator_auv/include/thrust_allocator_auv/allocator.hpp

motion/thrust_allocator_auv/include/thrust_allocator_auv/allocator_config.hpp

Andeshog · 2026-02-05T15:38:11Z

motion/thrust_allocator_auv/include/thrust_allocator_auv/allocator_factory.hpp

+class Factory {
+   public:
+    /**
+     * @brief A function that makes an allocator based on a string
+     * defining the desired type.
+     *
+     * @param allocator_type String of the desired allocator type.
+     * @return unique pointer to the created allocator.
+     */
+    static std::unique_ptr<Allocator> make_allocator(
+        const std::string& allocator_type,
+        const AllocatorConfig& config);
+};


Why is this a class? Why not a free function inside a namespace?

This is a good question, i was inspired by the concept of factories and decided to use it here. Its something along the lines that thrust_allocator does not create itself. The factory is sent an order for a QP allocator and makes a qp allocator not a pseudoinverse allocator type of thing

motion/thrust_allocator_auv/include/thrust_allocator_auv/pseudoinverse_allocator.hpp

Andeshog · 2026-02-05T15:43:52Z

motion/thrust_allocator_auv/include/thrust_allocator_auv/pseudoinverse_allocator.hpp

+  Eigen::MatrixXd calculate_pseudoinverse(const Eigen::MatrixXd &T,
+                                          const Eigen::MatrixXd &W);


Generally, if a class method doesnt have any dependencies to the class it would be better off as a free function. guideline

motion/thrust_allocator_auv/include/thrust_allocator_auv/qp_allocator.hpp

Andeshog · 2026-02-05T15:52:49Z

motion/thrust_allocator_auv/solvers/qp_allocator.cpp

+void QPAllocator::formulate_as_qp(const AllocatorConfig &cfg) {
+  const int r = cfg.input_weight_matrix.rows(); // number of thrusters = 8
+  const int n = cfg.slack_weight_matrix.rows(); // degrees of freedom = 6


Why are the comments stating the values? Surely either a) the comments are wrong and they can be other values, or b) the comments are right and they are better off as constexpr values? 😄 In either case the comments are goners

this was mostly to remind myself what the actual value was :D

Andeshog · 2026-02-05T15:56:05Z

motion/thrust_allocator_auv/solvers/qp_allocator.cpp

+  // create and set the square term matrix.
+  Eigen::MatrixXd phi = Eigen::MatrixXd::Zero(r + n, r + n);
+  phi.block(0, 0, r, r) = cfg.input_weight_matrix;
+  phi.block(r, r, n, n) = cfg.slack_weight_matrix;


Just be aware that potential readers may not have access to the book, so names like r and n, and greek letters may not be very descriptive 😄

r and n are commented above as number of thrusters and degrees of freedom. Fossens book is at the office

motion/thrust_allocator_auv/solvers/qp_allocator.cpp

Andeshog · 2026-02-05T15:59:16Z

motion/thrust_allocator_auv/solvers/qp_allocator.cpp

+  if (!casadi_solver_initialized_) {
+    throw std::runtime_error("CasADi QP solver not initialized "
+                             "(formulate_as_qp_CasADi not called).");
+  }


perhaps using std::optional would be safer and easier to handle than throwing?

I will look up the difference and look into this

motion/thrust_allocator_auv/include/thrust_allocator_auv/qp_allocator.hpp

the qp allocator and refactored some code because the node dies everytime i try to launch it and use it in the simulator

codecov · 2026-02-05T19:31:51Z

Codecov Report

❌ Patch coverage is 54.01460% with 252 lines in your changes missing coverage. Please review.
✅ Project coverage is 38.16%. Comparing base (8bd1fb4) to head (da7c14d).

Files with missing lines	Patch %	Lines
...ust_allocator_auv/tests/thrust_allocator_tests.cpp	54.63%	3 Missing and 134 partials ⚠️
.../thrust_allocator_auv/src/thrust_allocator_ros.cpp	0.00%	98 Missing ⚠️
...lude/thrust_allocator_auv/thrust_allocator_ros.hpp	0.00%	5 Missing ⚠️
...tion/thrust_allocator_auv/solvers/qp_allocator.cpp	95.23%	2 Missing and 3 partials ⚠️
...thrust_allocator_auv/solvers/allocator_factory.cpp	62.50%	3 Missing ⚠️
..._allocator_auv/solvers/pseudoinverse_allocator.cpp	83.33%	1 Missing and 1 partial ⚠️
...e/thrust_allocator_auv/pseudoinverse_allocator.hpp	0.00%	1 Missing ⚠️
..._auv/include/thrust_allocator_auv/qp_allocator.hpp	0.00%	1 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main     #661      +/-   ##
==========================================
+ Coverage   33.79%   38.16%   +4.36%     
==========================================
  Files          44       49       +5     
  Lines        3003     3438     +435     
  Branches      780     1148     +368     
==========================================
+ Hits         1015     1312     +297     
+ Misses       1805     1802       -3     
- Partials      183      324     +141

Flag	Coverage Δ
unittests	`38.16% <54.01%> (+4.36%)`	⬆️

Flags with carried forward coverage won't be shown. Click here to find out more.

Files with missing lines	Coverage Δ
...de/thrust_allocator_auv/thrust_allocator_utils.hpp	`86.66% <100.00%> (+86.66%)`	⬆️
...e/thrust_allocator_auv/pseudoinverse_allocator.hpp	`0.00% <0.00%> (ø)`
..._auv/include/thrust_allocator_auv/qp_allocator.hpp	`0.00% <0.00%> (ø)`
..._allocator_auv/solvers/pseudoinverse_allocator.cpp	`83.33% <83.33%> (ø)`
...thrust_allocator_auv/solvers/allocator_factory.cpp	`62.50% <62.50%> (ø)`
...lude/thrust_allocator_auv/thrust_allocator_ros.hpp	`0.00% <0.00%> (ø)`
...tion/thrust_allocator_auv/solvers/qp_allocator.cpp	`95.23% <95.23%> (ø)`
.../thrust_allocator_auv/src/thrust_allocator_ros.cpp	`0.96% <0.00%> (+0.96%)`	⬆️
...ust_allocator_auv/tests/thrust_allocator_tests.cpp	`54.63% <54.63%> (ø)`

... and 2 files with indirect coverage changes

🚀 New features to boost your workflow:

❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

…unch, also muted CasADi prints in every iteration

Q3rkses added 9 commits December 11, 2025 19:22

added base filestructure for QP_solver added README and deported Vect…

2543f22

…or6d struct to vortex utils

Starting to implement the QP solver, for now refactored code such tha…

1cd5050

…t each solver is more independent and will do perform own calculations

Filled in more of the vision for the layout, namely added base class,…

349c650

… factory class and descriptions for them

code builds, and should be able to run with pseudoinverse allocator a…

14d424e

…s before. Will add tests whenever i find time again

made utils ROS FREE and added testing to make senior dev Anders happy

4ea4529

Added more tests

b696c7c

need to cleanup real quick

cc7b57e

fleshed out QPallocator and added CasADi solver (kinda vibe coded for…

f14ec7a

… now), will clean up the code and fix warm start for next time

fixed some early tests for the QP solver, added pre normalizing of wr…

d5d7518

…ench in order to keep feasability and warm starting the solver after initializing

Q3rkses requested review from Andeshog, Talhanc and chrstrom February 1, 2026 23:02

Q3rkses linked an issue Feb 1, 2026 that may be closed by this pull request

[Task & Research] Thrust allocation / interface for new drone #580

Open

3 tasks

Q3rkses requested a review from jorgenfj February 1, 2026 23:03

fixed pre-commit hooks, but jorgen pushed something to main that make…

a531809

…s node tests fail :(

Q3rkses force-pushed the 580-task-Thrust-Allocator-AUV branch from f792d80 to a531809 Compare February 2, 2026 13:06

Q3rkses requested a review from kluge7 February 2, 2026 13:06

Q3rkses self-assigned this Feb 2, 2026

Q3rkses added the AUV label Feb 2, 2026

kluge7 requested changes Feb 2, 2026

View reviewed changes

motion/thrust_allocator_auv/.gitignore Outdated Show resolved Hide resolved

motion/thrust_allocator_auv/LICENSE Show resolved Hide resolved

motion/thrust_allocator_auv/include/thrust_allocator_auv/allocator_config.hpp Outdated Show resolved Hide resolved

Q3rkses and others added 3 commits February 2, 2026 14:20

Delete motion/thrust_allocator_auv/.gitignore

58d053d

Added more comprehensible testing, figured out that some tests were d…

fd2c65a

…ubious and not testing the expected functionality. These are mostly tests for moments as sometimes the desired wrench and actual wrench will not physically be able to match given the constraints.

Merge branch 'main' into 580-task-Thrust-Allocator-AUV

f406ffd