From 5dba0f95a03b136f39145c846ec5ebbfb5a09599 Mon Sep 17 00:00:00 2001
From: Johannes Huemer <johannes.huemer@ait.ac.at>
Date: Wed, 4 Oct 2023 10:41:20 +0200
Subject: [PATCH] Fix stuck passive joints (#237)

---
 gazebo_ros2_control/src/gazebo_system.cpp | 11 +++++++++--
 1 file changed, 9 insertions(+), 2 deletions(-)

diff --git a/gazebo_ros2_control/src/gazebo_system.cpp b/gazebo_ros2_control/src/gazebo_system.cpp
index b325d46b..a9292bd9 100644
--- a/gazebo_ros2_control/src/gazebo_system.cpp
+++ b/gazebo_ros2_control/src/gazebo_system.cpp
@@ -59,6 +59,9 @@ class gazebo_ros2_control::GazeboSystemPrivate
   /// \brief vector with the control method defined in the URDF for each joint.
   std::vector<GazeboSystemInterface::ControlMethod> joint_control_methods_;
 
+  /// \brief vector with indication for actuated joints (vs. passive joints)
+  std::vector<bool> is_joint_actuated_;
+
   /// \brief handles to the joints from within Gazebo
   std::vector<gazebo::physics::JointPtr> sim_joints_;
 
@@ -144,6 +147,7 @@ void GazeboSystem::registerJoints(
 
   this->dataPtr->joint_names_.resize(this->dataPtr->n_dof_);
   this->dataPtr->joint_control_methods_.resize(this->dataPtr->n_dof_);
+  this->dataPtr->is_joint_actuated_.resize(this->dataPtr->n_dof_);
   this->dataPtr->joint_position_.resize(this->dataPtr->n_dof_);
   this->dataPtr->joint_velocity_.resize(this->dataPtr->n_dof_);
   this->dataPtr->joint_effort_.resize(this->dataPtr->n_dof_);
@@ -293,6 +297,9 @@ void GazeboSystem::registerJoints(
         this->dataPtr->sim_joints_[j]->SetForce(0, initial_effort);
       }
     }
+
+    // check if joint is actuated (has command interfaces) or passive
+    this->dataPtr->is_joint_actuated_[j] = (joint_info.command_interfaces.size() > 0);
   }
 }
 
@@ -594,8 +601,8 @@ 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 {
-        // Fallback case is a velocity command of zero
+      } else if (this->dataPtr->is_joint_actuated_[j]) {
+        // Fallback case is a velocity command of zero (only for actuated joints)
         this->dataPtr->sim_joints_[j]->SetVelocity(0, 0.0);
       }
     }