Port end effector trajectory controller from Angler

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 (??)
+
 ## 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-04-28)
+
+- 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,21 @@
+auv_control_msgs/EndEffectorTrajectory trajectory
+
+float64 path_tolerance
+
+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
+
+string error_string
+
+---
+std_msgs/Header header
+geometry_msgs/Pose desired
+geometry_msgs/Pose actual
+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 duration 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.

auv_control_msgs/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>auv_control_msgs</name>
-  <version>0.1.0</version>
+  <version>0.2.0</version>
   <description>Custom messages for AUV controllers</description>
 
   <maintainer email="[email protected]">Rakesh Vivekanandan</maintainer>

auv_controllers/CHANGELOG.md

@@ -1,5 +1,9 @@
 # Changelog for package auv_controllers
 
+## 0.2.0 (??)
+
+- Adds the end effector trajectory controller
+
 ## 0.1.0 (2025-04-27)
 
 - Adds the adaptive integral terminal sliding mode controller

auv_controllers/package.xml

@@ -3,7 +3,7 @@
 <package format="3">
 
   <name>auv_controllers</name>
-  <version>0.1.0</version>
+  <version>0.2.0</version>
   <description>Meta package for auv_controllers</description>
 
   <maintainer email="[email protected]">Evan Palmer</maintainer>

controller_common/CHANGELOG.md

@@ -1,5 +1,7 @@
 # Changelog for package controller_common
 
+## 0.2.0 (??)
+
 ## 0.1.0 (2025-04-27)
 
 - Ports reset message functions and error calculation to a common API

controller_common/include/controller_common/common.hpp

@@ -64,6 +64,8 @@ auto has_nan(const std::vector<double> & vec) -> bool;
 
 auto all_nan(const std::vector<double> & vec) -> bool;
 
+auto isclose(double a, double b, double rtol = 1e-05, double atol = 1e-08) -> bool;
+
 }  // namespace math
 
 }  // namespace common

controller_common/package.xml

@@ -3,7 +3,7 @@
 <package format="3">
 
   <name>controller_common</name>
-  <version>0.1.0</version>
+  <version>0.2.0</version>
   <description>Common interfaces for controllers used in this project</description>
 
   <maintainer email="[email protected]">Evan Palmer</maintainer>

controller_common/src/common.cpp

@@ -171,6 +171,11 @@ auto all_nan(const std::vector<double> & vec) -> bool
   return std::ranges::all_of(vec, [](double x) { return std::isnan(x); });
 }
 
+auto isclose(double a, double b, double rtol, double atol) -> bool
+{
+  return std::abs(a - b) <= (atol + rtol * std::abs(b));
+}
+
 }  // namespace math
 
 }  // namespace common

end_effector_trajectory_controller/CMakeLists.txt

@@ -0,0 +1,96 @@
+cmake_minimum_required(VERSION 3.23)
+project(end_effector_trajectory_controller)
+
+if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
+    add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+include(GNUInstallDirs)
+
+find_package(ament_cmake REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(realtime_tools REQUIRED)
+find_package(auv_control_msgs REQUIRED)
+find_package(controller_common REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(tf2_eigen REQUIRED)
+find_package(hardware_interface REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
+find_package(generate_parameter_library REQUIRED)
+find_package(controller_interface REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(lifecycle_msgs REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+
+generate_parameter_library(end_effector_trajectory_controller_parameters
+  src/end_effector_trajectory_controller_parameters.yaml
+)
+
+add_library(end_effector_trajectory_controller SHARED)
+target_sources(
+    end_effector_trajectory_controller
+    PRIVATE src/end_effector_trajectory_controller.cpp src/trajectory.cpp
+    PUBLIC
+        FILE_SET HEADERS
+        BASE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include
+        FILES
+            ${CMAKE_CURRENT_SOURCE_DIR}/include/end_effector_trajectory_controller/end_effector_trajectory_controller.hpp
+            ${CMAKE_CURRENT_SOURCE_DIR}/include/end_effector_trajectory_controller/trajectory.hpp
+)
+target_compile_features(end_effector_trajectory_controller PUBLIC cxx_std_23)
+target_link_libraries(
+    end_effector_trajectory_controller
+    PUBLIC
+        end_effector_trajectory_controller_parameters
+        realtime_tools::realtime_tools
+        controller_common::controller_common
+        hardware_interface::hardware_interface
+        rclcpp::rclcpp
+        rclcpp_lifecycle::rclcpp_lifecycle
+        tf2_ros::tf2_ros
+        tf2_eigen::tf2_eigen
+        tf2::tf2
+        controller_interface::controller_interface
+        rclcpp_action::rclcpp_action
+        tf2_geometry_msgs::tf2_geometry_msgs
+        ${geometry_msgs_TARGETS}
+        ${auv_control_msgs_TARGETS}
+        ${lifecycle_msgs_TARGETS}
+)
+
+pluginlib_export_plugin_description_file(controller_interface end_effector_trajectory_controller.xml)
+
+install(
+    TARGETS
+        end_effector_trajectory_controller
+        end_effector_trajectory_controller_parameters
+    EXPORT export_end_effector_trajectory_controller
+    LIBRARY DESTINATION lib/${PROJECT_NAME}
+    ARCHIVE DESTINATION lib/${PROJECT_NAME}
+    RUNTIME DESTINATION bin/${PROJECT_NAME}
+    FILE_SET HEADERS
+)
+
+ament_export_targets(export_end_effector_trajectory_controller HAS_LIBRARY_TARGET)
+ament_export_dependencies(
+  "geometry_msgs"
+  "realtime_tools"
+  "auv_control_msgs"
+  "controller_common"
+  "tf2"
+  "tf2_ros"
+  "tf2_eigen"
+  "hardware_interface"
+  "rclcpp"
+  "rclcpp_lifecycle"
+  "controller_interface"
+  "rclcpp_action"
+  "lifecycle_msgs"
+  "tf2_geometry_msgs"
+)
+
+ament_package()

end_effector_trajectory_controller/LICENSE

@@ -0,0 +1,17 @@
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+THE SOFTWARE.

end_effector_trajectory_controller/README.md

@@ -0,0 +1,30 @@
+# End Effector Trajectory Controller
+
+The end effector trajectory controller interpolates an end effector motion plan
+for whole-body inverse kinematic control. The positions are interpolated using
+linear interpolation. The orientations are interpolated using spherical linear
+interpolation.
+
+## Plugin Library
+
+end_effector_trajectory_controller/EndEffectorTrajectoryController
+
+## References
+
+The input to this controller is a sequence of end effector poses.
+
+## Commands
+
+The output of this controller is a sampled end effector pose.
+
+## Subscribers
+
+- end_effector_trajectory_controller/trajectory [auv_control_msgs::msg::EndEffectorTrajectory]
+
+## Action Servers
+
+- end_effector_trajectory_controller/follow_trajectory [auv_control_msgs::action::FollowEndEffectorTrajectory]
+
+## Publishers
+
+- end_effector_trajectory_controller/status [auv_control_msgs::msg::EndEffectorTrajectoryControllerState]

end_effector_trajectory_controller/end_effector_trajectory_controller.xml

@@ -0,0 +1,9 @@
+<library path="end_effector_trajectory_controller">
+  <class name="end_effector_trajectory_controller/EndEffectorTrajectoryController"
+         type="end_effector_trajectory_controller::EndEffectorTrajectoryController"
+         base_class_type="controller_interface::ControllerInterface">
+    <description>
+      End effector trajectory controller for UVMS whole-body control
+    </description>
+  </class>
+</library>