diff --git a/README.md b/README.md index 5137acd..553e58b 100644 --- a/README.md +++ b/README.md @@ -1,9 +1,22 @@ # auv_controllers +<<<<<<< HEAD auv_controllers is a collection of chainable controllers for autonomous underwater vehicles (AUVs) implemented using ros2_control. The controllers have been designed to support the complete AUV control hierarchy and to enable benchmarking against other commonly-used control algorithms. +======= +auv_controllers is a collection of controllers for autonomous underwater +vehicles (AUVs) and underwater vehicle manipulator systems (UVMS) implemented +using ros2_control. The controllers have been designed to support the complete +system control hierarchy and to enable benchmarking against other commonly-used +control algorithms. + +

+ UVMS whole-body control + AUV control +

+>>>>>>> d197fc7 (Port end effector trajectory controller from Angler (#54)) > [!NOTE] > If you are interested in adding your own controller to this project, please @@ -32,10 +45,11 @@ rosdep update && \ rosdep install -y --from-paths src --ignore-src ``` -## Quick start +## Getting started To learn more about how to use the controllers provided in this project, please refer to the [examples package](https://github.com/Robotic-Decision-Making-Lab/auv_controllers/tree/main/auv_control_demos). +You can also find integration tutorials in the [Blue documentation](https://robotic-decision-making-lab.github.io/blue). ## Getting help diff --git a/auv_control_demos/CHANGELOG.md b/auv_control_demos/CHANGELOG.md new file mode 100644 index 0000000..d4819d7 --- /dev/null +++ b/auv_control_demos/CHANGELOG.md @@ -0,0 +1,8 @@ +# Changelog for package auv_control_demos + +## 0.2.0 (2025-05-03) + +## 0.1.0 (2025-04-27) + +- Updates the individual_controller and chained_controllers demos to use the +correct topic names diff --git a/auv_control_demos/package.xml b/auv_control_demos/package.xml index e43b061..2ea225a 100644 --- a/auv_control_demos/package.xml +++ b/auv_control_demos/package.xml @@ -2,9 +2,14 @@ auv_control_demos +<<<<<<< HEAD 0.0.1 Example package that includes demos for using auv_controllers in individual and chained modes +======= + 0.2.0 + Example package that includes demos for using auv_controllers in individual and chained modes +>>>>>>> d197fc7 (Port end effector trajectory controller from Angler (#54)) Colin Mitchell Everardo Gonzalez diff --git a/auv_control_msgs/CHANGELOG.md b/auv_control_msgs/CHANGELOG.md new file mode 100644 index 0000000..cf6d0d6 --- /dev/null +++ b/auv_control_msgs/CHANGELOG.md @@ -0,0 +1,13 @@ +# Changelog for package auv_control_msgs + +## 0.2.0 (2025-05-03) + +- Implements the EndEffectorTrajectory message +- Implements the EndEffectorTrajectoryPoint message +- Implements the EndEffectorTrajectoryControllerState message +- Adds the FollowEndEffectorTrajectory action + +## 0.1.0 (2025-04-27) + +- Implements the IKControllerStateStamped message to support the new IK +controller diff --git a/auv_control_msgs/CMakeLists.txt b/auv_control_msgs/CMakeLists.txt index 357b0da..3f3dccb 100644 --- a/auv_control_msgs/CMakeLists.txt +++ b/auv_control_msgs/CMakeLists.txt @@ -8,7 +8,16 @@ find_package(rosidl_default_generators REQUIRED) rosidl_generate_interfaces(auv_control_msgs "msg/MultiActuatorStateStamped.msg" +<<<<<<< HEAD DEPENDENCIES builtin_interfaces std_msgs +======= + "msg/IKControllerStateStamped.msg" + "msg/EndEffectorTrajectoryPoint.msg" + "msg/EndEffectorTrajectory.msg" + "msg/EndEffectorTrajectoryControllerState.msg" + "action/FollowEndEffectorTrajectory.action" + DEPENDENCIES builtin_interfaces std_msgs geometry_msgs trajectory_msgs +>>>>>>> d197fc7 (Port end effector trajectory controller from Angler (#54)) ) ament_package() diff --git a/auv_control_msgs/action/FollowEndEffectorTrajectory.action b/auv_control_msgs/action/FollowEndEffectorTrajectory.action new file mode 100644 index 0000000..1a12cd6 --- /dev/null +++ b/auv_control_msgs/action/FollowEndEffectorTrajectory.action @@ -0,0 +1,40 @@ +# The end effector trajectory to follow. +# +# The trajectory header stamp is used to set the trajectory start time. This can be +# set to zero to indicate that the controller should start following the trajectory +# now. +auv_control_msgs/EndEffectorTrajectory trajectory + +# The maximum error that the controller is allowed when following the trajectory. +# When this is set to 0, the tolerance will not be applied during control. +float64 path_tolerance + +# The maximum terminal error that the controller is allowed. +# When this is set to 0, the tolerance will not affect the success of the action. +float64 goal_tolerance + +--- +int32 error_code +int32 SUCCESSFUL = 0 +int32 INVALID_GOAL = -1 +int32 OLD_HEADER_TIMESTAMP = -2 +int32 PATH_TOLERANCE_VIOLATED = -3 +int32 GOAL_TOLERANCE_VIOLATED = -4 + +# A human-readable error description. +string error_string + +--- +std_msgs/Header header + +# The reference pose for the controller at the current time instance. +# This is distinct from the sample, which is retrieved at the next time +# instance. +geometry_msgs/Pose desired + +# The current end effector state. +geometry_msgs/Pose actual + +# The squared Euclidean norm of the geodesic distance between the desired +# and actual states. +float64 error diff --git a/auv_control_msgs/msg/EndEffectorTrajectory.msg b/auv_control_msgs/msg/EndEffectorTrajectory.msg new file mode 100644 index 0000000..114b86a --- /dev/null +++ b/auv_control_msgs/msg/EndEffectorTrajectory.msg @@ -0,0 +1,4 @@ +std_msgs/Header header + +# The sequence of end effector points to track. +auv_control_msgs/EndEffectorTrajectoryPoint[] points diff --git a/auv_control_msgs/msg/EndEffectorTrajectoryControllerState.msg b/auv_control_msgs/msg/EndEffectorTrajectoryControllerState.msg new file mode 100644 index 0000000..7410871 --- /dev/null +++ b/auv_control_msgs/msg/EndEffectorTrajectoryControllerState.msg @@ -0,0 +1,14 @@ +std_msgs/Header header + +# The reference sample from the trajectory. This is the sample from the current time instance and used +# for error calculation. +geometry_msgs/Pose reference + +# The current end effector state. +geometry_msgs/Pose feedback + +# The squared Euclidean norm of the geodesic distance between the reference state and end effector state. +float64 error + +# The trajectory controller command. This is the trajectory sample from the next time instance. +geometry_msgs/Pose output diff --git a/auv_control_msgs/msg/EndEffectorTrajectoryPoint.msg b/auv_control_msgs/msg/EndEffectorTrajectoryPoint.msg new file mode 100644 index 0000000..dbca6e6 --- /dev/null +++ b/auv_control_msgs/msg/EndEffectorTrajectoryPoint.msg @@ -0,0 +1,5 @@ +# The sequence of end effector poses. +geometry_msgs/Pose point + +# The time that this point should be reached, measured from the start of the trajectory. +builtin_interfaces/Duration time_from_start diff --git a/auv_control_msgs/msg/IKControllerStateStamped.msg b/auv_control_msgs/msg/IKControllerStateStamped.msg new file mode 100644 index 0000000..3db2b1d --- /dev/null +++ b/auv_control_msgs/msg/IKControllerStateStamped.msg @@ -0,0 +1,20 @@ +std_msgs/Header header + +# The name of the IK solver used by the controller. +string solver_name + +# Position DoF names, e.g., joint or axis names. +string[] position_joint_names + +# Velocity DoF names. +string[] velocity_joint_names + +# The reference end effector pose. +geometry_msgs/Pose reference + +# Time between two consecutive updates/execution of the control law. +# This is often used for integration of the solution to determine the desired positions. +float64 time_step + +# The IK solver solution. +trajectory_msgs/JointTrajectoryPoint solution diff --git a/auv_control_msgs/package.xml b/auv_control_msgs/package.xml index 5ee32cb..d2f1680 100644 --- a/auv_control_msgs/package.xml +++ b/auv_control_msgs/package.xml @@ -2,7 +2,11 @@ auv_control_msgs +<<<<<<< HEAD 0.0.1 +======= + 0.2.0 +>>>>>>> d197fc7 (Port end effector trajectory controller from Angler (#54)) Custom messages for AUV controllers Rakesh Vivekanandan diff --git a/auv_controllers/CHANGELOG.md b/auv_controllers/CHANGELOG.md new file mode 100644 index 0000000..48279b9 --- /dev/null +++ b/auv_controllers/CHANGELOG.md @@ -0,0 +1,15 @@ +# Changelog for package auv_controllers + +## 0.2.0 (2025-05-03) + +- Adds the end effector trajectory controller + +## 0.1.0 (2025-04-27) + +- Adds the adaptive integral terminal sliding mode controller +- Adds the task priority IK solver +- Adds the IK whole-body controller +- Adds the odom topic sensor +- Adds the controller_common package +- Adds the Gazebo passthrough thruster controller +- Adds the thruster rotation rate controller diff --git a/auv_controllers/package.xml b/auv_controllers/package.xml index 53a9e8f..25ffbeb 100644 --- a/auv_controllers/package.xml +++ b/auv_controllers/package.xml @@ -3,7 +3,11 @@ auv_controllers +<<<<<<< HEAD 0.0.1 +======= + 0.2.0 +>>>>>>> d197fc7 (Port end effector trajectory controller from Angler (#54)) Meta package for auv_controllers Evan Palmer diff --git a/controller_common/CHANGELOG.md b/controller_common/CHANGELOG.md new file mode 100644 index 0000000..63dadc7 --- /dev/null +++ b/controller_common/CHANGELOG.md @@ -0,0 +1,9 @@ +# Changelog for package controller_common + +## 0.2.0 (2025-05-03) + +- Adds the common::math::isclose method for comparing doubles + +## 0.1.0 (2025-04-27) + +- Ports reset message functions and error calculation to a common API diff --git a/controller_common/include/controller_common/common.hpp b/controller_common/include/controller_common/common.hpp new file mode 100644 index 0000000..4da7d36 --- /dev/null +++ b/controller_common/include/controller_common/common.hpp @@ -0,0 +1,71 @@ +// Copyright 2025, Evan Palmer +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#pragma once + +#include + +#include "geometry_msgs/msg/pose.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/wrench.hpp" +#include "nav_msgs/msg/odometry.hpp" + +namespace common +{ + +namespace messages +{ + +auto to_vector(const geometry_msgs::msg::Pose & pose) -> std::vector; + +auto to_vector(const geometry_msgs::msg::Twist & twist) -> std::vector; + +auto to_vector(const geometry_msgs::msg::Wrench & wrench) -> std::vector; + +auto to_vector(const nav_msgs::msg::Odometry & odom) -> 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 reset_message(geometry_msgs::msg::Pose * msg) -> void; + +auto reset_message(geometry_msgs::msg::Twist * msg) -> void; + +auto reset_message(geometry_msgs::msg::Wrench * msg) -> void; + +auto reset_message(nav_msgs::msg::Odometry * msg) -> void; + +} // namespace messages + +namespace math +{ + +auto calculate_error(const std::vector & reference, const std::vector & state) -> std::vector; + +auto has_nan(const std::vector & vec) -> bool; + +auto all_nan(const std::vector & vec) -> bool; + +auto isclose(double a, double b, double rtol = 1e-05, double atol = 1e-08) -> bool; + +} // namespace math + +} // namespace common diff --git a/controller_common/package.xml b/controller_common/package.xml new file mode 100644 index 0000000..9122575 --- /dev/null +++ b/controller_common/package.xml @@ -0,0 +1,26 @@ + + + + + controller_common + 0.2.0 + Common interfaces for controllers used in this project + + 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 + + rclcpp + geometry_msgs + nav_msgs + + + ament_cmake + + diff --git a/controller_common/src/common.cpp b/controller_common/src/common.cpp new file mode 100644 index 0000000..c781378 --- /dev/null +++ b/controller_common/src/common.cpp @@ -0,0 +1,181 @@ +// 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 "controller_common/common.hpp" + +#include +#include +#include + +namespace common +{ + +namespace +{ + +auto nan = std::numeric_limits::quiet_NaN(); + +} // namespace + +namespace messages +{ + +auto to_vector(const geometry_msgs::msg::Pose & pose) -> std::vector +{ + return { + pose.position.x, + pose.position.y, + pose.position.z, + pose.orientation.x, + pose.orientation.y, + pose.orientation.z, + pose.orientation.w}; +} + +auto to_vector(const geometry_msgs::msg::Twist & twist) -> std::vector +{ + return {twist.linear.x, twist.linear.y, twist.linear.z, twist.angular.x, twist.angular.y, twist.angular.z}; +} + +auto to_vector(const geometry_msgs::msg::Wrench & wrench) -> std::vector +{ + return {wrench.force.x, wrench.force.y, wrench.force.z, wrench.torque.x, wrench.torque.y, wrench.torque.z}; +} + +auto to_vector(const nav_msgs::msg::Odometry & odom) -> std::vector +{ + return { + odom.pose.pose.position.x, + odom.pose.pose.position.y, + odom.pose.pose.position.z, + odom.pose.pose.orientation.x, + odom.pose.pose.orientation.y, + odom.pose.pose.orientation.z, + odom.pose.pose.orientation.w, + odom.twist.twist.linear.x, + odom.twist.twist.linear.y, + odom.twist.twist.linear.z, + odom.twist.twist.angular.x, + odom.twist.twist.angular.y, + odom.twist.twist.angular.z}; +} + +auto to_msg(const std::vector & data, geometry_msgs::msg::Pose * msg) -> void +{ + msg->position.x = data[0]; + msg->position.y = data[1]; + msg->position.z = data[2]; + msg->orientation.x = data[3]; + msg->orientation.y = data[4]; + msg->orientation.z = data[5]; + msg->orientation.w = data[6]; +} + +auto to_msg(const std::vector & data, geometry_msgs::msg::Twist * msg) -> void +{ + msg->linear.x = data[0]; + msg->linear.y = data[1]; + msg->linear.z = data[2]; + msg->angular.x = data[3]; + msg->angular.y = data[4]; + msg->angular.z = data[5]; +} + +auto reset_message(geometry_msgs::msg::Pose * msg) -> void +{ + msg->position.x = nan; + msg->position.y = nan; + msg->position.z = nan; + msg->orientation.x = nan; + msg->orientation.y = nan; + msg->orientation.z = nan; + msg->orientation.w = nan; +} + +auto reset_message(geometry_msgs::msg::Twist * msg) -> void +{ + msg->linear.x = nan; + msg->linear.y = nan; + msg->linear.z = nan; + msg->angular.x = nan; + msg->angular.y = nan; + msg->angular.z = nan; +} + +auto reset_message(geometry_msgs::msg::Wrench * msg) -> void +{ + msg->force.x = nan; + msg->force.y = nan; + msg->force.z = nan; + msg->torque.x = nan; + msg->torque.y = nan; + msg->torque.z = nan; +} + +auto reset_message(nav_msgs::msg::Odometry * msg) -> void +{ + msg->pose.pose.position.x = nan; + msg->pose.pose.position.y = nan; + msg->pose.pose.position.z = nan; + msg->pose.pose.orientation.x = nan; + msg->pose.pose.orientation.y = nan; + msg->pose.pose.orientation.z = nan; + msg->pose.pose.orientation.w = nan; + msg->twist.twist.linear.x = nan; + msg->twist.twist.linear.y = nan; + msg->twist.twist.linear.z = nan; + msg->twist.twist.angular.x = nan; + msg->twist.twist.angular.y = nan; + msg->twist.twist.angular.z = nan; +} + +} // namespace messages + +namespace math +{ + +auto calculate_error(const std::vector & reference, const std::vector & state) -> std::vector +{ + std::vector error; + error.reserve(reference.size()); + std::ranges::transform(reference, state, std::back_inserter(error), [](double ref, double state) { // NOLINT + return !std::isnan(ref) && !std::isnan(state) ? ref - state : std::numeric_limits::quiet_NaN(); + }); + return error; +} + +auto has_nan(const std::vector & vec) -> bool +{ + return std::ranges::any_of(vec, [](double x) { return std::isnan(x); }); +} + +auto all_nan(const std::vector & vec) -> bool +{ + return std::ranges::all_of(vec, [](double x) { return std::isnan(x); }); +} + +auto isclose(double a, double b, double rtol, double atol) -> bool +{ + return std::abs(a - b) <= (atol + rtol * std::abs(b)); +} + +} // namespace math + +} // namespace common diff --git a/end_effector_trajectory_controller/CMakeLists.txt b/end_effector_trajectory_controller/CMakeLists.txt new file mode 100644 index 0000000..ae73f59 --- /dev/null +++ b/end_effector_trajectory_controller/CMakeLists.txt @@ -0,0 +1,96 @@ +cmake_minimum_required(VERSION 3.23) +project(end_effector_trajectory_controller) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +include(GNUInstallDirs) + +find_package(ament_cmake REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(realtime_tools REQUIRED) +find_package(auv_control_msgs REQUIRED) +find_package(controller_common REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2_eigen REQUIRED) +find_package(hardware_interface REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(generate_parameter_library REQUIRED) +find_package(controller_interface REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(lifecycle_msgs REQUIRED) +find_package(pluginlib REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) + +generate_parameter_library(end_effector_trajectory_controller_parameters + src/end_effector_trajectory_controller_parameters.yaml +) + +add_library(end_effector_trajectory_controller SHARED) +target_sources( + end_effector_trajectory_controller + PRIVATE src/end_effector_trajectory_controller.cpp src/trajectory.cpp + PUBLIC + FILE_SET HEADERS + BASE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include + FILES + ${CMAKE_CURRENT_SOURCE_DIR}/include/end_effector_trajectory_controller/end_effector_trajectory_controller.hpp + ${CMAKE_CURRENT_SOURCE_DIR}/include/end_effector_trajectory_controller/trajectory.hpp +) +target_compile_features(end_effector_trajectory_controller PUBLIC cxx_std_23) +target_link_libraries( + end_effector_trajectory_controller + PUBLIC + end_effector_trajectory_controller_parameters + realtime_tools::realtime_tools + controller_common::controller_common + hardware_interface::hardware_interface + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + tf2_ros::tf2_ros + tf2_eigen::tf2_eigen + tf2::tf2 + controller_interface::controller_interface + rclcpp_action::rclcpp_action + tf2_geometry_msgs::tf2_geometry_msgs + ${geometry_msgs_TARGETS} + ${auv_control_msgs_TARGETS} + ${lifecycle_msgs_TARGETS} +) + +pluginlib_export_plugin_description_file(controller_interface end_effector_trajectory_controller.xml) + +install( + TARGETS + end_effector_trajectory_controller + end_effector_trajectory_controller_parameters + EXPORT export_end_effector_trajectory_controller + LIBRARY DESTINATION lib/${PROJECT_NAME} + ARCHIVE DESTINATION lib/${PROJECT_NAME} + RUNTIME DESTINATION bin/${PROJECT_NAME} + FILE_SET HEADERS +) + +ament_export_targets(export_end_effector_trajectory_controller HAS_LIBRARY_TARGET) +ament_export_dependencies( + "geometry_msgs" + "realtime_tools" + "auv_control_msgs" + "controller_common" + "tf2" + "tf2_ros" + "tf2_eigen" + "hardware_interface" + "rclcpp" + "rclcpp_lifecycle" + "controller_interface" + "rclcpp_action" + "lifecycle_msgs" + "tf2_geometry_msgs" +) + +ament_package() diff --git a/end_effector_trajectory_controller/LICENSE b/end_effector_trajectory_controller/LICENSE new file mode 100644 index 0000000..30e8e2e --- /dev/null +++ b/end_effector_trajectory_controller/LICENSE @@ -0,0 +1,17 @@ +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. diff --git a/end_effector_trajectory_controller/README.md b/end_effector_trajectory_controller/README.md new file mode 100644 index 0000000..426bd81 --- /dev/null +++ b/end_effector_trajectory_controller/README.md @@ -0,0 +1,30 @@ +# End Effector Trajectory Controller + +The end effector trajectory controller interpolates an end effector motion plan +for whole-body inverse kinematic control. The positions are interpolated using +linear interpolation. The orientations are interpolated using spherical linear +interpolation. + +## Plugin Library + +end_effector_trajectory_controller/EndEffectorTrajectoryController + +## References + +The input to this controller is a sequence of end effector poses. + +## Commands + +The output of this controller is a sampled end effector pose. + +## Subscribers + +- end_effector_trajectory_controller/trajectory [auv_control_msgs::msg::EndEffectorTrajectory] + +## Action Servers + +- end_effector_trajectory_controller/follow_trajectory [auv_control_msgs::action::FollowEndEffectorTrajectory] + +## Publishers + +- end_effector_trajectory_controller/status [auv_control_msgs::msg::EndEffectorTrajectoryControllerState] diff --git a/end_effector_trajectory_controller/end_effector_trajectory_controller.xml b/end_effector_trajectory_controller/end_effector_trajectory_controller.xml new file mode 100644 index 0000000..9bf9cc0 --- /dev/null +++ b/end_effector_trajectory_controller/end_effector_trajectory_controller.xml @@ -0,0 +1,9 @@ + + + + End effector trajectory controller for UVMS whole-body control + + + diff --git a/end_effector_trajectory_controller/include/end_effector_trajectory_controller/end_effector_trajectory_controller.hpp b/end_effector_trajectory_controller/include/end_effector_trajectory_controller/end_effector_trajectory_controller.hpp new file mode 100644 index 0000000..933c91b --- /dev/null +++ b/end_effector_trajectory_controller/include/end_effector_trajectory_controller/end_effector_trajectory_controller.hpp @@ -0,0 +1,139 @@ +// Copyright 2025, Evan Palmer +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#pragma once + +#include + +#include "auv_control_msgs/action/follow_end_effector_trajectory.hpp" +#include "auv_control_msgs/msg/end_effector_trajectory_controller_state.hpp" +#include "controller_common/common.hpp" +#include "controller_interface/controller_interface.hpp" +#include "end_effector_trajectory_controller/trajectory.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/server.hpp" +#include "realtime_tools/realtime_buffer.hpp" +#include "realtime_tools/realtime_publisher.hpp" +#include "realtime_tools/realtime_server_goal_handle.hpp" +#include "tf2/exceptions.h" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" + +// auto-generated by generate_parameter_library +#include + +namespace end_effector_trajectory_controller +{ + +class EndEffectorTrajectoryController : public controller_interface::ControllerInterface +{ +public: + EndEffectorTrajectoryController() = default; + + auto on_init() -> controller_interface::CallbackReturn override; + + // NOLINTNEXTLINE(modernize-use-nodiscard) + auto command_interface_configuration() const -> controller_interface::InterfaceConfiguration override; + + // NOLINTNEXTLINE(modernize-use-nodiscard) + auto state_interface_configuration() const -> controller_interface::InterfaceConfiguration override; + + auto on_configure(const rclcpp_lifecycle::State & previous_state) -> controller_interface::CallbackReturn override; + + auto on_activate(const rclcpp_lifecycle::State & previous_state) -> controller_interface::CallbackReturn override; + + auto update(const rclcpp::Time & time, const rclcpp::Duration & period) -> controller_interface::return_type override; + +protected: + auto update_parameters() -> void; + + auto configure_parameters() -> controller_interface::CallbackReturn; + + auto update_end_effector_state() -> controller_interface::return_type; + + [[nodiscard]] auto validate_trajectory(const auv_control_msgs::msg::EndEffectorTrajectory & trajectory) const -> bool; + + // controller state + using ControllerState = auv_control_msgs::msg::EndEffectorTrajectoryControllerState; + std::shared_ptr> controller_state_pub_; + std::unique_ptr> rt_controller_state_pub_; + + // the end effector states can be captured in one of three ways: + // 1. using the topic interface - when available, this is preferred over the tf2 interface + // 2. using the state interfaces - this is the default, but often not available + // 3. using tf2 - this is the most common interface, but requires a transform to be published and is not as robust + realtime_tools::RealtimeBuffer end_effector_state_; + std::shared_ptr> end_effector_state_sub_; + + std::unique_ptr tf_buffer_; + std::unique_ptr tf_listener_; + + // the end effector trajectories can be set using either the topic or action server + // the action server is preferred and easier to integrate into state-machines/behavior trees, but can be a bit + // cumbersome to interface with. on the other hand, the topic interface is easier to use, but doesn't integrate + // well with high-level coordinators + realtime_tools::RealtimeBuffer rt_trajectory_; + std::shared_ptr> trajectory_sub_; + + using FollowTrajectory = auv_control_msgs::action::FollowEndEffectorTrajectory; + using RealtimeGoalHandle = realtime_tools::RealtimeServerGoalHandle; + using RealtimeGoalHandlePtr = std::shared_ptr; + using RealtimeGoalHandleBuffer = realtime_tools::RealtimeBuffer; + + std::shared_ptr> action_server_; + RealtimeGoalHandleBuffer rt_active_goal_; + realtime_tools::RealtimeBuffer rt_goal_in_progress_; + std::shared_ptr goal_handle_timer_; + std::chrono::nanoseconds action_monitor_period_ = std::chrono::nanoseconds(50000000); // 50ms + + realtime_tools::RealtimeBuffer rt_first_sample_; // used to sample the trajectory for the first time + realtime_tools::RealtimeBuffer rt_holding_position_; // used to hold the current end effector pose + + // the update period is used to sample the "next" trajectory point + rclcpp::Duration update_period_{0, 0}; + + std::shared_ptr param_listener_; + end_effector_trajectory_controller::Params params_; + + // error tolerances + // the default tolerances are extracted from the parameters and applied when the action interface is not used + // if the action interface is being used, then the tolerances set in the goal are applied + double default_path_tolerance_, default_goal_tolerance_; + realtime_tools::RealtimeBuffer rt_goal_tolerance_, rt_path_tolerance_; + + std::vector dofs_; + std::size_t n_dofs_; + + rclcpp::Logger logger_{rclcpp::get_logger("end_effector_trajectory_controller")}; + +private: + template + auto write_command(T & interfaces, const geometry_msgs::msg::Pose & command) -> void + { + const std::vector vec = common::messages::to_vector(command); + for (const auto & [i, dof] : std::views::enumerate(dofs_)) { + if (!interfaces[i].set_value(vec[i])) { + RCLCPP_WARN(logger_, "Failed to set command for joint %s", dof.c_str()); // NOLINT + } + } + } +}; + +} // namespace end_effector_trajectory_controller diff --git a/end_effector_trajectory_controller/include/end_effector_trajectory_controller/trajectory.hpp b/end_effector_trajectory_controller/include/end_effector_trajectory_controller/trajectory.hpp new file mode 100644 index 0000000..485b640 --- /dev/null +++ b/end_effector_trajectory_controller/include/end_effector_trajectory_controller/trajectory.hpp @@ -0,0 +1,78 @@ +// Copyright 2025, Evan Palmer +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#pragma once + +#include +#include +#include +#include + +#include "auv_control_msgs/msg/end_effector_trajectory.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace end_effector_trajectory_controller +{ + +enum class SampleError : std::uint8_t +{ + SAMPLE_TIME_BEFORE_START, + SAMPLE_TIME_AFTER_END, + EMPTY_TRAJECTORY, +}; + +class Trajectory +{ +public: + Trajectory() = default; + + /// Create a new trajectory given a trajectory message and the initial state. + Trajectory(const auv_control_msgs::msg::EndEffectorTrajectory & trajectory, const geometry_msgs::msg::Pose & state); + + /// Whether or not the trajectory is empty. + [[nodiscard]] auto empty() const -> bool; + + /// Get the starting time of the trajectory. + [[nodiscard]] auto start_time() const -> rclcpp::Time; + + /// Get the ending time of the trajectory. + [[nodiscard]] auto end_time() const -> rclcpp::Time; + + /// Get the first point in the trajectory. + [[nodiscard]] auto start_point() const -> std::optional; + + /// Get the last point in the trajectory. + [[nodiscard]] auto end_point() const -> std::optional; + + /// Sample a point in the trajectory at the given time. + [[nodiscard]] auto sample(const rclcpp::Time & sample_time) const + -> std::expected; + + /// Reset the initial end effector state and trajectory start time. + auto reset_initial_state(const geometry_msgs::msg::Pose & state) -> void; + +private: + auv_control_msgs::msg::EndEffectorTrajectory points_; + rclcpp::Time initial_time_; + geometry_msgs::msg::Pose initial_state_; +}; + +} // namespace end_effector_trajectory_controller diff --git a/end_effector_trajectory_controller/package.xml b/end_effector_trajectory_controller/package.xml new file mode 100644 index 0000000..910998f --- /dev/null +++ b/end_effector_trajectory_controller/package.xml @@ -0,0 +1,37 @@ + + + + + end_effector_trajectory_controller + 0.2.0 + End effector trajectory tracking controller for UVMS control + + Evan Palmer + MIT + + https://github.com/Robotic-Decision-Making-Lab/auv_controllers.git + https://github.com/Robotic-Decision-Making-Lab/auv_controllers/issues + + Evan Palmer + + ament_cmake + eigen3_cmake_module + + eigen + rclcpp + ros2_control + controller_interface + hardware_interface + rclcpp_lifecycle + generate_parameter_library + control_msgs + geometry_msgs + controller_common + auv_control_msgs + rclcpp_action + lifecycle_msgs + + + ament_cmake + + diff --git a/end_effector_trajectory_controller/src/end_effector_trajectory_controller.cpp b/end_effector_trajectory_controller/src/end_effector_trajectory_controller.cpp new file mode 100644 index 0000000..7af5d84 --- /dev/null +++ b/end_effector_trajectory_controller/src/end_effector_trajectory_controller.cpp @@ -0,0 +1,468 @@ +// Copyright 2025, Evan Palmer +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include "end_effector_trajectory_controller/end_effector_trajectory_controller.hpp" + +#include +#include + +#include "controller_common/common.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "tf2_eigen/tf2_eigen.hpp" + +namespace end_effector_trajectory_controller +{ + +namespace +{ + +auto geodesic_error(const geometry_msgs::msg::Pose & goal, const geometry_msgs::msg::Pose & state) -> double +{ + Eigen::Isometry3d goal_mat, state_mat; // NOLINT + tf2::fromMsg(goal, goal_mat); + tf2::fromMsg(state, state_mat); + return std::pow((goal_mat.inverse() * state_mat).matrix().log().norm(), 2); +} + +} // namespace + +auto EndEffectorTrajectoryController::on_init() -> controller_interface::CallbackReturn +{ + param_listener_ = std::make_shared(get_node()); + params_ = param_listener_->get_params(); + logger_ = get_node()->get_logger(); + return controller_interface::CallbackReturn::SUCCESS; +} + +// NOLINTNEXTLINE(readability-convert-member-functions-to-static) +auto EndEffectorTrajectoryController::update_parameters() -> void +{ + if (!param_listener_->is_old(params_)) { + return; + } + param_listener_->refresh_dynamic_parameters(); + params_ = param_listener_->get_params(); +} + +auto EndEffectorTrajectoryController::configure_parameters() -> controller_interface::CallbackReturn +{ + update_parameters(); + + dofs_ = params_.joints; + n_dofs_ = dofs_.size(); + + default_path_tolerance_ = params_.path_tolerance; + default_goal_tolerance_ = params_.goal_tolerance; + + rt_path_tolerance_.writeFromNonRT(params_.path_tolerance); + rt_goal_tolerance_.writeFromNonRT(params_.goal_tolerance); + + auto period = std::chrono::duration(1.0 / params_.action_monitor_rate); + action_monitor_period_ = std::chrono::duration_cast(period); + + return controller_interface::CallbackReturn::SUCCESS; +} + +auto EndEffectorTrajectoryController::validate_trajectory( + const auv_control_msgs::msg::EndEffectorTrajectory & trajectory) const -> bool +{ + if (trajectory.points.empty()) { + RCLCPP_ERROR(logger_, "Received empty trajectory"); // NOLINT + return false; + } + + for (const auto & point : trajectory.points) { + if (common::math::has_nan(common::messages::to_vector(point.point))) { + RCLCPP_ERROR(logger_, "Received trajectory point with NaN value"); // NOLINT + return false; + } + } + + const rclcpp::Time start_time = trajectory.header.stamp; + if (start_time.seconds() != 0.0) { + const rclcpp::Time end_time = start_time + trajectory.points.back().time_from_start; + if (end_time < get_node()->now()) { + RCLCPP_ERROR(logger_, "Received trajectory with end time in the past"); // NOLINT + return false; + } + } + + // NOLINTNEXTLINE(readability-use-anyofallof) + for (const auto [p1, p2] : std::views::zip(trajectory.points, trajectory.points | std::views::drop(1))) { + const rclcpp::Duration p1_start = p1.time_from_start; + const rclcpp::Duration p2_start = p2.time_from_start; + if (p1_start >= p2_start) { + RCLCPP_ERROR(logger_, "Trajectory points are not in order"); // NOLINT + return false; + } + } + + return true; +} + +auto EndEffectorTrajectoryController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) + -> controller_interface::CallbackReturn +{ + configure_parameters(); + + end_effector_state_.writeFromNonRT(geometry_msgs::msg::Pose()); + command_interfaces_.reserve(n_dofs_); + update_period_ = rclcpp::Duration(0.0, static_cast(1.0e9 / static_cast(get_update_rate()))); + + if (params_.use_external_measured_states) { + end_effector_state_sub_ = get_node()->create_subscription( + "~/end_effector_state", + rclcpp::SystemDefaultsQoS(), + [this](const std::shared_ptr msg) { // NOLINT + end_effector_state_.writeFromNonRT(*msg); + }); + } else { + if (params_.lookup_end_effector_transform) { + tf_buffer_ = std::make_unique(get_node()->get_clock()); + tf_listener_ = std::make_unique(*tf_buffer_); + } + } + + trajectory_sub_ = get_node()->create_subscription( + "~/trajectory", + rclcpp::SystemDefaultsQoS(), + [this](const std::shared_ptr msg) { // NOLINT + auto updated_msg = *msg; + if (!validate_trajectory(updated_msg)) { + RCLCPP_ERROR(logger_, "Ignoring invalid trajectory message"); // NOLINT + return; + } + const rclcpp::Time start_time = updated_msg.header.stamp; + if (common::math::isclose(start_time.seconds(), 0.0)) { + updated_msg.header.stamp = get_node()->now(); + } + RCLCPP_INFO(logger_, "Received new trajectory message"); // NOLINT + rt_trajectory_.writeFromNonRT(Trajectory(updated_msg, *end_effector_state_.readFromNonRT())); + rt_goal_tolerance_.writeFromNonRT(default_goal_tolerance_); + rt_path_tolerance_.writeFromNonRT(default_path_tolerance_); + rt_first_sample_.writeFromNonRT(true); + rt_holding_position_.writeFromNonRT(false); + }); + + auto handle_goal = [this]( + const rclcpp_action::GoalUUID & /*uuid*/, + std::shared_ptr goal) { // NOLINT + RCLCPP_INFO(logger_, "Received new trajectory goal"); // NOLINT + if (get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { + RCLCPP_ERROR(logger_, "Can't accept new action goals. Controller is not running."); // NOLINT + return rclcpp_action::GoalResponse::REJECT; + } + auto updated_msg = goal->trajectory; // make a non-const copy + if (!validate_trajectory(updated_msg)) { + RCLCPP_ERROR(logger_, "Ignoring invalid trajectory message"); // NOLINT + return rclcpp_action::GoalResponse::REJECT; + } + const rclcpp::Time start_time = updated_msg.header.stamp; + if (common::math::isclose(start_time.seconds(), 0.0)) { + updated_msg.header.stamp = get_node()->now(); + } + rt_trajectory_.writeFromNonRT(Trajectory(updated_msg, *end_effector_state_.readFromNonRT())); + rt_first_sample_.writeFromNonRT(true); + rt_holding_position_.writeFromNonRT(false); + RCLCPP_INFO(logger_, "Accepted new trajectory goal"); // NOLINT + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; + + auto handle_cancel = [this](const std::shared_ptr> gh) { // NOLINT + RCLCPP_INFO(logger_, "Received cancel action goal"); // NOLINT + const auto active_goal = *rt_active_goal_.readFromNonRT(); + if (active_goal && active_goal->gh_ == gh) { + RCLCPP_INFO(logger_, "Canceling active goal"); // NOLINT + auto action_result = std::make_shared(); + active_goal->setCanceled(action_result); + rt_holding_position_.writeFromNonRT(true); + rt_first_sample_.writeFromNonRT(true); + rt_goal_in_progress_.writeFromNonRT(false); + } + return rclcpp_action::CancelResponse::ACCEPT; + }; + + auto handle_accepted = [this](std::shared_ptr> gh) { // NOLINT + RCLCPP_INFO(logger_, "Received accepted action goal"); // NOLINT + rt_goal_in_progress_.writeFromNonRT(true); + const auto active_goal = *rt_active_goal_.readFromNonRT(); + if (active_goal) { + RCLCPP_INFO(logger_, "Canceling active goal"); // NOLINT + auto action_result = std::make_shared(); + action_result->error_code = FollowTrajectory::Result::INVALID_GOAL; + action_result->error_string = "Current goal cancelled by a new incoming action."; + active_goal->setCanceled(action_result); + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + } + + rt_goal_tolerance_.writeFromNonRT(gh->get_goal()->goal_tolerance); + rt_path_tolerance_.writeFromNonRT(gh->get_goal()->path_tolerance); + + const RealtimeGoalHandlePtr rt_gh = std::make_shared(gh); + rt_gh->execute(); + rt_active_goal_.writeFromNonRT(rt_gh); + + goal_handle_timer_.reset(); + goal_handle_timer_ = get_node()->create_wall_timer(action_monitor_period_, [rt_gh]() { rt_gh->runNonRealtime(); }); + }; + + action_server_ = rclcpp_action::create_server( + get_node(), "~/follow_trajectory", handle_goal, handle_cancel, handle_accepted); + + controller_state_pub_ = get_node()->create_publisher("~/status", rclcpp::SystemDefaultsQoS()); + rt_controller_state_pub_ = + std::make_unique>(controller_state_pub_); + + return controller_interface::CallbackReturn::SUCCESS; +} + +auto EndEffectorTrajectoryController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) + -> controller_interface::CallbackReturn +{ + rt_first_sample_.writeFromNonRT(true); + rt_holding_position_.writeFromNonRT(true); // hold position until a trajectory is received + common::messages::reset_message(end_effector_state_.readFromNonRT()); + return controller_interface::CallbackReturn::SUCCESS; +} + +auto EndEffectorTrajectoryController::command_interface_configuration() const + -> controller_interface::InterfaceConfiguration +{ + controller_interface::InterfaceConfiguration config; + config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + config.names.reserve(n_dofs_); + + std::ranges::transform(dofs_, std::back_inserter(config.names), [this](const auto & dof) { + return params_.reference_controller.empty() + ? std::format("{}/{}", dof, hardware_interface::HW_IF_POSITION) + : std::format("{}/{}/{}", params_.reference_controller, dof, hardware_interface::HW_IF_POSITION); + }); + + return config; +} + +auto EndEffectorTrajectoryController::state_interface_configuration() const + -> controller_interface::InterfaceConfiguration +{ + controller_interface::InterfaceConfiguration config; + if (params_.use_external_measured_states || params_.lookup_end_effector_transform) { + config.type = controller_interface::interface_configuration_type::NONE; + return config; + } + config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + config.names.reserve(n_dofs_); + + std::ranges::transform(dofs_, std::back_inserter(config.names), [](const auto & dof) { + return std::format("{}/{}", dof, hardware_interface::HW_IF_POSITION); + }); + + return config; +} + +auto EndEffectorTrajectoryController::update_end_effector_state() -> controller_interface::return_type +{ + auto * end_effector_state = end_effector_state_.readFromRT(); + if (params_.lookup_end_effector_transform) { + const auto to_frame = params_.end_effector_frame_id; + const auto from_frame = params_.odom_frame_id; + try { + const auto transform = tf_buffer_->lookupTransform(from_frame, to_frame, tf2::TimePointZero); + end_effector_state->position.x = transform.transform.translation.x; + end_effector_state->position.y = transform.transform.translation.y; + end_effector_state->position.z = transform.transform.translation.z; + end_effector_state->orientation = transform.transform.rotation; + } + catch (const tf2::TransformException & ex) { + const auto err = std::format("Failed to get transform from {} to {}: {}", from_frame, to_frame, ex.what()); + RCLCPP_DEBUG(logger_, err.c_str()); // NOLINT + return controller_interface::return_type::ERROR; + } + } else if (!params_.use_external_measured_states) { + auto get_value = [](const auto & interface) -> double { + return interface.get_optional().value_or(std::numeric_limits::quiet_NaN()); + }; + end_effector_state->position.x = get_value(state_interfaces_[0]); + end_effector_state->position.y = get_value(state_interfaces_[1]); + end_effector_state->position.z = get_value(state_interfaces_[2]); + end_effector_state->orientation.x = get_value(state_interfaces_[3]); + end_effector_state->orientation.y = get_value(state_interfaces_[4]); + end_effector_state->orientation.z = get_value(state_interfaces_[5]); + end_effector_state->orientation.w = get_value(state_interfaces_[6]); + } + + if (common::math::has_nan(common::messages::to_vector(*end_effector_state))) { + RCLCPP_DEBUG(logger_, "Received end effector state with NaN value."); // NOLINT + return controller_interface::return_type::ERROR; + } + return controller_interface::return_type::OK; +} + +auto EndEffectorTrajectoryController::update(const rclcpp::Time & time, const rclcpp::Duration & period) + -> controller_interface::return_type +{ + if (update_end_effector_state() != controller_interface::return_type::OK) { + RCLCPP_DEBUG(logger_, "Skipping controller update. Failed to update and validate interfaces"); // NOLINT + return controller_interface::return_type::OK; + } + + const auto active_goal = *rt_active_goal_.readFromRT(); + const bool goal_in_progress = *rt_goal_in_progress_.readFromRT(); + if (goal_in_progress && active_goal == nullptr) { + RCLCPP_DEBUG(logger_, "Goal in progress but no active goal. Ignoring update"); // NOLINT + return controller_interface::return_type::OK; + } + + const geometry_msgs::msg::Pose end_effector_state = *end_effector_state_.readFromRT(); + geometry_msgs::msg::Pose reference_state; + common::messages::reset_message(&reference_state); + auto command_state = end_effector_state; + double error = std::numeric_limits::quiet_NaN(); + + auto publish_controller_state = [this, &reference_state, &end_effector_state, &error, &command_state]() { + if (rt_controller_state_pub_ && rt_controller_state_pub_->trylock()) { + rt_controller_state_pub_->msg_.header.stamp = get_node()->now(); + rt_controller_state_pub_->msg_.reference = reference_state; + rt_controller_state_pub_->msg_.feedback = end_effector_state; + rt_controller_state_pub_->msg_.error = error; + rt_controller_state_pub_->msg_.output = command_state; + rt_controller_state_pub_->unlockAndPublish(); + } + }; + + // hold position until a new trajectory is received + if (*rt_holding_position_.readFromRT()) { + write_command(command_interfaces_, end_effector_state); + publish_controller_state(); + return controller_interface::return_type::OK; + } + + // set the sample time + rclcpp::Time sample_time = time; + if (*rt_first_sample_.readFromRT()) { + rt_trajectory_.readFromRT()->reset_initial_state(end_effector_state); + rt_first_sample_.writeFromNonRT(false); + sample_time += period; + } + + // we use the current sample to measure errors and the future sample as the command + // the future sample should be used in order to prevent the controller from lagging + const auto * trajectory = rt_trajectory_.readFromRT(); + const auto sampled_reference = trajectory->sample(sample_time); + const auto sampled_command = trajectory->sample(sample_time + update_period_); + + // get the reference state and error + // the scenarios where this will not have a value are when the reference time is before or after the trajectory + if (sampled_reference.has_value()) { + reference_state = sampled_reference.value(); + error = geodesic_error(reference_state, end_effector_state); + } + + bool path_tolerance_exceeded = false; + bool goal_tolerance_exceeded = false; + bool trajectory_suceeded = false; + + if (sampled_command.has_value()) { + command_state = sampled_command.value(); + if (!std::isnan(error)) { + const double path_tolerance = *rt_path_tolerance_.readFromRT(); + if (path_tolerance > 0.0 && error > path_tolerance) { + path_tolerance_exceeded = true; + rt_holding_position_.writeFromNonRT(true); + command_state = end_effector_state; + RCLCPP_WARN(logger_, "Aborting trajectory. Error threshold exceeded during execution: %f", error); // NOLINT + RCLCPP_INFO(logger_, "Holding position until a new trajectory is received"); // NOLINT + } + } + } else { + switch (sampled_command.error()) { + case SampleError::SAMPLE_TIME_BEFORE_START: + RCLCPP_INFO(logger_, "Trajectory sample time is before trajectory start time"); // NOLINT + RCLCPP_INFO(logger_, "Holding position until the trajectory starts"); // NOLINT + break; + + case SampleError::SAMPLE_TIME_AFTER_END: { + const double goal_tolerance = *rt_goal_tolerance_.readFromRT(); + const double goal_error = geodesic_error(trajectory->end_point().value(), end_effector_state); + RCLCPP_INFO(logger_, "Trajectory sample time is after trajectory end time."); // NOLINT + if (goal_tolerance > 0.0) { + if (goal_error > goal_tolerance) { + goal_tolerance_exceeded = true; + RCLCPP_WARN(logger_, "Aborting trajectory. Terminal error exceeded threshold: %f", goal_error); // NOLINT + } else { + trajectory_suceeded = true; + RCLCPP_INFO(logger_, "Trajectory execution completed successfully"); // NOLINT + } + } + rt_holding_position_.writeFromNonRT(true); + RCLCPP_INFO(logger_, "Holding position until a new trajectory is received"); // NOLINT + } break; + + default: + // default to position hold + rt_holding_position_.writeFromNonRT(true); + break; + } + } + + if (active_goal) { + // write feedback to the action server + auto feedback = std::make_shared(); + feedback->header.stamp = time; + feedback->desired = reference_state; + feedback->actual = end_effector_state; + feedback->error = error; + active_goal->setFeedback(feedback); + + // check terminal conditions + if (goal_tolerance_exceeded) { + auto action_result = std::make_shared(); + action_result->error_code = FollowTrajectory::Result::PATH_TOLERANCE_VIOLATED; + action_result->error_string = "Trajectory execution aborted. Goal tolerance exceeded."; + active_goal->setAborted(action_result); + rt_holding_position_.writeFromNonRT(true); + } else if (trajectory_suceeded) { + auto action_result = std::make_shared(); + action_result->error_code = FollowTrajectory::Result::SUCCESSFUL; + action_result->error_string = "Trajectory execution completed successfully!"; + active_goal->setSucceeded(action_result); + rt_holding_position_.writeFromNonRT(true); + } else if (path_tolerance_exceeded) { + auto action_result = std::make_shared(); + action_result->error_code = FollowTrajectory::Result::PATH_TOLERANCE_VIOLATED; + action_result->error_string = "Trajectory execution aborted. Path tolerance exceeded."; + active_goal->setAborted(action_result); + rt_holding_position_.writeFromNonRT(true); + } + } + + write_command(command_interfaces_, command_state); + publish_controller_state(); + + return controller_interface::return_type::OK; +} + +} // namespace end_effector_trajectory_controller + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS( + end_effector_trajectory_controller::EndEffectorTrajectoryController, + controller_interface::ControllerInterface) diff --git a/end_effector_trajectory_controller/src/end_effector_trajectory_controller_parameters.yaml b/end_effector_trajectory_controller/src/end_effector_trajectory_controller_parameters.yaml new file mode 100644 index 0000000..b5d2308 --- /dev/null +++ b/end_effector_trajectory_controller/src/end_effector_trajectory_controller_parameters.yaml @@ -0,0 +1,93 @@ +end_effector_trajectory_controller: + joints: + type: string_array + default_value: ["x", "y", "z", "qx", "qy", "qz", "qw"] + read_only: true + description: > + The list of position joints to use for the end effector position + interfaces. This can be used to configure a prefix for the interfaces. + validation: + fixed_size<>: 7 # position + orientation (quaternion) + + path_tolerance: + type: double + default_value: 0.0 + read_only: true + description: > + The maximum error between the end effector pose and the reference + pose in the trajectory. + + The error threshold is measured in terms of the squared norm of the + geodesic distance between the end effector pose and the terminal pose in + the trajectory. + validation: + gt_eq<>: 0.0 + + goal_tolerance: + type: double + default_value: 0.0 + read_only: true + description: > + The maximum error between the end effector pose and the terminal + pose in the trajectory at the end of execution. + + The error threshold is measured in terms of the squared norm of the + geodesic distance between the end effector pose and the terminal pose in + the trajectory. + validation: + gt_eq<>: 0.0 + + use_external_measured_states: + type: bool + default_value: false + read_only: true + description: > + Use end effector states provided via a topic instead of state interfaces + or tf2. If both this and lookup_end_effector_transform are set to true, + the controller will use the end effector states provided via a topic. + + lookup_end_effector_transform: + type: bool + default_value: false + read_only: true + description: > + Use tf2 to look up the end effector transform instead of using state + interfaces or a topic. This is useful in scenarios where the end effector + state is determined only by using tf2 to perform forward kinematics. + + If both this and use_external_end_effector_states are set to true, the + controller will use the end effector states provided via a topic. + + reference_controller: + type: string + default_value: "" + read_only: true + description: > + The prefix of the reference controller to send commands to. This can be + used to configure command interfaces in chained mode. + + odom_frame_id: + type: string + default_value: odom_ned + read_only: true + description: > + The name of the inertial frame. This is used to lookup the end effector + pose when using lookup_end_effector_transform. + + end_effector_frame_id: + type: string + default_value: tcp + read_only: true + description: > + The name of the end effector frame. This is used to lookup the end + effector pose when using lookup_end_effector_transform. + + action_monitor_rate: + type: int + default_value: 20 + read_only: true + description: > + The rate (Hz) at which the action server will monitor the trajectory + execution. + validation: + gt<>: 0 diff --git a/end_effector_trajectory_controller/src/trajectory.cpp b/end_effector_trajectory_controller/src/trajectory.cpp new file mode 100644 index 0000000..297dbd3 --- /dev/null +++ b/end_effector_trajectory_controller/src/trajectory.cpp @@ -0,0 +1,165 @@ +// Copyright 2025, Evan Palmer +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include "end_effector_trajectory_controller/trajectory.hpp" + +#include +#include + +#include "controller_common/common.hpp" +#include "tf2_eigen/tf2_eigen.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +namespace end_effector_trajectory_controller +{ + +namespace +{ + +/// Linear interpolation between two positions. +auto lerp(double start, double end, double t) -> double { return start + (t * (end - start)); } + +/// Spherical linear interpolation between two quaternions. +/// +/// See the following for more information: +/// https://www.mathworks.com/help/fusion/ref/quaternion.slerp.html +auto slerp(Eigen::Quaterniond start, Eigen::Quaterniond end, double t) -> Eigen::Quaterniond +{ + Eigen::Quaterniond result; + + start.normalize(); + end.normalize(); + + const double dot = start.dot(end); + if (common::math::isclose(dot, 1.0, 1e-5, 5e-5)) { + // if the quaternions are very close, just linearly interpolate between them to avoid numerical instability + result.coeffs() = start.coeffs() + t * (end.coeffs() - start.coeffs()); + } else { + // otherwise, use spherical linear interpolation + const double theta = std::acos(dot); + const double coeff0 = std::sin((1 - t) * theta) / std::sin(theta); + const double coeff1 = std::sin(t * theta) / std::sin(theta); + result.coeffs() = (coeff0 * start.coeffs()) + (coeff1 * end.coeffs()); + } + + result.normalize(); + return result; +} + +auto interpolate( + const geometry_msgs::msg::Pose & start, + const geometry_msgs::msg::Pose & end, + const rclcpp::Time & t0, + const rclcpp::Time & t1, + const rclcpp::Time & sample_time) -> geometry_msgs::msg::Pose +{ + const rclcpp::Duration time_from_start = sample_time - t0; + const rclcpp::Duration time_between_points = t1 - t0; + const double t = time_from_start.seconds() / time_between_points.seconds(); + + // linearly interpolate between the positions + geometry_msgs::msg::Pose out; + out.position.x = lerp(start.position.x, end.position.x, t); + out.position.y = lerp(start.position.y, end.position.y, t); + out.position.z = lerp(start.position.z, end.position.z, t); + + Eigen::Quaterniond q1, q2; // NOLINT + tf2::fromMsg(start.orientation, q1); + tf2::fromMsg(end.orientation, q2); + + // spherical linear interpolation between the orientations + const Eigen::Quaterniond q_out = slerp(q1, q2, t); + out.orientation = tf2::toMsg(q_out); + + return out; +} + +} // namespace + +Trajectory::Trajectory( + const auv_control_msgs::msg::EndEffectorTrajectory & trajectory, + const geometry_msgs::msg::Pose & state) +: points_(trajectory), + initial_time_(static_cast(trajectory.header.stamp)), + initial_state_(state) +{ +} + +auto Trajectory::empty() const -> bool { return points_.points.empty(); } + +auto Trajectory::start_time() const -> rclcpp::Time +{ + return empty() ? rclcpp::Time(0) : initial_time_ + points_.points.front().time_from_start; +} + +auto Trajectory::end_time() const -> rclcpp::Time +{ + return empty() ? rclcpp::Time(0) : initial_time_ + points_.points.back().time_from_start; +} + +auto Trajectory::start_point() const -> std::optional +{ + if (empty()) { + return std::nullopt; + } + return points_.points.front().point; +} + +auto Trajectory::end_point() const -> std::optional +{ + if (empty()) { + return std::nullopt; + } + return points_.points.back().point; +} + +auto Trajectory::sample(const rclcpp::Time & sample_time) const -> std::expected +{ + if (empty()) { + return std::unexpected(SampleError::EMPTY_TRAJECTORY); + } + + // the sample time is before the timestamp in the trajectory header + if (sample_time < initial_time_) { + return std::unexpected(SampleError::SAMPLE_TIME_BEFORE_START); + } + + // the sample time is before the first point in the trajectory, so we need to interpolate between the starting + // state and the first point in the trajectory + if (sample_time < start_time()) { + return interpolate(initial_state_, start_point().value(), initial_time_, start_time(), sample_time); + } + + for (const auto [p1, p2] : std::views::zip(points_.points, points_.points | std::views::drop(1))) { + const rclcpp::Time t0 = initial_time_ + p1.time_from_start; + const rclcpp::Time t1 = initial_time_ + p2.time_from_start; + + if (sample_time >= t0 && sample_time <= t1) { + return interpolate(p1.point, p2.point, t0, t1, sample_time); + } + } + + // the whole trajectory has been sampled + return std::unexpected(SampleError::SAMPLE_TIME_AFTER_END); +} + +auto Trajectory::reset_initial_state(const geometry_msgs::msg::Pose & state) -> void { initial_state_ = state; } + +} // namespace end_effector_trajectory_controller diff --git a/ik_solvers/CHANGELOG.md b/ik_solvers/CHANGELOG.md new file mode 100644 index 0000000..8c9f764 --- /dev/null +++ b/ik_solvers/CHANGELOG.md @@ -0,0 +1,10 @@ +# Changelog for package ik_solvers + +## 0.2.0 (2025-05-03) + +- Replace instances of `Eigen::Affine3d` with `Eigen::Isometry3d` + +## 0.1.0 (2025-04-27) + +- Implements a task priority IK solver with support for end effector pose +tracking and joint limits diff --git a/ik_solvers/include/ik_solvers/solver.hpp b/ik_solvers/include/ik_solvers/solver.hpp new file mode 100644 index 0000000..d0afe1b --- /dev/null +++ b/ik_solvers/include/ik_solvers/solver.hpp @@ -0,0 +1,81 @@ +// 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 "pinocchio/algorithm/joint-configuration.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "trajectory_msgs/msg/joint_trajectory_point.hpp" + +namespace ik_solvers +{ + +/// Error codes for the inverse kinematics solvers. +enum class SolverError : std::uint8_t +{ + NO_SOLUTION, + SOLVER_ERROR +}; + +/// Base class for inverse kinematics solvers. +class IKSolver +{ +public: + /// Constructor. + IKSolver() = default; + + /// Destructor. + virtual ~IKSolver() = default; + + /// Initialize the solver. + virtual auto initialize( + const std::shared_ptr & node, + const std::shared_ptr & model, + const std::shared_ptr & data, + const std::string & prefix) -> void; + + /// Solve the IK problem for a target pose given the integration period and current joint configuration. + [[nodiscard]] auto solve(const rclcpp::Duration & period, const Eigen::Isometry3d & goal, const Eigen::VectorXd & q) + -> std::expected; + +protected: + /// Solve the IK problem for the given target pose and joint configuration. + /// + /// This is wrapped by the public API. The public API handles updating pinocchio for Jacobian calculation and converts + /// of the result into a `JointTrajectoryPoint`. This method only needs to compute the IK solution. + [[nodiscard]] virtual auto solve_ik(const Eigen::Isometry3d & goal, const Eigen::VectorXd & q) + -> std::expected = 0; + + std::shared_ptr node_; + + std::string ee_frame_; + + std::shared_ptr model_; + std::shared_ptr data_; + +private: + auto update_pinocchio(const Eigen::VectorXd & q) const -> void; +}; + +} // namespace ik_solvers diff --git a/ik_solvers/include/ik_solvers/task_priority_solver.hpp b/ik_solvers/include/ik_solvers/task_priority_solver.hpp new file mode 100644 index 0000000..866dd76 --- /dev/null +++ b/ik_solvers/include/ik_solvers/task_priority_solver.hpp @@ -0,0 +1,238 @@ +// 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 + +#include "ik_solvers/solver.hpp" +#include "rclcpp/rclcpp.hpp" + +// auto-generated by generate_parameter_library +#include + +namespace ik_solvers +{ + +namespace hierarchy +{ + +/// Base class for constraints. +class Constraint +{ +public: + /// Create a new constraint given the current primal value, constraint value, task priority and feedback gain. + Constraint(Eigen::MatrixXd primal, Eigen::MatrixXd constraint, int priority, double gain) + : primal_(std::move(primal)), + constraint_(std::move(constraint)), + priority_(priority), + gain_(gain) {}; + + /// Destructor. + virtual ~Constraint() = default; + + /// Get the primal value for the constraint. + [[nodiscard]] auto primal() const -> Eigen::MatrixXd { return primal_; }; + + /// Get the Jacobian for the constraint. + [[nodiscard]] auto jacobian() const -> Eigen::MatrixXd { return jacobian_; }; + + /// Get the error between the primal and constraint value. + [[nodiscard]] auto error() const -> Eigen::VectorXd { return error_; }; + + /// Get the constraint priority. + [[nodiscard]] auto priority() const -> int { return priority_; } + + /// Get the feedback gain. + [[nodiscard]] auto gain() const -> double { return gain_; } + +protected: + Eigen::MatrixXd primal_; + Eigen::MatrixXd constraint_; + Eigen::MatrixXd jacobian_; + Eigen::VectorXd error_; + + int priority_; + double gain_; +}; + +/// Base class for scalar set constraints. +class SetConstraint : public Constraint +{ +public: + /// Create a new set constraint given the current primal value, constraint value, constraint bounds, tolerance, + /// activation threshold, task priority,and gain. + SetConstraint(double primal, double ub, double lb, double tol, double activation, int priority, double gain) + : Constraint(Eigen::MatrixXd::Constant(1, 1, primal), Eigen::MatrixXd::Zero(1, 1), priority, gain), + upper_limit_(ub), + lower_limit_(lb), + upper_safety_(ub - tol), + lower_safety_(lb + tol), + upper_threshold_(ub - tol - activation), + lower_threshold_(lb + tol + activation) + { + // Set the constraint value based on whether or not the task is active + // The constraint value is set to the safety value if the task is active + if (primal < lower_threshold_ + std::numeric_limits::epsilon()) { + constraint_ = Eigen::MatrixXd::Constant(1, 1, lower_safety_); + } else if (primal > upper_threshold_ - std::numeric_limits::epsilon()) { + constraint_ = Eigen::MatrixXd::Constant(1, 1, upper_safety_); + } else { + constraint_ = primal_; + } + }; + + /// Check whether or not the constraint is active. Set constraints are only active when the current value is within + /// the activation threshold for the task. + [[nodiscard]] auto is_active() const -> bool + { + return primal_.value() < lower_threshold_ || primal_.value() > upper_threshold_; + }; + + /// Get the primal value for the constraint. + [[nodiscard]] auto primal() const -> double { return primal_.value(); }; + + /// Get the upper activation threshold for the constraint. + [[nodiscard]] auto upper_threshold() const -> double { return upper_threshold_; } + + /// Get the lower activation threshold for the constraint. + [[nodiscard]] auto lower_threshold() const -> double { return lower_threshold_; } + +protected: + double upper_limit_, lower_limit_; + double upper_safety_, lower_safety_; + double upper_threshold_, lower_threshold_; +}; + +/// Class used to manage an end effector pose constraint. +class PoseConstraint : public Constraint +{ +public: + /// Create a new end effector pose constraint given the system model and data, the primal and constraint poses, the + /// name of the end effector frame, and the feedback gain. + /// + /// The constraint priority is set to 2 by default. + PoseConstraint( + const std::shared_ptr & model, + const std::shared_ptr & data, + const Eigen::Isometry3d & primal, + const Eigen::Isometry3d & constraint, + const std::string & frame, + double gain, + int priority = 2); +}; + +/// Class used to manage a joint constraint. +class JointConstraint : public SetConstraint +{ +public: + /// Create a new joint constraint given the system model and data, the current joint value, the upper and lower bounds + /// for the joint, the tolerance, the activation threshold, the joint ID, and the feedback gain. + /// + /// The constraint priority is set to 1 by default. + JointConstraint( + const std::shared_ptr & model, + double primal, + double ub, + double lb, + double tol, + double activation, + const std::string & joint_name, + double gain, + int priority = 1); +}; + +/// Comparator used to sort constraints based on their priority. +/// If the priorities are equal, the constraints are sorted based on their memory address. +struct ConstraintCompare +{ + auto operator()(const std::shared_ptr & lhs, const std::shared_ptr & rhs) const -> bool + { + return lhs->priority() == rhs->priority() ? std::less<>{}(lhs, rhs) : lhs->priority() < rhs->priority(); + } +}; + +/// Type alias for a set of constraints. +using ConstraintSet = std::set, ConstraintCompare>; + +/// Class used to manage the task hierarchy. +class TaskHierarchy +{ +public: + /// Constructor. + TaskHierarchy() = default; + + /// Insert a new constraint into the hierarchy. This will automatically sort the constraints based on their priority. + /// + /// For now, only equality constraints and high priority set constraints are supported. + auto insert(const std::shared_ptr & constraint) -> void; + + /// Clear all constraints from the hierarchy. + auto clear() -> void; + + /// Get the set of all active inequality constraints in the hierarchy. + [[nodiscard]] auto set_constraints() const -> ConstraintSet; + + /// Get the set of all equality constraints in the hierarchy. + [[nodiscard]] auto equality_constraints() const -> ConstraintSet; + + /// Get the set of all potential hierarchies for the active tasks. + [[nodiscard]] auto hierarchies() const -> std::vector; + +private: + ConstraintSet constraints_; +}; + +} // namespace hierarchy + +class TaskPriorityIKSolver : public IKSolver +{ +public: + TaskPriorityIKSolver() = default; + + auto initialize( + const std::shared_ptr & node, + const std::shared_ptr & model, + const std::shared_ptr & data, + const std::string & prefix) -> void override; + +protected: + [[nodiscard]] auto solve_ik(const Eigen::Isometry3d & goal, const Eigen::VectorXd & q) + -> std::expected override; + + /// Update the dynamic ROS 2 parameters. + auto update_parameters() -> void; + + /// Configure the solver based on the current ROS 2 parameters. + auto configure_parameters() -> void; + + hierarchy::TaskHierarchy hierarchy_; + + double damping_{1e-6}; + + std::shared_ptr param_listener_; + task_priority_solver::Params params_; +}; + +} // namespace ik_solvers diff --git a/ik_solvers/package.xml b/ik_solvers/package.xml new file mode 100644 index 0000000..cae9040 --- /dev/null +++ b/ik_solvers/package.xml @@ -0,0 +1,31 @@ + + + + + ik_solvers + 0.2.0 + Inverse kinematics solvers used for whole-body control + + Evan Palmer + MIT + + https://github.com/Robotic-Decision-Making-Lab/auv_controllers.git + https://github.com/Robotic-Decision-Making-Lab/auv_controllers/issues + + Evan Palmer + + ament_cmake + eigen3_cmake_module + + eigen + rclcpp + pluginlib + rclcpp_lifecycle + trajectory_msgs + generate_parameter_library + pinocchio + + + ament_cmake + + diff --git a/ik_solvers/src/solver.cpp b/ik_solvers/src/solver.cpp new file mode 100644 index 0000000..7aa8586 --- /dev/null +++ b/ik_solvers/src/solver.cpp @@ -0,0 +1,75 @@ +// 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 "ik_solvers/solver.hpp" + +#include "pinocchio/algorithm/frames.hpp" +#include "pinocchio/algorithm/kinematics.hpp" + +namespace ik_solvers +{ + +auto IKSolver::initialize( + const std::shared_ptr & node, + const std::shared_ptr & model, + const std::shared_ptr & data, + const std::string & /*prefix*/) -> void +{ + node_ = node; + model_ = model; + data_ = data; +} + +auto IKSolver::update_pinocchio(const Eigen::VectorXd & q) const -> void +{ + pinocchio::forwardKinematics(*model_, *data_, q); + pinocchio::updateFramePlacements(*model_, *data_); + pinocchio::computeJointJacobians(*model_, *data_); +} + +auto IKSolver::solve(const rclcpp::Duration & period, const Eigen::Isometry3d & goal, const Eigen::VectorXd & q) + -> std::expected +{ + // update the pinocchio data and model to use the current joint configuration + update_pinocchio(q); + + const auto result = solve_ik(goal, q); + if (!result.has_value()) { + return std::unexpected(result.error()); + } + const Eigen::VectorXd solution = result.value(); + + // Integrate the solution to get the new joint positions + const Eigen::VectorXd q_next = pinocchio::integrate(*model_, q, period.seconds() * solution); + + // Convert the result into a JointTrajectoryPoint + trajectory_msgs::msg::JointTrajectoryPoint point; + point.time_from_start = period; + + point.positions.reserve(q_next.size()); + std::ranges::copy(q_next, std::back_inserter(point.positions)); + + point.velocities.reserve(solution.size()); + std::ranges::copy(solution, std::back_inserter(point.velocities)); + + return point; +} + +} // namespace ik_solvers diff --git a/ik_solvers/src/task_priority_solver.cpp b/ik_solvers/src/task_priority_solver.cpp new file mode 100644 index 0000000..ab1b6f1 --- /dev/null +++ b/ik_solvers/src/task_priority_solver.cpp @@ -0,0 +1,356 @@ +// 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 "ik_solvers/task_priority_solver.hpp" + +#include +#include +#include +#include + +#include "pinocchio/algorithm/frames.hpp" +#include "pinocchio/algorithm/jacobian.hpp" +#include "pinocchio/algorithm/kinematics.hpp" +#include "pseudoinverse.hpp" +#include "tf2_eigen/tf2_eigen.hpp" + +namespace ik_solvers +{ + +namespace hierarchy +{ + +namespace +{ + +/// Return the set of active tasks in the given hierarchy. +auto active_tasks(const ConstraintSet & tasks) -> ConstraintSet +{ + ConstraintSet result; + std::ranges::copy( + tasks | std::views::filter([](const auto & task) { + auto set_task = std::dynamic_pointer_cast(task); + return !std::dynamic_pointer_cast(set_task) || set_task->is_active(); + }), + std::inserter(result, result.end())); + return result; +} + +/// Compute the error between two quaternions using eq. 2.12 in Gianluca Antonelli's Underwater Robotics book. +/// Note that we only need to minimize the vector part of the error. +auto quaternion_error(const Eigen::Quaterniond & q1, const Eigen::Quaterniond & q2) -> Eigen::Vector3d +{ + const Eigen::Vector3d q1_vec = q1.vec(); + const Eigen::Vector3d q2_vec = q2.vec(); + + const double q1_w = q1.w(); + const double q2_w = q2.w(); + + const Eigen::Vector3d vec_error = (q2_w * q1_vec) - (q1_w * q2_vec) + q2_vec.cross(q1_vec); + + // This is how we would compute the scalar error if we needed it + // const double scalar_error = q1_w * q2_w + q1_vec.dot(q2_vec); + + return {vec_error.x(), vec_error.y(), vec_error.z()}; +} + +} // namespace + +auto TaskHierarchy::insert(const std::shared_ptr & constraint) -> void { constraints_.insert(constraint); } + +auto TaskHierarchy::clear() -> void { constraints_.clear(); } + +auto TaskHierarchy::set_constraints() const -> ConstraintSet // NOLINT +{ + const ConstraintSet tasks = active_tasks(constraints_); + ConstraintSet result; + std::ranges::copy( + tasks | + std::views::filter([](const auto & task) { return std::dynamic_pointer_cast(task) != nullptr; }), + std::inserter(result, result.end())); + return result; +} + +auto TaskHierarchy::equality_constraints() const -> ConstraintSet // NOLINT +{ + const ConstraintSet tasks = active_tasks(constraints_); + ConstraintSet result; + std::ranges::copy( + tasks | std::views::filter([](const auto & task) { return !std::dynamic_pointer_cast(task); }), + std::inserter(result, result.end())); + return result; +} + +auto TaskHierarchy::hierarchies() const -> std::vector +{ + const ConstraintSet equality_constraints = this->equality_constraints(); + const ConstraintSet set_constraints = this->set_constraints(); + + // Return the equality tasks if there are no set constraints + if (set_constraints.empty()) { + return {equality_constraints}; + } + + // Generate the power set of the set constraints, excluding the empty set + const std::vector> set_constraints_vector(set_constraints.begin(), set_constraints.end()); + const size_t n_subsets = (1 << set_constraints.size()) - 1; + + std::vector result; + result.reserve(n_subsets); + + for (const size_t mask : std::views::iota(1U, 1U << set_constraints.size())) { + ConstraintSet subset; + for (size_t i = 0; i < set_constraints.size(); ++i) { + if (((mask >> i) & 1U) != 0) { + subset.insert(set_constraints_vector[i]); + } + } + subset.insert(equality_constraints.begin(), equality_constraints.end()); + result.push_back(std::move(subset)); + } + + return result; +} + +PoseConstraint::PoseConstraint( + const std::shared_ptr & model, + const std::shared_ptr & data, + const Eigen::Isometry3d & primal, + const Eigen::Isometry3d & constraint, + const std::string & frame, + double gain, + int priority) +: Constraint(primal.matrix(), constraint.matrix(), gain, priority) +{ + error_ = Eigen::VectorXd::Zero(6); + error_.head<3>() = (constraint.translation() - primal.translation()).eval(); + error_.tail<3>() = quaternion_error(Eigen::Quaterniond(constraint.rotation()), Eigen::Quaterniond(primal.rotation())); + jacobian_ = Eigen::MatrixXd::Zero(6, model->nv); + pinocchio::getFrameJacobian(*model, *data, model->getFrameId(frame), pinocchio::LOCAL_WORLD_ALIGNED, jacobian_); +} + +JointConstraint::JointConstraint( + const std::shared_ptr & model, + double primal, + double ub, + double lb, + double tol, + double activation, + const std::string & joint_name, + double gain, + int priority) +: SetConstraint(primal, ub, lb, tol, activation, gain, priority) +{ + error_ = constraint_ - primal_; + + // The Jacobian is a row vector with a 1 in the column corresponding to the joint + jacobian_ = Eigen::MatrixXd::Zero(1, model->nv); + jacobian_(0, model->joints[model->getJointId(joint_name)].idx_v()) = 1; +} + +} // namespace hierarchy + +namespace +{ + +/// Compute the nullspace of the Jacobian matrix using the pseudoinverse. +auto compute_nullspace(const Eigen::MatrixXd & augmented_jacobian) -> Eigen::MatrixXd +{ + const Eigen::MatrixXd eye = Eigen::MatrixXd::Identity(augmented_jacobian.cols(), augmented_jacobian.cols()); + return eye - (pinv::least_squares(augmented_jacobian) * augmented_jacobian); +} + +/// Construct the augmented Jacobian matrix from a list of Jacobian matrices. +auto construct_augmented_jacobian(const std::vector & jacobians) -> Eigen::MatrixXd +{ + if (jacobians.empty()) { + throw std::invalid_argument("At least one Jacobian matrix must be provided."); + } + + const int n_cols = jacobians.front().cols(); + const int n_rows = std::accumulate( + jacobians.begin(), jacobians.end(), 0, [](int sum, const Eigen::MatrixXd & jac) { return sum + jac.rows(); }); + + Eigen::MatrixXd augmented_jacobian(n_rows, n_cols); + int current_row = 0; // NOLINT(misc-const-correctness) + + for (const auto & jac : jacobians) { + augmented_jacobian.block(current_row, 0, jac.rows(), jac.cols()) = jac; + current_row += jac.rows(); + } + + return augmented_jacobian; +} + +/// Closed-loop task priority IK. +auto tpik(const hierarchy::ConstraintSet & tasks, size_t nv, double damping) + -> std::expected +{ + if (tasks.empty()) { + return std::unexpected(SolverError::NO_SOLUTION); + } + + std::vector jacobians; + jacobians.reserve(tasks.size()); + + Eigen::VectorXd vel = Eigen::VectorXd::Zero(nv); + Eigen::MatrixXd nullspace = Eigen::MatrixXd::Identity(nv, nv); + + for (const auto & task : tasks) { + const Eigen::MatrixXd jacobian_inv = pinv::damped_least_squares(task->jacobian(), damping); + vel += nullspace * jacobian_inv * (task->gain() * task->error()); + + jacobians.push_back(task->jacobian()); + nullspace = compute_nullspace(construct_augmented_jacobian(jacobians)); + } + + return vel; +} + +/// Check if the solution is feasible. +auto is_feasible(const hierarchy::ConstraintSet & constraints, const Eigen::VectorXd & solution) -> bool +{ + return std::ranges::all_of(constraints, [&solution](const auto & constraint) { + auto set_task = std::dynamic_pointer_cast(constraint); + const double pred = (constraint->jacobian() * solution).value(); + return ( + (set_task->primal() > set_task->lower_threshold() && pred < 0) || + (set_task->primal() < set_task->upper_threshold() && pred > 0)); + }); +} + +/// Search for the feasible solutions to the task hierarchy and return the one with the smallest norm. +auto search_solutions( + const hierarchy::ConstraintSet & set_tasks, + const std::vector & hierarchies, + size_t nv, + double damping) -> std::expected +{ + if (hierarchies.empty()) { + return std::unexpected(SolverError::NO_SOLUTION); + } + + // there is only one solution if there are no set tasks + if (set_tasks.empty()) { + return tpik(hierarchies.front(), nv, damping); + } + + std::vector solutions; + solutions.reserve(hierarchies.size()); + + for (const auto & tasks : hierarchies) { + const auto out = tpik(tasks, nv, damping); + if (!out.has_value()) { + continue; + } + + const Eigen::VectorXd & current_solution = out.value(); + if (is_feasible(set_tasks, current_solution)) { + solutions.push_back(current_solution); + } + } + + if (solutions.empty()) { + return std::unexpected(SolverError::NO_SOLUTION); + } + + // Choose the solution with the smallest norm + return *std::ranges::min_element(solutions, {}, [](const auto & a) { return a.norm(); }); +} + +auto pinocchio_to_eigen(const pinocchio::SE3 & pose) -> Eigen::Isometry3d +{ + Eigen::Isometry3d result; + result.translation() = pose.translation(); + result.linear() = pose.rotation(); + return result; +} + +} // namespace + +auto TaskPriorityIKSolver::initialize( + const std::shared_ptr & node, + const std::shared_ptr & model, + const std::shared_ptr & data, + const std::string & prefix) -> void +{ + IKSolver::initialize(node, model, data, prefix); + param_listener_ = std::make_shared(node_, prefix); + params_ = param_listener_->get_params(); +} + +// NOLINTNEXTLINE(readability-convert-member-functions-to-static) +auto TaskPriorityIKSolver::update_parameters() -> void +{ + if (!param_listener_->is_old(params_)) { + return; + } + param_listener_->refresh_dynamic_parameters(); + params_ = param_listener_->get_params(); +} + +auto TaskPriorityIKSolver::configure_parameters() -> void +{ + update_parameters(); + damping_ = params_.damping; + ee_frame_ = params_.end_effector_frame_id; +} + +// NOLINTNEXTLINE(readability-convert-member-functions-to-static) +auto TaskPriorityIKSolver::solve_ik(const Eigen::Isometry3d & goal, const Eigen::VectorXd & q) + -> std::expected +{ + configure_parameters(); + + hierarchy_.clear(); + + // get the end effector pose as an Eigen Isometry3d + const Eigen::Isometry3d ee_pose = pinocchio_to_eigen(data_->oMf[model_->getFrameId(ee_frame_)]); + + // insert the pose constraint + const double gain = params_.end_effector_pose_task.gain; + hierarchy_.insert(std::make_shared(model_, data_, ee_pose, goal, ee_frame_, gain)); + + for (const auto & joint_name : params_.constrained_joints) { + // get the joint index in the configuration vector + const int joint_id = model_->getJointId(joint_name); + const int idx_q = model_->joints[joint_id].idx_q(); + + // get the constraint parameters + const double primal = q[idx_q]; + const double ub = model_->upperPositionLimit[idx_q]; + const double lb = model_->lowerPositionLimit[idx_q]; + const double tol = params_.joint_limit_task.constrained_joints_map[joint_name].safety_tolerance; + const double activation = params_.joint_limit_task.constrained_joints_map[joint_name].activation_threshold; + const double joint_limit_gain = params_.joint_limit_task.constrained_joints_map[joint_name].gain; + + // insert the joint limit constraint + hierarchy_.insert(std::make_shared( + model_, primal, ub, lb, tol, activation, joint_name, joint_limit_gain)); + } + + // find the safe solutions and choose the one with the smallest norm + return search_solutions(hierarchy_.set_constraints(), hierarchy_.hierarchies(), model_->nv, damping_); +} + +} // namespace ik_solvers + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(ik_solvers::TaskPriorityIKSolver, ik_solvers::IKSolver) diff --git a/media/teleop.gif b/media/teleop.gif new file mode 100644 index 0000000..63dd3ea Binary files /dev/null and b/media/teleop.gif differ diff --git a/media/uvms.gif b/media/uvms.gif new file mode 100644 index 0000000..98f76ef Binary files /dev/null and b/media/uvms.gif differ diff --git a/thruster_allocation_matrix_controller/CHANGELOG.md b/thruster_allocation_matrix_controller/CHANGELOG.md new file mode 100644 index 0000000..fb5c325 --- /dev/null +++ b/thruster_allocation_matrix_controller/CHANGELOG.md @@ -0,0 +1,8 @@ +# Changelog for package thruster_allocation_matrix_controller + +## 0.2.0 (2025-05-03) + +## 0.1.0 (2025-04-27) + +- Updates the minimum CMake version to CMake 23 and upgrades the API to use +C++ 23 diff --git a/thruster_allocation_matrix_controller/package.xml b/thruster_allocation_matrix_controller/package.xml index 1a53e28..590a04b 100644 --- a/thruster_allocation_matrix_controller/package.xml +++ b/thruster_allocation_matrix_controller/package.xml @@ -3,7 +3,11 @@ thruster_allocation_matrix_controller +<<<<<<< HEAD 0.0.2 +======= + 0.2.0 +>>>>>>> d197fc7 (Port end effector trajectory controller from Angler (#54)) 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 new file mode 100644 index 0000000..6e590ff --- /dev/null +++ b/thruster_controllers/CHANGELOG.md @@ -0,0 +1,10 @@ +# Changelog for package thruster_controllers + +## 0.2.0 (2025-05-03) + +## 0.1.0 (2025-04-27) + +- Implements the Gazebo passthrough thruster controller +- Implements the thruster rotation rate controller +- Updates the API to use C++ 23 +- Bumps the minimum CMake version to CMake 23 diff --git a/thruster_controllers/package.xml b/thruster_controllers/package.xml index 6060d89..0ca552d 100644 --- a/thruster_controllers/package.xml +++ b/thruster_controllers/package.xml @@ -2,7 +2,11 @@ thruster_controllers +<<<<<<< HEAD 0.0.1 +======= + 0.2.0 +>>>>>>> d197fc7 (Port end effector trajectory controller from Angler (#54)) A collection of thruster controllers for AUV control ros diff --git a/topic_sensors/CHANGELOG.md b/topic_sensors/CHANGELOG.md new file mode 100644 index 0000000..e2f4a4e --- /dev/null +++ b/topic_sensors/CHANGELOG.md @@ -0,0 +1,7 @@ +# Changelog for package topic_sensors + +## 0.2.0 (2025-05-03) + +## 0.1.0 (2025-04-27) + +- Implements an nav_msgs/Odometry topic sensor diff --git a/topic_sensors/package.xml b/topic_sensors/package.xml new file mode 100644 index 0000000..da96fb0 --- /dev/null +++ b/topic_sensors/package.xml @@ -0,0 +1,33 @@ + + + + + topic_sensors + 0.2.0 + Sensor plugins used to write ROS 2 messages to state interfaces + + Evan Palmer + MIT + + https://github.com/Robotic-Decision-Making-Lab/auv_controllers.git + https://github.com/Robotic-Decision-Making-Lab/auv_controllers/issues + + Akshaya Agrawal + Evan Palmer + + ament_cmake + + rclcpp + ros2_control + pluginlib + hardware_interface + nav_msgs + geometry_msgs + generate_parameter_library + controller_common + message_transforms + + + ament_cmake + + diff --git a/velocity_controllers/CHANGELOG.md b/velocity_controllers/CHANGELOG.md new file mode 100644 index 0000000..9463314 --- /dev/null +++ b/velocity_controllers/CHANGELOG.md @@ -0,0 +1,15 @@ +# Changelog for package velocity_controllers + +## 0.2.0 (2025-05-03) + +## 0.1.0 (2025-04-27) + +- Updates the API to use C++ 23 +- Bumps the minimum CMake version to CMake 23 +- Implements the adaptive integral terminal sliding mode controller +- Changes the integral sliding mode controller external state message interface +from the geometry_msgs/TwistStamped message type to nav_msgs/Odometry +- Makes the integral sliding mode controller joint names configurable for users +that want to integrate prefixes into the command/state interfaces +- Updates the integral sliding mode controller to use the latest hydrodynamics +parsing capabilities diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index e4fe0c8..178b62c 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -3,7 +3,11 @@ velocity_controllers +<<<<<<< HEAD 0.0.1 +======= + 0.2.0 +>>>>>>> d197fc7 (Port end effector trajectory controller from Angler (#54)) A collection of velocity controllers for underwater vehicles Evan Palmer diff --git a/whole_body_controllers/CHANGELOG.md b/whole_body_controllers/CHANGELOG.md new file mode 100644 index 0000000..85f2fb8 --- /dev/null +++ b/whole_body_controllers/CHANGELOG.md @@ -0,0 +1,12 @@ +# Changelog for package whole_body_controllers + +## 0.2.0 (2025-05-03) + +- Replaces instances of `Eigen::Affine3d` with `Eigen::Isometry3d` +- Fixes a bug in the ik_controller reference interfaces where the values sent + to the reference interfaces themselves (i.e., not as a message) were not + being transformed into the appropriate coordinate frame for Pinocchio. + +## 0.1.0 (2025-04-27) + +- Implements an IK controller for controlling a UVMS with a single manipulator diff --git a/whole_body_controllers/CMakeLists.txt b/whole_body_controllers/CMakeLists.txt new file mode 100644 index 0000000..58f9e29 --- /dev/null +++ b/whole_body_controllers/CMakeLists.txt @@ -0,0 +1,99 @@ +cmake_minimum_required(VERSION 3.23) +project(whole_body_controllers) + +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(nav_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(control_msgs REQUIRED) +find_package(pinocchio REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2_eigen REQUIRED) +find_package(message_transforms REQUIRED) +find_package(controller_common REQUIRED) +find_package(ik_solvers REQUIRED) +find_package(auv_control_msgs REQUIRED) +find_package(ament_cmake REQUIRED) +find_package(generate_parameter_library REQUIRED) +find_package(pluginlib REQUIRED) + +generate_parameter_library(ik_controller_parameters + src/ik_controller_parameters.yaml +) + +add_library(whole_body_controllers SHARED) +target_sources( + whole_body_controllers + PRIVATE src/ik_controller.cpp + PUBLIC + FILE_SET HEADERS + BASE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include + FILES + ${CMAKE_CURRENT_SOURCE_DIR}/include/whole_body_controllers/ik_controller.hpp +) +target_compile_features(whole_body_controllers PUBLIC cxx_std_23) +target_link_libraries( + whole_body_controllers + PUBLIC + ik_controller_parameters + controller_interface::controller_interface + hardware_interface::hardware_interface + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + realtime_tools::realtime_tools + pinocchio::pinocchio + tf2_ros::tf2_ros + ik_solvers::ik_solvers + tf2_eigen::tf2_eigen + message_transforms::message_transforms + controller_common::controller_common + Eigen3::Eigen + ${geometry_msgs_TARGETS} + ${nav_msgs_TARGETS} + ${std_msgs_TARGETS} + ${control_msgs_TARGETS} + ${auv_control_msgs_TARGETS} +) +pluginlib_export_plugin_description_file(controller_interface whole_body_controllers.xml) + +install( + TARGETS whole_body_controllers ik_controller_parameters + EXPORT export_whole_body_controllers + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + FILE_SET HEADERS +) + +ament_export_targets(export_whole_body_controllers HAS_LIBRARY_TARGET) +ament_export_dependencies( + "controller_interface" + "hardware_interface" + "rclcpp" + "rclcpp_lifecycle" + "realtime_tools" + "nav_msgs" + "std_msgs" + "control_msgs" + "pinocchio" + "geometry_msgs" + "tf2_ros" + "tf2_eigen" + "message_transforms" + "controller_common" + "ik_solvers" + "auv_control_msgs" + "Eigen3" +) + +ament_package() diff --git a/whole_body_controllers/include/whole_body_controllers/ik_controller.hpp b/whole_body_controllers/include/whole_body_controllers/ik_controller.hpp new file mode 100644 index 0000000..ff249fc --- /dev/null +++ b/whole_body_controllers/include/whole_body_controllers/ik_controller.hpp @@ -0,0 +1,119 @@ +// 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 "auv_control_msgs/msg/ik_controller_state_stamped.hpp" +#include "controller_interface/chainable_controller_interface.hpp" +#include "controller_interface/controller_interface.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "ik_solvers/solver.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "pinocchio/algorithm/joint-configuration.hpp" +#include "pinocchio/algorithm/kinematics.hpp" +#include "pluginlib/class_loader.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.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 whole_body_controllers +{ + +class IKController : public controller_interface::ChainableControllerInterface +{ +public: + IKController() = 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_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_chained_reference_values() -> controller_interface::return_type; + + auto update_parameters() -> void; + + auto configure_parameters() -> controller_interface::CallbackReturn; + + auto update_and_validate_interfaces() -> controller_interface::return_type; + + std::shared_ptr model_; + std::shared_ptr data_; + + realtime_tools::RealtimeBuffer reference_; + std::shared_ptr> reference_sub_; + + // allow users to send the vehicle state as a message - this can simplify integration with state estimators + // that publish the vehicle state without the manipulator states + std::shared_ptr> vehicle_state_sub_; + realtime_tools::RealtimeBuffer vehicle_state_; + + std::vector position_state_values_, velocity_state_values_; + + std::unique_ptr> loader_; + std::shared_ptr solver_; + + std::unique_ptr param_listener_; + ik_controller::Params params_; + + std::shared_ptr> controller_state_pub_; + std::unique_ptr> + rt_controller_state_pub_; + + // store the names of the base joints + std::vector free_flyer_pos_dofs_, free_flyer_vel_dofs_; + + // store the names of the position and velocity interfaces + // this is stored in the same order as the pinocchio model to simplify integration with the solver + std::vector position_interface_names_, velocity_interface_names_; + + // track the interfaces used by the controller + bool use_position_commands_, use_velocity_commands_, use_position_states_, use_velocity_states_; + std::size_t n_command_interfaces_, n_state_interfaces_; + + rclcpp::Logger logger_{rclcpp::get_logger("ik_controller")}; +}; + +} // namespace whole_body_controllers diff --git a/whole_body_controllers/package.xml b/whole_body_controllers/package.xml new file mode 100644 index 0000000..70e9197 --- /dev/null +++ b/whole_body_controllers/package.xml @@ -0,0 +1,37 @@ + + + + + whole_body_controllers + 0.2.0 + Whole-body controllers for underwater vehicle 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 + + rclcpp + ros2_control + pluginlib + controller_interface + hardware_interface + rclcpp_lifecycle + generate_parameter_library + control_msgs + pinocchio + nav_msgs + message_transforms + controller_common + ik_solvers + auv_control_msgs + + + ament_cmake + + diff --git a/whole_body_controllers/src/ik_controller.cpp b/whole_body_controllers/src/ik_controller.cpp new file mode 100644 index 0000000..f1b0a05 --- /dev/null +++ b/whole_body_controllers/src/ik_controller.cpp @@ -0,0 +1,491 @@ +// 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 "whole_body_controllers/ik_controller.hpp" + +#include +#include +#include +#include + +#include "controller_common/common.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "message_transforms/transforms.hpp" +#include "pinocchio/algorithm/model.hpp" +#include "pinocchio/parsers/urdf.hpp" + +namespace whole_body_controllers +{ + +namespace +{ + +auto to_eigen(const std::vector & vec) -> Eigen::Isometry3d +{ + if (vec.size() != 7) { + throw std::invalid_argument("Invalid size for pose vector"); + } + const Eigen::Translation3d translation = {vec[0], vec[1], vec[2]}; + Eigen::Quaterniond rotation = {vec[6], vec[3], vec[4], vec[5]}; + rotation.normalize(); + return {translation * rotation}; +} + +} // namespace + +auto IKController::on_init() -> controller_interface::CallbackReturn +{ + param_listener_ = std::make_unique(get_node()); + params_ = param_listener_->get_params(); + logger_ = get_node()->get_logger(); + + // store the joint names + free_flyer_pos_dofs_ = params_.free_flyer_position_joints; + free_flyer_vel_dofs_ = params_.free_flyer_velocity_joints; + + // initialize the pinocchio model + model_ = std::make_shared(); + pinocchio::urdf::buildModelFromXML(get_robot_description(), pinocchio::JointModelFreeFlyer(), *model_); + + // lock all uncontrolled joints + std::vector controlled_joints = {"universe", "root_joint"}; + std::ranges::copy(params_.controlled_joints, std::back_inserter(controlled_joints)); + + std::vector locked_joint_names; + std::ranges::copy_if(model_->names, std::back_inserter(locked_joint_names), [&controlled_joints](const auto & name) { + return std::ranges::find(controlled_joints, name) == controlled_joints.end(); + }); + + std::vector locked_joints; + std::ranges::transform(locked_joint_names, std::back_inserter(locked_joints), [this](const auto & name) { + return model_->getJointId(name); + }); + + // build the reduced model + pinocchio::Model reduced_model; + pinocchio::buildReducedModel(*model_, locked_joints, pinocchio::neutral(*model_), reduced_model); + *model_ = reduced_model; + data_ = std::make_shared(*model_); + + position_interface_names_.resize(model_->nq); + velocity_interface_names_.resize(model_->nv); + + // save the interface names in the same order as the model + // start at index 1 to skip the universe joint + for (std::size_t i = 1; i < model_->names.size(); ++i) { + const std::string name = model_->names[i]; + const auto joint = model_->joints[model_->getJointId(name)]; + if (name == "root_joint") { + std::ranges::copy(free_flyer_pos_dofs_, position_interface_names_.begin() + joint.idx_q()); + std::ranges::copy(free_flyer_vel_dofs_, velocity_interface_names_.begin() + joint.idx_v()); + } else { + position_interface_names_[joint.idx_q()] = name; + velocity_interface_names_[joint.idx_v()] = name; + } + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +// NOLINTNEXTLINE(readability-convert-member-functions-to-static) +auto IKController::update_parameters() -> void +{ + if (!param_listener_->is_old(params_)) { + return; + } + param_listener_->refresh_dynamic_parameters(); + params_ = param_listener_->get_params(); +} + +auto IKController::configure_parameters() -> controller_interface::CallbackReturn +{ + update_parameters(); + + auto has_interface = [](const std::vector & interfaces, const std::string & type) -> bool { + return std::ranges::find(interfaces, type) != interfaces.end(); + }; + + use_position_commands_ = has_interface(params_.command_interfaces, "position"); + use_velocity_commands_ = has_interface(params_.command_interfaces, "velocity"); + + n_command_interfaces_ = 0; + if (use_position_commands_) { + n_command_interfaces_ += position_interface_names_.size(); + } + if (use_velocity_commands_) { + n_command_interfaces_ += velocity_interface_names_.size(); + } + + use_position_states_ = has_interface(params_.state_interfaces, "position"); + use_velocity_states_ = has_interface(params_.state_interfaces, "velocity"); + + n_state_interfaces_ = 0; + if (use_position_states_) { + n_state_interfaces_ += position_interface_names_.size(); + } + if (use_velocity_states_) { + n_state_interfaces_ += velocity_interface_names_.size(); + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +// NOLINTNEXTLINE(readability-convert-member-functions-to-static) +auto IKController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) + -> controller_interface::CallbackReturn +{ + configure_parameters(); + + reference_.writeFromNonRT(geometry_msgs::msg::Pose()); + vehicle_state_.writeFromNonRT(nav_msgs::msg::Odometry()); + + command_interfaces_.reserve(n_command_interfaces_); + position_state_values_.resize(position_interface_names_.size(), std::numeric_limits::quiet_NaN()); + velocity_state_values_.resize(velocity_interface_names_.size(), std::numeric_limits::quiet_NaN()); + + reference_sub_ = get_node()->create_subscription( + "~/reference", rclcpp::SystemDefaultsQoS(), [this](const std::shared_ptr msg) { // NOLINT + m2m::transform_message(*msg); + reference_.writeFromNonRT(*msg); + }); + + if (params_.use_external_measured_vehicle_states) { + RCLCPP_INFO(logger_, "Using external measured vehicle states"); // NOLINT + vehicle_state_sub_ = get_node()->create_subscription( + "~/vehicle_state", + rclcpp::SystemDefaultsQoS(), + [this](const std::shared_ptr msg) { // NOLINT + m2m::transform_message(*msg, "map", "base_link"); + vehicle_state_.writeFromNonRT(*msg); + }); + } + + loader_ = std::make_unique>("ik_solvers", "ik_solvers::IKSolver"); + solver_ = loader_->createSharedInstance(params_.ik_solver); + solver_->initialize(get_node(), model_, data_, params_.ik_solver); + RCLCPP_INFO(logger_, "Configured the IK controller with solver %s", params_.ik_solver.c_str()); // NOLINT + + controller_state_pub_ = get_node()->create_publisher( + "~/status", rclcpp::SystemDefaultsQoS()); + rt_controller_state_pub_ = + std::make_unique>( + controller_state_pub_); + + rt_controller_state_pub_->lock(); + rt_controller_state_pub_->msg_.solver_name = params_.ik_solver; + std::ranges::copy(position_interface_names_, std::back_inserter(rt_controller_state_pub_->msg_.position_joint_names)); + std::ranges::copy(velocity_interface_names_, std::back_inserter(rt_controller_state_pub_->msg_.velocity_joint_names)); + rt_controller_state_pub_->unlock(); + + return controller_interface::CallbackReturn::SUCCESS; +} + +auto IKController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) + -> controller_interface::CallbackReturn +{ + common::messages::reset_message(reference_.readFromNonRT()); + common::messages::reset_message(vehicle_state_.readFromNonRT()); + reference_interfaces_.assign(reference_interfaces_.size(), std::numeric_limits::quiet_NaN()); + position_state_values_.assign(position_state_values_.size(), std::numeric_limits::quiet_NaN()); + velocity_state_values_.assign(velocity_state_values_.size(), std::numeric_limits::quiet_NaN()); + return controller_interface::CallbackReturn::SUCCESS; +} + +auto IKController::command_interface_configuration() const -> controller_interface::InterfaceConfiguration +{ + controller_interface::InterfaceConfiguration config; + config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + config.names.reserve(n_command_interfaces_); + + auto format_interface = [](const std::string & name, const std::string & type, const std::string & reference) { + return reference.empty() ? std::format("{}/{}", name, type) : std::format("{}/{}/{}", reference, name, type); + }; + + if (use_position_commands_) { + std::ranges::transform( + position_interface_names_, std::back_inserter(config.names), [this, &format_interface](const auto & name) { + if (std::ranges::find(free_flyer_pos_dofs_, name) != free_flyer_pos_dofs_.end()) { + return format_interface(name, hardware_interface::HW_IF_POSITION, params_.vehicle_reference_controller); + } + return format_interface(name, hardware_interface::HW_IF_POSITION, params_.manipulator_reference_controller); + }); + } + + if (use_velocity_commands_) { + std::ranges::transform( + velocity_interface_names_, std::back_inserter(config.names), [this, &format_interface](const auto & name) { + if (std::ranges::find(free_flyer_vel_dofs_, name) != free_flyer_vel_dofs_.end()) { + return format_interface(name, hardware_interface::HW_IF_VELOCITY, params_.vehicle_reference_controller); + } + return format_interface(name, hardware_interface::HW_IF_VELOCITY, params_.manipulator_reference_controller); + }); + } + + return config; +} + +auto IKController::state_interface_configuration() const -> controller_interface::InterfaceConfiguration +{ + controller_interface::InterfaceConfiguration config; + config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + auto insert_interfaces = [&config](const std::vector & interface_names, const std::string & type) { + std::ranges::transform(interface_names, std::back_inserter(config.names), [type](const auto & name) { + return std::format("{}/{}", name, type); + }); + }; + + if (params_.use_external_measured_vehicle_states) { + config.names.reserve(params_.controlled_joints.size()); + if (use_position_states_) { + insert_interfaces(params_.controlled_joints, hardware_interface::HW_IF_POSITION); + } + if (use_velocity_states_) { + insert_interfaces(params_.controlled_joints, hardware_interface::HW_IF_VELOCITY); + } + } else { + config.names.reserve(n_state_interfaces_); + if (use_position_states_) { + insert_interfaces(position_interface_names_, hardware_interface::HW_IF_POSITION); + } + if (use_velocity_states_) { + insert_interfaces(velocity_interface_names_, hardware_interface::HW_IF_VELOCITY); + } + } + + return config; +} + +// NOLINTNEXTLINE(readability-convert-member-functions-to-static) +auto IKController::on_export_reference_interfaces() -> std::vector +{ + reference_interfaces_.resize(free_flyer_pos_dofs_.size(), std::numeric_limits::quiet_NaN()); + std::vector interfaces; + interfaces.reserve(free_flyer_pos_dofs_.size()); + + for (const auto & [i, dof] : std::views::enumerate(free_flyer_pos_dofs_)) { + const std::string interface = std::format("{}/{}", dof, hardware_interface::HW_IF_POSITION); + interfaces.emplace_back(get_node()->get_name(), interface, &reference_interfaces_[i]); + } + + return interfaces; +} + +// NOLINTNEXTLINE(readability-convert-member-functions-to-static) +auto IKController::update_reference_from_subscribers(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) + -> controller_interface::return_type +{ + auto * current_reference = reference_.readFromRT(); + std::vector reference = common::messages::to_vector(*current_reference); + for (auto && [interface, ref] : std::views::zip(reference_interfaces_, reference)) { + interface = ref; + } + common::messages::reset_message(current_reference); + return controller_interface::return_type::OK; +} + +// NOLINTNEXTLINE(readability-convert-member-functions-to-static) +auto IKController::update_system_state_values() -> controller_interface::return_type +{ + // if we are using the external measured vehicle states, the message has already been transformed into the + // appropriate frame, so we can just copy the values into the system state values. otherwise, we need to transform + // the states first, then save them. + if (params_.use_external_measured_vehicle_states) { + const auto * state_msg = vehicle_state_.readFromRT(); + const std::vector state = common::messages::to_vector(*state_msg); + std::ranges::copy(state.begin(), state.begin() + free_flyer_pos_dofs_.size(), position_state_values_.begin()); + std::ranges::copy(state.begin() + free_flyer_pos_dofs_.size(), state.end(), velocity_state_values_.begin()); + } else { + auto save_states = [](const auto & interfaces, auto out) { + std::ranges::transform(interfaces, out, [](const auto & interface) { + return interface.get_optional().value_or(std::numeric_limits::quiet_NaN()); + }); + }; + + // retrieve the vehicle position and velocity state interfaces + const auto position_interfaces_end = state_interfaces_.begin() + free_flyer_pos_dofs_.size(); + const auto velocity_interfaces_start = position_interfaces_end + params_.controlled_joints.size(); + const auto velocity_interfaces_end = velocity_interfaces_start + free_flyer_vel_dofs_.size(); + + const auto position_interfaces = std::span(state_interfaces_.begin(), position_interfaces_end); + const auto velocity_interfaces = std::span(velocity_interfaces_start, velocity_interfaces_end); + + std::vector position_states, velocity_states; // NOLINT(readability-isolate-declaration) + position_states.reserve(position_interfaces.size()); + velocity_states.reserve(velocity_interfaces.size()); + + save_states(position_interfaces, std::back_inserter(position_states)); + save_states(velocity_interfaces, std::back_inserter(velocity_states)); + + // transform the states into the appropriate frame and save them + geometry_msgs::msg::Pose pose; + common::messages::to_msg(position_states, &pose); + m2m::transform_message(pose); + std::ranges::copy(common::messages::to_vector(pose), position_state_values_.begin()); + + geometry_msgs::msg::Twist twist; + common::messages::to_msg(velocity_states, &twist); + m2m::transform_message(twist); + std::ranges::copy(common::messages::to_vector(twist), velocity_state_values_.begin()); + } + + auto find_interface = [](const auto & interfaces, const std::string & name, const std::string & type) { + return std::ranges::find_if(interfaces, [&name, &type](const auto & interface) { + return interface.get_name() == std::format("{}/{}", name, type); + }); + }; + + // save the manipulator states + for (const auto & joint_name : params_.controlled_joints) { + const pinocchio::JointModel joint = model_->joints[model_->getJointId(joint_name)]; + + const auto pos_it = find_interface(state_interfaces_, joint_name, hardware_interface::HW_IF_POSITION); + if (pos_it != state_interfaces_.end()) { + const double pos = pos_it->get_optional().value_or(std::numeric_limits::quiet_NaN()); + position_state_values_[joint.idx_q()] = pos; + } + + const auto vel_it = find_interface(state_interfaces_, joint_name, hardware_interface::HW_IF_VELOCITY); + if (vel_it != state_interfaces_.end()) { + const double vel = vel_it->get_optional().value_or(std::numeric_limits::quiet_NaN()); + velocity_state_values_[joint.idx_v()] = vel; + } + } + + if (use_position_states_) { + if (common::math::has_nan(position_state_values_)) { + RCLCPP_DEBUG(logger_, "Received position states with NaN value."); // NOLINT + return controller_interface::return_type::ERROR; + } + } + + if (use_velocity_states_) { + if (common::math::has_nan(velocity_state_values_)) { + RCLCPP_DEBUG(logger_, "Received velocity states with NaN value."); // NOLINT + return controller_interface::return_type::ERROR; + } + } + + return controller_interface::return_type::OK; +} + +auto IKController::update_chained_reference_values() -> controller_interface::return_type +{ + // the reference interfaces in chained mode are parsed directly from the command interfaces + // + // because the command interfaces expect the commands to be in the maritime coordinate frame standard, we need + // this extra method to transform the values into a frame suitable for pinocchio + geometry_msgs::msg::Pose reference_transformed; + common::messages::to_msg(reference_interfaces_, &reference_transformed); + m2m::transform_message(reference_transformed); + std::ranges::copy(common::messages::to_vector(reference_transformed), reference_interfaces_.begin()); + return controller_interface::return_type::OK; +} + +auto IKController::update_and_validate_interfaces() -> controller_interface::return_type +{ + if (update_system_state_values() != controller_interface::return_type::OK) { + RCLCPP_DEBUG(logger_, "Failed to update system state values"); // NOLINT + return controller_interface::return_type::ERROR; + } + if (is_in_chained_mode()) { + update_chained_reference_values(); + } + if (common::math::has_nan(reference_interfaces_)) { + RCLCPP_DEBUG(logger_, "Received reference with NaN value."); // NOLINT + return controller_interface::return_type::ERROR; + } + return controller_interface::return_type::OK; +} + +// NOLINTNEXTLINE(readability-convert-member-functions-to-static) +auto IKController::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(); + + const Eigen::VectorXd q = Eigen::VectorXd::Map(position_state_values_.data(), position_state_values_.size()); + const Eigen::Isometry3d goal = to_eigen(reference_interfaces_); + + // TODO(anyone): add solver support for velocity states + // right now we only use the positions for the solver + const auto result = solver_->solve(period, goal, q); + + if (!result.has_value()) { + const auto err = result.error(); + if (err == ik_solvers::SolverError::NO_SOLUTION) { + RCLCPP_DEBUG(logger_, "The solver could not find a solution to the current IK problem"); // NOLINT + } else if (err == ik_solvers::SolverError::SOLVER_ERROR) { + RCLCPP_WARN(logger_, "The solver experienced an error while solving the IK problem"); // NOLINT + } + return controller_interface::return_type::OK; + } + + trajectory_msgs::msg::JointTrajectoryPoint point = result.value(); + + // transform the solution into the appropriate frame + geometry_msgs::msg::Twist twist; + common::messages::to_msg({point.velocities.begin(), point.velocities.begin() + free_flyer_vel_dofs_.size()}, &twist); + m2m::transform_message(twist); + std::ranges::copy(common::messages::to_vector(twist), point.velocities.begin()); + + // transform the pose into the appropriate frame + geometry_msgs::msg::Pose pose; + common::messages::to_msg({point.positions.begin(), point.positions.begin() + free_flyer_pos_dofs_.size()}, &pose); + m2m::transform_message(pose); + std::ranges::copy(common::messages::to_vector(pose), point.positions.begin()); + + if (use_position_commands_) { + for (const auto & [i, joint_name] : std::views::enumerate(position_interface_names_)) { + if (!command_interfaces_[i].set_value(point.positions[i])) { + RCLCPP_WARN(logger_, "Failed to set position command value for joint %s", joint_name.c_str()); // NOLINT + } + } + } + + if (use_velocity_commands_) { + for (const auto & [i, joint_name] : std::views::enumerate(velocity_interface_names_)) { + const std::size_t idx = use_position_commands_ ? position_interface_names_.size() + i : i; + if (!command_interfaces_[idx].set_value(point.velocities[i])) { + RCLCPP_WARN(logger_, "Failed to set velocity command value for joint %s", joint_name.c_str()); // NOLINT + } + } + } + + if (rt_controller_state_pub_ && rt_controller_state_pub_->trylock()) { + rt_controller_state_pub_->msg_.header.stamp = get_node()->now(); + rt_controller_state_pub_->msg_.time_step = period.seconds(); + common::messages::to_msg(reference_interfaces_, &rt_controller_state_pub_->msg_.reference); + rt_controller_state_pub_->msg_.solution = point; + rt_controller_state_pub_->unlockAndPublish(); + } + + return controller_interface::return_type::OK; +} + +} // namespace whole_body_controllers + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(whole_body_controllers::IKController, controller_interface::ChainableControllerInterface)