diff --git a/doc/index.rst b/doc/index.rst index 181480b3..81db92de 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -189,6 +189,30 @@ Where the parameters are as follows: The same definitions apply to the ``vel_*`` parameters. +The PID parameters can be defined for ``position`` or ``position_pid`` and ``velocity`` or ``velocity_pid`` command interfaces as explained above, or defining them in a YAML file and loading it in the ``gazebo_ros2_control`` plugin as below: + +.. code-block:: yaml + + gazebo_ros2_control: + ros__parameters: + pid_gains: + position_pid: # (or) position + slider_to_cart: {kp: 100.0, kd: 10.0, ki: 1.0, max_integral_error: 10000.0} + +.. code-block:: xml + + + + ... + + --ros-args + --params-file + Path to the configuration file + + + + + Add the gazebo_ros2_control plugin ========================================== @@ -392,6 +416,7 @@ The following examples shows a vertical cart control by a PID joint using positi .. code-block:: shell + ros2 launch gazebo_ros2_control_demos vertical_cart_example_position_pids_in_yaml.launch.py ros2 launch gazebo_ros2_control_demos vertical_cart_example_position_pid.launch.py ros2 launch gazebo_ros2_control_demos vertical_cart_example_velocity_pid.launch.py @@ -399,3 +424,13 @@ The following examples shows a vertical cart control by a PID joint using positi ros2 run gazebo_ros2_control_demos example_position_pid ros2 run gazebo_ros2_control_demos example_velocity + +The ``vertical_cart_example_position_pids_in_yaml.launch.py`` example uses a YAML file as following to set the PID gains: + +.. code-block:: yaml + + gazebo_ros2_control: + ros__parameters: + pid_gains: + position: + slider_to_cart: {kp: 100.0, kd: 10.0, ki: 1.0, max_integral_error: 10000.0} diff --git a/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system.hpp b/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system.hpp index 8473b9b6..16064210 100644 --- a/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system.hpp +++ b/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system.hpp @@ -90,9 +90,12 @@ class GazeboSystem : public GazeboSystemInterface const hardware_interface::HardwareInfo & hardware_info, gazebo::physics::ModelPtr parent_model); - control_toolbox::Pid extractPID( - std::string prefix, - hardware_interface::ComponentInfo joint_info); + bool extractPID( + const std::string & prefix, + const hardware_interface::ComponentInfo & joint_info, control_toolbox::Pid & pid); + + bool extractPIDFromParameters( + const std::string & control_mode, const std::string & joint_name, control_toolbox::Pid & pid); /// \brief Private data class std::unique_ptr dataPtr; diff --git a/gazebo_ros2_control/src/gazebo_system.cpp b/gazebo_ros2_control/src/gazebo_system.cpp index 8d61c7c8..3e71b1a9 100644 --- a/gazebo_ros2_control/src/gazebo_system.cpp +++ b/gazebo_ros2_control/src/gazebo_system.cpp @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include #include @@ -119,6 +120,12 @@ class gazebo_ros2_control::GazeboSystemPrivate // Should hold the joints if no control_mode is active bool hold_joints_ = true; + + // Current position and velocity control method + GazeboSystemInterface::ControlMethod_ position_control_method_ = + GazeboSystemInterface::ControlMethod_::POSITION; + GazeboSystemInterface::ControlMethod_ velocity_control_method_ = + GazeboSystemInterface::ControlMethod_::VELOCITY; }; namespace gazebo_ros2_control @@ -166,7 +173,8 @@ bool GazeboSystem::initSim( this->nh_->get_logger(), "Using default value: " << this->dataPtr->hold_joints_); } RCLCPP_DEBUG_STREAM( - this->nh_->get_logger(), "hold_joints (system): " << this->dataPtr->hold_joints_ << std::endl); + this->nh_->get_logger(), + "hold_joints (system): " << std::boolalpha << this->dataPtr->hold_joints_ << std::endl); registerJoints(hardware_info, parent_model); registerSensors(hardware_info, parent_model); @@ -179,9 +187,9 @@ bool GazeboSystem::initSim( return true; } -control_toolbox::Pid GazeboSystem::extractPID( - std::string prefix, - hardware_interface::ComponentInfo joint_info) +bool GazeboSystem::extractPID( + const std::string & prefix, + const hardware_interface::ComponentInfo & joint_info, control_toolbox::Pid & pid) { double kp; double ki; @@ -190,52 +198,127 @@ control_toolbox::Pid GazeboSystem::extractPID( double min_integral_error; bool antiwindup = false; + bool are_pids_set = false; if (joint_info.parameters.find(prefix + "kp") != joint_info.parameters.end()) { kp = std::stod(joint_info.parameters.at(prefix + "kp")); + are_pids_set = true; } else { kp = 0.0; } if (joint_info.parameters.find(prefix + "ki") != joint_info.parameters.end()) { ki = std::stod(joint_info.parameters.at(prefix + "ki")); + are_pids_set = true; } else { ki = 0.0; } if (joint_info.parameters.find(prefix + "kd") != joint_info.parameters.end()) { kd = std::stod(joint_info.parameters.at(prefix + "kd")); + are_pids_set = true; } else { kd = 0.0; } - if (joint_info.parameters.find(prefix + "max_integral_error") != joint_info.parameters.end()) { - max_integral_error = std::stod(joint_info.parameters.at(prefix + "max_integral_error")); - } else { - max_integral_error = std::numeric_limits::max(); - } + if (are_pids_set) { + if (joint_info.parameters.find(prefix + "max_integral_error") != joint_info.parameters.end()) { + max_integral_error = std::stod(joint_info.parameters.at(prefix + "max_integral_error")); + } else { + max_integral_error = std::numeric_limits::max(); + } - if (joint_info.parameters.find(prefix + "min_integral_error") != joint_info.parameters.end()) { - min_integral_error = std::stod(joint_info.parameters.at(prefix + "min_integral_error")); - } else { - min_integral_error = std::numeric_limits::min(); - } + if (joint_info.parameters.find(prefix + "min_integral_error") != joint_info.parameters.end()) { + min_integral_error = std::stod(joint_info.parameters.at(prefix + "min_integral_error")); + } else { + min_integral_error = std::numeric_limits::min(); + } - if (joint_info.parameters.find(prefix + "antiwindup") != joint_info.parameters.end()) { - if (joint_info.parameters.at(prefix + "antiwindup") == "true" || - joint_info.parameters.at(prefix + "antiwindup") == "True") - { - antiwindup = true; + if (joint_info.parameters.find(prefix + "antiwindup") != joint_info.parameters.end()) { + if (joint_info.parameters.at(prefix + "antiwindup") == "true" || + joint_info.parameters.at(prefix + "antiwindup") == "True") + { + antiwindup = true; + } } + + RCLCPP_INFO_STREAM( + this->nh_->get_logger(), + "Setting kp = " << kp << "\t" + << " ki = " << ki << "\t" + << " kd = " << kd << "\t" + << " max_integral_error = " << max_integral_error << "\t" + << "antiwindup =" << std::boolalpha << antiwindup); + + pid.initPid(kp, ki, kd, max_integral_error, min_integral_error, antiwindup); } + return are_pids_set; +} - RCLCPP_INFO_STREAM( - this->nh_->get_logger(), - "Setting kp = " << kp << "\t" - << " ki = " << ki << "\t" - << " kd = " << kd << "\t" - << " max_integral_error = " << max_integral_error); - return control_toolbox::Pid(kp, ki, kd, max_integral_error, min_integral_error, antiwindup); +bool GazeboSystem::extractPIDFromParameters( + const std::string & control_mode, const std::string & joint_name, + control_toolbox::Pid & pid) +{ + const std::string parameter_prefix = "pid_gains." + control_mode + "." + joint_name; + auto get_pid_entry = [this, parameter_prefix](const std::string & entry, double & value) -> bool { + try { + // Check if the parameter is declared, if not, declare the default value NaN + if (!this->nh_->has_parameter(parameter_prefix + "." + entry)) { + this->nh_->declare_parameter( + parameter_prefix + "." + entry, + std::numeric_limits::quiet_NaN()); + } + value = this->nh_->get_parameter(parameter_prefix + "." + entry).as_double(); + } catch (rclcpp::exceptions::ParameterNotDeclaredException & ex) { + RCLCPP_ERROR( + this->nh_->get_logger(), + "Parameter '%s' not declared, with error %s", entry.c_str(), ex.what()); + return false; + } catch (rclcpp::exceptions::InvalidParameterTypeException & ex) { + RCLCPP_ERROR( + this->nh_->get_logger(), + "Parameter '%s' has wrong type: %s", entry.c_str(), ex.what()); + return false; + } + return std::isfinite(value); + }; + bool are_pids_set = true; + double kp, ki, kd, max_integral_error, min_integral_error; + bool antiwindup; + are_pids_set &= get_pid_entry("kp", kp); + are_pids_set &= get_pid_entry("ki", ki); + are_pids_set &= get_pid_entry("kd", kd); + if (are_pids_set) { + get_pid_entry("max_integral_error", max_integral_error); + get_pid_entry("min_integral_error", min_integral_error); + const std::string antiwindup_str = "antiwindup"; + if (!this->nh_->has_parameter(parameter_prefix + "." + antiwindup_str)) { + this->nh_->declare_parameter( + parameter_prefix + "." + antiwindup_str, + false); + } + try { + antiwindup = this->nh_->get_parameter(parameter_prefix + "." + antiwindup_str).as_bool(); + } catch (rclcpp::exceptions::ParameterNotDeclaredException & ex) { + RCLCPP_ERROR( + this->nh_->get_logger(), + "Parameter '%s' not declared, with error %s", antiwindup_str.c_str(), ex.what()); + } catch (rclcpp::ParameterTypeException & ex) { + RCLCPP_ERROR( + this->nh_->get_logger(), + "Parameter '%s' has wrong type: %s", antiwindup_str.c_str(), ex.what()); + } + RCLCPP_INFO_STREAM( + this->nh_->get_logger(), + "Setting kp = " << kp << "\t" + << " ki = " << ki << "\t" + << " kd = " << kd << "\t" + << " max_integral_error = " << max_integral_error << "\t" + << "antiwindup =" << std::boolalpha << antiwindup << " from node parameters"); + pid.initPid(kp, ki, kd, max_integral_error, min_integral_error, antiwindup); + } + + return are_pids_set; } void GazeboSystem::registerJoints( @@ -260,7 +343,7 @@ void GazeboSystem::registerJoints( for (unsigned int j = 0; j < this->dataPtr->n_dof_; j++) { auto & joint_info = hardware_info.joints[j]; - std::string joint_name = this->dataPtr->joint_names_[j] = joint_info.name; + const std::string joint_name = this->dataPtr->joint_names_[j] = joint_info.name; gazebo::physics::JointPtr simjoint = parent_model->GetJoint(joint_name); this->dataPtr->sim_joints_.push_back(simjoint); @@ -377,8 +460,8 @@ void GazeboSystem::registerJoints( if (has_already_registered_pos) { RCLCPP_ERROR_STREAM( this->nh_->get_logger(), - "can't have position and position" - << "pid command_interface at same time !!!"); + "can't have position and position_pid" + << "command_interface at same time !!!"); continue; } has_already_registered_pos = true; @@ -386,11 +469,15 @@ void GazeboSystem::registerJoints( this->nh_->get_logger(), "\t\t " << joint_info.command_interfaces[i].name); - if (joint_info.command_interfaces[i].name == "position_pid") { - this->dataPtr->is_pos_pid[j] = true; - this->dataPtr->pos_pid[j] = this->extractPID(POSITION_PID_PARAMS_PREFIX, joint_info); - } else { - this->dataPtr->is_pos_pid[j] = false; + this->dataPtr->is_pos_pid[j] = this->extractPID( + POSITION_PID_PARAMS_PREFIX, joint_info, + this->dataPtr->pos_pid[j]) || this->extractPIDFromParameters( + joint_info.command_interfaces[i].name, joint_name, this->dataPtr->pos_pid[j]); + + if (this->dataPtr->is_pos_pid[j]) { + RCLCPP_INFO_STREAM( + this->nh_->get_logger(), "Found position PIDs for joint: " << joint_name << "!"); + this->dataPtr->position_control_method_ = POSITION_PID; } this->dataPtr->command_interfaces_.emplace_back( @@ -420,10 +507,17 @@ void GazeboSystem::registerJoints( this->nh_->get_logger(), "\t\t " << joint_info.command_interfaces[i].name); - if (joint_info.command_interfaces[i].name == "velocity_pid") { - this->dataPtr->is_vel_pid[j] = true; - this->dataPtr->vel_pid[j] = this->extractPID(VELOCITY_PID_PARAMS_PREFIX, joint_info); + this->dataPtr->is_vel_pid[j] = this->extractPID( + VELOCITY_PID_PARAMS_PREFIX, joint_info, + this->dataPtr->vel_pid[j]) || this->extractPIDFromParameters( + joint_info.command_interfaces[i].name, joint_name, this->dataPtr->vel_pid[j]); + + if (this->dataPtr->is_vel_pid[j]) { + RCLCPP_INFO_STREAM( + this->nh_->get_logger(), "Found velocity PIDs for joint: " << joint_name << "!"); + this->dataPtr->velocity_control_method_ = VELOCITY_PID; } + this->dataPtr->command_interfaces_.emplace_back( joint_name + suffix, hardware_interface::HW_IF_VELOCITY, @@ -657,19 +751,13 @@ GazeboSystem::perform_command_mode_switch( if (interface_name == (this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_POSITION)) { - if (!this->dataPtr->is_pos_pid[j]) { - this->dataPtr->joint_control_methods_[j] |= POSITION; - } else { - this->dataPtr->joint_control_methods_[j] |= POSITION_PID; - } + this->dataPtr->joint_control_methods_[j] |= + static_cast(this->dataPtr->position_control_method_); } else if (interface_name == // NOLINT (this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_VELOCITY)) { - if (!this->dataPtr->is_vel_pid[j]) { - this->dataPtr->joint_control_methods_[j] |= VELOCITY; - } else { - this->dataPtr->joint_control_methods_[j] |= VELOCITY_PID; - } + this->dataPtr->joint_control_methods_[j] |= + static_cast(this->dataPtr->velocity_control_method_); } else if (interface_name == // NOLINT (this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_EFFORT)) { diff --git a/gazebo_ros2_control_demos/config/cart_controller_position_with_pids.yaml b/gazebo_ros2_control_demos/config/cart_controller_position_with_pids.yaml new file mode 100644 index 00000000..2caee08c --- /dev/null +++ b/gazebo_ros2_control_demos/config/cart_controller_position_with_pids.yaml @@ -0,0 +1,20 @@ +controller_manager: + ros__parameters: + update_rate: 100 # Hz + + position_controller: + type: position_controllers/JointGroupPositionController + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + +position_controller: + ros__parameters: + joints: + - slider_to_cart + +gazebo_ros2_control: + ros__parameters: + pid_gains: + position: + slider_to_cart: {kp: 100.0, kd: 10.0, ki: 1.0, max_integral_error: 10000.0} diff --git a/gazebo_ros2_control_demos/launch/vertical_cart_example_position_pids_in_yaml.launch.py b/gazebo_ros2_control_demos/launch/vertical_cart_example_position_pids_in_yaml.launch.py new file mode 100644 index 00000000..1f468df1 --- /dev/null +++ b/gazebo_ros2_control_demos/launch/vertical_cart_example_position_pids_in_yaml.launch.py @@ -0,0 +1,85 @@ +# Copyright 2024 Open Source Robotics Foundation, Inc. +# +# 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. + +import os + +from ament_index_python.packages import get_package_share_directory + + +from launch import LaunchDescription +from launch.actions import ExecuteProcess, IncludeLaunchDescription, RegisterEventHandler +from launch.event_handlers import OnProcessExit +from launch.launch_description_sources import PythonLaunchDescriptionSource + +from launch_ros.actions import Node + +import xacro + + +def generate_launch_description(): + gazebo = IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('gazebo_ros'), 'launch'), '/gazebo.launch.py']), + ) + + gazebo_ros2_control_demos_path = os.path.join( + get_package_share_directory('gazebo_ros2_control_demos')) + + xacro_file = os.path.join(gazebo_ros2_control_demos_path, + 'urdf', + 'test_vertical_cart_position_pids_in_yaml.xacro.urdf') + doc = xacro.parse(open(xacro_file)) + xacro.process_doc(doc) + params = {'robot_description': doc.toxml()} + + node_robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + output='screen', + parameters=[params] + ) + + spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py', + arguments=['-topic', 'robot_description', + '-entity', 'cart'], + output='screen') + + load_joint_state_broadcaster = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'joint_state_broadcaster'], + output='screen' + ) + + load_joint_trajectory_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 'position_controller'], + output='screen' + ) + + return LaunchDescription([ + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=spawn_entity, + on_exit=[load_joint_state_broadcaster], + ) + ), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=load_joint_state_broadcaster, + on_exit=[load_joint_trajectory_controller], + ) + ), + gazebo, + node_robot_state_publisher, + spawn_entity, + ]) diff --git a/gazebo_ros2_control_demos/urdf/test_vertical_cart_position_pids_in_yaml.xacro.urdf b/gazebo_ros2_control_demos/urdf/test_vertical_cart_position_pids_in_yaml.xacro.urdf new file mode 100644 index 00000000..aca7fffd --- /dev/null +++ b/gazebo_ros2_control_demos/urdf/test_vertical_cart_position_pids_in_yaml.xacro.urdf @@ -0,0 +1,76 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + gazebo_ros2_control/GazeboSystem + + + + + + + + + + + + + + + --ros-args + --params-file + $(find gazebo_ros2_control_demos)/config/cart_controller_position_with_pids.yaml + + $(find gazebo_ros2_control_demos)/config/cart_controller_position_with_pids.yaml + + +