diff --git a/README.md b/README.md index 815043a..9f3e62e 100644 --- a/README.md +++ b/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. + +

+ UVMS whole-body control + AUV control +

## 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 diff --git a/auv_control_demos/CHANGELOG.md b/auv_control_demos/CHANGELOG.md index 0ffd412..d4819d7 100644 --- a/auv_control_demos/CHANGELOG.md +++ b/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 diff --git a/auv_control_demos/package.xml b/auv_control_demos/package.xml index 7ad7ca3..eda73df 100644 --- a/auv_control_demos/package.xml +++ b/auv_control_demos/package.xml @@ -3,7 +3,7 @@ auv_control_demos - 0.1.0 + 0.2.0 Example package that includes demos for using auv_controllers in individual and chained modes Colin Mitchell diff --git a/auv_control_msgs/CHANGELOG.md b/auv_control_msgs/CHANGELOG.md index bff2966..cf6d0d6 100644 --- a/auv_control_msgs/CHANGELOG.md +++ b/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 diff --git a/auv_control_msgs/CMakeLists.txt b/auv_control_msgs/CMakeLists.txt index 0db9d40..83ab507 100644 --- a/auv_control_msgs/CMakeLists.txt +++ b/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 ) diff --git a/auv_control_msgs/action/FollowEndEffectorTrajectory.action b/auv_control_msgs/action/FollowEndEffectorTrajectory.action new file mode 100644 index 0000000..1a12cd6 --- /dev/null +++ b/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 diff --git a/auv_control_msgs/msg/EndEffectorTrajectory.msg b/auv_control_msgs/msg/EndEffectorTrajectory.msg new file mode 100644 index 0000000..114b86a --- /dev/null +++ b/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 diff --git a/auv_control_msgs/msg/EndEffectorTrajectoryControllerState.msg b/auv_control_msgs/msg/EndEffectorTrajectoryControllerState.msg new file mode 100644 index 0000000..7410871 --- /dev/null +++ b/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 diff --git a/auv_control_msgs/msg/EndEffectorTrajectoryPoint.msg b/auv_control_msgs/msg/EndEffectorTrajectoryPoint.msg new file mode 100644 index 0000000..dbca6e6 --- /dev/null +++ b/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 diff --git a/auv_control_msgs/msg/IKControllerStateStamped.msg b/auv_control_msgs/msg/IKControllerStateStamped.msg index 0838322..3db2b1d 100644 --- a/auv_control_msgs/msg/IKControllerStateStamped.msg +++ b/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. diff --git a/auv_control_msgs/package.xml b/auv_control_msgs/package.xml index 3cb49cf..fa31191 100644 --- a/auv_control_msgs/package.xml +++ b/auv_control_msgs/package.xml @@ -2,7 +2,7 @@ auv_control_msgs - 0.1.0 + 0.2.0 Custom messages for AUV controllers Rakesh Vivekanandan diff --git a/auv_controllers/CHANGELOG.md b/auv_controllers/CHANGELOG.md index 1e4386d..48279b9 100644 --- a/auv_controllers/CHANGELOG.md +++ b/auv_controllers/CHANGELOG.md @@ -1,5 +1,9 @@ # 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 diff --git a/auv_controllers/package.xml b/auv_controllers/package.xml index c8cf318..11d30cf 100644 --- a/auv_controllers/package.xml +++ b/auv_controllers/package.xml @@ -3,7 +3,7 @@ auv_controllers - 0.1.0 + 0.2.0 Meta package for auv_controllers Evan Palmer diff --git a/controller_common/CHANGELOG.md b/controller_common/CHANGELOG.md index 986eb12..63dadc7 100644 --- a/controller_common/CHANGELOG.md +++ b/controller_common/CHANGELOG.md @@ -1,5 +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 diff --git a/controller_common/include/controller_common/common.hpp b/controller_common/include/controller_common/common.hpp index ebd77e0..4da7d36 100644 --- a/controller_common/include/controller_common/common.hpp +++ b/controller_common/include/controller_common/common.hpp @@ -64,6 +64,8 @@ auto has_nan(const std::vector & vec) -> bool; auto all_nan(const std::vector & vec) -> bool; +auto isclose(double a, double b, double rtol = 1e-05, double atol = 1e-08) -> bool; + } // namespace math } // namespace common diff --git a/controller_common/package.xml b/controller_common/package.xml index fffc813..9122575 100644 --- a/controller_common/package.xml +++ b/controller_common/package.xml @@ -3,7 +3,7 @@ controller_common - 0.1.0 + 0.2.0 Common interfaces for controllers used in this project Evan Palmer diff --git a/controller_common/src/common.cpp b/controller_common/src/common.cpp index 163ad61..c781378 100644 --- a/controller_common/src/common.cpp +++ b/controller_common/src/common.cpp @@ -171,6 +171,11 @@ auto all_nan(const std::vector & 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 diff --git a/end_effector_trajectory_controller/CMakeLists.txt b/end_effector_trajectory_controller/CMakeLists.txt new file mode 100644 index 0000000..ae73f59 --- /dev/null +++ b/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() diff --git a/end_effector_trajectory_controller/LICENSE b/end_effector_trajectory_controller/LICENSE new file mode 100644 index 0000000..30e8e2e --- /dev/null +++ b/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. diff --git a/end_effector_trajectory_controller/README.md b/end_effector_trajectory_controller/README.md new file mode 100644 index 0000000..426bd81 --- /dev/null +++ b/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] diff --git a/end_effector_trajectory_controller/end_effector_trajectory_controller.xml b/end_effector_trajectory_controller/end_effector_trajectory_controller.xml new file mode 100644 index 0000000..9bf9cc0 --- /dev/null +++ b/end_effector_trajectory_controller/end_effector_trajectory_controller.xml @@ -0,0 +1,9 @@ + + + + End effector trajectory controller for UVMS whole-body control + + + diff --git a/end_effector_trajectory_controller/include/end_effector_trajectory_controller/end_effector_trajectory_controller.hpp b/end_effector_trajectory_controller/include/end_effector_trajectory_controller/end_effector_trajectory_controller.hpp new file mode 100644 index 0000000..933c91b --- /dev/null +++ b/end_effector_trajectory_controller/include/end_effector_trajectory_controller/end_effector_trajectory_controller.hpp @@ -0,0 +1,139 @@ +// 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 + +#include "auv_control_msgs/action/follow_end_effector_trajectory.hpp" +#include "auv_control_msgs/msg/end_effector_trajectory_controller_state.hpp" +#include "controller_common/common.hpp" +#include "controller_interface/controller_interface.hpp" +#include "end_effector_trajectory_controller/trajectory.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/server.hpp" +#include "realtime_tools/realtime_buffer.hpp" +#include "realtime_tools/realtime_publisher.hpp" +#include "realtime_tools/realtime_server_goal_handle.hpp" +#include "tf2/exceptions.h" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" + +// auto-generated by generate_parameter_library +#include + +namespace end_effector_trajectory_controller +{ + +class EndEffectorTrajectoryController : public controller_interface::ControllerInterface +{ +public: + EndEffectorTrajectoryController() = default; + + auto on_init() -> controller_interface::CallbackReturn override; + + // NOLINTNEXTLINE(modernize-use-nodiscard) + auto command_interface_configuration() const -> controller_interface::InterfaceConfiguration override; + + // NOLINTNEXTLINE(modernize-use-nodiscard) + auto state_interface_configuration() const -> controller_interface::InterfaceConfiguration override; + + auto on_configure(const rclcpp_lifecycle::State & previous_state) -> controller_interface::CallbackReturn override; + + auto on_activate(const rclcpp_lifecycle::State & previous_state) -> controller_interface::CallbackReturn override; + + auto update(const rclcpp::Time & time, const rclcpp::Duration & period) -> controller_interface::return_type override; + +protected: + auto update_parameters() -> void; + + auto configure_parameters() -> controller_interface::CallbackReturn; + + auto update_end_effector_state() -> controller_interface::return_type; + + [[nodiscard]] auto validate_trajectory(const auv_control_msgs::msg::EndEffectorTrajectory & trajectory) const -> bool; + + // controller state + using ControllerState = auv_control_msgs::msg::EndEffectorTrajectoryControllerState; + std::shared_ptr> controller_state_pub_; + std::unique_ptr> rt_controller_state_pub_; + + // the end effector states can be captured in one of three ways: + // 1. using the topic interface - when available, this is preferred over the tf2 interface + // 2. using the state interfaces - this is the default, but often not available + // 3. using tf2 - this is the most common interface, but requires a transform to be published and is not as robust + realtime_tools::RealtimeBuffer end_effector_state_; + std::shared_ptr> end_effector_state_sub_; + + std::unique_ptr tf_buffer_; + std::unique_ptr tf_listener_; + + // the end effector trajectories can be set using either the topic or action server + // the action server is preferred and easier to integrate into state-machines/behavior trees, but can be a bit + // cumbersome to interface with. on the other hand, the topic interface is easier to use, but doesn't integrate + // well with high-level coordinators + realtime_tools::RealtimeBuffer rt_trajectory_; + std::shared_ptr> trajectory_sub_; + + using FollowTrajectory = auv_control_msgs::action::FollowEndEffectorTrajectory; + using RealtimeGoalHandle = realtime_tools::RealtimeServerGoalHandle; + using RealtimeGoalHandlePtr = std::shared_ptr; + using RealtimeGoalHandleBuffer = realtime_tools::RealtimeBuffer; + + std::shared_ptr> action_server_; + RealtimeGoalHandleBuffer rt_active_goal_; + realtime_tools::RealtimeBuffer rt_goal_in_progress_; + std::shared_ptr goal_handle_timer_; + std::chrono::nanoseconds action_monitor_period_ = std::chrono::nanoseconds(50000000); // 50ms + + realtime_tools::RealtimeBuffer rt_first_sample_; // used to sample the trajectory for the first time + realtime_tools::RealtimeBuffer rt_holding_position_; // used to hold the current end effector pose + + // the update period is used to sample the "next" trajectory point + rclcpp::Duration update_period_{0, 0}; + + std::shared_ptr param_listener_; + end_effector_trajectory_controller::Params params_; + + // error tolerances + // the default tolerances are extracted from the parameters and applied when the action interface is not used + // if the action interface is being used, then the tolerances set in the goal are applied + double default_path_tolerance_, default_goal_tolerance_; + realtime_tools::RealtimeBuffer rt_goal_tolerance_, rt_path_tolerance_; + + std::vector dofs_; + std::size_t n_dofs_; + + rclcpp::Logger logger_{rclcpp::get_logger("end_effector_trajectory_controller")}; + +private: + template + auto write_command(T & interfaces, const geometry_msgs::msg::Pose & command) -> void + { + const std::vector vec = common::messages::to_vector(command); + for (const auto & [i, dof] : std::views::enumerate(dofs_)) { + if (!interfaces[i].set_value(vec[i])) { + RCLCPP_WARN(logger_, "Failed to set command for joint %s", dof.c_str()); // NOLINT + } + } + } +}; + +} // namespace end_effector_trajectory_controller diff --git a/end_effector_trajectory_controller/include/end_effector_trajectory_controller/trajectory.hpp b/end_effector_trajectory_controller/include/end_effector_trajectory_controller/trajectory.hpp new file mode 100644 index 0000000..485b640 --- /dev/null +++ b/end_effector_trajectory_controller/include/end_effector_trajectory_controller/trajectory.hpp @@ -0,0 +1,78 @@ +// 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 +#include +#include +#include + +#include "auv_control_msgs/msg/end_effector_trajectory.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace end_effector_trajectory_controller +{ + +enum class SampleError : std::uint8_t +{ + SAMPLE_TIME_BEFORE_START, + SAMPLE_TIME_AFTER_END, + EMPTY_TRAJECTORY, +}; + +class Trajectory +{ +public: + Trajectory() = default; + + /// Create a new trajectory given a trajectory message and the initial state. + Trajectory(const auv_control_msgs::msg::EndEffectorTrajectory & trajectory, const geometry_msgs::msg::Pose & state); + + /// Whether or not the trajectory is empty. + [[nodiscard]] auto empty() const -> bool; + + /// Get the starting time of the trajectory. + [[nodiscard]] auto start_time() const -> rclcpp::Time; + + /// Get the ending time of the trajectory. + [[nodiscard]] auto end_time() const -> rclcpp::Time; + + /// Get the first point in the trajectory. + [[nodiscard]] auto start_point() const -> std::optional; + + /// Get the last point in the trajectory. + [[nodiscard]] auto end_point() const -> std::optional; + + /// Sample a point in the trajectory at the given time. + [[nodiscard]] auto sample(const rclcpp::Time & sample_time) const + -> std::expected; + + /// Reset the initial end effector state and trajectory start time. + auto reset_initial_state(const geometry_msgs::msg::Pose & state) -> void; + +private: + auv_control_msgs::msg::EndEffectorTrajectory points_; + rclcpp::Time initial_time_; + geometry_msgs::msg::Pose initial_state_; +}; + +} // namespace end_effector_trajectory_controller diff --git a/end_effector_trajectory_controller/package.xml b/end_effector_trajectory_controller/package.xml new file mode 100644 index 0000000..910998f --- /dev/null +++ b/end_effector_trajectory_controller/package.xml @@ -0,0 +1,37 @@ + + + + + end_effector_trajectory_controller + 0.2.0 + End effector trajectory tracking controller for UVMS control + + Evan Palmer + MIT + + https://github.com/Robotic-Decision-Making-Lab/auv_controllers.git + https://github.com/Robotic-Decision-Making-Lab/auv_controllers/issues + + Evan Palmer + + ament_cmake + eigen3_cmake_module + + eigen + rclcpp + ros2_control + controller_interface + hardware_interface + rclcpp_lifecycle + generate_parameter_library + control_msgs + geometry_msgs + controller_common + auv_control_msgs + rclcpp_action + lifecycle_msgs + + + ament_cmake + + diff --git a/end_effector_trajectory_controller/src/end_effector_trajectory_controller.cpp b/end_effector_trajectory_controller/src/end_effector_trajectory_controller.cpp new file mode 100644 index 0000000..7af5d84 --- /dev/null +++ b/end_effector_trajectory_controller/src/end_effector_trajectory_controller.cpp @@ -0,0 +1,468 @@ +// 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. + +#include "end_effector_trajectory_controller/end_effector_trajectory_controller.hpp" + +#include +#include + +#include "controller_common/common.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "tf2_eigen/tf2_eigen.hpp" + +namespace end_effector_trajectory_controller +{ + +namespace +{ + +auto geodesic_error(const geometry_msgs::msg::Pose & goal, const geometry_msgs::msg::Pose & state) -> double +{ + Eigen::Isometry3d goal_mat, state_mat; // NOLINT + tf2::fromMsg(goal, goal_mat); + tf2::fromMsg(state, state_mat); + return std::pow((goal_mat.inverse() * state_mat).matrix().log().norm(), 2); +} + +} // namespace + +auto EndEffectorTrajectoryController::on_init() -> controller_interface::CallbackReturn +{ + param_listener_ = std::make_shared(get_node()); + params_ = param_listener_->get_params(); + logger_ = get_node()->get_logger(); + return controller_interface::CallbackReturn::SUCCESS; +} + +// NOLINTNEXTLINE(readability-convert-member-functions-to-static) +auto EndEffectorTrajectoryController::update_parameters() -> void +{ + if (!param_listener_->is_old(params_)) { + return; + } + param_listener_->refresh_dynamic_parameters(); + params_ = param_listener_->get_params(); +} + +auto EndEffectorTrajectoryController::configure_parameters() -> controller_interface::CallbackReturn +{ + update_parameters(); + + dofs_ = params_.joints; + n_dofs_ = dofs_.size(); + + default_path_tolerance_ = params_.path_tolerance; + default_goal_tolerance_ = params_.goal_tolerance; + + rt_path_tolerance_.writeFromNonRT(params_.path_tolerance); + rt_goal_tolerance_.writeFromNonRT(params_.goal_tolerance); + + auto period = std::chrono::duration(1.0 / params_.action_monitor_rate); + action_monitor_period_ = std::chrono::duration_cast(period); + + return controller_interface::CallbackReturn::SUCCESS; +} + +auto EndEffectorTrajectoryController::validate_trajectory( + const auv_control_msgs::msg::EndEffectorTrajectory & trajectory) const -> bool +{ + if (trajectory.points.empty()) { + RCLCPP_ERROR(logger_, "Received empty trajectory"); // NOLINT + return false; + } + + for (const auto & point : trajectory.points) { + if (common::math::has_nan(common::messages::to_vector(point.point))) { + RCLCPP_ERROR(logger_, "Received trajectory point with NaN value"); // NOLINT + return false; + } + } + + const rclcpp::Time start_time = trajectory.header.stamp; + if (start_time.seconds() != 0.0) { + const rclcpp::Time end_time = start_time + trajectory.points.back().time_from_start; + if (end_time < get_node()->now()) { + RCLCPP_ERROR(logger_, "Received trajectory with end time in the past"); // NOLINT + return false; + } + } + + // NOLINTNEXTLINE(readability-use-anyofallof) + for (const auto [p1, p2] : std::views::zip(trajectory.points, trajectory.points | std::views::drop(1))) { + const rclcpp::Duration p1_start = p1.time_from_start; + const rclcpp::Duration p2_start = p2.time_from_start; + if (p1_start >= p2_start) { + RCLCPP_ERROR(logger_, "Trajectory points are not in order"); // NOLINT + return false; + } + } + + return true; +} + +auto EndEffectorTrajectoryController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) + -> controller_interface::CallbackReturn +{ + configure_parameters(); + + end_effector_state_.writeFromNonRT(geometry_msgs::msg::Pose()); + command_interfaces_.reserve(n_dofs_); + update_period_ = rclcpp::Duration(0.0, static_cast(1.0e9 / static_cast(get_update_rate()))); + + if (params_.use_external_measured_states) { + end_effector_state_sub_ = get_node()->create_subscription( + "~/end_effector_state", + rclcpp::SystemDefaultsQoS(), + [this](const std::shared_ptr msg) { // NOLINT + end_effector_state_.writeFromNonRT(*msg); + }); + } else { + if (params_.lookup_end_effector_transform) { + tf_buffer_ = std::make_unique(get_node()->get_clock()); + tf_listener_ = std::make_unique(*tf_buffer_); + } + } + + trajectory_sub_ = get_node()->create_subscription( + "~/trajectory", + rclcpp::SystemDefaultsQoS(), + [this](const std::shared_ptr msg) { // NOLINT + auto updated_msg = *msg; + if (!validate_trajectory(updated_msg)) { + RCLCPP_ERROR(logger_, "Ignoring invalid trajectory message"); // NOLINT + return; + } + const rclcpp::Time start_time = updated_msg.header.stamp; + if (common::math::isclose(start_time.seconds(), 0.0)) { + updated_msg.header.stamp = get_node()->now(); + } + RCLCPP_INFO(logger_, "Received new trajectory message"); // NOLINT + rt_trajectory_.writeFromNonRT(Trajectory(updated_msg, *end_effector_state_.readFromNonRT())); + rt_goal_tolerance_.writeFromNonRT(default_goal_tolerance_); + rt_path_tolerance_.writeFromNonRT(default_path_tolerance_); + rt_first_sample_.writeFromNonRT(true); + rt_holding_position_.writeFromNonRT(false); + }); + + auto handle_goal = [this]( + const rclcpp_action::GoalUUID & /*uuid*/, + std::shared_ptr goal) { // NOLINT + RCLCPP_INFO(logger_, "Received new trajectory goal"); // NOLINT + if (get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { + RCLCPP_ERROR(logger_, "Can't accept new action goals. Controller is not running."); // NOLINT + return rclcpp_action::GoalResponse::REJECT; + } + auto updated_msg = goal->trajectory; // make a non-const copy + if (!validate_trajectory(updated_msg)) { + RCLCPP_ERROR(logger_, "Ignoring invalid trajectory message"); // NOLINT + return rclcpp_action::GoalResponse::REJECT; + } + const rclcpp::Time start_time = updated_msg.header.stamp; + if (common::math::isclose(start_time.seconds(), 0.0)) { + updated_msg.header.stamp = get_node()->now(); + } + rt_trajectory_.writeFromNonRT(Trajectory(updated_msg, *end_effector_state_.readFromNonRT())); + rt_first_sample_.writeFromNonRT(true); + rt_holding_position_.writeFromNonRT(false); + RCLCPP_INFO(logger_, "Accepted new trajectory goal"); // NOLINT + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; + + auto handle_cancel = [this](const std::shared_ptr> gh) { // NOLINT + RCLCPP_INFO(logger_, "Received cancel action goal"); // NOLINT + const auto active_goal = *rt_active_goal_.readFromNonRT(); + if (active_goal && active_goal->gh_ == gh) { + RCLCPP_INFO(logger_, "Canceling active goal"); // NOLINT + auto action_result = std::make_shared(); + active_goal->setCanceled(action_result); + rt_holding_position_.writeFromNonRT(true); + rt_first_sample_.writeFromNonRT(true); + rt_goal_in_progress_.writeFromNonRT(false); + } + return rclcpp_action::CancelResponse::ACCEPT; + }; + + auto handle_accepted = [this](std::shared_ptr> gh) { // NOLINT + RCLCPP_INFO(logger_, "Received accepted action goal"); // NOLINT + rt_goal_in_progress_.writeFromNonRT(true); + const auto active_goal = *rt_active_goal_.readFromNonRT(); + if (active_goal) { + RCLCPP_INFO(logger_, "Canceling active goal"); // NOLINT + auto action_result = std::make_shared(); + action_result->error_code = FollowTrajectory::Result::INVALID_GOAL; + action_result->error_string = "Current goal cancelled by a new incoming action."; + active_goal->setCanceled(action_result); + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + } + + rt_goal_tolerance_.writeFromNonRT(gh->get_goal()->goal_tolerance); + rt_path_tolerance_.writeFromNonRT(gh->get_goal()->path_tolerance); + + const RealtimeGoalHandlePtr rt_gh = std::make_shared(gh); + rt_gh->execute(); + rt_active_goal_.writeFromNonRT(rt_gh); + + goal_handle_timer_.reset(); + goal_handle_timer_ = get_node()->create_wall_timer(action_monitor_period_, [rt_gh]() { rt_gh->runNonRealtime(); }); + }; + + action_server_ = rclcpp_action::create_server( + get_node(), "~/follow_trajectory", handle_goal, handle_cancel, handle_accepted); + + controller_state_pub_ = get_node()->create_publisher("~/status", rclcpp::SystemDefaultsQoS()); + rt_controller_state_pub_ = + std::make_unique>(controller_state_pub_); + + return controller_interface::CallbackReturn::SUCCESS; +} + +auto EndEffectorTrajectoryController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) + -> controller_interface::CallbackReturn +{ + rt_first_sample_.writeFromNonRT(true); + rt_holding_position_.writeFromNonRT(true); // hold position until a trajectory is received + common::messages::reset_message(end_effector_state_.readFromNonRT()); + return controller_interface::CallbackReturn::SUCCESS; +} + +auto EndEffectorTrajectoryController::command_interface_configuration() const + -> controller_interface::InterfaceConfiguration +{ + controller_interface::InterfaceConfiguration config; + config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + config.names.reserve(n_dofs_); + + std::ranges::transform(dofs_, std::back_inserter(config.names), [this](const auto & dof) { + return params_.reference_controller.empty() + ? std::format("{}/{}", dof, hardware_interface::HW_IF_POSITION) + : std::format("{}/{}/{}", params_.reference_controller, dof, hardware_interface::HW_IF_POSITION); + }); + + return config; +} + +auto EndEffectorTrajectoryController::state_interface_configuration() const + -> controller_interface::InterfaceConfiguration +{ + controller_interface::InterfaceConfiguration config; + if (params_.use_external_measured_states || params_.lookup_end_effector_transform) { + config.type = controller_interface::interface_configuration_type::NONE; + return config; + } + config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + config.names.reserve(n_dofs_); + + std::ranges::transform(dofs_, std::back_inserter(config.names), [](const auto & dof) { + return std::format("{}/{}", dof, hardware_interface::HW_IF_POSITION); + }); + + return config; +} + +auto EndEffectorTrajectoryController::update_end_effector_state() -> controller_interface::return_type +{ + auto * end_effector_state = end_effector_state_.readFromRT(); + if (params_.lookup_end_effector_transform) { + const auto to_frame = params_.end_effector_frame_id; + const auto from_frame = params_.odom_frame_id; + try { + const auto transform = tf_buffer_->lookupTransform(from_frame, to_frame, tf2::TimePointZero); + end_effector_state->position.x = transform.transform.translation.x; + end_effector_state->position.y = transform.transform.translation.y; + end_effector_state->position.z = transform.transform.translation.z; + end_effector_state->orientation = transform.transform.rotation; + } + catch (const tf2::TransformException & ex) { + const auto err = std::format("Failed to get transform from {} to {}: {}", from_frame, to_frame, ex.what()); + RCLCPP_DEBUG(logger_, err.c_str()); // NOLINT + return controller_interface::return_type::ERROR; + } + } else if (!params_.use_external_measured_states) { + auto get_value = [](const auto & interface) -> double { + return interface.get_optional().value_or(std::numeric_limits::quiet_NaN()); + }; + end_effector_state->position.x = get_value(state_interfaces_[0]); + end_effector_state->position.y = get_value(state_interfaces_[1]); + end_effector_state->position.z = get_value(state_interfaces_[2]); + end_effector_state->orientation.x = get_value(state_interfaces_[3]); + end_effector_state->orientation.y = get_value(state_interfaces_[4]); + end_effector_state->orientation.z = get_value(state_interfaces_[5]); + end_effector_state->orientation.w = get_value(state_interfaces_[6]); + } + + if (common::math::has_nan(common::messages::to_vector(*end_effector_state))) { + RCLCPP_DEBUG(logger_, "Received end effector state with NaN value."); // NOLINT + return controller_interface::return_type::ERROR; + } + return controller_interface::return_type::OK; +} + +auto EndEffectorTrajectoryController::update(const rclcpp::Time & time, const rclcpp::Duration & period) + -> controller_interface::return_type +{ + if (update_end_effector_state() != controller_interface::return_type::OK) { + RCLCPP_DEBUG(logger_, "Skipping controller update. Failed to update and validate interfaces"); // NOLINT + return controller_interface::return_type::OK; + } + + const auto active_goal = *rt_active_goal_.readFromRT(); + const bool goal_in_progress = *rt_goal_in_progress_.readFromRT(); + if (goal_in_progress && active_goal == nullptr) { + RCLCPP_DEBUG(logger_, "Goal in progress but no active goal. Ignoring update"); // NOLINT + return controller_interface::return_type::OK; + } + + const geometry_msgs::msg::Pose end_effector_state = *end_effector_state_.readFromRT(); + geometry_msgs::msg::Pose reference_state; + common::messages::reset_message(&reference_state); + auto command_state = end_effector_state; + double error = std::numeric_limits::quiet_NaN(); + + auto publish_controller_state = [this, &reference_state, &end_effector_state, &error, &command_state]() { + if (rt_controller_state_pub_ && rt_controller_state_pub_->trylock()) { + rt_controller_state_pub_->msg_.header.stamp = get_node()->now(); + rt_controller_state_pub_->msg_.reference = reference_state; + rt_controller_state_pub_->msg_.feedback = end_effector_state; + rt_controller_state_pub_->msg_.error = error; + rt_controller_state_pub_->msg_.output = command_state; + rt_controller_state_pub_->unlockAndPublish(); + } + }; + + // hold position until a new trajectory is received + if (*rt_holding_position_.readFromRT()) { + write_command(command_interfaces_, end_effector_state); + publish_controller_state(); + return controller_interface::return_type::OK; + } + + // set the sample time + rclcpp::Time sample_time = time; + if (*rt_first_sample_.readFromRT()) { + rt_trajectory_.readFromRT()->reset_initial_state(end_effector_state); + rt_first_sample_.writeFromNonRT(false); + sample_time += period; + } + + // we use the current sample to measure errors and the future sample as the command + // the future sample should be used in order to prevent the controller from lagging + const auto * trajectory = rt_trajectory_.readFromRT(); + const auto sampled_reference = trajectory->sample(sample_time); + const auto sampled_command = trajectory->sample(sample_time + update_period_); + + // get the reference state and error + // the scenarios where this will not have a value are when the reference time is before or after the trajectory + if (sampled_reference.has_value()) { + reference_state = sampled_reference.value(); + error = geodesic_error(reference_state, end_effector_state); + } + + bool path_tolerance_exceeded = false; + bool goal_tolerance_exceeded = false; + bool trajectory_suceeded = false; + + if (sampled_command.has_value()) { + command_state = sampled_command.value(); + if (!std::isnan(error)) { + const double path_tolerance = *rt_path_tolerance_.readFromRT(); + if (path_tolerance > 0.0 && error > path_tolerance) { + path_tolerance_exceeded = true; + rt_holding_position_.writeFromNonRT(true); + command_state = end_effector_state; + RCLCPP_WARN(logger_, "Aborting trajectory. Error threshold exceeded during execution: %f", error); // NOLINT + RCLCPP_INFO(logger_, "Holding position until a new trajectory is received"); // NOLINT + } + } + } else { + switch (sampled_command.error()) { + case SampleError::SAMPLE_TIME_BEFORE_START: + RCLCPP_INFO(logger_, "Trajectory sample time is before trajectory start time"); // NOLINT + RCLCPP_INFO(logger_, "Holding position until the trajectory starts"); // NOLINT + break; + + case SampleError::SAMPLE_TIME_AFTER_END: { + const double goal_tolerance = *rt_goal_tolerance_.readFromRT(); + const double goal_error = geodesic_error(trajectory->end_point().value(), end_effector_state); + RCLCPP_INFO(logger_, "Trajectory sample time is after trajectory end time."); // NOLINT + if (goal_tolerance > 0.0) { + if (goal_error > goal_tolerance) { + goal_tolerance_exceeded = true; + RCLCPP_WARN(logger_, "Aborting trajectory. Terminal error exceeded threshold: %f", goal_error); // NOLINT + } else { + trajectory_suceeded = true; + RCLCPP_INFO(logger_, "Trajectory execution completed successfully"); // NOLINT + } + } + rt_holding_position_.writeFromNonRT(true); + RCLCPP_INFO(logger_, "Holding position until a new trajectory is received"); // NOLINT + } break; + + default: + // default to position hold + rt_holding_position_.writeFromNonRT(true); + break; + } + } + + if (active_goal) { + // write feedback to the action server + auto feedback = std::make_shared(); + feedback->header.stamp = time; + feedback->desired = reference_state; + feedback->actual = end_effector_state; + feedback->error = error; + active_goal->setFeedback(feedback); + + // check terminal conditions + if (goal_tolerance_exceeded) { + auto action_result = std::make_shared(); + action_result->error_code = FollowTrajectory::Result::PATH_TOLERANCE_VIOLATED; + action_result->error_string = "Trajectory execution aborted. Goal tolerance exceeded."; + active_goal->setAborted(action_result); + rt_holding_position_.writeFromNonRT(true); + } else if (trajectory_suceeded) { + auto action_result = std::make_shared(); + action_result->error_code = FollowTrajectory::Result::SUCCESSFUL; + action_result->error_string = "Trajectory execution completed successfully!"; + active_goal->setSucceeded(action_result); + rt_holding_position_.writeFromNonRT(true); + } else if (path_tolerance_exceeded) { + auto action_result = std::make_shared(); + action_result->error_code = FollowTrajectory::Result::PATH_TOLERANCE_VIOLATED; + action_result->error_string = "Trajectory execution aborted. Path tolerance exceeded."; + active_goal->setAborted(action_result); + rt_holding_position_.writeFromNonRT(true); + } + } + + write_command(command_interfaces_, command_state); + publish_controller_state(); + + return controller_interface::return_type::OK; +} + +} // namespace end_effector_trajectory_controller + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS( + end_effector_trajectory_controller::EndEffectorTrajectoryController, + controller_interface::ControllerInterface) diff --git a/end_effector_trajectory_controller/src/end_effector_trajectory_controller_parameters.yaml b/end_effector_trajectory_controller/src/end_effector_trajectory_controller_parameters.yaml new file mode 100644 index 0000000..b5d2308 --- /dev/null +++ b/end_effector_trajectory_controller/src/end_effector_trajectory_controller_parameters.yaml @@ -0,0 +1,93 @@ +end_effector_trajectory_controller: + joints: + type: string_array + default_value: ["x", "y", "z", "qx", "qy", "qz", "qw"] + read_only: true + description: > + The list of position joints to use for the end effector position + interfaces. This can be used to configure a prefix for the interfaces. + validation: + fixed_size<>: 7 # position + orientation (quaternion) + + path_tolerance: + type: double + default_value: 0.0 + read_only: true + description: > + The maximum error between the end effector pose and the reference + pose in the trajectory. + + The error threshold is measured in terms of the squared norm of the + geodesic distance between the end effector pose and the terminal pose in + the trajectory. + validation: + gt_eq<>: 0.0 + + goal_tolerance: + type: double + default_value: 0.0 + read_only: true + description: > + The maximum error between the end effector pose and the terminal + pose in the trajectory at the end of execution. + + The error threshold is measured in terms of the squared norm of the + geodesic distance between the end effector pose and the terminal pose in + the trajectory. + validation: + gt_eq<>: 0.0 + + use_external_measured_states: + type: bool + default_value: false + read_only: true + description: > + Use end effector states provided via a topic instead of state interfaces + or tf2. If both this and lookup_end_effector_transform are set to true, + the controller will use the end effector states provided via a topic. + + lookup_end_effector_transform: + type: bool + default_value: false + read_only: true + description: > + Use tf2 to look up the end effector transform instead of using state + interfaces or a topic. This is useful in scenarios where the end effector + state is determined only by using tf2 to perform forward kinematics. + + If both this and use_external_end_effector_states are set to true, the + controller will use the end effector states provided via a topic. + + reference_controller: + type: string + default_value: "" + read_only: true + description: > + The prefix of the reference controller to send commands to. This can be + used to configure command interfaces in chained mode. + + odom_frame_id: + type: string + default_value: odom_ned + read_only: true + description: > + The name of the inertial frame. This is used to lookup the end effector + pose when using lookup_end_effector_transform. + + end_effector_frame_id: + type: string + default_value: tcp + read_only: true + description: > + The name of the end effector frame. This is used to lookup the end + effector pose when using lookup_end_effector_transform. + + action_monitor_rate: + type: int + default_value: 20 + read_only: true + description: > + The rate (Hz) at which the action server will monitor the trajectory + execution. + validation: + gt<>: 0 diff --git a/end_effector_trajectory_controller/src/trajectory.cpp b/end_effector_trajectory_controller/src/trajectory.cpp new file mode 100644 index 0000000..297dbd3 --- /dev/null +++ b/end_effector_trajectory_controller/src/trajectory.cpp @@ -0,0 +1,165 @@ +// 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. + +#include "end_effector_trajectory_controller/trajectory.hpp" + +#include +#include + +#include "controller_common/common.hpp" +#include "tf2_eigen/tf2_eigen.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +namespace end_effector_trajectory_controller +{ + +namespace +{ + +/// Linear interpolation between two positions. +auto lerp(double start, double end, double t) -> double { return start + (t * (end - start)); } + +/// Spherical linear interpolation between two quaternions. +/// +/// See the following for more information: +/// https://www.mathworks.com/help/fusion/ref/quaternion.slerp.html +auto slerp(Eigen::Quaterniond start, Eigen::Quaterniond end, double t) -> Eigen::Quaterniond +{ + Eigen::Quaterniond result; + + start.normalize(); + end.normalize(); + + const double dot = start.dot(end); + if (common::math::isclose(dot, 1.0, 1e-5, 5e-5)) { + // if the quaternions are very close, just linearly interpolate between them to avoid numerical instability + result.coeffs() = start.coeffs() + t * (end.coeffs() - start.coeffs()); + } else { + // otherwise, use spherical linear interpolation + const double theta = std::acos(dot); + const double coeff0 = std::sin((1 - t) * theta) / std::sin(theta); + const double coeff1 = std::sin(t * theta) / std::sin(theta); + result.coeffs() = (coeff0 * start.coeffs()) + (coeff1 * end.coeffs()); + } + + result.normalize(); + return result; +} + +auto interpolate( + const geometry_msgs::msg::Pose & start, + const geometry_msgs::msg::Pose & end, + const rclcpp::Time & t0, + const rclcpp::Time & t1, + const rclcpp::Time & sample_time) -> geometry_msgs::msg::Pose +{ + const rclcpp::Duration time_from_start = sample_time - t0; + const rclcpp::Duration time_between_points = t1 - t0; + const double t = time_from_start.seconds() / time_between_points.seconds(); + + // linearly interpolate between the positions + geometry_msgs::msg::Pose out; + out.position.x = lerp(start.position.x, end.position.x, t); + out.position.y = lerp(start.position.y, end.position.y, t); + out.position.z = lerp(start.position.z, end.position.z, t); + + Eigen::Quaterniond q1, q2; // NOLINT + tf2::fromMsg(start.orientation, q1); + tf2::fromMsg(end.orientation, q2); + + // spherical linear interpolation between the orientations + const Eigen::Quaterniond q_out = slerp(q1, q2, t); + out.orientation = tf2::toMsg(q_out); + + return out; +} + +} // namespace + +Trajectory::Trajectory( + const auv_control_msgs::msg::EndEffectorTrajectory & trajectory, + const geometry_msgs::msg::Pose & state) +: points_(trajectory), + initial_time_(static_cast(trajectory.header.stamp)), + initial_state_(state) +{ +} + +auto Trajectory::empty() const -> bool { return points_.points.empty(); } + +auto Trajectory::start_time() const -> rclcpp::Time +{ + return empty() ? rclcpp::Time(0) : initial_time_ + points_.points.front().time_from_start; +} + +auto Trajectory::end_time() const -> rclcpp::Time +{ + return empty() ? rclcpp::Time(0) : initial_time_ + points_.points.back().time_from_start; +} + +auto Trajectory::start_point() const -> std::optional +{ + if (empty()) { + return std::nullopt; + } + return points_.points.front().point; +} + +auto Trajectory::end_point() const -> std::optional +{ + if (empty()) { + return std::nullopt; + } + return points_.points.back().point; +} + +auto Trajectory::sample(const rclcpp::Time & sample_time) const -> std::expected +{ + if (empty()) { + return std::unexpected(SampleError::EMPTY_TRAJECTORY); + } + + // the sample time is before the timestamp in the trajectory header + if (sample_time < initial_time_) { + return std::unexpected(SampleError::SAMPLE_TIME_BEFORE_START); + } + + // the sample time is before the first point in the trajectory, so we need to interpolate between the starting + // state and the first point in the trajectory + if (sample_time < start_time()) { + return interpolate(initial_state_, start_point().value(), initial_time_, start_time(), sample_time); + } + + for (const auto [p1, p2] : std::views::zip(points_.points, points_.points | std::views::drop(1))) { + const rclcpp::Time t0 = initial_time_ + p1.time_from_start; + const rclcpp::Time t1 = initial_time_ + p2.time_from_start; + + if (sample_time >= t0 && sample_time <= t1) { + return interpolate(p1.point, p2.point, t0, t1, sample_time); + } + } + + // the whole trajectory has been sampled + return std::unexpected(SampleError::SAMPLE_TIME_AFTER_END); +} + +auto Trajectory::reset_initial_state(const geometry_msgs::msg::Pose & state) -> void { initial_state_ = state; } + +} // namespace end_effector_trajectory_controller diff --git a/ik_solvers/CHANGELOG.md b/ik_solvers/CHANGELOG.md index d83dbd2..8c9f764 100644 --- a/ik_solvers/CHANGELOG.md +++ b/ik_solvers/CHANGELOG.md @@ -1,5 +1,9 @@ # Changelog for package ik_solvers +## 0.2.0 (2025-05-03) + +- Replace instances of `Eigen::Affine3d` with `Eigen::Isometry3d` + ## 0.1.0 (2025-04-27) - Implements a task priority IK solver with support for end effector pose diff --git a/ik_solvers/include/ik_solvers/solver.hpp b/ik_solvers/include/ik_solvers/solver.hpp index 59bddff..d0afe1b 100644 --- a/ik_solvers/include/ik_solvers/solver.hpp +++ b/ik_solvers/include/ik_solvers/solver.hpp @@ -56,7 +56,7 @@ class IKSolver const std::string & prefix) -> void; /// Solve the IK problem for a target pose given the integration period and current joint configuration. - [[nodiscard]] auto solve(const rclcpp::Duration & period, const Eigen::Affine3d & goal, const Eigen::VectorXd & q) + [[nodiscard]] auto solve(const rclcpp::Duration & period, const Eigen::Isometry3d & goal, const Eigen::VectorXd & q) -> std::expected; protected: @@ -64,7 +64,7 @@ class IKSolver /// /// This is wrapped by the public API. The public API handles updating pinocchio for Jacobian calculation and converts /// of the result into a `JointTrajectoryPoint`. This method only needs to compute the IK solution. - [[nodiscard]] virtual auto solve_ik(const Eigen::Affine3d & goal, const Eigen::VectorXd & q) + [[nodiscard]] virtual auto solve_ik(const Eigen::Isometry3d & goal, const Eigen::VectorXd & q) -> std::expected = 0; std::shared_ptr node_; diff --git a/ik_solvers/include/ik_solvers/task_priority_solver.hpp b/ik_solvers/include/ik_solvers/task_priority_solver.hpp index 60a785d..866dd76 100644 --- a/ik_solvers/include/ik_solvers/task_priority_solver.hpp +++ b/ik_solvers/include/ik_solvers/task_priority_solver.hpp @@ -136,8 +136,8 @@ class PoseConstraint : public Constraint PoseConstraint( const std::shared_ptr & model, const std::shared_ptr & data, - const Eigen::Affine3d & primal, - const Eigen::Affine3d & constraint, + const Eigen::Isometry3d & primal, + const Eigen::Isometry3d & constraint, const std::string & frame, double gain, int priority = 2); @@ -218,7 +218,7 @@ class TaskPriorityIKSolver : public IKSolver const std::string & prefix) -> void override; protected: - [[nodiscard]] auto solve_ik(const Eigen::Affine3d & goal, const Eigen::VectorXd & q) + [[nodiscard]] auto solve_ik(const Eigen::Isometry3d & goal, const Eigen::VectorXd & q) -> std::expected override; /// Update the dynamic ROS 2 parameters. diff --git a/ik_solvers/package.xml b/ik_solvers/package.xml index aeac258..cae9040 100644 --- a/ik_solvers/package.xml +++ b/ik_solvers/package.xml @@ -3,7 +3,7 @@ ik_solvers - 0.1.0 + 0.2.0 Inverse kinematics solvers used for whole-body control Evan Palmer diff --git a/ik_solvers/src/solver.cpp b/ik_solvers/src/solver.cpp index 9750f75..7aa8586 100644 --- a/ik_solvers/src/solver.cpp +++ b/ik_solvers/src/solver.cpp @@ -44,7 +44,7 @@ auto IKSolver::update_pinocchio(const Eigen::VectorXd & q) const -> void pinocchio::computeJointJacobians(*model_, *data_); } -auto IKSolver::solve(const rclcpp::Duration & period, const Eigen::Affine3d & goal, const Eigen::VectorXd & q) +auto IKSolver::solve(const rclcpp::Duration & period, const Eigen::Isometry3d & goal, const Eigen::VectorXd & q) -> std::expected { // update the pinocchio data and model to use the current joint configuration diff --git a/ik_solvers/src/task_priority_solver.cpp b/ik_solvers/src/task_priority_solver.cpp index 82c41d1..ab1b6f1 100644 --- a/ik_solvers/src/task_priority_solver.cpp +++ b/ik_solvers/src/task_priority_solver.cpp @@ -132,8 +132,8 @@ auto TaskHierarchy::hierarchies() const -> std::vector PoseConstraint::PoseConstraint( const std::shared_ptr & model, const std::shared_ptr & data, - const Eigen::Affine3d & primal, - const Eigen::Affine3d & constraint, + const Eigen::Isometry3d & primal, + const Eigen::Isometry3d & constraint, const std::string & frame, double gain, int priority) @@ -275,9 +275,9 @@ auto search_solutions( return *std::ranges::min_element(solutions, {}, [](const auto & a) { return a.norm(); }); } -auto pinocchio_to_eigen(const pinocchio::SE3 & pose) -> Eigen::Affine3d +auto pinocchio_to_eigen(const pinocchio::SE3 & pose) -> Eigen::Isometry3d { - Eigen::Affine3d result; + Eigen::Isometry3d result; result.translation() = pose.translation(); result.linear() = pose.rotation(); return result; @@ -314,15 +314,15 @@ auto TaskPriorityIKSolver::configure_parameters() -> void } // NOLINTNEXTLINE(readability-convert-member-functions-to-static) -auto TaskPriorityIKSolver::solve_ik(const Eigen::Affine3d & goal, const Eigen::VectorXd & q) +auto TaskPriorityIKSolver::solve_ik(const Eigen::Isometry3d & goal, const Eigen::VectorXd & q) -> std::expected { configure_parameters(); hierarchy_.clear(); - // get the end effector pose as an Eigen Affine3d - const Eigen::Affine3d ee_pose = pinocchio_to_eigen(data_->oMf[model_->getFrameId(ee_frame_)]); + // get the end effector pose as an Eigen Isometry3d + const Eigen::Isometry3d ee_pose = pinocchio_to_eigen(data_->oMf[model_->getFrameId(ee_frame_)]); // insert the pose constraint const double gain = params_.end_effector_pose_task.gain; diff --git a/media/teleop.gif b/media/teleop.gif new file mode 100644 index 0000000..63dd3ea Binary files /dev/null and b/media/teleop.gif differ diff --git a/media/uvms.gif b/media/uvms.gif new file mode 100644 index 0000000..98f76ef Binary files /dev/null and b/media/uvms.gif differ diff --git a/thruster_allocation_matrix_controller/CHANGELOG.md b/thruster_allocation_matrix_controller/CHANGELOG.md index 4df04a1..fb5c325 100644 --- a/thruster_allocation_matrix_controller/CHANGELOG.md +++ b/thruster_allocation_matrix_controller/CHANGELOG.md @@ -1,5 +1,7 @@ # Changelog for package thruster_allocation_matrix_controller +## 0.2.0 (2025-05-03) + ## 0.1.0 (2025-04-27) - Updates the minimum CMake version to CMake 23 and upgrades the API to use diff --git a/thruster_allocation_matrix_controller/package.xml b/thruster_allocation_matrix_controller/package.xml index d163cbe..66a1823 100644 --- a/thruster_allocation_matrix_controller/package.xml +++ b/thruster_allocation_matrix_controller/package.xml @@ -3,7 +3,7 @@ thruster_allocation_matrix_controller - 0.1.0 + 0.2.0 Thruster allocation matrix controller used to convert wrench commands into thrust commands Evan Palmer diff --git a/thruster_controllers/CHANGELOG.md b/thruster_controllers/CHANGELOG.md index ef0f38f..6e590ff 100644 --- a/thruster_controllers/CHANGELOG.md +++ b/thruster_controllers/CHANGELOG.md @@ -1,5 +1,7 @@ # Changelog for package thruster_controllers +## 0.2.0 (2025-05-03) + ## 0.1.0 (2025-04-27) - Implements the Gazebo passthrough thruster controller diff --git a/thruster_controllers/package.xml b/thruster_controllers/package.xml index fda06a2..0282d47 100644 --- a/thruster_controllers/package.xml +++ b/thruster_controllers/package.xml @@ -3,7 +3,7 @@ thruster_controllers - 0.1.0 + 0.2.0 A collection of thruster controllers for AUV control Evan Palmer diff --git a/topic_sensors/CHANGELOG.md b/topic_sensors/CHANGELOG.md index 778bf32..e2f4a4e 100644 --- a/topic_sensors/CHANGELOG.md +++ b/topic_sensors/CHANGELOG.md @@ -1,5 +1,7 @@ # Changelog for package topic_sensors +## 0.2.0 (2025-05-03) + ## 0.1.0 (2025-04-27) - Implements an nav_msgs/Odometry topic sensor diff --git a/topic_sensors/package.xml b/topic_sensors/package.xml index dbacd33..da96fb0 100644 --- a/topic_sensors/package.xml +++ b/topic_sensors/package.xml @@ -3,7 +3,7 @@ topic_sensors - 0.1.0 + 0.2.0 Sensor plugins used to write ROS 2 messages to state interfaces Evan Palmer diff --git a/velocity_controllers/CHANGELOG.md b/velocity_controllers/CHANGELOG.md index 56e6b26..9463314 100644 --- a/velocity_controllers/CHANGELOG.md +++ b/velocity_controllers/CHANGELOG.md @@ -1,5 +1,7 @@ # Changelog for package velocity_controllers +## 0.2.0 (2025-05-03) + ## 0.1.0 (2025-04-27) - Updates the API to use C++ 23 diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 8b0e217..c2f9b1f 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -3,7 +3,7 @@ velocity_controllers - 0.1.0 + 0.2.0 A collection of velocity controllers for underwater vehicles Evan Palmer diff --git a/whole_body_controllers/CHANGELOG.md b/whole_body_controllers/CHANGELOG.md index fb39525..85f2fb8 100644 --- a/whole_body_controllers/CHANGELOG.md +++ b/whole_body_controllers/CHANGELOG.md @@ -1,5 +1,12 @@ # Changelog for package whole_body_controllers +## 0.2.0 (2025-05-03) + +- Replaces instances of `Eigen::Affine3d` with `Eigen::Isometry3d` +- Fixes a bug in the ik_controller reference interfaces where the values sent + to the reference interfaces themselves (i.e., not as a message) were not + being transformed into the appropriate coordinate frame for Pinocchio. + ## 0.1.0 (2025-04-27) - Implements an IK controller for controlling a UVMS with a single manipulator diff --git a/whole_body_controllers/CMakeLists.txt b/whole_body_controllers/CMakeLists.txt index e51b67c..58f9e29 100644 --- a/whole_body_controllers/CMakeLists.txt +++ b/whole_body_controllers/CMakeLists.txt @@ -57,6 +57,7 @@ target_link_libraries( tf2_eigen::tf2_eigen message_transforms::message_transforms controller_common::controller_common + Eigen3::Eigen ${geometry_msgs_TARGETS} ${nav_msgs_TARGETS} ${std_msgs_TARGETS} @@ -92,6 +93,7 @@ ament_export_dependencies( "controller_common" "ik_solvers" "auv_control_msgs" + "Eigen3" ) ament_package() diff --git a/whole_body_controllers/include/whole_body_controllers/ik_controller.hpp b/whole_body_controllers/include/whole_body_controllers/ik_controller.hpp index 8b740a2..ff249fc 100644 --- a/whole_body_controllers/include/whole_body_controllers/ik_controller.hpp +++ b/whole_body_controllers/include/whole_body_controllers/ik_controller.hpp @@ -71,6 +71,8 @@ class IKController : public controller_interface::ChainableControllerInterface auto update_system_state_values() -> controller_interface::return_type; + auto update_chained_reference_values() -> controller_interface::return_type; + auto update_parameters() -> void; auto configure_parameters() -> controller_interface::CallbackReturn; diff --git a/whole_body_controllers/package.xml b/whole_body_controllers/package.xml index e355c8b..70e9197 100644 --- a/whole_body_controllers/package.xml +++ b/whole_body_controllers/package.xml @@ -1,8 +1,9 @@ + whole_body_controllers - 0.1.0 + 0.2.0 Whole-body controllers for underwater vehicle manipulator systems Evan Palmer diff --git a/whole_body_controllers/src/ik_controller.cpp b/whole_body_controllers/src/ik_controller.cpp index 99461d7..f1b0a05 100644 --- a/whole_body_controllers/src/ik_controller.cpp +++ b/whole_body_controllers/src/ik_controller.cpp @@ -37,7 +37,7 @@ namespace whole_body_controllers namespace { -auto to_eigen(const std::vector & vec) -> Eigen::Affine3d +auto to_eigen(const std::vector & vec) -> Eigen::Isometry3d { if (vec.size() != 7) { throw std::invalid_argument("Invalid size for pose vector"); @@ -319,7 +319,7 @@ auto IKController::update_system_state_values() -> controller_interface::return_ }); }; - // retrieve the vehicle position and velocity state interfaces + // retrieve the vehicle position and velocity state interfaces const auto position_interfaces_end = state_interfaces_.begin() + free_flyer_pos_dofs_.size(); const auto velocity_interfaces_start = position_interfaces_end + params_.controlled_joints.size(); const auto velocity_interfaces_end = velocity_interfaces_start + free_flyer_vel_dofs_.size(); @@ -386,12 +386,28 @@ auto IKController::update_system_state_values() -> controller_interface::return_ return controller_interface::return_type::OK; } +auto IKController::update_chained_reference_values() -> controller_interface::return_type +{ + // the reference interfaces in chained mode are parsed directly from the command interfaces + // + // because the command interfaces expect the commands to be in the maritime coordinate frame standard, we need + // this extra method to transform the values into a frame suitable for pinocchio + geometry_msgs::msg::Pose reference_transformed; + common::messages::to_msg(reference_interfaces_, &reference_transformed); + m2m::transform_message(reference_transformed); + std::ranges::copy(common::messages::to_vector(reference_transformed), reference_interfaces_.begin()); + return controller_interface::return_type::OK; +} + auto IKController::update_and_validate_interfaces() -> controller_interface::return_type { if (update_system_state_values() != controller_interface::return_type::OK) { RCLCPP_DEBUG(logger_, "Failed to update system state values"); // NOLINT return controller_interface::return_type::ERROR; } + if (is_in_chained_mode()) { + update_chained_reference_values(); + } if (common::math::has_nan(reference_interfaces_)) { RCLCPP_DEBUG(logger_, "Received reference with NaN value."); // NOLINT return controller_interface::return_type::ERROR; @@ -411,7 +427,7 @@ auto IKController::update_and_write_commands(const rclcpp::Time & /*time*/, cons configure_parameters(); const Eigen::VectorXd q = Eigen::VectorXd::Map(position_state_values_.data(), position_state_values_.size()); - const Eigen::Affine3d goal = to_eigen(reference_interfaces_); + const Eigen::Isometry3d goal = to_eigen(reference_interfaces_); // TODO(anyone): add solver support for velocity states // right now we only use the positions for the solver