if button emulated, init HW clutch to 0.0

ICube-Robotics · Aug 20, 2024 · f2055d1 · f2055d1
1 parent 432de6d
commit f2055d1
Showing 1 changed file with 9 additions and 1 deletion.
diff --git a/fd_hardware/src/fd_effort_hi.cpp b/fd_hardware/src/fd_effort_hi.cpp
@@ -175,6 +175,15 @@ CallbackReturn FDEffortHardwareInterface::on_init(
     inertia_interface_name_ = "fd_inertia";
   }
 
+  // Contingency for emulated button
+  // (commanded clutch force might be always left to NaN...)
+  if (emulate_button_ &&
+    (info_.joints.size() == 4 || info_.joints.size() > 6))
+  {
+    // Prevent NaN in clutch cmd
+    hw_commands_effort_[info_.joints.size() - 1] = 0.0;
+  }
+
   return CallbackReturn::SUCCESS;
 }
 // ------------------------------------------------------------------------------------------
@@ -348,7 +357,6 @@ hardware_interface::return_type FDEffortHardwareInterface::write(
   const rclcpp::Time & /*time*/,
   const rclcpp::Duration & /*period*/)
 {
-  // TODO(mcbed): write to FD system
   bool isNan = false;
   for (auto & command : hw_commands_effort_) {
     if (command != command) {