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.55303% with 335 lines in your changes missing coverage. Please review.
✅ Project coverage is 35.24%. Comparing base (e2dde64) to head (f1df4f7).

Files with missing lines	Patch %	Lines
...ntrol/pid_controller_dp/src/pid_controller_ros.cpp	0.00%	155 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   33.53%   35.24%   +1.70%     
==========================================
  Files          44       45       +1     
  Lines        3003     3459     +456     
  Branches      780     1078     +298     
==========================================
+ Hits         1007     1219     +212     
- Misses       1813     1944     +131     
- Partials      183      296     +113

Flag	Coverage Δ
unittests	`35.24% <36.55%> (+1.70%)`	⬆️

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

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

control/pid_controller_dp/src/pid_controller_utils.cpp

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

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

control/pid_controller_dp/include/pid_controller_dp/pid_controller_ros.hpp

+    // 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

control/pid_controller_dp/config/pid_params.yaml

-    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

control/pid_controller_dp/CMakeLists.txt

  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

control/pid_controller_dp/src/pid_controller_ros.cpp

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

control/pid_controller_dp/src/pid_controller_ros.cpp

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

control/pid_controller_dp/src/pid_controller.cpp

+    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

control/pid_controller_dp/src/pid_controller.cpp

 #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

control/pid_controller_dp/include/pid_controller_dp/pid_controller_ros.hpp

    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

control/pid_controller_dp/src/pid_controller_ros.cpp

+    // 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

control/pid_controller_dp/src/pid_controller_utils.cpp

+    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

control/pid_controller_dp/src/pid_controller_utils.cpp

+    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

control/pid_controller_dp/include/pid_controller_dp/pid_controller.hpp

    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

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

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

		void PIDControllerNode::set_pid_params() {

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

Comments

Conversation

ppakr commented Nov 12, 2025

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!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

4 participants

codecov bot commented Nov 12, 2025 •

edited

Loading