Robotic-Decision-Making-Lab · evan-palmer · May 29, 2026 · May 29, 2026 · May 29, 2026 · May 29, 2026
diff --git a/.dockerignore b/.dockerignore
@@ -11,6 +11,6 @@
 !thruster_allocation_matrix_controller
 !thruster_controllers
 !topic_sensors
-!velocity_controllers
+!twist_controllers
 !whole_body_controllers
 !ros2.repos
diff --git a/auv_control_demos/CHANGELOG.md b/auv_control_demos/CHANGELOG.md
@@ -1,5 +1,7 @@
 # Changelog for package auv_control_demos
 
+## 0.5.0 (2026-05-29)
+
 ## 0.4.3 (2026-04-25)
 
 ## 0.4.2 (2026-03-30)

diff --git a/auv_control_demos/chained_controllers/README.md b/auv_control_demos/chained_controllers/README.md
@@ -23,7 +23,7 @@ applies a PWM command to the hardware interface.
    The output should resemble:
 
    ```bash
-   adaptive_integral_terminal_sliding_mode_controller velocity_controllers/AdaptiveIntegralTerminalSlidingModeController        active
+   adaptive_integral_terminal_sliding_mode_controller twist_controllers/AdaptiveIntegralTerminalSlidingModeController        active
    thruster_allocation_matrix_controller              thruster_allocation_matrix_controller/ThrusterAllocationMatrixController  active
    thruster_8_controller                              thruster_controllers/PolynomialThrustCurveController                      active
    thruster_7_controller                              thruster_controllers/PolynomialThrustCurveController                      active
@@ -130,7 +130,7 @@ applies a PWM command to the hardware interface.
   - [chaining.launch.py](https://github.com/Robotic-Decision-Making-Lab/auv_controllers/blob/main/auv_control_demos/chained_controllers/launch/chaining.launch.py)
 
 - Controllers:
-  - [Adaptive Integral Terminal Sliding Mode Controller](https://github.com/Robotic-Decision-Making-Lab/auv_controllers/tree/main/velocity_controllers)
+  - [Adaptive Integral Terminal Sliding Mode Controller](https://github.com/Robotic-Decision-Making-Lab/auv_controllers/tree/main/twist_controllers)
   - [Thruster Allocation Matrix Controller](https://github.com/Robotic-Decision-Making-Lab/auv_controllers/tree/main/thruster_allocation_matrix_controller)
   - [Polynomial Thrust Curve Controller](https://github.com/Robotic-Decision-Making-Lab/auv_controllers/tree/main/thruster_controllers)
 

diff --git a/auv_control_demos/chained_controllers/config/chained_controllers.yaml b/auv_control_demos/chained_controllers/config/chained_controllers.yaml
@@ -3,7 +3,7 @@ controller_manager:
     update_rate: 30 # Hz
 
     adaptive_integral_terminal_sliding_mode_controller:
-      type: velocity_controllers/AdaptiveIntegralTerminalSlidingModeController
+      type: twist_controllers/AdaptiveIntegralTerminalSlidingModeController
 
     thruster_allocation_matrix_controller:
       type: thruster_allocation_matrix_controller/ThrusterAllocationMatrixController

diff --git a/auv_control_demos/individual_controller/README.md b/auv_control_demos/individual_controller/README.md
@@ -1,6 +1,6 @@
 # Example 1: Individual Controller
 
-This example uses the [integral sliding mode controller](https://github.com/Robotic-Decision-Making-Lab/auv_controllers/tree/main/velocity_controllers) to demonstrate how to launch a single controller.
+This example uses the [integral sliding mode controller](https://github.com/Robotic-Decision-Making-Lab/auv_controllers/tree/main/twist_controllers) to demonstrate how to launch a single controller.
 
 ## Tutorial Steps
 
@@ -21,7 +21,7 @@ This example uses the [integral sliding mode controller](https://github.com/Robo
    The output should resemble:
 
    ```bash
-   adaptive_integral_terminal_sliding_mode_controller velocity_controllers/AdaptiveIntegralTerminalSlidingModeController  active
+   adaptive_integral_terminal_sliding_mode_controller twist_controllers/AdaptiveIntegralTerminalSlidingModeController  active
    ```
 
 3. Check that the hardware interfaces have been properly loaded by opening
@@ -148,7 +148,7 @@ This example uses the [integral sliding mode controller](https://github.com/Robo
   - [individual.launch.py](https://github.com/Robotic-Decision-Making-Lab/auv_controllers/blob/main/auv_control_demos/individual_controller/launch/individual.launch.py)
 
 - Controllers:
-  - [Adaptive Integral Terminal Sliding Mode Controller](https://github.com/Robotic-Decision-Making-Lab/auv_controllers/tree/main/velocity_controllers)
+  - [Adaptive Integral Terminal Sliding Mode Controller](https://github.com/Robotic-Decision-Making-Lab/auv_controllers/tree/main/twist_controllers)
 
 - Controller Config:
   - [individual_controller.yaml](https://github.com/Robotic-Decision-Making-Lab/auv_controllers/blob/main/auv_control_demos/individual_controller/config/individual_controller.yaml)

diff --git a/auv_control_demos/individual_controller/config/individual_controller.yaml b/auv_control_demos/individual_controller/config/individual_controller.yaml
@@ -3,7 +3,7 @@ controller_manager:
     update_rate: 30 # Hz
 
     adaptive_integral_terminal_sliding_mode_controller:
-      type: velocity_controllers/AdaptiveIntegralTerminalSlidingModeController
+      type: twist_controllers/AdaptiveIntegralTerminalSlidingModeController
 
 adaptive_integral_terminal_sliding_mode_controller:
   ros__parameters:

diff --git a/auv_control_demos/package.xml b/auv_control_demos/package.xml
@@ -3,7 +3,7 @@
 <package format="3">
 
   <name>auv_control_demos</name>
-  <version>0.4.3</version>
+  <version>0.5.0</version>
   <description>Example package that includes demos for using auv_controllers in individual and chained modes</description>
 
   <maintainer email="evanp922@gmail.com">Evan Palmer</maintainer>

diff --git a/auv_control_msgs/CHANGELOG.md b/auv_control_msgs/CHANGELOG.md
@@ -1,5 +1,7 @@
 # Changelog for package auv_control_msgs
 
+## 0.5.0 (2026-05-29)
+
 ## 0.4.3 (2026-04-25)
 
 ## 0.4.2 (2026-03-30)

diff --git a/auv_control_msgs/package.xml b/auv_control_msgs/package.xml
@@ -3,7 +3,7 @@
 <package format="3">
 
   <name>auv_control_msgs</name>
-  <version>0.4.3</version>
+  <version>0.5.0</version>
   <description>Custom messages for AUV controllers</description>
 
   <maintainer email="evanp922@gmail.com">Evan Palmer</maintainer>

diff --git a/auv_controllers/CHANGELOG.md b/auv_controllers/CHANGELOG.md
@@ -1,5 +1,7 @@
 # Changelog for package auv_controllers
 
+## 0.5.0 (2026-05-29)
+
 ## 0.4.3 (2026-04-25)
 
 ## 0.4.2 (2026-03-30)

diff --git a/auv_controllers/package.xml b/auv_controllers/package.xml
@@ -3,7 +3,7 @@
 <package format="3">
 
   <name>auv_controllers</name>
-  <version>0.4.3</version>
+  <version>0.5.0</version>
   <description>Meta package for auv_controllers</description>
 
   <maintainer email="evanp922@gmail.com">Evan Palmer</maintainer>
@@ -22,7 +22,7 @@
   <exec_depend>thruster_allocation_matrix_controller</exec_depend>
   <exec_depend>thruster_controllers</exec_depend>
   <exec_depend>topic_sensors</exec_depend>
-  <exec_depend>velocity_controllers</exec_depend>
+  <exec_depend>twist_controllers</exec_depend>
   <exec_depend>whole_body_controllers</exec_depend>
 
   <export>

diff --git a/controller_common/CHANGELOG.md b/controller_common/CHANGELOG.md
@@ -1,5 +1,7 @@
 # Changelog for package controller_common
 
+## 0.5.0 (2026-05-29)
+
 ## 0.4.3 (2026-04-25)
 
 ## 0.4.2 (2026-03-30)

diff --git a/controller_common/package.xml b/controller_common/package.xml
@@ -3,7 +3,7 @@
 <package format="3">
 
   <name>controller_common</name>
-  <version>0.4.3</version>
+  <version>0.5.0</version>
   <description>Common interfaces for controllers used in this project</description>
 
   <maintainer email="evanp922@gmail.com">Evan Palmer</maintainer>

diff --git a/controller_coordinator/CHANGELOG.md b/controller_coordinator/CHANGELOG.md
@@ -1,5 +1,7 @@
 # Changelog for package controller_coordinator
 
+## 0.5.0 (2026-05-29)
+
 ## 0.4.3 (2026-04-25)
 
 ## 0.4.2 (2026-03-30)

diff --git a/controller_coordinator/package.xml b/controller_coordinator/package.xml
@@ -3,7 +3,7 @@
 <package format="3">
 
   <name>controller_coordinator</name>
-  <version>0.4.3</version>
+  <version>0.5.0</version>
   <description>A high-level node used to load and activate/deactivate control systems</description>
 
   <maintainer email="evanp922@gmail.com">Evan Palmer</maintainer>

diff --git a/ik_solvers/CHANGELOG.md b/ik_solvers/CHANGELOG.md
@@ -1,5 +1,9 @@
 # Changelog for package ik_solvers
 
+## 0.5.0 (2026-05-29)
+
+- Replace the quaternion error with the left-invariant error
+
 ## 0.4.3 (2026-04-25)
 
 ## 0.4.2 (2026-03-30)

diff --git a/ik_solvers/CMakeLists.txt b/ik_solvers/CMakeLists.txt
@@ -17,6 +17,7 @@ find_package(tf2_eigen REQUIRED)
 find_package(ament_cmake REQUIRED)
 find_package(ament_cmake_ros REQUIRED)
 find_package(generate_parameter_library REQUIRED)
+find_package(hydrodynamics REQUIRED)
 
 generate_parameter_library(task_priority_solver_parameters
   src/task_priority_solver_parameters.yaml
@@ -68,6 +69,7 @@ target_link_libraries(
         Eigen3::Eigen
         pinocchio::pinocchio
         pluginlib::pluginlib
+        hydrodynamics::hydrodynamics
         ${trajectory_msgs_TARGETS}
 )
 
@@ -92,6 +94,7 @@ ament_export_dependencies(
     "pinocchio"
     "trajectory_msgs"
     "tf2_eigen"
+    "hydrodynamics"
 )
 
 ament_package()
diff --git a/ik_solvers/package.xml b/ik_solvers/package.xml
@@ -3,7 +3,7 @@
 <package format="3">
 
   <name>ik_solvers</name>
-  <version>0.4.3</version>
+  <version>0.5.0</version>
   <description>Inverse kinematics solvers used for whole-body control</description>
 
   <maintainer email="evanp922@gmail.com">Evan Palmer</maintainer>

diff --git a/ik_solvers/src/task_priority_solver.cpp b/ik_solvers/src/task_priority_solver.cpp
@@ -23,8 +23,10 @@
 #include <algorithm>
 #include <memory>
 #include <ranges>
+#include <unsupported/Eigen/MatrixFunctions>
 #include <vector>
 
+#include "hydrodynamics/hydrodynamics.hpp"
 #include "pinocchio/algorithm/frames.hpp"
 #include "pinocchio/algorithm/jacobian.hpp"
 #include "pinocchio/algorithm/kinematics.hpp"
@@ -53,22 +55,17 @@ auto active_tasks(const ConstraintSet & tasks) -> ConstraintSet
   return result;
 }
 
-/// Compute the error between two quaternions using eq. 2.12 in Gianluca Antonelli's Underwater Robotics book.
-/// Note that we only need to minimize the vector part of the error.
-auto quaternion_error(const Eigen::Quaterniond & q1, const Eigen::Quaterniond & q2) -> Eigen::Vector3d
+/// Derepresent a tangent vector.
+auto vee(const Eigen::Matrix4d & mat) -> Eigen::Vector6d
 {
-  const Eigen::Vector3d q1_vec = q1.vec();
-  const Eigen::Vector3d q2_vec = q2.vec();
-
-  const double q1_w = q1.w();
-  const double q2_w = q2.w();
-
-  const Eigen::Vector3d vec_error = (q2_w * q1_vec) - (q1_w * q2_vec) + q2_vec.cross(q1_vec);
-
-  // This is how we would compute the scalar error if we needed it
-  // const double scalar_error = q1_w * q2_w + q1_vec.dot(q2_vec);
+  return {mat(0, 3), mat(1, 3), mat(2, 3), mat(2, 1), mat(0, 2), mat(1, 0)};
+}
 
-  return {vec_error.x(), vec_error.y(), vec_error.z()};
+/// Calculate the left-invariant geodesic error between a goal pose and the current pose.
+auto geodesic_error(const Eigen::Isometry3d & goal, const Eigen::Isometry3d & state) -> Eigen::Vector6d
+{
+  const Eigen::Matrix4d error = (state.inverse() * goal).matrix().log();
+  return vee(error);
 }
 
 }  // namespace
@@ -139,11 +136,9 @@ PoseConstraint::PoseConstraint(
   int priority)
 : Constraint(primal.matrix(), constraint.matrix(), gain, priority)
 {
-  error_ = Eigen::VectorXd::Zero(6);
-  error_.head<3>() = (constraint.translation() - primal.translation()).eval();
-  error_.tail<3>() = quaternion_error(Eigen::Quaterniond(constraint.rotation()), Eigen::Quaterniond(primal.rotation()));
+  error_ = geodesic_error(constraint, primal);
   jacobian_ = Eigen::MatrixXd::Zero(6, model->nv);
-  pinocchio::getFrameJacobian(*model, *data, model->getFrameId(frame), pinocchio::LOCAL_WORLD_ALIGNED, jacobian_);
+  pinocchio::getFrameJacobian(*model, *data, model->getFrameId(frame), pinocchio::LOCAL, jacobian_);
 }
 
 JointConstraint::JointConstraint(

diff --git a/impedance_controller/CHANGELOG.md b/impedance_controller/CHANGELOG.md
@@ -1,5 +1,7 @@
 # Changelog for package impedance_controller
 
+## 0.5.0 (2026-05-29)
+
 ## 0.4.3 (2026-04-25)
 
 ## 0.4.2 (2026-03-30)

diff --git a/impedance_controller/package.xml b/impedance_controller/package.xml
@@ -3,7 +3,7 @@
 <package format="3">
 
   <name>impedance_controller</name>
-  <version>0.4.3</version>
+  <version>0.5.0</version>
   <description>An impedance controller for underwater vehicles</description>
 
   <maintainer email="evanp922@gmail.com">Evan Palmer</maintainer>

diff --git a/thruster_allocation_matrix_controller/CHANGELOG.md b/thruster_allocation_matrix_controller/CHANGELOG.md
@@ -1,5 +1,9 @@
 # Changelog for package thruster_allocation_matrix_controller
 
+## 0.5.0 (2026-05-29)
+
+- Set NaN reference force/torque to zero thrust.
+
 ## 0.4.3 (2026-04-25)
 
 ## 0.4.2 (2026-03-30)

diff --git a/thruster_allocation_matrix_controller/package.xml b/thruster_allocation_matrix_controller/package.xml
@@ -3,7 +3,7 @@
 <package format="3">
 
   <name>thruster_allocation_matrix_controller</name>
-  <version>0.4.3</version>
+  <version>0.5.0</version>
   <description>Thruster allocation matrix controller used to convert wrench commands into thrust commands</description>
 
   <maintainer email="evanp922@gmail.com">Evan Palmer</maintainer>

diff --git a/thruster_allocation_matrix_controller/src/thruster_allocation_matrix_controller.cpp b/thruster_allocation_matrix_controller/src/thruster_allocation_matrix_controller.cpp
@@ -21,6 +21,7 @@
 #include "thruster_allocation_matrix_controller/thruster_allocation_matrix_controller.hpp"
 
 #include <Eigen/Dense>
+#include <algorithm>
 #include <format>
 #include <ranges>
 #include <stdexcept>
@@ -182,9 +183,9 @@ auto ThrusterAllocationMatrixController::update_reference_from_subscribers(
 {
   auto * current_reference = reference_.readFromNonRT();
   const std::vector<double> wrench = common::messages::to_vector(*current_reference);
-  for (auto && [interface, ref] : std::views::zip(reference_interfaces_, wrench)) {
-    if (!std::isnan(ref)) {
-      interface = ref;
+  if (!std::ranges::all_of(wrench, [](double x) -> bool { return std::isnan(x); })) {
+    for (auto && [interface, ref] : std::views::zip(reference_interfaces_, wrench)) {
+      interface = std::isnan(ref) ? 0.0 : ref;
     }
   }
   common::messages::reset_message(current_reference);

diff --git a/thruster_controllers/CHANGELOG.md b/thruster_controllers/CHANGELOG.md
@@ -1,5 +1,7 @@
 # Changelog for package thruster_controllers
 
+## 0.5.0 (2026-05-29)
+
 ## 0.4.3 (2026-04-25)
 
 - Implements a `reverse_spin_direction` parameter into the thrust curve

diff --git a/thruster_controllers/package.xml b/thruster_controllers/package.xml
@@ -3,7 +3,7 @@
 <package format="3">
 
   <name>thruster_controllers</name>
-  <version>0.4.3</version>
+  <version>0.5.0</version>
   <description>A collection of thruster controllers for AUV control</description>
 
   <maintainer email="evanp922@gmail.com">Evan Palmer</maintainer>

diff --git a/topic_sensors/CHANGELOG.md b/topic_sensors/CHANGELOG.md
@@ -1,5 +1,7 @@
 # Changelog for package topic_sensors
 
+## 0.5.0 (2026-05-29)
+
 ## 0.4.3 (2026-04-25)
 
 ## 0.4.2 (2026-03-30)

diff --git a/topic_sensors/package.xml b/topic_sensors/package.xml
@@ -3,7 +3,7 @@
 <package format="3">
 
   <name>topic_sensors</name>
-  <version>0.4.3</version>
+  <version>0.5.0</version>
   <description>Sensor plugins used to write ROS 2 messages to state interfaces</description>
 
   <maintainer email="evanp922@gmail.com">Evan Palmer</maintainer>

diff --git a/trajectory_controllers/CHANGELOG.md b/trajectory_controllers/CHANGELOG.md
@@ -1,5 +1,7 @@
 # Changelog for package trajectory_controllers
 
+## 0.5.0 (2026-05-29)
+
 ## 0.4.3 (2026-04-25)
 
 ## 0.4.2 (2026-03-30)

diff --git a/trajectory_controllers/package.xml b/trajectory_controllers/package.xml
@@ -3,7 +3,7 @@
 <package format="3">
 
   <name>trajectory_controllers</name>
-  <version>0.4.3</version>
+  <version>0.5.0</version>
   <description>Trajectory controllers for underwater vehicles and manipulator systems</description>
 
   <maintainer email="evanp922@gmail.com">Evan Palmer</maintainer>

diff --git a/velocity_controllers/CHANGELOG.md → twist_controllers/CHANGELOG.md b/velocity_controllers/CHANGELOG.md → twist_controllers/CHANGELOG.md
@@ -1,4 +1,9 @@
-# Changelog for package velocity_controllers
+# Changelog for package twist_controllers
+
+## 0.5.0 (2026-05-29)
+
+- Rename `velocity_controllers` to `twist_controllers` to avoid name collision
+  with ros2_controllers.
 
 ## 0.4.3 (2026-04-25)