Skip to content

Commit

Permalink
Add option for not holding joints if not claimed
Browse files Browse the repository at this point in the history
  • Loading branch information
christophfroehlich committed Dec 31, 2023
1 parent 3c8f5bb commit 5b7b7f1
Show file tree
Hide file tree
Showing 3 changed files with 27 additions and 1 deletion.
Original file line number Diff line number Diff line change
Expand Up @@ -88,6 +88,9 @@ class GazeboSystem : public GazeboSystemInterface

/// \brief Private data class
std::unique_ptr<GazeboSystemPrivate> dataPtr;

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

} // namespace gazebo_ros2_control
Expand Down
11 changes: 11 additions & 0 deletions gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -182,6 +182,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<bool>();
}

// There's currently no direct way to set parameters to the plugin's node
// So we have to parse the plugin file manually and set it to the node's context.
Expand Down Expand Up @@ -306,6 +312,11 @@ 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 std::exception & e) {
RCLCPP_ERROR(impl_->model_nh_->get_logger(), "%s", e.what());
}
if (!gazeboSystem->initSim(
node_ros2,
impl_->parent_model_,
Expand Down
14 changes: 13 additions & 1 deletion gazebo_ros2_control/src/gazebo_system.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -128,6 +128,18 @@ bool GazeboSystem::initSim(
return false;
}

try {
hold_joints_ = this->nh_->get_parameter("hold_joints").as_bool();
} catch (const std::exception & e) {
RCLCPP_WARN(
this->nh_->get_logger(),
"Parameter 'hold_joints' not found, with error %s", e.what());
RCLCPP_WARN_STREAM(
this->nh_->get_logger(), "Using default value: " << hold_joints_);
}
RCLCPP_DEBUG_STREAM(
this->nh_->get_logger(), "hold_joints (system): " << hold_joints_ << std::endl);

registerJoints(hardware_info, parent_model);
registerSensors(hardware_info, parent_model);

Expand Down Expand Up @@ -601,7 +613,7 @@ hardware_interface::return_type GazeboSystem::write(
this->dataPtr->sim_joints_[j]->SetVelocity(0, this->dataPtr->joint_velocity_cmd_[j]);
} else if (this->dataPtr->joint_control_methods_[j] & EFFORT) { // NOLINT
this->dataPtr->sim_joints_[j]->SetForce(0, this->dataPtr->joint_effort_cmd_[j]);
} else if (this->dataPtr->is_joint_actuated_[j]) {
} else if (this->dataPtr->is_joint_actuated_[j] && hold_joints_) {
// Fallback case is a velocity command of zero (only for actuated joints)
this->dataPtr->sim_joints_[j]->SetVelocity(0, 0.0);
}
Expand Down

0 comments on commit 5b7b7f1

Please sign in to comment.