From 4d6234ae24d863c8eafe23ce145573563c437aad Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Mon, 8 Jul 2024 10:08:34 +0200 Subject: [PATCH] Add `hold_joints` parameter (backport #251) (#354) Co-authored-by: Christoph Froehlich --- doc/index.rst | 1 + .../src/gazebo_ros2_control_plugin.cpp | 23 +++++++++++++ gazebo_ros2_control/src/gazebo_system.cpp | 33 ++++++++++++++++--- 3 files changed, 52 insertions(+), 5 deletions(-) diff --git a/doc/index.rst b/doc/index.rst index 09d92547..af925542 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -213,6 +213,7 @@ The *gazebo_ros2_control* ```` tag also has the following optional child * ````: The location of the ``robot_description`` (URDF) on the parameter server, defaults to ``robot_description`` * ````: Name of the node where the ``robot_param`` is located, defaults to ``robot_state_publisher`` * ````: YAML file with the configuration of the controllers +* ````: 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. Default gazebo_ros2_control Behavior ----------------------------------------------------------- diff --git a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp index c77293fe..77a627af 100644 --- a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp +++ b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp @@ -184,6 +184,12 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element } else { impl_->robot_description_node_ = "robot_state_publisher"; // default } + // Hold joints if no control mode is active? + bool hold_joints = true; // default + if (sdf->HasElement("hold_joints")) { + hold_joints = + sdf->GetElement("hold_joints")->Get(); + } // Read urdf from ros parameter server std::string urdf_string; @@ -326,6 +332,23 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element RCLCPP_DEBUG( impl_->model_nh_->get_logger(), "Loaded hardware interface %s!", robot_hw_sim_type_str_.c_str()); + try { + node_ros2->declare_parameter("hold_joints", rclcpp::ParameterValue(hold_joints)); + } catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException & e) { + RCLCPP_ERROR( + impl_->model_nh_->get_logger(), "Parameter 'hold_joints' has already been declared, %s", + e.what()); + } catch (const rclcpp::exceptions::InvalidParametersException & e) { + RCLCPP_ERROR( + impl_->model_nh_->get_logger(), "Parameter 'hold_joints' has invalid name, %s", e.what()); + } catch (const rclcpp::exceptions::InvalidParameterValueException & e) { + RCLCPP_ERROR( + impl_->model_nh_->get_logger(), "Parameter 'hold_joints' value is invalid, %s", e.what()); + } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { + RCLCPP_ERROR( + impl_->model_nh_->get_logger(), "Parameter 'hold_joints' value has wrong type, %s", + e.what()); + } if (!gazeboSystem->initSim( node_ros2, impl_->parent_model_, diff --git a/gazebo_ros2_control/src/gazebo_system.cpp b/gazebo_ros2_control/src/gazebo_system.cpp index 44b67df4..8d61c7c8 100644 --- a/gazebo_ros2_control/src/gazebo_system.cpp +++ b/gazebo_ros2_control/src/gazebo_system.cpp @@ -116,6 +116,9 @@ class gazebo_ros2_control::GazeboSystemPrivate /// \brief mapping of mimicked joints to index of joint they mimic std::vector mimic_joints_; + + // Should hold the joints if no control_mode is active + bool hold_joints_ = true; }; namespace gazebo_ros2_control @@ -141,6 +144,30 @@ bool GazeboSystem::initSim( return false; } + try { + this->dataPtr->hold_joints_ = this->nh_->get_parameter("hold_joints").as_bool(); + } catch (rclcpp::exceptions::ParameterUninitializedException & ex) { + RCLCPP_ERROR( + this->nh_->get_logger(), + "Parameter 'hold_joints' not initialized, with error %s", ex.what()); + RCLCPP_WARN_STREAM( + this->nh_->get_logger(), "Using default value: " << this->dataPtr->hold_joints_); + } catch (rclcpp::exceptions::ParameterNotDeclaredException & ex) { + RCLCPP_ERROR( + this->nh_->get_logger(), + "Parameter 'hold_joints' not declared, with error %s", ex.what()); + RCLCPP_WARN_STREAM( + this->nh_->get_logger(), "Using default value: " << this->dataPtr->hold_joints_); + } catch (rclcpp::ParameterTypeException & ex) { + RCLCPP_ERROR( + this->nh_->get_logger(), + "Parameter 'hold_joints' has wrong type: %s", ex.what()); + RCLCPP_WARN_STREAM( + 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); + registerJoints(hardware_info, parent_model); registerSensors(hardware_info, parent_model); @@ -739,25 +766,21 @@ hardware_interface::return_type GazeboSystem::write( double cmd = this->dataPtr->joint_position_cmd_[j]; this->dataPtr->sim_joints_[j]->SetPosition(0, cmd, true); this->dataPtr->sim_joints_[j]->SetVelocity(0, 0.0); - } else if (this->dataPtr->joint_control_methods_[j] & VELOCITY) { this->dataPtr->sim_joints_[j]->SetVelocity(0, this->dataPtr->joint_velocity_cmd_[j]); - } else if (this->dataPtr->joint_control_methods_[j] & EFFORT) { this->dataPtr->sim_joints_[j]->SetForce(0, this->dataPtr->joint_effort_cmd_[j]); - } else if (this->dataPtr->joint_control_methods_[j] & VELOCITY_PID) { double vel_goal = this->dataPtr->joint_velocity_cmd_[j]; double vel = this->dataPtr->sim_joints_[j]->GetVelocity(0); double cmd = this->dataPtr->vel_pid[j].computeCommand(vel_goal - vel, dt); this->dataPtr->sim_joints_[j]->SetForce(0, cmd); - } else if (this->dataPtr->joint_control_methods_[j] & POSITION_PID) { double pos_goal = this->dataPtr->joint_position_cmd_[j]; double pos = this->dataPtr->sim_joints_[j]->Position(0); double cmd = this->dataPtr->pos_pid[j].computeCommand(pos_goal - pos, dt); this->dataPtr->sim_joints_[j]->SetForce(0, cmd); - } else if (this->dataPtr->is_joint_actuated_[j]) { + } else if (this->dataPtr->is_joint_actuated_[j] && this->dataPtr->hold_joints_) { // Fallback case is a velocity command of zero (only for actuated joints) this->dataPtr->sim_joints_[j]->SetVelocity(0, 0.0); }