Robotic-Decision-Making-Lab · evan-palmer · Jul 31, 2025 · Jul 31, 2025 · Aug 1, 2025 · Aug 1, 2025
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.4.0 (2025-08-01)
+
 ## 0.3.3 (2025-07-29)
 
 ## 0.3.2 (2025-07-22)

diff --git a/auv_control_demos/COLCON_IGNORE b/auv_control_demos/COLCON_IGNORE
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.3.3</version>
+  <version>0.4.0</version>
   <description>Example package that includes demos for using auv_controllers in individual and chained modes</description>
 
   <maintainer email="mitchcol@oregonstate.edu">Colin Mitchell</maintainer>

diff --git a/auv_control_msgs/CHANGELOG.md b/auv_control_msgs/CHANGELOG.md
@@ -1,5 +1,13 @@
 # Changelog for package auv_control_msgs
 
+## 0.4.0 (2025-08-01)
+
+- Renames the Trajectory message to CartesianTrajectory
+- Renames the TrajectoryPoint message to CartesianTrajectoryPoint
+- Renames the TrajectoryControllerState message to CartesianTrajectoryControllerStateStamped
+- Renames the FollowTrajectory action to FollowCartesianTrajectory
+- Implements the ImpedanceCommand message
+
 ## 0.3.3 (2025-07-29)
 
 ## 0.3.2 (2025-07-22)
@@ -12,10 +20,10 @@
 
 ## 0.2.0 (2025-05-03)
 
-- Implements the EndEffectorTrajectory message
-- Implements the EndEffectorTrajectoryPoint message
-- Implements the EndEffectorTrajectoryControllerState message
-- Adds the FollowEndEffectorTrajectory action
+- Implements the Trajectory message
+- Implements the TrajectoryPoint message
+- Implements the TrajectoryControllerState message
+- Adds the FollowTrajectory action
 
 ## 0.1.0 (2025-04-27)
 

diff --git a/auv_control_msgs/CMakeLists.txt b/auv_control_msgs/CMakeLists.txt
@@ -11,10 +11,11 @@ find_package(trajectory_msgs REQUIRED)
 rosidl_generate_interfaces(auv_control_msgs
   "msg/MultiActuatorStateStamped.msg"
   "msg/IKControllerStateStamped.msg"
-  "msg/EndEffectorTrajectoryPoint.msg"
-  "msg/EndEffectorTrajectory.msg"
-  "msg/EndEffectorTrajectoryControllerState.msg"
-  "action/FollowEndEffectorTrajectory.action"
+  "msg/CartesianTrajectoryPoint.msg"
+  "msg/CartesianTrajectory.msg"
+  "msg/CartesianTrajectoryControllerStateStamped.msg"
+  "msg/ImpedanceCommand.msg"
+  "action/FollowCartesianTrajectory.action"
   DEPENDENCIES builtin_interfaces std_msgs geometry_msgs trajectory_msgs
 )
 

diff --git a/...action/FollowEndEffectorTrajectory.action → ...s/action/FollowCartesianTrajectory.action b/...action/FollowEndEffectorTrajectory.action → ...s/action/FollowCartesianTrajectory.action
@@ -3,7 +3,7 @@
 # The trajectory header stamp is used to set the trajectory start time. This can be
 # set to zero to indicate that the controller should start following the trajectory
 # now.
-auv_control_msgs/EndEffectorTrajectory trajectory
+auv_control_msgs/CartesianTrajectory trajectory
 
 # The maximum error that the controller is allowed when following the trajectory.
 # When this is set to 0, the tolerance will not be applied during control.

diff --git a/...ontrol_msgs/msg/EndEffectorTrajectory.msg → auv_control_msgs/msg/CartesianTrajectory.msg b/...ontrol_msgs/msg/EndEffectorTrajectory.msg → auv_control_msgs/msg/CartesianTrajectory.msg
@@ -1,4 +1,4 @@
 std_msgs/Header header
 
 # The sequence of end effector points to track.
-auv_control_msgs/EndEffectorTrajectoryPoint[] points
+auv_control_msgs/CartesianTrajectoryPoint[] points
diff --git a/.../EndEffectorTrajectoryControllerState.msg → ...esianTrajectoryControllerStateStamped.msg b/.../EndEffectorTrajectoryControllerState.msg → ...esianTrajectoryControllerStateStamped.msg
@@ -4,10 +4,10 @@ std_msgs/Header header
 # for error calculation.
 geometry_msgs/Pose reference
 
-# The current end effector state.
+# The current state.
 geometry_msgs/Pose feedback
 
-# The squared Euclidean norm of the geodesic distance between the reference state and end effector state.
+# The squared Euclidean norm of the geodesic distance between the reference state and state.
 float64 error
 
 # The trajectory controller command. This is the trajectory sample from the next time instance.

diff --git a/...l_msgs/msg/EndEffectorTrajectoryPoint.msg → ...rol_msgs/msg/CartesianTrajectoryPoint.msg b/...l_msgs/msg/EndEffectorTrajectoryPoint.msg → ...rol_msgs/msg/CartesianTrajectoryPoint.msg
@@ -1,4 +1,4 @@
-# The sequence of end effector poses.
+# The sequence of poses.
 geometry_msgs/Pose point
 
 # The time that this point should be reached, measured from the start of the trajectory.

diff --git a/auv_control_msgs/msg/ImpedanceCommand.msg b/auv_control_msgs/msg/ImpedanceCommand.msg
@@ -0,0 +1,9 @@
+std_msgs/Header header
+
+string child_frame_id
+
+geometry_msgs/Pose pose
+
+geometry_msgs/Twist twist
+
+geometry_msgs/Wrench wrench
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.3.3</version>
+  <version>0.4.0</version>
   <description>Custom messages for AUV controllers</description>
 
   <maintainer email="rakeshvivek97@gmail.com">Rakesh Vivekanandan</maintainer>

diff --git a/auv_controllers/CHANGELOG.md b/auv_controllers/CHANGELOG.md
@@ -1,5 +1,11 @@
 # Changelog for package auv_controllers
 
+## 0.4.0 (2025-08-01)
+
+- Implements the impedance controller
+- Refactors the `end_effector_trajectory_controller` to be `cartesian_trajectory_controller`
+- Implements the trajectory controllers package
+
 ## 0.3.3 (2025-07-29)
 
 ## 0.3.2 (2025-07-22)

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.3.3</version>
+  <version>0.4.0</version>
   <description>Meta package for auv_controllers</description>
 
   <maintainer email="evanp922@gmail.com">Evan Palmer</maintainer>

diff --git a/controller_common/CHANGELOG.md b/controller_common/CHANGELOG.md
@@ -1,5 +1,9 @@
 # Changelog for package controller_common
 
+## 0.4.0 (2025-08-01)
+
+- Adds API support for the ImpedanceCommand message type
+
 ## 0.3.3 (2025-07-29)
 
 ## 0.3.2 (2025-07-22)

diff --git a/controller_common/CMakeLists.txt b/controller_common/CMakeLists.txt
@@ -11,6 +11,7 @@ find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(nav_msgs REQUIRED)
+find_package(auv_control_msgs REQUIRED)
 
 add_library(controller_common SHARED)
 target_sources(
@@ -25,7 +26,11 @@ target_sources(
 target_compile_features(controller_common PUBLIC cxx_std_23)
 target_link_libraries(
     controller_common
-    PUBLIC rclcpp::rclcpp ${geometry_msgs_TARGETS} ${nav_msgs_TARGETS}
+    PUBLIC
+        rclcpp::rclcpp
+        ${geometry_msgs_TARGETS}
+        ${nav_msgs_TARGETS}
+        ${auv_control_msgs_TARGETS}
 )
 
 install(
@@ -38,6 +43,6 @@ install(
 )
 
 ament_export_targets(export_controller_common HAS_LIBRARY_TARGET)
-ament_export_dependencies("rclcpp" "geometry_msgs" "nav_msgs")
+ament_export_dependencies("rclcpp" "geometry_msgs" "nav_msgs" "auv_control_msgs")
 
 ament_package()
diff --git a/controller_common/include/controller_common/common.hpp b/controller_common/include/controller_common/common.hpp
@@ -22,6 +22,7 @@
 
 #include <vector>
 
+#include "auv_control_msgs/msg/impedance_command.hpp"
 #include "geometry_msgs/msg/pose.hpp"
 #include "geometry_msgs/msg/twist.hpp"
 #include "geometry_msgs/msg/wrench.hpp"
@@ -41,10 +42,14 @@ auto to_vector(const geometry_msgs::msg::Wrench & wrench) -> std::vector<double>
 
 auto to_vector(const nav_msgs::msg::Odometry & odom) -> std::vector<double>;
 
+auto to_vector(const auv_control_msgs::msg::ImpedanceCommand & command) -> std::vector<double>;
+
 auto to_msg(const std::vector<double> & data, geometry_msgs::msg::Pose * msg) -> void;
 
 auto to_msg(const std::vector<double> & data, geometry_msgs::msg::Twist * msg) -> void;
 
+auto to_msg(const std::vector<double> & data, geometry_msgs::msg::Wrench * msg) -> void;
+
 auto reset_message(geometry_msgs::msg::Pose * msg) -> void;
 
 auto reset_message(geometry_msgs::msg::Twist * msg) -> void;
@@ -53,6 +58,8 @@ auto reset_message(geometry_msgs::msg::Wrench * msg) -> void;
 
 auto reset_message(nav_msgs::msg::Odometry * msg) -> void;
 
+auto reset_message(auv_control_msgs::msg::ImpedanceCommand * msg) -> void;
+
 }  // namespace messages
 
 namespace math

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.3.3</version>
+  <version>0.4.0</version>
   <description>Common interfaces for controllers used in this project</description>
 
   <maintainer email="evanp922@gmail.com">Evan Palmer</maintainer>
@@ -19,6 +19,7 @@
   <depend>rclcpp</depend>
   <depend>geometry_msgs</depend>
   <depend>nav_msgs</depend>
+  <depend>auv_control_msgs</depend>
 
   <export>
     <build_type>ament_cmake</build_type>

diff --git a/controller_common/src/common.cpp b/controller_common/src/common.cpp
@@ -77,6 +77,19 @@ auto to_vector(const nav_msgs::msg::Odometry & odom) -> std::vector<double>
     odom.twist.twist.angular.z};
 }
 
+auto to_vector(const auv_control_msgs::msg::ImpedanceCommand & command) -> std::vector<double>
+{
+  std::vector<double> data;
+  auto pose = to_vector(command.pose);
+  auto twist = to_vector(command.twist);
+  auto wrench = to_vector(command.wrench);
+  data.reserve(pose.size() + twist.size() + wrench.size());
+  std::ranges::copy(pose, std::back_inserter(data));
+  std::ranges::copy(twist, std::back_inserter(data));
+  std::ranges::copy(wrench, std::back_inserter(data));
+  return data;
+}
+
 auto to_msg(const std::vector<double> & data, geometry_msgs::msg::Pose * msg) -> void
 {
   msg->position.x = data[0];
@@ -98,6 +111,16 @@ auto to_msg(const std::vector<double> & data, geometry_msgs::msg::Twist * msg) -
   msg->angular.z = data[5];
 }
 
+auto to_msg(const std::vector<double> & data, geometry_msgs::msg::Wrench * msg) -> void
+{
+  msg->force.x = data[0];
+  msg->force.y = data[1];
+  msg->force.z = data[2];
+  msg->torque.x = data[3];
+  msg->torque.y = data[4];
+  msg->torque.z = data[5];
+}
+
 auto reset_message(geometry_msgs::msg::Pose * msg) -> void
 {
   msg->position.x = nan;
@@ -146,6 +169,13 @@ auto reset_message(nav_msgs::msg::Odometry * msg) -> void
   msg->twist.twist.angular.z = nan;
 }
 
+auto reset_message(auv_control_msgs::msg::ImpedanceCommand * msg) -> void
+{
+  reset_message(&msg->pose);
+  reset_message(&msg->twist);
+  reset_message(&msg->wrench);
+}
+
 }  // namespace messages
 
 namespace math

diff --git a/controller_coordinator/CHANGELOG.md b/controller_coordinator/CHANGELOG.md
@@ -1,5 +1,7 @@
 # Changelog for package controller_coordinator
 
+## 0.4.0 (2025-08-01)
+
 ## 0.3.3 (2025-07-29)
 
 - Enabled activating and deactivating multiple hardware interfaces.

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.3.3</version>
+  <version>0.4.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/end_effector_trajectory_controller/README.md b/end_effector_trajectory_controller/README.md
diff --git a/end_effector_trajectory_controller/end_effector_trajectory_controller.xml b/end_effector_trajectory_controller/end_effector_trajectory_controller.xml
diff --git a/ik_solvers/CHANGELOG.md b/ik_solvers/CHANGELOG.md
@@ -1,5 +1,7 @@
 # Changelog for package ik_solvers
 
+## 0.4.0 (2025-08-01)
+
 ## 0.3.3 (2025-07-29)
 
 ## 0.3.2 (2025-07-22)

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.3.3</version>
+  <version>0.4.0</version>
   <description>Inverse kinematics solvers used for whole-body control</description>
 
   <maintainer email="evanp922@gmail.com">Evan Palmer</maintainer>

diff --git a/impedance_controller/CHANGELOG.md b/impedance_controller/CHANGELOG.md
@@ -0,0 +1,5 @@
+# Changelog for package impedance_controller
+
+## 0.4.0 (2025-08-01)
+
+- Implements an impedance controller for underwater vehicle control