From e3a4acf5dc219b48caede643152a3a768b292774 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 3 Sep 2024 16:23:32 +0200 Subject: [PATCH 01/14] update extractPID to pass the PID struct as reference for configuring it --- .../gazebo_ros2_control/gazebo_system.hpp | 6 ++--- gazebo_ros2_control/src/gazebo_system.cpp | 23 ++++++++++++------- 2 files changed, 18 insertions(+), 11 deletions(-) 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..1c848d78 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,9 @@ 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); /// \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 5d64604a..0b413e93 100644 --- a/gazebo_ros2_control/src/gazebo_system.cpp +++ b/gazebo_ros2_control/src/gazebo_system.cpp @@ -170,9 +170,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; @@ -181,20 +181,24 @@ 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; } @@ -226,7 +230,8 @@ control_toolbox::Pid GazeboSystem::extractPID( << " kd = " << kd << "\t" << " max_integral_error = " << max_integral_error); - return control_toolbox::Pid(kp, ki, kd, max_integral_error, min_integral_error, antiwindup); + pid.initPid(kp, ki, kd, max_integral_error, min_integral_error, antiwindup); + return are_pids_set; } void GazeboSystem::registerJoints( @@ -362,8 +367,9 @@ void GazeboSystem::registerJoints( << 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); + this->dataPtr->is_pos_pid[j] = this->extractPID( + POSITION_PID_PARAMS_PREFIX, joint_info, + this->dataPtr->pos_pid[j]); } else { this->dataPtr->is_pos_pid[j] = false; } @@ -396,8 +402,9 @@ void GazeboSystem::registerJoints( << 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->dataPtr->command_interfaces_.emplace_back( joint_name, From e90181503506d50a393567d50d351096d6dae8a2 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 3 Sep 2024 16:55:57 +0200 Subject: [PATCH 02/14] display properly the error on position and position_pid interface --- gazebo_ros2_control/src/gazebo_system.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gazebo_ros2_control/src/gazebo_system.cpp b/gazebo_ros2_control/src/gazebo_system.cpp index 0b413e93..8d3b3b6b 100644 --- a/gazebo_ros2_control/src/gazebo_system.cpp +++ b/gazebo_ros2_control/src/gazebo_system.cpp @@ -357,8 +357,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; From bb34d213cbede2498d0853694f1d1176db29e9d1 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 3 Sep 2024 18:51:43 +0200 Subject: [PATCH 03/14] Only if the PIDs are set, check for other entities --- gazebo_ros2_control/src/gazebo_system.cpp | 44 +++++++++++++++-------- 1 file changed, 29 insertions(+), 15 deletions(-) diff --git a/gazebo_ros2_control/src/gazebo_system.cpp b/gazebo_ros2_control/src/gazebo_system.cpp index 8d3b3b6b..2cdb09ec 100644 --- a/gazebo_ros2_control/src/gazebo_system.cpp +++ b/gazebo_ros2_control/src/gazebo_system.cpp @@ -203,25 +203,39 @@ bool GazeboSystem::extractPID( 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(), From ca3ae4c3e02778c63da843082d51a48ade394c48 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 3 Sep 2024 18:59:57 +0200 Subject: [PATCH 04/14] make joint_name const --- gazebo_ros2_control/src/gazebo_system.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gazebo_ros2_control/src/gazebo_system.cpp b/gazebo_ros2_control/src/gazebo_system.cpp index 2cdb09ec..f477884c 100644 --- a/gazebo_ros2_control/src/gazebo_system.cpp +++ b/gazebo_ros2_control/src/gazebo_system.cpp @@ -270,7 +270,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); From 79ba7fc3aa241739b81e540bfd14ddebffb92dc9 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 3 Sep 2024 19:01:20 +0200 Subject: [PATCH 05/14] print proper true or false regarding hold_joint --- gazebo_ros2_control/src/gazebo_system.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/gazebo_ros2_control/src/gazebo_system.cpp b/gazebo_ros2_control/src/gazebo_system.cpp index f477884c..6d003e0c 100644 --- a/gazebo_ros2_control/src/gazebo_system.cpp +++ b/gazebo_ros2_control/src/gazebo_system.cpp @@ -157,7 +157,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); From 5a4915d1ecfa82b54ff0130b20bb95a75b314e40 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 3 Sep 2024 19:02:14 +0200 Subject: [PATCH 06/14] Add support for extracting PID parameter from the loaded parameters --- .../gazebo_ros2_control/gazebo_system.hpp | 3 + gazebo_ros2_control/src/gazebo_system.cpp | 119 ++++++++++++++---- 2 files changed, 95 insertions(+), 27 deletions(-) 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 1c848d78..16064210 100644 --- a/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system.hpp +++ b/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system.hpp @@ -94,6 +94,9 @@ class GazeboSystem : public GazeboSystemInterface 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 6d003e0c..c435e2e9 100644 --- a/gazebo_ros2_control/src/gazebo_system.cpp +++ b/gazebo_ros2_control/src/gazebo_system.cpp @@ -110,6 +110,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 @@ -238,14 +244,70 @@ bool GazeboSystem::extractPID( 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); - pid.initPid(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; } @@ -381,12 +443,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] = this->extractPID( - POSITION_PID_PARAMS_PREFIX, joint_info, - this->dataPtr->pos_pid[j]); - } 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( @@ -416,11 +481,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] = this->extractPID( - VELOCITY_PID_PARAMS_PREFIX, joint_info, - this->dataPtr->vel_pid[j]); + 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, hardware_interface::HW_IF_VELOCITY, @@ -663,19 +734,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)) { From cdcc6ca21a1d06cd592b1cb50767dd3142a1fa2c Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 4 Sep 2024 10:44:45 +0200 Subject: [PATCH 07/14] add cmath header --- gazebo_ros2_control/src/gazebo_system.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/gazebo_ros2_control/src/gazebo_system.cpp b/gazebo_ros2_control/src/gazebo_system.cpp index c435e2e9..f53d19d2 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 From fcfb0771489438b9310123db7a026a1bf308db05 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 4 Sep 2024 10:45:13 +0200 Subject: [PATCH 08/14] add example on how to parse the PIDs via parameters file --- doc/index.rst | 33 +++++++ .../cart_controller_position_with_pids.yaml | 20 +++++ ...rt_example_position_pids_in_yaml.launch.py | 85 +++++++++++++++++++ ...ical_cart_position_pids_in_yaml.xacro.urdf | 76 +++++++++++++++++ 4 files changed, 214 insertions(+) create mode 100644 gazebo_ros2_control_demos/config/cart_controller_position_with_pids.yaml create mode 100644 gazebo_ros2_control_demos/launch/vertical_cart_example_position_pids_in_yaml.launch.py create mode 100644 gazebo_ros2_control_demos/urdf/test_vertical_cart_position_pids_in_yaml.xacro.urdf diff --git a/doc/index.rst b/doc/index.rst index c0aab03b..1edb076f 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -185,6 +185,39 @@ 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 definiting 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: + slider_to_cart: {kp: 100.0, kd: 10.0, ki: 1.0, max_integral_error: 10000.0} + + +.. code-block:: yaml + + gazebo_ros2_control: + ros__parameters: + pid_gains: + position_pid: + slider_to_cart: {kp: 100.0, kd: 10.0, ki: 1.0, max_integral_error: 10000.0} + +.. code-block:: xml + + + + ... + + --ros-args + --params-file + + + + + + Add the gazebo_ros2_control plugin ========================================== 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..32deeeb2 --- /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} \ No newline at end of file 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 + + + From be3761f5b3370c3d47cb61609eeea61818637089 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 4 Sep 2024 10:46:39 +0200 Subject: [PATCH 09/14] add precommit changes --- .../config/cart_controller_position_with_pids.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 index 32deeeb2..2caee08c 100644 --- a/gazebo_ros2_control_demos/config/cart_controller_position_with_pids.yaml +++ b/gazebo_ros2_control_demos/config/cart_controller_position_with_pids.yaml @@ -17,4 +17,4 @@ 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} \ No newline at end of file + slider_to_cart: {kp: 100.0, kd: 10.0, ki: 1.0, max_integral_error: 10000.0} From 5a957d37676797fc25a35cb8110804f0a003eb54 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 4 Sep 2024 10:49:02 +0200 Subject: [PATCH 10/14] change the text inside the docs --- doc/index.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/index.rst b/doc/index.rst index 1edb076f..ee05afd3 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -212,7 +212,7 @@ The PID parameters can be defined for ``position`` or ``position_pid`` and ``vel --ros-args --params-file - + Path to the configuration file From 779f1b077f305533cc53e349417d7b8af96a17dc Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 5 Sep 2024 18:47:00 +0200 Subject: [PATCH 11/14] Add proper indentation to the parameters MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Christoph Fröhlich --- doc/index.rst | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/doc/index.rst b/doc/index.rst index ee05afd3..9fd27833 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -190,19 +190,19 @@ The PID parameters can be defined for ``position`` or ``position_pid`` and ``vel .. 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} + ros__parameters: + pid_gains: + position: + slider_to_cart: {kp: 100.0, kd: 10.0, ki: 1.0, max_integral_error: 10000.0} .. code-block:: yaml gazebo_ros2_control: - ros__parameters: - pid_gains: - position_pid: - slider_to_cart: {kp: 100.0, kd: 10.0, ki: 1.0, max_integral_error: 10000.0} + ros__parameters: + pid_gains: + position_pid: + slider_to_cart: {kp: 100.0, kd: 10.0, ki: 1.0, max_integral_error: 10000.0} .. code-block:: xml From 98e91d59905a7bddbc272968012edf8d97f0d805 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 5 Sep 2024 18:50:55 +0200 Subject: [PATCH 12/14] update the docs of the demos section --- doc/index.rst | 24 +++++++++++++----------- 1 file changed, 13 insertions(+), 11 deletions(-) diff --git a/doc/index.rst b/doc/index.rst index 9fd27833..182a4152 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -185,23 +185,14 @@ 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 definiting them in a yaml file and loading it in the ``gazebo_ros2_control`` plugin as below: +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: - slider_to_cart: {kp: 100.0, kd: 10.0, ki: 1.0, max_integral_error: 10000.0} - - -.. code-block:: yaml - - gazebo_ros2_control: - ros__parameters: - pid_gains: - position_pid: + position_pid: # (or) position slider_to_cart: {kp: 100.0, kd: 10.0, ki: 1.0, max_integral_error: 10000.0} .. code-block:: xml @@ -419,6 +410,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 @@ -426,3 +418,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} \ No newline at end of file From 08c2d0b173b109116e93b2749fff1d9abde53149 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 5 Sep 2024 18:53:23 +0200 Subject: [PATCH 13/14] fix the formatting indent --- doc/index.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/index.rst b/doc/index.rst index 182a4152..3c3a33a6 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -427,4 +427,4 @@ The ``vertical_cart_example_position_pids_in_yaml.launch.py`` example uses a yam ros__parameters: pid_gains: position: - slider_to_cart: {kp: 100.0, kd: 10.0, ki: 1.0, max_integral_error: 10000.0} \ No newline at end of file + slider_to_cart: {kp: 100.0, kd: 10.0, ki: 1.0, max_integral_error: 10000.0} From 49cd4257036892420c6c6ea1cf2971c6cfa05c8a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 16 Sep 2024 18:50:50 +0200 Subject: [PATCH 14/14] Change to YAML MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Alejandro Hernández Cordero --- doc/index.rst | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/doc/index.rst b/doc/index.rst index 3c3a33a6..cc3b17fb 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -185,7 +185,7 @@ 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: +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 @@ -419,7 +419,7 @@ 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: +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