adding torque check

odriverobotics · Dec 18, 2024 · a6fdc4c · a6fdc4c
1 parent 33c3db9
commit a6fdc4c
Showing 1 changed file with 3 additions and 1 deletion.
diff --git a/odrive_ros2_control/src/odrive_hardware_interface.cpp b/odrive_ros2_control/src/odrive_hardware_interface.cpp
@@ -327,10 +327,12 @@ bool ODriveHardwareInterface::set_axis_command_mode(Axis axis) {
         RCLCPP_INFO(rclcpp::get_logger("ODriveHardwareInterface"), "Setting to velocity control");
         msg.Control_Mode = CONTROL_MODE_VELOCITY_CONTROL;
         msg.Input_Mode = INPUT_MODE_PASSTHROUGH;
-    } else {
+    } else if (axis.torque_input_enabled_) {
         RCLCPP_INFO(rclcpp::get_logger("ODriveHardwareInterface"), "Setting to torque control");
         msg.Control_Mode = CONTROL_MODE_TORQUE_CONTROL;
         msg.Input_Mode = INPUT_MODE_PASSTHROUGH;
+    } else {
+        RCLCPP_INFO(rclcpp::get_logger("ODriveHardwareInterface"), "No control mode specified");
     }
 
     bool any_enabled = axis.pos_input_enabled_ || axis.vel_input_enabled_ || axis.torque_input_enabled_;