From 8cecc69b7aa698dfd996ae545c186a42b6799d87 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 26 Aug 2024 11:43:22 +0200 Subject: [PATCH] Parse `position_proportional_gain` parameter from URDF and update docs (#393) 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 | 15 ++++++++- .../src/gz_ros2_control_plugin.cpp | 31 +++++++++++++++++++ gz_ros2_control/src/gz_system.cpp | 30 +++++++++++++----- 3 files changed, 68 insertions(+), 8 deletions(-) diff --git a/doc/index.rst b/doc/index.rst index 149cd734..b8401f8f 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -139,9 +139,22 @@ robot hardware interfaces between *ros2_control* and Gazebo. The *gz_ros2_control* ```` tag also has the following optional child elements: * ````: A YAML file with the configuration of the controllers. This element can be given multiple times to load multiple files. -* ````: if set to true (default), it will hold the joints' position if their interface was not claimed, e.g., the controller hasn't been activated yet. * ````: Set controller manager name (default: ``controller_manager``) +The following additional parameters can be set via child elements in the URDF or via ROS parameters in the YAML file above: + +* ````: if set to true (default), it will hold the joints' position if their interface was not claimed, e.g., the controller hasn't been activated yet. +* ````: Set the proportional gain. (default: 0.1) This determines the setpoint for a position-controlled joint ``joint_velocity = joint_position_error * position_proportional_gain``. + +or via ROS parameters: + +.. code-block:: yaml + + gz_ros_control: + ros__parameters: + hold_joints: false + position_proportional_gain: 0.5 + Additionally, one can specify a namespace and remapping rules, which will be forwarded to the controller_manager and loaded controllers. Add the following ```` section: .. code-block:: xml diff --git a/gz_ros2_control/src/gz_ros2_control_plugin.cpp b/gz_ros2_control/src/gz_ros2_control_plugin.cpp index ff26e583..14e594ca 100644 --- a/gz_ros2_control/src/gz_ros2_control_plugin.cpp +++ b/gz_ros2_control/src/gz_ros2_control_plugin.cpp @@ -315,6 +315,11 @@ void GazeboSimROS2ControlPlugin::Configure( hold_joints = sdfPtr->GetElement("hold_joints")->Get(); } + double position_proportional_gain = 0.1; // default + if (sdfPtr->HasElement("position_proportional_gain")) { + position_proportional_gain = + sdfPtr->GetElement("position_proportional_gain")->Get(); + } if (sdfPtr->HasElement("ros")) { sdf::ElementPtr sdfRos = sdfPtr->GetElement("ros"); @@ -400,6 +405,32 @@ void GazeboSimROS2ControlPlugin::Configure( e.what()); } + try { + this->dataPtr->node_->declare_parameter( + "position_proportional_gain", + rclcpp::ParameterValue(position_proportional_gain)); + } catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException & e) { + RCLCPP_ERROR( + this->dataPtr->node_->get_logger(), + "Parameter 'position_proportional_gain' has already been declared, %s", + e.what()); + } catch (const rclcpp::exceptions::InvalidParametersException & e) { + RCLCPP_ERROR( + this->dataPtr->node_->get_logger(), + "Parameter 'position_proportional_gain' has invalid name, %s", + e.what()); + } catch (const rclcpp::exceptions::InvalidParameterValueException & e) { + RCLCPP_ERROR( + this->dataPtr->node_->get_logger(), + "Parameter 'position_proportional_gain' value is invalid, %s", + e.what()); + } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { + RCLCPP_ERROR( + this->dataPtr->node_->get_logger(), + "Parameter 'position_proportional_gain' value has wrong type, %s", + e.what()); + } + std::unique_ptr resource_manager_ = std::make_unique(this->dataPtr->node_, _ecm, enabledJoints); diff --git a/gz_ros2_control/src/gz_system.cpp b/gz_ros2_control/src/gz_system.cpp index 2f48f954..d331cd27 100644 --- a/gz_ros2_control/src/gz_system.cpp +++ b/gz_ros2_control/src/gz_system.cpp @@ -200,14 +200,30 @@ bool GazeboSimSystem::initSim( this->dataPtr->joints_.resize(this->dataPtr->n_dof_); - constexpr double default_gain = 0.1; - try { - this->dataPtr->position_proportional_gain_ = this->nh_->declare_parameter( - "position_proportional_gain", default_gain); - } catch (rclcpp::exceptions::ParameterAlreadyDeclaredException & ex) { - this->nh_->get_parameter( - "position_proportional_gain", this->dataPtr->position_proportional_gain_); + this->dataPtr->position_proportional_gain_ = + this->nh_->get_parameter("position_proportional_gain").as_double(); + } catch (rclcpp::exceptions::ParameterUninitializedException & ex) { + RCLCPP_ERROR( + this->nh_->get_logger(), + "Parameter 'position_proportional_gain' not initialized, with error %s", ex.what()); + RCLCPP_WARN_STREAM( + this->nh_->get_logger(), + "Using default value: " << this->dataPtr->position_proportional_gain_); + } catch (rclcpp::exceptions::ParameterNotDeclaredException & ex) { + RCLCPP_ERROR( + this->nh_->get_logger(), + "Parameter 'position_proportional_gain' not declared, with error %s", ex.what()); + RCLCPP_WARN_STREAM( + this->nh_->get_logger(), + "Using default value: " << this->dataPtr->position_proportional_gain_); + } catch (rclcpp::ParameterTypeException & ex) { + RCLCPP_ERROR( + this->nh_->get_logger(), + "Parameter 'position_proportional_gain' has wrong type: %s", ex.what()); + RCLCPP_WARN_STREAM( + this->nh_->get_logger(), + "Using default value: " << this->dataPtr->position_proportional_gain_); } RCLCPP_INFO_STREAM(