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

(cherry picked from commit d197fc7)

# Conflicts:
#	README.md
#	auv_control_demos/CHANGELOG.md
#	auv_control_demos/package.xml
#	auv_control_msgs/CHANGELOG.md
#	auv_control_msgs/CMakeLists.txt
#	auv_control_msgs/msg/IKControllerStateStamped.msg
#	auv_control_msgs/package.xml
#	auv_controllers/CHANGELOG.md
#	auv_controllers/package.xml
#	controller_common/CHANGELOG.md
#	controller_common/include/controller_common/common.hpp
#	controller_common/package.xml
#	controller_common/src/common.cpp
#	ik_solvers/CHANGELOG.md
#	ik_solvers/include/ik_solvers/solver.hpp
#	ik_solvers/include/ik_solvers/task_priority_solver.hpp
#	ik_solvers/package.xml
#	ik_solvers/src/solver.cpp
#	ik_solvers/src/task_priority_solver.cpp
#	thruster_allocation_matrix_controller/CHANGELOG.md
#	thruster_allocation_matrix_controller/package.xml
#	thruster_controllers/CHANGELOG.md
#	thruster_controllers/package.xml
#	topic_sensors/CHANGELOG.md
#	topic_sensors/package.xml
#	velocity_controllers/CHANGELOG.md
#	velocity_controllers/package.xml
#	whole_body_controllers/CHANGELOG.md
#	whole_body_controllers/CMakeLists.txt
#	whole_body_controllers/include/whole_body_controllers/ik_controller.hpp
#	whole_body_controllers/package.xml
#	whole_body_controllers/src/ik_controller.cpp

Author: evan-palmer

README.md

@@ -1,9 +1,22 @@
 # auv_controllers
 
+<<<<<<< HEAD
 auv_controllers is a collection of chainable 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.
+=======
+auv_controllers is a collection of controllers for autonomous underwater
+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>
+>>>>>>> d197fc7 (Port end effector trajectory controller from Angler (#54))
 
 > [!NOTE]
 > If you are interested in adding your own controller to this project, please
@@ -32,10 +45,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

@@ -0,0 +1,8 @@
+# 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
+correct topic names

auv_control_demos/package.xml

@@ -2,9 +2,14 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>auv_control_demos</name>
+<<<<<<< HEAD
   <version>0.0.1</version>
   <description>Example package that includes demos for using auv_controllers in individual and
     chained modes</description>
+=======
+  <version>0.2.0</version>
+  <description>Example package that includes demos for using auv_controllers in individual and chained modes</description>
+>>>>>>> d197fc7 (Port end effector trajectory controller from Angler (#54))
 
   <maintainer email="[email protected]">Colin Mitchell</maintainer>
   <maintainer email="[email protected]">Everardo Gonzalez</maintainer>

auv_control_msgs/CHANGELOG.md

@@ -0,0 +1,13 @@
+# 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
+controller

auv_control_msgs/CMakeLists.txt

@@ -8,7 +8,16 @@ find_package(rosidl_default_generators REQUIRED)
 
 rosidl_generate_interfaces(auv_control_msgs
   "msg/MultiActuatorStateStamped.msg"
+<<<<<<< HEAD
   DEPENDENCIES builtin_interfaces std_msgs
+=======
+  "msg/IKControllerStateStamped.msg"
+  "msg/EndEffectorTrajectoryPoint.msg"
+  "msg/EndEffectorTrajectory.msg"
+  "msg/EndEffectorTrajectoryControllerState.msg"
+  "action/FollowEndEffectorTrajectory.action"
+  DEPENDENCIES builtin_interfaces std_msgs geometry_msgs trajectory_msgs
+>>>>>>> d197fc7 (Port end effector trajectory controller from Angler (#54))
 )
 
 ament_package()

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

@@ -0,0 +1,20 @@
+std_msgs/Header header
+
+# The name of the IK solver used by the controller.
+string solver_name
+
+# Position DoF names, e.g., joint or axis names.
+string[] position_joint_names
+
+# Velocity DoF names.
+string[] velocity_joint_names
+
+# The reference end effector pose.
+geometry_msgs/Pose reference
+
+# Time between two consecutive updates/execution of the control law.
+# This is often used for integration of the solution to determine the desired positions.
+float64 time_step
+
+# The IK solver solution.
+trajectory_msgs/JointTrajectoryPoint solution