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

auv_setup/launch/dp_quat.launch.py

...t_backs_controller_quat/include/dp_adapt_backs_controller_quat/dp_adapt_backs_controller.hpp

...s_controller_quat/include/dp_adapt_backs_controller_quat/dp_adapt_backs_controller_utils.hpp

control/dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat/typedefs.hpp

control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller.cpp

control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller_utils.cpp

control/dp_adapt_backs_controller_quat/README.md

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.75%. Comparing base (8a0f2e8) to head (fe8b381).

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.75%   +1.04%     
==========================================
  Files          79       82       +3     
  Lines        5332     6265     +933     
  Branches     1715     2411     +696     
==========================================
+ Hits         2117     2553     +436     
- Misses       2769     3037     +268     
- Partials      446      675     +229

Flag	Coverage Δ
unittests	`40.75% <39.17%> (+1.04%)`	⬆️

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 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.

control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller_ros.cpp

control/dp_adapt_backs_controller_quat/test/CMakeLists.txt

control/dp_adapt_backs_controller_quat/CMakeLists.txt

control/dp_adapt_backs_controller_quat/package.xml

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

control/pid_controller_dp/include/pid_controller_dp/pid_controller.hpp

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

control/pid_controller_dp/src/pid_controller.cpp

+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

control/pid_controller_dp/src/pid_controller.cpp

+    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

control/pid_controller_dp/src/pid_controller.cpp

+    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:48:02Z

control/pid_controller_dp/src/pid_controller_conversions.cpp

 types::Eta eta_convert_from_ros_to_eigen(
-    const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) {
+    const geometry_msgs::msg::PoseWithCovariance& msg) {
    types::Eta eta;
-    eta.pos << msg->pose.pose.position.x, msg->pose.pose.position.y,
-        msg->pose.pose.position.z;
-    eta.ori.w() = msg->pose.pose.orientation.w;
-    eta.ori.x() = msg->pose.pose.orientation.x;
-    eta.ori.y() = msg->pose.pose.orientation.y;
-    eta.ori.z() = msg->pose.pose.orientation.z;
+    eta.x = msg.pose.position.x;
+    eta.y = msg.pose.position.y;
+    eta.z = msg.pose.position.z;
+    eta.qw = msg.pose.orientation.w;
+    eta.qx = msg.pose.orientation.x;
+    eta.qy = msg.pose.orientation.y;
+    eta.qz = msg.pose.orientation.z;

    return eta;
 }

 types::Nu nu_convert_from_ros_to_eigen(
-    const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) {
+    const geometry_msgs::msg::TwistWithCovariance& msg) {
    types::Nu nu;
-    nu.linear_speed << msg->twist.twist.linear.x, msg->twist.twist.linear.y,
-        msg->twist.twist.linear.z;
-    nu.angular_speed << msg->twist.twist.angular.x, msg->twist.twist.angular.y,
-        msg->twist.twist.angular.z;
+    nu.u = msg.twist.linear.x;
+    nu.v = msg.twist.linear.y;
+    nu.w = msg.twist.linear.z;
+    nu.p = msg.twist.angular.x;
+    nu.q = msg.twist.angular.y;
+    nu.r = msg.twist.angular.z;
+
    return nu;
 }


Just use what exists in vortex utils?

Andeshog · 2026-04-07T14:48:27Z

control/pid_controller_dp/src/pid_controller_ros.cpp

+    callback_handle_ = this->add_on_set_parameters_callback(std::bind(
+        &PIDControllerNode::parametersCallback, this, std::placeholders::_1));


lambda over bind

Andeshog · 2026-04-07T14:50:11Z

control/pid_controller_dp/src/pid_controller_ros.cpp

+    this->declare_parameter<double>("Kp_x", 1.0);
+    this->declare_parameter<double>("Kp_y", 1.0);
+    this->declare_parameter<double>("Kp_z", 1.0);
+    this->declare_parameter<double>("Kp_roll", 1.0);
+    this->declare_parameter<double>("Kp_pitch", 1.0);
+    this->declare_parameter<double>("Kp_yaw", 1.0);
+    this->declare_parameter<double>("Ki_x", 0.1);
+    this->declare_parameter<double>("Ki_y", 0.1);
+    this->declare_parameter<double>("Ki_z", 0.1);
+    this->declare_parameter<double>("Ki_roll", 0.1);
+    this->declare_parameter<double>("Ki_pitch", 0.1);
+    this->declare_parameter<double>("Ki_yaw", 0.1);
+    this->declare_parameter<double>("Kd_x", 0.1);
+    this->declare_parameter<double>("Kd_y", 0.1);
+    this->declare_parameter<double>("Kd_z", 0.1);
+    this->declare_parameter<double>("Kd_roll", 0.1);
+    this->declare_parameter<double>("Kd_pitch", 0.1);
+    this->declare_parameter<double>("Kd_yaw", 0.1);


Would personally drop default values

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

control/pid_controller_dp/src/pid_controller_ros.cpp

+    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:43:20Z

guidance/reference_filter_dp_quat/src/lib/waypoint_follower.cpp

+        /** Enforce positive hemisphere to prevent sign flips in the published
+         * reference quaternion that would cause the downstream controller to
+         * see large spurious orientation errors.
+         */


It makes more sense for the controllers that compare quaternions to be responsible for the sign handling instead of assuming an input convention. But this is still nice for plotting 👍

I like plots converging :(

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

control/dp_adapt_backs_controller_quat/config/adapt_params_nautilus.yaml

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)

jorgenfj · 2026-04-07T20:46:34Z

control/pid_controller_dp/include/pid_controller_dp/pid_controller_utils.hpp

+/**
+ * @brief Compute the 6D quaternion error state ε̃ = [Δp; ε_q].
+ *
+ * The scalar part q̃_w is not returned. The sign of the quaternion error is
+ * flipped when q̃_w < 0 to enforce the shortest-path convention.
+ *
+ * @param eta  Actual vehicle pose [x, y, z, qw, qx, qy, qz]
+ * @param eta_d Desired vehicle pose [x, y, z, qw, qx, qy, qz]
+ * @return Eta struct whose [qx, qy, qz] fields carry ε_q (qw is discarded)
+ */
 types::Eta error_eta(const types::Eta& eta, const types::Eta& eta_d);


Is this comment correct?

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

control/pid_controller_dp/src/pid_controller_utils.cpp

 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:00:27Z

mission/joystick_interface_auv/joystick_interface_auv/joystick_interface_auv_node.py

-        reference_msg.header.frame_id = "odom"
+        # reference_msg.header.frame_id = "odom"
+        reference_msg.header.frame_id = "base_link"


This should still be odom

bro one line changes you could do yourself T-T

Out of the scope of this review

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

mission/joystick_interface_auv/joystick_interface_auv/joystick_interface_auv_node.py

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

jorgenfj · 2026-04-08T07:15:43Z

control/pid_controller_dp/src/pid_controller_utils.cpp

-    eta_error.ori = eta_error.ori.normalized();
-
-    return eta_error;
+    types::Eta error = eta - eta_d;


I am also not a fan of the - operator for quaternions types

By the way wouldn't it be more accurate to use the full quaternion identity for error using the tan^-1 function (in utils) so that errors dont trickle down

Depends on use case IMO. For the reference filter and controllers the errors should ideally never be large enough that it is a problem. This also avoids the singularity of the tan error

So for the reference filter it is fine, but if you want to use the tan version for the controllers you are free to do so

…ushed vortex utils since im not a tyrant

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

attempted fix at reference filter workflow

7653a28

Q3rkses requested review from Andeshog and jorgenfj April 7, 2026 11:52

hopefully final workflow fix

b240ca4