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/COLCON_IGNORE b/auv_control_demos/COLCON_IGNORE new file mode 100644 index 0000000..e69de29 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 60ff528..3b15f7f 100644 --- a/auv_control_msgs/CHANGELOG.md +++ b/auv_control_msgs/CHANGELOG.md @@ -1,5 +1,13 @@ # Changelog for package auv_control_msgs +## 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 + ## 0.3.3 (2025-07-29) ## 0.3.2 (2025-07-22) @@ -12,10 +20,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 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 83ab507..78e50aa 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/EndEffectorTrajectoryPoint.msg" - "msg/EndEffectorTrajectory.msg" - "msg/EndEffectorTrajectoryControllerState.msg" - "action/FollowEndEffectorTrajectory.action" + "msg/CartesianTrajectoryPoint.msg" + "msg/CartesianTrajectory.msg" + "msg/CartesianTrajectoryControllerStateStamped.msg" + "msg/ImpedanceCommand.msg" + "action/FollowCartesianTrajectory.action" DEPENDENCIES builtin_interfaces std_msgs geometry_msgs trajectory_msgs ) diff --git a/auv_control_msgs/action/FollowEndEffectorTrajectory.action b/auv_control_msgs/action/FollowCartesianTrajectory.action similarity index 95% rename from auv_control_msgs/action/FollowEndEffectorTrajectory.action rename to auv_control_msgs/action/FollowCartesianTrajectory.action index 1a12cd6..e0a7c1b 100644 --- a/auv_control_msgs/action/FollowEndEffectorTrajectory.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/EndEffectorTrajectory 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/EndEffectorTrajectory.msg b/auv_control_msgs/msg/CartesianTrajectory.msg similarity index 57% rename from auv_control_msgs/msg/EndEffectorTrajectory.msg rename to auv_control_msgs/msg/CartesianTrajectory.msg index 114b86a..21c6688 100644 --- a/auv_control_msgs/msg/EndEffectorTrajectory.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/EndEffectorTrajectoryPoint[] points +auv_control_msgs/CartesianTrajectoryPoint[] points diff --git a/auv_control_msgs/msg/EndEffectorTrajectoryControllerState.msg b/auv_control_msgs/msg/CartesianTrajectoryControllerStateStamped.msg similarity index 84% rename from auv_control_msgs/msg/EndEffectorTrajectoryControllerState.msg rename to auv_control_msgs/msg/CartesianTrajectoryControllerStateStamped.msg index 7410871..1b0be17 100644 --- a/auv_control_msgs/msg/EndEffectorTrajectoryControllerState.msg +++ b/auv_control_msgs/msg/CartesianTrajectoryControllerStateStamped.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/EndEffectorTrajectoryPoint.msg b/auv_control_msgs/msg/CartesianTrajectoryPoint.msg similarity index 80% rename from auv_control_msgs/msg/EndEffectorTrajectoryPoint.msg rename to auv_control_msgs/msg/CartesianTrajectoryPoint.msg index dbca6e6..5715121 100644 --- a/auv_control_msgs/msg/EndEffectorTrajectoryPoint.msg +++ b/auv_control_msgs/msg/CartesianTrajectoryPoint.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/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/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/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/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_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/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/end_effector_trajectory_controller/README.md b/end_effector_trajectory_controller/README.md deleted file mode 100644 index 426bd81..0000000 --- a/end_effector_trajectory_controller/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 - -end_effector_trajectory_controller/EndEffectorTrajectoryController - -## References - -The input to this controller is a sequence of end effector poses. - -## Commands - -The output of this controller is a sampled end effector pose. - -## Subscribers - -- end_effector_trajectory_controller/trajectory [auv_control_msgs::msg::EndEffectorTrajectory] - -## Action Servers - -- end_effector_trajectory_controller/follow_trajectory [auv_control_msgs::action::FollowEndEffectorTrajectory] - -## Publishers - -- end_effector_trajectory_controller/status [auv_control_msgs::msg::EndEffectorTrajectoryControllerState] diff --git a/end_effector_trajectory_controller/end_effector_trajectory_controller.xml b/end_effector_trajectory_controller/end_effector_trajectory_controller.xml 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/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 new file mode 100644 index 0000000..3121ae3 --- /dev/null +++ b/impedance_controller/CMakeLists.txt @@ -0,0 +1,93 @@ +cmake_minimum_required(VERSION 3.23) +project(impedance_controller) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +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) +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/end_effector_trajectory_controller/LICENSE b/impedance_controller/LICENSE similarity index 100% rename from end_effector_trajectory_controller/LICENSE rename to impedance_controller/LICENSE diff --git a/impedance_controller/README.md b/impedance_controller/README.md new file mode 100644 index 0000000..7fa0159 --- /dev/null +++ b/impedance_controller/README.md @@ -0,0 +1,42 @@ +# 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}_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. + +## 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 new file mode 100644 index 0000000..dd5407b --- /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_, n_reference_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..54719ad --- /dev/null +++ b/impedance_controller/package.xml @@ -0,0 +1,43 @@ + + + + + impedance_controller + 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 new file mode 100644 index 0000000..68b82ab --- /dev/null +++ b/impedance_controller/src/impedance_controller.cpp @@ -0,0 +1,339 @@ +// 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 +#include + +#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 +{ + +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); + const Eigen::Matrix4d error = (goal_mat.inverse() * state_mat).matrix().log(); + return vee(error); +} + +} // 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(); + n_reference_dofs_ = n_command_dofs_ + n_state_dofs_; + + 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_); + 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 = n_pose_dofs; 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_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_)) { + 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 + // 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(), + 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_.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()); + }); + } + + 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 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); + 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 into 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_reference_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 + +PLUGINLIB_EXPORT_CLASS( + impedance_controller::ImpedanceController, + controller_interface::ChainableControllerInterface) \ No newline at end of file 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/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/end_effector_trajectory_controller/CHANGELOG.md b/trajectory_controllers/CHANGELOG.md similarity index 55% rename from end_effector_trajectory_controller/CHANGELOG.md rename to trajectory_controllers/CHANGELOG.md index e4cb19b..0ab31a7 100644 --- a/end_effector_trajectory_controller/CHANGELOG.md +++ b/trajectory_controllers/CHANGELOG.md @@ -1,4 +1,9 @@ -# Changelog for package end_effector_trajectory_controller +# 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) diff --git a/end_effector_trajectory_controller/CMakeLists.txt b/trajectory_controllers/CMakeLists.txt similarity index 64% rename from end_effector_trajectory_controller/CMakeLists.txt rename to trajectory_controllers/CMakeLists.txt index cefc572..0207dc9 100644 --- a/end_effector_trajectory_controller/CMakeLists.txt +++ b/trajectory_controllers/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.23) -project(end_effector_trajectory_controller) +project(trajectory_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(cartesian_trajectory_controller_parameters + src/cartesian_trajectory_controller_parameters.yaml ) -add_library(end_effector_trajectory_controller SHARED) +add_library(trajectory_controllers SHARED) target_sources( - end_effector_trajectory_controller - PRIVATE src/end_effector_trajectory_controller.cpp src/trajectory.cpp + 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/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/trajectory_controllers/cartesian_trajectory_controller.hpp + ${CMAKE_CURRENT_SOURCE_DIR}/include/trajectory_controllers/cartesian_trajectory.hpp ) -target_compile_features(end_effector_trajectory_controller PUBLIC cxx_std_23) +target_compile_features(trajectory_controllers PUBLIC cxx_std_23) target_link_libraries( - end_effector_trajectory_controller + trajectory_controllers PUBLIC - end_effector_trajectory_controller_parameters + cartesian_trajectory_controller_parameters realtime_tools::realtime_tools controller_common::controller_common hardware_interface::hardware_interface @@ -62,20 +62,18 @@ 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 trajectory_controllers.xml) install( - TARGETS - end_effector_trajectory_controller - end_effector_trajectory_controller_parameters - EXPORT export_end_effector_trajectory_controller + 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_end_effector_trajectory_controller HAS_LIBRARY_TARGET) +ament_export_targets(export_trajectory_controllers HAS_LIBRARY_TARGET) ament_export_dependencies( "geometry_msgs" "realtime_tools" 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/end_effector_trajectory_controller/include/end_effector_trajectory_controller/trajectory.hpp b/trajectory_controllers/include/trajectory_controllers/cartesian_trajectory.hpp similarity index 86% rename from end_effector_trajectory_controller/include/end_effector_trajectory_controller/trajectory.hpp rename to trajectory_controllers/include/trajectory_controllers/cartesian_trajectory.hpp index 485b640..8d874a5 100644 --- a/end_effector_trajectory_controller/include/end_effector_trajectory_controller/trajectory.hpp +++ b/trajectory_controllers/include/trajectory_controllers/cartesian_trajectory.hpp @@ -25,11 +25,11 @@ #include #include -#include "auv_control_msgs/msg/end_effector_trajectory.hpp" +#include "auv_control_msgs/msg/cartesian_trajectory.hpp" #include "geometry_msgs/msg/pose.hpp" #include "rclcpp/rclcpp.hpp" -namespace end_effector_trajectory_controller +namespace trajectory_controllers { enum class SampleError : std::uint8_t @@ -39,13 +39,15 @@ enum class SampleError : std::uint8_t EMPTY_TRAJECTORY, }; -class Trajectory +class CartesianTrajectory { public: - Trajectory() = default; + CartesianTrajectory() = 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); + 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; @@ -70,9 +72,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::CartesianTrajectory points_; rclcpp::Time initial_time_; geometry_msgs::msg::Pose initial_state_; }; -} // namespace end_effector_trajectory_controller +} // namespace trajectory_controllers diff --git a/end_effector_trajectory_controller/include/end_effector_trajectory_controller/end_effector_trajectory_controller.hpp b/trajectory_controllers/include/trajectory_controllers/cartesian_trajectory_controller.hpp similarity index 77% rename from end_effector_trajectory_controller/include/end_effector_trajectory_controller/end_effector_trajectory_controller.hpp rename to trajectory_controllers/include/trajectory_controllers/cartesian_trajectory_controller.hpp index 1227848..c3283fc 100644 --- a/end_effector_trajectory_controller/include/end_effector_trajectory_controller/end_effector_trajectory_controller.hpp +++ b/trajectory_controllers/include/trajectory_controllers/cartesian_trajectory_controller.hpp @@ -22,11 +22,10 @@ #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_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 "end_effector_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_controllers/cartesian_trajectory.hpp" // auto-generated by generate_parameter_library -#include +#include -namespace end_effector_trajectory_controller +namespace trajectory_controllers { -class EndEffectorTrajectoryController : public controller_interface::ControllerInterface +class CartesianTrajectoryController : public controller_interface::ControllerInterface { public: - EndEffectorTrajectoryController() = default; + CartesianTrajectoryController() = 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::CartesianTrajectory & trajectory) const -> bool; // controller state - using ControllerState = auv_control_msgs::msg::EndEffectorTrajectoryControllerState; + using ControllerState = auv_control_msgs::msg::CartesianTrajectoryControllerStateStamped; 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 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 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_; + realtime_tools::RealtimeBuffer rt_trajectory_; + std::shared_ptr> trajectory_sub_; - using FollowTrajectory = auv_control_msgs::action::FollowEndEffectorTrajectory; + using FollowTrajectory = auv_control_msgs::action::FollowCartesianTrajectory; using RealtimeGoalHandle = realtime_tools::RealtimeServerGoalHandle; using RealtimeGoalHandlePtr = std::shared_ptr; using RealtimeGoalHandleBuffer = realtime_tools::RealtimeBuffer; @@ -105,13 +105,13 @@ class EndEffectorTrajectoryController : public controller_interface::ControllerI 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}; - std::shared_ptr param_listener_; - end_effector_trajectory_controller::Params params_; + 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 @@ -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("trajectory_controller")}; private: template @@ -137,4 +137,4 @@ class EndEffectorTrajectoryController : public controller_interface::ControllerI } }; -} // namespace end_effector_trajectory_controller +} // namespace trajectory_controllers diff --git a/end_effector_trajectory_controller/package.xml b/trajectory_controllers/package.xml similarity index 87% rename from end_effector_trajectory_controller/package.xml rename to trajectory_controllers/package.xml index f18d2fc..825df44 100644 --- a/end_effector_trajectory_controller/package.xml +++ b/trajectory_controllers/package.xml @@ -2,9 +2,9 @@ - end_effector_trajectory_controller - 0.3.3 - End effector trajectory tracking controller for UVMS control + trajectory_controllers + 0.4.0 + Trajectory controllers for underwater vehicles and manipulator systems Evan Palmer MIT diff --git a/end_effector_trajectory_controller/src/trajectory.cpp b/trajectory_controllers/src/cartesian_trajectory.cpp similarity index 85% rename from end_effector_trajectory_controller/src/trajectory.cpp rename to trajectory_controllers/src/cartesian_trajectory.cpp index 297dbd3..552f5c4 100644 --- a/end_effector_trajectory_controller/src/trajectory.cpp +++ b/trajectory_controllers/src/cartesian_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 "trajectory_controllers/cartesian_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 trajectory_controllers { namespace @@ -93,8 +93,8 @@ auto interpolate( } // namespace -Trajectory::Trajectory( - const auv_control_msgs::msg::EndEffectorTrajectory & trajectory, +CartesianTrajectory::CartesianTrajectory( + const auv_control_msgs::msg::CartesianTrajectory & trajectory, const geometry_msgs::msg::Pose & state) : points_(trajectory), initial_time_(static_cast(trajectory.header.stamp)), @@ -102,19 +102,19 @@ Trajectory::Trajectory( { } -auto Trajectory::empty() const -> bool { return points_.points.empty(); } +auto CartesianTrajectory::empty() const -> bool { return points_.points.empty(); } -auto Trajectory::start_time() const -> rclcpp::Time +auto CartesianTrajectory::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 +auto CartesianTrajectory::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 +auto CartesianTrajectory::start_point() const -> std::optional { if (empty()) { return std::nullopt; @@ -122,7 +122,7 @@ auto Trajectory::start_point() const -> std::optional return points_.points.front().point; } -auto Trajectory::end_point() const -> std::optional +auto CartesianTrajectory::end_point() const -> std::optional { if (empty()) { return std::nullopt; @@ -130,7 +130,8 @@ auto Trajectory::end_point() const -> std::optional return points_.points.back().point; } -auto Trajectory::sample(const rclcpp::Time & sample_time) const -> std::expected +auto CartesianTrajectory::sample(const rclcpp::Time & sample_time) const + -> std::expected { if (empty()) { return std::unexpected(SampleError::EMPTY_TRAJECTORY); @@ -160,6 +161,9 @@ auto Trajectory::sample(const rclcpp::Time & sample_time) const -> std::expected return std::unexpected(SampleError::SAMPLE_TIME_AFTER_END); } -auto Trajectory::reset_initial_state(const geometry_msgs::msg::Pose & state) -> void { initial_state_ = state; } +auto CartesianTrajectory::reset_initial_state(const geometry_msgs::msg::Pose & state) -> void +{ + initial_state_ = state; +} -} // namespace end_effector_trajectory_controller +} // namespace trajectory_controllers diff --git a/end_effector_trajectory_controller/src/end_effector_trajectory_controller.cpp b/trajectory_controllers/src/cartesian_trajectory_controller.cpp similarity index 76% rename from end_effector_trajectory_controller/src/end_effector_trajectory_controller.cpp rename to trajectory_controllers/src/cartesian_trajectory_controller.cpp index 8008d7d..5ab13bc 100644 --- a/end_effector_trajectory_controller/src/end_effector_trajectory_controller.cpp +++ b/trajectory_controllers/src/cartesian_trajectory_controller.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/end_effector_trajectory_controller.hpp" +#include "trajectory_controllers/cartesian_trajectory_controller.hpp" #include #include @@ -29,32 +29,40 @@ #include "rclcpp_action/rclcpp_action.hpp" #include "tf2_eigen/tf2_eigen.hpp" -namespace end_effector_trajectory_controller +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); - return std::pow((goal_mat.inverse() * state_mat).matrix().log().norm(), 2); + const Eigen::Matrix4d error = (goal_mat.inverse() * state_mat).matrix().log(); + return std::pow(vee(error).norm(), 2); } } // namespace -auto EndEffectorTrajectoryController::on_init() -> controller_interface::CallbackReturn +auto CartesianTrajectoryController::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 CartesianTrajectoryController::update_parameters() -> void { if (!param_listener_->is_old(params_)) { return; @@ -63,7 +71,7 @@ auto EndEffectorTrajectoryController::update_parameters() -> void params_ = param_listener_->get_params(); } -auto EndEffectorTrajectoryController::configure_parameters() -> controller_interface::CallbackReturn +auto CartesianTrajectoryController::configure_parameters() -> controller_interface::CallbackReturn { update_parameters(); @@ -82,8 +90,8 @@ auto EndEffectorTrajectoryController::configure_parameters() -> controller_inter return controller_interface::CallbackReturn::SUCCESS; } -auto EndEffectorTrajectoryController::validate_trajectory( - const auv_control_msgs::msg::EndEffectorTrajectory & trajectory) const -> bool +auto CartesianTrajectoryController::validate_trajectory( + const auv_control_msgs::msg::CartesianTrajectory & trajectory) const -> bool { if (trajectory.points.empty()) { RCLCPP_ERROR(logger_, "Received empty trajectory"); // NOLINT @@ -111,7 +119,7 @@ auto EndEffectorTrajectoryController::validate_trajectory( 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 + RCLCPP_ERROR(logger_, "CartesianTrajectory points are not in order"); // NOLINT return false; } } @@ -119,33 +127,33 @@ auto EndEffectorTrajectoryController::validate_trajectory( return true; } -auto EndEffectorTrajectoryController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) +auto CartesianTrajectoryController::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( - "~/end_effector_state", + state_sub_ = get_node()->create_subscription( + "~/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) { + 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 @@ -156,7 +164,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(CartesianTrajectory(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 +188,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(CartesianTrajectory(updated_msg, *state_.readFromNonRT())); rt_first_sample_.writeFromNonRT(true); rt_holding_position_.writeFromNonRT(false); RCLCPP_INFO(logger_, "Accepted new trajectory goal"); // NOLINT @@ -225,7 +233,7 @@ auto EndEffectorTrajectoryController::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()); @@ -235,16 +243,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 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(end_effector_state_.readFromNonRT()); + common::messages::reset_message(state_.readFromNonRT()); return controller_interface::CallbackReturn::SUCCESS; } -auto EndEffectorTrajectoryController::command_interface_configuration() const +auto CartesianTrajectoryController::command_interface_configuration() const -> controller_interface::InterfaceConfiguration { controller_interface::InterfaceConfiguration config; @@ -260,11 +268,11 @@ auto EndEffectorTrajectoryController::command_interface_configuration() const return config; } -auto EndEffectorTrajectoryController::state_interface_configuration() const +auto CartesianTrajectoryController::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; } @@ -278,18 +286,18 @@ auto EndEffectorTrajectoryController::state_interface_configuration() const return config; } -auto EndEffectorTrajectoryController::update_end_effector_state() -> controller_interface::return_type +auto CartesianTrajectoryController::update_system_state() -> controller_interface::return_type { - auto * end_effector_state = end_effector_state_.readFromRT(); - if (params_.lookup_end_effector_transform) { - const auto to_frame = params_.end_effector_frame_id; - const auto from_frame = params_.odom_frame_id; + 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()); @@ -300,26 +308,26 @@ auto EndEffectorTrajectoryController::update_end_effector_state() -> controller_ 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; } -auto EndEffectorTrajectoryController::update(const rclcpp::Time & time, const rclcpp::Duration & period) +auto CartesianTrajectoryController::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,16 +339,16 @@ 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 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_); @@ -348,7 +356,7 @@ auto EndEffectorTrajectoryController::update(const rclcpp::Time & time, const rc // 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; } @@ -356,7 +364,7 @@ auto EndEffectorTrajectoryController::update(const rclcpp::Time & time, const rc // 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; } @@ -371,7 +379,7 @@ auto EndEffectorTrajectoryController::update(const rclcpp::Time & time, const rc // 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; @@ -385,7 +393,7 @@ auto EndEffectorTrajectoryController::update(const rclcpp::Time & time, const rc 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 } @@ -393,21 +401,21 @@ auto EndEffectorTrajectoryController::update(const rclcpp::Time & time, const rc } 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 + 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(), end_effector_state); - RCLCPP_INFO(logger_, "Trajectory sample time is after trajectory end time."); // NOLINT + 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_, "Trajectory execution completed successfully"); // NOLINT + RCLCPP_INFO(logger_, "CartesianTrajectory execution completed successfully"); // NOLINT } } rt_holding_position_.writeFromNonRT(true); @@ -426,7 +434,7 @@ auto EndEffectorTrajectoryController::update(const rclcpp::Time & time, const rc 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); @@ -434,19 +442,19 @@ auto EndEffectorTrajectoryController::update(const rclcpp::Time & time, const rc 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."; + 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 = "Trajectory execution completed successfully!"; + 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 = "Trajectory execution aborted. Path tolerance exceeded."; + action_result->error_string = "CartesianTrajectory execution aborted. Path tolerance exceeded."; active_goal->setAborted(action_result); rt_holding_position_.writeFromNonRT(true); } @@ -458,9 +466,7 @@ auto EndEffectorTrajectoryController::update(const rclcpp::Time & time, const rc return controller_interface::return_type::OK; } -} // namespace end_effector_trajectory_controller +} // namespace trajectory_controllers #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS( - end_effector_trajectory_controller::EndEffectorTrajectoryController, - controller_interface::ControllerInterface) +PLUGINLIB_EXPORT_CLASS(trajectory_controllers::CartesianTrajectoryController, controller_interface::ControllerInterface) diff --git a/end_effector_trajectory_controller/src/end_effector_trajectory_controller_parameters.yaml b/trajectory_controllers/src/cartesian_trajectory_controller_parameters.yaml similarity index 68% rename from end_effector_trajectory_controller/src/end_effector_trajectory_controller_parameters.yaml rename to trajectory_controllers/src/cartesian_trajectory_controller_parameters.yaml index b5d2308..947aeaf 100644 --- a/end_effector_trajectory_controller/src/end_effector_trajectory_controller_parameters.yaml +++ b/trajectory_controllers/src/cartesian_trajectory_controller_parameters.yaml @@ -1,4 +1,4 @@ -end_effector_trajectory_controller: +cartesian_trajectory_controller: joints: type: string_array default_value: ["x", "y", "z", "qx", "qy", "qz", "qw"] @@ -42,21 +42,21 @@ end_effector_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_end_effector_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_end_effector_transform: + 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_end_effector_states are set to true, the - controller will use the end effector states provided via a topic. + 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 @@ -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: + 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. + The name of the parent frame. This is used to lookup the body pose when + using lookup_transform. - end_effector_frame_id: + 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_end_effector_transform. + The name of the body frame. This is used to lookup the body pose when + using lookup_transform. action_monitor_rate: type: int 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/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 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