Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Add hold_joints parameter #213

Merged
merged 2 commits into from
Jan 3, 2024
Merged

Add hold_joints parameter #213

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
64372df
Add hold_joints parameter
ahcorde Jan 3, 2024
6c5fb5e
make linters happy
ahcorde Jan 3, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions doc/index.rst
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -153,6 +153,7 @@ robot hardware interfaces between *ros2_control* and Gazebo.
The *gz_ros2_control* ``<plugin>`` tag also has the following optional child elements:

* ``<parameters>``: YAML file with the configuration of the controllers
* ``<hold_joints>``: 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 gz_ros2_control Behavior
-----------------------------------------------------------
Expand Down
28 changes: 28 additions & 0 deletions gz_ros2_control/src/gz_ros2_control_plugin.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -299,6 +299,13 @@ void GazeboSimROS2ControlPlugin::Configure(
std::string controllerManagerNodeName{"controller_manager"};
std::string ns = "/";

// Hold joints if no control mode is active?
bool hold_joints = true; // default
if (sdfPtr->HasElement("hold_joints")) {
hold_joints =
sdfPtr->GetElement("hold_joints")->Get<bool>();
}

if (sdfPtr->HasElement("ros")) {
sdf::ElementPtr sdfRos = sdfPtr->GetElement("ros");

Expand Down Expand Up @@ -417,6 +424,27 @@ void GazeboSimROS2ControlPlugin::Configure(
ex.what());
continue;
}

try {
this->dataPtr->node_->declare_parameter("hold_joints", rclcpp::ParameterValue(hold_joints));
} catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException & e) {
RCLCPP_ERROR(
this->dataPtr->node_->get_logger(), "Parameter 'hold_joints' has already been declared, %s",
e.what());
} catch (const rclcpp::exceptions::InvalidParametersException & e) {
RCLCPP_ERROR(
this->dataPtr->node_->get_logger(), "Parameter 'hold_joints' has invalid name, %s",
e.what());
} catch (const rclcpp::exceptions::InvalidParameterValueException & e) {
RCLCPP_ERROR(
this->dataPtr->node_->get_logger(), "Parameter 'hold_joints' value is invalid, %s",
e.what());
} catch (const rclcpp::exceptions::InvalidParameterTypeException & e) {
RCLCPP_ERROR(
this->dataPtr->node_->get_logger(), "Parameter 'hold_joints' value has wrong type, %s",
e.what());
}

if (!gzSimSystem->initSim(
this->dataPtr->node_,
enabledJoints,
Expand Down
30 changes: 29 additions & 1 deletion gz_ros2_control/src/gz_system.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -175,6 +175,9 @@ class gz_ros2_control::GazeboSimSystemPrivate

/// \brief Gain which converts position error to a velocity command
double position_proportional_gain_;

// Should hold the joints if no control_mode is active
bool hold_joints_ = true;
};

namespace gz_ros2_control
Expand All @@ -196,6 +199,31 @@ bool GazeboSimSystem::initSim(

this->dataPtr->update_rate = &update_rate;

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);


RCLCPP_DEBUG(this->nh_->get_logger(), "n_dof_ %lu", this->dataPtr->n_dof_);

this->dataPtr->joints_.resize(this->dataPtr->n_dof_);
Expand Down Expand Up @@ -658,7 +686,7 @@ hardware_interface::return_type GazeboSimSystem::write(
*jointEffortCmd = sim::components::JointForceCmd(
{this->dataPtr->joints_[i].joint_effort_cmd});
}
} else if (this->dataPtr->joints_[i].is_actuated) {
} else if (this->dataPtr->joints_[i].is_actuated && this->dataPtr->hold_joints_) {
// Fallback case is a velocity command of zero (only for actuated joints)
double target_vel = 0.0;
auto vel =
Expand Down