From ff0e4a48d213d2e24c229ade26acfda83b2e8286 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 6 Jul 2022 21:32:33 +0200 Subject: [PATCH 1/3] [JTC] Add additional parameter to enable configuration of interfaces for following controllers in a chain. --- joint_trajectory_controller/doc/userdoc.rst | 27 ++++++- .../joint_trajectory_controller.hpp | 1 + .../src/joint_trajectory_controller.cpp | 23 +++++- .../test_load_joint_trajectory_controller.cpp | 3 +- .../test/test_trajectory_controller.cpp | 81 +++++++++++++++++++ .../test/test_trajectory_controller_utils.hpp | 8 ++ 6 files changed, 138 insertions(+), 5 deletions(-) diff --git a/joint_trajectory_controller/doc/userdoc.rst b/joint_trajectory_controller/doc/userdoc.rst index ec58a423f1..17c0c511c5 100644 --- a/joint_trajectory_controller/doc/userdoc.rst +++ b/joint_trajectory_controller/doc/userdoc.rst @@ -32,6 +32,26 @@ Other features Robust to system clock changes: Discontinuous system clock changes do not cause discontinuities in the execution of already queued trajectory segments. +ros2_control interfaces +------------------------ + +References +^^^^^^^^^^^ +(the controller is not yet implemented as chainable controller) + +States +^^^^^^^ +The state interfaces are defined with ``joints`` and ``state_interfaces`` parameters as follows: ``/``. +Supported state interfaces are ``position``, ``velocity``, ``acceleration`` and ``effort`` as defined in the [hardware_interface/hardware_interface_type_values.hpp](https://github.com/ros-controls/ros2_control/blob/master/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp). +Legal combinations of state interfaces are: +- ``position`` +- ``position`` and ``velocity`` +- ``position``, ``velocity`` and ``acceleration`` +- ``effort`` + +Commands +^^^^^^^^^ + Using Joint Trajectory Controller(s) ------------------------------------ @@ -84,8 +104,11 @@ A yaml file for using it could be: Details about parameters ^^^^^^^^^^^^^^^^^^^^^^^^ -joint (list(string)): - Joint names to control. +joints (list(string)): + Joint names to control and listen to. + +command_joints (list(string)): + Joint names to control. This parameters is used if JTC is used in a controller chain where command and state interfaces don't have same names. command_interface (list(string)): Command interfaces provided by the hardware interface for all joints. 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 4c2f612112..79fe6bccf4 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 @@ -113,6 +113,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa protected: std::vector joint_names_; + std::vector command_joint_names_; std::vector command_interface_types_; std::vector state_interface_types_; diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 5898a6fe95..81f55c4def 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -56,6 +56,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_init() { // with the lifecycle node being initialized, we can declare parameters joint_names_ = auto_declare>("joints", joint_names_); + command_joint_names_ = + auto_declare>("command_joints", command_interface_types_); command_interface_types_ = auto_declare>("command_interfaces", command_interface_types_); state_interface_types_ = @@ -97,7 +99,7 @@ JointTrajectoryController::command_interface_configuration() const std::exit(EXIT_FAILURE); } conf.names.reserve(dof_ * command_interface_types_.size()); - for (const auto & joint_name : joint_names_) + for (const auto & joint_name : command_joint_names_) { for (const auto & interface_type : command_interface_types_) { @@ -493,6 +495,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( dof_ = joint_names_.size(); + // TODO(destogl): why is this here? Add comment or move if (!reset()) { return CallbackReturn::FAILURE; @@ -500,9 +503,25 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( if (joint_names_.empty()) { + // TODO(destogl): is this correct? Can we really move-on if no joint names are not provided? RCLCPP_WARN(logger, "'joints' parameter is empty."); } + command_joint_names_ = get_node()->get_parameter("command_joints").as_string_array(); + + if (command_joint_names_.empty()) + { + command_joint_names_ = joint_names_; + RCLCPP_INFO( + logger, "No specific joint names are used for command interfaces. Using 'joints' parameter."); + } + else if (command_joint_names_.size() != joint_names_.size()) + { + RCLCPP_ERROR( + logger, "'command_joints' parameter has to have the same size as 'joints' parameter."); + return CallbackReturn::FAILURE; + } + // Specialized, child controllers set interfaces before calling configure function. if (command_interface_types_.empty()) { @@ -797,7 +816,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); auto index = std::distance(allowed_interface_types_.begin(), it); if (!controller_interface::get_ordered_interfaces( - command_interfaces_, joint_names_, interface, joint_command_interface_[index])) + command_interfaces_, command_joint_names_, interface, joint_command_interface_[index])) { RCLCPP_ERROR( get_node()->get_logger(), "Expected %zu '%s' command interfaces, got %zu.", dof_, diff --git a/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp b/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp index e0dfdc6353..0f94d0b16c 100644 --- a/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp @@ -12,9 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include +#include "gmock/gmock.h" + #include "controller_manager/controller_manager.hpp" #include "hardware_interface/resource_manager.hpp" #include "rclcpp/executor.hpp" diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 0d717515cf..f23b632aec 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -95,6 +95,87 @@ TEST_P(TrajectoryControllerTestParameterized, configure_state_ignores_commands) executor.cancel(); } +TEST_P(TrajectoryControllerTestParameterized, check_interface_names) +{ + SetUpTrajectoryController(); + + const auto state = traj_controller_->get_node()->configure(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); + + std::vector state_interface_names; + state_interface_names.reserve(joint_names_.size() * state_interface_types_.size()); + for (const auto & joint : joint_names_) + { + for (const auto & interface : state_interface_types_) + { + state_interface_names.push_back(joint + "/" + interface); + } + } + auto state_interfaces = traj_controller_->state_interface_configuration(); + EXPECT_EQ(state_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); + EXPECT_EQ(state_interfaces.names.size(), joint_names_.size() * state_interface_types_.size()); + ASSERT_THAT(state_interfaces.names, testing::UnorderedElementsAreArray(state_interface_names)); + + std::vector command_interface_names; + command_interface_names.reserve(joint_names_.size() * command_interface_types_.size()); + for (const auto & joint : joint_names_) + { + for (const auto & interface : command_interface_types_) + { + command_interface_names.push_back(joint + "/" + interface); + } + } + auto command_interfaces = traj_controller_->command_interface_configuration(); + EXPECT_EQ( + command_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); + EXPECT_EQ(command_interfaces.names.size(), joint_names_.size() * command_interface_types_.size()); + ASSERT_THAT( + command_interfaces.names, testing::UnorderedElementsAreArray(command_interface_names)); +} + +TEST_P(TrajectoryControllerTestParameterized, check_interface_names_with_command_joints) +{ + SetUpTrajectoryController(); + + // set command_joints parameter + const rclcpp::Parameter command_joint_names_param("command_joints", command_joint_names_); + traj_controller_->get_node()->set_parameter({command_joint_names_param}); + + const auto state = traj_controller_->get_node()->configure(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); + + std::vector state_interface_names; + state_interface_names.reserve(joint_names_.size() * state_interface_types_.size()); + for (const auto & joint : joint_names_) + { + for (const auto & interface : state_interface_types_) + { + state_interface_names.push_back(joint + "/" + interface); + } + } + auto state_interfaces = traj_controller_->state_interface_configuration(); + EXPECT_EQ(state_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); + EXPECT_EQ(state_interfaces.names.size(), joint_names_.size() * state_interface_types_.size()); + ASSERT_THAT(state_interfaces.names, testing::UnorderedElementsAreArray(state_interface_names)); + + std::vector command_interface_names; + command_interface_names.reserve(command_joint_names_.size() * command_interface_types_.size()); + for (const auto & joint : joint_names_) + { + for (const auto & interface : command_interface_types_) + { + command_interface_names.push_back(joint + "/" + interface); + } + } + auto command_interfaces = traj_controller_->command_interface_configuration(); + EXPECT_EQ( + command_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); + EXPECT_EQ( + command_interfaces.names.size(), command_joint_names_.size() * command_interface_types_.size()); + ASSERT_THAT( + command_interfaces.names, testing::UnorderedElementsAreArray(command_interface_names)); +} + TEST_P(TrajectoryControllerTestParameterized, activate) { SetUpTrajectoryController(); diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index f12bd8b8bc..be11d274ad 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -81,6 +81,11 @@ class TestableJointTrajectoryController void set_joint_names(const std::vector & joint_names) { joint_names_ = 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) { command_interface_types_ = command_interfaces; @@ -115,6 +120,8 @@ class TrajectoryControllerTest : public ::testing::Test controller_name_ = "test_joint_trajectory_controller"; joint_names_ = {"joint1", "joint2", "joint3"}; + command_joint_names_ = { + "following_controller/joint1", "following_controller/joint2", "following_controller/joint3"}; joint_pos_.resize(joint_names_.size(), 0.0); joint_state_pos_.resize(joint_names_.size(), 0.0); joint_vel_.resize(joint_names_.size(), 0.0); @@ -396,6 +403,7 @@ class TrajectoryControllerTest : public ::testing::Test std::string controller_name_; std::vector joint_names_; + std::vector command_joint_names_; std::vector command_interface_types_; std::vector state_interface_types_; From 1b68b47a7f92ef74bd83f79874d86857b3529703 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Mon, 25 Jul 2022 16:45:11 +0200 Subject: [PATCH 2/3] Use command_joint_states correctly in parameters depending on joint states. --- .../src/joint_trajectory_controller.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 81f55c4def..a57bfeca4f 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -611,12 +611,13 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( // Init PID gains from ROS parameter server for (size_t i = 0; i < pids_.size(); ++i) { - const std::string prefix = "gains." + joint_names_[i]; + const std::string prefix = "gains." + command_joint_names_[i]; const auto k_p = auto_declare(prefix + ".p", 0.0); const auto k_i = auto_declare(prefix + ".i", 0.0); const auto k_d = auto_declare(prefix + ".d", 0.0); const auto i_clamp = auto_declare(prefix + ".i_clamp", 0.0); - ff_velocity_scale_[i] = auto_declare("ff_velocity_scale/" + joint_names_[i], 0.0); + ff_velocity_scale_[i] = + auto_declare("ff_velocity_scale/" + command_joint_names_[i], 0.0); // Initialize PID pids_[i] = std::make_shared(k_p, k_i, k_d, i_clamp, -i_clamp); } @@ -721,7 +722,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( get_interface_list(command_interface_types_).c_str(), get_interface_list(state_interface_types_).c_str()); - default_tolerances_ = get_segment_tolerances(*get_node(), joint_names_); + default_tolerances_ = get_segment_tolerances(*get_node(), command_joint_names_); // Read parameters customizing controller for special cases open_loop_control_ = get_node()->get_parameter("open_loop_control").get_value(); @@ -757,7 +758,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( state_publisher_ = std::make_unique(publisher_); state_publisher_->lock(); - state_publisher_->msg_.joint_names = joint_names_; + state_publisher_->msg_.joint_names = command_joint_names_; state_publisher_->msg_.desired.positions.resize(dof_); state_publisher_->msg_.desired.velocities.resize(dof_); state_publisher_->msg_.desired.accelerations.resize(dof_); From 61b3727168d82966f384cca4dac2846c653facd7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 10 Aug 2022 21:00:32 +0200 Subject: [PATCH 3/3] Fixup tests. --- joint_trajectory_controller/src/joint_trajectory_controller.cpp | 2 +- joint_trajectory_controller/test/test_trajectory_controller.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index a57bfeca4f..64092e1ab5 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -57,7 +57,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_init() // with the lifecycle node being initialized, we can declare parameters joint_names_ = auto_declare>("joints", joint_names_); command_joint_names_ = - auto_declare>("command_joints", command_interface_types_); + auto_declare>("command_joints", command_joint_names_); command_interface_types_ = auto_declare>("command_interfaces", command_interface_types_); state_interface_types_ = diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index f23b632aec..cc5b175281 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -160,7 +160,7 @@ TEST_P(TrajectoryControllerTestParameterized, check_interface_names_with_command std::vector command_interface_names; command_interface_names.reserve(command_joint_names_.size() * command_interface_types_.size()); - for (const auto & joint : joint_names_) + for (const auto & joint : command_joint_names_) { for (const auto & interface : command_interface_types_) {