From 61617b3f58a2d5915d9ea1121fb1cb362891a46e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sat, 2 Mar 2024 20:55:12 +0100 Subject: [PATCH] [JTC] Parse URDF for continuous joints (#949) --- joint_trajectory_controller/CMakeLists.txt | 1 + .../doc/parameters_context.yaml | 7 +- .../joint_trajectory_controller.hpp | 3 + joint_trajectory_controller/package.xml | 1 + .../src/joint_trajectory_controller.cpp | 43 ++- ...oint_trajectory_controller_parameters.yaml | 7 +- .../test/test_assets.hpp | 278 ++++++++++++++++++ .../test/test_trajectory_controller.cpp | 28 +- .../test/test_trajectory_controller_utils.hpp | 24 +- 9 files changed, 360 insertions(+), 32 deletions(-) create mode 100644 joint_trajectory_controller/test/test_assets.hpp diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index c92d61b3f3..4b0ca82df8 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 + urdf ) find_package(ament_cmake REQUIRED) diff --git a/joint_trajectory_controller/doc/parameters_context.yaml b/joint_trajectory_controller/doc/parameters_context.yaml index 2ffe8aed36..4e4dacf202 100644 --- a/joint_trajectory_controller/doc/parameters_context.yaml +++ b/joint_trajectory_controller/doc/parameters_context.yaml @@ -9,5 +9,10 @@ gains: | .. math:: u = k_{ff} v_d + k_p e + k_i \sum e dt + k_d (v_d - v) - with the desired velocity :math:`v_d`, the measured velocity :math:`v`, the position error :math:`e` (definition see ``angle_wraparound`` below), + with the desired velocity :math:`v_d`, the measured velocity :math:`v`, the position error :math:`e` (definition see below), the controller period :math:`dt`, and the ``velocity`` or ``effort`` manipulated variable (control variable) :math:`u`, respectively. + + If the joint is of continuous type, the position error :math:`e = normalize(s_d - s)` is normalized between :math:`-\pi, \pi`, + i.e., the shortest rotation to the target position is the desired motion. + Otherwise :math:`e = s_d - s` is used, with the desired position :math:`s_d` and the measured + position :math:`s` from the state interface. 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 111837cc17..ae370df0a6 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,6 +43,7 @@ #include "realtime_tools/realtime_server_goal_handle.h" #include "trajectory_msgs/msg/joint_trajectory.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" +#include "urdf/model.h" // auto-generated by generate_parameter_library #include "joint_trajectory_controller_parameters.hpp" @@ -298,6 +299,8 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa void resize_joint_trajectory_point_command( trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size); + urdf::Model model_; + /** * @brief Assigns the values from a trajectory point interface to a joint interface. * diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 5aad4cb86e..8457c02aeb 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -25,6 +25,7 @@ rsl tl_expected trajectory_msgs + urdf 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 46544bf875..6cae29e083 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -36,6 +36,7 @@ #include "rclcpp_action/create_server.hpp" #include "rclcpp_action/server_goal_handle.hpp" #include "rclcpp_lifecycle/state.hpp" +#include "urdf/model.h" namespace joint_trajectory_controller { @@ -46,6 +47,23 @@ JointTrajectoryController::JointTrajectoryController() controller_interface::CallbackReturn JointTrajectoryController::on_init() { + if (!urdf_.empty()) + { + if (!model_.initString(urdf_)) + { + RCLCPP_ERROR(get_node()->get_logger(), "Failed to parse URDF file"); + } + else + { + RCLCPP_DEBUG(get_node()->get_logger(), "Successfully parsed URDF file"); + } + } + else + { + // empty URDF is used for some tests + RCLCPP_DEBUG(get_node()->get_logger(), "No URDF file given"); + } + try { // Create the parameter listener and get the parameters @@ -684,12 +702,33 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( update_pids(); } - // Configure joint position error normalization from ROS parameters (angle_wraparound) + // Configure joint position error normalization (angle_wraparound) joints_angle_wraparound_.resize(dof_); for (size_t i = 0; i < dof_; ++i) { const auto & gains = params_.gains.joints_map.at(params_.joints[i]); - joints_angle_wraparound_[i] = gains.angle_wraparound; + if (gains.angle_wraparound) + { + // TODO(christophfroehlich): remove this warning in a future release (ROS-J) + RCLCPP_WARN( + logger, + "[Deprecated] Parameter 'gains..angle_wraparound' is deprecated. The " + "angle_wraparound is now used if a continuous joint is configured in the URDF."); + joints_angle_wraparound_[i] = true; + } + + if (!urdf_.empty()) + { + auto urdf_joint = model_.getJoint(params_.joints[i]); + if (urdf_joint && urdf_joint->type == urdf::Joint::CONTINUOUS) + { + RCLCPP_DEBUG( + logger, "joint '%s' is of type continuous, use angle_wraparound.", + params_.joints[i].c_str()); + joints_angle_wraparound_[i] = true; + } + // do nothing if joint is not found in the URDF + } } if (params_.state_interfaces.empty()) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index ded5bb7ca3..b17229cab8 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -125,11 +125,8 @@ joint_trajectory_controller: angle_wraparound: { type: bool, default_value: false, - description: 'For joints that wrap around (without end stop, ie. are continuous), - where the shortest rotation to the target position is the desired motion. - If true, the position error :math:`e = normalize(s_d - s)` is normalized between :math:`-\pi, \pi`. - Otherwise :math:`e = s_d - s` is used, with the desired position :math:`s_d` and the measured - position :math:`s` from the state interface.' + description: "[deprecated] For joints that wrap around (ie. are continuous). + Normalizes position-error to -pi to pi." } constraints: stopped_velocity_tolerance: { diff --git a/joint_trajectory_controller/test/test_assets.hpp b/joint_trajectory_controller/test/test_assets.hpp new file mode 100644 index 0000000000..ccdbaf4c8a --- /dev/null +++ b/joint_trajectory_controller/test/test_assets.hpp @@ -0,0 +1,278 @@ +// Copyright 2023 ros2_control Development Team +// +// 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_ASSETS_HPP_ +#define TEST_ASSETS_HPP_ + +#include + +namespace test_trajectory_controllers +{ +const auto urdf_rrrbot_revolute = + R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + +const auto urdf_rrrbot_continuous = + R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + +} // namespace test_trajectory_controllers + +#endif // TEST_ASSETS_HPP_ diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 2bfe6f150d..67eb959df2 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -45,6 +45,7 @@ #include "trajectory_msgs/msg/joint_trajectory.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" +#include "test_assets.hpp" #include "test_trajectory_controller_utils.hpp" using lifecycle_msgs::msg::State; @@ -463,14 +464,15 @@ TEST_P(TrajectoryControllerTestParameterized, hold_on_startup) const double EPS = 1e-6; /** - * @brief check if calculated trajectory error is correct with angle wraparound=true + * @brief check if calculated trajectory error is correct (angle wraparound) for continuous joints */ TEST_P(TrajectoryControllerTestParameterized, compute_error_angle_wraparound_true) { rclcpp::executors::MultiThreadedExecutor executor; std::vector params = {}; SetUpAndActivateTrajectoryController( - executor, params, true, 0.0, 1.0, true /* angle_wraparound */); + executor, params, true, 0.0, 1.0, INITIAL_POS_JOINTS, INITIAL_VEL_JOINTS, INITIAL_ACC_JOINTS, + INITIAL_EFF_JOINTS, test_trajectory_controllers::urdf_rrrbot_continuous); size_t n_joints = joint_names_.size(); @@ -554,14 +556,15 @@ TEST_P(TrajectoryControllerTestParameterized, compute_error_angle_wraparound_tru } /** - * @brief check if calculated trajectory error is correct with angle wraparound=false + * @brief check if calculated trajectory error is correct (no angle wraparound) for revolute joints */ TEST_P(TrajectoryControllerTestParameterized, compute_error_angle_wraparound_false) { rclcpp::executors::MultiThreadedExecutor executor; std::vector params = {}; SetUpAndActivateTrajectoryController( - executor, params, true, 0.0, 1.0, false /* angle_wraparound */); + executor, params, true, 0.0, 1.0, INITIAL_POS_JOINTS, INITIAL_VEL_JOINTS, INITIAL_ACC_JOINTS, + INITIAL_EFF_JOINTS, test_trajectory_controllers::urdf_rrrbot_revolute); size_t n_joints = joint_names_.size(); @@ -636,15 +639,16 @@ TEST_P(TrajectoryControllerTestParameterized, compute_error_angle_wraparound_fal } /** - * @brief check if position error of revolute joints are wrapped around if not configured so + * @brief check if position error of revolute joints aren't wrapped around (state topic) */ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparound) { rclcpp::executors::MultiThreadedExecutor executor; constexpr double k_p = 10.0; std::vector params = {}; - bool angle_wraparound = false; - SetUpAndActivateTrajectoryController(executor, params, true, k_p, 0.0, angle_wraparound); + SetUpAndActivateTrajectoryController( + executor, params, true, k_p, 0.0, INITIAL_POS_JOINTS, INITIAL_VEL_JOINTS, INITIAL_ACC_JOINTS, + INITIAL_EFF_JOINTS, test_trajectory_controllers::urdf_rrrbot_revolute); subscribeToState(); size_t n_joints = joint_names_.size(); @@ -737,14 +741,16 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun } /** - * @brief check if position error of revolute joints are wrapped around if configured so + * @brief check if position error of continuous joints are wrapped around (state topic) */ TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound) { rclcpp::executors::MultiThreadedExecutor executor; constexpr double k_p = 10.0; std::vector params = {}; - SetUpAndActivateTrajectoryController(executor, params, true, k_p, 0.0, true); + SetUpAndActivateTrajectoryController( + executor, params, true, k_p, 0.0, INITIAL_POS_JOINTS, INITIAL_VEL_JOINTS, INITIAL_ACC_JOINTS, + INITIAL_EFF_JOINTS, test_trajectory_controllers::urdf_rrrbot_continuous); size_t n_joints = joint_names_.size(); @@ -1844,7 +1850,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_first_co std::vector initial_acc_cmd{3, std::numeric_limits::quiet_NaN()}; SetUpAndActivateTrajectoryController( - executor, {is_open_loop_parameters}, true, 0., 1., false, initial_pos_cmd, initial_vel_cmd, + executor, {is_open_loop_parameters}, true, 0., 1., initial_pos_cmd, initial_vel_cmd, initial_acc_cmd); // no call of update method, so the values should be read from state interfaces @@ -1888,7 +1894,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_later_co initial_acc_cmd.push_back(0.02 + static_cast(i) / 10.0); } SetUpAndActivateTrajectoryController( - executor, {is_open_loop_parameters}, true, 0., 1., false, initial_pos_cmd, initial_vel_cmd, + executor, {is_open_loop_parameters}, true, 0., 1., initial_pos_cmd, initial_vel_cmd, initial_acc_cmd); // no call of update method, so the values should be read from command interfaces diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 6978d0e452..baa4111239 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -215,9 +215,10 @@ class TrajectoryControllerTest : public ::testing::Test } void SetUpTrajectoryController( - rclcpp::Executor & executor, const std::vector & parameters = {}) + rclcpp::Executor & executor, const std::vector & parameters = {}, + const std::string & urdf = "") { - auto ret = SetUpTrajectoryControllerLocal(parameters); + auto ret = SetUpTrajectoryControllerLocal(parameters, urdf); if (ret != controller_interface::return_type::OK) { FAIL(); @@ -226,7 +227,7 @@ class TrajectoryControllerTest : public ::testing::Test } controller_interface::return_type SetUpTrajectoryControllerLocal( - const std::vector & parameters = {}) + const std::vector & parameters = {}, const std::string & urdf = "") { traj_controller_ = std::make_shared(); @@ -241,11 +242,10 @@ class TrajectoryControllerTest : public ::testing::Test traj_controller_->set_node_options(node_options); return traj_controller_->init( - controller_name_, "", 0, "", traj_controller_->define_custom_node_options()); + controller_name_, urdf, 0, "", traj_controller_->define_custom_node_options()); } - void SetPidParameters( - double p_value = 0.0, double ff_value = 1.0, bool angle_wraparound_value = false) + void SetPidParameters(double p_value = 0.0, double ff_value = 1.0) { traj_controller_->trigger_declare_parameters(); auto node = traj_controller_->get_node(); @@ -258,20 +258,18 @@ 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_value); - const rclcpp::Parameter angle_wraparound( - prefix + ".angle_wraparound", angle_wraparound_value); - 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}); } } void SetUpAndActivateTrajectoryController( rclcpp::Executor & executor, const std::vector & parameters = {}, bool separate_cmd_and_state_values = false, double k_p = 0.0, double ff = 1.0, - bool angle_wraparound = false, const std::vector initial_pos_joints = INITIAL_POS_JOINTS, const std::vector initial_vel_joints = INITIAL_VEL_JOINTS, const std::vector initial_acc_joints = INITIAL_ACC_JOINTS, - const std::vector initial_eff_joints = INITIAL_EFF_JOINTS) + const std::vector initial_eff_joints = INITIAL_EFF_JOINTS, + const std::string & urdf = "") { auto has_nonzero_vel_param = std::find_if( @@ -287,10 +285,10 @@ class TrajectoryControllerTest : public ::testing::Test parameters_local.emplace_back("allow_nonzero_velocity_at_trajectory_end", true); } // read-only parameters have to be set before init -> won't be read otherwise - SetUpTrajectoryController(executor, parameters_local); + SetUpTrajectoryController(executor, parameters_local, urdf); // set pid parameters before configure - SetPidParameters(k_p, ff, angle_wraparound); + SetPidParameters(k_p, ff); traj_controller_->get_node()->configure(); ActivateTrajectoryController(