From aaeda71ebd182e8ed99e9620691c5d9faa186d47 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Wed, 30 Jul 2025 21:04:27 -0700 Subject: [PATCH 01/19] start moving trajectory controller and setup pd tracking controller --- auv_control_msgs/CHANGELOG.md | 3 ++ auv_control_msgs/CMakeLists.txt | 8 +-- ...ction => FollowGeometricTrajectory.action} | 0 ...Trajectory.msg => GeometricTrajectory.msg} | 2 +- ...=> GeometricTrajectoryControllerState.msg} | 0 ...Point.msg => GeometricTrajectoryPoint.msg} | 0 .../CHANGELOG.md | 17 ------- .../end_effector_trajectory_controller.xml | 9 ---- geometric_controllers/CHANGELOG.md | 0 .../CMakeLists.txt | 32 ++++++------ .../LICENSE | 0 .../README.md | 0 .../tracking_controller.hpp | 0 .../geometric_controllers}/trajectory.hpp | 0 .../trajectory_controller.hpp | 40 +++++++-------- .../package.xml | 4 +- .../src/tracking_controller.cpp | 0 .../src/trajectory.cpp | 0 .../src/trajectory_controller.cpp | 49 +++++++++---------- .../src/trajectory_controller_parameters.yaml | 16 +++--- 20 files changed, 78 insertions(+), 102 deletions(-) rename auv_control_msgs/action/{FollowEndEffectorTrajectory.action => FollowGeometricTrajectory.action} (100%) rename auv_control_msgs/msg/{EndEffectorTrajectory.msg => GeometricTrajectory.msg} (57%) rename auv_control_msgs/msg/{EndEffectorTrajectoryControllerState.msg => GeometricTrajectoryControllerState.msg} (100%) rename auv_control_msgs/msg/{EndEffectorTrajectoryPoint.msg => GeometricTrajectoryPoint.msg} (100%) delete mode 100644 end_effector_trajectory_controller/CHANGELOG.md delete mode 100644 end_effector_trajectory_controller/end_effector_trajectory_controller.xml create mode 100644 geometric_controllers/CHANGELOG.md rename {end_effector_trajectory_controller => geometric_controllers}/CMakeLists.txt (64%) rename {end_effector_trajectory_controller => geometric_controllers}/LICENSE (100%) rename {end_effector_trajectory_controller => geometric_controllers}/README.md (100%) create mode 100644 geometric_controllers/include/geometric_controllers/tracking_controller.hpp rename {end_effector_trajectory_controller/include/end_effector_trajectory_controller => geometric_controllers/include/geometric_controllers}/trajectory.hpp (100%) rename end_effector_trajectory_controller/include/end_effector_trajectory_controller/end_effector_trajectory_controller.hpp => geometric_controllers/include/geometric_controllers/trajectory_controller.hpp (78%) rename {end_effector_trajectory_controller => geometric_controllers}/package.xml (89%) create mode 100644 geometric_controllers/src/tracking_controller.cpp rename {end_effector_trajectory_controller => geometric_controllers}/src/trajectory.cpp (100%) rename end_effector_trajectory_controller/src/end_effector_trajectory_controller.cpp => geometric_controllers/src/trajectory_controller.cpp (90%) rename end_effector_trajectory_controller/src/end_effector_trajectory_controller_parameters.yaml => geometric_controllers/src/trajectory_controller_parameters.yaml (87%) diff --git a/auv_control_msgs/CHANGELOG.md b/auv_control_msgs/CHANGELOG.md index 60ff528..0079dd4 100644 --- a/auv_control_msgs/CHANGELOG.md +++ b/auv_control_msgs/CHANGELOG.md @@ -2,6 +2,9 @@ ## 0.3.3 (2025-07-29) +- Re-names the EndEffector- messages to Geometric- to match changes to the + end effectory trajectory controller. + ## 0.3.2 (2025-07-22) ## 0.3.1 (2025-07-09) diff --git a/auv_control_msgs/CMakeLists.txt b/auv_control_msgs/CMakeLists.txt index 83ab507..7ca6a9c 100644 --- a/auv_control_msgs/CMakeLists.txt +++ b/auv_control_msgs/CMakeLists.txt @@ -11,10 +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" + "msg/GeometricTrajectoryPoint.msg" + "msg/GeometricTrajectory.msg" + "msg/GeometricTrajectoryControllerState.msg" + "action/FollowGeometricTrajectory.action" DEPENDENCIES builtin_interfaces std_msgs geometry_msgs trajectory_msgs ) diff --git a/auv_control_msgs/action/FollowEndEffectorTrajectory.action b/auv_control_msgs/action/FollowGeometricTrajectory.action similarity index 100% rename from auv_control_msgs/action/FollowEndEffectorTrajectory.action rename to auv_control_msgs/action/FollowGeometricTrajectory.action diff --git a/auv_control_msgs/msg/EndEffectorTrajectory.msg b/auv_control_msgs/msg/GeometricTrajectory.msg similarity index 57% rename from auv_control_msgs/msg/EndEffectorTrajectory.msg rename to auv_control_msgs/msg/GeometricTrajectory.msg index 114b86a..792b740 100644 --- a/auv_control_msgs/msg/EndEffectorTrajectory.msg +++ b/auv_control_msgs/msg/GeometricTrajectory.msg @@ -1,4 +1,4 @@ std_msgs/Header header # The sequence of end effector points to track. -auv_control_msgs/EndEffectorTrajectoryPoint[] points +auv_control_msgs/GeometricTrajectoryPoint[] points diff --git a/auv_control_msgs/msg/EndEffectorTrajectoryControllerState.msg b/auv_control_msgs/msg/GeometricTrajectoryControllerState.msg similarity index 100% rename from auv_control_msgs/msg/EndEffectorTrajectoryControllerState.msg rename to auv_control_msgs/msg/GeometricTrajectoryControllerState.msg diff --git a/auv_control_msgs/msg/EndEffectorTrajectoryPoint.msg b/auv_control_msgs/msg/GeometricTrajectoryPoint.msg similarity index 100% rename from auv_control_msgs/msg/EndEffectorTrajectoryPoint.msg rename to auv_control_msgs/msg/GeometricTrajectoryPoint.msg diff --git a/end_effector_trajectory_controller/CHANGELOG.md b/end_effector_trajectory_controller/CHANGELOG.md deleted file mode 100644 index e4cb19b..0000000 --- a/end_effector_trajectory_controller/CHANGELOG.md +++ /dev/null @@ -1,17 +0,0 @@ -# Changelog for package end_effector_trajectory_controller - -## 0.3.3 (2025-07-29) - -## 0.3.2 (2025-07-22) - -- Replaces the deprecated `unlockAndPublish` API with `try_publish` - -## 0.3.1 (2025-07-09) - -## 0.3.0 (2025-06-07) - -## 0.2.1 (2025-06-03) - -## 0.2.0 (2025-05-03) - -- Implements the end_effector_trajectory_controller diff --git a/end_effector_trajectory_controller/end_effector_trajectory_controller.xml b/end_effector_trajectory_controller/end_effector_trajectory_controller.xml deleted file mode 100644 index 9bf9cc0..0000000 --- a/end_effector_trajectory_controller/end_effector_trajectory_controller.xml +++ /dev/null @@ -1,9 +0,0 @@ - - - - End effector trajectory controller for UVMS whole-body control - - - diff --git a/geometric_controllers/CHANGELOG.md b/geometric_controllers/CHANGELOG.md new file mode 100644 index 0000000..e69de29 diff --git a/end_effector_trajectory_controller/CMakeLists.txt b/geometric_controllers/CMakeLists.txt similarity index 64% rename from end_effector_trajectory_controller/CMakeLists.txt rename to geometric_controllers/CMakeLists.txt index cefc572..c63b408 100644 --- a/end_effector_trajectory_controller/CMakeLists.txt +++ b/geometric_controllers/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.23) -project(end_effector_trajectory_controller) +project(geometric_controllers) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic) @@ -26,26 +26,26 @@ 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 +generate_parameter_library(geometric_trajectory_controller_parameters + src/geometric_trajectory_controller_parameters.yaml ) -add_library(end_effector_trajectory_controller SHARED) +add_library(geometric_trajectory_controller SHARED) target_sources( - end_effector_trajectory_controller - PRIVATE src/end_effector_trajectory_controller.cpp src/trajectory.cpp + geometric_trajectory_controller + PRIVATE src/geometric_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 + ${CMAKE_CURRENT_SOURCE_DIR}/include/geometric_trajectory_controller/geometric_trajectory_controller.hpp + ${CMAKE_CURRENT_SOURCE_DIR}/include/geometric_trajectory_controller/trajectory.hpp ) -target_compile_features(end_effector_trajectory_controller PUBLIC cxx_std_23) +target_compile_features(geometric_trajectory_controller PUBLIC cxx_std_23) target_link_libraries( - end_effector_trajectory_controller + geometric_trajectory_controller PUBLIC - end_effector_trajectory_controller_parameters + geometric_trajectory_controller_parameters realtime_tools::realtime_tools controller_common::controller_common hardware_interface::hardware_interface @@ -62,20 +62,20 @@ target_link_libraries( ${lifecycle_msgs_TARGETS} ) -pluginlib_export_plugin_description_file(controller_interface end_effector_trajectory_controller.xml) +pluginlib_export_plugin_description_file(controller_interface geometric_trajectory_controller.xml) install( TARGETS - end_effector_trajectory_controller - end_effector_trajectory_controller_parameters - EXPORT export_end_effector_trajectory_controller + geometric_trajectory_controller + geometric_trajectory_controller_parameters + EXPORT export_geometric_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_targets(export_geometric_trajectory_controller HAS_LIBRARY_TARGET) ament_export_dependencies( "geometry_msgs" "realtime_tools" diff --git a/end_effector_trajectory_controller/LICENSE b/geometric_controllers/LICENSE similarity index 100% rename from end_effector_trajectory_controller/LICENSE rename to geometric_controllers/LICENSE diff --git a/end_effector_trajectory_controller/README.md b/geometric_controllers/README.md similarity index 100% rename from end_effector_trajectory_controller/README.md rename to geometric_controllers/README.md diff --git a/geometric_controllers/include/geometric_controllers/tracking_controller.hpp b/geometric_controllers/include/geometric_controllers/tracking_controller.hpp new file mode 100644 index 0000000..e69de29 diff --git a/end_effector_trajectory_controller/include/end_effector_trajectory_controller/trajectory.hpp b/geometric_controllers/include/geometric_controllers/trajectory.hpp similarity index 100% rename from end_effector_trajectory_controller/include/end_effector_trajectory_controller/trajectory.hpp rename to geometric_controllers/include/geometric_controllers/trajectory.hpp diff --git a/end_effector_trajectory_controller/include/end_effector_trajectory_controller/end_effector_trajectory_controller.hpp b/geometric_controllers/include/geometric_controllers/trajectory_controller.hpp similarity index 78% rename from end_effector_trajectory_controller/include/end_effector_trajectory_controller/end_effector_trajectory_controller.hpp rename to geometric_controllers/include/geometric_controllers/trajectory_controller.hpp index 1227848..9ebbd6e 100644 --- a/end_effector_trajectory_controller/include/end_effector_trajectory_controller/end_effector_trajectory_controller.hpp +++ b/geometric_controllers/include/geometric_controllers/trajectory_controller.hpp @@ -22,11 +22,11 @@ #include -#include "auv_control_msgs/action/follow_end_effector_trajectory.hpp" -#include "auv_control_msgs/msg/end_effector_trajectory_controller_state.hpp" +#include "auv_control_msgs/action/follow_geometric_trajectory.hpp" +#include "auv_control_msgs/msg/geometric_trajectory_controller_state.hpp" #include "controller_common/common.hpp" #include "controller_interface/controller_interface.hpp" -#include "end_effector_trajectory_controller/trajectory.hpp" +#include "geometric_trajectory_controller/trajectory.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/server.hpp" #include "realtime_tools/realtime_buffer.hpp" @@ -37,15 +37,15 @@ #include "tf2_ros/transform_listener.h" // auto-generated by generate_parameter_library -#include +#include -namespace end_effector_trajectory_controller +namespace geometric_controllers { -class EndEffectorTrajectoryController : public controller_interface::ControllerInterface +class GeometricTrajectoryController : public controller_interface::ControllerInterface { public: - EndEffectorTrajectoryController() = default; + GeometricTrajectoryController() = default; auto on_init() -> controller_interface::CallbackReturn override; @@ -66,34 +66,34 @@ class EndEffectorTrajectoryController : public controller_interface::ControllerI auto configure_parameters() -> controller_interface::CallbackReturn; - auto update_end_effector_state() -> controller_interface::return_type; + auto update_system_state() -> controller_interface::return_type; - [[nodiscard]] auto validate_trajectory(const auv_control_msgs::msg::EndEffectorTrajectory & trajectory) const -> bool; + [[nodiscard]] auto validate_trajectory(const auv_control_msgs::msg::GeometricTrajectory & trajectory) const -> bool; // controller state - using ControllerState = auv_control_msgs::msg::EndEffectorTrajectoryControllerState; + using ControllerState = auv_control_msgs::msg::GeometricTrajectoryControllerState; std::shared_ptr> controller_state_pub_; std::unique_ptr> rt_controller_state_pub_; ControllerState controller_state_; - // the end effector states can be captured in one of three ways: + // the 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_; + realtime_tools::RealtimeBuffer state_; + std::shared_ptr> 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 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_; + std::shared_ptr> trajectory_sub_; - using FollowTrajectory = auv_control_msgs::action::FollowEndEffectorTrajectory; + using FollowTrajectory = auv_control_msgs::action::FollowGeometricTrajectory; using RealtimeGoalHandle = realtime_tools::RealtimeServerGoalHandle; using RealtimeGoalHandlePtr = std::shared_ptr; using RealtimeGoalHandleBuffer = realtime_tools::RealtimeBuffer; @@ -110,8 +110,8 @@ class EndEffectorTrajectoryController : public controller_interface::ControllerI // 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_; + std::shared_ptr param_listener_; + geometric_trajectory_controller::Params params_; // error tolerances // the default tolerances are extracted from the parameters and applied when the action interface is not used @@ -122,7 +122,7 @@ class EndEffectorTrajectoryController : public controller_interface::ControllerI std::vector dofs_; std::size_t n_dofs_; - rclcpp::Logger logger_{rclcpp::get_logger("end_effector_trajectory_controller")}; + rclcpp::Logger logger_{rclcpp::get_logger("geometric_trajectory_controller")}; private: template @@ -137,4 +137,4 @@ class EndEffectorTrajectoryController : public controller_interface::ControllerI } }; -} // namespace end_effector_trajectory_controller +} // namespace geometric_controllers diff --git a/end_effector_trajectory_controller/package.xml b/geometric_controllers/package.xml similarity index 89% rename from end_effector_trajectory_controller/package.xml rename to geometric_controllers/package.xml index f18d2fc..43e0bad 100644 --- a/end_effector_trajectory_controller/package.xml +++ b/geometric_controllers/package.xml @@ -2,9 +2,9 @@ - end_effector_trajectory_controller + geometric_controllers 0.3.3 - End effector trajectory tracking controller for UVMS control + Geometrically consistent controllers for AUVs and UVMS Evan Palmer MIT diff --git a/geometric_controllers/src/tracking_controller.cpp b/geometric_controllers/src/tracking_controller.cpp new file mode 100644 index 0000000..e69de29 diff --git a/end_effector_trajectory_controller/src/trajectory.cpp b/geometric_controllers/src/trajectory.cpp similarity index 100% rename from end_effector_trajectory_controller/src/trajectory.cpp rename to geometric_controllers/src/trajectory.cpp diff --git a/end_effector_trajectory_controller/src/end_effector_trajectory_controller.cpp b/geometric_controllers/src/trajectory_controller.cpp similarity index 90% rename from end_effector_trajectory_controller/src/end_effector_trajectory_controller.cpp rename to geometric_controllers/src/trajectory_controller.cpp index 8008d7d..0cd8162 100644 --- a/end_effector_trajectory_controller/src/end_effector_trajectory_controller.cpp +++ b/geometric_controllers/src/trajectory_controller.cpp @@ -18,18 +18,17 @@ // 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 "geometric_trajectory_controller/geometric_trajectory_controller.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 geometric_controllers { namespace @@ -45,16 +44,16 @@ auto geodesic_error(const geometry_msgs::msg::Pose & goal, const geometry_msgs:: } // namespace -auto EndEffectorTrajectoryController::on_init() -> controller_interface::CallbackReturn +auto GeometricTrajectoryController::on_init() -> controller_interface::CallbackReturn { - param_listener_ = std::make_shared(get_node()); + 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 +auto GeometricTrajectoryController::update_parameters() -> void { if (!param_listener_->is_old(params_)) { return; @@ -63,7 +62,7 @@ auto EndEffectorTrajectoryController::update_parameters() -> void params_ = param_listener_->get_params(); } -auto EndEffectorTrajectoryController::configure_parameters() -> controller_interface::CallbackReturn +auto GeometricTrajectoryController::configure_parameters() -> controller_interface::CallbackReturn { update_parameters(); @@ -82,7 +81,7 @@ auto EndEffectorTrajectoryController::configure_parameters() -> controller_inter return controller_interface::CallbackReturn::SUCCESS; } -auto EndEffectorTrajectoryController::validate_trajectory( +auto GeometricTrajectoryController::validate_trajectory( const auv_control_msgs::msg::EndEffectorTrajectory & trajectory) const -> bool { if (trajectory.points.empty()) { @@ -119,21 +118,21 @@ auto EndEffectorTrajectoryController::validate_trajectory( return true; } -auto EndEffectorTrajectoryController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) +auto GeometricTrajectoryController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) -> controller_interface::CallbackReturn { configure_parameters(); - end_effector_state_.writeFromNonRT(geometry_msgs::msg::Pose()); + 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( + state_sub_ = get_node()->create_subscription( "~/end_effector_state", rclcpp::SystemDefaultsQoS(), [this](const std::shared_ptr msg) { // NOLINT - end_effector_state_.writeFromNonRT(*msg); + state_.writeFromNonRT(*msg); }); } else { if (params_.lookup_end_effector_transform) { @@ -156,7 +155,7 @@ auto EndEffectorTrajectoryController::on_configure(const rclcpp_lifecycle::State 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_trajectory_.writeFromNonRT(Trajectory(updated_msg, *state_.readFromNonRT())); rt_goal_tolerance_.writeFromNonRT(default_goal_tolerance_); rt_path_tolerance_.writeFromNonRT(default_path_tolerance_); rt_first_sample_.writeFromNonRT(true); @@ -180,7 +179,7 @@ auto EndEffectorTrajectoryController::on_configure(const rclcpp_lifecycle::State 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_trajectory_.writeFromNonRT(Trajectory(updated_msg, *state_.readFromNonRT())); rt_first_sample_.writeFromNonRT(true); rt_holding_position_.writeFromNonRT(false); RCLCPP_INFO(logger_, "Accepted new trajectory goal"); // NOLINT @@ -235,16 +234,16 @@ auto EndEffectorTrajectoryController::on_configure(const rclcpp_lifecycle::State return controller_interface::CallbackReturn::SUCCESS; } -auto EndEffectorTrajectoryController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) +auto GeometricTrajectoryController::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()); + common::messages::reset_message(state_.readFromNonRT()); return controller_interface::CallbackReturn::SUCCESS; } -auto EndEffectorTrajectoryController::command_interface_configuration() const +auto GeometricTrajectoryController::command_interface_configuration() const -> controller_interface::InterfaceConfiguration { controller_interface::InterfaceConfiguration config; @@ -260,7 +259,7 @@ auto EndEffectorTrajectoryController::command_interface_configuration() const return config; } -auto EndEffectorTrajectoryController::state_interface_configuration() const +auto GeometricTrajectoryController::state_interface_configuration() const -> controller_interface::InterfaceConfiguration { controller_interface::InterfaceConfiguration config; @@ -278,9 +277,9 @@ auto EndEffectorTrajectoryController::state_interface_configuration() const return config; } -auto EndEffectorTrajectoryController::update_end_effector_state() -> controller_interface::return_type +auto GeometricTrajectoryController::update_system_state() -> controller_interface::return_type { - auto * end_effector_state = end_effector_state_.readFromRT(); + auto * end_effector_state = 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; @@ -316,10 +315,10 @@ auto EndEffectorTrajectoryController::update_end_effector_state() -> controller_ return controller_interface::return_type::OK; } -auto EndEffectorTrajectoryController::update(const rclcpp::Time & time, const rclcpp::Duration & period) +auto GeometricTrajectoryController::update(const rclcpp::Time & time, const rclcpp::Duration & period) -> controller_interface::return_type { - if (update_end_effector_state() != controller_interface::return_type::OK) { + if (update_system_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; } @@ -331,7 +330,7 @@ auto EndEffectorTrajectoryController::update(const rclcpp::Time & time, const rc return controller_interface::return_type::OK; } - const geometry_msgs::msg::Pose end_effector_state = *end_effector_state_.readFromRT(); + const geometry_msgs::msg::Pose end_effector_state = *state_.readFromRT(); geometry_msgs::msg::Pose reference_state; common::messages::reset_message(&reference_state); auto command_state = end_effector_state; @@ -458,9 +457,9 @@ auto EndEffectorTrajectoryController::update(const rclcpp::Time & time, const rc return controller_interface::return_type::OK; } -} // namespace end_effector_trajectory_controller +} // namespace geometric_controllers #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS( - end_effector_trajectory_controller::EndEffectorTrajectoryController, + geometric_trajectory_controller::GeometricTrajectoryController, controller_interface::ControllerInterface) diff --git a/end_effector_trajectory_controller/src/end_effector_trajectory_controller_parameters.yaml b/geometric_controllers/src/trajectory_controller_parameters.yaml similarity index 87% rename from end_effector_trajectory_controller/src/end_effector_trajectory_controller_parameters.yaml rename to geometric_controllers/src/trajectory_controller_parameters.yaml index b5d2308..d65d17e 100644 --- a/end_effector_trajectory_controller/src/end_effector_trajectory_controller_parameters.yaml +++ b/geometric_controllers/src/trajectory_controller_parameters.yaml @@ -1,4 +1,4 @@ -end_effector_trajectory_controller: +geometric_trajectory_controller: joints: type: string_array default_value: ["x", "y", "z", "qx", "qy", "qz", "qw"] @@ -43,10 +43,10 @@ end_effector_trajectory_controller: 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, + or tf2. If both this and lookup_transform are set to true, the controller will use the end effector states provided via a topic. - lookup_end_effector_transform: + lookup_transform: type: bool default_value: false read_only: true @@ -55,7 +55,7 @@ end_effector_trajectory_controller: 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 + If both this and use_external_measured_states are set to true, the controller will use the end effector states provided via a topic. reference_controller: @@ -66,21 +66,21 @@ end_effector_trajectory_controller: The prefix of the reference controller to send commands to. This can be used to configure command interfaces in chained mode. - odom_frame_id: + parent_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. + pose when using lookup_transform. - end_effector_frame_id: + child_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. + effector pose when using lookup_transform. action_monitor_rate: type: int From bf9c6f12a69ba702d40ef36494bae9bdfda57864 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Wed, 30 Jul 2025 21:42:51 -0700 Subject: [PATCH 02/19] continue migration --- auv_control_msgs/CHANGELOG.md | 8 +-- .../action/FollowGeometricTrajectory.action | 2 +- geometric_controllers/CHANGELOG.md | 17 +++++ geometric_controllers/CMakeLists.txt | 22 +++--- geometric_controllers/README.md | 8 +-- .../geometric_controllers.xml | 9 +++ .../geometric_controllers/trajectory.hpp | 10 +-- .../trajectory_controller.hpp | 4 +- geometric_controllers/src/trajectory.cpp | 8 +-- .../src/trajectory_controller.cpp | 68 +++++++++---------- .../src/trajectory_controller_parameters.yaml | 26 +++---- 11 files changed, 104 insertions(+), 78 deletions(-) create mode 100644 geometric_controllers/geometric_controllers.xml diff --git a/auv_control_msgs/CHANGELOG.md b/auv_control_msgs/CHANGELOG.md index 0079dd4..90ee48d 100644 --- a/auv_control_msgs/CHANGELOG.md +++ b/auv_control_msgs/CHANGELOG.md @@ -15,10 +15,10 @@ ## 0.2.0 (2025-05-03) -- Implements the EndEffectorTrajectory message -- Implements the EndEffectorTrajectoryPoint message -- Implements the EndEffectorTrajectoryControllerState message -- Adds the FollowEndEffectorTrajectory action +- Implements the GeometricTrajectory message +- Implements the GeometricTrajectoryPoint message +- Implements the GeometricTrajectoryControllerState message +- Adds the FollowGeometricTrajectory action ## 0.1.0 (2025-04-27) diff --git a/auv_control_msgs/action/FollowGeometricTrajectory.action b/auv_control_msgs/action/FollowGeometricTrajectory.action index 1a12cd6..1d6114a 100644 --- a/auv_control_msgs/action/FollowGeometricTrajectory.action +++ b/auv_control_msgs/action/FollowGeometricTrajectory.action @@ -3,7 +3,7 @@ # 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 +auv_control_msgs/GeometricTrajectory 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. diff --git a/geometric_controllers/CHANGELOG.md b/geometric_controllers/CHANGELOG.md index e69de29..6a05108 100644 --- a/geometric_controllers/CHANGELOG.md +++ b/geometric_controllers/CHANGELOG.md @@ -0,0 +1,17 @@ +# Changelog for package geometric_controllers + +## 0.3.3 (2025-07-29) + +## 0.3.2 (2025-07-22) + +- Replaces the deprecated `unlockAndPublish` API with `try_publish` + +## 0.3.1 (2025-07-09) + +## 0.3.0 (2025-06-07) + +## 0.2.1 (2025-06-03) + +## 0.2.0 (2025-05-03) + +- Implements the geometric_trajectory_controller diff --git a/geometric_controllers/CMakeLists.txt b/geometric_controllers/CMakeLists.txt index c63b408..94d95a6 100644 --- a/geometric_controllers/CMakeLists.txt +++ b/geometric_controllers/CMakeLists.txt @@ -30,20 +30,20 @@ generate_parameter_library(geometric_trajectory_controller_parameters src/geometric_trajectory_controller_parameters.yaml ) -add_library(geometric_trajectory_controller SHARED) +add_library(geometric_controllers SHARED) target_sources( - geometric_trajectory_controller - PRIVATE src/geometric_trajectory_controller.cpp src/trajectory.cpp + geometric_controllers + PRIVATE src/trajectory_controller.cpp src/trajectory.cpp PUBLIC FILE_SET HEADERS BASE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include FILES - ${CMAKE_CURRENT_SOURCE_DIR}/include/geometric_trajectory_controller/geometric_trajectory_controller.hpp - ${CMAKE_CURRENT_SOURCE_DIR}/include/geometric_trajectory_controller/trajectory.hpp + ${CMAKE_CURRENT_SOURCE_DIR}/include/geometric_controllers/trajectory_controller.hpp + ${CMAKE_CURRENT_SOURCE_DIR}/include/geometric_controllers/trajectory.hpp ) -target_compile_features(geometric_trajectory_controller PUBLIC cxx_std_23) +target_compile_features(geometric_controllers PUBLIC cxx_std_23) target_link_libraries( - geometric_trajectory_controller + geometric_controllers PUBLIC geometric_trajectory_controller_parameters realtime_tools::realtime_tools @@ -62,20 +62,20 @@ target_link_libraries( ${lifecycle_msgs_TARGETS} ) -pluginlib_export_plugin_description_file(controller_interface geometric_trajectory_controller.xml) +pluginlib_export_plugin_description_file(controller_interface geometric_controllers.xml) install( TARGETS - geometric_trajectory_controller + geometric_controllers geometric_trajectory_controller_parameters - EXPORT export_geometric_trajectory_controller + EXPORT export_geometric_controllers LIBRARY DESTINATION lib/${PROJECT_NAME} ARCHIVE DESTINATION lib/${PROJECT_NAME} RUNTIME DESTINATION bin/${PROJECT_NAME} FILE_SET HEADERS ) -ament_export_targets(export_geometric_trajectory_controller HAS_LIBRARY_TARGET) +ament_export_targets(export_geometric_controllers HAS_LIBRARY_TARGET) ament_export_dependencies( "geometry_msgs" "realtime_tools" diff --git a/geometric_controllers/README.md b/geometric_controllers/README.md index 426bd81..e42ca41 100644 --- a/geometric_controllers/README.md +++ b/geometric_controllers/README.md @@ -7,7 +7,7 @@ interpolation. ## Plugin Library -end_effector_trajectory_controller/EndEffectorTrajectoryController +geometric_trajectory_controller/GeometricTrajectoryController ## References @@ -19,12 +19,12 @@ The output of this controller is a sampled end effector pose. ## Subscribers -- end_effector_trajectory_controller/trajectory [auv_control_msgs::msg::EndEffectorTrajectory] +- geometric_trajectory_controller/trajectory [auv_control_msgs::msg::GeometricTrajectory] ## Action Servers -- end_effector_trajectory_controller/follow_trajectory [auv_control_msgs::action::FollowEndEffectorTrajectory] +- geometric_trajectory_controller/follow_trajectory [auv_control_msgs::action::FollowGeometricTrajectory] ## Publishers -- end_effector_trajectory_controller/status [auv_control_msgs::msg::EndEffectorTrajectoryControllerState] +- geometric_trajectory_controller/status [auv_control_msgs::msg::GeometricTrajectoryControllerState] diff --git a/geometric_controllers/geometric_controllers.xml b/geometric_controllers/geometric_controllers.xml new file mode 100644 index 0000000..156eaf2 --- /dev/null +++ b/geometric_controllers/geometric_controllers.xml @@ -0,0 +1,9 @@ + + + + Trajectory controller for AUVs and UVMS that uses geometric methods to compute the desired trajectory. + + + diff --git a/geometric_controllers/include/geometric_controllers/trajectory.hpp b/geometric_controllers/include/geometric_controllers/trajectory.hpp index 485b640..730f814 100644 --- a/geometric_controllers/include/geometric_controllers/trajectory.hpp +++ b/geometric_controllers/include/geometric_controllers/trajectory.hpp @@ -25,11 +25,11 @@ #include #include -#include "auv_control_msgs/msg/end_effector_trajectory.hpp" +#include "auv_control_msgs/msg/geometric_trajectory.hpp" #include "geometry_msgs/msg/pose.hpp" #include "rclcpp/rclcpp.hpp" -namespace end_effector_trajectory_controller +namespace geometric_trajectory_controller { enum class SampleError : std::uint8_t @@ -45,7 +45,7 @@ class Trajectory 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); + Trajectory(const auv_control_msgs::msg::GeometricTrajectory & trajectory, const geometry_msgs::msg::Pose & state); /// Whether or not the trajectory is empty. [[nodiscard]] auto empty() const -> bool; @@ -70,9 +70,9 @@ class Trajectory auto reset_initial_state(const geometry_msgs::msg::Pose & state) -> void; private: - auv_control_msgs::msg::EndEffectorTrajectory points_; + auv_control_msgs::msg::GeometricTrajectory points_; rclcpp::Time initial_time_; geometry_msgs::msg::Pose initial_state_; }; -} // namespace end_effector_trajectory_controller +} // namespace geometric_trajectory_controller diff --git a/geometric_controllers/include/geometric_controllers/trajectory_controller.hpp b/geometric_controllers/include/geometric_controllers/trajectory_controller.hpp index 9ebbd6e..be9b55d 100644 --- a/geometric_controllers/include/geometric_controllers/trajectory_controller.hpp +++ b/geometric_controllers/include/geometric_controllers/trajectory_controller.hpp @@ -76,7 +76,7 @@ class GeometricTrajectoryController : public controller_interface::ControllerInt std::unique_ptr> rt_controller_state_pub_; ControllerState controller_state_; - // the states can be captured in one of three ways: + // the state 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 @@ -105,7 +105,7 @@ class GeometricTrajectoryController : public controller_interface::ControllerInt 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 + realtime_tools::RealtimeBuffer rt_holding_position_; // used to maintain the current pose // the update period is used to sample the "next" trajectory point rclcpp::Duration update_period_{0, 0}; diff --git a/geometric_controllers/src/trajectory.cpp b/geometric_controllers/src/trajectory.cpp index 297dbd3..05c5159 100644 --- a/geometric_controllers/src/trajectory.cpp +++ b/geometric_controllers/src/trajectory.cpp @@ -18,7 +18,7 @@ // 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 "geometric_trajectory_controller/trajectory.hpp" #include #include @@ -27,7 +27,7 @@ #include "tf2_eigen/tf2_eigen.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -namespace end_effector_trajectory_controller +namespace geometric_trajectory_controller { namespace @@ -94,7 +94,7 @@ auto interpolate( } // namespace Trajectory::Trajectory( - const auv_control_msgs::msg::EndEffectorTrajectory & trajectory, + const auv_control_msgs::msg::GeometricTrajectory & trajectory, const geometry_msgs::msg::Pose & state) : points_(trajectory), initial_time_(static_cast(trajectory.header.stamp)), @@ -162,4 +162,4 @@ auto Trajectory::sample(const rclcpp::Time & sample_time) const -> std::expected auto Trajectory::reset_initial_state(const geometry_msgs::msg::Pose & state) -> void { initial_state_ = state; } -} // namespace end_effector_trajectory_controller +} // namespace geometric_trajectory_controller diff --git a/geometric_controllers/src/trajectory_controller.cpp b/geometric_controllers/src/trajectory_controller.cpp index 0cd8162..1920211 100644 --- a/geometric_controllers/src/trajectory_controller.cpp +++ b/geometric_controllers/src/trajectory_controller.cpp @@ -82,7 +82,7 @@ auto GeometricTrajectoryController::configure_parameters() -> controller_interfa } auto GeometricTrajectoryController::validate_trajectory( - const auv_control_msgs::msg::EndEffectorTrajectory & trajectory) const -> bool + const auv_control_msgs::msg::GeometricTrajectory & trajectory) const -> bool { if (trajectory.points.empty()) { RCLCPP_ERROR(logger_, "Received empty trajectory"); // NOLINT @@ -129,22 +129,22 @@ auto GeometricTrajectoryController::on_configure(const rclcpp_lifecycle::State & if (params_.use_external_measured_states) { state_sub_ = get_node()->create_subscription( - "~/end_effector_state", + "~/state", rclcpp::SystemDefaultsQoS(), [this](const std::shared_ptr msg) { // NOLINT state_.writeFromNonRT(*msg); }); } else { - if (params_.lookup_end_effector_transform) { + if (params_.lookup_transform) { tf_buffer_ = std::make_unique(get_node()->get_clock()); tf_listener_ = std::make_unique(*tf_buffer_); } } - trajectory_sub_ = get_node()->create_subscription( + trajectory_sub_ = get_node()->create_subscription( "~/trajectory", rclcpp::SystemDefaultsQoS(), - [this](const std::shared_ptr msg) { // NOLINT + [this](const std::shared_ptr msg) { // NOLINT auto updated_msg = *msg; if (!validate_trajectory(updated_msg)) { RCLCPP_ERROR(logger_, "Ignoring invalid trajectory message"); // NOLINT @@ -224,7 +224,7 @@ auto GeometricTrajectoryController::on_configure(const rclcpp_lifecycle::State & goal_handle_timer_ = get_node()->create_wall_timer(action_monitor_period_, [rt_gh]() { rt_gh->runNonRealtime(); }); }; - action_server_ = rclcpp_action::create_server( + 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()); @@ -263,7 +263,7 @@ auto GeometricTrajectoryController::state_interface_configuration() const -> controller_interface::InterfaceConfiguration { controller_interface::InterfaceConfiguration config; - if (params_.use_external_measured_states || params_.lookup_end_effector_transform) { + if (params_.use_external_measured_states || params_.lookup_transform) { config.type = controller_interface::interface_configuration_type::NONE; return config; } @@ -279,16 +279,16 @@ auto GeometricTrajectoryController::state_interface_configuration() const auto GeometricTrajectoryController::update_system_state() -> controller_interface::return_type { - auto * end_effector_state = 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; + auto * system_state = state_.readFromRT(); + if (params_.lookup_transform) { + const auto to_frame = params_.child_frame_id; + const auto from_frame = params_.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; + system_state->position.x = transform.transform.translation.x; + system_state->position.y = transform.transform.translation.y; + system_state->position.z = transform.transform.translation.z; + system_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()); @@ -299,17 +299,17 @@ auto GeometricTrajectoryController::update_system_state() -> controller_interfac 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]); + system_state->position.x = get_value(state_interfaces_[0]); + system_state->position.y = get_value(state_interfaces_[1]); + system_state->position.z = get_value(state_interfaces_[2]); + system_state->orientation.x = get_value(state_interfaces_[3]); + system_state->orientation.y = get_value(state_interfaces_[4]); + system_state->orientation.z = get_value(state_interfaces_[5]); + system_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 + if (common::math::has_nan(common::messages::to_vector(*system_state))) { + RCLCPP_DEBUG(logger_, "Received state with NaN value."); // NOLINT return controller_interface::return_type::ERROR; } return controller_interface::return_type::OK; @@ -330,16 +330,16 @@ auto GeometricTrajectoryController::update(const rclcpp::Time & time, const rclc return controller_interface::return_type::OK; } - const geometry_msgs::msg::Pose end_effector_state = *state_.readFromRT(); + const geometry_msgs::msg::Pose system_state = *state_.readFromRT(); geometry_msgs::msg::Pose reference_state; common::messages::reset_message(&reference_state); - auto command_state = end_effector_state; + auto command_state = system_state; double error = std::numeric_limits::quiet_NaN(); - auto publish_controller_state = [this, &reference_state, &end_effector_state, &error, &command_state]() { + auto publish_controller_state = [this, &reference_state, &system_state, &error, &command_state]() { controller_state_.header.stamp = get_node()->now(); controller_state_.reference = reference_state; - controller_state_.feedback = end_effector_state; + controller_state_.feedback = system_state; controller_state_.error = error; controller_state_.output = command_state; rt_controller_state_pub_->try_publish(controller_state_); @@ -347,7 +347,7 @@ auto GeometricTrajectoryController::update(const rclcpp::Time & time, const rclc // hold position until a new trajectory is received if (*rt_holding_position_.readFromRT()) { - write_command(command_interfaces_, end_effector_state); + write_command(command_interfaces_, system_state); publish_controller_state(); return controller_interface::return_type::OK; } @@ -355,7 +355,7 @@ auto GeometricTrajectoryController::update(const rclcpp::Time & time, const rclc // set the sample time rclcpp::Time sample_time = time; if (*rt_first_sample_.readFromRT()) { - rt_trajectory_.readFromRT()->reset_initial_state(end_effector_state); + rt_trajectory_.readFromRT()->reset_initial_state(system_state); rt_first_sample_.writeFromNonRT(false); sample_time += period; } @@ -370,7 +370,7 @@ auto GeometricTrajectoryController::update(const rclcpp::Time & time, const rclc // 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); + error = geodesic_error(reference_state, system_state); } bool path_tolerance_exceeded = false; @@ -384,7 +384,7 @@ auto GeometricTrajectoryController::update(const rclcpp::Time & time, const rclc if (path_tolerance > 0.0 && error > path_tolerance) { path_tolerance_exceeded = true; rt_holding_position_.writeFromNonRT(true); - command_state = end_effector_state; + command_state = system_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 } @@ -398,7 +398,7 @@ auto GeometricTrajectoryController::update(const rclcpp::Time & time, const rclc 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); + const double goal_error = geodesic_error(trajectory->end_point().value(), system_state); RCLCPP_INFO(logger_, "Trajectory sample time is after trajectory end time."); // NOLINT if (goal_tolerance > 0.0) { if (goal_error > goal_tolerance) { @@ -425,7 +425,7 @@ auto GeometricTrajectoryController::update(const rclcpp::Time & time, const rclc auto feedback = std::make_shared(); feedback->header.stamp = time; feedback->desired = reference_state; - feedback->actual = end_effector_state; + feedback->actual = system_state; feedback->error = error; active_goal->setFeedback(feedback); diff --git a/geometric_controllers/src/trajectory_controller_parameters.yaml b/geometric_controllers/src/trajectory_controller_parameters.yaml index d65d17e..ef43994 100644 --- a/geometric_controllers/src/trajectory_controller_parameters.yaml +++ b/geometric_controllers/src/trajectory_controller_parameters.yaml @@ -42,21 +42,21 @@ geometric_trajectory_controller: 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_transform are set to true, - the controller will use the end effector states provided via a topic. + Use states provided via a topic instead of state interfaces or tf2. If + both this and lookup_transform are set to true, the controller will use + the states provided via a topic. lookup_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. + Use tf2 to look up the state transform instead of using state + interfaces or a topic. This is useful in scenarios where the pose + is determined only by using tf2 to perform forward kinematics. If both this and use_external_measured_states are set to true, the - controller will use the end effector states provided via a topic. + controller will use the states provided via a topic. reference_controller: type: string @@ -66,21 +66,21 @@ geometric_trajectory_controller: The prefix of the reference controller to send commands to. This can be used to configure command interfaces in chained mode. - parent_frame_id: + 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_transform. + The name of the parent frame. This is used to lookup the body pose when + using lookup_transform. child_frame_id: type: string - default_value: tcp + default_value: base_link_frd read_only: true description: > - The name of the end effector frame. This is used to lookup the end - effector pose when using lookup_transform. + The name of the body frame. This is used to lookup the body pose when + using lookup_transform. action_monitor_rate: type: int From 9cec1d6931163ff95fec6be97b6fa5a065707d62 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Thu, 31 Jul 2025 18:38:17 -0700 Subject: [PATCH 03/19] refactor and made initial pass at pd controller --- auv_control_msgs/CMakeLists.txt | 9 +- ...jectory.action => FollowTrajectory.action} | 2 +- auv_control_msgs/msg/ImpedanceCommand.msg | 9 + ...GeometricTrajectory.msg => Trajectory.msg} | 2 +- ...g => TrajectoryControllerStateStamped.msg} | 4 +- ...rajectoryPoint.msg => TrajectoryPoint.msg} | 2 +- .../include/controller_common/common.hpp | 7 + controller_common/src/common.cpp | 30 ++ geometric_controllers/CHANGELOG.md | 17 - geometric_controllers/README.md | 30 -- .../tracking_controller.hpp | 0 .../src/tracking_controller.cpp | 0 impedance_controller/CMakeLists.txt | 26 ++ .../LICENSE | 0 .../impedance_controller.hpp | 106 ++++++ impedance_controller/package.xml | 18 + .../src/impedance_controller.cpp | 319 ++++++++++++++++++ .../src/impedance_controller_parameters.yaml | 59 ++++ .../CMakeLists.txt | 30 +- trajectory_controller/LICENSE | 17 + trajectory_controller/README.md | 29 ++ .../trajectory_controller}/trajectory.hpp | 10 +- .../trajectory_controller.hpp | 30 +- .../package.xml | 4 +- .../src/trajectory.cpp | 10 +- .../src/trajectory_controller.cpp | 42 ++- .../src/trajectory_controller_parameters.yaml | 2 +- .../trajectory_controller.xml | 6 +- ...egral_terminal_sliding_mode_controller.cpp | 2 +- 29 files changed, 695 insertions(+), 127 deletions(-) rename auv_control_msgs/action/{FollowGeometricTrajectory.action => FollowTrajectory.action} (96%) create mode 100644 auv_control_msgs/msg/ImpedanceCommand.msg rename auv_control_msgs/msg/{GeometricTrajectory.msg => Trajectory.msg} (58%) rename auv_control_msgs/msg/{GeometricTrajectoryControllerState.msg => TrajectoryControllerStateStamped.msg} (84%) rename auv_control_msgs/msg/{GeometricTrajectoryPoint.msg => TrajectoryPoint.msg} (80%) delete mode 100644 geometric_controllers/CHANGELOG.md delete mode 100644 geometric_controllers/README.md delete mode 100644 geometric_controllers/include/geometric_controllers/tracking_controller.hpp delete mode 100644 geometric_controllers/src/tracking_controller.cpp create mode 100644 impedance_controller/CMakeLists.txt rename {geometric_controllers => impedance_controller}/LICENSE (100%) create mode 100644 impedance_controller/include/impedance_controller/impedance_controller.hpp create mode 100644 impedance_controller/package.xml create mode 100644 impedance_controller/src/impedance_controller.cpp create mode 100644 impedance_controller/src/impedance_controller_parameters.yaml rename {geometric_controllers => trajectory_controller}/CMakeLists.txt (71%) create mode 100644 trajectory_controller/LICENSE create mode 100644 trajectory_controller/README.md rename {geometric_controllers/include/geometric_controllers => trajectory_controller/include/trajectory_controller}/trajectory.hpp (88%) rename {geometric_controllers/include/geometric_controllers => trajectory_controller/include/trajectory_controller}/trajectory_controller.hpp (84%) rename {geometric_controllers => trajectory_controller}/package.xml (90%) rename {geometric_controllers => trajectory_controller}/src/trajectory.cpp (95%) rename {geometric_controllers => trajectory_controller}/src/trajectory_controller.cpp (92%) rename {geometric_controllers => trajectory_controller}/src/trajectory_controller_parameters.yaml (98%) rename geometric_controllers/geometric_controllers.xml => trajectory_controller/trajectory_controller.xml (57%) diff --git a/auv_control_msgs/CMakeLists.txt b/auv_control_msgs/CMakeLists.txt index 7ca6a9c..34a29f2 100644 --- a/auv_control_msgs/CMakeLists.txt +++ b/auv_control_msgs/CMakeLists.txt @@ -11,10 +11,11 @@ find_package(trajectory_msgs REQUIRED) rosidl_generate_interfaces(auv_control_msgs "msg/MultiActuatorStateStamped.msg" "msg/IKControllerStateStamped.msg" - "msg/GeometricTrajectoryPoint.msg" - "msg/GeometricTrajectory.msg" - "msg/GeometricTrajectoryControllerState.msg" - "action/FollowGeometricTrajectory.action" + "msg/TrajectoryPoint.msg" + "msg/Trajectory.msg" + "msg/TrajectoryControllerStateStamped.msg" + "msg/ImpedanceCommand.msg" + "action/FollowTrajectory.action" DEPENDENCIES builtin_interfaces std_msgs geometry_msgs trajectory_msgs ) diff --git a/auv_control_msgs/action/FollowGeometricTrajectory.action b/auv_control_msgs/action/FollowTrajectory.action similarity index 96% rename from auv_control_msgs/action/FollowGeometricTrajectory.action rename to auv_control_msgs/action/FollowTrajectory.action index 1d6114a..afd37e6 100644 --- a/auv_control_msgs/action/FollowGeometricTrajectory.action +++ b/auv_control_msgs/action/FollowTrajectory.action @@ -3,7 +3,7 @@ # 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/GeometricTrajectory trajectory +auv_control_msgs/Trajectory 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. diff --git a/auv_control_msgs/msg/ImpedanceCommand.msg b/auv_control_msgs/msg/ImpedanceCommand.msg new file mode 100644 index 0000000..9bf4d5b --- /dev/null +++ b/auv_control_msgs/msg/ImpedanceCommand.msg @@ -0,0 +1,9 @@ +std_msgs/Header header + +string child_frame_id + +geometry_msgs/Pose pose + +geometry_msgs/Twist twist + +geometry_msgs/Wrench wrench diff --git a/auv_control_msgs/msg/GeometricTrajectory.msg b/auv_control_msgs/msg/Trajectory.msg similarity index 58% rename from auv_control_msgs/msg/GeometricTrajectory.msg rename to auv_control_msgs/msg/Trajectory.msg index 792b740..943ea7d 100644 --- a/auv_control_msgs/msg/GeometricTrajectory.msg +++ b/auv_control_msgs/msg/Trajectory.msg @@ -1,4 +1,4 @@ std_msgs/Header header # The sequence of end effector points to track. -auv_control_msgs/GeometricTrajectoryPoint[] points +auv_control_msgs/TrajectoryPoint[] points diff --git a/auv_control_msgs/msg/GeometricTrajectoryControllerState.msg b/auv_control_msgs/msg/TrajectoryControllerStateStamped.msg similarity index 84% rename from auv_control_msgs/msg/GeometricTrajectoryControllerState.msg rename to auv_control_msgs/msg/TrajectoryControllerStateStamped.msg index 7410871..1b0be17 100644 --- a/auv_control_msgs/msg/GeometricTrajectoryControllerState.msg +++ b/auv_control_msgs/msg/TrajectoryControllerStateStamped.msg @@ -4,10 +4,10 @@ std_msgs/Header header # for error calculation. geometry_msgs/Pose reference -# The current end effector state. +# The current state. geometry_msgs/Pose feedback -# The squared Euclidean norm of the geodesic distance between the reference state and end effector state. +# The squared Euclidean norm of the geodesic distance between the reference state and state. float64 error # The trajectory controller command. This is the trajectory sample from the next time instance. diff --git a/auv_control_msgs/msg/GeometricTrajectoryPoint.msg b/auv_control_msgs/msg/TrajectoryPoint.msg similarity index 80% rename from auv_control_msgs/msg/GeometricTrajectoryPoint.msg rename to auv_control_msgs/msg/TrajectoryPoint.msg index dbca6e6..5715121 100644 --- a/auv_control_msgs/msg/GeometricTrajectoryPoint.msg +++ b/auv_control_msgs/msg/TrajectoryPoint.msg @@ -1,4 +1,4 @@ -# The sequence of end effector poses. +# The sequence of poses. geometry_msgs/Pose point # The time that this point should be reached, measured from the start of the trajectory. diff --git a/controller_common/include/controller_common/common.hpp b/controller_common/include/controller_common/common.hpp index 4da7d36..15895fc 100644 --- a/controller_common/include/controller_common/common.hpp +++ b/controller_common/include/controller_common/common.hpp @@ -22,6 +22,7 @@ #include +#include "auv_control_msgs/msg/impedance_command.hpp" #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/twist.hpp" #include "geometry_msgs/msg/wrench.hpp" @@ -41,10 +42,14 @@ auto to_vector(const geometry_msgs::msg::Wrench & wrench) -> std::vector auto to_vector(const nav_msgs::msg::Odometry & odom) -> std::vector; +auto to_vector(const auv_control_msgs::msg::ImpedanceCommand & command) -> std::vector; + auto to_msg(const std::vector & data, geometry_msgs::msg::Pose * msg) -> void; auto to_msg(const std::vector & data, geometry_msgs::msg::Twist * msg) -> void; +auto to_msg(const std::vector & data, geometry_msgs::msg::Wrench * msg) -> void; + auto reset_message(geometry_msgs::msg::Pose * msg) -> void; auto reset_message(geometry_msgs::msg::Twist * msg) -> void; @@ -53,6 +58,8 @@ auto reset_message(geometry_msgs::msg::Wrench * msg) -> void; auto reset_message(nav_msgs::msg::Odometry * msg) -> void; +auto reset_message(auv_control_msgs::msg::ImpedanceCommand * msg) -> void; + } // namespace messages namespace math diff --git a/controller_common/src/common.cpp b/controller_common/src/common.cpp index c781378..266badc 100644 --- a/controller_common/src/common.cpp +++ b/controller_common/src/common.cpp @@ -77,6 +77,19 @@ auto to_vector(const nav_msgs::msg::Odometry & odom) -> std::vector odom.twist.twist.angular.z}; } +auto to_vector(const auv_control_msgs::msg::ImpedanceCommand & command) -> std::vector +{ + std::vector data; + auto pose = to_vector(command.pose); + auto twist = to_vector(command.twist); + auto wrench = to_vector(command.wrench); + data.reserve(pose.size() + twist.size() + wrench.size()); + std::ranges::copy(pose, std::back_inserter(data)); + std::ranges::copy(twist, std::back_inserter(data)); + std::ranges::copy(wrench, std::back_inserter(data)); + return data; +} + auto to_msg(const std::vector & data, geometry_msgs::msg::Pose * msg) -> void { msg->position.x = data[0]; @@ -98,6 +111,16 @@ auto to_msg(const std::vector & data, geometry_msgs::msg::Twist * msg) - msg->angular.z = data[5]; } +auto to_msg(const std::vector & data, geometry_msgs::msg::Wrench * msg) -> void +{ + msg->force.x = data[0]; + msg->force.y = data[1]; + msg->force.z = data[2]; + msg->torque.x = data[3]; + msg->torque.y = data[4]; + msg->torque.z = data[5]; +} + auto reset_message(geometry_msgs::msg::Pose * msg) -> void { msg->position.x = nan; @@ -146,6 +169,13 @@ auto reset_message(nav_msgs::msg::Odometry * msg) -> void msg->twist.twist.angular.z = nan; } +auto reset_message(auv_control_msgs::msg::ImpedanceCommand * msg) -> void +{ + reset_message(&msg->pose); + reset_message(&msg->twist); + reset_message(&msg->wrench); +} + } // namespace messages namespace math diff --git a/geometric_controllers/CHANGELOG.md b/geometric_controllers/CHANGELOG.md deleted file mode 100644 index 6a05108..0000000 --- a/geometric_controllers/CHANGELOG.md +++ /dev/null @@ -1,17 +0,0 @@ -# Changelog for package geometric_controllers - -## 0.3.3 (2025-07-29) - -## 0.3.2 (2025-07-22) - -- Replaces the deprecated `unlockAndPublish` API with `try_publish` - -## 0.3.1 (2025-07-09) - -## 0.3.0 (2025-06-07) - -## 0.2.1 (2025-06-03) - -## 0.2.0 (2025-05-03) - -- Implements the geometric_trajectory_controller diff --git a/geometric_controllers/README.md b/geometric_controllers/README.md deleted file mode 100644 index e42ca41..0000000 --- a/geometric_controllers/README.md +++ /dev/null @@ -1,30 +0,0 @@ -# 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 - -geometric_trajectory_controller/GeometricTrajectoryController - -## 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 - -- geometric_trajectory_controller/trajectory [auv_control_msgs::msg::GeometricTrajectory] - -## Action Servers - -- geometric_trajectory_controller/follow_trajectory [auv_control_msgs::action::FollowGeometricTrajectory] - -## Publishers - -- geometric_trajectory_controller/status [auv_control_msgs::msg::GeometricTrajectoryControllerState] diff --git a/geometric_controllers/include/geometric_controllers/tracking_controller.hpp b/geometric_controllers/include/geometric_controllers/tracking_controller.hpp deleted file mode 100644 index e69de29..0000000 diff --git a/geometric_controllers/src/tracking_controller.cpp b/geometric_controllers/src/tracking_controller.cpp deleted file mode 100644 index e69de29..0000000 diff --git a/impedance_controller/CMakeLists.txt b/impedance_controller/CMakeLists.txt new file mode 100644 index 0000000..551c48b --- /dev/null +++ b/impedance_controller/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 3.8) +project(impedance_controller) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/geometric_controllers/LICENSE b/impedance_controller/LICENSE similarity index 100% rename from geometric_controllers/LICENSE rename to impedance_controller/LICENSE diff --git a/impedance_controller/include/impedance_controller/impedance_controller.hpp b/impedance_controller/include/impedance_controller/impedance_controller.hpp new file mode 100644 index 0000000..7c8c14e --- /dev/null +++ b/impedance_controller/include/impedance_controller/impedance_controller.hpp @@ -0,0 +1,106 @@ +// 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/msg/impedance_command.hpp" +#include "control_msgs/msg/multi_dof_state_stamped.hpp" +#include "controller_interface/chainable_controller_interface.hpp" +#include "controller_interface/controller_interface.hpp" +#include "hydrodynamics/hydrodynamics.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/realtime_buffer.hpp" +#include "realtime_tools/realtime_publisher.hpp" +#include "std_msgs/msg/string.hpp" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" + +// auto-generated by generate_parameter_library +#include + +namespace impedance_controller +{ + +class ImpedanceController : public controller_interface::ChainableControllerInterface +{ +public: + ImpedanceController() = default; + + auto on_init() -> controller_interface::CallbackReturn 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 command_interface_configuration() const -> controller_interface::InterfaceConfiguration override; + + auto state_interface_configuration() const -> controller_interface::InterfaceConfiguration override; + + auto update_and_write_commands(const rclcpp::Time & time, const rclcpp::Duration & period) + -> controller_interface::return_type override; + +protected: + auto on_export_reference_interfaces() -> std::vector override; + + auto update_reference_from_subscribers(const rclcpp::Time & time, const rclcpp::Duration & period) + -> controller_interface::return_type override; + + auto update_system_state_values() -> controller_interface::return_type; + + auto update_parameters() -> void; + + auto configure_parameters() -> controller_interface::CallbackReturn; + + auto update_and_validate_interfaces() -> controller_interface::return_type; + + // provide an interface for setting reference commands from a topic + // this is helpful when we want to set the reference using a non-ros2_control interface + realtime_tools::RealtimeBuffer reference_; + std::shared_ptr> reference_sub_; + + realtime_tools::RealtimeBuffer system_state_; + std::shared_ptr> system_state_sub_; + std::vector system_state_values_; + + using ControllerState = control_msgs::msg::MultiDOFStateStamped; + std::shared_ptr> controller_state_pub_; + std::unique_ptr> rt_controller_state_pub_; + ControllerState controller_state_; + + std::shared_ptr param_listener_; + impedance_controller::Params params_; + + // the dofs are passed as a parameter to the controller + // we could make these static, but there may be scenarios where users want to rename the interfaces + std::vector command_dofs_, state_dofs_; + std::size_t n_command_dofs_, n_state_dofs_; + + // controller gains + Eigen::Matrix6d kp_, kd_; + + rclcpp::Logger logger_{rclcpp::get_logger("impedance_controller")}; +}; + +} // namespace impedance_controller diff --git a/impedance_controller/package.xml b/impedance_controller/package.xml new file mode 100644 index 0000000..10c8308 --- /dev/null +++ b/impedance_controller/package.xml @@ -0,0 +1,18 @@ + + + + impedance_controller + 0.0.0 + TODO: Package description + Evan Palmer + MIT + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/impedance_controller/src/impedance_controller.cpp b/impedance_controller/src/impedance_controller.cpp new file mode 100644 index 0000000..46c43af --- /dev/null +++ b/impedance_controller/src/impedance_controller.cpp @@ -0,0 +1,319 @@ +// 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 "impedance_controller/impedance_controller.hpp" + +#include "controller_common/common.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "tf2_eigen/tf2_eigen.hpp" + +namespace impedance_controller +{ + +namespace +{ + +auto geodesic_error(const geometry_msgs::msg::Pose & goal, const geometry_msgs::msg::Pose & state) -> Eigen::Vector6d +{ + Eigen::Isometry3d goal_mat, state_mat; // NOLINT + tf2::fromMsg(goal, goal_mat); + tf2::fromMsg(state, state_mat); + return (goal_mat.inverse() * state_mat).matrix().log(); +} + +} // namespace + +auto ImpedanceController::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; +} + +auto ImpedanceController::update_parameters() -> void +{ + if (!param_listener_->is_old(params_)) { + return; + } + param_listener_->refresh_dynamic_parameters(); + params_ = param_listener_->get_params(); +} + +auto ImpedanceController::configure_parameters() -> controller_interface::CallbackReturn +{ + update_parameters(); + + command_dofs_ = params_.command_joints; + n_command_dofs_ = command_dofs_.size(); + + state_dofs_ = params_.state_joints; + n_state_dofs_ = state_dofs_.size(); + + auto get_gains = [this](auto field) { + auto gains = command_dofs_ | + std::views::transform([&](const auto & dof) { return params_.gains.command_joints_map[dof].*field; }); + return std::vector(gains.begin(), gains.end()); + }; + + // proportional gain + auto kp = get_gains(&impedance_controller::Params::Gains::MapCommandJoints::kp); + kp_ = Eigen::Vector6d(kp.data()).asDiagonal(); + + // derivative gain + auto kd = get_gains(&impedance_controller::Params::Gains::MapCommandJoints::kd); + kd_ = Eigen::Vector6d(kd.data()).asDiagonal(); + + return controller_interface::CallbackReturn::SUCCESS; +} + +auto ImpedanceController::on_configure(const rclcpp_lifecycle::State & previous_state) + -> controller_interface::CallbackReturn +{ + configure_parameters(); + + reference_.writeFromNonRT(auv_control_msgs::msg::ImpedanceCommand()); + system_state_.writeFromNonRT(nav_msgs::msg::Odometry()); + + command_interfaces_.reserve(n_command_dofs_); + state_interfaces_.reserve(n_state_dofs_); + system_state_values_.resize(n_state_dofs_, std::numeric_limits::quiet_NaN()); + + if (params_.use_external_measured_states) { + RCLCPP_INFO(logger_, "Using external measured states"); // NOLINT + system_state_sub_ = get_node()->create_subscription( + "~/system_state", + rclcpp::SystemDefaultsQoS(), + [this](const std::shared_ptr msg) { // NOLINT + system_state_.writeFromNonRT(*msg); + }); + } + + controller_state_pub_ = get_node()->create_publisher("~/status", rclcpp::SystemDefaultsQoS()); + rt_controller_state_pub_ = + std::make_unique>(controller_state_pub_); + + controller_state_.dof_states.resize(n_command_dofs_); + for (auto && [state, dof] : std::views::zip(controller_state_.dof_states, command_dofs_)) { + state.name = dof; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +auto ImpedanceController::on_activate(const rclcpp_lifecycle::State & previous_state) + -> controller_interface::CallbackReturn +{ + common::messages::reset_message(reference_.readFromNonRT()); + common::messages::reset_message(system_state_.readFromNonRT()); + + reference_interfaces_.assign(reference_interfaces_.size(), std::numeric_limits::quiet_NaN()); + system_state_values_.assign(system_state_values_.size(), std::numeric_limits::quiet_NaN()); + + return controller_interface::CallbackReturn::SUCCESS; +} + +auto ImpedanceController::command_interface_configuration() const -> controller_interface::InterfaceConfiguration +{ + controller_interface::InterfaceConfiguration config; + config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + config.names.reserve(n_command_dofs_); + + for (const auto & dof : command_dofs_) { + config.names.emplace_back( + params_.reference_controller.empty() + ? std::format("{}/{}", dof, hardware_interface::HW_IF_EFFORT) + : std::format("{}/{}/{}", params_.reference_controller, dof, hardware_interface::HW_IF_EFFORT)); + } + + return config; +} + +auto ImpedanceController::state_interface_configuration() const -> controller_interface::InterfaceConfiguration +{ + controller_interface::InterfaceConfiguration config; + if (params_.use_external_measured_states) { + config.type = controller_interface::interface_configuration_type::NONE; + } else { + config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + config.names.reserve(n_state_dofs_); + for (std::size_t i = 0; i < 7; ++i) { + config.names.emplace_back(std::format("{}/{}", state_dofs_[i], hardware_interface::HW_IF_POSITION)); + } + for (std::size_t i = 7; i < n_state_dofs_; ++i) { + config.names.emplace_back(std::format("{}/{}", state_dofs_[i], hardware_interface::HW_IF_VELOCITY)); + } + } + return config; +} + +auto ImpedanceController::on_export_reference_interfaces() -> std::vector +{ + // the reference command includes the desired pose, velocity, and force/torque + reference_interfaces_.resize(n_command_dofs_ + n_state_dofs_, std::numeric_limits::quiet_NaN()); + std::vector interfaces; + interfaces.reserve(reference_interfaces_.size()); + + // add the pose & velocity interfaces + for (const auto [i, dof] : std::views::enumerate(state_dofs_)) { + interfaces.emplace_back( + get_node()->get_name(), std::format("{}/{}", dof, hardware_interface::HW_IF_POSITION), &reference_interfaces_[i]); + } + + // add the force/torque interfaces + for (const auto [i, dof] : std::views::enumerate(command_dofs_)) { + interfaces.emplace_back( + get_node()->get_name(), + std::format("{}/{}", dof, hardware_interface::HW_IF_EFFORT), + &reference_interfaces_[i + n_state_dofs_]); + } + + return interfaces; +} + +auto ImpedanceController::update_reference_from_subscribers(const rclcpp::Time & time, const rclcpp::Duration & period) + -> controller_interface::return_type +{ + auto * current_reference = reference_.readFromNonRT(); + const std::vector reference = common::messages::to_vector(*current_reference); + for (auto && [interface, ref] : std::views::zip(reference_interfaces_, reference)) { + if (!std::isnan(ref)) { + interface = ref; + } + } + common::messages::reset_message(current_reference); + return controller_interface::return_type::OK; +} + +auto ImpedanceController::update_system_state_values() -> controller_interface::return_type +{ + if (params_.use_external_measured_states) { + auto * current_state = system_state_.readFromNonRT(); + std::ranges::copy(common::messages::to_vector(current_state), system_state_values_.begin()); + } else { + std::ranges::transform(state_interfaces_, system_state_values_.begin(), [](const auto & interface) { + return interface.get_optional().value_or(std::numeric_limits::quiet_NaN()); + }); + } + + if (common::math::has_nan(system_state_values_)) { + RCLCPP_DEBUG(logger_, "Received system state with NaN value."); // NOLINT + return controller_interface::return_type::ERROR; + } + + return controller_interface::return_type::OK; +} + +auto ImpedanceController::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 (common::math::has_nan(reference_interfaces_)) { + RCLCPP_DEBUG(logger_, "Received reference with NaN value."); // NOLINT + return controller_interface::return_type::ERROR; + } + return controller_interface::return_type::OK; +} + +auto ImpedanceController::update_and_write_commands(const rclcpp::Time & time, const rclcpp::Duration & period) + -> controller_interface::return_type +{ + if (update_and_validate_interfaces() != controller_interface::return_type::OK) { + RCLCPP_DEBUG(logger_, "Skipping controller update. Failed to update and validate interfaces"); // NOLINT + return controller_interface::return_type::OK; + } + + configure_parameters(); + + // get the reference pose, twist, and wrench values + const auto pose_end = reference_interfaces_.begin() + 7; + const auto twist_end = pose_end + 6; + const int wrench_end = twist_end + 6; + + const std::vector ref_pose_values(reference_interfaces_.begin(), pose_end); + const std::vector ref_twist_values(pose_end, twist_end); + const std::vector ref_wrench_values(twist_end, wrench_end); + + // get the state pose and twist values + const std::vector state_pose_values(system_state_values_.begin(), system_state_values_.begin() + 7); + const std::vector state_twist_values(system_state_values_.begin() + 7, system_state_values_.end()); + + // get the state and reference pose as messages + geometry_msgs::msg::Pose reference_pose; + common::messages::to_msg(ref_pose_values, &reference_pose); + + geometry_msgs::msg::Pose state_pose; + common::messages::to_msg(state_pose_values, &state_pose); + + // calculate the pose error + const Eigen::Vector6d pose_error = geodesic_error(reference_pose, state_pose); + const std::vector pose_error_values(pose_error.data(), pose_error.data() + pose_error.size()); + + // calculate the velocity error + const std::vector twist_error_values = common::math::calculate_error(ref_twist_values, state_twist_values); + const Eigen::Vector6d twist_error(twist_error_values.data()); + + if (common::math::all_nan(pose_error_values)) { + RCLCPP_DEBUG(logger_, "All pose error values are NaN. Skipping control update."); // NOLINT + return controller_interface::return_type::OK; + } + + if (common::math::all_nan(twist_error_values)) { + RCLCPP_DEBUG(logger_, "All velocity error values are NaN. Skipping control update."); // NOLINT + return controller_interface::return_type::OK; + } + + // convert the reference wrench values to an Eigen vector + Eigen::Vector6d reference_wrench(ref_wrench_values.data()); + + // calculate the control command + Eigen::Vector6d t = reference_wrench + kp_ * pose_error + kd_ * twist_error; + + for (auto && [command_interface, tau] : std::views::zip(command_interfaces_, t)) { + if (!command_interface.set_value(tau)) { + RCLCPP_WARN(logger_, "Failed to set command for joint %s", command_interface.get_name().c_str()); // NOLINT + } + } + + controller_state_.header.stamp = time; + for (auto && [i, state] : std::views::enumerate(controller_state_.dof_states)) { + const auto out = command_interfaces_[i].get_optional(); + state.error = pose_error_values[i]; + state.error_dot = twist_error_values[i]; + state.time_step = period.seconds(); + state.output = out.value_or(std::numeric_limits::quiet_NaN()); + } + + // the feedback and reference values have different sizes than the command interfaces + for (std::size_t i = 0; i < n_state_dofs_; ++i) { + controller_state_.dof_states[i].feedback = system_state_values_[i]; + } + for (std::size_t i = n_state_dofs_; i < n_command_dofs_ + n_state_dofs_; ++i) { + controller_state_.dof_states[i].reference = reference_interfaces_[i]; + } + + rt_controller_state_pub_->try_publish(controller_state_); + + return controller_interface::return_type::OK; +} + +} // namespace impedance_controller diff --git a/impedance_controller/src/impedance_controller_parameters.yaml b/impedance_controller/src/impedance_controller_parameters.yaml new file mode 100644 index 0000000..4de934e --- /dev/null +++ b/impedance_controller/src/impedance_controller_parameters.yaml @@ -0,0 +1,59 @@ +impedance_controller: + state_joints: + type: string_array + default_value: [x, y, z, qx, qy, qz, qw, x, y, z, rx, ry, rz] + read_only: true + description: > + The state joints. This should be provided in the same position, + orientation, and velocity order as the default. + validation: + fixed_size<>: 13 + + command_joints: + type: string_array + default_value: [x, y, z, rx, ry, rz] + read_only: true + description: The controlled force/torque joints. + validation: + fixed_size<>: 6 + + use_external_measured_states: + type: bool + read_only: true + default_value: false + description: > + Use state measurements obtained from a topic instead of state interfaces. + + 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. + + frame_id: + type: string + default_value: odom_ned + read_only: true + description: The name of the inertial frame. + + child_frame_id: + type: string + default_value: base_link_fsd + read_only: true + description: The name of the body frame. + + gains: + __map_command_joints: + kp: + type: double + default_value: 0.0 + read_only: false + description: The proportional feedback gain. + + kd: + type: double + default_value: 0.0 + read_only: false + description: The derivative feedback gain. diff --git a/geometric_controllers/CMakeLists.txt b/trajectory_controller/CMakeLists.txt similarity index 71% rename from geometric_controllers/CMakeLists.txt rename to trajectory_controller/CMakeLists.txt index 94d95a6..24df18a 100644 --- a/geometric_controllers/CMakeLists.txt +++ b/trajectory_controller/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.23) -project(geometric_controllers) +project(trajectory_controller) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic) @@ -26,26 +26,26 @@ find_package(lifecycle_msgs REQUIRED) find_package(pluginlib REQUIRED) find_package(tf2_geometry_msgs REQUIRED) -generate_parameter_library(geometric_trajectory_controller_parameters - src/geometric_trajectory_controller_parameters.yaml +generate_parameter_library(trajectory_controller_parameters + src/trajectory_controller_parameters.yaml ) -add_library(geometric_controllers SHARED) +add_library(trajectory_controller SHARED) target_sources( - geometric_controllers + trajectory_controller PRIVATE src/trajectory_controller.cpp src/trajectory.cpp PUBLIC FILE_SET HEADERS BASE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include FILES - ${CMAKE_CURRENT_SOURCE_DIR}/include/geometric_controllers/trajectory_controller.hpp - ${CMAKE_CURRENT_SOURCE_DIR}/include/geometric_controllers/trajectory.hpp + ${CMAKE_CURRENT_SOURCE_DIR}/include/trajectory_controller/trajectory_controller.hpp + ${CMAKE_CURRENT_SOURCE_DIR}/include/trajectory_controller/trajectory.hpp ) -target_compile_features(geometric_controllers PUBLIC cxx_std_23) +target_compile_features(trajectory_controller PUBLIC cxx_std_23) target_link_libraries( - geometric_controllers + trajectory_controller PUBLIC - geometric_trajectory_controller_parameters + trajectory_controller_parameters realtime_tools::realtime_tools controller_common::controller_common hardware_interface::hardware_interface @@ -62,20 +62,20 @@ target_link_libraries( ${lifecycle_msgs_TARGETS} ) -pluginlib_export_plugin_description_file(controller_interface geometric_controllers.xml) +pluginlib_export_plugin_description_file(controller_interface trajectory_controller.xml) install( TARGETS - geometric_controllers - geometric_trajectory_controller_parameters - EXPORT export_geometric_controllers + trajectory_controller + trajectory_controller_parameters + EXPORT export_trajectory_controller LIBRARY DESTINATION lib/${PROJECT_NAME} ARCHIVE DESTINATION lib/${PROJECT_NAME} RUNTIME DESTINATION bin/${PROJECT_NAME} FILE_SET HEADERS ) -ament_export_targets(export_geometric_controllers HAS_LIBRARY_TARGET) +ament_export_targets(export_trajectory_controller HAS_LIBRARY_TARGET) ament_export_dependencies( "geometry_msgs" "realtime_tools" diff --git a/trajectory_controller/LICENSE b/trajectory_controller/LICENSE new file mode 100644 index 0000000..30e8e2e --- /dev/null +++ b/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/trajectory_controller/README.md b/trajectory_controller/README.md new file mode 100644 index 0000000..0bd7046 --- /dev/null +++ b/trajectory_controller/README.md @@ -0,0 +1,29 @@ +# Trajectory Controller + +The trajectory controller interpolates an SE(3) motion plan for pose control. +The positions are interpolated using linear interpolation, and the orientations +are interpolated using spherical linear interpolation. + +## Plugin Library + +trajectory_controller/TrajectoryController + +## References + +The input to this controller is a sequence of poses. + +## Commands + +The output of this controller is a sampled pose. + +## Subscribers + +- trajectory_controller/trajectory [auv_control_msgs::msg::Trajectory] + +## Action Servers + +- trajectory_controller/follow_trajectory [auv_control_msgs::action::FollowTrajectory] + +## Publishers + +- trajectory_controller/status [auv_control_msgs::msg::TrajectoryControllerState] diff --git a/geometric_controllers/include/geometric_controllers/trajectory.hpp b/trajectory_controller/include/trajectory_controller/trajectory.hpp similarity index 88% rename from geometric_controllers/include/geometric_controllers/trajectory.hpp rename to trajectory_controller/include/trajectory_controller/trajectory.hpp index 730f814..572adee 100644 --- a/geometric_controllers/include/geometric_controllers/trajectory.hpp +++ b/trajectory_controller/include/trajectory_controller/trajectory.hpp @@ -25,11 +25,11 @@ #include #include -#include "auv_control_msgs/msg/geometric_trajectory.hpp" +#include "auv_control_msgs/msg/trajectory.hpp" #include "geometry_msgs/msg/pose.hpp" #include "rclcpp/rclcpp.hpp" -namespace geometric_trajectory_controller +namespace trajectory_controller { enum class SampleError : std::uint8_t @@ -45,7 +45,7 @@ class Trajectory Trajectory() = default; /// Create a new trajectory given a trajectory message and the initial state. - Trajectory(const auv_control_msgs::msg::GeometricTrajectory & trajectory, const geometry_msgs::msg::Pose & state); + Trajectory(const auv_control_msgs::msg::Trajectory & trajectory, const geometry_msgs::msg::Pose & state); /// Whether or not the trajectory is empty. [[nodiscard]] auto empty() const -> bool; @@ -70,9 +70,9 @@ class Trajectory auto reset_initial_state(const geometry_msgs::msg::Pose & state) -> void; private: - auv_control_msgs::msg::GeometricTrajectory points_; + auv_control_msgs::msg::Trajectory points_; rclcpp::Time initial_time_; geometry_msgs::msg::Pose initial_state_; }; -} // namespace geometric_trajectory_controller +} // namespace trajectory_controller diff --git a/geometric_controllers/include/geometric_controllers/trajectory_controller.hpp b/trajectory_controller/include/trajectory_controller/trajectory_controller.hpp similarity index 84% rename from geometric_controllers/include/geometric_controllers/trajectory_controller.hpp rename to trajectory_controller/include/trajectory_controller/trajectory_controller.hpp index be9b55d..f8762b3 100644 --- a/geometric_controllers/include/geometric_controllers/trajectory_controller.hpp +++ b/trajectory_controller/include/trajectory_controller/trajectory_controller.hpp @@ -22,11 +22,10 @@ #include -#include "auv_control_msgs/action/follow_geometric_trajectory.hpp" -#include "auv_control_msgs/msg/geometric_trajectory_controller_state.hpp" +#include "auv_control_msgs/action/follow_trajectory.hpp" +#include "auv_control_msgs/msg/trajectory_controller_state_stamped.hpp" #include "controller_common/common.hpp" #include "controller_interface/controller_interface.hpp" -#include "geometric_trajectory_controller/trajectory.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/server.hpp" #include "realtime_tools/realtime_buffer.hpp" @@ -35,17 +34,18 @@ #include "tf2/exceptions.hpp" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" +#include "trajectory_controller/trajectory.hpp" // auto-generated by generate_parameter_library -#include +#include -namespace geometric_controllers +namespace trajectory_controller { -class GeometricTrajectoryController : public controller_interface::ControllerInterface +class TrajectoryController : public controller_interface::ControllerInterface { public: - GeometricTrajectoryController() = default; + TrajectoryController() = default; auto on_init() -> controller_interface::CallbackReturn override; @@ -68,10 +68,10 @@ class GeometricTrajectoryController : public controller_interface::ControllerInt auto update_system_state() -> controller_interface::return_type; - [[nodiscard]] auto validate_trajectory(const auv_control_msgs::msg::GeometricTrajectory & trajectory) const -> bool; + [[nodiscard]] auto validate_trajectory(const auv_control_msgs::msg::Trajectory & trajectory) const -> bool; // controller state - using ControllerState = auv_control_msgs::msg::GeometricTrajectoryControllerState; + using ControllerState = auv_control_msgs::msg::TrajectoryControllerStateStamped; std::shared_ptr> controller_state_pub_; std::unique_ptr> rt_controller_state_pub_; ControllerState controller_state_; @@ -91,9 +91,9 @@ class GeometricTrajectoryController : public controller_interface::ControllerInt // 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_; + std::shared_ptr> trajectory_sub_; - using FollowTrajectory = auv_control_msgs::action::FollowGeometricTrajectory; + using FollowTrajectory = auv_control_msgs::action::FollowTrajectory; using RealtimeGoalHandle = realtime_tools::RealtimeServerGoalHandle; using RealtimeGoalHandlePtr = std::shared_ptr; using RealtimeGoalHandleBuffer = realtime_tools::RealtimeBuffer; @@ -110,8 +110,8 @@ class GeometricTrajectoryController : public controller_interface::ControllerInt // the update period is used to sample the "next" trajectory point rclcpp::Duration update_period_{0, 0}; - std::shared_ptr param_listener_; - geometric_trajectory_controller::Params params_; + std::shared_ptr param_listener_; + trajectory_controller::Params params_; // error tolerances // the default tolerances are extracted from the parameters and applied when the action interface is not used @@ -122,7 +122,7 @@ class GeometricTrajectoryController : public controller_interface::ControllerInt std::vector dofs_; std::size_t n_dofs_; - rclcpp::Logger logger_{rclcpp::get_logger("geometric_trajectory_controller")}; + rclcpp::Logger logger_{rclcpp::get_logger("trajectory_controller")}; private: template @@ -137,4 +137,4 @@ class GeometricTrajectoryController : public controller_interface::ControllerInt } }; -} // namespace geometric_controllers +} // namespace trajectory_controller diff --git a/geometric_controllers/package.xml b/trajectory_controller/package.xml similarity index 90% rename from geometric_controllers/package.xml rename to trajectory_controller/package.xml index 43e0bad..7f7c0c6 100644 --- a/geometric_controllers/package.xml +++ b/trajectory_controller/package.xml @@ -2,9 +2,9 @@ - geometric_controllers + trajectory_controller 0.3.3 - Geometrically consistent controllers for AUVs and UVMS + Trajectory controller for single-link rigid-body systems Evan Palmer MIT diff --git a/geometric_controllers/src/trajectory.cpp b/trajectory_controller/src/trajectory.cpp similarity index 95% rename from geometric_controllers/src/trajectory.cpp rename to trajectory_controller/src/trajectory.cpp index 05c5159..75d95f6 100644 --- a/geometric_controllers/src/trajectory.cpp +++ b/trajectory_controller/src/trajectory.cpp @@ -18,7 +18,7 @@ // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE // SOFTWARE. -#include "geometric_trajectory_controller/trajectory.hpp" +#include "trajectory_controller/trajectory.hpp" #include #include @@ -27,7 +27,7 @@ #include "tf2_eigen/tf2_eigen.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -namespace geometric_trajectory_controller +namespace trajectory_controller { namespace @@ -93,9 +93,7 @@ auto interpolate( } // namespace -Trajectory::Trajectory( - const auv_control_msgs::msg::GeometricTrajectory & trajectory, - const geometry_msgs::msg::Pose & state) +Trajectory::Trajectory(const auv_control_msgs::msg::Trajectory & trajectory, const geometry_msgs::msg::Pose & state) : points_(trajectory), initial_time_(static_cast(trajectory.header.stamp)), initial_state_(state) @@ -162,4 +160,4 @@ auto Trajectory::sample(const rclcpp::Time & sample_time) const -> std::expected auto Trajectory::reset_initial_state(const geometry_msgs::msg::Pose & state) -> void { initial_state_ = state; } -} // namespace geometric_trajectory_controller +} // namespace trajectory_controller diff --git a/geometric_controllers/src/trajectory_controller.cpp b/trajectory_controller/src/trajectory_controller.cpp similarity index 92% rename from geometric_controllers/src/trajectory_controller.cpp rename to trajectory_controller/src/trajectory_controller.cpp index 1920211..3baa8c2 100644 --- a/geometric_controllers/src/trajectory_controller.cpp +++ b/trajectory_controller/src/trajectory_controller.cpp @@ -18,17 +18,18 @@ // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE // SOFTWARE. +#include "trajectory_controller/trajectory_controller.hpp" + #include #include #include "controller_common/common.hpp" -#include "geometric_trajectory_controller/geometric_trajectory_controller.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 geometric_controllers +namespace trajectory_controller { namespace @@ -44,16 +45,16 @@ auto geodesic_error(const geometry_msgs::msg::Pose & goal, const geometry_msgs:: } // namespace -auto GeometricTrajectoryController::on_init() -> controller_interface::CallbackReturn +auto TrajectoryController::on_init() -> controller_interface::CallbackReturn { - param_listener_ = std::make_shared(get_node()); + 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 GeometricTrajectoryController::update_parameters() -> void +auto TrajectoryController::update_parameters() -> void { if (!param_listener_->is_old(params_)) { return; @@ -62,7 +63,7 @@ auto GeometricTrajectoryController::update_parameters() -> void params_ = param_listener_->get_params(); } -auto GeometricTrajectoryController::configure_parameters() -> controller_interface::CallbackReturn +auto TrajectoryController::configure_parameters() -> controller_interface::CallbackReturn { update_parameters(); @@ -81,8 +82,7 @@ auto GeometricTrajectoryController::configure_parameters() -> controller_interfa return controller_interface::CallbackReturn::SUCCESS; } -auto GeometricTrajectoryController::validate_trajectory( - const auv_control_msgs::msg::GeometricTrajectory & trajectory) const -> bool +auto TrajectoryController::validate_trajectory(const auv_control_msgs::msg::Trajectory & trajectory) const -> bool { if (trajectory.points.empty()) { RCLCPP_ERROR(logger_, "Received empty trajectory"); // NOLINT @@ -118,7 +118,7 @@ auto GeometricTrajectoryController::validate_trajectory( return true; } -auto GeometricTrajectoryController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) +auto TrajectoryController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) -> controller_interface::CallbackReturn { configure_parameters(); @@ -141,10 +141,10 @@ auto GeometricTrajectoryController::on_configure(const rclcpp_lifecycle::State & } } - trajectory_sub_ = get_node()->create_subscription( + trajectory_sub_ = get_node()->create_subscription( "~/trajectory", rclcpp::SystemDefaultsQoS(), - [this](const std::shared_ptr msg) { // NOLINT + [this](const std::shared_ptr msg) { // NOLINT auto updated_msg = *msg; if (!validate_trajectory(updated_msg)) { RCLCPP_ERROR(logger_, "Ignoring invalid trajectory message"); // NOLINT @@ -224,7 +224,7 @@ auto GeometricTrajectoryController::on_configure(const rclcpp_lifecycle::State & goal_handle_timer_ = get_node()->create_wall_timer(action_monitor_period_, [rt_gh]() { rt_gh->runNonRealtime(); }); }; - action_server_ = rclcpp_action::create_server( + 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()); @@ -234,7 +234,7 @@ auto GeometricTrajectoryController::on_configure(const rclcpp_lifecycle::State & return controller_interface::CallbackReturn::SUCCESS; } -auto GeometricTrajectoryController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) +auto TrajectoryController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) -> controller_interface::CallbackReturn { rt_first_sample_.writeFromNonRT(true); @@ -243,8 +243,7 @@ auto GeometricTrajectoryController::on_activate(const rclcpp_lifecycle::State & return controller_interface::CallbackReturn::SUCCESS; } -auto GeometricTrajectoryController::command_interface_configuration() const - -> controller_interface::InterfaceConfiguration +auto TrajectoryController::command_interface_configuration() const -> controller_interface::InterfaceConfiguration { controller_interface::InterfaceConfiguration config; config.type = controller_interface::interface_configuration_type::INDIVIDUAL; @@ -259,8 +258,7 @@ auto GeometricTrajectoryController::command_interface_configuration() const return config; } -auto GeometricTrajectoryController::state_interface_configuration() const - -> controller_interface::InterfaceConfiguration +auto TrajectoryController::state_interface_configuration() const -> controller_interface::InterfaceConfiguration { controller_interface::InterfaceConfiguration config; if (params_.use_external_measured_states || params_.lookup_transform) { @@ -277,7 +275,7 @@ auto GeometricTrajectoryController::state_interface_configuration() const return config; } -auto GeometricTrajectoryController::update_system_state() -> controller_interface::return_type +auto TrajectoryController::update_system_state() -> controller_interface::return_type { auto * system_state = state_.readFromRT(); if (params_.lookup_transform) { @@ -315,7 +313,7 @@ auto GeometricTrajectoryController::update_system_state() -> controller_interfac return controller_interface::return_type::OK; } -auto GeometricTrajectoryController::update(const rclcpp::Time & time, const rclcpp::Duration & period) +auto TrajectoryController::update(const rclcpp::Time & time, const rclcpp::Duration & period) -> controller_interface::return_type { if (update_system_state() != controller_interface::return_type::OK) { @@ -457,9 +455,7 @@ auto GeometricTrajectoryController::update(const rclcpp::Time & time, const rclc return controller_interface::return_type::OK; } -} // namespace geometric_controllers +} // namespace trajectory_controller #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS( - geometric_trajectory_controller::GeometricTrajectoryController, - controller_interface::ControllerInterface) +PLUGINLIB_EXPORT_CLASS(trajectory_controller::TrajectoryController, controller_interface::ControllerInterface) diff --git a/geometric_controllers/src/trajectory_controller_parameters.yaml b/trajectory_controller/src/trajectory_controller_parameters.yaml similarity index 98% rename from geometric_controllers/src/trajectory_controller_parameters.yaml rename to trajectory_controller/src/trajectory_controller_parameters.yaml index ef43994..cbf77a0 100644 --- a/geometric_controllers/src/trajectory_controller_parameters.yaml +++ b/trajectory_controller/src/trajectory_controller_parameters.yaml @@ -1,4 +1,4 @@ -geometric_trajectory_controller: +trajectory_controller: joints: type: string_array default_value: ["x", "y", "z", "qx", "qy", "qz", "qw"] diff --git a/geometric_controllers/geometric_controllers.xml b/trajectory_controller/trajectory_controller.xml similarity index 57% rename from geometric_controllers/geometric_controllers.xml rename to trajectory_controller/trajectory_controller.xml index 156eaf2..1105bcd 100644 --- a/geometric_controllers/geometric_controllers.xml +++ b/trajectory_controller/trajectory_controller.xml @@ -1,6 +1,6 @@ - - + Trajectory controller for AUVs and UVMS that uses geometric methods to compute the desired trajectory. diff --git a/velocity_controllers/src/adaptive_integral_terminal_sliding_mode_controller.cpp b/velocity_controllers/src/adaptive_integral_terminal_sliding_mode_controller.cpp index eae46e7..4161aeb 100644 --- a/velocity_controllers/src/adaptive_integral_terminal_sliding_mode_controller.cpp +++ b/velocity_controllers/src/adaptive_integral_terminal_sliding_mode_controller.cpp @@ -323,7 +323,7 @@ auto AdaptiveIntegralTerminalSlidingModeController::update_and_write_commands( for (auto && [command_interface, tau] : std::views::zip(command_interfaces_, t)) { if (!command_interface.set_value(tau)) { RCLCPP_WARN(logger_, "Failed to set command for joint %s", command_interface.get_name().c_str()); // NOLINT - }; + } } // update the adaptive gain From f5cff09a014de2665955eb50812fdf26638a8536 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Fri, 1 Aug 2025 14:14:49 -0700 Subject: [PATCH 04/19] compile and cleanup --- auv_control_demos/CHANGELOG.md | 2 + auv_control_demos/package.xml | 2 +- auv_control_msgs/CHANGELOG.md | 19 +- auv_control_msgs/CMakeLists.txt | 8 +- ...ction => FollowCartesianTrajectory.action} | 2 +- ...Trajectory.msg => CartesianTrajectory.msg} | 2 +- ...esianTrajectoryControllerStateStamped.msg} | 0 ...Point.msg => CartesianTrajectoryPoint.msg} | 0 auv_control_msgs/package.xml | 2 +- auv_controllers/CHANGELOG.md | 6 + auv_controllers/package.xml | 2 +- controller_common/CHANGELOG.md | 4 + controller_common/CMakeLists.txt | 9 +- controller_common/package.xml | 3 +- controller_coordinator/CHANGELOG.md | 2 + controller_coordinator/package.xml | 2 +- ik_solvers/CHANGELOG.md | 2 + ik_solvers/package.xml | 2 +- impedance_controller/CHANGELOG.md | 5 + impedance_controller/CMakeLists.txt | 105 +++- impedance_controller/README.md | 43 ++ impedance_controller/impedance_controller.xml | 11 + .../impedance_controller.hpp | 2 +- impedance_controller/package.xml | 29 +- .../src/impedance_controller.cpp | 41 +- .../CHANGELOG.md | 2 + .../package.xml | 2 +- thruster_controllers/CHANGELOG.md | 2 + thruster_controllers/package.xml | 2 +- topic_sensors/CHANGELOG.md | 2 + topic_sensors/package.xml | 2 +- trajectory_controllers/CHANGELOG.md | 22 + trajectory_controllers/CMakeLists.txt | 94 ++++ trajectory_controllers/LICENSE | 17 + trajectory_controllers/README.md | 34 ++ .../cartesian_trajectory.hpp | 80 +++ .../cartesian_trajectory_controller.hpp | 140 ++++++ trajectory_controllers/package.xml | 37 ++ .../src/cartesian_trajectory.cpp | 169 +++++++ .../src/cartesian_trajectory_controller.cpp | 472 ++++++++++++++++++ ...sian_trajectory_controller_parameters.yaml | 93 ++++ .../trajectory_controllers.xml | 9 + velocity_controllers/CHANGELOG.md | 2 + velocity_controllers/package.xml | 2 +- whole_body_controllers/CHANGELOG.md | 2 + whole_body_controllers/package.xml | 2 +- 46 files changed, 1431 insertions(+), 61 deletions(-) rename auv_control_msgs/action/{FollowTrajectory.action => FollowCartesianTrajectory.action} (96%) rename auv_control_msgs/msg/{Trajectory.msg => CartesianTrajectory.msg} (58%) rename auv_control_msgs/msg/{TrajectoryControllerStateStamped.msg => CartesianTrajectoryControllerStateStamped.msg} (100%) rename auv_control_msgs/msg/{TrajectoryPoint.msg => CartesianTrajectoryPoint.msg} (100%) create mode 100644 impedance_controller/CHANGELOG.md create mode 100644 impedance_controller/README.md create mode 100644 impedance_controller/impedance_controller.xml create mode 100644 trajectory_controllers/CHANGELOG.md create mode 100644 trajectory_controllers/CMakeLists.txt create mode 100644 trajectory_controllers/LICENSE create mode 100644 trajectory_controllers/README.md create mode 100644 trajectory_controllers/include/trajectory_controllers/cartesian_trajectory.hpp create mode 100644 trajectory_controllers/include/trajectory_controllers/cartesian_trajectory_controller.hpp create mode 100644 trajectory_controllers/package.xml create mode 100644 trajectory_controllers/src/cartesian_trajectory.cpp create mode 100644 trajectory_controllers/src/cartesian_trajectory_controller.cpp create mode 100644 trajectory_controllers/src/cartesian_trajectory_controller_parameters.yaml create mode 100644 trajectory_controllers/trajectory_controllers.xml diff --git a/auv_control_demos/CHANGELOG.md b/auv_control_demos/CHANGELOG.md index 414eb18..9aaf83a 100644 --- a/auv_control_demos/CHANGELOG.md +++ b/auv_control_demos/CHANGELOG.md @@ -1,5 +1,7 @@ # Changelog for package auv_control_demos +## 0.4.0 (2025-08-01) + ## 0.3.3 (2025-07-29) ## 0.3.2 (2025-07-22) diff --git a/auv_control_demos/package.xml b/auv_control_demos/package.xml index 1ff1551..9483340 100644 --- a/auv_control_demos/package.xml +++ b/auv_control_demos/package.xml @@ -3,7 +3,7 @@ auv_control_demos - 0.3.3 + 0.4.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 90ee48d..3b15f7f 100644 --- a/auv_control_msgs/CHANGELOG.md +++ b/auv_control_msgs/CHANGELOG.md @@ -1,9 +1,14 @@ # Changelog for package auv_control_msgs -## 0.3.3 (2025-07-29) +## 0.4.0 (2025-08-01) + +- Renames the Trajectory message to CartesianTrajectory +- Renames the TrajectoryPoint message to CartesianTrajectoryPoint +- Renames the TrajectoryControllerState message to CartesianTrajectoryControllerStateStamped +- Renames the FollowTrajectory action to FollowCartesianTrajectory +- Implements the ImpedanceCommand message -- Re-names the EndEffector- messages to Geometric- to match changes to the - end effectory trajectory controller. +## 0.3.3 (2025-07-29) ## 0.3.2 (2025-07-22) @@ -15,10 +20,10 @@ ## 0.2.0 (2025-05-03) -- Implements the GeometricTrajectory message -- Implements the GeometricTrajectoryPoint message -- Implements the GeometricTrajectoryControllerState message -- Adds the FollowGeometricTrajectory action +- Implements the Trajectory message +- Implements the TrajectoryPoint message +- Implements the TrajectoryControllerState message +- Adds the FollowTrajectory action ## 0.1.0 (2025-04-27) diff --git a/auv_control_msgs/CMakeLists.txt b/auv_control_msgs/CMakeLists.txt index 34a29f2..78e50aa 100644 --- a/auv_control_msgs/CMakeLists.txt +++ b/auv_control_msgs/CMakeLists.txt @@ -11,11 +11,11 @@ find_package(trajectory_msgs REQUIRED) rosidl_generate_interfaces(auv_control_msgs "msg/MultiActuatorStateStamped.msg" "msg/IKControllerStateStamped.msg" - "msg/TrajectoryPoint.msg" - "msg/Trajectory.msg" - "msg/TrajectoryControllerStateStamped.msg" + "msg/CartesianTrajectoryPoint.msg" + "msg/CartesianTrajectory.msg" + "msg/CartesianTrajectoryControllerStateStamped.msg" "msg/ImpedanceCommand.msg" - "action/FollowTrajectory.action" + "action/FollowCartesianTrajectory.action" DEPENDENCIES builtin_interfaces std_msgs geometry_msgs trajectory_msgs ) diff --git a/auv_control_msgs/action/FollowTrajectory.action b/auv_control_msgs/action/FollowCartesianTrajectory.action similarity index 96% rename from auv_control_msgs/action/FollowTrajectory.action rename to auv_control_msgs/action/FollowCartesianTrajectory.action index afd37e6..e0a7c1b 100644 --- a/auv_control_msgs/action/FollowTrajectory.action +++ b/auv_control_msgs/action/FollowCartesianTrajectory.action @@ -3,7 +3,7 @@ # 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/Trajectory trajectory +auv_control_msgs/CartesianTrajectory 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. diff --git a/auv_control_msgs/msg/Trajectory.msg b/auv_control_msgs/msg/CartesianTrajectory.msg similarity index 58% rename from auv_control_msgs/msg/Trajectory.msg rename to auv_control_msgs/msg/CartesianTrajectory.msg index 943ea7d..21c6688 100644 --- a/auv_control_msgs/msg/Trajectory.msg +++ b/auv_control_msgs/msg/CartesianTrajectory.msg @@ -1,4 +1,4 @@ std_msgs/Header header # The sequence of end effector points to track. -auv_control_msgs/TrajectoryPoint[] points +auv_control_msgs/CartesianTrajectoryPoint[] points diff --git a/auv_control_msgs/msg/TrajectoryControllerStateStamped.msg b/auv_control_msgs/msg/CartesianTrajectoryControllerStateStamped.msg similarity index 100% rename from auv_control_msgs/msg/TrajectoryControllerStateStamped.msg rename to auv_control_msgs/msg/CartesianTrajectoryControllerStateStamped.msg diff --git a/auv_control_msgs/msg/TrajectoryPoint.msg b/auv_control_msgs/msg/CartesianTrajectoryPoint.msg similarity index 100% rename from auv_control_msgs/msg/TrajectoryPoint.msg rename to auv_control_msgs/msg/CartesianTrajectoryPoint.msg diff --git a/auv_control_msgs/package.xml b/auv_control_msgs/package.xml index 5f3f9c5..0619730 100644 --- a/auv_control_msgs/package.xml +++ b/auv_control_msgs/package.xml @@ -3,7 +3,7 @@ auv_control_msgs - 0.3.3 + 0.4.0 Custom messages for AUV controllers Rakesh Vivekanandan diff --git a/auv_controllers/CHANGELOG.md b/auv_controllers/CHANGELOG.md index df22f0a..0283bb5 100644 --- a/auv_controllers/CHANGELOG.md +++ b/auv_controllers/CHANGELOG.md @@ -1,5 +1,11 @@ # Changelog for package auv_controllers +## 0.4.0 (2025-08-01) + +- Implements the impedance controller +- Refactors the `end_effector_trajectory_controller` to be `cartesian_trajectory_controller` +- Implements the trajectory controllers package + ## 0.3.3 (2025-07-29) ## 0.3.2 (2025-07-22) diff --git a/auv_controllers/package.xml b/auv_controllers/package.xml index f63b720..7e6ce66 100644 --- a/auv_controllers/package.xml +++ b/auv_controllers/package.xml @@ -3,7 +3,7 @@ auv_controllers - 0.3.3 + 0.4.0 Meta package for auv_controllers Evan Palmer diff --git a/controller_common/CHANGELOG.md b/controller_common/CHANGELOG.md index da37424..72fa953 100644 --- a/controller_common/CHANGELOG.md +++ b/controller_common/CHANGELOG.md @@ -1,5 +1,9 @@ # Changelog for package controller_common +## 0.4.0 (2025-08-01) + +- Adds API support for the ImpedanceCommand message type + ## 0.3.3 (2025-07-29) ## 0.3.2 (2025-07-22) diff --git a/controller_common/CMakeLists.txt b/controller_common/CMakeLists.txt index 78abd3d..8d79054 100644 --- a/controller_common/CMakeLists.txt +++ b/controller_common/CMakeLists.txt @@ -11,6 +11,7 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav_msgs REQUIRED) +find_package(auv_control_msgs REQUIRED) add_library(controller_common SHARED) target_sources( @@ -25,7 +26,11 @@ target_sources( target_compile_features(controller_common PUBLIC cxx_std_23) target_link_libraries( controller_common - PUBLIC rclcpp::rclcpp ${geometry_msgs_TARGETS} ${nav_msgs_TARGETS} + PUBLIC + rclcpp::rclcpp + ${geometry_msgs_TARGETS} + ${nav_msgs_TARGETS} + ${auv_control_msgs_TARGETS} ) install( @@ -38,6 +43,6 @@ install( ) ament_export_targets(export_controller_common HAS_LIBRARY_TARGET) -ament_export_dependencies("rclcpp" "geometry_msgs" "nav_msgs") +ament_export_dependencies("rclcpp" "geometry_msgs" "nav_msgs" "auv_control_msgs") ament_package() diff --git a/controller_common/package.xml b/controller_common/package.xml index a295412..226c5b4 100644 --- a/controller_common/package.xml +++ b/controller_common/package.xml @@ -3,7 +3,7 @@ controller_common - 0.3.3 + 0.4.0 Common interfaces for controllers used in this project Evan Palmer @@ -19,6 +19,7 @@ rclcpp geometry_msgs nav_msgs + auv_control_msgs ament_cmake diff --git a/controller_coordinator/CHANGELOG.md b/controller_coordinator/CHANGELOG.md index 18e641d..7bc49e3 100644 --- a/controller_coordinator/CHANGELOG.md +++ b/controller_coordinator/CHANGELOG.md @@ -1,5 +1,7 @@ # Changelog for package controller_coordinator +## 0.4.0 (2025-08-01) + ## 0.3.3 (2025-07-29) - Enabled activating and deactivating multiple hardware interfaces. diff --git a/controller_coordinator/package.xml b/controller_coordinator/package.xml index badcffe..b67d095 100644 --- a/controller_coordinator/package.xml +++ b/controller_coordinator/package.xml @@ -3,7 +3,7 @@ controller_coordinator - 0.3.3 + 0.4.0 A high-level node used to load and activate/deactivate control systems Evan Palmer diff --git a/ik_solvers/CHANGELOG.md b/ik_solvers/CHANGELOG.md index d5a8501..836b9ce 100644 --- a/ik_solvers/CHANGELOG.md +++ b/ik_solvers/CHANGELOG.md @@ -1,5 +1,7 @@ # Changelog for package ik_solvers +## 0.4.0 (2025-08-01) + ## 0.3.3 (2025-07-29) ## 0.3.2 (2025-07-22) diff --git a/ik_solvers/package.xml b/ik_solvers/package.xml index 6a82be2..d4d5bd5 100644 --- a/ik_solvers/package.xml +++ b/ik_solvers/package.xml @@ -3,7 +3,7 @@ ik_solvers - 0.3.3 + 0.4.0 Inverse kinematics solvers used for whole-body control Evan Palmer diff --git a/impedance_controller/CHANGELOG.md b/impedance_controller/CHANGELOG.md new file mode 100644 index 0000000..6ec60f5 --- /dev/null +++ b/impedance_controller/CHANGELOG.md @@ -0,0 +1,5 @@ +# Changelog for package impedance_controller + +## 0.4.0 (2025-08-01) + +- Implements an impedance controller for underwater vehicle control diff --git a/impedance_controller/CMakeLists.txt b/impedance_controller/CMakeLists.txt index 551c48b..3121ae3 100644 --- a/impedance_controller/CMakeLists.txt +++ b/impedance_controller/CMakeLists.txt @@ -1,26 +1,93 @@ -cmake_minimum_required(VERSION 3.8) +cmake_minimum_required(VERSION 3.23) project(impedance_controller) -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Wpedantic) endif() -# find dependencies +include(GNUInstallDirs) + +find_package(controller_interface REQUIRED) +find_package(hardware_interface REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(realtime_tools REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(control_msgs REQUIRED) +find_package(tf2_eigen REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(controller_common REQUIRED) +find_package(hydrodynamics REQUIRED) find_package(ament_cmake REQUIRED) -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() +find_package(generate_parameter_library REQUIRED) +find_package(pluginlib REQUIRED) + +generate_parameter_library(impedance_controller_parameters + src/impedance_controller_parameters.yaml +) + +add_library(impedance_controller SHARED) + +target_sources( + impedance_controller + PRIVATE src/impedance_controller.cpp + PUBLIC + FILE_SET HEADERS + BASE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include + FILES + ${CMAKE_CURRENT_SOURCE_DIR}/include/impedance_controller/impedance_controller.hpp +) +target_compile_features(impedance_controller PUBLIC cxx_std_23) +target_link_libraries( + impedance_controller + PRIVATE + impedance_controller_parameters + controller_interface::controller_interface + hardware_interface::hardware_interface + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + realtime_tools::realtime_tools + Eigen3::Eigen + tf2_eigen::tf2_eigen + tf2::tf2 + tf2_ros::tf2_ros + controller_common::controller_common + hydrodynamics::hydrodynamics + ${geometry_msgs_TARGETS} + ${control_msgs_TARGETS} + ${nav_msgs_TARGETS} +) + +pluginlib_export_plugin_description_file(controller_interface impedance_controller.xml) + +install( + TARGETS impedance_controller impedance_controller_parameters + EXPORT export_impedance_controller + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + FILE_SET HEADERS +) + +ament_export_targets(export_impedance_controller HAS_LIBRARY_TARGET) +ament_export_dependencies( + "controller_interface" + "hardware_interface" + "rclcpp" + "rclcpp_lifecycle" + "realtime_tools" + "Eigen3" + "geometry_msgs" + "control_msgs" + "tf2_eigen" + "tf2" + "tf2_ros" + "nav_msgs" + "controller_common" + "hydrodynamics" +) ament_package() diff --git a/impedance_controller/README.md b/impedance_controller/README.md new file mode 100644 index 0000000..449d188 --- /dev/null +++ b/impedance_controller/README.md @@ -0,0 +1,43 @@ +# Impedance Controller + +A chainable impedance controller. Given an AUV pose $g$ with velocity $\nu$, +the implemented control law is given as follows + +```math +\tau = \tau_{\text{ref}} + \textbf{K}_\text{p}(g_\text{ref}^\top g)^{\vee} + \textbf{K}_\text{d}(\nu_\text{ref} - \nu) +``` + +where $\textbf{K}_\text{p}$ is the desired stiffness, and $\textbf{K}_\text{d}$ is the desired damping. + +This control law is commonly used as an inner-loop controller in an MPC +framework. [^1] [^2] + +[^1]: I. Dadiotis, A. Laurenzi, and N. Tsagarakis. "Whole-body MPC for highly redundant legged manipulators: experimental evaluation with a 37 DoF dual-arm quadruped," in *IEEE International Conference on Humanoid Robots (Humanoids)*, 2023. +[^2]: J. -P. Sleiman, F. Farshidian, M. V. Minniti and M. Hutter, "A Unified MPC Framework for Whole-Body Dynamic Locomotion and Manipulation," in *IEEE Robotics and Automation Letters*, vol. 6, no. 3, pp. 4688-4695, July 2021. + +## Plugin Library + +impedance_controller/ImpedanceController + +## References + +* Target pose $g_\text{ref}$ +* Target velocity $\nu_\text{ref}$ +* Target force/torque $\tau_{\text{ref}}$ + +## State Feedback + +* Measured pose $g$ +* Measured velocity $\nu$ + +## Commands + +The output of this controller are the desired forces/torques $\tau$ + +## Subscribers + +* impedance_controller/reference [auv_control_msgs::msg::ImpedanceCommand] + +## Publishers + +* impedance_controller/status [control_msgs::msg::MultiDOFStateStamped] diff --git a/impedance_controller/impedance_controller.xml b/impedance_controller/impedance_controller.xml new file mode 100644 index 0000000..888c581 --- /dev/null +++ b/impedance_controller/impedance_controller.xml @@ -0,0 +1,11 @@ + + + + + Impedance controller used to apply a reference pose, velocity, and feedforward force to a robot. + + + + diff --git a/impedance_controller/include/impedance_controller/impedance_controller.hpp b/impedance_controller/include/impedance_controller/impedance_controller.hpp index 7c8c14e..dd5407b 100644 --- a/impedance_controller/include/impedance_controller/impedance_controller.hpp +++ b/impedance_controller/include/impedance_controller/impedance_controller.hpp @@ -95,7 +95,7 @@ class ImpedanceController : public controller_interface::ChainableControllerInte // the dofs are passed as a parameter to the controller // we could make these static, but there may be scenarios where users want to rename the interfaces std::vector command_dofs_, state_dofs_; - std::size_t n_command_dofs_, n_state_dofs_; + std::size_t n_command_dofs_, n_state_dofs_, n_reference_dofs_; // controller gains Eigen::Matrix6d kp_, kd_; diff --git a/impedance_controller/package.xml b/impedance_controller/package.xml index 10c8308..54719ad 100644 --- a/impedance_controller/package.xml +++ b/impedance_controller/package.xml @@ -1,17 +1,42 @@ + impedance_controller - 0.0.0 - TODO: Package description + 0.4.0 + An impedance controller for underwater vehicles + 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 + pluginlib + controller_interface + hardware_interface + rclcpp_lifecycle + generate_parameter_library + control_msgs + tf2_eigen + geometry_msgs + nav_msgs + controller_common + hydrodynamics ament_lint_auto ament_lint_common + eigen3_cmake_module + ament_cmake diff --git a/impedance_controller/src/impedance_controller.cpp b/impedance_controller/src/impedance_controller.cpp index 46c43af..0694f07 100644 --- a/impedance_controller/src/impedance_controller.cpp +++ b/impedance_controller/src/impedance_controller.cpp @@ -20,6 +20,9 @@ #include "impedance_controller/impedance_controller.hpp" +#include +#include + #include "controller_common/common.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "tf2_eigen/tf2_eigen.hpp" @@ -30,12 +33,18 @@ namespace impedance_controller namespace { +auto vee(const Eigen::Matrix4d & mat) -> Eigen::Vector6d +{ + return {mat(0, 3), mat(1, 3), mat(2, 3), mat(2, 1), mat(0, 2), mat(1, 0)}; +} + auto geodesic_error(const geometry_msgs::msg::Pose & goal, const geometry_msgs::msg::Pose & state) -> Eigen::Vector6d { Eigen::Isometry3d goal_mat, state_mat; // NOLINT tf2::fromMsg(goal, goal_mat); tf2::fromMsg(state, state_mat); - return (goal_mat.inverse() * state_mat).matrix().log(); + const Eigen::Matrix4d error = (goal_mat.inverse() * state_mat).matrix().log(); + return vee(error); } } // namespace @@ -66,6 +75,7 @@ auto ImpedanceController::configure_parameters() -> controller_interface::Callba state_dofs_ = params_.state_joints; n_state_dofs_ = state_dofs_.size(); + n_reference_dofs_ = n_command_dofs_ + n_state_dofs_; auto get_gains = [this](auto field) { auto gains = command_dofs_ | @@ -84,7 +94,7 @@ auto ImpedanceController::configure_parameters() -> controller_interface::Callba return controller_interface::CallbackReturn::SUCCESS; } -auto ImpedanceController::on_configure(const rclcpp_lifecycle::State & previous_state) +auto ImpedanceController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) -> controller_interface::CallbackReturn { configure_parameters(); @@ -94,6 +104,7 @@ auto ImpedanceController::on_configure(const rclcpp_lifecycle::State & previous_ command_interfaces_.reserve(n_command_dofs_); state_interfaces_.reserve(n_state_dofs_); + system_state_values_.resize(n_state_dofs_, std::numeric_limits::quiet_NaN()); if (params_.use_external_measured_states) { @@ -118,7 +129,7 @@ auto ImpedanceController::on_configure(const rclcpp_lifecycle::State & previous_ return controller_interface::CallbackReturn::SUCCESS; } -auto ImpedanceController::on_activate(const rclcpp_lifecycle::State & previous_state) +auto ImpedanceController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) -> controller_interface::CallbackReturn { common::messages::reset_message(reference_.readFromNonRT()); @@ -154,10 +165,11 @@ auto ImpedanceController::state_interface_configuration() const -> controller_in } else { config.type = controller_interface::interface_configuration_type::INDIVIDUAL; config.names.reserve(n_state_dofs_); - for (std::size_t i = 0; i < 7; ++i) { + const std::size_t n_pose_dofs = 7; + for (std::size_t i = 0; i < n_pose_dofs; ++i) { config.names.emplace_back(std::format("{}/{}", state_dofs_[i], hardware_interface::HW_IF_POSITION)); } - for (std::size_t i = 7; i < n_state_dofs_; ++i) { + for (std::size_t i = n_pose_dofs; i < n_state_dofs_; ++i) { config.names.emplace_back(std::format("{}/{}", state_dofs_[i], hardware_interface::HW_IF_VELOCITY)); } } @@ -167,17 +179,19 @@ auto ImpedanceController::state_interface_configuration() const -> controller_in auto ImpedanceController::on_export_reference_interfaces() -> std::vector { // the reference command includes the desired pose, velocity, and force/torque - reference_interfaces_.resize(n_command_dofs_ + n_state_dofs_, std::numeric_limits::quiet_NaN()); + reference_interfaces_.resize(n_reference_dofs_, std::numeric_limits::quiet_NaN()); std::vector interfaces; interfaces.reserve(reference_interfaces_.size()); // add the pose & velocity interfaces + // this uses the same position/velocity joint names as the state interfaces for (const auto [i, dof] : std::views::enumerate(state_dofs_)) { interfaces.emplace_back( get_node()->get_name(), std::format("{}/{}", dof, hardware_interface::HW_IF_POSITION), &reference_interfaces_[i]); } // add the force/torque interfaces + // this uses the same effort joint names as the command interfaces for (const auto [i, dof] : std::views::enumerate(command_dofs_)) { interfaces.emplace_back( get_node()->get_name(), @@ -188,8 +202,9 @@ auto ImpedanceController::on_export_reference_interfaces() -> std::vector controller_interface::return_type +auto ImpedanceController::update_reference_from_subscribers( + const rclcpp::Time & /*time*/, + const rclcpp::Duration & /*period*/) -> controller_interface::return_type { auto * current_reference = reference_.readFromNonRT(); const std::vector reference = common::messages::to_vector(*current_reference); @@ -205,8 +220,8 @@ auto ImpedanceController::update_reference_from_subscribers(const rclcpp::Time & auto ImpedanceController::update_system_state_values() -> controller_interface::return_type { if (params_.use_external_measured_states) { - auto * current_state = system_state_.readFromNonRT(); - std::ranges::copy(common::messages::to_vector(current_state), system_state_values_.begin()); + auto * current_state = system_state_.readFromRT(); + std::ranges::copy(common::messages::to_vector(*current_state), system_state_values_.begin()); } else { std::ranges::transform(state_interfaces_, system_state_values_.begin(), [](const auto & interface) { return interface.get_optional().value_or(std::numeric_limits::quiet_NaN()); @@ -247,7 +262,7 @@ auto ImpedanceController::update_and_write_commands(const rclcpp::Time & time, c // get the reference pose, twist, and wrench values const auto pose_end = reference_interfaces_.begin() + 7; const auto twist_end = pose_end + 6; - const int wrench_end = twist_end + 6; + const auto wrench_end = twist_end + 6; const std::vector ref_pose_values(reference_interfaces_.begin(), pose_end); const std::vector ref_twist_values(pose_end, twist_end); @@ -282,7 +297,7 @@ auto ImpedanceController::update_and_write_commands(const rclcpp::Time & time, c return controller_interface::return_type::OK; } - // convert the reference wrench values to an Eigen vector + // convert the reference wrench values into an Eigen vector Eigen::Vector6d reference_wrench(ref_wrench_values.data()); // calculate the control command @@ -307,7 +322,7 @@ auto ImpedanceController::update_and_write_commands(const rclcpp::Time & time, c for (std::size_t i = 0; i < n_state_dofs_; ++i) { controller_state_.dof_states[i].feedback = system_state_values_[i]; } - for (std::size_t i = n_state_dofs_; i < n_command_dofs_ + n_state_dofs_; ++i) { + for (std::size_t i = n_state_dofs_; i < n_reference_dofs_; ++i) { controller_state_.dof_states[i].reference = reference_interfaces_[i]; } diff --git a/thruster_allocation_matrix_controller/CHANGELOG.md b/thruster_allocation_matrix_controller/CHANGELOG.md index 0251b25..ae15e7b 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.4.0 (2025-08-01) + ## 0.3.3 (2025-07-29) ## 0.3.2 (2025-07-22) diff --git a/thruster_allocation_matrix_controller/package.xml b/thruster_allocation_matrix_controller/package.xml index 3606543..fce0916 100644 --- a/thruster_allocation_matrix_controller/package.xml +++ b/thruster_allocation_matrix_controller/package.xml @@ -3,7 +3,7 @@ thruster_allocation_matrix_controller - 0.3.3 + 0.4.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 59054f3..eac85e4 100644 --- a/thruster_controllers/CHANGELOG.md +++ b/thruster_controllers/CHANGELOG.md @@ -1,5 +1,7 @@ # Changelog for package thruster_controllers +## 0.4.0 (2025-08-01) + ## 0.3.3 (2025-07-29) ## 0.3.2 (2025-07-22) diff --git a/thruster_controllers/package.xml b/thruster_controllers/package.xml index 02ea338..32ec4fd 100644 --- a/thruster_controllers/package.xml +++ b/thruster_controllers/package.xml @@ -3,7 +3,7 @@ thruster_controllers - 0.3.3 + 0.4.0 A collection of thruster controllers for AUV control Evan Palmer diff --git a/topic_sensors/CHANGELOG.md b/topic_sensors/CHANGELOG.md index 45389b3..60ab515 100644 --- a/topic_sensors/CHANGELOG.md +++ b/topic_sensors/CHANGELOG.md @@ -1,5 +1,7 @@ # Changelog for package topic_sensors +## 0.4.0 (2025-08-01) + ## 0.3.3 (2025-07-29) ## 0.3.2 (2025-07-22) diff --git a/topic_sensors/package.xml b/topic_sensors/package.xml index e60ea6e..324fc6d 100644 --- a/topic_sensors/package.xml +++ b/topic_sensors/package.xml @@ -3,7 +3,7 @@ topic_sensors - 0.3.3 + 0.4.0 Sensor plugins used to write ROS 2 messages to state interfaces Evan Palmer diff --git a/trajectory_controllers/CHANGELOG.md b/trajectory_controllers/CHANGELOG.md new file mode 100644 index 0000000..0ab31a7 --- /dev/null +++ b/trajectory_controllers/CHANGELOG.md @@ -0,0 +1,22 @@ +# Changelog for package trajectory_controllers + +## 0.4.0 (2025-08-01) + +- Renames the package to be trajectory_controllers +- Renames the end_effector_trajectory_controller to be cartesian_trajectory_controller + +## 0.3.3 (2025-07-29) + +## 0.3.2 (2025-07-22) + +- Replaces the deprecated `unlockAndPublish` API with `try_publish` + +## 0.3.1 (2025-07-09) + +## 0.3.0 (2025-06-07) + +## 0.2.1 (2025-06-03) + +## 0.2.0 (2025-05-03) + +- Implements the end_effector_trajectory_controller diff --git a/trajectory_controllers/CMakeLists.txt b/trajectory_controllers/CMakeLists.txt new file mode 100644 index 0000000..0207dc9 --- /dev/null +++ b/trajectory_controllers/CMakeLists.txt @@ -0,0 +1,94 @@ +cmake_minimum_required(VERSION 3.23) +project(trajectory_controllers) + +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(cartesian_trajectory_controller_parameters + src/cartesian_trajectory_controller_parameters.yaml +) + +add_library(trajectory_controllers SHARED) +target_sources( + trajectory_controllers + PRIVATE src/cartesian_trajectory_controller.cpp src/cartesian_trajectory.cpp + PUBLIC + FILE_SET HEADERS + BASE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include + FILES + ${CMAKE_CURRENT_SOURCE_DIR}/include/trajectory_controllers/cartesian_trajectory_controller.hpp + ${CMAKE_CURRENT_SOURCE_DIR}/include/trajectory_controllers/cartesian_trajectory.hpp +) +target_compile_features(trajectory_controllers PUBLIC cxx_std_23) +target_link_libraries( + trajectory_controllers + PUBLIC + cartesian_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 trajectory_controllers.xml) + +install( + TARGETS trajectory_controllers cartesian_trajectory_controller_parameters + EXPORT export_trajectory_controllers + LIBRARY DESTINATION lib/${PROJECT_NAME} + ARCHIVE DESTINATION lib/${PROJECT_NAME} + RUNTIME DESTINATION bin/${PROJECT_NAME} + FILE_SET HEADERS +) + +ament_export_targets(export_trajectory_controllers 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/trajectory_controllers/LICENSE b/trajectory_controllers/LICENSE new file mode 100644 index 0000000..30e8e2e --- /dev/null +++ b/trajectory_controllers/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/trajectory_controllers/README.md b/trajectory_controllers/README.md new file mode 100644 index 0000000..e0fd56f --- /dev/null +++ b/trajectory_controllers/README.md @@ -0,0 +1,34 @@ +# Trajectory Controllers + +This package provides a collection of trajectory controllers, which interpolate +a nominal trajectory for tracking by an inner-loop controller. + +## Cartesian Trajectory Controller + +A cartesian trajectory controller that interpolates SE(3) motion plans for +cartesian control. The positions are interpolated using linear interpolation, +and the orientations are interpolated using spherical linear interpolation. + +### Plugin Library + +trajectory_controller/TrajectoryController + +### References + +The input to this controller is a sequence of SE(3) poses. + +### Commands + +The output of this controller is a sampled pose. + +### Subscribers + +- cartesian_trajectory_controller/trajectory [auv_control_msgs::msg::CartesianTrajectory] + +### Action Servers + +- cartesian_trajectory_controller/follow_trajectory [auv_control_msgs::action::FollowCartesianTrajectory] + +### Publishers + +- cartesian_trajectory_controller/status [auv_control_msgs::msg::CartesianTrajectoryControllerState] diff --git a/trajectory_controllers/include/trajectory_controllers/cartesian_trajectory.hpp b/trajectory_controllers/include/trajectory_controllers/cartesian_trajectory.hpp new file mode 100644 index 0000000..8d874a5 --- /dev/null +++ b/trajectory_controllers/include/trajectory_controllers/cartesian_trajectory.hpp @@ -0,0 +1,80 @@ +// 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/cartesian_trajectory.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace trajectory_controllers +{ + +enum class SampleError : std::uint8_t +{ + SAMPLE_TIME_BEFORE_START, + SAMPLE_TIME_AFTER_END, + EMPTY_TRAJECTORY, +}; + +class CartesianTrajectory +{ +public: + CartesianTrajectory() = default; + + /// Create a new trajectory given a trajectory message and the initial state. + CartesianTrajectory( + const auv_control_msgs::msg::CartesianTrajectory & 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::CartesianTrajectory points_; + rclcpp::Time initial_time_; + geometry_msgs::msg::Pose initial_state_; +}; + +} // namespace trajectory_controllers diff --git a/trajectory_controllers/include/trajectory_controllers/cartesian_trajectory_controller.hpp b/trajectory_controllers/include/trajectory_controllers/cartesian_trajectory_controller.hpp new file mode 100644 index 0000000..c3283fc --- /dev/null +++ b/trajectory_controllers/include/trajectory_controllers/cartesian_trajectory_controller.hpp @@ -0,0 +1,140 @@ +// 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_cartesian_trajectory.hpp" +#include "auv_control_msgs/msg/cartesian_trajectory_controller_state_stamped.hpp" +#include "controller_common/common.hpp" +#include "controller_interface/controller_interface.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.hpp" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" +#include "trajectory_controllers/cartesian_trajectory.hpp" + +// auto-generated by generate_parameter_library +#include + +namespace trajectory_controllers +{ + +class CartesianTrajectoryController : public controller_interface::ControllerInterface +{ +public: + CartesianTrajectoryController() = 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_system_state() -> controller_interface::return_type; + + [[nodiscard]] auto validate_trajectory(const auv_control_msgs::msg::CartesianTrajectory & trajectory) const -> bool; + + // controller state + using ControllerState = auv_control_msgs::msg::CartesianTrajectoryControllerStateStamped; + std::shared_ptr> controller_state_pub_; + std::unique_ptr> rt_controller_state_pub_; + ControllerState controller_state_; + + // the state 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 state_; + std::shared_ptr> state_sub_; + + std::unique_ptr tf_buffer_; + std::unique_ptr tf_listener_; + + // the 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::FollowCartesianTrajectory; + 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 maintain the current pose + + // the update period is used to sample the "next" trajectory point + rclcpp::Duration update_period_{0, 0}; + + std::shared_ptr param_listener_; + cartesian_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("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 trajectory_controllers diff --git a/trajectory_controllers/package.xml b/trajectory_controllers/package.xml new file mode 100644 index 0000000..825df44 --- /dev/null +++ b/trajectory_controllers/package.xml @@ -0,0 +1,37 @@ + + + + + trajectory_controllers + 0.4.0 + Trajectory controllers for underwater vehicles and manipulator systems + + 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/trajectory_controllers/src/cartesian_trajectory.cpp b/trajectory_controllers/src/cartesian_trajectory.cpp new file mode 100644 index 0000000..552f5c4 --- /dev/null +++ b/trajectory_controllers/src/cartesian_trajectory.cpp @@ -0,0 +1,169 @@ +// 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 "trajectory_controllers/cartesian_trajectory.hpp" + +#include +#include + +#include "controller_common/common.hpp" +#include "tf2_eigen/tf2_eigen.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +namespace trajectory_controllers +{ + +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 + +CartesianTrajectory::CartesianTrajectory( + const auv_control_msgs::msg::CartesianTrajectory & trajectory, + const geometry_msgs::msg::Pose & state) +: points_(trajectory), + initial_time_(static_cast(trajectory.header.stamp)), + initial_state_(state) +{ +} + +auto CartesianTrajectory::empty() const -> bool { return points_.points.empty(); } + +auto CartesianTrajectory::start_time() const -> rclcpp::Time +{ + return empty() ? rclcpp::Time(0) : initial_time_ + points_.points.front().time_from_start; +} + +auto CartesianTrajectory::end_time() const -> rclcpp::Time +{ + return empty() ? rclcpp::Time(0) : initial_time_ + points_.points.back().time_from_start; +} + +auto CartesianTrajectory::start_point() const -> std::optional +{ + if (empty()) { + return std::nullopt; + } + return points_.points.front().point; +} + +auto CartesianTrajectory::end_point() const -> std::optional +{ + if (empty()) { + return std::nullopt; + } + return points_.points.back().point; +} + +auto CartesianTrajectory::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 CartesianTrajectory::reset_initial_state(const geometry_msgs::msg::Pose & state) -> void +{ + initial_state_ = state; +} + +} // namespace trajectory_controllers diff --git a/trajectory_controllers/src/cartesian_trajectory_controller.cpp b/trajectory_controllers/src/cartesian_trajectory_controller.cpp new file mode 100644 index 0000000..5ab13bc --- /dev/null +++ b/trajectory_controllers/src/cartesian_trajectory_controller.cpp @@ -0,0 +1,472 @@ +// 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 "trajectory_controllers/cartesian_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 trajectory_controllers +{ + +namespace +{ + +auto vee(const Eigen::Matrix4d & mat) -> Eigen::VectorXd +{ + Eigen::VectorXd result(6); + result << mat(0, 3), mat(1, 3), mat(2, 3), mat(2, 1), mat(0, 2), mat(1, 0); + return result; +} + +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); + const Eigen::Matrix4d error = (goal_mat.inverse() * state_mat).matrix().log(); + return std::pow(vee(error).norm(), 2); +} + +} // namespace + +auto CartesianTrajectoryController::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 CartesianTrajectoryController::update_parameters() -> void +{ + if (!param_listener_->is_old(params_)) { + return; + } + param_listener_->refresh_dynamic_parameters(); + params_ = param_listener_->get_params(); +} + +auto CartesianTrajectoryController::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 CartesianTrajectoryController::validate_trajectory( + const auv_control_msgs::msg::CartesianTrajectory & 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_, "CartesianTrajectory points are not in order"); // NOLINT + return false; + } + } + + return true; +} + +auto CartesianTrajectoryController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) + -> controller_interface::CallbackReturn +{ + configure_parameters(); + + 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) { + state_sub_ = get_node()->create_subscription( + "~/state", + rclcpp::SystemDefaultsQoS(), + [this](const std::shared_ptr msg) { // NOLINT + state_.writeFromNonRT(*msg); + }); + } else { + if (params_.lookup_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(CartesianTrajectory(updated_msg, *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(CartesianTrajectory(updated_msg, *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 CartesianTrajectoryController::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(state_.readFromNonRT()); + return controller_interface::CallbackReturn::SUCCESS; +} + +auto CartesianTrajectoryController::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 CartesianTrajectoryController::state_interface_configuration() const + -> controller_interface::InterfaceConfiguration +{ + controller_interface::InterfaceConfiguration config; + if (params_.use_external_measured_states || params_.lookup_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 CartesianTrajectoryController::update_system_state() -> controller_interface::return_type +{ + auto * system_state = state_.readFromRT(); + if (params_.lookup_transform) { + const auto to_frame = params_.child_frame_id; + const auto from_frame = params_.frame_id; + try { + const auto transform = tf_buffer_->lookupTransform(from_frame, to_frame, tf2::TimePointZero); + system_state->position.x = transform.transform.translation.x; + system_state->position.y = transform.transform.translation.y; + system_state->position.z = transform.transform.translation.z; + system_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()); + }; + system_state->position.x = get_value(state_interfaces_[0]); + system_state->position.y = get_value(state_interfaces_[1]); + system_state->position.z = get_value(state_interfaces_[2]); + system_state->orientation.x = get_value(state_interfaces_[3]); + system_state->orientation.y = get_value(state_interfaces_[4]); + system_state->orientation.z = get_value(state_interfaces_[5]); + system_state->orientation.w = get_value(state_interfaces_[6]); + } + + if (common::math::has_nan(common::messages::to_vector(*system_state))) { + RCLCPP_DEBUG(logger_, "Received state with NaN value."); // NOLINT + return controller_interface::return_type::ERROR; + } + return controller_interface::return_type::OK; +} + +auto CartesianTrajectoryController::update(const rclcpp::Time & time, const rclcpp::Duration & period) + -> controller_interface::return_type +{ + if (update_system_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 system_state = *state_.readFromRT(); + geometry_msgs::msg::Pose reference_state; + common::messages::reset_message(&reference_state); + auto command_state = system_state; + double error = std::numeric_limits::quiet_NaN(); + + auto publish_controller_state = [this, &reference_state, &system_state, &error, &command_state]() { + controller_state_.header.stamp = get_node()->now(); + controller_state_.reference = reference_state; + controller_state_.feedback = system_state; + controller_state_.error = error; + controller_state_.output = command_state; + rt_controller_state_pub_->try_publish(controller_state_); + }; + + // hold position until a new trajectory is received + if (*rt_holding_position_.readFromRT()) { + write_command(command_interfaces_, system_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(system_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, system_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 = system_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_, "CartesianTrajectory 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(), system_state); + RCLCPP_INFO(logger_, "CartesianTrajectory 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_, "CartesianTrajectory 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 = system_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 = "CartesianTrajectory 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 = "CartesianTrajectory 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 = "CartesianTrajectory 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 trajectory_controllers + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(trajectory_controllers::CartesianTrajectoryController, controller_interface::ControllerInterface) diff --git a/trajectory_controllers/src/cartesian_trajectory_controller_parameters.yaml b/trajectory_controllers/src/cartesian_trajectory_controller_parameters.yaml new file mode 100644 index 0000000..947aeaf --- /dev/null +++ b/trajectory_controllers/src/cartesian_trajectory_controller_parameters.yaml @@ -0,0 +1,93 @@ +cartesian_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 states provided via a topic instead of state interfaces or tf2. If + both this and lookup_transform are set to true, the controller will use + the states provided via a topic. + + lookup_transform: + type: bool + default_value: false + read_only: true + description: > + Use tf2 to look up the state transform instead of using state + interfaces or a topic. This is useful in scenarios where the pose + is determined only by using tf2 to perform forward kinematics. + + If both this and use_external_measured_states are set to true, the + controller will use the 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. + + frame_id: + type: string + default_value: odom_ned + read_only: true + description: > + The name of the parent frame. This is used to lookup the body pose when + using lookup_transform. + + child_frame_id: + type: string + default_value: base_link_frd + read_only: true + description: > + The name of the body frame. This is used to lookup the body pose when + using lookup_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/trajectory_controllers/trajectory_controllers.xml b/trajectory_controllers/trajectory_controllers.xml new file mode 100644 index 0000000..60c666e --- /dev/null +++ b/trajectory_controllers/trajectory_controllers.xml @@ -0,0 +1,9 @@ + + + + Trajectory controllers for AUVs and UVMS. + + + diff --git a/velocity_controllers/CHANGELOG.md b/velocity_controllers/CHANGELOG.md index 921d571..39a24cf 100644 --- a/velocity_controllers/CHANGELOG.md +++ b/velocity_controllers/CHANGELOG.md @@ -1,5 +1,7 @@ # Changelog for package velocity_controllers +## 0.4.0 (2025-08-01) + ## 0.3.3 (2025-07-29) ## 0.3.2 (2025-07-22) diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 06d39cc..1c9a06b 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -3,7 +3,7 @@ velocity_controllers - 0.3.3 + 0.4.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 9a2248d..a90b374 100644 --- a/whole_body_controllers/CHANGELOG.md +++ b/whole_body_controllers/CHANGELOG.md @@ -1,5 +1,7 @@ # Changelog for package whole_body_controllers +## 0.4.0 (2025-08-01) + ## 0.3.3 (2025-07-29) ## 0.3.2 (2025-07-22) diff --git a/whole_body_controllers/package.xml b/whole_body_controllers/package.xml index ad2a866..664d852 100644 --- a/whole_body_controllers/package.xml +++ b/whole_body_controllers/package.xml @@ -3,7 +3,7 @@ whole_body_controllers - 0.3.3 + 0.4.0 Whole-body controllers for underwater vehicle manipulator systems Evan Palmer From b9e6c4e79b80571c65b74125cbaebceab1035aa5 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Fri, 1 Aug 2025 14:17:15 -0700 Subject: [PATCH 05/19] testing --- impedance_controller/README.md | 2 +- trajectory_controller/CMakeLists.txt | 96 ---- trajectory_controller/LICENSE | 17 - trajectory_controller/README.md | 29 -- .../trajectory_controller/trajectory.hpp | 78 --- .../trajectory_controller.hpp | 140 ------ trajectory_controller/package.xml | 37 -- trajectory_controller/src/trajectory.cpp | 163 ------- .../src/trajectory_controller.cpp | 461 ------------------ .../src/trajectory_controller_parameters.yaml | 93 ---- .../trajectory_controller.xml | 9 - 11 files changed, 1 insertion(+), 1124 deletions(-) delete mode 100644 trajectory_controller/CMakeLists.txt delete mode 100644 trajectory_controller/LICENSE delete mode 100644 trajectory_controller/README.md delete mode 100644 trajectory_controller/include/trajectory_controller/trajectory.hpp delete mode 100644 trajectory_controller/include/trajectory_controller/trajectory_controller.hpp delete mode 100644 trajectory_controller/package.xml delete mode 100644 trajectory_controller/src/trajectory.cpp delete mode 100644 trajectory_controller/src/trajectory_controller.cpp delete mode 100644 trajectory_controller/src/trajectory_controller_parameters.yaml delete mode 100644 trajectory_controller/trajectory_controller.xml diff --git a/impedance_controller/README.md b/impedance_controller/README.md index 449d188..bf193a1 100644 --- a/impedance_controller/README.md +++ b/impedance_controller/README.md @@ -7,7 +7,7 @@ the implemented control law is given as follows \tau = \tau_{\text{ref}} + \textbf{K}_\text{p}(g_\text{ref}^\top g)^{\vee} + \textbf{K}_\text{d}(\nu_\text{ref} - \nu) ``` -where $\textbf{K}_\text{p}$ is the desired stiffness, and $\textbf{K}_\text{d}$ is the desired damping. +where $\textbf{K}_p$ is the desired stiffness, and $\textbf{K}_\text{d}$ is the desired damping. This control law is commonly used as an inner-loop controller in an MPC framework. [^1] [^2] diff --git a/trajectory_controller/CMakeLists.txt b/trajectory_controller/CMakeLists.txt deleted file mode 100644 index 24df18a..0000000 --- a/trajectory_controller/CMakeLists.txt +++ /dev/null @@ -1,96 +0,0 @@ -cmake_minimum_required(VERSION 3.23) -project(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(trajectory_controller_parameters - src/trajectory_controller_parameters.yaml -) - -add_library(trajectory_controller SHARED) -target_sources( - trajectory_controller - PRIVATE src/trajectory_controller.cpp src/trajectory.cpp - PUBLIC - FILE_SET HEADERS - BASE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - FILES - ${CMAKE_CURRENT_SOURCE_DIR}/include/trajectory_controller/trajectory_controller.hpp - ${CMAKE_CURRENT_SOURCE_DIR}/include/trajectory_controller/trajectory.hpp -) -target_compile_features(trajectory_controller PUBLIC cxx_std_23) -target_link_libraries( - trajectory_controller - PUBLIC - 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 trajectory_controller.xml) - -install( - TARGETS - trajectory_controller - trajectory_controller_parameters - EXPORT export_trajectory_controller - LIBRARY DESTINATION lib/${PROJECT_NAME} - ARCHIVE DESTINATION lib/${PROJECT_NAME} - RUNTIME DESTINATION bin/${PROJECT_NAME} - FILE_SET HEADERS -) - -ament_export_targets(export_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/trajectory_controller/LICENSE b/trajectory_controller/LICENSE deleted file mode 100644 index 30e8e2e..0000000 --- a/trajectory_controller/LICENSE +++ /dev/null @@ -1,17 +0,0 @@ -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/trajectory_controller/README.md b/trajectory_controller/README.md deleted file mode 100644 index 0bd7046..0000000 --- a/trajectory_controller/README.md +++ /dev/null @@ -1,29 +0,0 @@ -# Trajectory Controller - -The trajectory controller interpolates an SE(3) motion plan for pose control. -The positions are interpolated using linear interpolation, and the orientations -are interpolated using spherical linear interpolation. - -## Plugin Library - -trajectory_controller/TrajectoryController - -## References - -The input to this controller is a sequence of poses. - -## Commands - -The output of this controller is a sampled pose. - -## Subscribers - -- trajectory_controller/trajectory [auv_control_msgs::msg::Trajectory] - -## Action Servers - -- trajectory_controller/follow_trajectory [auv_control_msgs::action::FollowTrajectory] - -## Publishers - -- trajectory_controller/status [auv_control_msgs::msg::TrajectoryControllerState] diff --git a/trajectory_controller/include/trajectory_controller/trajectory.hpp b/trajectory_controller/include/trajectory_controller/trajectory.hpp deleted file mode 100644 index 572adee..0000000 --- a/trajectory_controller/include/trajectory_controller/trajectory.hpp +++ /dev/null @@ -1,78 +0,0 @@ -// 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/trajectory.hpp" -#include "geometry_msgs/msg/pose.hpp" -#include "rclcpp/rclcpp.hpp" - -namespace 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::Trajectory & 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::Trajectory points_; - rclcpp::Time initial_time_; - geometry_msgs::msg::Pose initial_state_; -}; - -} // namespace trajectory_controller diff --git a/trajectory_controller/include/trajectory_controller/trajectory_controller.hpp b/trajectory_controller/include/trajectory_controller/trajectory_controller.hpp deleted file mode 100644 index f8762b3..0000000 --- a/trajectory_controller/include/trajectory_controller/trajectory_controller.hpp +++ /dev/null @@ -1,140 +0,0 @@ -// 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_trajectory.hpp" -#include "auv_control_msgs/msg/trajectory_controller_state_stamped.hpp" -#include "controller_common/common.hpp" -#include "controller_interface/controller_interface.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.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" -#include "trajectory_controller/trajectory.hpp" - -// auto-generated by generate_parameter_library -#include - -namespace trajectory_controller -{ - -class TrajectoryController : public controller_interface::ControllerInterface -{ -public: - TrajectoryController() = 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_system_state() -> controller_interface::return_type; - - [[nodiscard]] auto validate_trajectory(const auv_control_msgs::msg::Trajectory & trajectory) const -> bool; - - // controller state - using ControllerState = auv_control_msgs::msg::TrajectoryControllerStateStamped; - std::shared_ptr> controller_state_pub_; - std::unique_ptr> rt_controller_state_pub_; - ControllerState controller_state_; - - // the state 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 state_; - std::shared_ptr> state_sub_; - - std::unique_ptr tf_buffer_; - std::unique_ptr tf_listener_; - - // the 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::FollowTrajectory; - 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 maintain the current pose - - // the update period is used to sample the "next" trajectory point - rclcpp::Duration update_period_{0, 0}; - - std::shared_ptr param_listener_; - 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("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 trajectory_controller diff --git a/trajectory_controller/package.xml b/trajectory_controller/package.xml deleted file mode 100644 index 7f7c0c6..0000000 --- a/trajectory_controller/package.xml +++ /dev/null @@ -1,37 +0,0 @@ - - - - - trajectory_controller - 0.3.3 - Trajectory controller for single-link rigid-body systems - - 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/trajectory_controller/src/trajectory.cpp b/trajectory_controller/src/trajectory.cpp deleted file mode 100644 index 75d95f6..0000000 --- a/trajectory_controller/src/trajectory.cpp +++ /dev/null @@ -1,163 +0,0 @@ -// 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 "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 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::Trajectory & 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 trajectory_controller diff --git a/trajectory_controller/src/trajectory_controller.cpp b/trajectory_controller/src/trajectory_controller.cpp deleted file mode 100644 index 3baa8c2..0000000 --- a/trajectory_controller/src/trajectory_controller.cpp +++ /dev/null @@ -1,461 +0,0 @@ -// 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 "trajectory_controller/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 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 TrajectoryController::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 TrajectoryController::update_parameters() -> void -{ - if (!param_listener_->is_old(params_)) { - return; - } - param_listener_->refresh_dynamic_parameters(); - params_ = param_listener_->get_params(); -} - -auto TrajectoryController::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 TrajectoryController::validate_trajectory(const auv_control_msgs::msg::Trajectory & 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 TrajectoryController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) - -> controller_interface::CallbackReturn -{ - configure_parameters(); - - 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) { - state_sub_ = get_node()->create_subscription( - "~/state", - rclcpp::SystemDefaultsQoS(), - [this](const std::shared_ptr msg) { // NOLINT - state_.writeFromNonRT(*msg); - }); - } else { - if (params_.lookup_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, *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, *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 TrajectoryController::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(state_.readFromNonRT()); - return controller_interface::CallbackReturn::SUCCESS; -} - -auto TrajectoryController::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 TrajectoryController::state_interface_configuration() const -> controller_interface::InterfaceConfiguration -{ - controller_interface::InterfaceConfiguration config; - if (params_.use_external_measured_states || params_.lookup_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 TrajectoryController::update_system_state() -> controller_interface::return_type -{ - auto * system_state = state_.readFromRT(); - if (params_.lookup_transform) { - const auto to_frame = params_.child_frame_id; - const auto from_frame = params_.frame_id; - try { - const auto transform = tf_buffer_->lookupTransform(from_frame, to_frame, tf2::TimePointZero); - system_state->position.x = transform.transform.translation.x; - system_state->position.y = transform.transform.translation.y; - system_state->position.z = transform.transform.translation.z; - system_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()); - }; - system_state->position.x = get_value(state_interfaces_[0]); - system_state->position.y = get_value(state_interfaces_[1]); - system_state->position.z = get_value(state_interfaces_[2]); - system_state->orientation.x = get_value(state_interfaces_[3]); - system_state->orientation.y = get_value(state_interfaces_[4]); - system_state->orientation.z = get_value(state_interfaces_[5]); - system_state->orientation.w = get_value(state_interfaces_[6]); - } - - if (common::math::has_nan(common::messages::to_vector(*system_state))) { - RCLCPP_DEBUG(logger_, "Received state with NaN value."); // NOLINT - return controller_interface::return_type::ERROR; - } - return controller_interface::return_type::OK; -} - -auto TrajectoryController::update(const rclcpp::Time & time, const rclcpp::Duration & period) - -> controller_interface::return_type -{ - if (update_system_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 system_state = *state_.readFromRT(); - geometry_msgs::msg::Pose reference_state; - common::messages::reset_message(&reference_state); - auto command_state = system_state; - double error = std::numeric_limits::quiet_NaN(); - - auto publish_controller_state = [this, &reference_state, &system_state, &error, &command_state]() { - controller_state_.header.stamp = get_node()->now(); - controller_state_.reference = reference_state; - controller_state_.feedback = system_state; - controller_state_.error = error; - controller_state_.output = command_state; - rt_controller_state_pub_->try_publish(controller_state_); - }; - - // hold position until a new trajectory is received - if (*rt_holding_position_.readFromRT()) { - write_command(command_interfaces_, system_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(system_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, system_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 = system_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(), system_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 = system_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 trajectory_controller - -#include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(trajectory_controller::TrajectoryController, controller_interface::ControllerInterface) diff --git a/trajectory_controller/src/trajectory_controller_parameters.yaml b/trajectory_controller/src/trajectory_controller_parameters.yaml deleted file mode 100644 index cbf77a0..0000000 --- a/trajectory_controller/src/trajectory_controller_parameters.yaml +++ /dev/null @@ -1,93 +0,0 @@ -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 states provided via a topic instead of state interfaces or tf2. If - both this and lookup_transform are set to true, the controller will use - the states provided via a topic. - - lookup_transform: - type: bool - default_value: false - read_only: true - description: > - Use tf2 to look up the state transform instead of using state - interfaces or a topic. This is useful in scenarios where the pose - is determined only by using tf2 to perform forward kinematics. - - If both this and use_external_measured_states are set to true, the - controller will use the 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. - - frame_id: - type: string - default_value: odom_ned - read_only: true - description: > - The name of the parent frame. This is used to lookup the body pose when - using lookup_transform. - - child_frame_id: - type: string - default_value: base_link_frd - read_only: true - description: > - The name of the body frame. This is used to lookup the body pose when - using lookup_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/trajectory_controller/trajectory_controller.xml b/trajectory_controller/trajectory_controller.xml deleted file mode 100644 index 1105bcd..0000000 --- a/trajectory_controller/trajectory_controller.xml +++ /dev/null @@ -1,9 +0,0 @@ - - - - Trajectory controller for AUVs and UVMS that uses geometric methods to compute the desired trajectory. - - - From 49e7da9051c3ffe6ac099212c970bbcd0d51f2fb Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Fri, 1 Aug 2025 14:17:53 -0700 Subject: [PATCH 06/19] what --- impedance_controller/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/impedance_controller/README.md b/impedance_controller/README.md index bf193a1..80d38ef 100644 --- a/impedance_controller/README.md +++ b/impedance_controller/README.md @@ -7,7 +7,7 @@ the implemented control law is given as follows \tau = \tau_{\text{ref}} + \textbf{K}_\text{p}(g_\text{ref}^\top g)^{\vee} + \textbf{K}_\text{d}(\nu_\text{ref} - \nu) ``` -where $\textbf{K}_p$ is the desired stiffness, and $\textbf{K}_\text{d}$ is the desired damping. +where $g_\text{ref}$ is the desired stiffness, and $\textbf{K}_\text{d}$ is the desired damping. This control law is commonly used as an inner-loop controller in an MPC framework. [^1] [^2] From 417bf0b2afe2715b8ec472bcfaed39a5bd700634 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Fri, 1 Aug 2025 14:18:25 -0700 Subject: [PATCH 07/19] confused --- impedance_controller/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/impedance_controller/README.md b/impedance_controller/README.md index 80d38ef..449d188 100644 --- a/impedance_controller/README.md +++ b/impedance_controller/README.md @@ -7,7 +7,7 @@ the implemented control law is given as follows \tau = \tau_{\text{ref}} + \textbf{K}_\text{p}(g_\text{ref}^\top g)^{\vee} + \textbf{K}_\text{d}(\nu_\text{ref} - \nu) ``` -where $g_\text{ref}$ is the desired stiffness, and $\textbf{K}_\text{d}$ is the desired damping. +where $\textbf{K}_\text{p}$ is the desired stiffness, and $\textbf{K}_\text{d}$ is the desired damping. This control law is commonly used as an inner-loop controller in an MPC framework. [^1] [^2] From a15573c20515bb6b6743e3ba731f5ef145275a38 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Fri, 1 Aug 2025 14:19:27 -0700 Subject: [PATCH 08/19] maybe --- impedance_controller/README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/impedance_controller/README.md b/impedance_controller/README.md index 449d188..00b85e1 100644 --- a/impedance_controller/README.md +++ b/impedance_controller/README.md @@ -4,10 +4,10 @@ A chainable impedance controller. Given an AUV pose $g$ with velocity $\nu$, the implemented control law is given as follows ```math -\tau = \tau_{\text{ref}} + \textbf{K}_\text{p}(g_\text{ref}^\top g)^{\vee} + \textbf{K}_\text{d}(\nu_\text{ref} - \nu) +\tau = \tau_{\text{ref}} + \textbf{K}_{\text{p}}(g_\text{ref}^\top g)^{\vee} + \textbf{K}_{\text{d}}(\nu_\text{ref} - \nu) ``` -where $\textbf{K}_\text{p}$ is the desired stiffness, and $\textbf{K}_\text{d}$ is the desired damping. +where $\textbf{K}_{\text{p}}$ is the desired stiffness, and $\textbf{K}_{\text{d}}$ is the desired damping. This control law is commonly used as an inner-loop controller in an MPC framework. [^1] [^2] From f1af1e22597bc1b462de770ca9edf160dd5cfbd6 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Fri, 1 Aug 2025 14:20:52 -0700 Subject: [PATCH 09/19] maybe --- impedance_controller/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/impedance_controller/README.md b/impedance_controller/README.md index 00b85e1..6ebbe0e 100644 --- a/impedance_controller/README.md +++ b/impedance_controller/README.md @@ -7,7 +7,7 @@ the implemented control law is given as follows \tau = \tau_{\text{ref}} + \textbf{K}_{\text{p}}(g_\text{ref}^\top g)^{\vee} + \textbf{K}_{\text{d}}(\nu_\text{ref} - \nu) ``` -where $\textbf{K}_{\text{p}}$ is the desired stiffness, and $\textbf{K}_{\text{d}}$ is the desired damping. +where $\mathbf{K}_{\text{p}}$ is the desired stiffness, and $\mathbf{K}_{\text{d}}$ is the desired damping. This control law is commonly used as an inner-loop controller in an MPC framework. [^1] [^2] From 694c8001ddfc99a2acbac77381235db8ca95ef62 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Fri, 1 Aug 2025 14:21:40 -0700 Subject: [PATCH 10/19] maybe --- impedance_controller/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/impedance_controller/README.md b/impedance_controller/README.md index 6ebbe0e..cbb9da8 100644 --- a/impedance_controller/README.md +++ b/impedance_controller/README.md @@ -7,7 +7,7 @@ the implemented control law is given as follows \tau = \tau_{\text{ref}} + \textbf{K}_{\text{p}}(g_\text{ref}^\top g)^{\vee} + \textbf{K}_{\text{d}}(\nu_\text{ref} - \nu) ``` -where $\mathbf{K}_{\text{p}}$ is the desired stiffness, and $\mathbf{K}_{\text{d}}$ is the desired damping. +where $\text{\textbf{K}}_{\text{p}}$ is the desired stiffness, and $\mathbf{K}_{\text{d}}$ is the desired damping. This control law is commonly used as an inner-loop controller in an MPC framework. [^1] [^2] From e4148937d617c976216173f403e66742bf5522e3 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Fri, 1 Aug 2025 14:22:39 -0700 Subject: [PATCH 11/19] maybe --- impedance_controller/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/impedance_controller/README.md b/impedance_controller/README.md index cbb9da8..d2636e5 100644 --- a/impedance_controller/README.md +++ b/impedance_controller/README.md @@ -7,7 +7,7 @@ the implemented control law is given as follows \tau = \tau_{\text{ref}} + \textbf{K}_{\text{p}}(g_\text{ref}^\top g)^{\vee} + \textbf{K}_{\text{d}}(\nu_\text{ref} - \nu) ``` -where $\text{\textbf{K}}_{\text{p}}$ is the desired stiffness, and $\mathbf{K}_{\text{d}}$ is the desired damping. +where 
```math \textbf{K}_{\text{p}}```
is the desired stiffness, and $\mathbf{K}_{\text{d}}$ is the desired damping. This control law is commonly used as an inner-loop controller in an MPC framework. [^1] [^2] From 8af215c9c059d12d35c66e5dede32ee2aea1defe Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Fri, 1 Aug 2025 14:23:04 -0700 Subject: [PATCH 12/19] maybe --- impedance_controller/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/impedance_controller/README.md b/impedance_controller/README.md index d2636e5..14a880d 100644 --- a/impedance_controller/README.md +++ b/impedance_controller/README.md @@ -7,7 +7,7 @@ the implemented control law is given as follows \tau = \tau_{\text{ref}} + \textbf{K}_{\text{p}}(g_\text{ref}^\top g)^{\vee} + \textbf{K}_{\text{d}}(\nu_\text{ref} - \nu) ``` -where 
```math \textbf{K}_{\text{p}}```
is the desired stiffness, and $\mathbf{K}_{\text{d}}$ is the desired damping. +where ```math \textbf{K}_{\text{p}}``` is the desired stiffness, and $\mathbf{K}_{\text{d}}$ is the desired damping. This control law is commonly used as an inner-loop controller in an MPC framework. [^1] [^2] From d0d17e9d02751f6e41a5cadf3e707f599254d8c3 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Fri, 1 Aug 2025 14:24:28 -0700 Subject: [PATCH 13/19] maybe --- impedance_controller/README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/impedance_controller/README.md b/impedance_controller/README.md index 14a880d..0ae3758 100644 --- a/impedance_controller/README.md +++ b/impedance_controller/README.md @@ -4,10 +4,10 @@ A chainable impedance controller. Given an AUV pose $g$ with velocity $\nu$, the implemented control law is given as follows ```math -\tau = \tau_{\text{ref}} + \textbf{K}_{\text{p}}(g_\text{ref}^\top g)^{\vee} + \textbf{K}_{\text{d}}(\nu_\text{ref} - \nu) +\tau = \tau_{\text{ref}} + \textbf{K}_\text{p}(g_\text{ref}^\top g)^{\vee} + \textbf{K}_\text{d}(\nu_\text{ref} - \nu) ``` -where ```math \textbf{K}_{\text{p}}``` is the desired stiffness, and $\mathbf{K}_{\text{d}}$ is the desired damping. +where $\textbf{K}_p$ is the desired stiffness, and $\textbf{K}_d$ is the desired damping. This control law is commonly used as an inner-loop controller in an MPC framework. [^1] [^2] From c86cf7705ab8310905f81edcc8dfdc2aee3e4565 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Fri, 1 Aug 2025 14:26:32 -0700 Subject: [PATCH 14/19] cleanup? --- impedance_controller/README.md | 7 +++---- thruster_allocation_matrix_controller/README.md | 5 ++--- 2 files changed, 5 insertions(+), 7 deletions(-) diff --git a/impedance_controller/README.md b/impedance_controller/README.md index 0ae3758..7fa0159 100644 --- a/impedance_controller/README.md +++ b/impedance_controller/README.md @@ -7,10 +7,9 @@ the implemented control law is given as follows \tau = \tau_{\text{ref}} + \textbf{K}_\text{p}(g_\text{ref}^\top g)^{\vee} + \textbf{K}_\text{d}(\nu_\text{ref} - \nu) ``` -where $\textbf{K}_p$ is the desired stiffness, and $\textbf{K}_d$ is the desired damping. - -This control law is commonly used as an inner-loop controller in an MPC -framework. [^1] [^2] +where $\textbf{K}_p$ is the desired stiffness, and $\textbf{K}_d$ is the +desired damping. This control law is commonly used as an inner-loop controller +in an MPC framework. [^1] [^2] [^1]: I. Dadiotis, A. Laurenzi, and N. Tsagarakis. "Whole-body MPC for highly redundant legged manipulators: experimental evaluation with a 37 DoF dual-arm quadruped," in *IEEE International Conference on Humanoid Robots (Humanoids)*, 2023. [^2]: J. -P. Sleiman, F. Farshidian, M. V. Minniti and M. Hutter, "A Unified MPC Framework for Whole-Body Dynamic Locomotion and Manipulation," in *IEEE Robotics and Automation Letters*, vol. 6, no. 3, pp. 4688-4695, July 2021. diff --git a/thruster_allocation_matrix_controller/README.md b/thruster_allocation_matrix_controller/README.md index f181b6f..f0b0f5c 100644 --- a/thruster_allocation_matrix_controller/README.md +++ b/thruster_allocation_matrix_controller/README.md @@ -11,9 +11,8 @@ thruster_allocation_matrix_controller/ThrusterAllocationMatrixController ## References -The input to this controller is a wrench with force components Fx, -Fy, Fz and torque components Trx, -Try, Trz in N and Nm, respectively. +The input to this controller is a wrench with force components $F_x$, $F_y$, +$F_z$, and torque components $T_{rx}$, $T_{ry}$, $T_{rz}$ in N and Nm, respectively. ## Commands From 8f6e65cbfbea9e5b59809ea386db9dcf2fbea519 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Fri, 1 Aug 2025 14:27:31 -0700 Subject: [PATCH 15/19] cleanup? --- thruster_allocation_matrix_controller/README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/thruster_allocation_matrix_controller/README.md b/thruster_allocation_matrix_controller/README.md index f0b0f5c..2d12d13 100644 --- a/thruster_allocation_matrix_controller/README.md +++ b/thruster_allocation_matrix_controller/README.md @@ -11,8 +11,8 @@ thruster_allocation_matrix_controller/ThrusterAllocationMatrixController ## References -The input to this controller is a wrench with force components $F_x$, $F_y$, -$F_z$, and torque components $T_{rx}$, $T_{ry}$, $T_{rz}$ in N and Nm, respectively. +The input to this controller is a wrench with force components $F_\text{x}$, $F_\text{y}$, +$F_\text{z}$, and torque components $T_\text{rx}$, $T_\text{ry}$, $T_\text{rz}$ in N and Nm, respectively. ## Commands From ac424629610e95712b4d10c54b2518ad943d0141 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Fri, 1 Aug 2025 14:27:57 -0700 Subject: [PATCH 16/19] test --- thruster_allocation_matrix_controller/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/thruster_allocation_matrix_controller/README.md b/thruster_allocation_matrix_controller/README.md index 2d12d13..e77baa2 100644 --- a/thruster_allocation_matrix_controller/README.md +++ b/thruster_allocation_matrix_controller/README.md @@ -11,7 +11,7 @@ thruster_allocation_matrix_controller/ThrusterAllocationMatrixController ## References -The input to this controller is a wrench with force components $F_\text{x}$, $F_\text{y}$, +The input to this controller is a wrench with force components $\text{F}_\text{x}$, $F_\text{y}$, $F_\text{z}$, and torque components $T_\text{rx}$, $T_\text{ry}$, $T_\text{rz}$ in N and Nm, respectively. ## Commands From 1d8990dfc82e5a9621ac2ba9dee2dcc4349c4aa9 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Fri, 1 Aug 2025 14:29:07 -0700 Subject: [PATCH 17/19] test --- thruster_allocation_matrix_controller/README.md | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/thruster_allocation_matrix_controller/README.md b/thruster_allocation_matrix_controller/README.md index e77baa2..fb92d08 100644 --- a/thruster_allocation_matrix_controller/README.md +++ b/thruster_allocation_matrix_controller/README.md @@ -11,8 +11,9 @@ thruster_allocation_matrix_controller/ThrusterAllocationMatrixController ## References -The input to this controller is a wrench with force components $\text{F}_\text{x}$, $F_\text{y}$, -$F_\text{z}$, and torque components $T_\text{rx}$, $T_\text{ry}$, $T_\text{rz}$ in N and Nm, respectively. +The input to this controller is a wrench with force components $\text{F}_x$, +$\text{F}_y$, $\text{F}_z$, and torque components $\text{T}_{rx}$, +$\text{T}_{ry}$, $\text{T}_{rz}$ in N and Nm, respectively. ## Commands From c9b2400e42782d4fd258103a64be60491c56d6eb Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Fri, 1 Aug 2025 14:30:04 -0700 Subject: [PATCH 18/19] revert for now --- thruster_allocation_matrix_controller/README.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/thruster_allocation_matrix_controller/README.md b/thruster_allocation_matrix_controller/README.md index fb92d08..f181b6f 100644 --- a/thruster_allocation_matrix_controller/README.md +++ b/thruster_allocation_matrix_controller/README.md @@ -11,9 +11,9 @@ thruster_allocation_matrix_controller/ThrusterAllocationMatrixController ## References -The input to this controller is a wrench with force components $\text{F}_x$, -$\text{F}_y$, $\text{F}_z$, and torque components $\text{T}_{rx}$, -$\text{T}_{ry}$, $\text{T}_{rz}$ in N and Nm, respectively. +The input to this controller is a wrench with force components Fx, +Fy, Fz and torque components Trx, +Try, Trz in N and Nm, respectively. ## Commands From e8d85c756cb613fdf5ddfe744fa56632944d8b8e Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Wed, 6 Aug 2025 22:44:58 +0000 Subject: [PATCH 19/19] initial testing of impedance controller --- auv_control_demos/COLCON_IGNORE | 0 impedance_controller/src/impedance_controller.cpp | 9 +++++++-- 2 files changed, 7 insertions(+), 2 deletions(-) create mode 100644 auv_control_demos/COLCON_IGNORE diff --git a/auv_control_demos/COLCON_IGNORE b/auv_control_demos/COLCON_IGNORE new file mode 100644 index 0000000..e69de29 diff --git a/impedance_controller/src/impedance_controller.cpp b/impedance_controller/src/impedance_controller.cpp index 0694f07..68b82ab 100644 --- a/impedance_controller/src/impedance_controller.cpp +++ b/impedance_controller/src/impedance_controller.cpp @@ -26,6 +26,7 @@ #include "controller_common/common.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "tf2_eigen/tf2_eigen.hpp" +#include "pluginlib/class_list_macros.hpp" namespace impedance_controller { @@ -186,8 +187,8 @@ auto ImpedanceController::on_export_reference_interfaces() -> std::vectorget_name(), std::format("{}/{}", dof, hardware_interface::HW_IF_POSITION), &reference_interfaces_[i]); + const std::string type = i < 7 ? hardware_interface::HW_IF_POSITION : hardware_interface::HW_IF_VELOCITY; + interfaces.emplace_back(get_node()->get_name(), std::format("{}/{}", dof, type), &reference_interfaces_[i]); } // add the force/torque interfaces @@ -332,3 +333,7 @@ auto ImpedanceController::update_and_write_commands(const rclcpp::Time & time, c } } // namespace impedance_controller + +PLUGINLIB_EXPORT_CLASS( + impedance_controller::ImpedanceController, + controller_interface::ChainableControllerInterface) \ No newline at end of file