From 67e55bfa11b6fe2665c72b1656173e8f4754f7a8 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 6 Dec 2023 12:35:56 +0000 Subject: [PATCH 01/30] First draft of trajectory controllers plugins --- joint_trajectory_controller/CMakeLists.txt | 39 +++++++ .../joint_trajectory_controller.hpp | 19 ++- .../trajectory_controller_base.hpp | 70 +++++++++++ .../src/joint_trajectory_controller.cpp | 69 +++++------ .../src/pid_trajectory.cpp | 109 ++++++++++++++++++ .../test/test_load_pid_trajectory.cpp | 47 ++++++++ .../trajectory_controllers.xml | 6 + 7 files changed, 309 insertions(+), 50 deletions(-) create mode 100644 joint_trajectory_controller/include/joint_trajectory_controller/trajectory_controller_base.hpp create mode 100644 joint_trajectory_controller/src/pid_trajectory.cpp create mode 100644 joint_trajectory_controller/test/test_load_pid_trajectory.cpp create mode 100644 joint_trajectory_controller/trajectory_controllers.xml diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index 88bb7891a3..96cb2b05cd 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -51,6 +51,26 @@ ament_target_dependencies(joint_trajectory_controller PUBLIC ${THIS_PACKAGE_INCL target_compile_definitions(joint_trajectory_controller PRIVATE "JOINT_TRAJECTORY_CONTROLLER_BUILDING_DLL" "_USE_MATH_DEFINES") pluginlib_export_plugin_description_file(controller_interface joint_trajectory_plugin.xml) +# trajectory controllers shipped with JTC +add_library(pid_trajectory SHARED + src/pid_trajectory.cpp +) +target_compile_features(pid_trajectory PUBLIC cxx_std_17) +target_include_directories(pid_trajectory PUBLIC + $ + $ +) +target_link_libraries(pid_trajectory PUBLIC + joint_trajectory_controller_parameters +) +ament_target_dependencies(pid_trajectory PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +# target_compile_definitions(pid_trajectory PRIVATE "JOINT_TRAJECTORY_CONTROLLER_BUILDING_DLL" "_USE_MATH_DEFINES") +target_compile_definitions(pid_trajectory PRIVATE "JOINT_TRAJECTORY_CONTROLLER_BUILDING_LIBRARY") +pluginlib_export_plugin_description_file(joint_trajectory_controller trajectory_controllers.xml) + if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(controller_manager REQUIRED) @@ -86,6 +106,16 @@ if(BUILD_TESTING) target_link_libraries(test_trajectory_actions joint_trajectory_controller ) + + ament_add_gmock(test_load_pid_trajectory + test/test_load_pid_trajectory.cpp + ) + target_link_libraries(test_load_pid_trajectory + joint_trajectory_controller + ) + ament_target_dependencies(test_load_pid_trajectory + ${THIS_PACKAGE_INCLUDE_DEPENDS} + ) endif() @@ -96,6 +126,7 @@ install( install(TARGETS joint_trajectory_controller joint_trajectory_controller_parameters + pid_trajectory EXPORT export_joint_trajectory_controller RUNTIME DESTINATION bin ARCHIVE DESTINATION lib @@ -104,4 +135,12 @@ install(TARGETS ament_export_targets(export_joint_trajectory_controller HAS_LIBRARY_TARGET) ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) + +ament_export_libraries( + joint_trajectory_controller +) +ament_export_libraries( + pid_trajectory +) + ament_package() diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index d16e4f8267..4f9844482e 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -25,12 +25,8 @@ #include "control_msgs/action/follow_joint_trajectory.hpp" #include "control_msgs/msg/joint_trajectory_controller_state.hpp" #include "control_msgs/srv/query_trajectory_state.hpp" -#include "control_toolbox/pid.hpp" #include "controller_interface/controller_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "joint_trajectory_controller/interpolation_methods.hpp" -#include "joint_trajectory_controller/tolerances.hpp" -#include "joint_trajectory_controller/visibility_control.h" #include "rclcpp/duration.hpp" #include "rclcpp/subscription.hpp" #include "rclcpp/time.hpp" @@ -45,8 +41,11 @@ #include "trajectory_msgs/msg/joint_trajectory.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" -// auto-generated by generate_parameter_library -#include "joint_trajectory_controller_parameters.hpp" +#include "joint_trajectory_controller/interpolation_methods.hpp" +#include "joint_trajectory_controller/tolerances.hpp" +#include "joint_trajectory_controller/trajectory_controller_base.hpp" +#include "joint_trajectory_controller/visibility_control.h" +#include "joint_trajectory_controller_parameters.hpp" // auto-generated by generate_parameter_library using namespace std::chrono_literals; // NOLINT @@ -163,10 +162,8 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa /// If true, a velocity feedforward term plus corrective PID term is used bool use_closed_loop_pid_adapter_ = false; - using PidPtr = std::shared_ptr; - std::vector pids_; - // Feed-forward velocity weight factor when calculating closed loop pid adapter's command - std::vector ff_velocity_scale_; + // The actual trajectory controller used for closed loop pid adapter + std::shared_ptr traj_contr_; // Configuration for every joint, if position error is wrapped around std::vector joints_angle_wraparound_; // reserved storage for result of the command when closed loop pid adapter is used @@ -285,8 +282,6 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa std::shared_ptr response); private: - void update_pids(); - bool contains_interface_type( const std::vector & interface_type_list, const std::string & interface_type); diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory_controller_base.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory_controller_base.hpp new file mode 100644 index 0000000000..00973042fb --- /dev/null +++ b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory_controller_base.hpp @@ -0,0 +1,70 @@ +// Copyright (c) 2023 AIT Austrian Institute of Technology +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef JOINT_TRAJECTORY_CONTROLLER__TRAJECTORY_CONTROLLER_BASE_HPP_ +#define JOINT_TRAJECTORY_CONTROLLER__TRAJECTORY_CONTROLLER_BASE_HPP_ + +#include + +#include "rclcpp/rclcpp.hpp" + +#include "rclcpp/time.hpp" +#include "trajectory_msgs/msg/joint_trajectory.hpp" +#include "trajectory_msgs/msg/joint_trajectory_point.hpp" + +#include "joint_trajectory_controller/visibility_control.h" + +namespace joint_trajectory_controller +{ +class TrajectoryControllerBase +{ +public: + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + TrajectoryControllerBase() = default; + + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + virtual ~TrajectoryControllerBase() = default; + + /** + */ + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + virtual bool initialize(rclcpp_lifecycle::LifecycleNode::SharedPtr node) = 0; + + /** + */ + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + virtual bool computeGains(const trajectory_msgs::msg::JointTrajectory trajectory) = 0; + + /** + */ + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + virtual void computeCommands( + std::vector & tmp_command, const trajectory_msgs::msg::JointTrajectoryPoint current, + const trajectory_msgs::msg::JointTrajectoryPoint error, + const trajectory_msgs::msg::JointTrajectoryPoint desired, const rclcpp::Time & time, + const rclcpp::Duration & period) = 0; + + /** + */ + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + virtual void reset() = 0; + +protected: + // the node handle for parameter handling + rclcpp::Node::SharedPtr node_; +}; + +} // namespace joint_trajectory_controller + +#endif // JOINT_TRAJECTORY_CONTROLLER__TRAJECTORY_CONTROLLER_BASE_HPP_ diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 59e2c408c5..cd7daadd88 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -29,8 +29,8 @@ #include "controller_interface/helpers.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "joint_trajectory_controller/trajectory.hpp" #include "lifecycle_msgs/msg/state.hpp" +#include "pluginlib/class_loader.hpp" #include "rclcpp/event_handler.hpp" #include "rclcpp/logging.hpp" #include "rclcpp/parameter.hpp" @@ -43,6 +43,8 @@ #include "rclcpp_lifecycle/state.hpp" #include "std_msgs/msg/header.hpp" +#include "joint_trajectory_controller/trajectory.hpp" + namespace joint_trajectory_controller { JointTrajectoryController::JointTrajectoryController() @@ -279,14 +281,8 @@ controller_interface::return_type JointTrajectoryController::update( { if (use_closed_loop_pid_adapter_) { - // Update PIDs - for (auto i = 0ul; i < dof_; ++i) - { - tmp_command_[i] = (state_desired_.velocities[i] * ff_velocity_scale_[i]) + - pids_[i]->computeCommand( - state_error_.positions[i], state_error_.velocities[i], - (uint64_t)period.nanoseconds()); - } + traj_contr_->computeCommands( + tmp_command_, state_current_, state_error_, state_desired_, time, period); } // set values for next hardware write() @@ -712,11 +708,31 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( if (use_closed_loop_pid_adapter_) { - pids_.resize(dof_); - ff_velocity_scale_.resize(dof_); - tmp_command_.resize(dof_, 0.0); + pluginlib::ClassLoader + traj_controller_loader( + "joint_trajectory_controller", "joint_trajectory_controller::TrajectoryControllerBase"); + try + { + RCLCPP_INFO(logger, "Load the library"); + traj_contr_ = traj_controller_loader.createSharedInstance( + "joint_trajectory_controller::PidTrajectoryController"); + RCLCPP_INFO(logger, "Loaded the library"); + } + catch (pluginlib::PluginlibException & ex) + { + RCLCPP_FATAL( + logger, "The trajectory controller plugin failed to load for some reason. Error: %s\n", + ex.what()); + return CallbackReturn::FAILURE; + } + if (!traj_contr_->initialize(get_node())) + { + RCLCPP_FATAL( + logger, "The trajectory controller plugin failed to initialize for some reason. Aborting."); + return CallbackReturn::FAILURE; + } - update_pids(); + tmp_command_.resize(dof_, 0.0); } // Configure joint position error normalization from ROS parameters (angle_wraparound) @@ -1036,12 +1052,9 @@ bool JointTrajectoryController::reset() subscriber_is_active_ = false; joint_command_subscriber_.reset(); - for (const auto & pid : pids_) + if (traj_contr_) { - if (pid) - { - pid->reset(); - } + traj_contr_->reset(); } traj_external_point_ptr_.reset(); @@ -1528,26 +1541,6 @@ bool JointTrajectoryController::has_active_trajectory() const return traj_external_point_ptr_ != nullptr && traj_external_point_ptr_->has_trajectory_msg(); } -void JointTrajectoryController::update_pids() -{ - for (size_t i = 0; i < dof_; ++i) - { - const auto & gains = params_.gains.joints_map.at(params_.joints[i]); - if (pids_[i]) - { - // update PIDs with gains from ROS parameters - pids_[i]->setGains(gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp); - } - else - { - // Init PIDs with gains from ROS parameters - pids_[i] = std::make_shared( - gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp); - } - ff_velocity_scale_[i] = gains.ff_velocity_scale; - } -} - void JointTrajectoryController::init_hold_position_msg() { hold_position_msg_ptr_ = std::make_shared(); diff --git a/joint_trajectory_controller/src/pid_trajectory.cpp b/joint_trajectory_controller/src/pid_trajectory.cpp new file mode 100644 index 0000000000..6cc446c4cb --- /dev/null +++ b/joint_trajectory_controller/src/pid_trajectory.cpp @@ -0,0 +1,109 @@ +// Copyright (c) 2023 AIT Austrian Institute of Technology +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "control_toolbox/pid.hpp" + +#include "joint_trajectory_controller/trajectory_controller_base.hpp" +#include "joint_trajectory_controller_parameters.hpp" // auto-generated by generate_parameter_library + +using PidPtr = std::shared_ptr; + +namespace joint_trajectory_controller +{ +class PidTrajectoryController : public TrajectoryControllerBase +{ +public: + bool initialize(rclcpp_lifecycle::LifecycleNode::SharedPtr node) override + { + node_ = node; + + try + { + // Create the parameter listener and get the parameters + param_listener_ = std::make_shared(node_); + params_ = param_listener_->get_params(); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return false; + } + + size_t dof_ = params_.joints.size(); + pids_.resize(dof_); + ff_velocity_scale_.resize(dof_); + + // Init PID gains from ROS parameters + for (size_t i = 0; i < dof_; ++i) + { + const auto & gains = params_.gains.joints_map.at(params_.joints[i]); + pids_[i] = std::make_shared( + gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp); + + ff_velocity_scale_[i] = gains.ff_velocity_scale; + } + return true; + } + + bool computeGains(const trajectory_msgs::msg::JointTrajectory trajectory) override + { + return true; + } + + void computeCommands( + std::vector & tmp_command, const trajectory_msgs::msg::JointTrajectoryPoint current, + const trajectory_msgs::msg::JointTrajectoryPoint error, + const trajectory_msgs::msg::JointTrajectoryPoint desired, const rclcpp::Time & time, + const rclcpp::Duration & period) override + { + // Update PIDs + for (auto i = 0ul; i < tmp_command.size(); ++i) + { + tmp_command[i] = (desired.velocities[i] * ff_velocity_scale_[i]) + + pids_[i]->computeCommand( + error.positions[i], error.velocities[i], (uint64_t)period.nanoseconds()); + } + } + + void reset() override + { + for (const auto & pid : pids_) + { + if (pid) + { + pid->reset(); + } + } + } + +protected: + // the node handle for parameter handling + rclcpp_lifecycle::LifecycleNode::SharedPtr node_; + // PID controllers + std::vector pids_; + // Feed-forward velocity weight factor when calculating closed loop pid adapter's command + std::vector ff_velocity_scale_; + + // Parameters from ROS for joint_trajectory_controller + std::shared_ptr param_listener_; + Params params_; +}; + +} // namespace joint_trajectory_controller + +#include + +PLUGINLIB_EXPORT_CLASS( + joint_trajectory_controller::PidTrajectoryController, + joint_trajectory_controller::TrajectoryControllerBase) diff --git a/joint_trajectory_controller/test/test_load_pid_trajectory.cpp b/joint_trajectory_controller/test/test_load_pid_trajectory.cpp new file mode 100644 index 0000000000..74a39c2bd4 --- /dev/null +++ b/joint_trajectory_controller/test/test_load_pid_trajectory.cpp @@ -0,0 +1,47 @@ +// Copyright 2020 PAL Robotics SL. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "gmock/gmock.h" + +#include "joint_trajectory_controller/trajectory_controller_base.hpp" +#include "pluginlib/class_loader.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +TEST(TestLoadPidController, load_controller) +{ + rclcpp::init(0, nullptr); + + pluginlib::ClassLoader + traj_controller_loader( + "joint_trajectory_controller", "joint_trajectory_controller::TrajectoryControllerBase"); + + std::shared_ptr traj_contr; + + auto available_classes = traj_controller_loader.getDeclaredClasses(); + std::stringstream sstr; + sstr << "available filters:" << std::endl; + for (const auto & available_class : available_classes) + { + sstr << " " << available_class << std::endl; + } + + std::string controller_type = "joint_trajectory_controller::PidTrajectoryController"; + ASSERT_TRUE(traj_controller_loader.isClassAvailable(controller_type)) << sstr.str(); + ASSERT_NO_THROW(traj_contr = traj_controller_loader.createSharedInstance(controller_type)); + + rclcpp::shutdown(); +} diff --git a/joint_trajectory_controller/trajectory_controllers.xml b/joint_trajectory_controller/trajectory_controllers.xml new file mode 100644 index 0000000000..9b89e123e3 --- /dev/null +++ b/joint_trajectory_controller/trajectory_controllers.xml @@ -0,0 +1,6 @@ + + + A trajectory controller consisting of a PID controller per joint. + + From 82b5ce0ac5c9295afe6f4e1764b3dc30762730e0 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 6 Dec 2023 12:35:56 +0000 Subject: [PATCH 02/30] Move pid plugin to own package --- joint_trajectory_controller/CMakeLists.txt | 34 --- .../trajectory_controller_base.hpp | 2 +- .../src/pid_trajectory.cpp | 109 ---------- .../test/test_trajectory_controller.cpp | 87 ++++---- .../trajectory_controllers.xml | 6 - trajectory_plugins/CMakeLists.txt | 74 +++++++ trajectory_plugins/LICENSE | 202 ++++++++++++++++++ .../pid_trajectory_plugin.hpp | 62 ++++++ .../trajectory_plugins/visibility_control.h | 49 +++++ trajectory_plugins/package.xml | 23 ++ trajectory_plugins/plugins.xml | 6 + .../src/pid_trajectory_plugin.cpp | 88 ++++++++ .../src/pid_trajectory_plugin_parameters.yaml | 48 +++++ .../test/test_load_pid_trajectory.cpp | 13 +- 14 files changed, 606 insertions(+), 197 deletions(-) delete mode 100644 joint_trajectory_controller/src/pid_trajectory.cpp delete mode 100644 joint_trajectory_controller/trajectory_controllers.xml create mode 100644 trajectory_plugins/CMakeLists.txt create mode 100644 trajectory_plugins/LICENSE create mode 100644 trajectory_plugins/include/trajectory_plugins/pid_trajectory_plugin.hpp create mode 100644 trajectory_plugins/include/trajectory_plugins/visibility_control.h create mode 100644 trajectory_plugins/package.xml create mode 100644 trajectory_plugins/plugins.xml create mode 100644 trajectory_plugins/src/pid_trajectory_plugin.cpp create mode 100644 trajectory_plugins/src/pid_trajectory_plugin_parameters.yaml rename {joint_trajectory_controller => trajectory_plugins}/test/test_load_pid_trajectory.cpp (84%) diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index 96cb2b05cd..93269c9d4b 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -51,26 +51,6 @@ ament_target_dependencies(joint_trajectory_controller PUBLIC ${THIS_PACKAGE_INCL target_compile_definitions(joint_trajectory_controller PRIVATE "JOINT_TRAJECTORY_CONTROLLER_BUILDING_DLL" "_USE_MATH_DEFINES") pluginlib_export_plugin_description_file(controller_interface joint_trajectory_plugin.xml) -# trajectory controllers shipped with JTC -add_library(pid_trajectory SHARED - src/pid_trajectory.cpp -) -target_compile_features(pid_trajectory PUBLIC cxx_std_17) -target_include_directories(pid_trajectory PUBLIC - $ - $ -) -target_link_libraries(pid_trajectory PUBLIC - joint_trajectory_controller_parameters -) -ament_target_dependencies(pid_trajectory PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) - -# Causes the visibility macros to use dllexport rather than dllimport, -# which is appropriate when building the dll but not consuming it. -# target_compile_definitions(pid_trajectory PRIVATE "JOINT_TRAJECTORY_CONTROLLER_BUILDING_DLL" "_USE_MATH_DEFINES") -target_compile_definitions(pid_trajectory PRIVATE "JOINT_TRAJECTORY_CONTROLLER_BUILDING_LIBRARY") -pluginlib_export_plugin_description_file(joint_trajectory_controller trajectory_controllers.xml) - if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(controller_manager REQUIRED) @@ -106,16 +86,6 @@ if(BUILD_TESTING) target_link_libraries(test_trajectory_actions joint_trajectory_controller ) - - ament_add_gmock(test_load_pid_trajectory - test/test_load_pid_trajectory.cpp - ) - target_link_libraries(test_load_pid_trajectory - joint_trajectory_controller - ) - ament_target_dependencies(test_load_pid_trajectory - ${THIS_PACKAGE_INCLUDE_DEPENDS} - ) endif() @@ -126,7 +96,6 @@ install( install(TARGETS joint_trajectory_controller joint_trajectory_controller_parameters - pid_trajectory EXPORT export_joint_trajectory_controller RUNTIME DESTINATION bin ARCHIVE DESTINATION lib @@ -139,8 +108,5 @@ ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_export_libraries( joint_trajectory_controller ) -ament_export_libraries( - pid_trajectory -) ament_package() diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory_controller_base.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory_controller_base.hpp index 00973042fb..e13505a24e 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory_controller_base.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory_controller_base.hpp @@ -18,8 +18,8 @@ #include #include "rclcpp/rclcpp.hpp" - #include "rclcpp/time.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" #include "trajectory_msgs/msg/joint_trajectory.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" diff --git a/joint_trajectory_controller/src/pid_trajectory.cpp b/joint_trajectory_controller/src/pid_trajectory.cpp deleted file mode 100644 index 6cc446c4cb..0000000000 --- a/joint_trajectory_controller/src/pid_trajectory.cpp +++ /dev/null @@ -1,109 +0,0 @@ -// Copyright (c) 2023 AIT Austrian Institute of Technology -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "control_toolbox/pid.hpp" - -#include "joint_trajectory_controller/trajectory_controller_base.hpp" -#include "joint_trajectory_controller_parameters.hpp" // auto-generated by generate_parameter_library - -using PidPtr = std::shared_ptr; - -namespace joint_trajectory_controller -{ -class PidTrajectoryController : public TrajectoryControllerBase -{ -public: - bool initialize(rclcpp_lifecycle::LifecycleNode::SharedPtr node) override - { - node_ = node; - - try - { - // Create the parameter listener and get the parameters - param_listener_ = std::make_shared(node_); - params_ = param_listener_->get_params(); - } - catch (const std::exception & e) - { - fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); - return false; - } - - size_t dof_ = params_.joints.size(); - pids_.resize(dof_); - ff_velocity_scale_.resize(dof_); - - // Init PID gains from ROS parameters - for (size_t i = 0; i < dof_; ++i) - { - const auto & gains = params_.gains.joints_map.at(params_.joints[i]); - pids_[i] = std::make_shared( - gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp); - - ff_velocity_scale_[i] = gains.ff_velocity_scale; - } - return true; - } - - bool computeGains(const trajectory_msgs::msg::JointTrajectory trajectory) override - { - return true; - } - - void computeCommands( - std::vector & tmp_command, const trajectory_msgs::msg::JointTrajectoryPoint current, - const trajectory_msgs::msg::JointTrajectoryPoint error, - const trajectory_msgs::msg::JointTrajectoryPoint desired, const rclcpp::Time & time, - const rclcpp::Duration & period) override - { - // Update PIDs - for (auto i = 0ul; i < tmp_command.size(); ++i) - { - tmp_command[i] = (desired.velocities[i] * ff_velocity_scale_[i]) + - pids_[i]->computeCommand( - error.positions[i], error.velocities[i], (uint64_t)period.nanoseconds()); - } - } - - void reset() override - { - for (const auto & pid : pids_) - { - if (pid) - { - pid->reset(); - } - } - } - -protected: - // the node handle for parameter handling - rclcpp_lifecycle::LifecycleNode::SharedPtr node_; - // PID controllers - std::vector pids_; - // Feed-forward velocity weight factor when calculating closed loop pid adapter's command - std::vector ff_velocity_scale_; - - // Parameters from ROS for joint_trajectory_controller - std::shared_ptr param_listener_; - Params params_; -}; - -} // namespace joint_trajectory_controller - -#include - -PLUGINLIB_EXPORT_CLASS( - joint_trajectory_controller::PidTrajectoryController, - joint_trajectory_controller::TrajectoryControllerBase) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 05010c562c..73b2dddd9b 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -58,6 +58,7 @@ using test_trajectory_controllers::TrajectoryControllerTestParameterized; void spin(rclcpp::executors::MultiThreadedExecutor * exe) { exe->spin(); } +#if 0 TEST_P(TrajectoryControllerTestParameterized, configure_state_ignores_commands) { rclcpp::executors::MultiThreadedExecutor executor; @@ -463,7 +464,7 @@ TEST_P(TrajectoryControllerTestParameterized, hold_on_startup) executor.cancel(); } - +#endif // Floating-point value comparison threshold const double EPS = 1e-6; /** @@ -694,7 +695,7 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound) executor.cancel(); } - +#if 0 /** * @brief cmd_timeout must be greater than constraints.goal_time */ @@ -829,6 +830,7 @@ TEST_P(TrajectoryControllerTestParameterized, timeout) executor.cancel(); } +#endif /** * @brief check if use_closed_loop_pid is active @@ -850,7 +852,7 @@ TEST_P(TrajectoryControllerTestParameterized, use_closed_loop_pid) EXPECT_TRUE(traj_controller_->use_closed_loop_pid_adapter()); } } - +#if 0 /** * @brief check if velocity error is calculated correctly */ @@ -1836,44 +1838,47 @@ TEST_P(TrajectoryControllerTestParameterized, test_goal_tolerances_fail) // it should be still holding the old point expectCommandPoint(hold_position); } - -// position controllers -INSTANTIATE_TEST_SUITE_P( - PositionTrajectoryControllers, TrajectoryControllerTestParameterized, - ::testing::Values( - std::make_tuple(std::vector({"position"}), std::vector({"position"})), - std::make_tuple( - std::vector({"position"}), std::vector({"position", "velocity"})), - std::make_tuple( - std::vector({"position"}), - std::vector({"position", "velocity", "acceleration"})))); - -// position_velocity controllers -INSTANTIATE_TEST_SUITE_P( - PositionVelocityTrajectoryControllers, TrajectoryControllerTestParameterized, - ::testing::Values( - std::make_tuple( - std::vector({"position", "velocity"}), std::vector({"position"})), - std::make_tuple( - std::vector({"position", "velocity"}), - std::vector({"position", "velocity"})), - std::make_tuple( - std::vector({"position", "velocity"}), - std::vector({"position", "velocity", "acceleration"})))); - -// position_velocity_acceleration controllers -INSTANTIATE_TEST_SUITE_P( - PositionVelocityAccelerationTrajectoryControllers, TrajectoryControllerTestParameterized, - ::testing::Values( - std::make_tuple( - std::vector({"position", "velocity", "acceleration"}), - std::vector({"position"})), - std::make_tuple( - std::vector({"position", "velocity", "acceleration"}), - std::vector({"position", "velocity"})), - std::make_tuple( - std::vector({"position", "velocity", "acceleration"}), - std::vector({"position", "velocity", "acceleration"})))); +#endif + +// // position controllers +// INSTANTIATE_TEST_SUITE_P( +// PositionTrajectoryControllers, TrajectoryControllerTestParameterized, +// ::testing::Values( +// std::make_tuple(std::vector({"position"}), +// std::vector({"position"})), std::make_tuple( +// std::vector({"position"}), std::vector({"position", +// "velocity"})), +// std::make_tuple( +// std::vector({"position"}), +// std::vector({"position", "velocity", "acceleration"})))); + +// // position_velocity controllers +// INSTANTIATE_TEST_SUITE_P( +// PositionVelocityTrajectoryControllers, TrajectoryControllerTestParameterized, +// ::testing::Values( +// std::make_tuple( +// std::vector({"position", "velocity"}), +// std::vector({"position"})), +// std::make_tuple( +// std::vector({"position", "velocity"}), +// std::vector({"position", "velocity"})), +// std::make_tuple( +// std::vector({"position", "velocity"}), +// std::vector({"position", "velocity", "acceleration"})))); + +// // position_velocity_acceleration controllers +// INSTANTIATE_TEST_SUITE_P( +// PositionVelocityAccelerationTrajectoryControllers, TrajectoryControllerTestParameterized, +// ::testing::Values( +// std::make_tuple( +// std::vector({"position", "velocity", "acceleration"}), +// std::vector({"position"})), +// std::make_tuple( +// std::vector({"position", "velocity", "acceleration"}), +// std::vector({"position", "velocity"})), +// std::make_tuple( +// std::vector({"position", "velocity", "acceleration"}), +// std::vector({"position", "velocity", "acceleration"})))); // only velocity controller INSTANTIATE_TEST_SUITE_P( diff --git a/joint_trajectory_controller/trajectory_controllers.xml b/joint_trajectory_controller/trajectory_controllers.xml deleted file mode 100644 index 9b89e123e3..0000000000 --- a/joint_trajectory_controller/trajectory_controllers.xml +++ /dev/null @@ -1,6 +0,0 @@ - - - A trajectory controller consisting of a PID controller per joint. - - diff --git a/trajectory_plugins/CMakeLists.txt b/trajectory_plugins/CMakeLists.txt new file mode 100644 index 0000000000..d0474e06d9 --- /dev/null +++ b/trajectory_plugins/CMakeLists.txt @@ -0,0 +1,74 @@ +cmake_minimum_required(VERSION 3.8) +project(trajectory_plugins) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_ros REQUIRED) +find_package(joint_trajectory_controller REQUIRED) +find_package(control_toolbox REQUIRED) +find_package(pluginlib REQUIRED) +find_package(generate_parameter_library REQUIRED) + +generate_parameter_library(pid_trajectory_plugin_parameters + src/pid_trajectory_plugin_parameters.yaml +) + +add_library(pid_trajectory_plugin SHARED src/pid_trajectory_plugin.cpp) +add_library(trajectory_plugins::pid_trajectory_plugin ALIAS pid_trajectory_plugin) +target_compile_features(pid_trajectory_plugin PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +target_include_directories(pid_trajectory_plugin PUBLIC + $ + $) +ament_target_dependencies(pid_trajectory_plugin PUBLIC + "joint_trajectory_controller" + "control_toolbox" + "pluginlib" + "generate_parameter_library" +) +target_link_libraries(pid_trajectory_plugin PUBLIC + pid_trajectory_plugin_parameters +) +pluginlib_export_plugin_description_file(joint_trajectory_controller plugins.xml) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(pid_trajectory_plugin PRIVATE "TRAJECTORY_PLUGINS_BUILDING_LIBRARY") + +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) +install( + TARGETS pid_trajectory_plugin pid_trajectory_plugin_parameters + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +if(BUILD_TESTING) + ament_add_gmock(test_load_pid_trajectory test/test_load_pid_trajectory.cpp) + target_link_libraries(test_load_pid_trajectory pid_trajectory_plugin_parameters) + ament_target_dependencies(test_load_pid_trajectory + "joint_trajectory_controller" + "rclcpp" + "pluginlib" + "generate_parameter_library" + ) +endif() + +ament_export_include_directories( + "include/${PROJECT_NAME}" +) +ament_export_libraries( + pid_trajectory_plugin +) +ament_export_targets( + export_${PROJECT_NAME} +) + +ament_package() diff --git a/trajectory_plugins/LICENSE b/trajectory_plugins/LICENSE new file mode 100644 index 0000000000..d645695673 --- /dev/null +++ b/trajectory_plugins/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/trajectory_plugins/include/trajectory_plugins/pid_trajectory_plugin.hpp b/trajectory_plugins/include/trajectory_plugins/pid_trajectory_plugin.hpp new file mode 100644 index 0000000000..8eaf577ffd --- /dev/null +++ b/trajectory_plugins/include/trajectory_plugins/pid_trajectory_plugin.hpp @@ -0,0 +1,62 @@ +// Copyright 2023 AIT Austrian Institute of Technology +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TRAJECTORY_PLUGINS__PID_TRAJECTORY_PLUGIN_HPP_ +#define TRAJECTORY_PLUGINS__PID_TRAJECTORY_PLUGIN_HPP_ + +#include +#include + +#include "control_toolbox/pid.hpp" +#include "joint_trajectory_controller/trajectory_controller_base.hpp" + +#include "pid_trajectory_plugin_parameters.hpp" // auto-generated by generate_parameter_library +#include "trajectory_plugins/visibility_control.h" + +using PidPtr = std::shared_ptr; + +namespace trajectory_plugins +{ + +class PidTrajectoryPlugin : public joint_trajectory_controller::TrajectoryControllerBase +{ +public: + bool initialize(rclcpp_lifecycle::LifecycleNode::SharedPtr node) override; + + bool computeGains(const trajectory_msgs::msg::JointTrajectory trajectory) override; + + void computeCommands( + std::vector & tmp_command, const trajectory_msgs::msg::JointTrajectoryPoint current, + const trajectory_msgs::msg::JointTrajectoryPoint error, + const trajectory_msgs::msg::JointTrajectoryPoint desired, const rclcpp::Time & time, + const rclcpp::Duration & period) override; + + void reset() override; + +protected: + // the node handle for parameter handling + rclcpp_lifecycle::LifecycleNode::SharedPtr node_; + // PID controllers + std::vector pids_; + // Feed-forward velocity weight factor when calculating closed loop pid adapter's command + std::vector ff_velocity_scale_; + + // Parameters from ROS for joint_trajectory_controller + std::shared_ptr param_listener_; + Params params_; +}; + +} // namespace trajectory_plugins + +#endif // TRAJECTORY_PLUGINS__PID_TRAJECTORY_PLUGIN_HPP_ diff --git a/trajectory_plugins/include/trajectory_plugins/visibility_control.h b/trajectory_plugins/include/trajectory_plugins/visibility_control.h new file mode 100644 index 0000000000..14cea41dd1 --- /dev/null +++ b/trajectory_plugins/include/trajectory_plugins/visibility_control.h @@ -0,0 +1,49 @@ +// Copyright 2023 AIT Austrian Institute of Technology +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TRAJECTORY_PLUGINS__VISIBILITY_CONTROL_H_ +#define TRAJECTORY_PLUGINS__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define TRAJECTORY_PLUGINS_EXPORT __attribute__((dllexport)) +#define TRAJECTORY_PLUGINS_IMPORT __attribute__((dllimport)) +#else +#define TRAJECTORY_PLUGINS_EXPORT __declspec(dllexport) +#define TRAJECTORY_PLUGINS_IMPORT __declspec(dllimport) +#endif +#ifdef TRAJECTORY_PLUGINS_BUILDING_LIBRARY +#define TRAJECTORY_PLUGINS_PUBLIC TRAJECTORY_PLUGINS_EXPORT +#else +#define TRAJECTORY_PLUGINS_PUBLIC TRAJECTORY_PLUGINS_IMPORT +#endif +#define TRAJECTORY_PLUGINS_PUBLIC_TYPE TRAJECTORY_PLUGINS_PUBLIC +#define TRAJECTORY_PLUGINS_LOCAL +#else +#define TRAJECTORY_PLUGINS_EXPORT __attribute__((visibility("default"))) +#define TRAJECTORY_PLUGINS_IMPORT +#if __GNUC__ >= 4 +#define TRAJECTORY_PLUGINS_PUBLIC __attribute__((visibility("default"))) +#define TRAJECTORY_PLUGINS_LOCAL __attribute__((visibility("hidden"))) +#else +#define TRAJECTORY_PLUGINS_PUBLIC +#define TRAJECTORY_PLUGINS_LOCAL +#endif +#define TRAJECTORY_PLUGINS_PUBLIC_TYPE +#endif + +#endif // TRAJECTORY_PLUGINS__VISIBILITY_CONTROL_H_ diff --git a/trajectory_plugins/package.xml b/trajectory_plugins/package.xml new file mode 100644 index 0000000000..a39f5abecb --- /dev/null +++ b/trajectory_plugins/package.xml @@ -0,0 +1,23 @@ + + + + trajectory_plugins + 0.0.0 + TODO: Package description + vscode + Apache-2.0 + + ament_cmake_ros + + joint_trajectory_controller + control_toolbox + pluginlib + generate_parameter_library + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/trajectory_plugins/plugins.xml b/trajectory_plugins/plugins.xml new file mode 100644 index 0000000000..84c6cc4583 --- /dev/null +++ b/trajectory_plugins/plugins.xml @@ -0,0 +1,6 @@ + + + A trajectory controller consisting of a PID controller per joint. + + diff --git a/trajectory_plugins/src/pid_trajectory_plugin.cpp b/trajectory_plugins/src/pid_trajectory_plugin.cpp new file mode 100644 index 0000000000..b543830f28 --- /dev/null +++ b/trajectory_plugins/src/pid_trajectory_plugin.cpp @@ -0,0 +1,88 @@ +// Copyright 2023 AIT Austrian Institute of Technology +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "trajectory_plugins/pid_trajectory_plugin.hpp" + +namespace trajectory_plugins +{ + +bool PidTrajectoryPlugin::initialize(rclcpp_lifecycle::LifecycleNode::SharedPtr node) +{ + node_ = node; + + try + { + // Create the parameter listener and get the parameters + param_listener_ = std::make_shared(node_); + params_ = param_listener_->get_params(); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return false; + } + + size_t dof_ = params_.joints.size(); + pids_.resize(dof_); + ff_velocity_scale_.resize(dof_); + + // Init PID gains from ROS parameters + for (size_t i = 0; i < dof_; ++i) + { + const auto & gains = params_.gains.joints_map.at(params_.joints[i]); + pids_[i] = std::make_shared( + gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp); + + ff_velocity_scale_[i] = gains.ff_velocity_scale; + } + return true; +} + +bool PidTrajectoryPlugin::computeGains(const trajectory_msgs::msg::JointTrajectory trajectory) +{ + return true; +} + +void PidTrajectoryPlugin::computeCommands( + std::vector & tmp_command, const trajectory_msgs::msg::JointTrajectoryPoint current, + const trajectory_msgs::msg::JointTrajectoryPoint error, + const trajectory_msgs::msg::JointTrajectoryPoint desired, const rclcpp::Time & time, + const rclcpp::Duration & period) +{ + // Update PIDs + for (auto i = 0ul; i < tmp_command.size(); ++i) + { + tmp_command[i] = (desired.velocities[i] * ff_velocity_scale_[i]) + + pids_[i]->computeCommand( + error.positions[i], error.velocities[i], (uint64_t)period.nanoseconds()); + } +} + +void PidTrajectoryPlugin::reset() +{ + for (const auto & pid : pids_) + { + if (pid) + { + pid->reset(); + } + } +} + +} // namespace trajectory_plugins + +#include + +PLUGINLIB_EXPORT_CLASS( + trajectory_plugins::PidTrajectoryPlugin, joint_trajectory_controller::TrajectoryControllerBase) diff --git a/trajectory_plugins/src/pid_trajectory_plugin_parameters.yaml b/trajectory_plugins/src/pid_trajectory_plugin_parameters.yaml new file mode 100644 index 0000000000..4bb292b90d --- /dev/null +++ b/trajectory_plugins/src/pid_trajectory_plugin_parameters.yaml @@ -0,0 +1,48 @@ +trajectory_plugins: + joints: { + type: string_array, + default_value: [], + description: "Names of joints used by the controller", + read_only: true, + validation: { + unique<>: null, + } + } + gains: + __map_joints: + p: { + type: double, + default_value: 0.0, + description: "Proportional gain for PID" + } + i: { + type: double, + default_value: 0.0, + description: "Integral gain for PID" + } + d: { + type: double, + default_value: 0.0, + description: "Derivative gain for PID" + } + i_clamp: { + type: double, + default_value: 0.0, + description: "Integral clamp. Symmetrical in both positive and negative direction." + } + ff_velocity_scale: { + type: double, + default_value: 0.0, + description: "Feed-forward scaling of velocity." + } + normalize_error: { + type: bool, + default_value: false, + description: "(Deprecated) Use position error normalization to -pi to pi." + } + angle_wraparound: { + type: bool, + default_value: false, + description: "For joints that wrap around (ie. are continuous). + Normalizes position-error to -pi to pi." + } diff --git a/joint_trajectory_controller/test/test_load_pid_trajectory.cpp b/trajectory_plugins/test/test_load_pid_trajectory.cpp similarity index 84% rename from joint_trajectory_controller/test/test_load_pid_trajectory.cpp rename to trajectory_plugins/test/test_load_pid_trajectory.cpp index 74a39c2bd4..16321b48af 100644 --- a/joint_trajectory_controller/test/test_load_pid_trajectory.cpp +++ b/trajectory_plugins/test/test_load_pid_trajectory.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 PAL Robotics SL. +// Copyright 2023 AIT Austrian Institute of Technology // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -13,6 +13,7 @@ // limitations under the License. #include +#include #include "gmock/gmock.h" @@ -32,15 +33,15 @@ TEST(TestLoadPidController, load_controller) std::shared_ptr traj_contr; auto available_classes = traj_controller_loader.getDeclaredClasses(); - std::stringstream sstr; - sstr << "available filters:" << std::endl; + + std::cout << "available filters:" << std::endl; for (const auto & available_class : available_classes) { - sstr << " " << available_class << std::endl; + std::cout << " " << available_class << std::endl; } - std::string controller_type = "joint_trajectory_controller::PidTrajectoryController"; - ASSERT_TRUE(traj_controller_loader.isClassAvailable(controller_type)) << sstr.str(); + std::string controller_type = "trajectory_plugins::PidTrajectoryPlugin"; + ASSERT_TRUE(traj_controller_loader.isClassAvailable(controller_type)); ASSERT_NO_THROW(traj_contr = traj_controller_loader.createSharedInstance(controller_type)); rclcpp::shutdown(); From be6ed1231bfe100265d4a7328e71274e36749b65 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 6 Dec 2023 12:35:56 +0000 Subject: [PATCH 03/30] Move base class to separate package --- joint_trajectory_controller/CMakeLists.txt | 5 +-- .../joint_trajectory_controller.hpp | 4 +- joint_trajectory_controller/package.xml | 1 + .../src/joint_trajectory_controller.cpp | 9 ++-- trajectory_plugins/CMakeLists.txt | 43 +++++++++---------- .../pid_trajectory_plugin.hpp | 6 +-- .../trajectory_controller_base.hpp | 24 +++++------ trajectory_plugins/package.xml | 2 +- trajectory_plugins/plugins.xml | 2 +- .../src/pid_trajectory_plugin.cpp | 2 +- .../test/test_load_pid_trajectory.cpp | 9 ++-- 11 files changed, 51 insertions(+), 56 deletions(-) rename {joint_trajectory_controller/include/joint_trajectory_controller => trajectory_plugins/include/trajectory_plugins}/trajectory_controller_base.hpp (74%) diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index 93269c9d4b..efaeea33c7 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -19,6 +19,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS rsl tl_expected trajectory_msgs + trajectory_plugins ) find_package(ament_cmake REQUIRED) @@ -105,8 +106,4 @@ install(TARGETS ament_export_targets(export_joint_trajectory_controller HAS_LIBRARY_TARGET) ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) -ament_export_libraries( - joint_trajectory_controller -) - ament_package() diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index 4f9844482e..0bdf1210f1 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -43,9 +43,9 @@ #include "joint_trajectory_controller/interpolation_methods.hpp" #include "joint_trajectory_controller/tolerances.hpp" -#include "joint_trajectory_controller/trajectory_controller_base.hpp" #include "joint_trajectory_controller/visibility_control.h" #include "joint_trajectory_controller_parameters.hpp" // auto-generated by generate_parameter_library +#include "trajectory_plugins/trajectory_controller_base.hpp" using namespace std::chrono_literals; // NOLINT @@ -163,7 +163,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa /// If true, a velocity feedforward term plus corrective PID term is used bool use_closed_loop_pid_adapter_ = false; // The actual trajectory controller used for closed loop pid adapter - std::shared_ptr traj_contr_; + std::shared_ptr traj_contr_; // Configuration for every joint, if position error is wrapped around std::vector joints_angle_wraparound_; // reserved storage for result of the command when closed loop pid adapter is used diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index b41a7cad0c..bd174bd064 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -30,6 +30,7 @@ rsl tl_expected trajectory_msgs + trajectory_plugins ament_cmake_gmock controller_manager diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index cd7daadd88..2aa94ba182 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -708,14 +708,13 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( if (use_closed_loop_pid_adapter_) { - pluginlib::ClassLoader - traj_controller_loader( - "joint_trajectory_controller", "joint_trajectory_controller::TrajectoryControllerBase"); + pluginlib::ClassLoader traj_controller_loader( + "trajectory_plugins", "trajectory_plugins::TrajectoryControllerBase"); try { RCLCPP_INFO(logger, "Load the library"); - traj_contr_ = traj_controller_loader.createSharedInstance( - "joint_trajectory_controller::PidTrajectoryController"); + traj_contr_ = + traj_controller_loader.createSharedInstance("trajectory_plugins::PidTrajectoryPlugin"); RCLCPP_INFO(logger, "Loaded the library"); } catch (pluginlib::PluginlibException & ex) diff --git a/trajectory_plugins/CMakeLists.txt b/trajectory_plugins/CMakeLists.txt index d0474e06d9..d1018de6c5 100644 --- a/trajectory_plugins/CMakeLists.txt +++ b/trajectory_plugins/CMakeLists.txt @@ -1,17 +1,26 @@ cmake_minimum_required(VERSION 3.8) -project(trajectory_plugins) +project(trajectory_plugins LANGUAGES CXX) -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Wconversion) endif() # find dependencies +set(THIS_PACKAGE_INCLUDE_DEPENDS + control_toolbox + generate_parameter_library + pluginlib + rclcpp + rclcpp_lifecycle + trajectory_msgs +) + find_package(ament_cmake REQUIRED) find_package(ament_cmake_ros REQUIRED) -find_package(joint_trajectory_controller REQUIRED) -find_package(control_toolbox REQUIRED) -find_package(pluginlib REQUIRED) -find_package(generate_parameter_library REQUIRED) +find_package(backward_ros REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() generate_parameter_library(pid_trajectory_plugin_parameters src/pid_trajectory_plugin_parameters.yaml @@ -19,20 +28,15 @@ generate_parameter_library(pid_trajectory_plugin_parameters add_library(pid_trajectory_plugin SHARED src/pid_trajectory_plugin.cpp) add_library(trajectory_plugins::pid_trajectory_plugin ALIAS pid_trajectory_plugin) -target_compile_features(pid_trajectory_plugin PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +target_compile_features(pid_trajectory_plugin PUBLIC cxx_std_17) target_include_directories(pid_trajectory_plugin PUBLIC $ $) -ament_target_dependencies(pid_trajectory_plugin PUBLIC - "joint_trajectory_controller" - "control_toolbox" - "pluginlib" - "generate_parameter_library" -) +ament_target_dependencies(pid_trajectory_plugin PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) target_link_libraries(pid_trajectory_plugin PUBLIC pid_trajectory_plugin_parameters ) -pluginlib_export_plugin_description_file(joint_trajectory_controller plugins.xml) +pluginlib_export_plugin_description_file(trajectory_plugins plugins.xml) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. @@ -52,13 +56,8 @@ install( if(BUILD_TESTING) ament_add_gmock(test_load_pid_trajectory test/test_load_pid_trajectory.cpp) - target_link_libraries(test_load_pid_trajectory pid_trajectory_plugin_parameters) - ament_target_dependencies(test_load_pid_trajectory - "joint_trajectory_controller" - "rclcpp" - "pluginlib" - "generate_parameter_library" - ) + target_link_libraries(test_load_pid_trajectory pid_trajectory_plugin pid_trajectory_plugin_parameters) + ament_target_dependencies(test_load_pid_trajectory ${THIS_PACKAGE_INCLUDE_DEPENDS} ) endif() ament_export_include_directories( diff --git a/trajectory_plugins/include/trajectory_plugins/pid_trajectory_plugin.hpp b/trajectory_plugins/include/trajectory_plugins/pid_trajectory_plugin.hpp index 8eaf577ffd..6f1bee7fca 100644 --- a/trajectory_plugins/include/trajectory_plugins/pid_trajectory_plugin.hpp +++ b/trajectory_plugins/include/trajectory_plugins/pid_trajectory_plugin.hpp @@ -19,9 +19,9 @@ #include #include "control_toolbox/pid.hpp" -#include "joint_trajectory_controller/trajectory_controller_base.hpp" #include "pid_trajectory_plugin_parameters.hpp" // auto-generated by generate_parameter_library +#include "trajectory_plugins/trajectory_controller_base.hpp" #include "trajectory_plugins/visibility_control.h" using PidPtr = std::shared_ptr; @@ -29,7 +29,7 @@ using PidPtr = std::shared_ptr; namespace trajectory_plugins { -class PidTrajectoryPlugin : public joint_trajectory_controller::TrajectoryControllerBase +class PidTrajectoryPlugin : public TrajectoryControllerBase { public: bool initialize(rclcpp_lifecycle::LifecycleNode::SharedPtr node) override; @@ -52,7 +52,7 @@ class PidTrajectoryPlugin : public joint_trajectory_controller::TrajectoryContro // Feed-forward velocity weight factor when calculating closed loop pid adapter's command std::vector ff_velocity_scale_; - // Parameters from ROS for joint_trajectory_controller + // Parameters from ROS for trajectory_plugins std::shared_ptr param_listener_; Params params_; }; diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory_controller_base.hpp b/trajectory_plugins/include/trajectory_plugins/trajectory_controller_base.hpp similarity index 74% rename from joint_trajectory_controller/include/joint_trajectory_controller/trajectory_controller_base.hpp rename to trajectory_plugins/include/trajectory_plugins/trajectory_controller_base.hpp index e13505a24e..4d969b3749 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory_controller_base.hpp +++ b/trajectory_plugins/include/trajectory_plugins/trajectory_controller_base.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef JOINT_TRAJECTORY_CONTROLLER__TRAJECTORY_CONTROLLER_BASE_HPP_ -#define JOINT_TRAJECTORY_CONTROLLER__TRAJECTORY_CONTROLLER_BASE_HPP_ +#ifndef TRAJECTORY_PLUGINS__TRAJECTORY_CONTROLLER_BASE_HPP_ +#define TRAJECTORY_PLUGINS__TRAJECTORY_CONTROLLER_BASE_HPP_ #include @@ -23,32 +23,32 @@ #include "trajectory_msgs/msg/joint_trajectory.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" -#include "joint_trajectory_controller/visibility_control.h" +#include "trajectory_plugins/visibility_control.h" -namespace joint_trajectory_controller +namespace trajectory_plugins { class TrajectoryControllerBase { public: - JOINT_TRAJECTORY_CONTROLLER_PUBLIC + TRAJECTORY_PLUGINS_PUBLIC TrajectoryControllerBase() = default; - JOINT_TRAJECTORY_CONTROLLER_PUBLIC + TRAJECTORY_PLUGINS_PUBLIC virtual ~TrajectoryControllerBase() = default; /** */ - JOINT_TRAJECTORY_CONTROLLER_PUBLIC + TRAJECTORY_PLUGINS_PUBLIC virtual bool initialize(rclcpp_lifecycle::LifecycleNode::SharedPtr node) = 0; /** */ - JOINT_TRAJECTORY_CONTROLLER_PUBLIC + TRAJECTORY_PLUGINS_PUBLIC virtual bool computeGains(const trajectory_msgs::msg::JointTrajectory trajectory) = 0; /** */ - JOINT_TRAJECTORY_CONTROLLER_PUBLIC + TRAJECTORY_PLUGINS_PUBLIC virtual void computeCommands( std::vector & tmp_command, const trajectory_msgs::msg::JointTrajectoryPoint current, const trajectory_msgs::msg::JointTrajectoryPoint error, @@ -57,7 +57,7 @@ class TrajectoryControllerBase /** */ - JOINT_TRAJECTORY_CONTROLLER_PUBLIC + TRAJECTORY_PLUGINS_PUBLIC virtual void reset() = 0; protected: @@ -65,6 +65,6 @@ class TrajectoryControllerBase rclcpp::Node::SharedPtr node_; }; -} // namespace joint_trajectory_controller +} // namespace trajectory_plugins -#endif // JOINT_TRAJECTORY_CONTROLLER__TRAJECTORY_CONTROLLER_BASE_HPP_ +#endif // TRAJECTORY_PLUGINS__TRAJECTORY_CONTROLLER_BASE_HPP_ diff --git a/trajectory_plugins/package.xml b/trajectory_plugins/package.xml index a39f5abecb..97060ae1a4 100644 --- a/trajectory_plugins/package.xml +++ b/trajectory_plugins/package.xml @@ -9,9 +9,9 @@ ament_cmake_ros - joint_trajectory_controller control_toolbox pluginlib + trajectory_msgs generate_parameter_library ament_lint_auto diff --git a/trajectory_plugins/plugins.xml b/trajectory_plugins/plugins.xml index 84c6cc4583..8c94d5191b 100644 --- a/trajectory_plugins/plugins.xml +++ b/trajectory_plugins/plugins.xml @@ -1,6 +1,6 @@ + type="trajectory_plugins::PidTrajectoryPlugin" base_class_type="trajectory_plugins::TrajectoryControllerBase"> A trajectory controller consisting of a PID controller per joint. diff --git a/trajectory_plugins/src/pid_trajectory_plugin.cpp b/trajectory_plugins/src/pid_trajectory_plugin.cpp index b543830f28..f7fe8574d8 100644 --- a/trajectory_plugins/src/pid_trajectory_plugin.cpp +++ b/trajectory_plugins/src/pid_trajectory_plugin.cpp @@ -85,4 +85,4 @@ void PidTrajectoryPlugin::reset() #include PLUGINLIB_EXPORT_CLASS( - trajectory_plugins::PidTrajectoryPlugin, joint_trajectory_controller::TrajectoryControllerBase) + trajectory_plugins::PidTrajectoryPlugin, trajectory_plugins::TrajectoryControllerBase) diff --git a/trajectory_plugins/test/test_load_pid_trajectory.cpp b/trajectory_plugins/test/test_load_pid_trajectory.cpp index 16321b48af..cb48d0d77d 100644 --- a/trajectory_plugins/test/test_load_pid_trajectory.cpp +++ b/trajectory_plugins/test/test_load_pid_trajectory.cpp @@ -17,20 +17,19 @@ #include "gmock/gmock.h" -#include "joint_trajectory_controller/trajectory_controller_base.hpp" #include "pluginlib/class_loader.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "trajectory_plugins/trajectory_controller_base.hpp" TEST(TestLoadPidController, load_controller) { rclcpp::init(0, nullptr); - pluginlib::ClassLoader - traj_controller_loader( - "joint_trajectory_controller", "joint_trajectory_controller::TrajectoryControllerBase"); + pluginlib::ClassLoader traj_controller_loader( + "trajectory_plugins", "trajectory_plugins::TrajectoryControllerBase"); - std::shared_ptr traj_contr; + std::shared_ptr traj_contr; auto available_classes = traj_controller_loader.getDeclaredClasses(); From 8174ba76f84ae4c26a0ed9cbbdcbe1bb773095c4 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 6 Dec 2023 12:35:56 +0000 Subject: [PATCH 04/30] Delete duplicate parameters --- .../src/joint_trajectory_controller.cpp | 9 ++---- ...oint_trajectory_controller_parameters.yaml | 30 ++++--------------- .../src/pid_trajectory_plugin_parameters.yaml | 11 ------- 3 files changed, 8 insertions(+), 42 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 2aa94ba182..813322e0ed 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -712,16 +712,13 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( "trajectory_plugins", "trajectory_plugins::TrajectoryControllerBase"); try { - RCLCPP_INFO(logger, "Load the library"); - traj_contr_ = - traj_controller_loader.createSharedInstance("trajectory_plugins::PidTrajectoryPlugin"); - RCLCPP_INFO(logger, "Loaded the library"); + traj_contr_ = traj_controller_loader.createSharedInstance(params_.controller_plugin.c_str()); } catch (pluginlib::PluginlibException & ex) { RCLCPP_FATAL( - logger, "The trajectory controller plugin failed to load for some reason. Error: %s\n", - ex.what()); + logger, "The trajectory controller plugin `%s` failed to load for some reason. Error: %s\n", + params_.controller_plugin.c_str(), ex.what()); return CallbackReturn::FAILURE; } if (!traj_contr_->initialize(get_node())) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index 4981489d36..70dbd9ce59 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -86,33 +86,13 @@ joint_trajectory_controller: cmd_timeout must be greater than constraints.goal_time, otherwise ignored. If zero, timeout is deactivated", } + controller_plugin : { + type: string, + default_value: "trajectory_plugins::PidTrajectoryPlugin", + description: "Type of the plugin for the trajectory controller", + } gains: __map_joints: - p: { - type: double, - default_value: 0.0, - description: "Proportional gain for PID" - } - i: { - type: double, - default_value: 0.0, - description: "Integral gain for PID" - } - d: { - type: double, - default_value: 0.0, - description: "Derivative gain for PID" - } - i_clamp: { - type: double, - default_value: 0.0, - description: "Integral clamp. Symmetrical in both positive and negative direction." - } - ff_velocity_scale: { - type: double, - default_value: 0.0, - description: "Feed-forward scaling of velocity." - } angle_wraparound: { type: bool, default_value: false, diff --git a/trajectory_plugins/src/pid_trajectory_plugin_parameters.yaml b/trajectory_plugins/src/pid_trajectory_plugin_parameters.yaml index 4bb292b90d..cde7c9cdfc 100644 --- a/trajectory_plugins/src/pid_trajectory_plugin_parameters.yaml +++ b/trajectory_plugins/src/pid_trajectory_plugin_parameters.yaml @@ -35,14 +35,3 @@ trajectory_plugins: default_value: 0.0, description: "Feed-forward scaling of velocity." } - normalize_error: { - type: bool, - default_value: false, - description: "(Deprecated) Use position error normalization to -pi to pi." - } - angle_wraparound: { - type: bool, - default_value: false, - description: "For joints that wrap around (ie. are continuous). - Normalizes position-error to -pi to pi." - } From beeb779fd1885319621c1f78cedc8424f58176bc Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 6 Dec 2023 12:35:56 +0000 Subject: [PATCH 05/30] Reactivate tests --- .../test/test_trajectory_controller.cpp | 87 +++++++++---------- 1 file changed, 41 insertions(+), 46 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 73b2dddd9b..05010c562c 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -58,7 +58,6 @@ using test_trajectory_controllers::TrajectoryControllerTestParameterized; void spin(rclcpp::executors::MultiThreadedExecutor * exe) { exe->spin(); } -#if 0 TEST_P(TrajectoryControllerTestParameterized, configure_state_ignores_commands) { rclcpp::executors::MultiThreadedExecutor executor; @@ -464,7 +463,7 @@ TEST_P(TrajectoryControllerTestParameterized, hold_on_startup) executor.cancel(); } -#endif + // Floating-point value comparison threshold const double EPS = 1e-6; /** @@ -695,7 +694,7 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound) executor.cancel(); } -#if 0 + /** * @brief cmd_timeout must be greater than constraints.goal_time */ @@ -830,7 +829,6 @@ TEST_P(TrajectoryControllerTestParameterized, timeout) executor.cancel(); } -#endif /** * @brief check if use_closed_loop_pid is active @@ -852,7 +850,7 @@ TEST_P(TrajectoryControllerTestParameterized, use_closed_loop_pid) EXPECT_TRUE(traj_controller_->use_closed_loop_pid_adapter()); } } -#if 0 + /** * @brief check if velocity error is calculated correctly */ @@ -1838,47 +1836,44 @@ TEST_P(TrajectoryControllerTestParameterized, test_goal_tolerances_fail) // it should be still holding the old point expectCommandPoint(hold_position); } -#endif - -// // position controllers -// INSTANTIATE_TEST_SUITE_P( -// PositionTrajectoryControllers, TrajectoryControllerTestParameterized, -// ::testing::Values( -// std::make_tuple(std::vector({"position"}), -// std::vector({"position"})), std::make_tuple( -// std::vector({"position"}), std::vector({"position", -// "velocity"})), -// std::make_tuple( -// std::vector({"position"}), -// std::vector({"position", "velocity", "acceleration"})))); - -// // position_velocity controllers -// INSTANTIATE_TEST_SUITE_P( -// PositionVelocityTrajectoryControllers, TrajectoryControllerTestParameterized, -// ::testing::Values( -// std::make_tuple( -// std::vector({"position", "velocity"}), -// std::vector({"position"})), -// std::make_tuple( -// std::vector({"position", "velocity"}), -// std::vector({"position", "velocity"})), -// std::make_tuple( -// std::vector({"position", "velocity"}), -// std::vector({"position", "velocity", "acceleration"})))); - -// // position_velocity_acceleration controllers -// INSTANTIATE_TEST_SUITE_P( -// PositionVelocityAccelerationTrajectoryControllers, TrajectoryControllerTestParameterized, -// ::testing::Values( -// std::make_tuple( -// std::vector({"position", "velocity", "acceleration"}), -// std::vector({"position"})), -// std::make_tuple( -// std::vector({"position", "velocity", "acceleration"}), -// std::vector({"position", "velocity"})), -// std::make_tuple( -// std::vector({"position", "velocity", "acceleration"}), -// std::vector({"position", "velocity", "acceleration"})))); + +// position controllers +INSTANTIATE_TEST_SUITE_P( + PositionTrajectoryControllers, TrajectoryControllerTestParameterized, + ::testing::Values( + std::make_tuple(std::vector({"position"}), std::vector({"position"})), + std::make_tuple( + std::vector({"position"}), std::vector({"position", "velocity"})), + std::make_tuple( + std::vector({"position"}), + std::vector({"position", "velocity", "acceleration"})))); + +// position_velocity controllers +INSTANTIATE_TEST_SUITE_P( + PositionVelocityTrajectoryControllers, TrajectoryControllerTestParameterized, + ::testing::Values( + std::make_tuple( + std::vector({"position", "velocity"}), std::vector({"position"})), + std::make_tuple( + std::vector({"position", "velocity"}), + std::vector({"position", "velocity"})), + std::make_tuple( + std::vector({"position", "velocity"}), + std::vector({"position", "velocity", "acceleration"})))); + +// position_velocity_acceleration controllers +INSTANTIATE_TEST_SUITE_P( + PositionVelocityAccelerationTrajectoryControllers, TrajectoryControllerTestParameterized, + ::testing::Values( + std::make_tuple( + std::vector({"position", "velocity", "acceleration"}), + std::vector({"position"})), + std::make_tuple( + std::vector({"position", "velocity", "acceleration"}), + std::vector({"position", "velocity"})), + std::make_tuple( + std::vector({"position", "velocity", "acceleration"}), + std::vector({"position", "velocity", "acceleration"})))); // only velocity controller INSTANTIATE_TEST_SUITE_P( From 5fcb11d440f947f885fa3cc76fff58633d897e6f Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 6 Dec 2023 12:35:56 +0000 Subject: [PATCH 06/30] Fix unused parameter warnings --- trajectory_plugins/src/pid_trajectory_plugin.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/trajectory_plugins/src/pid_trajectory_plugin.cpp b/trajectory_plugins/src/pid_trajectory_plugin.cpp index f7fe8574d8..0c7d538ff0 100644 --- a/trajectory_plugins/src/pid_trajectory_plugin.cpp +++ b/trajectory_plugins/src/pid_trajectory_plugin.cpp @@ -49,15 +49,15 @@ bool PidTrajectoryPlugin::initialize(rclcpp_lifecycle::LifecycleNode::SharedPtr return true; } -bool PidTrajectoryPlugin::computeGains(const trajectory_msgs::msg::JointTrajectory trajectory) +bool PidTrajectoryPlugin::computeGains(const trajectory_msgs::msg::JointTrajectory /*trajectory*/) { return true; } void PidTrajectoryPlugin::computeCommands( - std::vector & tmp_command, const trajectory_msgs::msg::JointTrajectoryPoint current, + std::vector & tmp_command, const trajectory_msgs::msg::JointTrajectoryPoint /*current*/, const trajectory_msgs::msg::JointTrajectoryPoint error, - const trajectory_msgs::msg::JointTrajectoryPoint desired, const rclcpp::Time & time, + const trajectory_msgs::msg::JointTrajectoryPoint desired, const rclcpp::Time & /*time*/, const rclcpp::Duration & period) { // Update PIDs From 1e2deddcb763373e8f8ec050c7e2d65ceb462005 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 6 Dec 2023 12:35:56 +0000 Subject: [PATCH 07/30] Remove duplicate LICENSE file --- trajectory_plugins/LICENSE | 202 ------------------------------------- 1 file changed, 202 deletions(-) delete mode 100644 trajectory_plugins/LICENSE diff --git a/trajectory_plugins/LICENSE b/trajectory_plugins/LICENSE deleted file mode 100644 index d645695673..0000000000 --- a/trajectory_plugins/LICENSE +++ /dev/null @@ -1,202 +0,0 @@ - - Apache License - Version 2.0, January 2004 - http://www.apache.org/licenses/ - - TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION - - 1. Definitions. - - "License" shall mean the terms and conditions for use, reproduction, - and distribution as defined by Sections 1 through 9 of this document. - - "Licensor" shall mean the copyright owner or entity authorized by - the copyright owner that is granting the License. - - "Legal Entity" shall mean the union of the acting entity and all - other entities that control, are controlled by, or are under common - control with that entity. For the purposes of this definition, - "control" means (i) the power, direct or indirect, to cause the - direction or management of such entity, whether by contract or - otherwise, or (ii) ownership of fifty percent (50%) or more of the - outstanding shares, or (iii) beneficial ownership of such entity. - - "You" (or "Your") shall mean an individual or Legal Entity - exercising permissions granted by this License. - - "Source" form shall mean the preferred form for making modifications, - including but not limited to software source code, documentation - source, and configuration files. - - "Object" form shall mean any form resulting from mechanical - transformation or translation of a Source form, including but - not limited to compiled object code, generated documentation, - and conversions to other media types. - - "Work" shall mean the work of authorship, whether in Source or - Object form, made available under the License, as indicated by a - copyright notice that is included in or attached to the work - (an example is provided in the Appendix below). - - "Derivative Works" shall mean any work, whether in Source or Object - form, that is based on (or derived from) the Work and for which the - editorial revisions, annotations, elaborations, or other modifications - represent, as a whole, an original work of authorship. For the purposes - of this License, Derivative Works shall not include works that remain - separable from, or merely link (or bind by name) to the interfaces of, - the Work and Derivative Works thereof. - - "Contribution" shall mean any work of authorship, including - the original version of the Work and any modifications or additions - to that Work or Derivative Works thereof, that is intentionally - submitted to Licensor for inclusion in the Work by the copyright owner - or by an individual or Legal Entity authorized to submit on behalf of - the copyright owner. For the purposes of this definition, "submitted" - means any form of electronic, verbal, or written communication sent - to the Licensor or its representatives, including but not limited to - communication on electronic mailing lists, source code control systems, - and issue tracking systems that are managed by, or on behalf of, the - Licensor for the purpose of discussing and improving the Work, but - excluding communication that is conspicuously marked or otherwise - designated in writing by the copyright owner as "Not a Contribution." - - "Contributor" shall mean Licensor and any individual or Legal Entity - on behalf of whom a Contribution has been received by Licensor and - subsequently incorporated within the Work. - - 2. Grant of Copyright License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - copyright license to reproduce, prepare Derivative Works of, - publicly display, publicly perform, sublicense, and distribute the - Work and such Derivative Works in Source or Object form. - - 3. Grant of Patent License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - (except as stated in this section) patent license to make, have made, - use, offer to sell, sell, import, and otherwise transfer the Work, - where such license applies only to those patent claims licensable - by such Contributor that are necessarily infringed by their - Contribution(s) alone or by combination of their Contribution(s) - with the Work to which such Contribution(s) was submitted. If You - institute patent litigation against any entity (including a - cross-claim or counterclaim in a lawsuit) alleging that the Work - or a Contribution incorporated within the Work constitutes direct - or contributory patent infringement, then any patent licenses - granted to You under this License for that Work shall terminate - as of the date such litigation is filed. - - 4. Redistribution. You may reproduce and distribute copies of the - Work or Derivative Works thereof in any medium, with or without - modifications, and in Source or Object form, provided that You - meet the following conditions: - - (a) You must give any other recipients of the Work or - Derivative Works a copy of this License; and - - (b) You must cause any modified files to carry prominent notices - stating that You changed the files; and - - (c) You must retain, in the Source form of any Derivative Works - that You distribute, all copyright, patent, trademark, and - attribution notices from the Source form of the Work, - excluding those notices that do not pertain to any part of - the Derivative Works; and - - (d) If the Work includes a "NOTICE" text file as part of its - distribution, then any Derivative Works that You distribute must - include a readable copy of the attribution notices contained - within such NOTICE file, excluding those notices that do not - pertain to any part of the Derivative Works, in at least one - of the following places: within a NOTICE text file distributed - as part of the Derivative Works; within the Source form or - documentation, if provided along with the Derivative Works; or, - within a display generated by the Derivative Works, if and - wherever such third-party notices normally appear. The contents - of the NOTICE file are for informational purposes only and - do not modify the License. You may add Your own attribution - notices within Derivative Works that You distribute, alongside - or as an addendum to the NOTICE text from the Work, provided - that such additional attribution notices cannot be construed - as modifying the License. - - You may add Your own copyright statement to Your modifications and - may provide additional or different license terms and conditions - for use, reproduction, or distribution of Your modifications, or - for any such Derivative Works as a whole, provided Your use, - reproduction, and distribution of the Work otherwise complies with - the conditions stated in this License. - - 5. Submission of Contributions. Unless You explicitly state otherwise, - any Contribution intentionally submitted for inclusion in the Work - by You to the Licensor shall be under the terms and conditions of - this License, without any additional terms or conditions. - Notwithstanding the above, nothing herein shall supersede or modify - the terms of any separate license agreement you may have executed - with Licensor regarding such Contributions. - - 6. Trademarks. This License does not grant permission to use the trade - names, trademarks, service marks, or product names of the Licensor, - except as required for reasonable and customary use in describing the - origin of the Work and reproducing the content of the NOTICE file. - - 7. Disclaimer of Warranty. Unless required by applicable law or - agreed to in writing, Licensor provides the Work (and each - Contributor provides its Contributions) on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or - implied, including, without limitation, any warranties or conditions - of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A - PARTICULAR PURPOSE. You are solely responsible for determining the - appropriateness of using or redistributing the Work and assume any - risks associated with Your exercise of permissions under this License. - - 8. Limitation of Liability. In no event and under no legal theory, - whether in tort (including negligence), contract, or otherwise, - unless required by applicable law (such as deliberate and grossly - negligent acts) or agreed to in writing, shall any Contributor be - liable to You for damages, including any direct, indirect, special, - incidental, or consequential damages of any character arising as a - result of this License or out of the use or inability to use the - Work (including but not limited to damages for loss of goodwill, - work stoppage, computer failure or malfunction, or any and all - other commercial damages or losses), even if such Contributor - has been advised of the possibility of such damages. - - 9. Accepting Warranty or Additional Liability. While redistributing - the Work or Derivative Works thereof, You may choose to offer, - and charge a fee for, acceptance of support, warranty, indemnity, - or other liability obligations and/or rights consistent with this - License. However, in accepting such obligations, You may act only - on Your own behalf and on Your sole responsibility, not on behalf - of any other Contributor, and only if You agree to indemnify, - defend, and hold each Contributor harmless for any liability - incurred by, or claims asserted against, such Contributor by reason - of your accepting any such warranty or additional liability. - - END OF TERMS AND CONDITIONS - - APPENDIX: How to apply the Apache License to your work. - - To apply the Apache License to your work, attach the following - boilerplate notice, with the fields enclosed by brackets "[]" - replaced with your own identifying information. (Don't include - the brackets!) The text should be enclosed in the appropriate - comment syntax for the file format. We also recommend that a - file or class name and description of purpose be included on the - same "printed page" as the copyright notice for easier - identification within third-party archives. - - Copyright [yyyy] [name of copyright owner] - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. From 875f1a5865f306d91583526cef524b510b943013 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 6 Dec 2023 12:35:56 +0000 Subject: [PATCH 08/30] Rename jtc plugin package --- joint_trajectory_controller/CMakeLists.txt | 2 +- .../joint_trajectory_controller.hpp | 4 +- joint_trajectory_controller/package.xml | 2 +- .../src/joint_trajectory_controller.cpp | 6 ++- ...oint_trajectory_controller_parameters.yaml | 2 +- .../CMakeLists.txt | 8 +-- .../pid_trajectory_plugin.hpp | 16 +++--- .../trajectory_controller_base.hpp | 24 ++++----- .../visibility_control.h | 49 +++++++++++++++++++ .../package.xml | 2 +- .../plugins.xml | 2 +- .../src/pid_trajectory_plugin.cpp | 9 ++-- .../src/pid_trajectory_plugin_parameters.yaml | 2 +- .../test/test_load_pid_trajectory.cpp | 12 +++-- .../trajectory_plugins/visibility_control.h | 49 ------------------- 15 files changed, 97 insertions(+), 92 deletions(-) rename {trajectory_plugins => joint_trajectory_controller_plugins}/CMakeLists.txt (84%) rename {trajectory_plugins/include/trajectory_plugins => joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins}/pid_trajectory_plugin.hpp (77%) rename {trajectory_plugins/include/trajectory_plugins => joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins}/trajectory_controller_base.hpp (71%) create mode 100644 joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/visibility_control.h rename {trajectory_plugins => joint_trajectory_controller_plugins}/package.xml (93%) rename {trajectory_plugins => joint_trajectory_controller_plugins}/plugins.xml (53%) rename {trajectory_plugins => joint_trajectory_controller_plugins}/src/pid_trajectory_plugin.cpp (89%) rename {trajectory_plugins => joint_trajectory_controller_plugins}/src/pid_trajectory_plugin_parameters.yaml (95%) rename {trajectory_plugins => joint_trajectory_controller_plugins}/test/test_load_pid_trajectory.cpp (72%) delete mode 100644 trajectory_plugins/include/trajectory_plugins/visibility_control.h diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index efaeea33c7..f5c285a0d7 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -19,7 +19,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS rsl tl_expected trajectory_msgs - trajectory_plugins + joint_trajectory_controller_plugins ) find_package(ament_cmake REQUIRED) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index 0bdf1210f1..3e687480ca 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -45,7 +45,7 @@ #include "joint_trajectory_controller/tolerances.hpp" #include "joint_trajectory_controller/visibility_control.h" #include "joint_trajectory_controller_parameters.hpp" // auto-generated by generate_parameter_library -#include "trajectory_plugins/trajectory_controller_base.hpp" +#include "joint_trajectory_controller_plugins/trajectory_controller_base.hpp" using namespace std::chrono_literals; // NOLINT @@ -163,7 +163,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa /// If true, a velocity feedforward term plus corrective PID term is used bool use_closed_loop_pid_adapter_ = false; // The actual trajectory controller used for closed loop pid adapter - std::shared_ptr traj_contr_; + std::shared_ptr traj_contr_; // Configuration for every joint, if position error is wrapped around std::vector joints_angle_wraparound_; // reserved storage for result of the command when closed loop pid adapter is used diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index bd174bd064..78ef63aa0e 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -30,7 +30,7 @@ rsl tl_expected trajectory_msgs - trajectory_plugins + joint_trajectory_controller_plugins ament_cmake_gmock controller_manager diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 813322e0ed..cb54456425 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -708,8 +708,10 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( if (use_closed_loop_pid_adapter_) { - pluginlib::ClassLoader traj_controller_loader( - "trajectory_plugins", "trajectory_plugins::TrajectoryControllerBase"); + pluginlib::ClassLoader + traj_controller_loader( + "joint_trajectory_controller_plugins", + "joint_trajectory_controller_plugins::TrajectoryControllerBase"); try { traj_contr_ = traj_controller_loader.createSharedInstance(params_.controller_plugin.c_str()); diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index 70dbd9ce59..bc33c13148 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -88,7 +88,7 @@ joint_trajectory_controller: } controller_plugin : { type: string, - default_value: "trajectory_plugins::PidTrajectoryPlugin", + default_value: "joint_trajectory_controller_plugins::PidTrajectoryPlugin", description: "Type of the plugin for the trajectory controller", } gains: diff --git a/trajectory_plugins/CMakeLists.txt b/joint_trajectory_controller_plugins/CMakeLists.txt similarity index 84% rename from trajectory_plugins/CMakeLists.txt rename to joint_trajectory_controller_plugins/CMakeLists.txt index d1018de6c5..ab7f95a9c7 100644 --- a/trajectory_plugins/CMakeLists.txt +++ b/joint_trajectory_controller_plugins/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(trajectory_plugins LANGUAGES CXX) +project(joint_trajectory_controller_plugins LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wconversion) @@ -27,7 +27,7 @@ generate_parameter_library(pid_trajectory_plugin_parameters ) add_library(pid_trajectory_plugin SHARED src/pid_trajectory_plugin.cpp) -add_library(trajectory_plugins::pid_trajectory_plugin ALIAS pid_trajectory_plugin) +add_library(joint_trajectory_controller_plugins::pid_trajectory_plugin ALIAS pid_trajectory_plugin) target_compile_features(pid_trajectory_plugin PUBLIC cxx_std_17) target_include_directories(pid_trajectory_plugin PUBLIC $ @@ -36,11 +36,11 @@ ament_target_dependencies(pid_trajectory_plugin PUBLIC ${THIS_PACKAGE_INCLUDE_DE target_link_libraries(pid_trajectory_plugin PUBLIC pid_trajectory_plugin_parameters ) -pluginlib_export_plugin_description_file(trajectory_plugins plugins.xml) +pluginlib_export_plugin_description_file(joint_trajectory_controller_plugins plugins.xml) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. -target_compile_definitions(pid_trajectory_plugin PRIVATE "TRAJECTORY_PLUGINS_BUILDING_LIBRARY") +target_compile_definitions(pid_trajectory_plugin PRIVATE "JOINT_TRAJECTORY_CONTROLLER_PLUGINS_BUILDING_LIBRARY") install( DIRECTORY include/ diff --git a/trajectory_plugins/include/trajectory_plugins/pid_trajectory_plugin.hpp b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp similarity index 77% rename from trajectory_plugins/include/trajectory_plugins/pid_trajectory_plugin.hpp rename to joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp index 6f1bee7fca..883b0648d8 100644 --- a/trajectory_plugins/include/trajectory_plugins/pid_trajectory_plugin.hpp +++ b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp @@ -12,21 +12,21 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAJECTORY_PLUGINS__PID_TRAJECTORY_PLUGIN_HPP_ -#define TRAJECTORY_PLUGINS__PID_TRAJECTORY_PLUGIN_HPP_ +#ifndef JOINT_TRAJECTORY_CONTROLLER_PLUGINS__PID_TRAJECTORY_PLUGIN_HPP_ +#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS__PID_TRAJECTORY_PLUGIN_HPP_ #include #include #include "control_toolbox/pid.hpp" +#include "joint_trajectory_controller_plugins/trajectory_controller_base.hpp" +#include "joint_trajectory_controller_plugins/visibility_control.h" #include "pid_trajectory_plugin_parameters.hpp" // auto-generated by generate_parameter_library -#include "trajectory_plugins/trajectory_controller_base.hpp" -#include "trajectory_plugins/visibility_control.h" using PidPtr = std::shared_ptr; -namespace trajectory_plugins +namespace joint_trajectory_controller_plugins { class PidTrajectoryPlugin : public TrajectoryControllerBase @@ -52,11 +52,11 @@ class PidTrajectoryPlugin : public TrajectoryControllerBase // Feed-forward velocity weight factor when calculating closed loop pid adapter's command std::vector ff_velocity_scale_; - // Parameters from ROS for trajectory_plugins + // Parameters from ROS for joint_trajectory_controller_plugins std::shared_ptr param_listener_; Params params_; }; -} // namespace trajectory_plugins +} // namespace joint_trajectory_controller_plugins -#endif // TRAJECTORY_PLUGINS__PID_TRAJECTORY_PLUGIN_HPP_ +#endif // JOINT_TRAJECTORY_CONTROLLER_PLUGINS__PID_TRAJECTORY_PLUGIN_HPP_ diff --git a/trajectory_plugins/include/trajectory_plugins/trajectory_controller_base.hpp b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp similarity index 71% rename from trajectory_plugins/include/trajectory_plugins/trajectory_controller_base.hpp rename to joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp index 4d969b3749..be1bb6f82d 100644 --- a/trajectory_plugins/include/trajectory_plugins/trajectory_controller_base.hpp +++ b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAJECTORY_PLUGINS__TRAJECTORY_CONTROLLER_BASE_HPP_ -#define TRAJECTORY_PLUGINS__TRAJECTORY_CONTROLLER_BASE_HPP_ +#ifndef JOINT_TRAJECTORY_CONTROLLER_PLUGINS__TRAJECTORY_CONTROLLER_BASE_HPP_ +#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS__TRAJECTORY_CONTROLLER_BASE_HPP_ #include @@ -23,32 +23,32 @@ #include "trajectory_msgs/msg/joint_trajectory.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" -#include "trajectory_plugins/visibility_control.h" +#include "joint_trajectory_controller_plugins/visibility_control.h" -namespace trajectory_plugins +namespace joint_trajectory_controller_plugins { class TrajectoryControllerBase { public: - TRAJECTORY_PLUGINS_PUBLIC + JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC TrajectoryControllerBase() = default; - TRAJECTORY_PLUGINS_PUBLIC + JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC virtual ~TrajectoryControllerBase() = default; /** */ - TRAJECTORY_PLUGINS_PUBLIC + JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC virtual bool initialize(rclcpp_lifecycle::LifecycleNode::SharedPtr node) = 0; /** */ - TRAJECTORY_PLUGINS_PUBLIC + JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC virtual bool computeGains(const trajectory_msgs::msg::JointTrajectory trajectory) = 0; /** */ - TRAJECTORY_PLUGINS_PUBLIC + JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC virtual void computeCommands( std::vector & tmp_command, const trajectory_msgs::msg::JointTrajectoryPoint current, const trajectory_msgs::msg::JointTrajectoryPoint error, @@ -57,7 +57,7 @@ class TrajectoryControllerBase /** */ - TRAJECTORY_PLUGINS_PUBLIC + JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC virtual void reset() = 0; protected: @@ -65,6 +65,6 @@ class TrajectoryControllerBase rclcpp::Node::SharedPtr node_; }; -} // namespace trajectory_plugins +} // namespace joint_trajectory_controller_plugins -#endif // TRAJECTORY_PLUGINS__TRAJECTORY_CONTROLLER_BASE_HPP_ +#endif // JOINT_TRAJECTORY_CONTROLLER_PLUGINS__TRAJECTORY_CONTROLLER_BASE_HPP_ diff --git a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/visibility_control.h b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/visibility_control.h new file mode 100644 index 0000000000..71b56114c6 --- /dev/null +++ b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/visibility_control.h @@ -0,0 +1,49 @@ +// Copyright 2023 AIT Austrian Institute of Technology +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef JOINT_TRAJECTORY_CONTROLLER_PLUGINS__VISIBILITY_CONTROL_H_ +#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS_EXPORT __attribute__((dllexport)) +#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS_IMPORT __attribute__((dllimport)) +#else +#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS_EXPORT __declspec(dllexport) +#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS_IMPORT __declspec(dllimport) +#endif +#ifdef JOINT_TRAJECTORY_CONTROLLER_PLUGINS_BUILDING_LIBRARY +#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC JOINT_TRAJECTORY_CONTROLLER_PLUGINS_EXPORT +#else +#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC JOINT_TRAJECTORY_CONTROLLER_PLUGINS_IMPORT +#endif +#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC_TYPE JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC +#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS_LOCAL +#else +#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS_EXPORT __attribute__((visibility("default"))) +#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS_IMPORT +#if __GNUC__ >= 4 +#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC __attribute__((visibility("default"))) +#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS_LOCAL __attribute__((visibility("hidden"))) +#else +#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC +#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS_LOCAL +#endif +#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC_TYPE +#endif + +#endif // JOINT_TRAJECTORY_CONTROLLER_PLUGINS__VISIBILITY_CONTROL_H_ diff --git a/trajectory_plugins/package.xml b/joint_trajectory_controller_plugins/package.xml similarity index 93% rename from trajectory_plugins/package.xml rename to joint_trajectory_controller_plugins/package.xml index 97060ae1a4..b0812ed8f8 100644 --- a/trajectory_plugins/package.xml +++ b/joint_trajectory_controller_plugins/package.xml @@ -1,7 +1,7 @@ - trajectory_plugins + joint_trajectory_controller_plugins 0.0.0 TODO: Package description vscode diff --git a/trajectory_plugins/plugins.xml b/joint_trajectory_controller_plugins/plugins.xml similarity index 53% rename from trajectory_plugins/plugins.xml rename to joint_trajectory_controller_plugins/plugins.xml index 8c94d5191b..af91db2a73 100644 --- a/trajectory_plugins/plugins.xml +++ b/joint_trajectory_controller_plugins/plugins.xml @@ -1,6 +1,6 @@ + type="joint_trajectory_controller_plugins::PidTrajectoryPlugin" base_class_type="joint_trajectory_controller_plugins::TrajectoryControllerBase"> A trajectory controller consisting of a PID controller per joint. diff --git a/trajectory_plugins/src/pid_trajectory_plugin.cpp b/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp similarity index 89% rename from trajectory_plugins/src/pid_trajectory_plugin.cpp rename to joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp index 0c7d538ff0..3b40cf2e4b 100644 --- a/trajectory_plugins/src/pid_trajectory_plugin.cpp +++ b/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "trajectory_plugins/pid_trajectory_plugin.hpp" +#include "joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp" -namespace trajectory_plugins +namespace joint_trajectory_controller_plugins { bool PidTrajectoryPlugin::initialize(rclcpp_lifecycle::LifecycleNode::SharedPtr node) @@ -80,9 +80,10 @@ void PidTrajectoryPlugin::reset() } } -} // namespace trajectory_plugins +} // namespace joint_trajectory_controller_plugins #include PLUGINLIB_EXPORT_CLASS( - trajectory_plugins::PidTrajectoryPlugin, trajectory_plugins::TrajectoryControllerBase) + joint_trajectory_controller_plugins::PidTrajectoryPlugin, + joint_trajectory_controller_plugins::TrajectoryControllerBase) diff --git a/trajectory_plugins/src/pid_trajectory_plugin_parameters.yaml b/joint_trajectory_controller_plugins/src/pid_trajectory_plugin_parameters.yaml similarity index 95% rename from trajectory_plugins/src/pid_trajectory_plugin_parameters.yaml rename to joint_trajectory_controller_plugins/src/pid_trajectory_plugin_parameters.yaml index cde7c9cdfc..2e8282a175 100644 --- a/trajectory_plugins/src/pid_trajectory_plugin_parameters.yaml +++ b/joint_trajectory_controller_plugins/src/pid_trajectory_plugin_parameters.yaml @@ -1,4 +1,4 @@ -trajectory_plugins: +joint_trajectory_controller_plugins: joints: { type: string_array, default_value: [], diff --git a/trajectory_plugins/test/test_load_pid_trajectory.cpp b/joint_trajectory_controller_plugins/test/test_load_pid_trajectory.cpp similarity index 72% rename from trajectory_plugins/test/test_load_pid_trajectory.cpp rename to joint_trajectory_controller_plugins/test/test_load_pid_trajectory.cpp index cb48d0d77d..09a08d6cee 100644 --- a/trajectory_plugins/test/test_load_pid_trajectory.cpp +++ b/joint_trajectory_controller_plugins/test/test_load_pid_trajectory.cpp @@ -17,19 +17,21 @@ #include "gmock/gmock.h" +#include "joint_trajectory_controller_plugins/trajectory_controller_base.hpp" #include "pluginlib/class_loader.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "trajectory_plugins/trajectory_controller_base.hpp" TEST(TestLoadPidController, load_controller) { rclcpp::init(0, nullptr); - pluginlib::ClassLoader traj_controller_loader( - "trajectory_plugins", "trajectory_plugins::TrajectoryControllerBase"); + pluginlib::ClassLoader + traj_controller_loader( + "joint_trajectory_controller_plugins", + "joint_trajectory_controller_plugins::TrajectoryControllerBase"); - std::shared_ptr traj_contr; + std::shared_ptr traj_contr; auto available_classes = traj_controller_loader.getDeclaredClasses(); @@ -39,7 +41,7 @@ TEST(TestLoadPidController, load_controller) std::cout << " " << available_class << std::endl; } - std::string controller_type = "trajectory_plugins::PidTrajectoryPlugin"; + std::string controller_type = "joint_trajectory_controller_plugins::PidTrajectoryPlugin"; ASSERT_TRUE(traj_controller_loader.isClassAvailable(controller_type)); ASSERT_NO_THROW(traj_contr = traj_controller_loader.createSharedInstance(controller_type)); diff --git a/trajectory_plugins/include/trajectory_plugins/visibility_control.h b/trajectory_plugins/include/trajectory_plugins/visibility_control.h deleted file mode 100644 index 14cea41dd1..0000000000 --- a/trajectory_plugins/include/trajectory_plugins/visibility_control.h +++ /dev/null @@ -1,49 +0,0 @@ -// Copyright 2023 AIT Austrian Institute of Technology -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef TRAJECTORY_PLUGINS__VISIBILITY_CONTROL_H_ -#define TRAJECTORY_PLUGINS__VISIBILITY_CONTROL_H_ - -// This logic was borrowed (then namespaced) from the examples on the gcc wiki: -// https://gcc.gnu.org/wiki/Visibility - -#if defined _WIN32 || defined __CYGWIN__ -#ifdef __GNUC__ -#define TRAJECTORY_PLUGINS_EXPORT __attribute__((dllexport)) -#define TRAJECTORY_PLUGINS_IMPORT __attribute__((dllimport)) -#else -#define TRAJECTORY_PLUGINS_EXPORT __declspec(dllexport) -#define TRAJECTORY_PLUGINS_IMPORT __declspec(dllimport) -#endif -#ifdef TRAJECTORY_PLUGINS_BUILDING_LIBRARY -#define TRAJECTORY_PLUGINS_PUBLIC TRAJECTORY_PLUGINS_EXPORT -#else -#define TRAJECTORY_PLUGINS_PUBLIC TRAJECTORY_PLUGINS_IMPORT -#endif -#define TRAJECTORY_PLUGINS_PUBLIC_TYPE TRAJECTORY_PLUGINS_PUBLIC -#define TRAJECTORY_PLUGINS_LOCAL -#else -#define TRAJECTORY_PLUGINS_EXPORT __attribute__((visibility("default"))) -#define TRAJECTORY_PLUGINS_IMPORT -#if __GNUC__ >= 4 -#define TRAJECTORY_PLUGINS_PUBLIC __attribute__((visibility("default"))) -#define TRAJECTORY_PLUGINS_LOCAL __attribute__((visibility("hidden"))) -#else -#define TRAJECTORY_PLUGINS_PUBLIC -#define TRAJECTORY_PLUGINS_LOCAL -#endif -#define TRAJECTORY_PLUGINS_PUBLIC_TYPE -#endif - -#endif // TRAJECTORY_PLUGINS__VISIBILITY_CONTROL_H_ From 9e9958242b1deec2726c4b8b7d2397e84cbf0030 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 6 Dec 2023 12:35:56 +0000 Subject: [PATCH 09/30] Add class_loader as persistent jtc-class member variable --- .../joint_trajectory_controller.hpp | 7 ++++- .../src/joint_trajectory_controller.cpp | 26 +++++++++++++------ 2 files changed, 24 insertions(+), 9 deletions(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index 3e687480ca..3f58237a6c 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -27,6 +27,7 @@ #include "control_msgs/srv/query_trajectory_state.hpp" #include "controller_interface/controller_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "pluginlib/class_loader.hpp" #include "rclcpp/duration.hpp" #include "rclcpp/subscription.hpp" #include "rclcpp/time.hpp" @@ -162,7 +163,11 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa /// If true, a velocity feedforward term plus corrective PID term is used bool use_closed_loop_pid_adapter_ = false; - // The actual trajectory controller used for closed loop pid adapter + // class loader for actual trajectory controller + std::shared_ptr< + pluginlib::ClassLoader> + traj_controller_loader_; + // The actual trajectory controller std::shared_ptr traj_contr_; // Configuration for every joint, if position error is wrapped around std::vector joints_angle_wraparound_; diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index cb54456425..b426fc4d8b 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -30,7 +30,6 @@ #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "lifecycle_msgs/msg/state.hpp" -#include "pluginlib/class_loader.hpp" #include "rclcpp/event_handler.hpp" #include "rclcpp/logging.hpp" #include "rclcpp/parameter.hpp" @@ -48,7 +47,13 @@ namespace joint_trajectory_controller { JointTrajectoryController::JointTrajectoryController() -: controller_interface::ControllerInterface(), dof_(0) +: controller_interface::ControllerInterface(), + dof_(0), + traj_controller_loader_( + std::make_shared< + pluginlib::ClassLoader>( + "joint_trajectory_controller_plugins", + "joint_trajectory_controller_plugins::TrajectoryControllerBase")) { } @@ -708,13 +713,10 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( if (use_closed_loop_pid_adapter_) { - pluginlib::ClassLoader - traj_controller_loader( - "joint_trajectory_controller_plugins", - "joint_trajectory_controller_plugins::TrajectoryControllerBase"); try { - traj_contr_ = traj_controller_loader.createSharedInstance(params_.controller_plugin.c_str()); + traj_contr_ = + traj_controller_loader_->createSharedInstance(params_.controller_plugin.c_str()); } catch (pluginlib::PluginlibException & ex) { @@ -726,9 +728,17 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( if (!traj_contr_->initialize(get_node())) { RCLCPP_FATAL( - logger, "The trajectory controller plugin failed to initialize for some reason. Aborting."); + logger, + "The trajectory controller plugin `%s` failed to initialize for some reason. Aborting.", + params_.controller_plugin.c_str()); return CallbackReturn::FAILURE; } + else + { + RCLCPP_INFO( + logger, "The trajectory controller plugin `%s` was loaded.", + params_.controller_plugin.c_str()); + } tmp_command_.resize(dof_, 0.0); } From 63e630d231f1693ba35ea36ba09e3a7b44fadc56 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 6 Dec 2023 12:38:55 +0000 Subject: [PATCH 10/30] Set PID parameters during tests only if plugin is loaded --- .../test/test_trajectory_controller_utils.hpp | 31 ++++++++++++++----- 1 file changed, 24 insertions(+), 7 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 4a65b4ad51..85306a2705 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -214,8 +214,20 @@ class TrajectoryControllerTest : public ::testing::Test return traj_controller_->init(controller_name_, "", 0, "", node_options); } - void SetPidParameters( - double p_default = 0.0, double ff_default = 1.0, bool angle_wraparound_default = false) + void SetJointParameters(bool angle_wraparound_default = false) + { + traj_controller_->trigger_declare_parameters(); + auto node = traj_controller_->get_node(); + + for (size_t i = 0; i < joint_names_.size(); ++i) + { + const std::string prefix = "gains." + joint_names_[i]; + const rclcpp::Parameter angle_wraparound(prefix + ".angle_wraparound", angle_wraparound_default); + node->set_parameters({angle_wraparound}); + } + } + + void SetPidParameters(double p_default = 0.0, double ff_default = 1.0) { traj_controller_->trigger_declare_parameters(); auto node = traj_controller_->get_node(); @@ -228,9 +240,7 @@ class TrajectoryControllerTest : public ::testing::Test const rclcpp::Parameter k_d(prefix + ".d", 0.0); const rclcpp::Parameter i_clamp(prefix + ".i_clamp", 0.0); const rclcpp::Parameter ff_velocity_scale(prefix + ".ff_velocity_scale", ff_default); - const rclcpp::Parameter angle_wraparound( - prefix + ".angle_wraparound", angle_wraparound_default); - node->set_parameters({k_p, k_i, k_d, i_clamp, ff_velocity_scale, angle_wraparound}); + node->set_parameters({k_p, k_i, k_d, i_clamp, ff_velocity_scale}); } } @@ -259,10 +269,17 @@ class TrajectoryControllerTest : public ::testing::Test // read-only parameters have to be set before init -> won't be read otherwise SetUpTrajectoryController(executor, parameters_local); - // set pid parameters before configure - SetPidParameters(k_p, ff, angle_wraparound); + SetJointParameters(angle_wraparound); + traj_controller_->get_node()->configure(); + // set pid parameters before activate. The PID plugin has to be loaded already, otherwise + // parameters are not declared yet + if (traj_controller_->use_closed_loop_pid_adapter()) + { + SetPidParameters(k_p, ff); + } + ActivateTrajectoryController( separate_cmd_and_state_values, initial_pos_joints, initial_vel_joints, initial_acc_joints, initial_eff_joints); From 2ef3cc407cc72a444955a6c948b6587a88c45644 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 6 Dec 2023 12:38:55 +0000 Subject: [PATCH 11/30] Add test to propery initialize PID plugin --- .../CMakeLists.txt | 6 +- .../pid_trajectory_plugin.hpp | 6 +- .../trajectory_controller_base.hpp | 2 +- .../src/pid_trajectory_plugin.cpp | 24 +++-- .../src/pid_trajectory_plugin_parameters.yaml | 2 +- .../test/test_pid_trajectory.cpp | 98 +++++++++++++++++++ .../test/test_pid_trajectory.hpp | 72 ++++++++++++++ 7 files changed, 197 insertions(+), 13 deletions(-) create mode 100644 joint_trajectory_controller_plugins/test/test_pid_trajectory.cpp create mode 100644 joint_trajectory_controller_plugins/test/test_pid_trajectory.hpp diff --git a/joint_trajectory_controller_plugins/CMakeLists.txt b/joint_trajectory_controller_plugins/CMakeLists.txt index ab7f95a9c7..42f2275fb9 100644 --- a/joint_trajectory_controller_plugins/CMakeLists.txt +++ b/joint_trajectory_controller_plugins/CMakeLists.txt @@ -57,7 +57,11 @@ install( if(BUILD_TESTING) ament_add_gmock(test_load_pid_trajectory test/test_load_pid_trajectory.cpp) target_link_libraries(test_load_pid_trajectory pid_trajectory_plugin pid_trajectory_plugin_parameters) - ament_target_dependencies(test_load_pid_trajectory ${THIS_PACKAGE_INCLUDE_DEPENDS} ) + ament_target_dependencies(test_load_pid_trajectory ${THIS_PACKAGE_INCLUDE_DEPENDS}) + + ament_add_gmock(test_pid_trajectory test/test_pid_trajectory.cpp) + target_link_libraries(test_pid_trajectory pid_trajectory_plugin pid_trajectory_plugin_parameters) + ament_target_dependencies(test_pid_trajectory ${THIS_PACKAGE_INCLUDE_DEPENDS}) endif() ament_export_include_directories( diff --git a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp index 883b0648d8..54f797479e 100644 --- a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp +++ b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp @@ -32,7 +32,7 @@ namespace joint_trajectory_controller_plugins class PidTrajectoryPlugin : public TrajectoryControllerBase { public: - bool initialize(rclcpp_lifecycle::LifecycleNode::SharedPtr node) override; + bool initialize(rclcpp::Node::SharedPtr node) override; bool computeGains(const trajectory_msgs::msg::JointTrajectory trajectory) override; @@ -45,8 +45,8 @@ class PidTrajectoryPlugin : public TrajectoryControllerBase void reset() override; protected: - // the node handle for parameter handling - rclcpp_lifecycle::LifecycleNode::SharedPtr node_; + // degree of freedom + size_t dof_; // PID controllers std::vector pids_; // Feed-forward velocity weight factor when calculating closed loop pid adapter's command diff --git a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp index be1bb6f82d..5840d66fb6 100644 --- a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp +++ b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp @@ -39,7 +39,7 @@ class TrajectoryControllerBase /** */ JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC - virtual bool initialize(rclcpp_lifecycle::LifecycleNode::SharedPtr node) = 0; + virtual bool initialize(rclcpp::Node::SharedPtr node) = 0; /** */ diff --git a/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp b/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp index 3b40cf2e4b..4764950195 100644 --- a/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp +++ b/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp @@ -17,7 +17,7 @@ namespace joint_trajectory_controller_plugins { -bool PidTrajectoryPlugin::initialize(rclcpp_lifecycle::LifecycleNode::SharedPtr node) +bool PidTrajectoryPlugin::initialize(rclcpp::Node::SharedPtr node) { node_ = node; @@ -26,14 +26,25 @@ bool PidTrajectoryPlugin::initialize(rclcpp_lifecycle::LifecycleNode::SharedPtr // Create the parameter listener and get the parameters param_listener_ = std::make_shared(node_); params_ = param_listener_->get_params(); + dof_ = params_.joints.size(); } catch (const std::exception & e) { fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); return false; } + RCLCPP_INFO(node_->get_logger(), "[PidTrajectoryPlugin] Initialized with %lu joints.", dof_); + return true; +} + +bool PidTrajectoryPlugin::computeGains(const trajectory_msgs::msg::JointTrajectory /*trajectory*/) +{ + if (param_listener_->is_old(params_)) + { + params_ = param_listener_->get_params(); + RCLCPP_DEBUG(node_->get_logger(), "[PidTrajectoryPlugin] Updated parameters"); + } - size_t dof_ = params_.joints.size(); pids_.resize(dof_); ff_velocity_scale_.resize(dof_); @@ -46,11 +57,10 @@ bool PidTrajectoryPlugin::initialize(rclcpp_lifecycle::LifecycleNode::SharedPtr ff_velocity_scale_[i] = gains.ff_velocity_scale; } - return true; -} -bool PidTrajectoryPlugin::computeGains(const trajectory_msgs::msg::JointTrajectory /*trajectory*/) -{ + RCLCPP_INFO( + node_->get_logger(), + "[PidTrajectoryPlugin] Loaded PID gains from ROS parameters for %lu joints.", dof_); return true; } @@ -61,7 +71,7 @@ void PidTrajectoryPlugin::computeCommands( const rclcpp::Duration & period) { // Update PIDs - for (auto i = 0ul; i < tmp_command.size(); ++i) + for (auto i = 0ul; i < dof_; ++i) { tmp_command[i] = (desired.velocities[i] * ff_velocity_scale_[i]) + pids_[i]->computeCommand( diff --git a/joint_trajectory_controller_plugins/src/pid_trajectory_plugin_parameters.yaml b/joint_trajectory_controller_plugins/src/pid_trajectory_plugin_parameters.yaml index 2e8282a175..df9ccc0741 100644 --- a/joint_trajectory_controller_plugins/src/pid_trajectory_plugin_parameters.yaml +++ b/joint_trajectory_controller_plugins/src/pid_trajectory_plugin_parameters.yaml @@ -5,7 +5,7 @@ joint_trajectory_controller_plugins: description: "Names of joints used by the controller", read_only: true, validation: { - unique<>: null, + size_gt<>: [0], } } gains: diff --git a/joint_trajectory_controller_plugins/test/test_pid_trajectory.cpp b/joint_trajectory_controller_plugins/test/test_pid_trajectory.cpp new file mode 100644 index 0000000000..83de0efb68 --- /dev/null +++ b/joint_trajectory_controller_plugins/test/test_pid_trajectory.cpp @@ -0,0 +1,98 @@ +// Copyright 2023 AIT Austrian Institute of Technology +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test_pid_trajectory.hpp" + +TEST_F(PidTrajectoryTest, TestEmptySetup) +{ + std::shared_ptr traj_contr = + std::make_shared(); + + ASSERT_FALSE(traj_contr->initialize(node_)); +} + +TEST_F(PidTrajectoryTest, TestSingleJoint) +{ + std::shared_ptr traj_contr = + std::make_shared(); + + std::vector joint_names = {"joint1"}; + auto joint_names_paramv = rclcpp::ParameterValue(joint_names); + + // override read_only parameter + node_->declare_parameter("joints", joint_names_paramv); + + ASSERT_TRUE(traj_contr->initialize(node_)); + + // set dynamic parameters + traj_contr->trigger_declare_parameters(); + node_->set_parameter(rclcpp::Parameter("gains.joint1.p", 1.0)); + + ASSERT_TRUE(traj_contr->computeGains(trajectory_msgs::msg::JointTrajectory())); + + trajectory_msgs::msg::JointTrajectoryPoint traj_msg; + traj_msg.positions.push_back(0.0); + traj_msg.velocities.push_back(0.0); + std::vector tmp_command(1, 0.0); + const rclcpp::Time time; + const rclcpp::Duration period(1, 0); + + ASSERT_NO_THROW( + traj_contr->computeCommands(tmp_command, traj_msg, traj_msg, traj_msg, time, period)); +} + +TEST_F(PidTrajectoryTest, TestMultipleJoints) +{ + std::shared_ptr traj_contr = + std::make_shared(); + + std::vector joint_names = {"joint1", "joint2", "joint3"}; + auto joint_names_paramv = rclcpp::ParameterValue(joint_names); + + // override read_only parameter + node_->declare_parameter("joints", joint_names_paramv); + + ASSERT_TRUE(traj_contr->initialize(node_)); + + // set dynamic parameters + traj_contr->trigger_declare_parameters(); + node_->set_parameter(rclcpp::Parameter("gains.joint1.p", 1.0)); + node_->set_parameter(rclcpp::Parameter("gains.joint2.p", 1.0)); + node_->set_parameter(rclcpp::Parameter("gains.joint3.p", 1.0)); + + ASSERT_TRUE(traj_contr->computeGains(trajectory_msgs::msg::JointTrajectory())); + + trajectory_msgs::msg::JointTrajectoryPoint traj_msg; + traj_msg.positions.push_back(0.0); + traj_msg.positions.push_back(0.0); + traj_msg.positions.push_back(0.0); + traj_msg.velocities.push_back(0.0); + traj_msg.velocities.push_back(0.0); + traj_msg.velocities.push_back(0.0); + std::vector tmp_command(3, 0.0); + const rclcpp::Time time; + const rclcpp::Duration period(1, 0); + + ASSERT_NO_THROW( + traj_contr->computeCommands(tmp_command, traj_msg, traj_msg, traj_msg, time, period)); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/joint_trajectory_controller_plugins/test/test_pid_trajectory.hpp b/joint_trajectory_controller_plugins/test/test_pid_trajectory.hpp new file mode 100644 index 0000000000..ccd97dbb0a --- /dev/null +++ b/joint_trajectory_controller_plugins/test/test_pid_trajectory.hpp @@ -0,0 +1,72 @@ +// Copyright 2023 AIT Austrian Institute of Technology +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_PID_TRAJECTORY_HPP_ +#define TEST_PID_TRAJECTORY_HPP_ + +#include +#include +#include +#include + +#include "gmock/gmock.h" + +#include "joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "trajectory_msgs/msg/joint_trajectory.hpp" + +namespace +{ +static const rclcpp::Logger LOGGER = rclcpp::get_logger("test_pid_trajectory"); +} // namespace + +class TestableJointTrajectoryControllerPlugin +: public joint_trajectory_controller_plugins::PidTrajectoryPlugin +{ +public: + // updates mapped parameters, i.e., should be called after setting the joint names + void trigger_declare_parameters() { param_listener_->declare_params(); } +}; + +class PidTrajectoryTest : public ::testing::Test +{ +public: + void SetUp() override + { + auto testname = ::testing::UnitTest::GetInstance()->current_test_info()->name(); + node_ = std::make_shared(testname); + executor_->add_node(node_); + executor_thread_ = std::thread([this]() { executor_->spin(); }); + } + + PidTrajectoryTest() { executor_ = std::make_shared(); } + + void TearDown() override + { + executor_->cancel(); + if (executor_thread_.joinable()) + { + executor_thread_.join(); + } + node_.reset(); + } + +protected: + rclcpp::Node::SharedPtr node_; + rclcpp::Executor::SharedPtr executor_; + std::thread executor_thread_; +}; + +#endif // TEST_PID_TRAJECTORY_HPP_ From 45f7403d02c31be9b626c61c07fd3849a7bc5937 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 6 Dec 2023 12:38:55 +0000 Subject: [PATCH 12/30] Use lifecycle_node inside trajectory_controller plugin --- .../pid_trajectory_plugin.hpp | 2 +- .../trajectory_controller_base.hpp | 4 ++-- .../src/pid_trajectory_plugin.cpp | 2 +- .../test/test_pid_trajectory.hpp | 6 +++--- 4 files changed, 7 insertions(+), 7 deletions(-) diff --git a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp index 54f797479e..ecf678c018 100644 --- a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp +++ b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp @@ -32,7 +32,7 @@ namespace joint_trajectory_controller_plugins class PidTrajectoryPlugin : public TrajectoryControllerBase { public: - bool initialize(rclcpp::Node::SharedPtr node) override; + bool initialize(rclcpp_lifecycle::LifecycleNode::SharedPtr node) override; bool computeGains(const trajectory_msgs::msg::JointTrajectory trajectory) override; diff --git a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp index 5840d66fb6..1e2b78e44c 100644 --- a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp +++ b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp @@ -39,7 +39,7 @@ class TrajectoryControllerBase /** */ JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC - virtual bool initialize(rclcpp::Node::SharedPtr node) = 0; + virtual bool initialize(rclcpp_lifecycle::LifecycleNode::SharedPtr node) = 0; /** */ @@ -62,7 +62,7 @@ class TrajectoryControllerBase protected: // the node handle for parameter handling - rclcpp::Node::SharedPtr node_; + rclcpp_lifecycle::LifecycleNode::SharedPtr node_; }; } // namespace joint_trajectory_controller_plugins diff --git a/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp b/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp index 4764950195..506ee8ea5c 100644 --- a/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp +++ b/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp @@ -17,7 +17,7 @@ namespace joint_trajectory_controller_plugins { -bool PidTrajectoryPlugin::initialize(rclcpp::Node::SharedPtr node) +bool PidTrajectoryPlugin::initialize(rclcpp_lifecycle::LifecycleNode::SharedPtr node) { node_ = node; diff --git a/joint_trajectory_controller_plugins/test/test_pid_trajectory.hpp b/joint_trajectory_controller_plugins/test/test_pid_trajectory.hpp index ccd97dbb0a..ebe2b95a98 100644 --- a/joint_trajectory_controller_plugins/test/test_pid_trajectory.hpp +++ b/joint_trajectory_controller_plugins/test/test_pid_trajectory.hpp @@ -46,8 +46,8 @@ class PidTrajectoryTest : public ::testing::Test void SetUp() override { auto testname = ::testing::UnitTest::GetInstance()->current_test_info()->name(); - node_ = std::make_shared(testname); - executor_->add_node(node_); + node_ = std::make_shared(testname); + executor_->add_node(node_->get_node_base_interface()); executor_thread_ = std::thread([this]() { executor_->spin(); }); } @@ -64,7 +64,7 @@ class PidTrajectoryTest : public ::testing::Test } protected: - rclcpp::Node::SharedPtr node_; + rclcpp_lifecycle::LifecycleNode::SharedPtr node_; rclcpp::Executor::SharedPtr executor_; std::thread executor_thread_; }; From 0df086ec2794954278999f09c72ccf88e4071af1 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 6 Dec 2023 12:38:55 +0000 Subject: [PATCH 13/30] Deactivate reconfigure of controller plugin --- .../src/joint_trajectory_controller_parameters.yaml | 1 + 1 file changed, 1 insertion(+) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index bc33c13148..b5e20162f4 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -90,6 +90,7 @@ joint_trajectory_controller: type: string, default_value: "joint_trajectory_controller_plugins::PidTrajectoryPlugin", description: "Type of the plugin for the trajectory controller", + read_only: true, } gains: __map_joints: From d5590076de1f553c6106cb4dd6447468bab1df90 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 6 Dec 2023 12:38:55 +0000 Subject: [PATCH 14/30] Compute gains on on_activate --- .../src/joint_trajectory_controller.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index b426fc4d8b..fc1240cfa7 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -725,7 +725,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( params_.controller_plugin.c_str(), ex.what()); return CallbackReturn::FAILURE; } - if (!traj_contr_->initialize(get_node())) + if (traj_contr_->initialize(get_node()) == false) { RCLCPP_FATAL( logger, @@ -994,6 +994,14 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( cmd_timeout_ = 0.0; } + // update gains of PID controller + if (use_closed_loop_pid_adapter_) + { + trajectory_msgs::msg::JointTrajectory traj; + traj.points.push_back(state_current_); + traj_contr_->computeGains(traj); + } + return CallbackReturn::SUCCESS; } From f80bb68f8f4c8febe110c5a211cd2f443172615b Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 6 Dec 2023 12:38:55 +0000 Subject: [PATCH 15/30] Let PID controller parse command joint names instead of joints --- .../src/joint_trajectory_controller.cpp | 2 +- .../pid_trajectory_plugin.hpp | 11 +++++-- .../trajectory_controller_base.hpp | 5 ++- .../src/pid_trajectory_plugin.cpp | 31 +++++++++++++------ .../test/test_pid_trajectory.cpp | 6 ++-- 5 files changed, 38 insertions(+), 17 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index fc1240cfa7..4e0d1fd074 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -725,7 +725,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( params_.controller_plugin.c_str(), ex.what()); return CallbackReturn::FAILURE; } - if (traj_contr_->initialize(get_node()) == false) + if (traj_contr_->initialize(get_node(), command_joint_names_) == false) { RCLCPP_FATAL( logger, diff --git a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp index ecf678c018..7cd89aff36 100644 --- a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp +++ b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp @@ -16,6 +16,7 @@ #define JOINT_TRAJECTORY_CONTROLLER_PLUGINS__PID_TRAJECTORY_PLUGIN_HPP_ #include +#include #include #include "control_toolbox/pid.hpp" @@ -32,7 +33,9 @@ namespace joint_trajectory_controller_plugins class PidTrajectoryPlugin : public TrajectoryControllerBase { public: - bool initialize(rclcpp_lifecycle::LifecycleNode::SharedPtr node) override; + bool initialize( + rclcpp_lifecycle::LifecycleNode::SharedPtr node, + std::vector command_joint_names) override; bool computeGains(const trajectory_msgs::msg::JointTrajectory trajectory) override; @@ -45,8 +48,10 @@ class PidTrajectoryPlugin : public TrajectoryControllerBase void reset() override; protected: - // degree of freedom - size_t dof_; + // number of command joints + size_t num_cmd_joints_; + // name of the command joints + std::vector command_joint_names_; // PID controllers std::vector pids_; // Feed-forward velocity weight factor when calculating closed loop pid adapter's command diff --git a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp index 1e2b78e44c..ac2de8108e 100644 --- a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp +++ b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp @@ -15,6 +15,7 @@ #ifndef JOINT_TRAJECTORY_CONTROLLER_PLUGINS__TRAJECTORY_CONTROLLER_BASE_HPP_ #define JOINT_TRAJECTORY_CONTROLLER_PLUGINS__TRAJECTORY_CONTROLLER_BASE_HPP_ +#include #include #include "rclcpp/rclcpp.hpp" @@ -39,7 +40,9 @@ class TrajectoryControllerBase /** */ JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC - virtual bool initialize(rclcpp_lifecycle::LifecycleNode::SharedPtr node) = 0; + virtual bool initialize( + rclcpp_lifecycle::LifecycleNode::SharedPtr node, + std::vector command_joint_names) = 0; /** */ diff --git a/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp b/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp index 506ee8ea5c..da3c7b76d3 100644 --- a/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp +++ b/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp @@ -17,23 +17,36 @@ namespace joint_trajectory_controller_plugins { -bool PidTrajectoryPlugin::initialize(rclcpp_lifecycle::LifecycleNode::SharedPtr node) +bool PidTrajectoryPlugin::initialize( + rclcpp_lifecycle::LifecycleNode::SharedPtr node, std::vector command_joint_names) { node_ = node; + command_joint_names_ = command_joint_names; try { // Create the parameter listener and get the parameters param_listener_ = std::make_shared(node_); params_ = param_listener_->get_params(); - dof_ = params_.joints.size(); } catch (const std::exception & e) { fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); return false; } - RCLCPP_INFO(node_->get_logger(), "[PidTrajectoryPlugin] Initialized with %lu joints.", dof_); + + // parse read-only params + num_cmd_joints_ = command_joint_names_.size(); + if (num_cmd_joints_ == 0) + { + RCLCPP_ERROR(node_->get_logger(), "[PidTrajectoryPlugin] No command joints specified."); + return false; + } + pids_.resize(num_cmd_joints_); + ff_velocity_scale_.resize(num_cmd_joints_); + + RCLCPP_INFO( + node_->get_logger(), "[PidTrajectoryPlugin] Initialized with %lu joints.", num_cmd_joints_); return true; } @@ -45,13 +58,13 @@ bool PidTrajectoryPlugin::computeGains(const trajectory_msgs::msg::JointTrajecto RCLCPP_DEBUG(node_->get_logger(), "[PidTrajectoryPlugin] Updated parameters"); } - pids_.resize(dof_); - ff_velocity_scale_.resize(dof_); + pids_.resize(num_cmd_joints_); + ff_velocity_scale_.resize(num_cmd_joints_); // Init PID gains from ROS parameters - for (size_t i = 0; i < dof_; ++i) + for (size_t i = 0; i < num_cmd_joints_; ++i) { - const auto & gains = params_.gains.joints_map.at(params_.joints[i]); + const auto & gains = params_.gains.joints_map.at(command_joint_names_[i]); pids_[i] = std::make_shared( gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp); @@ -60,7 +73,7 @@ bool PidTrajectoryPlugin::computeGains(const trajectory_msgs::msg::JointTrajecto RCLCPP_INFO( node_->get_logger(), - "[PidTrajectoryPlugin] Loaded PID gains from ROS parameters for %lu joints.", dof_); + "[PidTrajectoryPlugin] Loaded PID gains from ROS parameters for %lu joints.", num_cmd_joints_); return true; } @@ -71,7 +84,7 @@ void PidTrajectoryPlugin::computeCommands( const rclcpp::Duration & period) { // Update PIDs - for (auto i = 0ul; i < dof_; ++i) + for (auto i = 0ul; i < num_cmd_joints_; ++i) { tmp_command[i] = (desired.velocities[i] * ff_velocity_scale_[i]) + pids_[i]->computeCommand( diff --git a/joint_trajectory_controller_plugins/test/test_pid_trajectory.cpp b/joint_trajectory_controller_plugins/test/test_pid_trajectory.cpp index 83de0efb68..7ab113a263 100644 --- a/joint_trajectory_controller_plugins/test/test_pid_trajectory.cpp +++ b/joint_trajectory_controller_plugins/test/test_pid_trajectory.cpp @@ -19,7 +19,7 @@ TEST_F(PidTrajectoryTest, TestEmptySetup) std::shared_ptr traj_contr = std::make_shared(); - ASSERT_FALSE(traj_contr->initialize(node_)); + ASSERT_FALSE(traj_contr->initialize(node_, std::vector())); } TEST_F(PidTrajectoryTest, TestSingleJoint) @@ -33,7 +33,7 @@ TEST_F(PidTrajectoryTest, TestSingleJoint) // override read_only parameter node_->declare_parameter("joints", joint_names_paramv); - ASSERT_TRUE(traj_contr->initialize(node_)); + ASSERT_TRUE(traj_contr->initialize(node_, joint_names)); // set dynamic parameters traj_contr->trigger_declare_parameters(); @@ -63,7 +63,7 @@ TEST_F(PidTrajectoryTest, TestMultipleJoints) // override read_only parameter node_->declare_parameter("joints", joint_names_paramv); - ASSERT_TRUE(traj_contr->initialize(node_)); + ASSERT_TRUE(traj_contr->initialize(node_, joint_names)); // set dynamic parameters traj_contr->trigger_declare_parameters(); From 0e76f19064991437249e349110ee1e9a6784b72b Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 6 Dec 2023 12:41:20 +0000 Subject: [PATCH 16/30] Rename pid_adapter variable and update gains with every new trajectory --- .../joint_trajectory_controller.hpp | 2 +- .../src/joint_trajectory_controller.cpp | 24 +++++++++---------- .../test/test_trajectory_controller.cpp | 8 +++---- .../test/test_trajectory_controller_utils.hpp | 6 ++--- .../pid_trajectory_plugin.hpp | 3 ++- .../trajectory_controller_base.hpp | 4 +++- .../src/pid_trajectory_plugin.cpp | 3 ++- .../test/test_pid_trajectory.cpp | 4 ++-- 8 files changed, 28 insertions(+), 26 deletions(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index 3f58237a6c..b0ab1f1f34 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -162,7 +162,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa bool has_effort_command_interface_ = false; /// If true, a velocity feedforward term plus corrective PID term is used - bool use_closed_loop_pid_adapter_ = false; + bool use_closed_loop_control_law_ = false; // class loader for actual trajectory controller std::shared_ptr< pluginlib::ClassLoader> diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 4e0d1fd074..fa797e9644 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -284,7 +284,7 @@ controller_interface::return_type JointTrajectoryController::update( // set values for next hardware write() if tolerance is met if (!tolerance_violated_while_moving && within_goal_time) { - if (use_closed_loop_pid_adapter_) + if (use_closed_loop_control_law_) { traj_contr_->computeCommands( tmp_command_, state_current_, state_error_, state_desired_, time, period); @@ -297,7 +297,7 @@ controller_interface::return_type JointTrajectoryController::update( } if (has_velocity_command_interface_) { - if (use_closed_loop_pid_adapter_) + if (use_closed_loop_control_law_) { assign_interface_from_point(joint_command_interface_[1], tmp_command_); } @@ -705,13 +705,13 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( contains_interface_type(params_.command_interfaces, hardware_interface::HW_IF_EFFORT); // if there is only velocity or if there is effort command interface - // then use also PID adapter - use_closed_loop_pid_adapter_ = + // then use also closed loop controller + use_closed_loop_control_law_ = (has_velocity_command_interface_ && params_.command_interfaces.size() == 1 && !params_.open_loop_control) || has_effort_command_interface_; - if (use_closed_loop_pid_adapter_) + if (use_closed_loop_control_law_) { try { @@ -994,14 +994,6 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( cmd_timeout_ = 0.0; } - // update gains of PID controller - if (use_closed_loop_pid_adapter_) - { - trajectory_msgs::msg::JointTrajectory traj; - traj.points.push_back(state_current_); - traj_contr_->computeGains(traj); - } - return CallbackReturn::SUCCESS; } @@ -1468,6 +1460,12 @@ void JointTrajectoryController::add_new_trajectory_msg( const std::shared_ptr & traj_msg) { traj_msg_external_point_ptr_.writeFromNonRT(traj_msg); + + // update gains of controller + if (use_closed_loop_control_law_) + { + traj_contr_->computeGains(traj_msg); + } } void JointTrajectoryController::preempt_active_goal() diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 05010c562c..e725a4b1b6 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -847,7 +847,7 @@ TEST_P(TrajectoryControllerTestParameterized, use_closed_loop_pid) !traj_controller_->is_open_loop()) || traj_controller_->has_effort_command_interface()) { - EXPECT_TRUE(traj_controller_->use_closed_loop_pid_adapter()); + EXPECT_TRUE(traj_controller_->use_closed_loop_control_law()); } } @@ -904,7 +904,7 @@ TEST_P(TrajectoryControllerTestParameterized, velocity_error) } // is the velocity error correct? if ( - traj_controller_->use_closed_loop_pid_adapter() // always needed for PID controller + traj_controller_->use_closed_loop_control_law() // always needed for PID controller || (traj_controller_->has_velocity_state_interface() && traj_controller_->has_velocity_command_interface())) { @@ -969,8 +969,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order) if (traj_controller_->has_velocity_command_interface()) { - // if use_closed_loop_pid_adapter_==false: we expect desired velocities from direct sampling - // if use_closed_loop_pid_adapter_==true: we expect desired velocities, because we use PID with + // if use_closed_loop_control_law_==false: we expect desired velocities from direct sampling + // if use_closed_loop_control_law_==true: we expect desired velocities, because we use PID with // feedforward term only EXPECT_GT(0.0, joint_vel_[0]); EXPECT_GT(0.0, joint_vel_[1]); diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 85306a2705..62c5a41429 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -126,7 +126,7 @@ class TestableJointTrajectoryController bool has_effort_command_interface() const { return has_effort_command_interface_; } - bool use_closed_loop_pid_adapter() const { return use_closed_loop_pid_adapter_; } + bool use_closed_loop_control_law() const { return use_closed_loop_control_law_; } bool is_open_loop() const { return params_.open_loop_control; } @@ -275,7 +275,7 @@ class TrajectoryControllerTest : public ::testing::Test // set pid parameters before activate. The PID plugin has to be loaded already, otherwise // parameters are not declared yet - if (traj_controller_->use_closed_loop_pid_adapter()) + if (traj_controller_->use_closed_loop_control_law()) { SetPidParameters(k_p, ff); } @@ -545,7 +545,7 @@ class TrajectoryControllerTest : public ::testing::Test // i.e., active but trivial trajectory (one point only) EXPECT_TRUE(traj_controller_->has_trivial_traj()); - if (traj_controller_->use_closed_loop_pid_adapter() == false) + if (traj_controller_->use_closed_loop_control_law() == false) { if (traj_controller_->has_position_command_interface()) { diff --git a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp index 7cd89aff36..e1e0f55375 100644 --- a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp +++ b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp @@ -37,7 +37,8 @@ class PidTrajectoryPlugin : public TrajectoryControllerBase rclcpp_lifecycle::LifecycleNode::SharedPtr node, std::vector command_joint_names) override; - bool computeGains(const trajectory_msgs::msg::JointTrajectory trajectory) override; + bool computeGains( + const std::shared_ptr & trajectory) override; void computeCommands( std::vector & tmp_command, const trajectory_msgs::msg::JointTrajectoryPoint current, diff --git a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp index ac2de8108e..8f0750bf81 100644 --- a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp +++ b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp @@ -15,6 +15,7 @@ #ifndef JOINT_TRAJECTORY_CONTROLLER_PLUGINS__TRAJECTORY_CONTROLLER_BASE_HPP_ #define JOINT_TRAJECTORY_CONTROLLER_PLUGINS__TRAJECTORY_CONTROLLER_BASE_HPP_ +#include #include #include @@ -47,7 +48,8 @@ class TrajectoryControllerBase /** */ JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC - virtual bool computeGains(const trajectory_msgs::msg::JointTrajectory trajectory) = 0; + virtual bool computeGains( + const std::shared_ptr & trajectory) = 0; /** */ diff --git a/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp b/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp index da3c7b76d3..f0c3db75f1 100644 --- a/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp +++ b/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp @@ -50,7 +50,8 @@ bool PidTrajectoryPlugin::initialize( return true; } -bool PidTrajectoryPlugin::computeGains(const trajectory_msgs::msg::JointTrajectory /*trajectory*/) +bool PidTrajectoryPlugin::computeGains( + const std::shared_ptr & /*trajectory*/) { if (param_listener_->is_old(params_)) { diff --git a/joint_trajectory_controller_plugins/test/test_pid_trajectory.cpp b/joint_trajectory_controller_plugins/test/test_pid_trajectory.cpp index 7ab113a263..0e66b687ec 100644 --- a/joint_trajectory_controller_plugins/test/test_pid_trajectory.cpp +++ b/joint_trajectory_controller_plugins/test/test_pid_trajectory.cpp @@ -39,7 +39,7 @@ TEST_F(PidTrajectoryTest, TestSingleJoint) traj_contr->trigger_declare_parameters(); node_->set_parameter(rclcpp::Parameter("gains.joint1.p", 1.0)); - ASSERT_TRUE(traj_contr->computeGains(trajectory_msgs::msg::JointTrajectory())); + ASSERT_TRUE(traj_contr->computeGains(std::make_shared())); trajectory_msgs::msg::JointTrajectoryPoint traj_msg; traj_msg.positions.push_back(0.0); @@ -71,7 +71,7 @@ TEST_F(PidTrajectoryTest, TestMultipleJoints) node_->set_parameter(rclcpp::Parameter("gains.joint2.p", 1.0)); node_->set_parameter(rclcpp::Parameter("gains.joint3.p", 1.0)); - ASSERT_TRUE(traj_contr->computeGains(trajectory_msgs::msg::JointTrajectory())); + ASSERT_TRUE(traj_contr->computeGains(std::make_shared())); trajectory_msgs::msg::JointTrajectoryPoint traj_msg; traj_msg.positions.push_back(0.0); From 7b01481654c6906a9fccd78e4358c65df4522f1f Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 6 Dec 2023 12:41:20 +0000 Subject: [PATCH 17/30] Split non-RT and RT methods --- .../src/joint_trajectory_controller.cpp | 22 ++++++++++-- .../test/test_trajectory_controller_utils.hpp | 1 + .../pid_trajectory_plugin.hpp | 11 ++++-- .../trajectory_controller_base.hpp | 19 +++++++++- .../src/pid_trajectory_plugin.cpp | 35 +++++++++++++------ .../test/test_pid_trajectory.cpp | 22 ++++++++---- 6 files changed, 86 insertions(+), 24 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index fa797e9644..153f80af7d 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -135,6 +135,18 @@ controller_interface::return_type JointTrajectoryController::update( } } + // update gains of controller + // TODO(christophfroehlich) activate this + // once https://github.com/ros-controls/ros2_controllers/pull/761 is merged + // if (use_closed_loop_control_law_) + // { + // if(traj_contr_->updateGainsRT() == false) + // { + // RCLCPP_ERROR(get_node()->get_logger(), "Could not update gains of controller"); + // return controller_interface::return_type::ERROR; + // } + // } + auto compute_error_for_joint = [&]( JointTrajectoryPoint & error, size_t index, const JointTrajectoryPoint & current, @@ -171,6 +183,7 @@ controller_interface::return_type JointTrajectoryController::update( auto current_external_msg = traj_external_point_ptr_->get_trajectory_msg(); auto new_external_msg = traj_msg_external_point_ptr_.readFromRT(); // Discard, if a goal is pending but still not active (somewhere stuck in goal_handle_timer_) + // TODO(christophfroehlich) wait until gains were computed by the trajectory controller if ( current_external_msg != *new_external_msg && (*(rt_has_pending_goal_.readFromRT()) && !active_goal) == false) @@ -705,7 +718,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( contains_interface_type(params_.command_interfaces, hardware_interface::HW_IF_EFFORT); // if there is only velocity or if there is effort command interface - // then use also closed loop controller + // then use also closed-loop controller use_closed_loop_control_law_ = (has_velocity_command_interface_ && params_.command_interfaces.size() == 1 && !params_.open_loop_control) || @@ -1461,10 +1474,13 @@ void JointTrajectoryController::add_new_trajectory_msg( { traj_msg_external_point_ptr_.writeFromNonRT(traj_msg); - // update gains of controller + // compute gains of controller if (use_closed_loop_control_law_) { - traj_contr_->computeGains(traj_msg); + if (traj_contr_->computeGainsNonRT(traj_msg) == false) + { + RCLCPP_ERROR(get_node()->get_logger(), "Failed to compute gains for trajectory."); + } } } diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 62c5a41429..bb0e05823a 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -232,6 +232,7 @@ class TrajectoryControllerTest : public ::testing::Test traj_controller_->trigger_declare_parameters(); auto node = traj_controller_->get_node(); + // TODO(christophfroehlich): use command joints instead of joint_names for (size_t i = 0; i < joint_names_.size(); ++i) { const std::string prefix = "gains." + joint_names_[i]; diff --git a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp index e1e0f55375..e40d436c1b 100644 --- a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp +++ b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp @@ -37,8 +37,10 @@ class PidTrajectoryPlugin : public TrajectoryControllerBase rclcpp_lifecycle::LifecycleNode::SharedPtr node, std::vector command_joint_names) override; - bool computeGains( - const std::shared_ptr & trajectory) override; + bool computeGainsNonRT( + const std::shared_ptr & /*trajectory*/) override; + + bool updateGainsRT() override; void computeCommands( std::vector & tmp_command, const trajectory_msgs::msg::JointTrajectoryPoint current, @@ -49,6 +51,11 @@ class PidTrajectoryPlugin : public TrajectoryControllerBase void reset() override; protected: + /** + * @brief parse PID gains from parameter struct + */ + void updateGains(); + // number of command joints size_t num_cmd_joints_; // name of the command joints diff --git a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp index 8f0750bf81..e06c723f7b 100644 --- a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp +++ b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp @@ -39,6 +39,8 @@ class TrajectoryControllerBase virtual ~TrajectoryControllerBase() = default; /** + * @brief initialize the controller plugin. + * parse read-only parameters and do pre-allocate memory for the controller */ JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC virtual bool initialize( @@ -46,12 +48,26 @@ class TrajectoryControllerBase std::vector command_joint_names) = 0; /** + * @brief compute the gains from the given trajectory from a non-RT thread + * @return true if the gains were computed, false otherwise */ JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC - virtual bool computeGains( + virtual bool computeGainsNonRT( const std::shared_ptr & trajectory) = 0; /** + * @brief update the gains from the given trajectory from a RT thread + * + * this method must finish quickly (within one controller-update rate, + * and should not allocate memory + * + * @return true if the gains were updated, false otherwise + */ + JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC + virtual bool updateGainsRT(void) = 0; + + /** + * @brief compute the commands with the calculated gains */ JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC virtual void computeCommands( @@ -61,6 +77,7 @@ class TrajectoryControllerBase const rclcpp::Duration & period) = 0; /** + * @brief reset internal states */ JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC virtual void reset() = 0; diff --git a/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp b/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp index f0c3db75f1..85b2377a37 100644 --- a/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp +++ b/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp @@ -45,37 +45,50 @@ bool PidTrajectoryPlugin::initialize( pids_.resize(num_cmd_joints_); ff_velocity_scale_.resize(num_cmd_joints_); - RCLCPP_INFO( - node_->get_logger(), "[PidTrajectoryPlugin] Initialized with %lu joints.", num_cmd_joints_); return true; } -bool PidTrajectoryPlugin::computeGains( - const std::shared_ptr & /*trajectory*/) +bool PidTrajectoryPlugin::updateGainsRT() { if (param_listener_->is_old(params_)) { params_ = param_listener_->get_params(); - RCLCPP_DEBUG(node_->get_logger(), "[PidTrajectoryPlugin] Updated parameters"); + updateGains(); } - pids_.resize(num_cmd_joints_); - ff_velocity_scale_.resize(num_cmd_joints_); + return true; +} - // Init PID gains from ROS parameters +bool PidTrajectoryPlugin::computeGainsNonRT( + const std::shared_ptr & /*trajectory*/) +{ + params_ = param_listener_->get_params(); + updateGains(); + return true; +}; + +void PidTrajectoryPlugin::updateGains() +{ for (size_t i = 0; i < num_cmd_joints_; ++i) { + RCLCPP_DEBUG( + node_->get_logger(), "[PidTrajectoryPlugin] command_joint_names_ %lu : %s", i, + command_joint_names_[i].c_str()); + const auto & gains = params_.gains.joints_map.at(command_joint_names_[i]); pids_[i] = std::make_shared( gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp); - ff_velocity_scale_[i] = gains.ff_velocity_scale; + + RCLCPP_DEBUG(node_->get_logger(), "[PidTrajectoryPlugin] gains.p: %f", gains.p); + RCLCPP_DEBUG( + node_->get_logger(), "[PidTrajectoryPlugin] ff_velocity_scale_: %f", ff_velocity_scale_[i]); } RCLCPP_INFO( node_->get_logger(), - "[PidTrajectoryPlugin] Loaded PID gains from ROS parameters for %lu joints.", num_cmd_joints_); - return true; + "[PidTrajectoryPlugin] Loaded PID gains from ROS parameters for %lu joint(s).", + num_cmd_joints_); } void PidTrajectoryPlugin::computeCommands( diff --git a/joint_trajectory_controller_plugins/test/test_pid_trajectory.cpp b/joint_trajectory_controller_plugins/test/test_pid_trajectory.cpp index 0e66b687ec..200613eadd 100644 --- a/joint_trajectory_controller_plugins/test/test_pid_trajectory.cpp +++ b/joint_trajectory_controller_plugins/test/test_pid_trajectory.cpp @@ -39,7 +39,9 @@ TEST_F(PidTrajectoryTest, TestSingleJoint) traj_contr->trigger_declare_parameters(); node_->set_parameter(rclcpp::Parameter("gains.joint1.p", 1.0)); - ASSERT_TRUE(traj_contr->computeGains(std::make_shared())); + ASSERT_TRUE( + traj_contr->computeGainsNonRT(std::make_shared())); + ASSERT_TRUE(traj_contr->updateGainsRT()); trajectory_msgs::msg::JointTrajectoryPoint traj_msg; traj_msg.positions.push_back(0.0); @@ -68,15 +70,17 @@ TEST_F(PidTrajectoryTest, TestMultipleJoints) // set dynamic parameters traj_contr->trigger_declare_parameters(); node_->set_parameter(rclcpp::Parameter("gains.joint1.p", 1.0)); - node_->set_parameter(rclcpp::Parameter("gains.joint2.p", 1.0)); - node_->set_parameter(rclcpp::Parameter("gains.joint3.p", 1.0)); + node_->set_parameter(rclcpp::Parameter("gains.joint2.p", 2.0)); + node_->set_parameter(rclcpp::Parameter("gains.joint3.p", 3.0)); - ASSERT_TRUE(traj_contr->computeGains(std::make_shared())); + ASSERT_TRUE( + traj_contr->computeGainsNonRT(std::make_shared())); + ASSERT_TRUE(traj_contr->updateGainsRT()); trajectory_msgs::msg::JointTrajectoryPoint traj_msg; - traj_msg.positions.push_back(0.0); - traj_msg.positions.push_back(0.0); - traj_msg.positions.push_back(0.0); + traj_msg.positions.push_back(1.0); + traj_msg.positions.push_back(1.0); + traj_msg.positions.push_back(1.0); traj_msg.velocities.push_back(0.0); traj_msg.velocities.push_back(0.0); traj_msg.velocities.push_back(0.0); @@ -86,6 +90,10 @@ TEST_F(PidTrajectoryTest, TestMultipleJoints) ASSERT_NO_THROW( traj_contr->computeCommands(tmp_command, traj_msg, traj_msg, traj_msg, time, period)); + + EXPECT_EQ(tmp_command[0], 1.0); + EXPECT_EQ(tmp_command[1], 2.0); + EXPECT_EQ(tmp_command[2], 3.0); } int main(int argc, char ** argv) From 9c2a3c4384d22f9f8aebb4589c2cb21424e71d19 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 6 Dec 2023 12:41:20 +0000 Subject: [PATCH 18/30] Use command joints instead of joints for gains structure --- .../joint_trajectory_controller.hpp | 2 ++ .../src/joint_trajectory_controller.cpp | 30 +++++++++++++++++++ ...oint_trajectory_controller_parameters.yaml | 2 +- .../test/test_trajectory_controller_utils.hpp | 21 ++++--------- .../src/pid_trajectory_plugin.cpp | 2 +- .../src/pid_trajectory_plugin_parameters.yaml | 6 ++-- .../test/test_pid_trajectory.cpp | 4 +-- 7 files changed, 44 insertions(+), 23 deletions(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index b0ab1f1f34..ff0233ffca 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -134,6 +134,8 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa // Storing command joint names for interfaces std::vector command_joint_names_; + // TODO(anyone) remove this if there is another way to lock command_joints parameter + rclcpp_lifecycle::LifecycleNode::PreSetParametersCallbackHandle::SharedPtr lock_cmd_joint_names; // Parameters from ROS for joint_trajectory_controller std::shared_ptr param_listener_; diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 153f80af7d..509ed8672c 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -690,6 +690,36 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( command_joint_names_ = params_.joints; RCLCPP_INFO( logger, "No specific joint names are used for command interfaces. Using 'joints' parameter."); + + // set the parameter for the controller plugin + auto result = + get_node()->set_parameter(rclcpp::Parameter("command_joints", command_joint_names_)); + if (result.successful == false) + { + RCLCPP_ERROR(logger, "Failed to set 'command_joints' parameter"); + return CallbackReturn::FAILURE; + } + // TODO(christophfroehlich) how to lock the parameter (set read_only to false)? + // Setting it to read_only but override is not supported + // https://github.com/ros2/rclcpp/issues/1762 get_node()->undeclare_parameter("command_joints"); + // rcl_interfaces::msg::ParameterDescriptor parameter_descriptor; + // parameter_descriptor.read_only = true; + // get_node()->declare_parameter("command_joints", + // rclcpp::ParameterValue(command_joint_names_), parameter_descriptor); + lock_cmd_joint_names = get_node()->add_pre_set_parameters_callback( + [this](std::vector & parameters) + { + for (auto & parameter : parameters) + { + if (parameter.get_name() == "command_joints") + { + RCLCPP_ERROR( + get_node()->get_logger(), + "The parameter 'command_joints' is read-only. You can't change it."); + parameter = rclcpp::Parameter("command_joints", command_joint_names_); + } + } + }); } else if (command_joint_names_.size() != params_.joints.size()) { diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index b5e20162f4..ba59083ad8 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -12,7 +12,7 @@ joint_trajectory_controller: type: string_array, default_value: [], description: "Names of the commanding joints used by the controller", - read_only: true, + read_only: false, # should be set to true after configuration of the controller validation: { unique<>: null, } diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index bb0e05823a..b65eb99413 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -86,32 +86,18 @@ class TestableJointTrajectoryController return success; } - void set_joint_names(const std::vector & joint_names) - { - params_.joints = joint_names; - } - void set_command_joint_names(const std::vector & command_joint_names) { command_joint_names_ = command_joint_names; } - void set_command_interfaces(const std::vector & command_interfaces) - { - params_.command_interfaces = command_interfaces; - } - - void set_state_interfaces(const std::vector & state_interfaces) - { - params_.state_interfaces = state_interfaces; - } - void trigger_declare_parameters() { param_listener_->declare_params(); } trajectory_msgs::msg::JointTrajectoryPoint get_current_state_when_offset() const { return last_commanded_state_; } + bool has_position_state_interface() const { return has_position_state_interface_; } bool has_velocity_state_interface() const { return has_velocity_state_interface_; } @@ -227,12 +213,15 @@ class TrajectoryControllerTest : public ::testing::Test } } + /** + * @brief set PIDs for every entry in joint_names_ + * be aware to update if PIDs should be configured for different command_joints than joint_names + */ void SetPidParameters(double p_default = 0.0, double ff_default = 1.0) { traj_controller_->trigger_declare_parameters(); auto node = traj_controller_->get_node(); - // TODO(christophfroehlich): use command joints instead of joint_names for (size_t i = 0; i < joint_names_.size(); ++i) { const std::string prefix = "gains." + joint_names_[i]; diff --git a/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp b/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp index 85b2377a37..4a133e1378 100644 --- a/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp +++ b/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp @@ -75,7 +75,7 @@ void PidTrajectoryPlugin::updateGains() node_->get_logger(), "[PidTrajectoryPlugin] command_joint_names_ %lu : %s", i, command_joint_names_[i].c_str()); - const auto & gains = params_.gains.joints_map.at(command_joint_names_[i]); + const auto & gains = params_.gains.command_joints_map.at(command_joint_names_[i]); pids_[i] = std::make_shared( gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp); ff_velocity_scale_[i] = gains.ff_velocity_scale; diff --git a/joint_trajectory_controller_plugins/src/pid_trajectory_plugin_parameters.yaml b/joint_trajectory_controller_plugins/src/pid_trajectory_plugin_parameters.yaml index df9ccc0741..f934fc9bdf 100644 --- a/joint_trajectory_controller_plugins/src/pid_trajectory_plugin_parameters.yaml +++ b/joint_trajectory_controller_plugins/src/pid_trajectory_plugin_parameters.yaml @@ -1,15 +1,15 @@ joint_trajectory_controller_plugins: - joints: { + command_joints: { type: string_array, default_value: [], - description: "Names of joints used by the controller", + description: "Names of joints used by the controller plugin", read_only: true, validation: { size_gt<>: [0], } } gains: - __map_joints: + __map_command_joints: p: { type: double, default_value: 0.0, diff --git a/joint_trajectory_controller_plugins/test/test_pid_trajectory.cpp b/joint_trajectory_controller_plugins/test/test_pid_trajectory.cpp index 200613eadd..3e8ad58959 100644 --- a/joint_trajectory_controller_plugins/test/test_pid_trajectory.cpp +++ b/joint_trajectory_controller_plugins/test/test_pid_trajectory.cpp @@ -31,7 +31,7 @@ TEST_F(PidTrajectoryTest, TestSingleJoint) auto joint_names_paramv = rclcpp::ParameterValue(joint_names); // override read_only parameter - node_->declare_parameter("joints", joint_names_paramv); + node_->declare_parameter("command_joints", joint_names_paramv); ASSERT_TRUE(traj_contr->initialize(node_, joint_names)); @@ -63,7 +63,7 @@ TEST_F(PidTrajectoryTest, TestMultipleJoints) auto joint_names_paramv = rclcpp::ParameterValue(joint_names); // override read_only parameter - node_->declare_parameter("joints", joint_names_paramv); + node_->declare_parameter("command_joints", joint_names_paramv); ASSERT_TRUE(traj_contr->initialize(node_, joint_names)); From 0da48833f820278e8cadd474398d10826bcebcc8 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 6 Dec 2023 12:45:35 +0000 Subject: [PATCH 19/30] Add RT buffer to base class --- .../src/joint_trajectory_controller.cpp | 59 ++++++++++-------- .../pid_trajectory_plugin.hpp | 12 ++-- .../trajectory_controller_base.hpp | 61 ++++++++++++++++--- .../src/pid_trajectory_plugin.cpp | 42 +++++++++---- .../test/test_pid_trajectory.cpp | 27 ++++---- 5 files changed, 136 insertions(+), 65 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 509ed8672c..6d266c5327 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -127,26 +127,19 @@ controller_interface::return_type JointTrajectoryController::update( { params_ = param_listener_->get_params(); default_tolerances_ = get_segment_tolerances(params_); - // update the PID gains - // variable use_closed_loop_pid_adapter_ is updated in on_configure only - if (use_closed_loop_pid_adapter_) + + // update gains of controller + // variable use_closed_loop_control_law_ is updated in on_configure only + if (use_closed_loop_control_law_) { - update_pids(); + if (traj_contr_->updateGainsRT() == false) + { + RCLCPP_ERROR(get_node()->get_logger(), "Could not update gains of controller"); + return controller_interface::return_type::ERROR; + } } } - // update gains of controller - // TODO(christophfroehlich) activate this - // once https://github.com/ros-controls/ros2_controllers/pull/761 is merged - // if (use_closed_loop_control_law_) - // { - // if(traj_contr_->updateGainsRT() == false) - // { - // RCLCPP_ERROR(get_node()->get_logger(), "Could not update gains of controller"); - // return controller_interface::return_type::ERROR; - // } - // } - auto compute_error_for_joint = [&]( JointTrajectoryPoint & error, size_t index, const JointTrajectoryPoint & current, @@ -182,11 +175,13 @@ controller_interface::return_type JointTrajectoryController::update( // Check if a new external message has been received from nonRT threads auto current_external_msg = traj_external_point_ptr_->get_trajectory_msg(); auto new_external_msg = traj_msg_external_point_ptr_.readFromRT(); - // Discard, if a goal is pending but still not active (somewhere stuck in goal_handle_timer_) - // TODO(christophfroehlich) wait until gains were computed by the trajectory controller + // Discard, + // if a goal is pending but still not active (somewhere stuck in goal_handle_timer_) + // and if use_closed_loop_control_law_: wait until control law is computed by the traj_contr_ if ( current_external_msg != *new_external_msg && - (*(rt_has_pending_goal_.readFromRT()) && !active_goal) == false) + (*(rt_has_pending_goal_.readFromRT()) && !active_goal) == false && + (use_closed_loop_control_law_ == false || traj_contr_->is_valid())) { fill_partial_goal(*new_external_msg); sort_to_local_joint_order(*new_external_msg); @@ -768,7 +763,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( params_.controller_plugin.c_str(), ex.what()); return CallbackReturn::FAILURE; } - if (traj_contr_->initialize(get_node(), command_joint_names_) == false) + if (traj_contr_->initialize(get_node()) == false) { RCLCPP_FATAL( logger, @@ -778,9 +773,20 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( } else { - RCLCPP_INFO( - logger, "The trajectory controller plugin `%s` was loaded.", - params_.controller_plugin.c_str()); + if (traj_contr_->configure() == false) + { + RCLCPP_FATAL( + logger, + "The trajectory controller plugin `%s` failed to initialize for some reason. Aborting.", + params_.controller_plugin.c_str()); + return CallbackReturn::FAILURE; + } + else + { + RCLCPP_INFO( + logger, "The trajectory controller plugin `%s` was loaded and configured.", + params_.controller_plugin.c_str()); + } } tmp_command_.resize(dof_, 0.0); @@ -1037,6 +1043,11 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( cmd_timeout_ = 0.0; } + if (use_closed_loop_control_law_) + { + traj_contr_->activate(); + } + return CallbackReturn::SUCCESS; } @@ -1507,7 +1518,7 @@ void JointTrajectoryController::add_new_trajectory_msg( // compute gains of controller if (use_closed_loop_control_law_) { - if (traj_contr_->computeGainsNonRT(traj_msg) == false) + if (traj_contr_->computeControlLawNonRT(traj_msg) == false) { RCLCPP_ERROR(get_node()->get_logger(), "Failed to compute gains for trajectory."); } diff --git a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp index e40d436c1b..c2152b7930 100644 --- a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp +++ b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp @@ -33,11 +33,13 @@ namespace joint_trajectory_controller_plugins class PidTrajectoryPlugin : public TrajectoryControllerBase { public: - bool initialize( - rclcpp_lifecycle::LifecycleNode::SharedPtr node, - std::vector command_joint_names) override; + bool initialize(rclcpp_lifecycle::LifecycleNode::SharedPtr node) override; - bool computeGainsNonRT( + bool configure() override; + + bool activate() override; + + bool computeControlLaw( const std::shared_ptr & /*trajectory*/) override; bool updateGainsRT() override; @@ -58,8 +60,6 @@ class PidTrajectoryPlugin : public TrajectoryControllerBase // number of command joints size_t num_cmd_joints_; - // name of the command joints - std::vector command_joint_names_; // PID controllers std::vector pids_; // Feed-forward velocity weight factor when calculating closed loop pid adapter's command diff --git a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp index e06c723f7b..3838566a3d 100644 --- a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp +++ b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp @@ -22,6 +22,7 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "realtime_tools/realtime_buffer.h" #include "trajectory_msgs/msg/joint_trajectory.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" @@ -40,26 +41,50 @@ class TrajectoryControllerBase /** * @brief initialize the controller plugin. - * parse read-only parameters and do pre-allocate memory for the controller + * declare parameters */ JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC - virtual bool initialize( - rclcpp_lifecycle::LifecycleNode::SharedPtr node, - std::vector command_joint_names) = 0; + virtual bool initialize(rclcpp_lifecycle::LifecycleNode::SharedPtr node) = 0; /** - * @brief compute the gains from the given trajectory from a non-RT thread + * @brief configure the controller plugin. + * parse read-only parameters, pre-allocate memory for the controller + */ + JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC + virtual bool configure() = 0; + + /** + * @brief activate the controller plugin. + * parse parameters + */ + JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC + virtual bool activate() = 0; + + /** + * @brief compute the control law for the given trajectory + * + * this method can take more time to compute the control law. Hence, it will block the execution + * of the trajectory until it finishes + * + * this method is not virtual, any overrides won't be called by JTC. Instead, override + * computeControlLaw for your implementation + * * @return true if the gains were computed, false otherwise */ JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC - virtual bool computeGainsNonRT( - const std::shared_ptr & trajectory) = 0; + bool computeControlLawNonRT( + const std::shared_ptr & trajectory) + { + rt_controller_valid_.writeFromNonRT(false); + auto ret = computeControlLaw(trajectory); + rt_controller_valid_.writeFromNonRT(true); + return ret; + } /** - * @brief update the gains from the given trajectory from a RT thread + * @brief update the gains from a RT thread * - * this method must finish quickly (within one controller-update rate, - * and should not allocate memory + * this method must finish quickly (within one controller-update rate) * * @return true if the gains were updated, false otherwise */ @@ -82,9 +107,25 @@ class TrajectoryControllerBase JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC virtual void reset() = 0; + /** + * @return true if the control law is valid (updated with the trajectory) + */ + JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC + bool is_valid() { return rt_controller_valid_.readFromRT(); } + protected: // the node handle for parameter handling rclcpp_lifecycle::LifecycleNode::SharedPtr node_; + // Are we computing the control law or is it valid? + realtime_tools::RealtimeBuffer rt_controller_valid_; + + /** + * @brief compute the control law from the given trajectory + * @return true if the gains were computed, false otherwise + */ + JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC + virtual bool computeControlLaw( + const std::shared_ptr & trajectory) = 0; }; } // namespace joint_trajectory_controller_plugins diff --git a/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp b/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp index 4a133e1378..098a718b8b 100644 --- a/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp +++ b/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp @@ -17,17 +17,14 @@ namespace joint_trajectory_controller_plugins { -bool PidTrajectoryPlugin::initialize( - rclcpp_lifecycle::LifecycleNode::SharedPtr node, std::vector command_joint_names) +bool PidTrajectoryPlugin::initialize(rclcpp_lifecycle::LifecycleNode::SharedPtr node) { node_ = node; - command_joint_names_ = command_joint_names; try { // Create the parameter listener and get the parameters param_listener_ = std::make_shared(node_); - params_ = param_listener_->get_params(); } catch (const std::exception & e) { @@ -35,8 +32,23 @@ bool PidTrajectoryPlugin::initialize( return false; } + return true; +} + +bool PidTrajectoryPlugin::configure() +{ + try + { + params_ = param_listener_->get_params(); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during configure stage with message: %s \n", e.what()); + return false; + } + // parse read-only params - num_cmd_joints_ = command_joint_names_.size(); + num_cmd_joints_ = params_.command_joints.size(); if (num_cmd_joints_ == 0) { RCLCPP_ERROR(node_->get_logger(), "[PidTrajectoryPlugin] No command joints specified."); @@ -46,7 +58,14 @@ bool PidTrajectoryPlugin::initialize( ff_velocity_scale_.resize(num_cmd_joints_); return true; -} +}; + +bool PidTrajectoryPlugin::activate() +{ + params_ = param_listener_->get_params(); + updateGains(); + return true; +}; bool PidTrajectoryPlugin::updateGainsRT() { @@ -59,11 +78,10 @@ bool PidTrajectoryPlugin::updateGainsRT() return true; } -bool PidTrajectoryPlugin::computeGainsNonRT( +bool PidTrajectoryPlugin::computeControlLaw( const std::shared_ptr & /*trajectory*/) { - params_ = param_listener_->get_params(); - updateGains(); + // nothing to do return true; }; @@ -72,10 +90,10 @@ void PidTrajectoryPlugin::updateGains() for (size_t i = 0; i < num_cmd_joints_; ++i) { RCLCPP_DEBUG( - node_->get_logger(), "[PidTrajectoryPlugin] command_joint_names_ %lu : %s", i, - command_joint_names_[i].c_str()); + node_->get_logger(), "[PidTrajectoryPlugin] params_.command_joints %lu : %s", i, + params_.command_joints[i].c_str()); - const auto & gains = params_.gains.command_joints_map.at(command_joint_names_[i]); + const auto & gains = params_.gains.command_joints_map.at(params_.command_joints[i]); pids_[i] = std::make_shared( gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp); ff_velocity_scale_[i] = gains.ff_velocity_scale; diff --git a/joint_trajectory_controller_plugins/test/test_pid_trajectory.cpp b/joint_trajectory_controller_plugins/test/test_pid_trajectory.cpp index 3e8ad58959..ba3314f901 100644 --- a/joint_trajectory_controller_plugins/test/test_pid_trajectory.cpp +++ b/joint_trajectory_controller_plugins/test/test_pid_trajectory.cpp @@ -19,7 +19,7 @@ TEST_F(PidTrajectoryTest, TestEmptySetup) std::shared_ptr traj_contr = std::make_shared(); - ASSERT_FALSE(traj_contr->initialize(node_, std::vector())); + ASSERT_FALSE(traj_contr->initialize(node_)); } TEST_F(PidTrajectoryTest, TestSingleJoint) @@ -30,17 +30,15 @@ TEST_F(PidTrajectoryTest, TestSingleJoint) std::vector joint_names = {"joint1"}; auto joint_names_paramv = rclcpp::ParameterValue(joint_names); - // override read_only parameter + // override parameter node_->declare_parameter("command_joints", joint_names_paramv); - ASSERT_TRUE(traj_contr->initialize(node_, joint_names)); - - // set dynamic parameters - traj_contr->trigger_declare_parameters(); + ASSERT_TRUE(traj_contr->initialize(node_)); node_->set_parameter(rclcpp::Parameter("gains.joint1.p", 1.0)); - + ASSERT_TRUE(traj_contr->configure()); + ASSERT_TRUE(traj_contr->activate()); ASSERT_TRUE( - traj_contr->computeGainsNonRT(std::make_shared())); + traj_contr->computeControlLawNonRT(std::make_shared())); ASSERT_TRUE(traj_contr->updateGainsRT()); trajectory_msgs::msg::JointTrajectoryPoint traj_msg; @@ -62,19 +60,22 @@ TEST_F(PidTrajectoryTest, TestMultipleJoints) std::vector joint_names = {"joint1", "joint2", "joint3"}; auto joint_names_paramv = rclcpp::ParameterValue(joint_names); - // override read_only parameter + // override parameter node_->declare_parameter("command_joints", joint_names_paramv); - ASSERT_TRUE(traj_contr->initialize(node_, joint_names)); - + ASSERT_TRUE(traj_contr->initialize(node_)); // set dynamic parameters - traj_contr->trigger_declare_parameters(); node_->set_parameter(rclcpp::Parameter("gains.joint1.p", 1.0)); node_->set_parameter(rclcpp::Parameter("gains.joint2.p", 2.0)); node_->set_parameter(rclcpp::Parameter("gains.joint3.p", 3.0)); + ASSERT_TRUE(traj_contr->configure()); + ASSERT_TRUE(traj_contr->activate()); + ASSERT_TRUE( + traj_contr->computeControlLawNonRT(std::make_shared())); + ASSERT_TRUE(traj_contr->updateGainsRT()); ASSERT_TRUE( - traj_contr->computeGainsNonRT(std::make_shared())); + traj_contr->computeControlLawNonRT(std::make_shared())); ASSERT_TRUE(traj_contr->updateGainsRT()); trajectory_msgs::msg::JointTrajectoryPoint traj_msg; From b2adbcd1088c34afc43e0d3ef3944d41863841ba Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 6 Dec 2023 12:47:12 +0000 Subject: [PATCH 20/30] Add trajectory start time --- .../src/joint_trajectory_controller.cpp | 15 ++++++--- .../trajectory_controller_base.hpp | 32 ++++++++++++++++--- 2 files changed, 37 insertions(+), 10 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 6d266c5327..5e955bd481 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -177,11 +177,11 @@ controller_interface::return_type JointTrajectoryController::update( auto new_external_msg = traj_msg_external_point_ptr_.readFromRT(); // Discard, // if a goal is pending but still not active (somewhere stuck in goal_handle_timer_) - // and if use_closed_loop_control_law_: wait until control law is computed by the traj_contr_ + // and if traj_contr_: wait until control law is computed by the traj_contr_ if ( current_external_msg != *new_external_msg && (*(rt_has_pending_goal_.readFromRT()) && !active_goal) == false && - (use_closed_loop_control_law_ == false || traj_contr_->is_valid())) + (traj_contr_ == nullptr || traj_contr_->is_ready())) { fill_partial_goal(*new_external_msg); sort_to_local_joint_order(*new_external_msg); @@ -220,6 +220,11 @@ controller_interface::return_type JointTrajectoryController::update( { traj_external_point_ptr_->set_point_before_trajectory_msg(time, state_current_); } + if (traj_contr_) + { + // set start time of trajectory to traj_contr_ + traj_contr_->start(time, traj_external_point_ptr_->time_from_start()); + } } // find segment for current timestamp @@ -292,7 +297,7 @@ controller_interface::return_type JointTrajectoryController::update( // set values for next hardware write() if tolerance is met if (!tolerance_violated_while_moving && within_goal_time) { - if (use_closed_loop_control_law_) + if (traj_contr_) { traj_contr_->computeCommands( tmp_command_, state_current_, state_error_, state_desired_, time, period); @@ -1043,7 +1048,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( cmd_timeout_ = 0.0; } - if (use_closed_loop_control_law_) + if (traj_contr_) { traj_contr_->activate(); } @@ -1516,7 +1521,7 @@ void JointTrajectoryController::add_new_trajectory_msg( traj_msg_external_point_ptr_.writeFromNonRT(traj_msg); // compute gains of controller - if (use_closed_loop_control_law_) + if (traj_contr_) { if (traj_contr_->computeControlLawNonRT(traj_msg) == false) { diff --git a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp index 3838566a3d..015170d6e4 100644 --- a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp +++ b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp @@ -75,12 +75,30 @@ class TrajectoryControllerBase bool computeControlLawNonRT( const std::shared_ptr & trajectory) { - rt_controller_valid_.writeFromNonRT(false); + rt_control_law_ready_.writeFromNonRT(false); auto ret = computeControlLaw(trajectory); - rt_controller_valid_.writeFromNonRT(true); + rt_control_law_ready_.writeFromNonRT(true); return ret; } + /** + * @brief set the time when the current trajectory started + * + * (same logic as trajectory class) + */ + void start(const rclcpp::Time time, const rclcpp::Time trajectory_start_time) + { + if (trajectory_start_time.seconds() == 0.0) + { + trajectory_start_time_ = time; + } + else + { + trajectory_start_time_ = trajectory_start_time; + } + start_trajectory_ = true; + } + /** * @brief update the gains from a RT thread * @@ -108,16 +126,20 @@ class TrajectoryControllerBase virtual void reset() = 0; /** - * @return true if the control law is valid (updated with the trajectory) + * @return true if the control law is ready (updated with the trajectory) */ JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC - bool is_valid() { return rt_controller_valid_.readFromRT(); } + bool is_ready() { return rt_control_law_ready_.readFromRT(); } protected: // the node handle for parameter handling rclcpp_lifecycle::LifecycleNode::SharedPtr node_; // Are we computing the control law or is it valid? - realtime_tools::RealtimeBuffer rt_controller_valid_; + realtime_tools::RealtimeBuffer rt_control_law_ready_; + // time when the current trajectory started, can be used to interpolate time-varying gains + rclcpp::Time trajectory_start_time_; + // use this variable to activate new gains from the non-RT thread + bool start_trajectory_ = false; /** * @brief compute the control law from the given trajectory From 441b556809b6b7ca42f297a959f09abeb3bdd4af Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 6 Dec 2023 12:50:43 +0000 Subject: [PATCH 21/30] Rename variable --- .../joint_trajectory_controller.hpp | 2 +- .../src/joint_trajectory_controller.cpp | 12 ++++---- .../test/test_trajectory_controller.cpp | 29 +++++++++---------- .../test/test_trajectory_controller_utils.hpp | 6 ++-- 4 files changed, 24 insertions(+), 25 deletions(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index ff0233ffca..edc0e649d6 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -164,7 +164,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa bool has_effort_command_interface_ = false; /// If true, a velocity feedforward term plus corrective PID term is used - bool use_closed_loop_control_law_ = false; + bool use_external_control_law_ = false; // class loader for actual trajectory controller std::shared_ptr< pluginlib::ClassLoader> diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 5e955bd481..10ad47d50f 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -130,7 +130,7 @@ controller_interface::return_type JointTrajectoryController::update( // update gains of controller // variable use_closed_loop_control_law_ is updated in on_configure only - if (use_closed_loop_control_law_) + if (use_external_control_law_) { if (traj_contr_->updateGainsRT() == false) { @@ -310,7 +310,7 @@ controller_interface::return_type JointTrajectoryController::update( } if (has_velocity_command_interface_) { - if (use_closed_loop_control_law_) + if (use_external_control_law_) { assign_interface_from_point(joint_command_interface_[1], tmp_command_); } @@ -748,13 +748,13 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( contains_interface_type(params_.command_interfaces, hardware_interface::HW_IF_EFFORT); // if there is only velocity or if there is effort command interface - // then use also closed-loop controller - use_closed_loop_control_law_ = + // then use external control law + use_external_control_law_ = (has_velocity_command_interface_ && params_.command_interfaces.size() == 1 && !params_.open_loop_control) || has_effort_command_interface_; - if (use_closed_loop_control_law_) + if (use_external_control_law_) { try { @@ -782,7 +782,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( { RCLCPP_FATAL( logger, - "The trajectory controller plugin `%s` failed to initialize for some reason. Aborting.", + "The trajectory controller plugin `%s` failed to configure for some reason. Aborting.", params_.controller_plugin.c_str()); return CallbackReturn::FAILURE; } diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index e725a4b1b6..cb52872c3d 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -367,7 +367,7 @@ TEST_P(TrajectoryControllerTestParameterized, update_dynamic_parameters) updateControllerAsync(); auto pids = traj_controller_->get_pids(); - if (traj_controller_->use_closed_loop_pid_adapter()) + if (traj_controller_->use_external_control_law()) { EXPECT_EQ(pids.size(), 3); auto gain_0 = pids.at(0)->getGains(); @@ -525,8 +525,8 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun if (traj_controller_->has_velocity_command_interface()) { - // use_closed_loop_pid_adapter_ - if (traj_controller_->use_closed_loop_pid_adapter()) + // use_external_control_law_ + if (traj_controller_->use_external_control_law()) { // we expect u = k_p * (s_d-s) for positions EXPECT_NEAR( @@ -551,8 +551,7 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun if (traj_controller_->has_effort_command_interface()) { - // use_closed_loop_pid_adapter_ - if (traj_controller_->use_closed_loop_pid_adapter()) + if (traj_controller_->use_external_control_law()) { // we expect u = k_p * (s_d-s) for positions EXPECT_NEAR( @@ -638,8 +637,8 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound) if (traj_controller_->has_velocity_command_interface()) { - // use_closed_loop_pid_adapter_ - if (traj_controller_->use_closed_loop_pid_adapter()) + // use_external_control_law_ + if (traj_controller_->use_external_control_law()) { // we expect u = k_p * (s_d-s) for positions[0] and positions[1] EXPECT_NEAR( @@ -666,8 +665,8 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound) if (traj_controller_->has_effort_command_interface()) { - // use_closed_loop_pid_adapter_ - if (traj_controller_->use_closed_loop_pid_adapter()) + // use_external_control_law_ + if (traj_controller_->use_external_control_law()) { // we expect u = k_p * (s_d-s) for positions[0] and positions[1] EXPECT_NEAR( @@ -831,9 +830,9 @@ TEST_P(TrajectoryControllerTestParameterized, timeout) } /** - * @brief check if use_closed_loop_pid is active + * @brief check if use_external_control_law is set */ -TEST_P(TrajectoryControllerTestParameterized, use_closed_loop_pid) +TEST_P(TrajectoryControllerTestParameterized, set_external_control_law) { rclcpp::executors::MultiThreadedExecutor executor; @@ -847,7 +846,7 @@ TEST_P(TrajectoryControllerTestParameterized, use_closed_loop_pid) !traj_controller_->is_open_loop()) || traj_controller_->has_effort_command_interface()) { - EXPECT_TRUE(traj_controller_->use_closed_loop_control_law()); + EXPECT_TRUE(traj_controller_->use_external_control_law()); } } @@ -904,7 +903,7 @@ TEST_P(TrajectoryControllerTestParameterized, velocity_error) } // is the velocity error correct? if ( - traj_controller_->use_closed_loop_control_law() // always needed for PID controller + traj_controller_->use_external_control_law() // always needed for PID controller || (traj_controller_->has_velocity_state_interface() && traj_controller_->has_velocity_command_interface())) { @@ -969,8 +968,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order) if (traj_controller_->has_velocity_command_interface()) { - // if use_closed_loop_control_law_==false: we expect desired velocities from direct sampling - // if use_closed_loop_control_law_==true: we expect desired velocities, because we use PID with + // if use_external_control_law_==false: we expect desired velocities from direct sampling + // if use_external_control_law_==true: we expect desired velocities, because we use PID with // feedforward term only EXPECT_GT(0.0, joint_vel_[0]); EXPECT_GT(0.0, joint_vel_[1]); diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index b65eb99413..883f81eadf 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -112,7 +112,7 @@ class TestableJointTrajectoryController bool has_effort_command_interface() const { return has_effort_command_interface_; } - bool use_closed_loop_control_law() const { return use_closed_loop_control_law_; } + bool use_external_control_law() const { return use_external_control_law_; } bool is_open_loop() const { return params_.open_loop_control; } @@ -265,7 +265,7 @@ class TrajectoryControllerTest : public ::testing::Test // set pid parameters before activate. The PID plugin has to be loaded already, otherwise // parameters are not declared yet - if (traj_controller_->use_closed_loop_control_law()) + if (traj_controller_->use_external_control_law()) { SetPidParameters(k_p, ff); } @@ -535,7 +535,7 @@ class TrajectoryControllerTest : public ::testing::Test // i.e., active but trivial trajectory (one point only) EXPECT_TRUE(traj_controller_->has_trivial_traj()); - if (traj_controller_->use_closed_loop_control_law() == false) + if (traj_controller_->use_external_control_law() == false) { if (traj_controller_->has_position_command_interface()) { From 408357c1f5547e11e0d55ecc84791143b592f165 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 6 Dec 2023 12:50:43 +0000 Subject: [PATCH 22/30] Remove local vars and just call compute-commands with duration in trajectory --- .../src/joint_trajectory_controller.cpp | 5 +-- .../pid_trajectory_plugin.hpp | 15 ++++++-- .../trajectory_controller_base.hpp | 35 ++++++++----------- .../src/pid_trajectory_plugin.cpp | 11 ++---- .../test/test_pid_trajectory.cpp | 12 +++---- 5 files changed, 37 insertions(+), 41 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 10ad47d50f..15c4531000 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -223,7 +223,7 @@ controller_interface::return_type JointTrajectoryController::update( if (traj_contr_) { // set start time of trajectory to traj_contr_ - traj_contr_->start(time, traj_external_point_ptr_->time_from_start()); + traj_contr_->start(); } } @@ -300,7 +300,8 @@ controller_interface::return_type JointTrajectoryController::update( if (traj_contr_) { traj_contr_->computeCommands( - tmp_command_, state_current_, state_error_, state_desired_, time, period); + tmp_command_, state_current_, state_error_, state_desired_, + time - traj_external_point_ptr_->time_from_start(), period); } // set values for next hardware write() diff --git a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp index c2152b7930..88b3ac9ffa 100644 --- a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp +++ b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp @@ -40,18 +40,27 @@ class PidTrajectoryPlugin : public TrajectoryControllerBase bool activate() override; bool computeControlLaw( - const std::shared_ptr & /*trajectory*/) override; + const std::shared_ptr & /*trajectory*/) override + { + // nothing to do + return true; + } bool updateGainsRT() override; void computeCommands( std::vector & tmp_command, const trajectory_msgs::msg::JointTrajectoryPoint current, const trajectory_msgs::msg::JointTrajectoryPoint error, - const trajectory_msgs::msg::JointTrajectoryPoint desired, const rclcpp::Time & time, - const rclcpp::Duration & period) override; + const trajectory_msgs::msg::JointTrajectoryPoint desired, + const rclcpp::Duration & duration_since_start, const rclcpp::Duration & period) override; void reset() override; + void start() override + { + // nothing to do + } + protected: /** * @brief parse PID gains from parameter struct diff --git a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp index 015170d6e4..402f8b1cab 100644 --- a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp +++ b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp @@ -82,22 +82,11 @@ class TrajectoryControllerBase } /** - * @brief set the time when the current trajectory started + * @brief called when the current trajectory started in update loop * - * (same logic as trajectory class) + * use this to implement switching of real-time buffers for updating the control law */ - void start(const rclcpp::Time time, const rclcpp::Time trajectory_start_time) - { - if (trajectory_start_time.seconds() == 0.0) - { - trajectory_start_time_ = time; - } - else - { - trajectory_start_time_ = trajectory_start_time; - } - start_trajectory_ = true; - } + virtual void start(void) = 0; /** * @brief update the gains from a RT thread @@ -110,14 +99,22 @@ class TrajectoryControllerBase virtual bool updateGainsRT(void) = 0; /** - * @brief compute the commands with the calculated gains + * @brief compute the commands with the precalculated control law + * + * @param[out] tmp_command the output command + * @param[in] current the current state + * @param[in] error the error between the current state and the desired state + * @param[in] desired the desired state + * @param[in] duration_since_start the duration since the start of the trajectory + * can be negative if the trajectory-start is in the future + * @param[in] period the period since the last update */ JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC virtual void computeCommands( std::vector & tmp_command, const trajectory_msgs::msg::JointTrajectoryPoint current, const trajectory_msgs::msg::JointTrajectoryPoint error, - const trajectory_msgs::msg::JointTrajectoryPoint desired, const rclcpp::Time & time, - const rclcpp::Duration & period) = 0; + const trajectory_msgs::msg::JointTrajectoryPoint desired, + const rclcpp::Duration & duration_since_start, const rclcpp::Duration & period) = 0; /** * @brief reset internal states @@ -136,10 +133,6 @@ class TrajectoryControllerBase rclcpp_lifecycle::LifecycleNode::SharedPtr node_; // Are we computing the control law or is it valid? realtime_tools::RealtimeBuffer rt_control_law_ready_; - // time when the current trajectory started, can be used to interpolate time-varying gains - rclcpp::Time trajectory_start_time_; - // use this variable to activate new gains from the non-RT thread - bool start_trajectory_ = false; /** * @brief compute the control law from the given trajectory diff --git a/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp b/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp index 098a718b8b..a894b6928a 100644 --- a/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp +++ b/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp @@ -78,13 +78,6 @@ bool PidTrajectoryPlugin::updateGainsRT() return true; } -bool PidTrajectoryPlugin::computeControlLaw( - const std::shared_ptr & /*trajectory*/) -{ - // nothing to do - return true; -}; - void PidTrajectoryPlugin::updateGains() { for (size_t i = 0; i < num_cmd_joints_; ++i) @@ -112,8 +105,8 @@ void PidTrajectoryPlugin::updateGains() void PidTrajectoryPlugin::computeCommands( std::vector & tmp_command, const trajectory_msgs::msg::JointTrajectoryPoint /*current*/, const trajectory_msgs::msg::JointTrajectoryPoint error, - const trajectory_msgs::msg::JointTrajectoryPoint desired, const rclcpp::Time & /*time*/, - const rclcpp::Duration & period) + const trajectory_msgs::msg::JointTrajectoryPoint desired, + const rclcpp::Duration & /*duration_since_start*/, const rclcpp::Duration & period) { // Update PIDs for (auto i = 0ul; i < num_cmd_joints_; ++i) diff --git a/joint_trajectory_controller_plugins/test/test_pid_trajectory.cpp b/joint_trajectory_controller_plugins/test/test_pid_trajectory.cpp index ba3314f901..ef8227a332 100644 --- a/joint_trajectory_controller_plugins/test/test_pid_trajectory.cpp +++ b/joint_trajectory_controller_plugins/test/test_pid_trajectory.cpp @@ -45,11 +45,11 @@ TEST_F(PidTrajectoryTest, TestSingleJoint) traj_msg.positions.push_back(0.0); traj_msg.velocities.push_back(0.0); std::vector tmp_command(1, 0.0); - const rclcpp::Time time; + const rclcpp::Duration time_since_start(1, 0); const rclcpp::Duration period(1, 0); - ASSERT_NO_THROW( - traj_contr->computeCommands(tmp_command, traj_msg, traj_msg, traj_msg, time, period)); + ASSERT_NO_THROW(traj_contr->computeCommands( + tmp_command, traj_msg, traj_msg, traj_msg, time_since_start, period)); } TEST_F(PidTrajectoryTest, TestMultipleJoints) @@ -86,11 +86,11 @@ TEST_F(PidTrajectoryTest, TestMultipleJoints) traj_msg.velocities.push_back(0.0); traj_msg.velocities.push_back(0.0); std::vector tmp_command(3, 0.0); - const rclcpp::Time time; + const rclcpp::Duration time_since_start(1, 0); const rclcpp::Duration period(1, 0); - ASSERT_NO_THROW( - traj_contr->computeCommands(tmp_command, traj_msg, traj_msg, traj_msg, time, period)); + ASSERT_NO_THROW(traj_contr->computeCommands( + tmp_command, traj_msg, traj_msg, traj_msg, time_since_start, period)); EXPECT_EQ(tmp_command[0], 1.0); EXPECT_EQ(tmp_command[1], 2.0); From 4e9814b745a2e98c9068fdb7479cc1cb8815334d Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 6 Dec 2023 12:51:21 +0000 Subject: [PATCH 23/30] Update control law also for hold position --- .../joint_trajectory_controller.hpp | 5 +- .../src/joint_trajectory_controller.cpp | 52 ++++++++++++------- .../pid_trajectory_plugin.hpp | 9 +++- .../trajectory_controller_base.hpp | 37 +++++++++++-- 4 files changed, 78 insertions(+), 25 deletions(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index edc0e649d6..53c6bea8dc 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -237,7 +237,10 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool validate_trajectory_msg(const trajectory_msgs::msg::JointTrajectory & trajectory) const; JOINT_TRAJECTORY_CONTROLLER_PUBLIC - void add_new_trajectory_msg( + void add_new_trajectory_msg_nonRT( + const std::shared_ptr & traj_msg); + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + void add_new_trajectory_msg_RT( const std::shared_ptr & traj_msg); JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool validate_trajectory_point_field( diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 15c4531000..95d62d5b30 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -256,8 +256,7 @@ controller_interface::return_type JointTrajectoryController::update( { RCLCPP_WARN(get_node()->get_logger(), "Aborted due to command timeout"); - traj_msg_external_point_ptr_.reset(); - traj_msg_external_point_ptr_.initRT(set_hold_position()); + add_new_trajectory_msg_RT(set_hold_position()); } // Check state/goal tolerance @@ -358,8 +357,7 @@ controller_interface::return_type JointTrajectoryController::update( RCLCPP_WARN(get_node()->get_logger(), "Aborted due to state tolerance violation"); - traj_msg_external_point_ptr_.reset(); - traj_msg_external_point_ptr_.initRT(set_hold_position()); + add_new_trajectory_msg_RT(set_hold_position()); } // check goal tolerance else if (!before_last_point) @@ -376,8 +374,7 @@ controller_interface::return_type JointTrajectoryController::update( RCLCPP_INFO(get_node()->get_logger(), "Goal reached, success!"); - traj_msg_external_point_ptr_.reset(); - traj_msg_external_point_ptr_.initRT(set_success_trajectory_point()); + add_new_trajectory_msg_RT(set_success_trajectory_point()); } else if (!within_goal_time) { @@ -393,8 +390,7 @@ controller_interface::return_type JointTrajectoryController::update( get_node()->get_logger(), "Aborted due goal_time_tolerance exceeding by %f seconds", time_difference); - traj_msg_external_point_ptr_.reset(); - traj_msg_external_point_ptr_.initRT(set_hold_position()); + add_new_trajectory_msg_RT(set_hold_position()); } } } @@ -403,16 +399,14 @@ controller_interface::return_type JointTrajectoryController::update( // we need to ensure that there is no pending goal -> we get a race condition otherwise RCLCPP_ERROR(get_node()->get_logger(), "Holding position due to state tolerance violation"); - traj_msg_external_point_ptr_.reset(); - traj_msg_external_point_ptr_.initRT(set_hold_position()); + add_new_trajectory_msg_RT(set_hold_position()); } else if ( !before_last_point && !within_goal_time && *(rt_has_pending_goal_.readFromRT()) == false) { RCLCPP_ERROR(get_node()->get_logger(), "Exceeded goal_time_tolerance: holding position..."); - traj_msg_external_point_ptr_.reset(); - traj_msg_external_point_ptr_.initRT(set_hold_position()); + add_new_trajectory_msg_RT(set_hold_position()); } // else, run another cycle while waiting for outside_goal_tolerance // to be satisfied (will stay in this state until new message arrives) @@ -1024,7 +1018,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( } // The controller should start by holding position at the beginning of active state - add_new_trajectory_msg(set_hold_position()); + add_new_trajectory_msg_nonRT(set_hold_position()); rt_is_holding_.writeFromNonRT(true); // parse timeout parameter @@ -1180,7 +1174,7 @@ void JointTrajectoryController::topic_callback( // always replace old msg with new one for now if (subscriber_is_active_) { - add_new_trajectory_msg(msg); + add_new_trajectory_msg_nonRT(msg); rt_is_holding_.writeFromNonRT(false); } }; @@ -1226,7 +1220,7 @@ rclcpp_action::CancelResponse JointTrajectoryController::goal_cancelled_callback rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); // Enter hold current position mode - add_new_trajectory_msg(set_hold_position()); + add_new_trajectory_msg_nonRT(set_hold_position()); } return rclcpp_action::CancelResponse::ACCEPT; } @@ -1243,7 +1237,7 @@ void JointTrajectoryController::goal_accepted_callback( auto traj_msg = std::make_shared(goal_handle->get_goal()->trajectory); - add_new_trajectory_msg(traj_msg); + add_new_trajectory_msg_nonRT(traj_msg); rt_is_holding_.writeFromNonRT(false); } @@ -1516,14 +1510,15 @@ bool JointTrajectoryController::validate_trajectory_msg( return true; } -void JointTrajectoryController::add_new_trajectory_msg( +void JointTrajectoryController::add_new_trajectory_msg_nonRT( const std::shared_ptr & traj_msg) { traj_msg_external_point_ptr_.writeFromNonRT(traj_msg); - // compute gains of controller + // compute control law if (traj_contr_) { + // this can take some time; trajectory won't start until control law is computed if (traj_contr_->computeControlLawNonRT(traj_msg) == false) { RCLCPP_ERROR(get_node()->get_logger(), "Failed to compute gains for trajectory."); @@ -1531,12 +1526,31 @@ void JointTrajectoryController::add_new_trajectory_msg( } } +void JointTrajectoryController::add_new_trajectory_msg_RT( + const std::shared_ptr & traj_msg) +{ + // TODO(christophfroehlich): Need a lock-free write here + // See https://github.com/ros-controls/ros2_controllers/issues/168 + traj_msg_external_point_ptr_.reset(); + traj_msg_external_point_ptr_.initRT(traj_msg); + + // compute control law + if (traj_contr_) + { + // this is used for set_hold_position() only -> this should (and must) not take a long time + if (traj_contr_->computeControlLawRT(traj_msg) == false) + { + RCLCPP_ERROR(get_node()->get_logger(), "Failed to compute gains for trajectory."); + } + } +} + void JointTrajectoryController::preempt_active_goal() { const auto active_goal = *rt_active_goal_.readFromNonRT(); if (active_goal) { - add_new_trajectory_msg(set_hold_position()); + add_new_trajectory_msg_nonRT(set_hold_position()); auto action_res = std::make_shared(); action_res->set__error_code(FollowJTrajAction::Result::INVALID_GOAL); action_res->set__error_string("Current goal cancelled due to new incoming action."); diff --git a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp index 88b3ac9ffa..9c054c0627 100644 --- a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp +++ b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp @@ -39,7 +39,14 @@ class PidTrajectoryPlugin : public TrajectoryControllerBase bool activate() override; - bool computeControlLaw( + bool computeControlLawNonRT_impl( + const std::shared_ptr & /*trajectory*/) override + { + // nothing to do + return true; + } + + bool computeControlLawRT_impl( const std::shared_ptr & /*trajectory*/) override { // nothing to do diff --git a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp index 402f8b1cab..9814ddfeb6 100644 --- a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp +++ b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp @@ -67,7 +67,7 @@ class TrajectoryControllerBase * of the trajectory until it finishes * * this method is not virtual, any overrides won't be called by JTC. Instead, override - * computeControlLaw for your implementation + * computeControlLawNonRT_impl for your implementation * * @return true if the gains were computed, false otherwise */ @@ -76,7 +76,28 @@ class TrajectoryControllerBase const std::shared_ptr & trajectory) { rt_control_law_ready_.writeFromNonRT(false); - auto ret = computeControlLaw(trajectory); + auto ret = computeControlLawNonRT_impl(trajectory); + rt_control_law_ready_.writeFromNonRT(true); + return ret; + } + + /** + * @brief compute the control law for the given trajectory + * + * this method must finish quickly (within one controller-update rate) + * + * this method is not virtual, any overrides won't be called by JTC. Instead, override + * computeControlLawRT_impl for your implementation + * + * @return true if the gains were computed, false otherwise + */ + JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC + bool computeControlLawRT( + const std::shared_ptr & trajectory) + { + // TODO(christophfroehlich): Need a lock-free write here + rt_control_law_ready_.writeFromNonRT(false); + auto ret = computeControlLawRT_impl(trajectory); rt_control_law_ready_.writeFromNonRT(true); return ret; } @@ -135,11 +156,19 @@ class TrajectoryControllerBase realtime_tools::RealtimeBuffer rt_control_law_ready_; /** - * @brief compute the control law from the given trajectory + * @brief compute the control law from the given trajectory (in the non-RT loop) + * @return true if the gains were computed, false otherwise + */ + JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC + virtual bool computeControlLawNonRT_impl( + const std::shared_ptr & trajectory) = 0; + + /** + * @brief compute the control law for a single point (in the RT loop) * @return true if the gains were computed, false otherwise */ JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC - virtual bool computeControlLaw( + virtual bool computeControlLawRT_impl( const std::shared_ptr & trajectory) = 0; }; From 89d63b650e9d23cfc00c36011345e8b1238273a5 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 6 Dec 2023 13:01:32 +0000 Subject: [PATCH 24/30] Fix parameter updates and tests after rebase --- .../src/joint_trajectory_controller.cpp | 4 +-- .../test/test_trajectory_controller.cpp | 26 ++++++++++++------- .../test/test_trajectory_controller_utils.hpp | 9 +++++-- 3 files changed, 25 insertions(+), 14 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 95d62d5b30..f75eb2f976 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -127,10 +127,8 @@ controller_interface::return_type JointTrajectoryController::update( { params_ = param_listener_->get_params(); default_tolerances_ = get_segment_tolerances(params_); - // update gains of controller - // variable use_closed_loop_control_law_ is updated in on_configure only - if (use_external_control_law_) + if (traj_contr_) { if (traj_contr_->updateGainsRT() == false) { diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index cb52872c3d..059a56ab2b 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -362,30 +362,38 @@ TEST_P(TrajectoryControllerTestParameterized, update_dynamic_parameters) { rclcpp::executors::MultiThreadedExecutor executor; + // with kp = 0.0 SetUpAndActivateTrajectoryController(executor); updateControllerAsync(); - auto pids = traj_controller_->get_pids(); + auto pids = traj_controller_->get_traj_contr(); if (traj_controller_->use_external_control_law()) { - EXPECT_EQ(pids.size(), 3); - auto gain_0 = pids.at(0)->getGains(); - EXPECT_EQ(gain_0.p_gain_, 0.0); + std::vector tmp_command{0.0, 0.0, 0.0}; + trajectory_msgs::msg::JointTrajectoryPoint error; + error.positions = {1.0, 0.0, 0.0}; + error.velocities = {0.0, 0.0, 0.0}; + trajectory_msgs::msg::JointTrajectoryPoint current; + trajectory_msgs::msg::JointTrajectoryPoint desired; + desired.velocities = {0.0, 0.0, 0.0}; + rclcpp::Duration duration_since_start(std::chrono::milliseconds(250)); + rclcpp::Duration period(std::chrono::milliseconds(100)); + + pids->computeCommands(tmp_command, current, error, desired, duration_since_start, period); + EXPECT_EQ(tmp_command.at(0), 0.0); double kp = 1.0; SetPidParameters(kp); updateControllerAsync(); - pids = traj_controller_->get_pids(); - EXPECT_EQ(pids.size(), 3); - gain_0 = pids.at(0)->getGains(); - EXPECT_EQ(gain_0.p_gain_, kp); + pids->computeCommands(tmp_command, current, error, desired, duration_since_start, period); + EXPECT_EQ(tmp_command.at(0), 1.0); } else { // nothing to check here, skip further test - EXPECT_EQ(pids.size(), 0); + EXPECT_EQ(pids, nullptr); } executor.cancel(); diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 883f81eadf..90fd8cfeab 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -116,7 +116,11 @@ class TestableJointTrajectoryController bool is_open_loop() const { return params_.open_loop_control; } - std::vector get_pids() const { return pids_; } + std::shared_ptr get_traj_contr() + const + { + return traj_contr_; + } joint_trajectory_controller::SegmentTolerances get_tolerances() const { @@ -208,7 +212,8 @@ class TrajectoryControllerTest : public ::testing::Test for (size_t i = 0; i < joint_names_.size(); ++i) { const std::string prefix = "gains." + joint_names_[i]; - const rclcpp::Parameter angle_wraparound(prefix + ".angle_wraparound", angle_wraparound_default); + const rclcpp::Parameter angle_wraparound( + prefix + ".angle_wraparound", angle_wraparound_default); node->set_parameters({angle_wraparound}); } } From 2bdc1625c1731420750e5c6a811ec82f32f13602 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 6 Dec 2023 13:01:32 +0000 Subject: [PATCH 25/30] Export dependencies and update comments --- .../src/joint_trajectory_controller_parameters.yaml | 2 +- joint_trajectory_controller_plugins/CMakeLists.txt | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index ba59083ad8..b9c00566dc 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -86,7 +86,7 @@ joint_trajectory_controller: cmd_timeout must be greater than constraints.goal_time, otherwise ignored. If zero, timeout is deactivated", } - controller_plugin : { + controller_plugin: { type: string, default_value: "joint_trajectory_controller_plugins::PidTrajectoryPlugin", description: "Type of the plugin for the trajectory controller", diff --git a/joint_trajectory_controller_plugins/CMakeLists.txt b/joint_trajectory_controller_plugins/CMakeLists.txt index 42f2275fb9..5ef16e7b54 100644 --- a/joint_trajectory_controller_plugins/CMakeLists.txt +++ b/joint_trajectory_controller_plugins/CMakeLists.txt @@ -73,5 +73,5 @@ ament_export_libraries( ament_export_targets( export_${PROJECT_NAME} ) - +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_package() From b9315a7d4cb68ffd978aaeefd8bf169717539a71 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 6 Dec 2023 13:01:32 +0000 Subject: [PATCH 26/30] Rename updateGains method --- .../pid_trajectory_plugin.hpp | 2 +- .../src/pid_trajectory_plugin.cpp | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp index 9c054c0627..3718f04847 100644 --- a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp +++ b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp @@ -72,7 +72,7 @@ class PidTrajectoryPlugin : public TrajectoryControllerBase /** * @brief parse PID gains from parameter struct */ - void updateGains(); + void parseGains(); // number of command joints size_t num_cmd_joints_; diff --git a/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp b/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp index a894b6928a..7ca9b8b4a9 100644 --- a/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp +++ b/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp @@ -63,7 +63,7 @@ bool PidTrajectoryPlugin::configure() bool PidTrajectoryPlugin::activate() { params_ = param_listener_->get_params(); - updateGains(); + parseGains(); return true; }; @@ -72,13 +72,13 @@ bool PidTrajectoryPlugin::updateGainsRT() if (param_listener_->is_old(params_)) { params_ = param_listener_->get_params(); - updateGains(); + parseGains(); } return true; } -void PidTrajectoryPlugin::updateGains() +void PidTrajectoryPlugin::parseGains() { for (size_t i = 0; i < num_cmd_joints_; ++i) { From 8b249df47bf317b0a72b5ed7950f501780b4ed23 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 6 Dec 2023 13:02:11 +0000 Subject: [PATCH 27/30] Call activate() of traj_contr_ --- .../src/joint_trajectory_controller.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index f75eb2f976..cfa3b107f3 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -220,7 +220,7 @@ controller_interface::return_type JointTrajectoryController::update( } if (traj_contr_) { - // set start time of trajectory to traj_contr_ + // switch RT buffer of traj_contr_ traj_contr_->start(); } } @@ -1041,9 +1041,11 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( cmd_timeout_ = 0.0; } - if (traj_contr_) + // activate traj_contr_, e.g., update gains + if (traj_contr_ && traj_contr_->activate() == false) { - traj_contr_->activate(); + RCLCPP_ERROR(get_node()->get_logger(), "Error during trajectory controller activation."); + return CallbackReturn::ERROR; } return CallbackReturn::SUCCESS; From 37639c435af9716de3138f30a342bd2b592d6526 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 12 Dec 2023 14:42:57 +0100 Subject: [PATCH 28/30] Add missing ) --- .../test/test_trajectory_controller_utils.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 7c2441d442..73531fc043 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -234,7 +234,7 @@ class TrajectoryControllerTest : public ::testing::Test const rclcpp::Parameter k_i(prefix + ".i", 0.0); const rclcpp::Parameter k_d(prefix + ".d", 0.0); const rclcpp::Parameter i_clamp(prefix + ".i_clamp", 0.0); - const rclcpp::Parameter ff_velocity_scale(prefix + ".ff_velocity_scale", ff_value; + const rclcpp::Parameter ff_velocity_scale(prefix + ".ff_velocity_scale", ff_value); node->set_parameters({k_p, k_i, k_d, i_clamp, ff_velocity_scale}); } } From b9ad9373bb6d65862a359712c3f8063a333257e5 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 14 Dec 2023 13:02:21 +0000 Subject: [PATCH 29/30] Parse traj_control gains before sending hold position --- .../src/joint_trajectory_controller.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index cfa3b107f3..4aef89f9a7 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -1015,6 +1015,13 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( read_state_from_state_interfaces(last_commanded_state_); } + // activate traj_contr_, e.g., update gains + if (traj_contr_ && traj_contr_->activate() == false) + { + RCLCPP_ERROR(get_node()->get_logger(), "Error during trajectory controller activation."); + return CallbackReturn::ERROR; + } + // The controller should start by holding position at the beginning of active state add_new_trajectory_msg_nonRT(set_hold_position()); rt_is_holding_.writeFromNonRT(true); @@ -1041,13 +1048,6 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( cmd_timeout_ = 0.0; } - // activate traj_contr_, e.g., update gains - if (traj_contr_ && traj_contr_->activate() == false) - { - RCLCPP_ERROR(get_node()->get_logger(), "Error during trajectory controller activation."); - return CallbackReturn::ERROR; - } - return CallbackReturn::SUCCESS; } From 97c5a810fd089142853a98e59550475108fa65d9 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 14 Feb 2024 11:42:57 +0000 Subject: [PATCH 30/30] Fix wrong text in test --- .../test/test_load_pid_trajectory.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/joint_trajectory_controller_plugins/test/test_load_pid_trajectory.cpp b/joint_trajectory_controller_plugins/test/test_load_pid_trajectory.cpp index 09a08d6cee..c53c35c64a 100644 --- a/joint_trajectory_controller_plugins/test/test_load_pid_trajectory.cpp +++ b/joint_trajectory_controller_plugins/test/test_load_pid_trajectory.cpp @@ -35,7 +35,7 @@ TEST(TestLoadPidController, load_controller) auto available_classes = traj_controller_loader.getDeclaredClasses(); - std::cout << "available filters:" << std::endl; + std::cout << "available plugins:" << std::endl; for (const auto & available_class : available_classes) { std::cout << " " << available_class << std::endl;