Port end effector trajectory controller from Angler (#54)

* testing gif in readme

* format

* bleh

* Added reference to blue docs

* Cleanup folders

* implemented trajectory

* whoops

* ee controller mvp done

* added error tolerance for execution

* add controller state publishing

* fix compilation errors

* update version and changelogs

* Start implementing action server interface

* adding action server and improving logic

* updates

* controller ready for testing

* add pluginlib description

* fix pluginlib

* fix plugin registration

* reintegrate default constructor

* fix library build

* add transform to chained references in ik controller

* bug fixes

* bug fixes

* added readme

* fix slerp

* fix link to blue docs

* add changelogs

* fix ci errors and pr comments

* cleanup

* bad idea :D

Author: evan-palmer

README.md

@@ -1,9 +1,15 @@
 # auv_controllers
 
 auv_controllers is a collection of controllers for autonomous underwater
-vehicles (AUVs) implemented using ros2_control. The controllers have
-been designed to support the complete AUV control hierarchy and to enable
-benchmarking against other commonly-used control algorithms.
+vehicles (AUVs) and underwater vehicle manipulator systems (UVMS) implemented
+using ros2_control. The controllers have been designed to support the complete
+system control hierarchy and to enable benchmarking against other commonly-used
+control algorithms.
+
+<p align="center">
+  <img src="media/uvms.gif" alt="UVMS whole-body control" width="49%" />
+  <img src="media/teleop.gif" alt="AUV control" width="49%" />
+</p>
 
 ## Installation
 
@@ -28,10 +34,11 @@ rosdep update && \
 rosdep install -y --from-paths src --ignore-src
 ```
 
-## Quick start
+## Getting started
 
 To learn more about how to use the controllers provided in this project, please
 refer to the [examples package](https://github.com/Robotic-Decision-Making-Lab/auv_controllers/tree/main/auv_control_demos).
+You can also find integration tutorials in the [Blue documentation](https://robotic-decision-making-lab.github.io/blue).
 
 ## Getting help
 

auv_control_demos/CHANGELOG.md

@@ -1,5 +1,7 @@
 # Changelog for package auv_control_demos
 
+## 0.2.0 (2025-05-03)
+
 ## 0.1.0 (2025-04-27)
 
 - Updates the individual_controller and chained_controllers demos to use the

auv_control_demos/package.xml

@@ -3,7 +3,7 @@
 <package format="3">
 
   <name>auv_control_demos</name>
-  <version>0.1.0</version>
+  <version>0.2.0</version>
   <description>Example package that includes demos for using auv_controllers in individual and chained modes</description>
 
   <maintainer email="[email protected]">Colin Mitchell</maintainer>

auv_control_msgs/CHANGELOG.md

@@ -1,5 +1,12 @@
 # Changelog for package auv_control_msgs
 
+## 0.2.0 (2025-05-03)
+
+- Implements the EndEffectorTrajectory message
+- Implements the EndEffectorTrajectoryPoint message
+- Implements the EndEffectorTrajectoryControllerState message
+- Adds the FollowEndEffectorTrajectory action
+
 ## 0.1.0 (2025-04-27)
 
 - Implements the IKControllerStateStamped message to support the new IK

auv_control_msgs/CMakeLists.txt

@@ -11,6 +11,10 @@ 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"
   DEPENDENCIES builtin_interfaces std_msgs geometry_msgs trajectory_msgs
 )
 

auv_control_msgs/action/FollowEndEffectorTrajectory.action

@@ -0,0 +1,40 @@
+# The end effector trajectory to follow.
+#
+# 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
+
+# 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.
+float64 path_tolerance
+
+# The maximum terminal error that the controller is allowed.
+# When this is set to 0, the tolerance will not affect the success of the action.
+float64 goal_tolerance
+
+---
+int32 error_code
+int32 SUCCESSFUL = 0
+int32 INVALID_GOAL = -1
+int32 OLD_HEADER_TIMESTAMP = -2
+int32 PATH_TOLERANCE_VIOLATED = -3
+int32 GOAL_TOLERANCE_VIOLATED = -4
+
+# A human-readable error description.
+string error_string
+
+---
+std_msgs/Header header
+
+# The reference pose for the controller at the current time instance.
+# This is distinct from the sample, which is retrieved at the next time
+# instance.
+geometry_msgs/Pose desired
+
+# The current end effector state.
+geometry_msgs/Pose actual
+
+# The squared Euclidean norm of the geodesic distance between the desired
+# and actual states.
+float64 error

auv_control_msgs/msg/EndEffectorTrajectory.msg

@@ -0,0 +1,4 @@
+std_msgs/Header header
+
+# The sequence of end effector points to track.
+auv_control_msgs/EndEffectorTrajectoryPoint[] points

auv_control_msgs/msg/EndEffectorTrajectoryControllerState.msg

@@ -0,0 +1,14 @@
+std_msgs/Header header
+
+# The reference sample from the trajectory. This is the sample from the current time instance and used
+# for error calculation.
+geometry_msgs/Pose reference
+
+# The current end effector state.
+geometry_msgs/Pose feedback
+
+# The squared Euclidean norm of the geodesic distance between the reference state and end effector state.
+float64 error
+
+# The trajectory controller command. This is the trajectory sample from the next time instance.
+geometry_msgs/Pose output

auv_control_msgs/msg/EndEffectorTrajectoryPoint.msg

@@ -0,0 +1,5 @@
+# The sequence of end effector poses.
+geometry_msgs/Pose point
+
+# The time that this point should be reached, measured from the start of the trajectory.
+builtin_interfaces/Duration time_from_start

auv_control_msgs/msg/IKControllerStateStamped.msg

@@ -1,6 +1,6 @@
 std_msgs/Header header
 
-# The name of the IK solver used by the controller
+# The name of the IK solver used by the controller.
 string solver_name
 
 # Position DoF names, e.g., joint or axis names.