From 0a8d4464e15e4f13ea0673311c60a891d6a836ec Mon Sep 17 00:00:00 2001 From: Johannes Huemer Date: Sat, 16 Dec 2023 15:49:34 +0100 Subject: [PATCH] Fix stuck passive joints (#184) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Fix stuck passive joints * Update comment * Fix variable naming --------- Co-authored-by: Christoph Fröhlich --- gz_ros2_control/src/gz_system.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/gz_ros2_control/src/gz_system.cpp b/gz_ros2_control/src/gz_system.cpp index 0d07fb95..ed54044e 100644 --- a/gz_ros2_control/src/gz_system.cpp +++ b/gz_ros2_control/src/gz_system.cpp @@ -85,6 +85,9 @@ struct jointData /// \brief Current cmd joint effort double joint_effort_cmd; + /// \brief flag if joint is actuated (has command interfaces) or passive + bool is_actuated; + /// \brief handles to the joints from within Gazebo sim::Entity sim_joint; @@ -388,6 +391,9 @@ bool GazeboSimSystem::initSim( this->dataPtr->joints_[j].joint_velocity = initial_velocity; } } + + // check if joint is actuated (has command interfaces) or passive + this->dataPtr->joints_[j].is_actuated = (joint_info.command_interfaces.size() > 0); } registerSensors(hardware_info); @@ -652,8 +658,8 @@ hardware_interface::return_type GazeboSimSystem::write( *jointEffortCmd = sim::components::JointForceCmd( {this->dataPtr->joints_[i].joint_effort_cmd}); } - } else { - // Fallback case is a velocity command of zero + } else if (this->dataPtr->joints_[i].is_actuated) { + // Fallback case is a velocity command of zero (only for actuated joints) double target_vel = 0.0; auto vel = this->dataPtr->ecm->Component(