From 8c822b3654fd7f1bcfabcc179eca406730f125a2 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Sat, 16 Dec 2023 20:13:02 +0100 Subject: [PATCH] Fix stuck passive joints (#184) (#206) 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 (cherry picked from commit 0a8d4464e15e4f13ea0673311c60a891d6a836ec) Co-authored-by: Johannes Huemer --- ign_ros2_control/src/ign_system.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/ign_ros2_control/src/ign_system.cpp b/ign_ros2_control/src/ign_system.cpp index 9921523a..ac6d4161 100644 --- a/ign_ros2_control/src/ign_system.cpp +++ b/ign_ros2_control/src/ign_system.cpp @@ -64,6 +64,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 ignition::gazebo::Entity sim_joint; @@ -367,6 +370,9 @@ bool IgnitionSystem::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); @@ -631,8 +637,8 @@ hardware_interface::return_type IgnitionSystem::write( *jointEffortCmd = ignition::gazebo::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(