Fix stuck passive joints (#184)

* Fix stuck passive joints * Update comment * Fix variable naming --------- Co-authored-by: Christoph Fröhlich <[email protected]>
ros-controls · Dec 16, 2023 · 0a8d446 · 0a8d446
1 parent 3c8d880
commit 0a8d446
Showing 1 changed file with 8 additions and 2 deletions.
diff --git 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<sim::components::JointVelocityCmd>(