feat/fix: PID quaternion-based DP controller by ppakr · Pull Request #643 · vortexntnu/vortex-auv

ppakr · 2025-11-12T20:21:15Z

Goal: correctly implement and validate the existing quaternion-based DP controller

Completed work

The core control mathematics have been reimplemented and validated. The implementation sucessfully passes all unit tests.

Known issue

Axis/Gain Mapping Swap: The control efforts/gains appear to be swapped for the planar axes (X and Y), and potentially for the rotational axes (Roll and Pitch). This needs investigation into the input vector mapping.
RQT Gain Bug: When updating a single gain via the rqt_reconfigure tool, the system receives the correct value, but all other gains are immediately reset to 0 in the controller's internal state

TODO

Resolve the XY axis mapping issue
Fix the RQT gain update to allow persistent configuration
Clean code
Documentation

…c reconfiguring

… pitch, and yaw

…ith new test setup

for more information, see https://pre-commit.ci

codecov · 2025-11-12T20:47:51Z

Codecov Report

❌ Patch coverage is 36.07477% with 342 lines in your changes missing coverage. Please review.
✅ Project coverage is 37.78%. Comparing base (4fba8ee) to head (ae83763).

Files with missing lines	Patch %	Lines
...ntrol/pid_controller_dp/src/pid_controller_ros.cpp	0.00%	162 Missing ⚠️
...ol/pid_controller_dp/test/pid_controller_tests.cpp	60.76%	0 Missing and 113 partials ⚠️
control/pid_controller_dp/src/pid_controller.cpp	16.94%	49 Missing ⚠️
...d_controller_dp/src/pid_controller_conversions.cpp	0.00%	13 Missing ⚠️
...ntroller_dp/include/pid_controller_dp/typedefs.hpp	0.00%	2 Missing ⚠️
...rol/pid_controller_dp/src/pid_controller_utils.cpp	80.00%	2 Missing ⚠️
...trol/pid_controller_dp/src/pid_controller_node.cpp	0.00%	1 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main     #643      +/-   ##
==========================================
+ Coverage   36.89%   37.78%   +0.88%     
==========================================
  Files          74       75       +1     
  Lines        5288     5738     +450     
  Branches     1601     1896     +295     
==========================================
+ Hits         1951     2168     +217     
- Misses       2916     3036     +120     
- Partials      421      534     +113

Flag	Coverage Δ
unittests	`37.78% <36.07%> (+0.88%)`	⬆️

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

Files with missing lines	Coverage Δ
...trol/pid_controller_dp/src/pid_controller_node.cpp	`0.00% <0.00%> (ø)`
...ntroller_dp/include/pid_controller_dp/typedefs.hpp	`0.00% <0.00%> (ø)`
...rol/pid_controller_dp/src/pid_controller_utils.cpp	`76.47% <80.00%> (+76.47%)`	⬆️
...d_controller_dp/src/pid_controller_conversions.cpp	`0.00% <0.00%> (ø)`
control/pid_controller_dp/src/pid_controller.cpp	`33.76% <16.94%> (+33.76%)`	⬆️
...ol/pid_controller_dp/test/pid_controller_tests.cpp	`60.76% <60.76%> (ø)`
...ntrol/pid_controller_dp/src/pid_controller_ros.cpp	`0.00% <0.00%> (ø)`

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

Since this is still a draft I wont comment on the debugs and prints and so on. But I do encourage utilizing Eta and Nu from vortex-utils, along with the relevant functions 👍 Also, when time comes it would be cool to see a version of the integration test utilizing this controller for a continuous proof of the quality 💯

Andeshog · 2025-11-13T13:03:41Z

Everything in this file exist in vortex-utils, and most is embedded in the Eta struct as well 😄

Andeshog · 2025-11-13T13:12:52Z

+    // TODO: parameter callback for dynamic reconfigure of PID gains
+    //@brief Callback function for parameter updates
+    // @param parameters: vector of parameters to be set
+    rcl_interfaces::msg::SetParametersResult parametersCallback(
+        const std::vector<rclcpp::Parameter>& parameters);


Consider using a yaml-based approach like Anbit did. Mixing this approach with a service call (Trigger) allows for the same dynamic reconfiguration, but without relying so much on ROS.

Andeshog · 2025-11-13T13:21:51Z

-    Kp: [70.0, 70.0, 70.0, 12.0, 12.0, 12.0]
-    Ki: [2.0, 2.0, 2.0, 0.12, 0.12, 0.12]
-    Kd: [10.0, 10.0, 10.0, 4.0, 5.0, 4.0]
+    # Kp: [70.0, 70.0, 70.0, 12.0, 12.0, 12.0]
+    # Ki: [2.0, 2.0, 2.0, 0.12, 0.12, 0.12]
+    # Kd: [10.0, 10.0, 10.0, 4.0, 5.0, 4.0]
+    Kp_x: 10.0
+    Kp_y: 0.0
+    Kp_z: 0.0
+    Kp_roll: 0.0
+    Kp_pitch: 0.0
+    Kp_yaw: 0.0
+    Ki_x: 0.0
+    Ki_y: 0.0
+    Ki_z: 0.0
+    Ki_roll: 0.0
+    Ki_pitch: 0.0
+    Ki_yaw: 0.0
+    Kd_x: 0.0
+    Kd_y: 0.0
+    Kd_z: 0.0
+    Kd_roll: 0.0
+    Kd_pitch: 0.0
+    Kd_yaw: 0.0


Is it not more tidy to have the parameters in vectors here?

chrstrom · 2026-02-13T10:08:10Z

@jorgenfj @ppakr will this be taken out of draft at some point or abandoned? If you're looking to merge I'd really want to do a thorough review of it just to minimize the chances of next year's team re-implementing it for the 7th time 😭 😭

jorgenfj · 2026-02-13T10:11:29Z

This is actively being worked on. I guess we just forgot to change it from draft

chrstrom

First pass, I'll have another read-through when its out of draft / feature-complete 🚀

chrstrom · 2026-02-13T10:22:17Z

  src/pid_controller_conversions.cpp
 )

+ament_target_dependencies(${LIB_NAME} PUBLIC


Sticking to the "keep non-ROS code separate from everything else", you wouldn't want to pull in anything to your lib using ament_target_dependencies. Instead, use pure cmake for the library, and link in ros-deps + the lib with ament for the final executable

chrstrom · 2026-02-13T10:30:19Z

+
+rcl_interfaces::msg::SetParametersResult PIDControllerNode::parametersCallback(
+    const std::vector<rclcpp::Parameter>& parameters) {
+    rcl_interfaces::msg::SetParametersResult result;
+    result.successful = true;
+    result.reason = "success";


would consider vectorizing parameters here, or make an unordered map for "parameter sting rep" -> "some struct holding gain type and eigen index"

chrstrom · 2026-02-13T10:34:08Z

@@ -114,20 +250,68 @@ void PIDControllerNode::publish_tau() {
 }

 void PIDControllerNode::set_pid_params() {


Would argue that you should disallow default parameters here and throw / panic if unset. Easy to miss one in config and could lead to painful debugging times

chrstrom · 2026-02-13T10:36:48Z

+    spdlog::info("Eta: ");
+    print_eta(eta);
+    spdlog::info("Eta desired: ");
+    print_eta(eta_d);
+    spdlog::info("Eta error:");
+    print_eta(error);


Would suggest making this spdlog::trace (you could pass in log level to the print methods). Lets you f.ex compile debug with trace log level and release with info/warn, and set different log levels for file/terminal (io is expensive)

chrstrom · 2026-02-13T10:38:18Z

 #include "pid_controller_dp/pid_controller.hpp"
 #include "pid_controller_dp/pid_controller_utils.hpp"

+void print_eta(const types::Eta& eta) {


For all these printing utils, consider checking out #include <spdlog/fmt/ostr.h> and/or overloading << and/or adding custom fmt:: formatters to the vortex-utils lib, would make things much neater

chrstrom · 2026-02-17T08:50:54Z

    bool killswitch_on_;

    std::string software_mode_;
+


probably applies to more than just this pr / package, but dealing with string-representation for stuff like this can get really annoying. if you need a string-rep at any point, i'd suggest converting to an enum (magic-enum f.ex) as early as possible in the chain

nvm seems to have been changed in #656

chrstrom · 2026-02-17T08:58:24Z

+    // calculate pid control
    types::Vector6d tau =
-        pid_controller_.calculate_tau(eta_, eta_d_, nu_, eta_dot_d_);
+        pid_controller_.calculate_tau(eta_body_, eta_d_body_, nu_, nu_d_body_);


been thinking about the calculate_tau interface for a bit, don't have anything definite here, but, some food for thought:

I think it could be very easy for someone to accidentally pass state/desired in different frames, but I also don't see an easy way to enforce this, without wrapping eta/nu in some POD with a field for frame definitions. Could be mitigated by letting calculate_tau just take errors, but then you're shifting a bit of a burden onto the user-side.

chrstrom · 2026-02-17T09:07:58Z

+    return eta - eta_d;
 }


been a while since i've dealt with any of this, but would you want to negate the quat part if the scalar error < 0? like to ensure shortest path

If not, then I guess you don't need to wrap this, just use the - overload directly wherever needed

chrstrom · 2026-02-17T09:10:11Z

+    return vortex::utils::math::clamp_values(values, min_val, max_val);
 }


could also just call this directly instead of wrapping it?

chrstrom · 2026-02-17T09:15:53Z

    types::Matrix6d Kp_;
    types::Matrix6d Ki_;
    types::Matrix6d Kd_;
    types::Vector7d integral_;


is this consistent? as in like, accumulating error in an overrepresentation (by 1 dim), instead of first transforming to 6d with J then keeping the integration 6d?

i guess in either case it would be nice to keep the integral 6d, so the anti-windup makes physical sense and everythings easier to reason with?

…lementation

…ement calculations

jorgenfj · 2026-03-19T20:27:58Z

                                             const types::Nu& nu,
                                             const types::Eta& eta_dot_d) {
-    types::Eta error = error_eta(eta, eta_d);
+    types::Eta error = error_eta(eta, eta_d);  // calculate eta error


Is it better to make the error 6d and change the corresponding matrices?

With 6d error you could also replace pseudo_inv with standard inverse

… and change type in utils from stamped -> no stamped

Q3rkses · 2026-03-23T18:38:56Z

 types::Vector6d PIDController::calculate_tau(const types::Eta& eta,
                                             const types::Eta& eta_d,
                                             const types::Nu& nu,
                                             const types::Eta& eta_dot_d) {
-    types::Eta error = error_eta(eta, eta_d);
+    types::Eta error = error_eta(eta, eta_d);  // calculate eta error
+
+    // set quaternion scalar part w = 0 (only use vector part of quaternion for
+    // error)
+    error.qw = 0.0;

-    types::Matrix6x7d J_inv = calculate_J_sudo_inv(error);
+    auto eta_dot_d_copy = eta_dot_d;
+    eta_dot_d_copy.qw = 0.0;  // set w = 0 for desired eta_dot

-    types::Vector6d nu_d = J_inv * eta_dot_d.as_vector();
+    types::Matrix6x7d J_inv =
+        calculate_J_sudo_inv(eta);  // calculate J pseudo inverse

-    types::Vector6d error_nu = nu.as_vector() - nu_d;
+    types::Vector6d nu_d =
+        J_inv * eta_dot_d_copy.to_vector();  // calculate velocity

-    types::Vector6d P = Kp_ * J_inv * error.as_vector();
+    types::Vector6d error_nu = nu.to_vector() - nu_d;  // calculate vel error

-    types::Vector6d I = Ki_ * J_inv * integral_;
+    types::Vector6d P = Kp_ * J_inv * error.to_vector();  // P term

-    types::Vector6d D = Kd_ * error_nu;
+    types::Vector6d I = Ki_ * J_inv * integral_;  // I term
+
+    types::Vector6d D = Kd_ * error_nu;  // D term

    types::Vector6d tau = -clamp_values((P + I + D), -80.0, 80.0);

    integral_ = anti_windup(dt_, error, integral_);

    return tau;


I agree with @jorgenfj. I think a cleaner way would be to simply extract the vector part of the quaternion, something like defining quaternion class or type as q_r and q_v then using q_v and a proper 6x6 jacobian (not the pseudoinv).

This is based on Fossens feedback approach where they prove that only the vector part of the quaternion is needed to be driven to 0 when proving lyapunov stability. I dont remember the exact page off the top of my head

ppakr and others added 6 commits October 15, 2025 15:51

feat: add debug parameters and logging for PID controller calculations

16f8668

feat: add rcl_interfaces dependency and parameter callback for dynami…

d333822

…c reconfiguring

feat: update PID controller parameters

a27998d

feat: add parameter handling with individual gains for x, y, z, roll,…

71df096

… pitch, and yaw

feat: update CMake configuration and add logging for PID controller w…

edcd63b

…ith new test setup

[pre-commit.ci] auto fixes from pre-commit.com hooks

1ed1d42

for more information, see https://pre-commit.ci

Andeshog reviewed Nov 13, 2025

View reviewed changes

ppakr added 5 commits January 5, 2026 12:09

WIP: before types migration

98c5d57

refactor: update typedefs and conversions for Eta and Nu structures

85f9b1a

fix: correct rqt gain update

457c2af

Merge branch 'main' into dp-quaternion-control

471e32d

fix: update type aliases for Eta and Nu to use Pose and Twist

b4aefb9

chrstrom reviewed Feb 17, 2026

View reviewed changes

ppakr added 2 commits February 21, 2026 17:08

Merge remote-tracking branch 'origin/main' into dp-quaternion-control

e7cdcf9

fix: add vortex_utils_ros to target dependencies

2d71411

ppakr force-pushed the dp-quaternion-control branch from b064f9f to 2d71411 Compare February 21, 2026 16:15

ppakr added 8 commits February 21, 2026 17:27

refactor: remove debug parameters and logging from PID controller imp…

afab550

…lementation

feat: transform joystick inputs from body frame to world frame in mov…

f1df4f7

…ement calculations

fix: update PID parameters and joystick interface

386fd99

docs: update docs

7c72929

Merge remote-tracking branch 'origin/main' into dp-quaternion-control

9fe9af2

Merge remote-tracking branch 'origin/main' into dp-quaternion-control

bf97597

refactor: remove commented-out PID parameter arrays from pid_params.yaml

d97c0f2

docs(controller): update readme

d69fed6

ppakr marked this pull request as ready for review March 11, 2026 18:52

jorgenfj reviewed Mar 19, 2026

View reviewed changes

ppakr added 2 commits March 21, 2026 16:25

refactor: update message types in PID controller to use Odom as input…

9a560f7

… and change type in utils from stamped -> no stamped

Merge remote-tracking branch 'origin/main' into dp-quaternion-control

ae83763

Q3rkses reviewed Mar 23, 2026

View reviewed changes

		@@ -114,20 +250,68 @@ void PIDControllerNode::publish_tau() {
		}

		void PIDControllerNode::set_pid_params() {

		return vortex::utils::math::clamp_values(values, min_val, max_val);
		}

Conversation

ppakr commented Nov 12, 2025 • edited by Q3rkses Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

codecov Bot commented Nov 12, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Codecov Report

Uh oh!

Andeshog left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

chrstrom commented Feb 13, 2026

Uh oh!

jorgenfj commented Feb 13, 2026

Uh oh!

chrstrom left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

5 participants

ppakr commented Nov 12, 2025 •

edited by Q3rkses

Loading

codecov Bot commented Nov 12, 2025 •

edited

Loading