merge control stack from easter testing into main by Q3rkses · Pull Request #706 · vortexntnu/vortex-auv

Q3rkses · 2026-04-06T18:12:07Z

implemented quat adaptive backstepping dp
modified quat pid dp
modified joystick interface
modified reference filter quat

…ht be a bit wonky

…backstepping controller, tuned the adaptive dp backstepping controller a bit for nautilus not yet finished. Added roll back into the controllable parameters, it looks good for now.

…nu/vortex-auv into feat/solver-and-drone-sim-tests

…tests

…r layout

… Vortex wiki and link it in README. Began changing variable names and adding neccesary functionality in vortex-utils

vortexuser

first round of reviews

kluge7

…o reduce scope. Got rid of magic numbers and made everything initializable and easy to configure. Fixed readme.

…ing_controler_to_quaternion

…n same PR

…verythin the control stack depends on from easter testing in one commit

codecov · 2026-04-06T20:22:30Z

Codecov Report

❌ Patch coverage is 39.17616% with 694 lines in your changes missing coverage. Please review.
✅ Project coverage is 40.79%. Comparing base (8a0f2e8) to head (b8b86b0).
⚠️ Report is 38 commits behind head on main.

Files with missing lines	Patch %	Lines
...ntrol/pid_controller_dp/src/pid_controller_ros.cpp	0.00%	192 Missing ⚠️
...troller_quat/src/dp_adapt_backs_controller_ros.cpp	0.60%	165 Missing ⚠️
...ller_quat/test/dp_adapt_backs_controller_tests.cpp	52.86%	1 Missing and 114 partials ⚠️
...ol/pid_controller_dp/test/pid_controller_tests.cpp	60.76%	0 Missing and 113 partials ⚠️
control/pid_controller_dp/src/pid_controller.cpp	24.24%	50 Missing ⚠️
...pid_controller_dp_euler/src/pid_controller_ros.cpp	0.00%	17 Missing ⚠️
...ce_filter_dp_quat/src/ros/reference_filter_ros.cpp	0.00%	12 Missing ⚠️
...rol/pid_controller_dp/src/pid_controller_utils.cpp	65.00%	6 Missing and 1 partial ⚠️
..._controller_quat/src/dp_adapt_backs_controller.cpp	89.47%	6 Missing ⚠️
...rence_filter_dp_quat/src/lib/waypoint_follower.cpp	0.00%	4 Missing ⚠️
... and 5 more

Additional details and impacted files

@@            Coverage Diff             @@
##             main     #706      +/-   ##
==========================================
+ Coverage   39.70%   40.79%   +1.09%     
==========================================
  Files          79       82       +3     
  Lines        5332     6265     +933     
  Branches     1715     2412     +697     
==========================================
+ Hits         2117     2556     +439     
- Misses       2769     3034     +265     
- Partials      446      675     +229

Flag	Coverage Δ
unittests	`40.79% <39.17%> (+1.09%)`	⬆️

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

Files with missing lines	Coverage Δ
...ference_filter_dp/src/ros/reference_filter_ros.cpp	`81.96% <100.00%> (ø)`
...de/thrust_allocator_auv/thrust_allocator_utils.hpp	`70.58% <ø> (-13.79%)`	⬇️
...ust_allocator_auv/tests/thrust_allocator_tests.cpp	`60.91% <100.00%> (ø)`
...trol/pid_controller_dp/src/pid_controller_node.cpp	`0.00% <0.00%> (ø)`
...s_controller/src/dp_adapt_backs_controller_ros.cpp	`0.66% <0.00%> (ø)`
...oller_quat/src/dp_adapt_backs_controller_utils.cpp	`94.64% <94.64%> (ø)`
...ntroller_dp/include/pid_controller_dp/typedefs.hpp	`0.00% <0.00%> (ø)`
...rence_filter_dp_quat/src/lib/waypoint_follower.cpp	`83.60% <0.00%> (-4.33%)`	⬇️
.../thrust_allocator_auv/src/thrust_allocator_ros.cpp	`0.93% <0.00%> (-0.01%)`	⬇️
..._controller_quat/src/dp_adapt_backs_controller.cpp	`89.47% <89.47%> (ø)`
... and 8 more

... and 1 file with indirect coverage changes

🚀 New features to boost your workflow:

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

Andeshog · 2026-04-07T14:17:28Z

is this controller going to be used? If so, maybe clean it up a bit. A lot of unused includes, outdated types/not using common types from vortex utils, redefinitions of existing utils functions

yes pid controllah was pretty 🔥

Andeshog · 2026-04-07T14:33:00Z

+void print_eta(const types::Eta& eta) {
+    spdlog::info("Eta values:");
+    auto pos = eta.pos_vector();
+    auto ori = eta.ori_quaternion();
+    spdlog::info("Position - North: {}, East: {}, Down: {}", pos[0], pos[1],
+                 pos[2]);
+    spdlog::info("Orientation - w: {}, x: {}, y: {}, z: {}", ori.w(), ori.x(),
+                 ori.y(), ori.z());
+}
+
+void print_nu(const types::Nu& nu) {
+    spdlog::info("Nu values:");
+    auto v = nu.to_vector();
+    spdlog::info("Linear Speed - u: {}, v: {}, w: {}", v(0), v(1), v(2));
+    spdlog::info("Angular Speed - p: {}, q: {}, r: {}", v(3), v(4), v(5));
+}
+
+void print_vect_6d(const types::Vector6d& vec) {
+    spdlog::info("Vector6d values:");
+    for (int i = 0; i < 6; ++i) {
+        spdlog::info("Element[{}]: {}", i, vec[i]);
+    }
+}
+
+void print_J_transformation(const types::J_transformation& J) {
+    spdlog::info("J_transformation:");
+
+    spdlog::info("R (3x3) elements:");
+    for (int i = 0; i < J.R.rows(); ++i) {
+        for (int j = 0; j < J.R.cols(); ++j) {
+            spdlog::info("R[{},{}] = {}", i, j, J.R(i, j));
+        }
+    }
+
+    spdlog::info("T (4x3) elements:");
+    for (int i = 0; i < J.T.rows(); ++i) {
+        for (int j = 0; j < J.T.cols(); ++j) {
+            spdlog::info("T[{},{}] = {}", i, j, J.T(i, j));
+        }
+    }
+
+    spdlog::info("Combined Matrix (7x6) elements:");
+    auto M = J.as_matrix();
+    for (int i = 0; i < M.rows(); ++i) {
+        for (int j = 0; j < M.cols(); ++j) {
+            spdlog::info("M[{},{}] = {}", i, j, M(i, j));
+        }
+    }
+}
+
+void print_Jinv_transformation(const types::Matrix6x7d& J_inv) {
+    spdlog::info("J_pseudo_inverse (6x7):");
+    for (int i = 0; i < J_inv.rows(); ++i) {
+        std::string row;
+        row.reserve(128);
+        row += "[";
+        for (int j = 0; j < J_inv.cols(); ++j) {
+            row += std::to_string(J_inv(i, j));
+            if (j < J_inv.cols() - 1)
+                row += ", ";
+        }
+        row += "]";
+        spdlog::info("{}", row);
+    }
+}


Why are these still here? We added those to debug the weird behavior last year, not to be used on main

Andeshog · 2026-04-07T14:35:54Z

+    types::Eta error = error_eta(eta, eta_d);  // calculate eta error



This comment doesnt add very much

Goes for the rest of the comments in this file too

Andeshog · 2026-04-07T14:46:15Z

+    types::Vector6d error_6;
+    error_6 << error.x, error.y, error.z, error.qx, error.qy, error.qz;


Minor, but having the 6 in the variable name doesnt really do any good, especially when there no longer is references to the 7 sized vector anymore. Also, would probably benefit from adding updated utility functions for the 6 dof quat maths

Andeshog · 2026-04-07T14:51:17Z

+    std::vector<double> Kp_vec = {
+        this->get_parameter("Kp_x").as_double(),
+        this->get_parameter("Kp_y").as_double(),
+        this->get_parameter("Kp_z").as_double(),
+        this->get_parameter("Kp_roll").as_double(),
+        this->get_parameter("Kp_pitch").as_double(),
+        this->get_parameter("Kp_yaw").as_double(),
+    };
+    std::vector<double> Ki_vec = {
+        this->get_parameter("Ki_x").as_double(),
+        this->get_parameter("Ki_y").as_double(),
+        this->get_parameter("Ki_z").as_double(),
+        this->get_parameter("Ki_roll").as_double(),
+        this->get_parameter("Ki_pitch").as_double(),
+        this->get_parameter("Ki_yaw").as_double(),
+    };
+    std::vector<double> Kd_vec = {
+        this->get_parameter("Kd_x").as_double(),
+        this->get_parameter("Kd_y").as_double(),
+        this->get_parameter("Kd_z").as_double(),
+        this->get_parameter("Kd_roll").as_double(),
+        this->get_parameter("Kd_pitch").as_double(),
+        this->get_parameter("Kd_yaw").as_double(),
+    };
+
+    types::Vector6d Kp_vec_eigen(Kp_vec.data());
+    types::Vector6d Ki_vec_eigen(Ki_vec.data());
+    types::Vector6d Kd_vec_eigen(Kd_vec.data());


No reason to go through std::vector here is it?

Have not loooked at it probably you are right

jorgenfj

We should also refactor the ros nodes to publish with unique_ptr and the subscriptions to follow this guideline

jorgenfj · 2026-04-07T20:44:01Z

Why are there 3 param files?

because i need to test that everything works, will change the workflows to use the sim file in the sim tests when my claude usage is back (i dont do devops dont blame me)

Still 3 param files?💂

jorgenfj · 2026-04-07T20:49:32Z

 types::Eta error_eta(const types::Eta& eta, const types::Eta& eta_d) {
-    types::Eta eta_error;
-
-    eta_error.pos = eta.pos - eta_d.pos;
-    eta_error.ori = eta_d.ori.conjugate() * eta.ori;
-
-    eta_error.ori = eta_error.ori.normalized();
-
-    return eta_error;
+    types::Eta error = eta - eta_d;
+    // Enforce shortest path: q and -q represent the same rotation, but only
+    // qw >= 0 gives the correct sign for the vector part used as error signal.
+    if (error.qw < 0.0) {
+        error.qw = -error.qw;
+        error.qx = -error.qx;
+        error.qy = -error.qy;
+        error.qz = -error.qz;
+    }
+    return error;
 }


If we want this to return the 6d error vector we should use the error_quaternion from vortex_utils math.hpp.

yes, i think i added that after doing this fix during easter

jorgenfj · 2026-04-07T21:05:52Z

Should avoid the RPY representation of pose/current_state so the reference mode does not inherit gimbal lock problems etc..

…ushed vortex utils since im not a tyrant

jorgenfj · 2026-04-13T13:36:11Z

Still 3 param files?💂

christopherbjorge · 2026-04-24T11:46:47Z

Good soup

github-actions · 2026-05-14T08:02:03Z

🎉 This PR is included in version 2.7.0 🎉

The release is available on GitHub release

Your semantic-release bot 📦🚀

jorgenfj and others added 16 commits March 25, 2026 21:14

rebase

ffff21f

waypoint sim test wrapper scripts

0eb1234

made it so that gripper does not have buoyancy or mass. Collision mig…

66cdafa

…ht be a bit wonky

rebase

ab5ee12

waypoint sim test wrapper scripts

463ea7c

made it so that gripper does not have buoyancy or mass. Collision mig…

5f9c567

…ht be a bit wonky

changed so that drone type is resolved at launch for the adaptive dp …

9df8a23

…backstepping controller, tuned the adaptive dp backstepping controller a bit for nautilus not yet finished. Added roll back into the controllable parameters, it looks good for now.

Merge branch 'feat/solver-and-drone-sim-tests' of github.com:vortexnt…

d3e5f8a

…nu/vortex-auv into feat/solver-and-drone-sim-tests

Merge remote-tracking branch 'origin' into feat/solver-and-drone-sim-…

0c6879a

…tests

updated thruster layout such that it fits the more spread out thruste…

05f725f

…r layout

Added TODOs, figured out error quaternions and prepared for changes

db646f4

Readded old controller, no need to throw it away.

71c1b84

started implementing quat dp

72cf921

Finished the Lyapunov proof for the controller, and will upload it to…

8481728

… Vortex wiki and link it in README. Began changing variable names and adding neccesary functionality in vortex-utils

launched dp adapt quat controller

7deec26

Updated error quaternion formulation to be correct

7c5c869

Q3rkses requested a review from kluge7 April 6, 2026 18:12

Q3rkses self-assigned this Apr 6, 2026

vortexuser requested changes Apr 6, 2026

View reviewed changes

kluge7 reviewed Apr 6, 2026

View reviewed changes

Q3rkses added 6 commits April 6, 2026 21:31

Made code more readable and added suitable helper/utility functions t…

62c6d9c

…o reduce scope. Got rid of magic numbers and made everything initializable and easy to configure. Fixed readme.

changed lqr back to how it was before

925c447

Merge remote-tracking branch 'origin/main' into dp_adaptive_backstepp…

4e782ad

…ing_controler_to_quaternion

added changes the quat reference filter such that everything passes o…

9abb46b

…n same PR

fixed pre commit

ccf0220

added changes to joystick_interface_auv to make sure tests work and e…

b78c032

…verythin the control stack depends on from easter testing in one commit

Q3rkses added 3 commits April 6, 2026 22:25

changed the topics to match the new ropic names for dp guidance.

aadf2d5

changed to dp_rpy for orca aswell

2a04778

fixed tuning and added font for refrence fitler

6efa53a