Port end effector trajectory controller from Angler (backport #54)

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="mitchcol@oregonstate.edu">Colin Mitchell</maintainer>
   <maintainer email="everardo.a.gonzalez@gmail.com">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

auv_control_msgs/package.xml

@@ -2,7 +2,11 @@
 <?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>
+<<<<<<< HEAD
   <version>0.0.1</version>
+=======
+  <version>0.2.0</version>
+>>>>>>> d197fc7 (Port end effector trajectory controller from Angler (#54))
   <description>Custom messages for AUV controllers</description>
 
   <maintainer email="rakeshvivek97@gmail.com">Rakesh Vivekanandan</maintainer>

auv_controllers/CHANGELOG.md

@@ -0,0 +1,15 @@
+# Changelog for package auv_controllers
+
+## 0.2.0 (2025-05-03)
+
+- Adds the end effector trajectory controller
+
+## 0.1.0 (2025-04-27)
+
+- Adds the adaptive integral terminal sliding mode controller
+- Adds the task priority IK solver
+- Adds the IK whole-body controller
+- Adds the odom topic sensor
+- Adds the controller_common package
+- Adds the Gazebo passthrough thruster controller
+- Adds the thruster rotation rate controller

auv_controllers/package.xml

@@ -3,7 +3,11 @@
 <package format="3">
 
   <name>auv_controllers</name>
+<<<<<<< HEAD
   <version>0.0.1</version>
+=======
+  <version>0.2.0</version>
+>>>>>>> d197fc7 (Port end effector trajectory controller from Angler (#54))
   <description>Meta package for auv_controllers</description>
 
   <maintainer email="evanp922@gmail.com">Evan Palmer</maintainer>

controller_common/CHANGELOG.md

@@ -0,0 +1,9 @@
+# Changelog for package controller_common
+
+## 0.2.0 (2025-05-03)
+
+- Adds the common::math::isclose method for comparing doubles
+
+## 0.1.0 (2025-04-27)
+
+- Ports reset message functions and error calculation to a common API

controller_common/include/controller_common/common.hpp

@@ -0,0 +1,71 @@
+// Copyright 2025, Evan Palmer
+//
+// 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.
+
+#pragma once
+
+#include <vector>
+
+#include "geometry_msgs/msg/pose.hpp"
+#include "geometry_msgs/msg/twist.hpp"
+#include "geometry_msgs/msg/wrench.hpp"
+#include "nav_msgs/msg/odometry.hpp"
+
+namespace common
+{
+
+namespace messages
+{
+
+auto to_vector(const geometry_msgs::msg::Pose & pose) -> std::vector<double>;
+
+auto to_vector(const geometry_msgs::msg::Twist & twist) -> std::vector<double>;
+
+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_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 reset_message(geometry_msgs::msg::Pose * msg) -> void;
+
+auto reset_message(geometry_msgs::msg::Twist * msg) -> void;
+
+auto reset_message(geometry_msgs::msg::Wrench * msg) -> void;
+
+auto reset_message(nav_msgs::msg::Odometry * msg) -> void;
+
+}  // namespace messages
+
+namespace math
+{
+
+auto calculate_error(const std::vector<double> & reference, const std::vector<double> & state) -> std::vector<double>;
+
+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

@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+
+  <name>controller_common</name>
+  <version>0.2.0</version>
+  <description>Common interfaces for controllers used in this project</description>
+
+  <maintainer email="evanp922@gmail.com">Evan Palmer</maintainer>
+  <license>MIT</license>
+
+  <url type="repository">https://github.com/Robotic-Decision-Making-Lab/auv_controllers.git</url>
+  <url type="bugtracker">https://github.com/Robotic-Decision-Making-Lab/auv_controllers/issues</url>
+
+  <author>Evan Palmer</author>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>