diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index f6967930eb..1f92dd66be 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -57,6 +57,11 @@ robot_description [std_msgs::msg::String] Parameters ----------- +enforce_command_limits (optional; bool; default: ``false`` for Jazzy and earlier distro and ``true`` for Rolling and newer distros) + Enforces the joint limits to the joint command interfaces. + If the command is outside the limits, the command is clamped to be within the limits depending on the type of configured joint limits in the URDF. + If the command is within the limits, the command is passed through without any changes. + .type Name of a plugin exported using ``pluginlib`` for a controller. This is a class from which controller's instance with name "``controller_name``" is created. diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 7acbe20166..4a55f1a93b 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -563,6 +563,7 @@ class ControllerManager : public rclcpp::Node std::string robot_description_; rclcpp::Subscription::SharedPtr robot_description_subscription_; rclcpp::TimerBase::SharedPtr robot_description_notification_timer_; + bool enforce_command_limits_; controller_manager::MovingAverageStatistics periodicity_stats_; diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index fd59c89ea8..4e14eb209c 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -278,7 +278,8 @@ ControllerManager::ControllerManager( chainable_loader_( std::make_shared>( kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)), - cm_node_options_(options) + cm_node_options_(options), + robot_description_(resource_manager_->get_urdf()) { initialize_parameters(); init_controller_manager(); @@ -289,6 +290,7 @@ void ControllerManager::init_controller_manager() // Get parameters needed for RT "update" loop to work if (is_resource_manager_initialized()) { + resource_manager_->import_joint_limiters(robot_description_); init_services(); } else @@ -322,6 +324,13 @@ void ControllerManager::init_controller_manager() diagnostics_updater_.add( "Controller Manager Activity", this, &ControllerManager::controller_manager_diagnostic_callback); + + // Declare the enforce_command_limits parameter such a way that it is enabled by default for + // rolling and newer alone + enforce_command_limits_ = + this->get_parameter_or("enforce_command_limits", RCLCPP_VERSION_MAJOR >= 29 ? true : false); + RCLCPP_INFO_EXPRESSION( + get_logger(), enforce_command_limits_, "Enforcing command limits is enabled..."); } void ControllerManager::initialize_parameters() @@ -363,6 +372,7 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String & get_logger(), "Resource Manager has been successfully initialized. Starting Controller Manager " "services..."); + resource_manager_->import_joint_limiters(robot_description_); init_services(); } } @@ -2600,6 +2610,11 @@ controller_interface::return_type ControllerManager::update( } } + if (enforce_command_limits_) + { + resource_manager_->enforce_command_limits(period); + } + // there are controllers to (de)activate if (switch_params_.do_switch) { diff --git a/controller_manager/test/test_controller_manager_hardware_error_handling.cpp b/controller_manager/test/test_controller_manager_hardware_error_handling.cpp index 936a2dd4c4..c1fb31d54d 100644 --- a/controller_manager/test/test_controller_manager_hardware_error_handling.cpp +++ b/controller_manager/test/test_controller_manager_hardware_error_handling.cpp @@ -66,6 +66,12 @@ class TestControllerManagerWithTestableCM public testing::WithParamInterface { public: + TestControllerManagerWithTestableCM() + : ControllerManagerFixture( + ros2_control_test_assets::minimal_robot_urdf_no_limits) + { + } + void SetupAndConfigureControllers(int strictness) { test_controller_actuator = std::make_shared(); diff --git a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp index c143ab4862..7b94ce254f 100644 --- a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp +++ b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp @@ -14,6 +14,7 @@ #include #include +#include #include #include #include @@ -106,9 +107,13 @@ class TestControllerChainingWithControllerManager void SetUp() { executor_ = std::make_shared(); + const std::regex velocity_pattern(R"(velocity\s*=\s*"-?[0-9]+(\.[0-9]+)?")"); + const std::string velocity_replacement = R"(velocity="10000.0")"; + const std::string diffbot_urdf_large_limits = std::regex_replace( + ros2_control_test_assets::diffbot_urdf, velocity_pattern, velocity_replacement); cm_ = std::make_shared( std::make_unique( - ros2_control_test_assets::diffbot_urdf, rm_node_->get_node_clock_interface(), + diffbot_urdf_large_limits, rm_node_->get_node_clock_interface(), rm_node_->get_node_logging_interface(), true), executor_, TEST_CM_NAME); run_updater_ = false; diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 58930ec6c4..cd5b06e560 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -86,6 +86,7 @@ controller_manager * The ``--service-call-timeout`` was added as parameter to the helper scripts ``spawner.py``. Useful when the CPU load is high at startup and the service call does not return immediately (`#1808 `_). * The ``cpu_affinity`` parameter can now accept of types ``int`` or ``int_array`` to bind the process to a specific CPU core or multiple CPU cores. (`#1915 `_). * A python module ``test_utils`` was added to the ``controller_manager`` package to help with integration testing (`#1955 `_). +* A new parameter ``enforce_command_limits`` is introduced to be able to enable and disable the enforcement of the command limits (`#1989 `_). hardware_interface ****************** diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index b67aba088d..efadf53528 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -89,6 +89,12 @@ class ResourceManager virtual bool load_and_initialize_components( const std::string & urdf, const unsigned int update_rate = 100); + /** + * @brief Import joint limiters from the URDF. + * @param urdf string containing the URDF. + */ + void import_joint_limiters(const std::string & urdf); + /** * @brief if the resource manager load_and_initialize_components(...) function has been called * this returns true. We want to permit to loading the urdf later on, but we currently don't want @@ -426,6 +432,14 @@ class ResourceManager return_type set_component_state( const std::string & component_name, rclcpp_lifecycle::State & target_state); + /** + * Enforce the command limits for the position, velocity, effort, and acceleration interfaces. + * @note This method is RT-safe + * @return true if the command interfaces are out of limits and the limits are enforced + * @return false if the command interfaces values are within limits + */ + bool enforce_command_limits(const rclcpp::Duration & period); + /// Reads all loaded hardware components. /** * Reads from all active hardware components. @@ -457,6 +471,8 @@ class ResourceManager */ bool state_interface_exists(const std::string & key) const; + const std::string & get_urdf() const; + protected: /// Gets the logger for the resource manager /** diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 7a616d890d..f5bf36a4f0 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -32,6 +32,9 @@ #include "hardware_interface/sensor_interface.hpp" #include "hardware_interface/system.hpp" #include "hardware_interface/system_interface.hpp" +#include "joint_limits/joint_limits_helpers.hpp" +#include "joint_limits/joint_saturation_limiter.hpp" +#include "joint_limits/joint_soft_limiter.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "pluginlib/class_loader.hpp" #include "rclcpp/logging.hpp" @@ -657,6 +660,106 @@ class ResourceStorage } } + void import_joint_limiters(const std::vector & hardware_infos) + { + for (const auto & hw_info : hardware_infos) + { + limiters_data_[hw_info.name] = {}; + for (const auto & [joint_name, limits] : hw_info.limits) + { + std::vector soft_limits; + const std::vector hard_limits{limits}; + joint_limits::JointInterfacesCommandLimiterData data; + data.joint_name = joint_name; + limiters_data_[hw_info.name].push_back(data); + // If the joint limits is found in the softlimits, then extract it + if (hw_info.soft_limits.find(joint_name) != hw_info.soft_limits.end()) + { + soft_limits = {hw_info.soft_limits.at(joint_name)}; + } + std::unique_ptr< + joint_limits::JointLimiterInterface> + limits_interface; + if (soft_limits.empty()) + { + limits_interface = std::make_unique< + joint_limits::JointSaturationLimiter>(); + } + else + { + limits_interface = std::make_unique(); + } + limits_interface->init({joint_name}, hard_limits, soft_limits, nullptr, nullptr); + joint_limiters_interface_[hw_info.name].push_back(std::move(limits_interface)); + } + } + } + + template + void update_joint_limiters_data( + const std::string & joint_name, const std::map & interface_map, + joint_limits::JointControlInterfacesData & data, bool is_command_itf = false) + { + data.joint_name = joint_name; + + const auto fill_interface_data = + [&](const std::string & interface_type, std::optional & value) + { + const std::string interface_name = joint_name + "/" + interface_type; + if (interface_map.find(interface_name) != interface_map.end()) + { + // If the command interface is not claimed, then the value is not set + if (is_command_itf && !claimed_command_interface_map_.at(interface_name)) + { + value = std::nullopt; + } + else + { + value = interface_map.at(interface_name)->get_value(); + } + } + }; + // update the actual data of the limiters + fill_interface_data(hardware_interface::HW_IF_POSITION, data.position); + fill_interface_data(hardware_interface::HW_IF_VELOCITY, data.velocity); + fill_interface_data(hardware_interface::HW_IF_EFFORT, data.effort); + fill_interface_data(hardware_interface::HW_IF_ACCELERATION, data.acceleration); + } + + template + void update_joint_limiters_commands( + const joint_limits::JointControlInterfacesData & state, + std::map & interface_map) + { + const auto set_interface_command = + [&](const std::string & interface_type, const std::optional & data) + { + const std::string interface_name = state.joint_name + "/" + interface_type; + if (interface_map.find(interface_name) != interface_map.end() && data.has_value()) + { + interface_map.at(interface_name)->set_value(data.value()); + } + }; + // update the command data of the limiters + set_interface_command(hardware_interface::HW_IF_POSITION, state.position); + set_interface_command(hardware_interface::HW_IF_VELOCITY, state.velocity); + set_interface_command(hardware_interface::HW_IF_EFFORT, state.effort); + set_interface_command(hardware_interface::HW_IF_ACCELERATION, state.acceleration); + } + + void update_joint_limiters_data() + { + for (auto & joint_limiter_data : limiters_data_) + { + for (auto & data : joint_limiter_data.second) + { + update_joint_limiters_data(data.joint_name, state_interface_map_, data.actual); + update_joint_limiters_data(data.joint_name, command_interface_map_, data.command, true); + data.limited = data.command; + } + } + } + std::string add_state_interface(StateInterface::ConstSharedPtr interface) { auto interface_name = interface->get_name(); @@ -1007,6 +1110,16 @@ class ResourceStorage /// List of all claimed command interfaces std::unordered_map claimed_command_interface_map_; + std::unordered_map> + limiters_data_; + + std::unordered_map< + std::string, std::vector>>> + joint_limiters_interface_; + + std::string robot_description_; + // Update rate of the controller manager, and the clock interface of its node // Used by async components. unsigned int cm_update_rate_ = 100; @@ -1027,6 +1140,7 @@ ResourceManager::ResourceManager( const unsigned int update_rate) : resource_storage_(std::make_unique(clock_interface, logger_interface)) { + resource_storage_->robot_description_ = urdf; load_and_initialize_components(urdf, update_rate); if (activate_all) @@ -1046,6 +1160,7 @@ bool ResourceManager::load_and_initialize_components( { components_are_loaded_and_initialized_ = true; + resource_storage_->robot_description_ = urdf; resource_storage_->cm_update_rate_ = update_rate; auto hardware_info = hardware_interface::parse_control_resources_from_urdf(urdf); @@ -1123,6 +1238,12 @@ bool ResourceManager::load_and_initialize_components( return components_are_loaded_and_initialized_; } +void ResourceManager::import_joint_limiters(const std::string & urdf) +{ + const auto hardware_info = hardware_interface::parse_control_resources_from_urdf(urdf); + resource_storage_->import_joint_limiters(hardware_info); +} + bool ResourceManager::are_components_initialized() const { return components_are_loaded_and_initialized_; @@ -1726,6 +1847,26 @@ return_type ResourceManager::set_component_state( return result; } +// CM API: Called in "update"-thread +bool ResourceManager::enforce_command_limits(const rclcpp::Duration & period) +{ + bool enforce_result = false; + // Joint Limiters operations + resource_storage_->update_joint_limiters_data(); + for (auto & [hw_name, limiters] : resource_storage_->joint_limiters_interface_) + { + for (size_t i = 0; i < limiters.size(); i++) + { + joint_limits::JointInterfacesCommandLimiterData & data = + resource_storage_->limiters_data_[hw_name][i]; + enforce_result |= limiters[i]->enforce(data.actual, data.limited, period); + resource_storage_->update_joint_limiters_commands( + data.limited, resource_storage_->command_interface_map_); + } + } + return enforce_result; +} + // CM API: Called in "update"-thread HardwareReadWriteStatus ResourceManager::read( const rclcpp::Time & time, const rclcpp::Duration & period) @@ -1919,7 +2060,10 @@ bool ResourceManager::state_interface_exists(const std::string & key) const return resource_storage_->state_interface_map_.find(key) != resource_storage_->state_interface_map_.end(); } - +const std::string & ResourceManager::get_urdf() const +{ + return resource_storage_->robot_description_; +} // END: "used only in tests and locally" rclcpp::Logger ResourceManager::get_logger() const { return resource_storage_->get_logger(); } diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index be9e672b3b..424d54e882 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -2158,6 +2158,217 @@ TEST_F(ResourceManagerTestAsyncReadWrite, test_components_with_async_components_ check_read_and_write_cycles(false); } +class ResourceManagerTestCommandLimitEnforcement : public ResourceManagerTest +{ +public: + void setup_resource_manager_and_do_initial_checks() + { + rm = std::make_shared( + node_, ros2_control_test_assets::minimal_robot_urdf, false); + rm->import_joint_limiters(ros2_control_test_assets::minimal_robot_urdf); + activate_components(*rm); + + cm_update_rate_ = 100u; // The default value inside + time = node_.get_clock()->now(); + + auto status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + EXPECT_EQ( + status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + + claimed_itfs.push_back( + rm->claim_command_interface(TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES[0])); + claimed_itfs.push_back(rm->claim_command_interface(TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES[0])); + + state_itfs.push_back(rm->claim_state_interface(TEST_ACTUATOR_HARDWARE_STATE_INTERFACES[1])); + state_itfs.push_back(rm->claim_state_interface(TEST_SYSTEM_HARDWARE_STATE_INTERFACES[1])); + + check_if_interface_available(true, true); + // with default values read and write should run without any problems + { + auto [ok, failed_hardware_names] = rm->read(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + } + { + claimed_itfs[0].set_value(10.0); + claimed_itfs[1].set_value(20.0); + auto [ok, failed_hardware_names] = rm->write(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + } + + time = time + duration; + check_if_interface_available(true, true); + } + + // check if all interfaces are available + void check_if_interface_available(const bool actuator_interfaces, const bool system_interfaces) + { + for (const auto & interface : TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES) + { + EXPECT_EQ(rm->command_interface_is_available(interface), actuator_interfaces); + } + for (const auto & interface : TEST_ACTUATOR_HARDWARE_STATE_INTERFACES) + { + EXPECT_EQ(rm->state_interface_is_available(interface), actuator_interfaces); + } + for (const auto & interface : TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES) + { + EXPECT_EQ(rm->command_interface_is_available(interface), system_interfaces); + } + for (const auto & interface : TEST_SYSTEM_HARDWARE_STATE_INTERFACES) + { + EXPECT_EQ(rm->state_interface_is_available(interface), system_interfaces); + } + }; + + void check_limit_enforcement() + { + { + auto [ok, failed_hardware_names] = rm->read(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + + claimed_itfs[0].set_value(10.0); + claimed_itfs[1].set_value(-20.0); + + auto [ok_write, failed_hardware_names_write] = rm->write(time, duration); + EXPECT_TRUE(ok_write); + EXPECT_TRUE(failed_hardware_names_write.empty()); + node_.get_clock()->sleep_until(time + duration); + time = node_.get_clock()->now(); + } + for (size_t i = 1; i < 100; i++) + { + // read now and check that without limit enforcement the values are half of command as this is + // the logic implemented in test components + auto [ok, failed_hardware_names] = rm->read(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + + EXPECT_EQ(state_itfs[0].get_value(), 5.0); + EXPECT_EQ(state_itfs[1].get_value(), -10.0); + auto [ok_write, failed_hardware_names_write] = rm->write(time, duration); + EXPECT_TRUE(ok_write); + EXPECT_TRUE(failed_hardware_names_write.empty()); + node_.get_clock()->sleep_until(time + duration); + time = node_.get_clock()->now(); + } + + // Let's enforce for one loop and then run the read and write again and reset interfaces to zero + // state + { + auto [ok, failed_hardware_names] = rm->read(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + + EXPECT_EQ(state_itfs[0].get_value(), 5.0); + EXPECT_EQ(state_itfs[1].get_value(), -10.0); + + claimed_itfs[0].set_value(0.0); + claimed_itfs[1].set_value(0.0); + EXPECT_EQ(claimed_itfs[0].get_value(), 0.0); + EXPECT_EQ(claimed_itfs[1].get_value(), 0.0); + + // enforcing limits + rm->enforce_command_limits(duration); + + EXPECT_EQ(claimed_itfs[0].get_value(), 0.0); + EXPECT_EQ(claimed_itfs[1].get_value(), 0.0); + + auto [ok_write, failed_hardware_names_write] = rm->write(time, duration); + EXPECT_TRUE(ok_write); + EXPECT_TRUE(failed_hardware_names_write.empty()); + node_.get_clock()->sleep_until(time + duration); + time = node_.get_clock()->now(); + + auto [read_ok, failed_hardware_names_read] = rm->read(time, duration); + EXPECT_TRUE(read_ok); + EXPECT_TRUE(failed_hardware_names_read.empty()); + } + + double new_state_value_1 = state_itfs[0].get_value(); + double new_state_value_2 = state_itfs[1].get_value(); + // Now loop and see that the joint limits are being enforced progressively + for (size_t i = 1; i < 300; i++) + { + // let's amplifiy the limit enforce period, to test more rapidly. It would work with 0.01s as + // well + const rclcpp::Duration enforce_period = + rclcpp::Duration::from_seconds(duration.seconds() * 10.0); + + auto [ok, failed_hardware_names] = rm->read(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + + EXPECT_EQ(state_itfs[0].get_value(), new_state_value_1); + EXPECT_EQ(state_itfs[1].get_value(), new_state_value_2); + + claimed_itfs[0].set_value(10.0); + claimed_itfs[1].set_value(-20.0); + EXPECT_EQ(claimed_itfs[0].get_value(), 10.0); + EXPECT_EQ(claimed_itfs[1].get_value(), -20.0); + + // enforcing limits + rm->enforce_command_limits(enforce_period); + + // the joint limits value is same as in the parsed URDF + const double velocity_joint_1 = 0.2; + EXPECT_NEAR( + claimed_itfs[0].get_value(), + std::min((velocity_joint_1 * (enforce_period.seconds() * static_cast(i))), M_PI), + 1.0e-8) + << "This should be progressively increasing as it is a position limit for iteration : " + << i; + EXPECT_NEAR(claimed_itfs[1].get_value(), -0.2, 1.0e-8) + << "This should be -0.2 as it is velocity limit"; + + // This is as per the logic of the test components internally + new_state_value_1 = claimed_itfs[0].get_value() / 2.0; + new_state_value_2 = claimed_itfs[1].get_value() / 2.0; + + auto [ok_write, failed_hardware_names_write] = rm->write(time, duration); + EXPECT_TRUE(ok_write); + EXPECT_TRUE(failed_hardware_names_write.empty()); + node_.get_clock()->sleep_until(time + duration); + time = node_.get_clock()->now(); + } + { + auto [ok, failed_hardware_names] = rm->read(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + + EXPECT_NEAR(state_itfs[0].get_value(), M_PI_2, 0.00001); + EXPECT_NEAR(state_itfs[1].get_value(), -0.1, 0.00001); + } + } + +public: + std::shared_ptr rm; + unsigned int actuator_rw_rate_, system_rw_rate_, cm_update_rate_; + std::vector claimed_itfs; + std::vector state_itfs; + + rclcpp::Time time = rclcpp::Time(1657232, 0); + const rclcpp::Duration duration = rclcpp::Duration::from_seconds(0.01); + + // values to set to hardware to simulate failure on read and write +}; + +TEST_F(ResourceManagerTestCommandLimitEnforcement, test_command_interfaces_limit_enforcement) +{ + setup_resource_manager_and_do_initial_checks(); + + check_limit_enforcement(); +} + int main(int argc, char ** argv) { rclcpp::init(argc, argv); diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp index 1744a806bc..09a360104d 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp @@ -324,6 +324,41 @@ const auto urdf_head_continuous_missing_limits = + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + )"; const auto urdf_head_continuous_with_limits = @@ -2030,6 +2065,8 @@ const auto diff_drive_robot_sdf = const auto minimal_robot_urdf = std::string(urdf_head) + std::string(hardware_resources) + std::string(urdf_tail); +const auto minimal_robot_urdf_no_limits = std::string(urdf_head_continuous_missing_limits) + + std::string(hardware_resources) + std::string(urdf_tail); const auto minimal_async_robot_urdf = std::string(urdf_head) + std::string(async_hardware_resources) + std::string(urdf_tail); const auto minimal_robot_urdf_with_different_hw_rw_rate =