From 057e41db5b018c30582a8f44b6b4c8425fd5ceb7 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 22 Aug 2024 11:54:53 +0000 Subject: [PATCH 1/4] Fix position_proportional_gain and update docs --- doc/index.rst | 1 + .../src/gz_ros2_control_plugin.cpp | 30 +++++++++++++++++++ gz_ros2_control/src/gz_system.cpp | 30 ++++++++++++++----- 3 files changed, 54 insertions(+), 7 deletions(-) diff --git a/doc/index.rst b/doc/index.rst index 149cd734..f7fbcf09 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -141,6 +141,7 @@ The *gz_ros2_control* ```` tag also has the following optional child ele * ````: 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``) +* ````: Set the proportional gain. (default: 0.1) This determines the setpoint for a position-controlled joint ``joint_velocity = joint_position_error * position_proportional_gain``. Additionally, one can specify a namespace and remapping rules, which will be forwarded to the controller_manager and loaded controllers. Add the following ```` section: diff --git a/gz_ros2_control/src/gz_ros2_control_plugin.cpp b/gz_ros2_control/src/gz_ros2_control_plugin.cpp index b476fb50..a48e460e 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,31 @@ 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..a9d6733b 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( From 6250b4ababaec43fa1d0c5f080f9dbe3efb500f0 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 22 Aug 2024 11:54:53 +0000 Subject: [PATCH 2/4] Fix format --- gz_ros2_control/src/gz_ros2_control_plugin.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/gz_ros2_control/src/gz_ros2_control_plugin.cpp b/gz_ros2_control/src/gz_ros2_control_plugin.cpp index a48e460e..3767bd56 100644 --- a/gz_ros2_control/src/gz_ros2_control_plugin.cpp +++ b/gz_ros2_control/src/gz_ros2_control_plugin.cpp @@ -406,27 +406,28 @@ void GazeboSimROS2ControlPlugin::Configure( } try { - this->dataPtr->node_->declare_parameter("position_proportional_gain", - rclcpp::ParameterValue(position_proportional_gain)); + 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", + "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", + "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", + "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", + "Parameter 'position_proportional_gain' value has wrong type, %s", e.what()); } From ff2d7583a3eb74401e36ec4bbefe1e41d8d227ec Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 22 Aug 2024 12:02:57 +0000 Subject: [PATCH 3/4] Update docs with ROS parameter settings --- doc/index.rst | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/doc/index.rst b/doc/index.rst index f7fbcf09..b8401f8f 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -139,10 +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 From 676ed97dddbf7bb5fd509481537be46595529e52 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 22 Aug 2024 12:03:58 +0000 Subject: [PATCH 4/4] Fix format --- gz_ros2_control/src/gz_system.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gz_ros2_control/src/gz_system.cpp b/gz_ros2_control/src/gz_system.cpp index a9d6733b..d331cd27 100644 --- a/gz_ros2_control/src/gz_system.cpp +++ b/gz_ros2_control/src/gz_system.cpp @@ -209,21 +209,21 @@ bool GazeboSimSystem::initSim( "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_); + "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_); + "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_); + "Using default value: " << this->dataPtr->position_proportional_gain_); } RCLCPP_INFO_STREAM(