Refactor trajectory control and introduce impedance controller (#78) (#79)

* start moving trajectory controller and setup pd tracking controller

* continue migration

* refactor and made initial pass at pd controller

* compile and cleanup

* testing

* what

* confused

* maybe

* maybe

* maybe

* maybe

* maybe

* maybe

* cleanup?

* cleanup?

* test

* test

* revert for now

* resolve upstream deprecations

* bump version

(cherry picked from commit 20f768d)

Co-authored-by: Evan Palmer <evanp922@gmail.com>

Author: mergify[bot]

auv_control_demos/CHANGELOG.md

@@ -1,5 +1,9 @@
 # Changelog for package auv_control_demos
 
+## 0.4.1 (2026-02-23)
+
+## 0.4.0 (2025-08-01)
+
 ## 0.3.3 (2025-07-29)
 
 ## 0.3.2 (2025-07-22)

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

auv_control_msgs/CHANGELOG.md

@@ -1,5 +1,15 @@
 # Changelog for package auv_control_msgs
 
+## 0.4.1 (2026-02-23)
+
+## 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 +22,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)
 

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
 )
 

…ction/FollowEndEffectorTrajectory.action …/action/FollowCartesianTrajectory.actionauv_control_msgs/action/FollowEndEffectorTrajectory.action renamed to auv_control_msgs/action/FollowCartesianTrajectory.action auv_control_msgs/action/FollowEndEffectorTrajectory.action renamed to auv_control_msgs/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.

…ntrol_msgs/msg/EndEffectorTrajectory.msg …control_msgs/msg/CartesianTrajectory.msgauv_control_msgs/msg/EndEffectorTrajectory.msg renamed to auv_control_msgs/msg/CartesianTrajectory.msg auv_control_msgs/msg/EndEffectorTrajectory.msg renamed to 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

…EndEffectorTrajectoryControllerState.msg …sianTrajectoryControllerStateStamped.msgauv_control_msgs/msg/EndEffectorTrajectoryControllerState.msg renamed to auv_control_msgs/msg/CartesianTrajectoryControllerStateStamped.msg auv_control_msgs/msg/EndEffectorTrajectoryControllerState.msg renamed to auv_control_msgs/msg/CartesianTrajectoryControllerStateStamped.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.

…_msgs/msg/EndEffectorTrajectoryPoint.msg …ol_msgs/msg/CartesianTrajectoryPoint.msgauv_control_msgs/msg/EndEffectorTrajectoryPoint.msg renamed to auv_control_msgs/msg/CartesianTrajectoryPoint.msg auv_control_msgs/msg/EndEffectorTrajectoryPoint.msg renamed to auv_control_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.

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

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.1</version>
   <description>Custom messages for AUV controllers</description>
 
   <maintainer email="rakeshvivek97@gmail.com">Rakesh Vivekanandan</maintainer>