From cdb6e84d2c5c2ec9c5747d478d859382206d9880 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Mon, 30 Mar 2026 00:00:30 -0700 Subject: [PATCH 1/9] fix coordinator --- controller_coordinator/src/coordinator.cpp | 63 +++++++++++++++------- 1 file changed, 45 insertions(+), 18 deletions(-) diff --git a/controller_coordinator/src/coordinator.cpp b/controller_coordinator/src/coordinator.cpp index 811c573..5b61084 100644 --- a/controller_coordinator/src/coordinator.cpp +++ b/controller_coordinator/src/coordinator.cpp @@ -20,6 +20,7 @@ #include "coordinator.hpp" +#include #include #include "lifecycle_msgs/msg/state.hpp" @@ -39,7 +40,7 @@ ControllerCoordinator::ControllerCoordinator() // helper function used to wait for services to come up // this will block indefinitely - auto wait_for_service = [this](const auto & client, const std::string & service_name) { + auto wait_for_service = [this](const auto & client, const std::string & service_name) -> void { while (!client->wait_for_service(std::chrono::seconds(1))) { RCLCPP_INFO(this->get_logger(), "Waiting for %s service to come up", service_name.c_str()); // NOLINT } @@ -60,7 +61,9 @@ ControllerCoordinator::ControllerCoordinator() // pre-configure the hardware activation/deactivation requests activate_hardware_requests_.reserve(params_.hardware_interfaces.size()); std::ranges::transform( - params_.hardware_interfaces, std::back_inserter(activate_hardware_requests_), [](const std::string & name) { + params_.hardware_interfaces, + std::back_inserter(activate_hardware_requests_), + [](const std::string & name) -> std::shared_ptr { auto request = std::make_shared(); request->name = name; request->target_state.id = lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE; @@ -69,7 +72,9 @@ ControllerCoordinator::ControllerCoordinator() deactivate_hardware_requests_.reserve(params_.hardware_interfaces.size()); std::ranges::transform( - params_.hardware_interfaces, std::back_inserter(deactivate_hardware_requests_), [](const std::string & name) { + params_.hardware_interfaces, + std::back_inserter(deactivate_hardware_requests_), + [](const std::string & name) -> std::shared_ptr { auto request = std::make_shared(); request->name = name; request->target_state.id = lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE; @@ -92,18 +97,19 @@ ControllerCoordinator::ControllerCoordinator() activate_system_service_ = this->create_service( "~/activate", [this]( - const std::shared_ptr /*request_header*/, // NOLINT - const std::shared_ptr request, // NOLINT - const std::shared_ptr response) { // NOLINT + const std::shared_ptr /*request_header*/, // NOLINT + const std::shared_ptr request, // NOLINT + const std::shared_ptr response) -> void { // NOLINT response->success = true; if (request->data) { RCLCPP_INFO(this->get_logger(), "Activating hardware interfaces and controllers"); // NOLINT for (const auto & activate_request : activate_hardware_requests_) { - hardware_client_->async_send_request( + auto future = hardware_client_->async_send_request( activate_request, [logger = this->get_logger(), response, activate_request]( rclcpp::Client::SharedFuture - result_response) { // NOLINT + result_response) // NOLINT + -> void { const auto & result = result_response.get(); if (result->ok) { RCLCPP_INFO(logger, "Successfully activated %s", activate_request->name.c_str()); // NOLINT @@ -113,12 +119,28 @@ ControllerCoordinator::ControllerCoordinator() response->message = "Failed to activate " + activate_request->name; } }); + + const std::future_status future_result = future.wait_for(std::chrono::duration(params_.timeout)); + if (future_result != std::future_status::ready) { + RCLCPP_ERROR(this->get_logger(), "Timeout while activating %s", activate_request->name.c_str()); // NOLINT + response->success = false; + response->message = "Timeout while activating " + activate_request->name; + } + + if (!response->success) { + RCLCPP_ERROR( // NOLINT + this->get_logger(), + "Aborting activation of remaining hardware interfaces and controllers"); + return; + } } + switch_controller_client_->async_send_request( activate_controllers_request_, [this, response]( - rclcpp::Client::SharedFuture result_response) { // NOLINT + rclcpp::Client::SharedFuture result_response) // NOLINT + -> void { const auto & result = result_response.get(); if (result->ok) { RCLCPP_INFO(this->get_logger(), "Successfully activated controllers"); // NOLINT @@ -131,18 +153,23 @@ ControllerCoordinator::ControllerCoordinator() } else { RCLCPP_INFO(this->get_logger(), "Deactivating controllers and hardware interfaces"); // NOLINT for (const auto & deactivate_request : deactivate_hardware_requests_) { - hardware_client_->async_send_request( + auto future = hardware_client_->async_send_request( deactivate_request, [this, response, deactivate_request]( rclcpp::Client::SharedFuture - result_response) { // NOLINT + result_response) // NOLINT + -> void { const auto & result = result_response.get(); if (result->ok) { - RCLCPP_INFO( - this->get_logger(), "Successfully deactivated %s", deactivate_request->name.c_str()); // NOLINT + RCLCPP_INFO( // NOLINT + this->get_logger(), + "Successfully deactivated %s", + deactivate_request->name.c_str()); } else { - RCLCPP_ERROR( - this->get_logger(), "Failed to deactivate %s", deactivate_request->name.c_str()); // NOLINT + RCLCPP_ERROR( // NOLINT + this->get_logger(), + "Failed to deactivate %s", + deactivate_request->name.c_str()); response->success = false; response->message = "Failed to deactivate " + deactivate_request->name; } @@ -150,9 +177,9 @@ ControllerCoordinator::ControllerCoordinator() } switch_controller_client_->async_send_request( deactivate_controllers_request_, - [this, - response]( - rclcpp::Client::SharedFuture result_response) { // NOLINT + [this, response]( + rclcpp::Client::SharedFuture result_response) // NOLINT + -> void { const auto & result = result_response.get(); if (result->ok) { RCLCPP_INFO(this->get_logger(), "Successfully deactivated controllers"); // NOLINT From ebdabd0b7fe9b8b4aa7437a84b63e161c156166f Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Mon, 30 Mar 2026 00:14:45 -0700 Subject: [PATCH 2/9] Fixed impedance controller export --- impedance_controller/src/impedance_controller.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/impedance_controller/src/impedance_controller.cpp b/impedance_controller/src/impedance_controller.cpp index 0694f07..e3e5286 100644 --- a/impedance_controller/src/impedance_controller.cpp +++ b/impedance_controller/src/impedance_controller.cpp @@ -332,3 +332,6 @@ auto ImpedanceController::update_and_write_commands(const rclcpp::Time & time, c } } // namespace impedance_controller + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(impedance_controller::ImpedanceController, controller_interface::ChainableControllerInterface) From fb089e4401cceca2c7245f6e7c3db460e70ecea3 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Mon, 30 Mar 2026 00:22:19 -0700 Subject: [PATCH 3/9] debugging impedance controller --- impedance_controller/src/impedance_controller.cpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/impedance_controller/src/impedance_controller.cpp b/impedance_controller/src/impedance_controller.cpp index e3e5286..5eae94b 100644 --- a/impedance_controller/src/impedance_controller.cpp +++ b/impedance_controller/src/impedance_controller.cpp @@ -186,8 +186,17 @@ auto ImpedanceController::on_export_reference_interfaces() -> std::vectorget_name(), std::format("{}/{}", dof, hardware_interface::HW_IF_POSITION), &reference_interfaces_[i]); + if (i < 7) { + interfaces.emplace_back( + get_node()->get_name(), + std::format("{}/{}", dof, hardware_interface::HW_IF_POSITION), + &reference_interfaces_[i]); + } else { + interfaces.emplace_back( + get_node()->get_name(), + std::format("{}/{}", dof, hardware_interface::HW_IF_VELOCITY), + &reference_interfaces_[i]); + } } // add the force/torque interfaces From d854ff51c2246be2a27021222e5bbc89aea9f0ea Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Mon, 30 Mar 2026 00:36:32 -0700 Subject: [PATCH 4/9] debugging impedance controller --- impedance_controller/src/impedance_controller.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/impedance_controller/src/impedance_controller.cpp b/impedance_controller/src/impedance_controller.cpp index 5eae94b..c2371c7 100644 --- a/impedance_controller/src/impedance_controller.cpp +++ b/impedance_controller/src/impedance_controller.cpp @@ -23,6 +23,7 @@ #include #include +#include "auv_control_msgs/msg/impedance_command.hpp" #include "controller_common/common.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "tf2_eigen/tf2_eigen.hpp" @@ -107,6 +108,14 @@ auto ImpedanceController::on_configure(const rclcpp_lifecycle::State & /*previou system_state_values_.resize(n_state_dofs_, std::numeric_limits::quiet_NaN()); + // NOLINTNEXTLINE(performance-unnecessary-value-param) + reference_sub_ = get_node()->create_subscription( + "~/reference", + rclcpp::SystemDefaultsQoS(), + [this](const std::shared_ptr msg) { // NOLINT + reference_.writeFromNonRT(*msg); + }); + if (params_.use_external_measured_states) { RCLCPP_INFO(logger_, "Using external measured states"); // NOLINT system_state_sub_ = get_node()->create_subscription( From 419961bb9c611a285b1aaf0ae2d84d45aff9031d Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Mon, 30 Mar 2026 01:17:07 -0700 Subject: [PATCH 5/9] fix more bugs - need to change impedance status message --- impedance_controller/src/impedance_controller.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/impedance_controller/src/impedance_controller.cpp b/impedance_controller/src/impedance_controller.cpp index c2371c7..588e32d 100644 --- a/impedance_controller/src/impedance_controller.cpp +++ b/impedance_controller/src/impedance_controller.cpp @@ -44,7 +44,7 @@ auto geodesic_error(const geometry_msgs::msg::Pose & goal, const geometry_msgs:: Eigen::Isometry3d goal_mat, state_mat; // NOLINT tf2::fromMsg(goal, goal_mat); tf2::fromMsg(state, state_mat); - const Eigen::Matrix4d error = (goal_mat.inverse() * state_mat).matrix().log(); + const Eigen::Matrix4d error = (state_mat.inverse() * goal_mat).matrix().log(); return vee(error); } @@ -336,6 +336,7 @@ auto ImpedanceController::update_and_write_commands(const rclcpp::Time & time, c state.output = out.value_or(std::numeric_limits::quiet_NaN()); } + // TODO(evan-palmer): replace this with a custom message type // the feedback and reference values have different sizes than the command interfaces for (std::size_t i = 0; i < n_state_dofs_; ++i) { controller_state_.dof_states[i].feedback = system_state_values_[i]; From 148b43601400a63739c198b6576607ff0f32258a Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Mon, 30 Mar 2026 20:57:10 -0700 Subject: [PATCH 6/9] Replace impedance state message with custom message --- auv_control_msgs/CMakeLists.txt | 1 + .../msg/ImpedanceStateStamped.msg | 30 +++++++++++++++++ .../impedance_controller.hpp | 4 +-- impedance_controller/package.xml | 2 +- .../src/impedance_controller.cpp | 33 +++++++------------ 5 files changed, 46 insertions(+), 24 deletions(-) create mode 100644 auv_control_msgs/msg/ImpedanceStateStamped.msg diff --git a/auv_control_msgs/CMakeLists.txt b/auv_control_msgs/CMakeLists.txt index 78e50aa..0ae5a82 100644 --- a/auv_control_msgs/CMakeLists.txt +++ b/auv_control_msgs/CMakeLists.txt @@ -16,6 +16,7 @@ rosidl_generate_interfaces(auv_control_msgs "msg/CartesianTrajectoryControllerStateStamped.msg" "msg/ImpedanceCommand.msg" "action/FollowCartesianTrajectory.action" + "msg/ImpedanceStateStamped.msg" DEPENDENCIES builtin_interfaces std_msgs geometry_msgs trajectory_msgs ) diff --git a/auv_control_msgs/msg/ImpedanceStateStamped.msg b/auv_control_msgs/msg/ImpedanceStateStamped.msg new file mode 100644 index 0000000..4e9a183 --- /dev/null +++ b/auv_control_msgs/msg/ImpedanceStateStamped.msg @@ -0,0 +1,30 @@ +# This message represents the state of the impedance controller. + +std_msgs/Header header + +# The configuration setpoint. +geometry_msgs/Pose reference_pose + +# The velocity setpoint. +geometry_msgs/Twist reference_twist + +# The force/torque feed-forward setpoint. +geometry_msgs/Wrench reference_wrench + +# The measured configuration. +geometry_msgs/Pose feedback_pose + +# The measured velocity. +geometry_msgs/Twist feedback_twist + +# The vector representation of the left-trivialized error. +geometry_msgs/Twist error_pose + +# The velocity error. +geometry_msgs/Twist error_twist + +# Time between two consecutive updates/execution of the control law. +float64 time_step + +# The calculated control output. +geometry_msgs/Wrench output diff --git a/impedance_controller/include/impedance_controller/impedance_controller.hpp b/impedance_controller/include/impedance_controller/impedance_controller.hpp index 972c562..8378fff 100644 --- a/impedance_controller/include/impedance_controller/impedance_controller.hpp +++ b/impedance_controller/include/impedance_controller/impedance_controller.hpp @@ -23,7 +23,7 @@ #include #include "auv_control_msgs/msg/impedance_command.hpp" -#include "control_msgs/msg/multi_dof_state_stamped.hpp" +#include "auv_control_msgs/msg/impedance_state_stamped.hpp" #include "controller_interface/chainable_controller_interface.hpp" #include "controller_interface/controller_interface.hpp" #include "hydrodynamics/hydrodynamics.hpp" @@ -84,7 +84,7 @@ class ImpedanceController : public controller_interface::ChainableControllerInte std::shared_ptr> system_state_sub_; std::vector system_state_values_; - using ControllerState = control_msgs::msg::MultiDOFStateStamped; + using ControllerState = auv_control_msgs::msg::ImpedanceStateStamped; std::shared_ptr> controller_state_pub_; std::unique_ptr> rt_controller_state_pub_; ControllerState controller_state_; diff --git a/impedance_controller/package.xml b/impedance_controller/package.xml index 62b7af6..8826a76 100644 --- a/impedance_controller/package.xml +++ b/impedance_controller/package.xml @@ -25,12 +25,12 @@ hardware_interface rclcpp_lifecycle generate_parameter_library - control_msgs tf2_eigen geometry_msgs nav_msgs controller_common hydrodynamics + auv_control_msgs ament_lint_auto ament_lint_common diff --git a/impedance_controller/src/impedance_controller.cpp b/impedance_controller/src/impedance_controller.cpp index 588e32d..ad5579d 100644 --- a/impedance_controller/src/impedance_controller.cpp +++ b/impedance_controller/src/impedance_controller.cpp @@ -130,11 +130,6 @@ auto ImpedanceController::on_configure(const rclcpp_lifecycle::State & /*previou rt_controller_state_pub_ = std::make_unique>(controller_state_pub_); - controller_state_.dof_states.resize(n_command_dofs_); - for (auto && [state, dof] : std::views::zip(controller_state_.dof_states, command_dofs_)) { - state.name = dof; - } - return controller_interface::CallbackReturn::SUCCESS; } @@ -327,23 +322,19 @@ auto ImpedanceController::update_and_write_commands(const rclcpp::Time & time, c } } + // update and publish the controller state controller_state_.header.stamp = time; - for (auto && [i, state] : std::views::enumerate(controller_state_.dof_states)) { - const auto out = command_interfaces_[i].get_optional(); - state.error = pose_error_values[i]; - state.error_dot = twist_error_values[i]; - state.time_step = period.seconds(); - state.output = out.value_or(std::numeric_limits::quiet_NaN()); - } - - // TODO(evan-palmer): replace this with a custom message type - // the feedback and reference values have different sizes than the command interfaces - for (std::size_t i = 0; i < n_state_dofs_; ++i) { - controller_state_.dof_states[i].feedback = system_state_values_[i]; - } - for (std::size_t i = n_state_dofs_; i < n_reference_dofs_; ++i) { - controller_state_.dof_states[i].reference = reference_interfaces_[i]; - } + common::messages::to_msg(ref_pose_values, &controller_state_.reference_pose); + common::messages::to_msg(ref_twist_values, &controller_state_.reference_twist); + common::messages::to_msg(ref_wrench_values, &controller_state_.reference_wrench); + common::messages::to_msg(state_pose_values, &controller_state_.feedback_pose); + common::messages::to_msg(state_twist_values, &controller_state_.feedback_twist); + common::messages::to_msg(pose_error_values, &controller_state_.error_pose); + common::messages::to_msg(twist_error_values, &controller_state_.error_twist); + controller_state_.time_step = period.seconds(); + + std::vector output_values(t.data(), t.data() + t.size()); + common::messages::to_msg(output_values, &controller_state_.output); rt_controller_state_pub_->try_publish(controller_state_); From 50e8bfb38b5f1b3eef0c5ee9a7cbbd4aa411f10d Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Mon, 30 Mar 2026 21:40:30 -0700 Subject: [PATCH 7/9] prepare for pr --- auv_control_demos/CHANGELOG.md | 2 ++ auv_control_demos/package.xml | 5 +---- auv_control_msgs/CHANGELOG.md | 4 ++++ auv_control_msgs/msg/ImpedanceCommand.msg | 4 ++++ auv_control_msgs/msg/ImpedanceStateStamped.msg | 2 +- auv_control_msgs/package.xml | 3 +-- auv_controllers/CHANGELOG.md | 6 ++++++ auv_controllers/package.xml | 2 +- controller_common/CHANGELOG.md | 2 ++ controller_common/package.xml | 2 +- controller_coordinator/CHANGELOG.md | 4 ++++ controller_coordinator/package.xml | 2 +- ik_solvers/CHANGELOG.md | 2 ++ ik_solvers/package.xml | 2 +- impedance_controller/CHANGELOG.md | 5 +++++ impedance_controller/package.xml | 2 +- thruster_allocation_matrix_controller/CHANGELOG.md | 2 ++ thruster_allocation_matrix_controller/package.xml | 2 +- thruster_controllers/CHANGELOG.md | 2 ++ thruster_controllers/package.xml | 2 +- topic_sensors/CHANGELOG.md | 2 ++ topic_sensors/package.xml | 2 +- trajectory_controllers/CHANGELOG.md | 2 ++ velocity_controllers/CHANGELOG.md | 2 ++ velocity_controllers/package.xml | 2 +- whole_body_controllers/CHANGELOG.md | 2 ++ whole_body_controllers/package.xml | 2 +- 27 files changed, 54 insertions(+), 17 deletions(-) diff --git a/auv_control_demos/CHANGELOG.md b/auv_control_demos/CHANGELOG.md index 1c939e0..9acbaeb 100644 --- a/auv_control_demos/CHANGELOG.md +++ b/auv_control_demos/CHANGELOG.md @@ -1,5 +1,7 @@ # Changelog for package auv_control_demos +## 0.4.2 (2026-03-30) + ## 0.4.1 (2026-02-23) ## 0.4.0 (2025-08-01) diff --git a/auv_control_demos/package.xml b/auv_control_demos/package.xml index 24c78b3..cf701c3 100644 --- a/auv_control_demos/package.xml +++ b/auv_control_demos/package.xml @@ -3,12 +3,9 @@ auv_control_demos - 0.4.1 + 0.4.2 Example package that includes demos for using auv_controllers in individual and chained modes - Colin Mitchell - Everardo Gonzalez - Rakesh Vivekanandan Evan Palmer MIT diff --git a/auv_control_msgs/CHANGELOG.md b/auv_control_msgs/CHANGELOG.md index b3ddf9b..b6bbb5d 100644 --- a/auv_control_msgs/CHANGELOG.md +++ b/auv_control_msgs/CHANGELOG.md @@ -1,5 +1,9 @@ # Changelog for package auv_control_msgs +## 0.4.2 (2026-03-30) + +- Implements the ImpedanceStateStamped message. + ## 0.4.1 (2026-02-23) ## 0.4.0 (2025-08-01) diff --git a/auv_control_msgs/msg/ImpedanceCommand.msg b/auv_control_msgs/msg/ImpedanceCommand.msg index 9bf4d5b..570d17d 100644 --- a/auv_control_msgs/msg/ImpedanceCommand.msg +++ b/auv_control_msgs/msg/ImpedanceCommand.msg @@ -1,9 +1,13 @@ std_msgs/Header header +# The frame ID of the velocity and force/torque setpoints. string child_frame_id +# The configuration setpoint. geometry_msgs/Pose pose +# The velocity setpoint. geometry_msgs/Twist twist +# The force/torque setpoint. geometry_msgs/Wrench wrench diff --git a/auv_control_msgs/msg/ImpedanceStateStamped.msg b/auv_control_msgs/msg/ImpedanceStateStamped.msg index 4e9a183..708eb7c 100644 --- a/auv_control_msgs/msg/ImpedanceStateStamped.msg +++ b/auv_control_msgs/msg/ImpedanceStateStamped.msg @@ -20,7 +20,7 @@ geometry_msgs/Twist feedback_twist # The vector representation of the left-trivialized error. geometry_msgs/Twist error_pose -# The velocity error. +# The velocity error defined in the child frame ID. geometry_msgs/Twist error_twist # Time between two consecutive updates/execution of the control law. diff --git a/auv_control_msgs/package.xml b/auv_control_msgs/package.xml index 5f2d556..dad7bed 100644 --- a/auv_control_msgs/package.xml +++ b/auv_control_msgs/package.xml @@ -3,10 +3,9 @@ auv_control_msgs - 0.4.1 + 0.4.2 Custom messages for AUV controllers - Rakesh Vivekanandan Evan Palmer MIT diff --git a/auv_controllers/CHANGELOG.md b/auv_controllers/CHANGELOG.md index faea825..a5f4dd1 100644 --- a/auv_controllers/CHANGELOG.md +++ b/auv_controllers/CHANGELOG.md @@ -1,5 +1,11 @@ # Changelog for package auv_controllers +## 0.4.2 (2026-03-30) + +- Implements the ImpedanceStateStamped message +- Fixes controller coordinator hardware interface activation +- Fixes impedance controller memory leak and miscellaneous bugs + ## 0.4.1 (2026-02-23) ## 0.4.0 (2025-08-01) diff --git a/auv_controllers/package.xml b/auv_controllers/package.xml index 2e74cb7..0d99b18 100644 --- a/auv_controllers/package.xml +++ b/auv_controllers/package.xml @@ -3,7 +3,7 @@ auv_controllers - 0.4.1 + 0.4.2 Meta package for auv_controllers Evan Palmer diff --git a/controller_common/CHANGELOG.md b/controller_common/CHANGELOG.md index 1d713c2..97d9c80 100644 --- a/controller_common/CHANGELOG.md +++ b/controller_common/CHANGELOG.md @@ -1,5 +1,7 @@ # Changelog for package controller_common +## 0.4.2 (2026-03-30) + ## 0.4.1 (2026-02-23) ## 0.4.0 (2025-08-01) diff --git a/controller_common/package.xml b/controller_common/package.xml index 14c9fac..df32a9b 100644 --- a/controller_common/package.xml +++ b/controller_common/package.xml @@ -3,7 +3,7 @@ controller_common - 0.4.1 + 0.4.2 Common interfaces for controllers used in this project Evan Palmer diff --git a/controller_coordinator/CHANGELOG.md b/controller_coordinator/CHANGELOG.md index 7dd6687..c383a1f 100644 --- a/controller_coordinator/CHANGELOG.md +++ b/controller_coordinator/CHANGELOG.md @@ -1,5 +1,9 @@ # Changelog for package controller_coordinator +## 0.4.2 (2026-03-30) + +- Fixes hardware activation bug + ## 0.4.1 (2026-02-23) ## 0.4.0 (2025-08-01) diff --git a/controller_coordinator/package.xml b/controller_coordinator/package.xml index f68d0c1..34f0823 100644 --- a/controller_coordinator/package.xml +++ b/controller_coordinator/package.xml @@ -3,7 +3,7 @@ controller_coordinator - 0.4.1 + 0.4.2 A high-level node used to load and activate/deactivate control systems Evan Palmer diff --git a/ik_solvers/CHANGELOG.md b/ik_solvers/CHANGELOG.md index bc03e46..73c4758 100644 --- a/ik_solvers/CHANGELOG.md +++ b/ik_solvers/CHANGELOG.md @@ -1,5 +1,7 @@ # Changelog for package ik_solvers +## 0.4.2 (2026-03-30) + ## 0.4.1 (2026-02-23) ## 0.4.0 (2025-08-01) diff --git a/ik_solvers/package.xml b/ik_solvers/package.xml index 636f483..fccf7c9 100644 --- a/ik_solvers/package.xml +++ b/ik_solvers/package.xml @@ -3,7 +3,7 @@ ik_solvers - 0.4.1 + 0.4.2 Inverse kinematics solvers used for whole-body control Evan Palmer diff --git a/impedance_controller/CHANGELOG.md b/impedance_controller/CHANGELOG.md index d01d42a..4f1c56e 100644 --- a/impedance_controller/CHANGELOG.md +++ b/impedance_controller/CHANGELOG.md @@ -1,5 +1,10 @@ # Changelog for package impedance_controller +## 0.4.2 (2026-03-30) + +- Addresses various bugs in implementation +- Replaces MultiDOFStateStamped message with ImpedanceStateStamped message + ## 0.4.1 (2026-02-23) - Addresses upstream deprecation of the `tf2_ros/buffer.h` and diff --git a/impedance_controller/package.xml b/impedance_controller/package.xml index 8826a76..d9c3efb 100644 --- a/impedance_controller/package.xml +++ b/impedance_controller/package.xml @@ -3,7 +3,7 @@ impedance_controller - 0.4.1 + 0.4.2 An impedance controller for underwater vehicles Evan Palmer diff --git a/thruster_allocation_matrix_controller/CHANGELOG.md b/thruster_allocation_matrix_controller/CHANGELOG.md index 4f17095..e4786a7 100644 --- a/thruster_allocation_matrix_controller/CHANGELOG.md +++ b/thruster_allocation_matrix_controller/CHANGELOG.md @@ -1,5 +1,7 @@ # Changelog for package thruster_allocation_matrix_controller +## 0.4.2 (2026-03-30) + ## 0.4.1 (2026-02-23) ## 0.4.0 (2025-08-01) diff --git a/thruster_allocation_matrix_controller/package.xml b/thruster_allocation_matrix_controller/package.xml index 7cbb0b9..546740a 100644 --- a/thruster_allocation_matrix_controller/package.xml +++ b/thruster_allocation_matrix_controller/package.xml @@ -3,7 +3,7 @@ thruster_allocation_matrix_controller - 0.4.1 + 0.4.2 Thruster allocation matrix controller used to convert wrench commands into thrust commands Evan Palmer diff --git a/thruster_controllers/CHANGELOG.md b/thruster_controllers/CHANGELOG.md index 59b6a88..3f2442e 100644 --- a/thruster_controllers/CHANGELOG.md +++ b/thruster_controllers/CHANGELOG.md @@ -1,5 +1,7 @@ # Changelog for package thruster_controllers +## 0.4.2 (2026-03-30) + ## 0.4.1 (2026-02-23) ## 0.4.0 (2025-08-01) diff --git a/thruster_controllers/package.xml b/thruster_controllers/package.xml index ca4f8e6..39e4b8a 100644 --- a/thruster_controllers/package.xml +++ b/thruster_controllers/package.xml @@ -3,7 +3,7 @@ thruster_controllers - 0.4.1 + 0.4.2 A collection of thruster controllers for AUV control Evan Palmer diff --git a/topic_sensors/CHANGELOG.md b/topic_sensors/CHANGELOG.md index e5f4aca..d2f71bb 100644 --- a/topic_sensors/CHANGELOG.md +++ b/topic_sensors/CHANGELOG.md @@ -1,5 +1,7 @@ # Changelog for package topic_sensors +## 0.4.2 (2026-03-30) + ## 0.4.1 (2026-02-23) - Addresses upstream deprecation of `hardware_interface::HardwareInfo` and diff --git a/topic_sensors/package.xml b/topic_sensors/package.xml index 8fac624..a470696 100644 --- a/topic_sensors/package.xml +++ b/topic_sensors/package.xml @@ -3,7 +3,7 @@ topic_sensors - 0.4.1 + 0.4.2 Sensor plugins used to write ROS 2 messages to state interfaces Evan Palmer diff --git a/trajectory_controllers/CHANGELOG.md b/trajectory_controllers/CHANGELOG.md index 70950c6..0238b7c 100644 --- a/trajectory_controllers/CHANGELOG.md +++ b/trajectory_controllers/CHANGELOG.md @@ -1,5 +1,7 @@ # Changelog for package trajectory_controllers +## 0.4.2 (2026-03-30) + ## 0.4.1 (2026-02-23) - Addresses upstream deprecation of the `tf2_ros/buffer.h` and diff --git a/velocity_controllers/CHANGELOG.md b/velocity_controllers/CHANGELOG.md index f8067ef..080ed8b 100644 --- a/velocity_controllers/CHANGELOG.md +++ b/velocity_controllers/CHANGELOG.md @@ -1,5 +1,7 @@ # Changelog for package velocity_controllers +## 0.4.2 (2026-03-30) + ## 0.4.1 (2026-02-23) - Addresses upstream deprecation of the `tf2_ros/buffer.h` and diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 8463c4d..376378d 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -3,7 +3,7 @@ velocity_controllers - 0.4.1 + 0.4.2 A collection of velocity controllers for underwater vehicles Evan Palmer diff --git a/whole_body_controllers/CHANGELOG.md b/whole_body_controllers/CHANGELOG.md index 119c5ad..b0f9a3a 100644 --- a/whole_body_controllers/CHANGELOG.md +++ b/whole_body_controllers/CHANGELOG.md @@ -1,5 +1,7 @@ # Changelog for package whole_body_controllers +## 0.4.2 (2026-03-30) + ## 0.4.1 (2026-02-23) - Addresses upstream deprecation of the `tf2_ros/buffer.h` and diff --git a/whole_body_controllers/package.xml b/whole_body_controllers/package.xml index 4c9e33d..facd4e4 100644 --- a/whole_body_controllers/package.xml +++ b/whole_body_controllers/package.xml @@ -3,7 +3,7 @@ whole_body_controllers - 0.4.1 + 0.4.2 Whole-body controllers for underwater vehicle manipulator systems Evan Palmer From 05ebfa5855f713f8b5f9023f434c12465081dd7b Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Mon, 30 Mar 2026 22:50:15 -0700 Subject: [PATCH 8/9] test different pose error --- .../src/impedance_controller.cpp | 35 +++++++++++++++---- 1 file changed, 28 insertions(+), 7 deletions(-) diff --git a/impedance_controller/src/impedance_controller.cpp b/impedance_controller/src/impedance_controller.cpp index ad5579d..d844e61 100644 --- a/impedance_controller/src/impedance_controller.cpp +++ b/impedance_controller/src/impedance_controller.cpp @@ -34,18 +34,39 @@ namespace impedance_controller namespace { -auto vee(const Eigen::Matrix4d & mat) -> Eigen::Vector6d +// auto vee(const Eigen::Matrix4d & mat) -> Eigen::Vector6d +// { +// return {mat(0, 3), mat(1, 3), mat(2, 3), mat(2, 1), mat(0, 2), mat(1, 0)}; +// } + +auto quaternion_error(const Eigen::Quaterniond & q1, const Eigen::Quaterniond & q2) -> Eigen::Vector3d { - return {mat(0, 3), mat(1, 3), mat(2, 3), mat(2, 1), mat(0, 2), mat(1, 0)}; + 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()}; } auto geodesic_error(const geometry_msgs::msg::Pose & goal, const geometry_msgs::msg::Pose & state) -> Eigen::Vector6d { - Eigen::Isometry3d goal_mat, state_mat; // NOLINT - tf2::fromMsg(goal, goal_mat); - tf2::fromMsg(state, state_mat); - const Eigen::Matrix4d error = (state_mat.inverse() * goal_mat).matrix().log(); - return vee(error); + Eigen::Isometry3d _goal, _state; // NOLINT + tf2::fromMsg(goal, _goal); + tf2::fromMsg(state, _state); + // const Eigen::Matrix4d error = (state_mat.inverse() * goal_mat).matrix().log(); + // return vee(error); + + Eigen::Vector6d error = Eigen::Vector6d::Zero(6); + error.head<3>() = (_goal.translation() - _state.translation()).eval(); + error.tail<3>() = quaternion_error(Eigen::Quaterniond(_goal.rotation()), Eigen::Quaterniond(_state.rotation())); + return error; } } // namespace From 93f3bdec0f3ffad21cc58c5265288631c7ad0a7e Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Tue, 31 Mar 2026 00:30:14 -0700 Subject: [PATCH 9/9] geodesic distance is fine, im just dumb --- .../src/impedance_controller.cpp | 29 +++---------------- 1 file changed, 4 insertions(+), 25 deletions(-) diff --git a/impedance_controller/src/impedance_controller.cpp b/impedance_controller/src/impedance_controller.cpp index d844e61..fa8d31b 100644 --- a/impedance_controller/src/impedance_controller.cpp +++ b/impedance_controller/src/impedance_controller.cpp @@ -34,25 +34,9 @@ namespace impedance_controller namespace { -// auto vee(const Eigen::Matrix4d & mat) -> Eigen::Vector6d -// { -// return {mat(0, 3), mat(1, 3), mat(2, 3), mat(2, 1), mat(0, 2), mat(1, 0)}; -// } - -auto quaternion_error(const Eigen::Quaterniond & q1, const Eigen::Quaterniond & q2) -> Eigen::Vector3d +auto vee(const Eigen::Matrix4d & mat) -> Eigen::Vector6d { - 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()}; + return {mat(0, 3), mat(1, 3), mat(2, 3), mat(2, 1), mat(0, 2), mat(1, 0)}; } auto geodesic_error(const geometry_msgs::msg::Pose & goal, const geometry_msgs::msg::Pose & state) -> Eigen::Vector6d @@ -60,13 +44,8 @@ auto geodesic_error(const geometry_msgs::msg::Pose & goal, const geometry_msgs:: Eigen::Isometry3d _goal, _state; // NOLINT tf2::fromMsg(goal, _goal); tf2::fromMsg(state, _state); - // const Eigen::Matrix4d error = (state_mat.inverse() * goal_mat).matrix().log(); - // return vee(error); - - Eigen::Vector6d error = Eigen::Vector6d::Zero(6); - error.head<3>() = (_goal.translation() - _state.translation()).eval(); - error.tail<3>() = quaternion_error(Eigen::Quaterniond(_goal.rotation()), Eigen::Quaterniond(_state.rotation())); - return error; + const Eigen::Matrix4d error = (_state.inverse() * _goal).matrix().log(); + return vee(error); } } // namespace