From a6fdc4c5063eb2675f5e87e5dc620d4b4a8750c1 Mon Sep 17 00:00:00 2001 From: "Oscar-0.4" Date: Wed, 18 Dec 2024 09:11:57 -0500 Subject: [PATCH] adding torque check --- odrive_ros2_control/src/odrive_hardware_interface.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/odrive_ros2_control/src/odrive_hardware_interface.cpp b/odrive_ros2_control/src/odrive_hardware_interface.cpp index 667f52e..ec1e136 100644 --- 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_;