From cb62bc6bd077323f3d1c6a5a3e15bcfea1627ecc Mon Sep 17 00:00:00 2001 From: NikoFeith Date: Tue, 7 May 2024 16:22:18 +0200 Subject: [PATCH] Interface working transformation from raw 0-255 (in reality 3-230) to 0.0-0.14 (reality 0.0-0.1426) not precise - need some additional work plan giving 3-5 gaps addtional to 0.0 to fit a calibration curve - maybe a calibration launch file which goes to specific positions and the user measures the values and enters the real values to calibrate the gripper --- .../robotiq_gripper_command_controller.hpp | 2 +- .../src/robotiq_forward_controller.cpp | 6 +- .../robotiq_gripper_command_controller.cpp | 10 +-- .../config/robotiq_2f_140_config.yaml | 2 +- .../Robotiq2fSocketAdapter.hpp | 4 +- .../hardware_interface.hpp | 12 +++- .../src/Robotiq2fSocketAdapter.cpp | 38 ++++++----- .../src/hardware_interface.cpp | 65 ++++++++++--------- 8 files changed, 78 insertions(+), 61 deletions(-) diff --git a/src/robotiq_2f/robotiq_2f_controllers/include/robotiq_2f_controllers/robotiq_gripper_command_controller.hpp b/src/robotiq_2f/robotiq_2f_controllers/include/robotiq_2f_controllers/robotiq_gripper_command_controller.hpp index fa2cd87..b09c84e 100644 --- a/src/robotiq_2f/robotiq_2f_controllers/include/robotiq_2f_controllers/robotiq_gripper_command_controller.hpp +++ b/src/robotiq_2f/robotiq_2f_controllers/include/robotiq_2f_controllers/robotiq_gripper_command_controller.hpp @@ -134,7 +134,7 @@ protected: // Const const int MAX_STALL_COUNT = 200; - const double POSITION_TOLERANCE = 0.0001; + const double POSITION_TOLERANCE = 0.0015; const double MIN_SPEED = 0.0; const double MIN_EFFORT = 0.0; diff --git a/src/robotiq_2f/robotiq_2f_controllers/src/robotiq_forward_controller.cpp b/src/robotiq_2f/robotiq_2f_controllers/src/robotiq_forward_controller.cpp index 16b1dd5..386334a 100644 --- a/src/robotiq_2f/robotiq_2f_controllers/src/robotiq_forward_controller.cpp +++ b/src/robotiq_2f/robotiq_2f_controllers/src/robotiq_forward_controller.cpp @@ -21,7 +21,7 @@ controller_interface::CallbackReturn RobotiqForwardController::on_init() return CallbackReturn::ERROR; } else{ - RCLCPP_INFO(get_node()->get_logger(), "Joint name: %s.", joint_name_.c_str()); + RCLCPP_DEBUG(get_node()->get_logger(), "Joint name: %s.", joint_name_.c_str()); } return CallbackReturn::SUCCESS; @@ -67,9 +67,9 @@ controller_interface::CallbackReturn RobotiqForwardController::on_configure(cons controller_interface::CallbackReturn RobotiqForwardController::on_activate(const rclcpp_lifecycle::State &) { - RCLCPP_INFO(get_node()->get_logger(), "Available command interfaces:"); + RCLCPP_DEBUG(get_node()->get_logger(), "Available command interfaces:"); for (const auto& interface : command_interfaces_) { - RCLCPP_INFO(get_node()->get_logger(), "Interface Name: %s, Interface Type: %s", interface.get_name().c_str(), interface.get_interface_name().c_str()); + RCLCPP_DEBUG(get_node()->get_logger(), "Interface Name: %s, Interface Type: %s", interface.get_name().c_str(), interface.get_interface_name().c_str()); } // clear out vectors in case of restart diff --git a/src/robotiq_2f/robotiq_2f_controllers/src/robotiq_gripper_command_controller.cpp b/src/robotiq_2f/robotiq_2f_controllers/src/robotiq_gripper_command_controller.cpp index 7fc4c2c..aff4aba 100644 --- a/src/robotiq_2f/robotiq_2f_controllers/src/robotiq_gripper_command_controller.cpp +++ b/src/robotiq_2f/robotiq_2f_controllers/src/robotiq_gripper_command_controller.cpp @@ -22,7 +22,7 @@ controller_interface::CallbackReturn RobotiqGripperCommandController::on_init() return CallbackReturn::ERROR; } else{ - RCLCPP_INFO(get_node()->get_logger(), "Joint name: %s.", joint_name_.c_str()); + RCLCPP_DEBUG(get_node()->get_logger(), "Joint name: %s.", joint_name_.c_str()); } return CallbackReturn::SUCCESS; @@ -76,9 +76,9 @@ controller_interface::CallbackReturn RobotiqGripperCommandController::on_configu controller_interface::CallbackReturn RobotiqGripperCommandController::on_activate(const rclcpp_lifecycle::State & ) { - RCLCPP_INFO(get_node()->get_logger(), "Available command interfaces:"); + RCLCPP_DEBUG(get_node()->get_logger(), "Available command interfaces:"); for (const auto& interface : command_interfaces_) { - RCLCPP_INFO(get_node()->get_logger(), "Interface Name: %s, Interface Type: %s", interface.get_name().c_str(), interface.get_interface_name().c_str()); + RCLCPP_DEBUG(get_node()->get_logger(), "Interface Name: %s, Interface Type: %s", interface.get_name().c_str(), interface.get_interface_name().c_str()); } // clear out vectors in case of restart @@ -99,12 +99,12 @@ controller_interface::CallbackReturn RobotiqGripperCommandController::on_activat state_interface_map_[interface.get_interface_name()]->push_back(interface); } - RCLCPP_INFO(get_node()->get_logger(), "Checking if command interfaces are initialized corretly..."); + RCLCPP_DEBUG(get_node()->get_logger(), "Checking if command interfaces are initialized corretly..."); if (!command_interface_checker_()) { return CallbackReturn::ERROR; } - RCLCPP_INFO(get_node()->get_logger(), "Command interfaces correctly initialized!"); + RCLCPP_DEBUG(get_node()->get_logger(), "Command interfaces correctly initialized!"); return CallbackReturn::SUCCESS; } diff --git a/src/robotiq_2f/robotiq_2f_description/config/robotiq_2f_140_config.yaml b/src/robotiq_2f/robotiq_2f_description/config/robotiq_2f_140_config.yaml index 8c1cf5f..9ad7210 100644 --- a/src/robotiq_2f/robotiq_2f_description/config/robotiq_2f_140_config.yaml +++ b/src/robotiq_2f/robotiq_2f_description/config/robotiq_2f_140_config.yaml @@ -1,7 +1,7 @@ robotiq_2f_gripper: # Physical limits min_position: 0.0 # Meters (fully closed) - max_position: 0.14 # Meters (fully open) + max_position: 0.1426 # Meters (fully open) max_angle: 0.695 # radiants (closed) min_speed: 0.02 # Meters per second max_speed: 0.15 # Meters per second diff --git a/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/Robotiq2fSocketAdapter.hpp b/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/Robotiq2fSocketAdapter.hpp index 741ea33..27fb1ab 100644 --- a/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/Robotiq2fSocketAdapter.hpp +++ b/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/Robotiq2fSocketAdapter.hpp @@ -243,8 +243,8 @@ private: std::mutex socket_mutex_; // Mutex for socket access synchronization // bounds of the encoded gripper states - int min_position_ = 0; - int max_position_ = 255; + int min_position_ = 3; + int max_position_ = 230; int min_speed_ = 0; int max_speed_ = 255; int min_force_ = 0; diff --git a/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/hardware_interface.hpp b/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/hardware_interface.hpp index 165ca57..497781d 100644 --- a/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/hardware_interface.hpp +++ b/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/hardware_interface.hpp @@ -69,9 +69,9 @@ protected: // Likely changes the access to protected from private // Conversion Methods double convertRawToGap(int raw_position); double convertRawToAngle(int raw_position); - int convertGapToRaw(double gap); - int convertSpeedToRaw(double speed); - int convertForceToRaw(double force); + unsigned int convertGapToRaw(double gap); + unsigned int convertSpeedToRaw(double speed); + unsigned int convertForceToRaw(double force); // Gripper Commands std::atomic write_command_; @@ -110,6 +110,12 @@ protected: // Likely changes the access to protected from private double min_force_; double max_force_; + // Last Command Values + int last_cmd_gap = 0; + int last_cmd_speed = 0; + int last_cmd_force = 0; + int last_raw_gap = 0; + // loop time const std::chrono::milliseconds gripperCommsLoopPeriod = std::chrono::milliseconds{ 2 }; diff --git a/src/robotiq_2f/robotiq_2f_interface/src/Robotiq2fSocketAdapter.cpp b/src/robotiq_2f/robotiq_2f_interface/src/Robotiq2fSocketAdapter.cpp index 21ddbef..2d32371 100644 --- a/src/robotiq_2f/robotiq_2f_interface/src/Robotiq2fSocketAdapter.cpp +++ b/src/robotiq_2f/robotiq_2f_interface/src/Robotiq2fSocketAdapter.cpp @@ -4,7 +4,6 @@ namespace robotiq_interface { Robotiq2fSocketAdapter::Robotiq2fSocketAdapter() : sockfd_(new int(-1)) { -RCLCPP_INFO(logger_, "Constructor"); } @@ -147,7 +146,6 @@ std::string Robotiq2fSocketAdapter::receiveResponse(int timeout_ms) { } } } - return result; } @@ -287,7 +285,11 @@ int Robotiq2fSocketAdapter::getGripperVariable(const std::string& variable) { std::istringstream iss(response); std::string var_name, value_str; - if (!(iss >> var_name >> value_str)) { + if (response == ACK_MESSAGE) { + RCLCPP_DEBUG(logger_, "[getGripperVariable]Gripper still responding with ACK_MESSAGE"); + return -1; + } else if (!(iss >> var_name >> value_str)) { + RCLCPP_ERROR(logger_, "Invalid gripper response format: %s for cmd: %s", response.c_str(), cmd.c_str()); throw std::runtime_error("Invalid gripper response format."); return -1; } @@ -312,10 +314,10 @@ void Robotiq2fSocketAdapter::activate(bool autoCalibrate) { setGripperVariable(CMD_ACT, static_cast(1)); // Activate the gripper waitForGripperStatus(GripperStatus::ACTIVE); // Wait until active - if (autoCalibrate) { + if (!autoCalibrate) { auto_calibrate(); } - RCLCPP_INFO(logger_, "Socket adapter activated."); + RCLCPP_DEBUG(logger_, "Socket adapter activated."); } void Robotiq2fSocketAdapter::reset() { @@ -355,6 +357,9 @@ std::tuple Robotiq2fSocketAdapter::move(int position, int speed, int unsigned int clippedSpeed = clip_val(min_speed_, speed, max_speed_); unsigned int clippedForce = clip_val(min_force_, force, max_force_); + RCLCPP_DEBUG(logger_, "Current min/max positions: [%d, %d]", min_position_, max_position_); + RCLCPP_DEBUG(logger_, "Clipped command: %d", clippedPosition); + // Create a map for gripper variables (similar to Python's OrderedDict) std::map variableMap = { { CMD_POS, clippedPosition }, @@ -362,8 +367,15 @@ std::tuple Robotiq2fSocketAdapter::move(int position, int speed, int { CMD_FOR, clippedForce }, { CMD_GTO, static_cast(1) } }; - bool setResult = setGripperVariables(variableMap); + if (!setResult) + { + std::string result; + for (const auto& pair : variableMap) { + result += "{" + pair.first + ", " + std::to_string(pair.second) + "}, "; + } + RCLCPP_ERROR(logger_, "[move] failed for CMD Map: %s", result.c_str()); + } return std::make_tuple(setResult, clippedPosition); } @@ -372,21 +384,14 @@ std::tuple Robotiq2fSocketAdapter::move_and_wait_for_pos(int unsigned int clippedSpeed = clip_val(min_speed_, speed, max_speed_); unsigned int clippedForce = clip_val(min_force_, force, max_force_); - // Set gripper variables to initiate the move - std::map variableMap = { - { CMD_POS, clippedPosition }, - { CMD_SPE, clippedSpeed }, - { CMD_FOR, clippedForce }, - { CMD_GTO, static_cast(1) } - }; - bool setResult = setGripperVariables(variableMap); - if (!setResult) { + auto move_result = move(clippedPosition, clippedSpeed, clippedForce); + if (!std::get<0>(move_result)) { // Handle the error case, e.g., throw an exception RCLCPP_ERROR(logger_, "Failed to set variables for move."); } // Wait until position is acknowledged: - while(getGripperVariable(CMD_PRE) != clippedPosition) { + while(getGripperVariable(CMD_PRE) != std::get<1>(move_result)) { std::this_thread::sleep_for(std::chrono::milliseconds(10)); // Adjust sleep as needed } @@ -406,6 +411,7 @@ std::tuple Robotiq2fSocketAdapter::move_and_wait_for_pos(int void Robotiq2fSocketAdapter::auto_calibrate(bool log) { // Open in case we are holding an object + RCLCPP_INFO(logger_, "Calibration started"); std::tuple result = move_and_wait_for_pos(open_position_, 64, 1); if (std::get<1>(result) != ObjectStatus::AT_DEST) { throw std::runtime_error("Calibration failed opening to start."); diff --git a/src/robotiq_2f/robotiq_2f_interface/src/hardware_interface.cpp b/src/robotiq_2f/robotiq_2f_interface/src/hardware_interface.cpp index cf9a6ff..b5e3878 100644 --- a/src/robotiq_2f/robotiq_2f_interface/src/hardware_interface.cpp +++ b/src/robotiq_2f/robotiq_2f_interface/src/hardware_interface.cpp @@ -36,7 +36,7 @@ const double max_force_default = 0.0; RobotiqGripperHardwareInterface::RobotiqGripperHardwareInterface() : communication_thread_is_running_(false) { -RCLCPP_INFO(logger_, "Constructor"); +RCLCPP_DEBUG(logger_, "Constructor"); } RobotiqGripperHardwareInterface::~RobotiqGripperHardwareInterface() @@ -53,7 +53,7 @@ RobotiqGripperHardwareInterface::~RobotiqGripperHardwareInterface() hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init(const hardware_interface::HardwareInfo& info) { - RCLCPP_INFO(logger_, "on_init"); + RCLCPP_DEBUG(logger_, "on_init"); if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) { @@ -75,7 +75,7 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init(cons } else { robot_port_ = robot_port_default; } - RCLCPP_INFO(logger_, "Accessing parameter: Robot Port, Value: %d", robot_port_); + RCLCPP_DEBUG(logger_, "Accessing parameter: Robot Port, Value: %d", robot_port_); std::string param_value; if (info.hardware_parameters.count(use_mock_hardware_parameter_name) > 0) { @@ -84,21 +84,21 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init(cons } else { use_mock_hardware_ = use_mock_hardware_default; } - RCLCPP_INFO(logger_, "Accessing parameter: use fake hardware , Value: %s", param_value.c_str()); + RCLCPP_DEBUG(logger_, "Accessing parameter: use fake hardware , Value: %s", param_value.c_str()); if (info.hardware_parameters.count(min_position_parameter_name) > 0) { min_position_ = std::stod(info.hardware_parameters.at(min_position_parameter_name)); } else { min_position_ = min_position_default; } - RCLCPP_INFO(logger_, "Accessing parameter: min position, Value: %f", min_position_); + RCLCPP_DEBUG(logger_, "Accessing parameter: min position, Value: %f", min_position_); if (info.hardware_parameters.count(max_position_parameter_name) > 0) { max_position_ = std::stod(info.hardware_parameters.at(max_position_parameter_name)); } else { max_position_ = max_position_default; } - RCLCPP_INFO(logger_, "Accessing parameter: max position, Value: %f", max_position_); + RCLCPP_DEBUG(logger_, "Accessing parameter: max position, Value: %f", max_position_); if (info.hardware_parameters.count(max_angle_parameter_name) > 0) { max_angle_ = std::stod(info.hardware_parameters.at(max_angle_parameter_name)); @@ -106,35 +106,35 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init(cons RCLCPP_WARN(logger_, "max angle not found falling back to default value!"); max_angle_ = max_angle_default; } - RCLCPP_INFO(logger_, "Accessing parameter: max angle , Value: %f", max_angle_); + RCLCPP_DEBUG(logger_, "Accessing parameter: max angle , Value: %f", max_angle_); if (info.hardware_parameters.count(min_speed_parameter_name) > 0) { min_speed_ = std::stod(info.hardware_parameters.at(min_speed_parameter_name)); } else { min_speed_ = min_speed_default; } - RCLCPP_INFO(logger_, "Accessing parameter: min speed, Value: %f", min_speed_); + RCLCPP_DEBUG(logger_, "Accessing parameter: min speed, Value: %f", min_speed_); if (info.hardware_parameters.count(max_speed_parameter_name) > 0) { max_speed_ = std::stod(info.hardware_parameters.at(max_speed_parameter_name)); } else { max_speed_ = max_speed_default; } - RCLCPP_INFO(logger_, "Accessing parameter: max speed, Value: %f", max_speed_); + RCLCPP_DEBUG(logger_, "Accessing parameter: max speed, Value: %f", max_speed_); if (info.hardware_parameters.count(min_force_parameter_name) > 0) { min_force_ = std::stod(info.hardware_parameters.at(min_force_parameter_name)); } else { min_force_ = robot_port_default; } - RCLCPP_INFO(logger_, "Accessing parameter: min force, Value: %f", min_force_); + RCLCPP_DEBUG(logger_, "Accessing parameter: min force, Value: %f", min_force_); if (info.hardware_parameters.count(max_force_parameter_name) > 0) { max_force_ = std::stod(info.hardware_parameters.at(max_force_parameter_name)); } else { max_force_ = max_force_default; } - RCLCPP_INFO(logger_, "Accessing parameter: max force, Value: %f", max_force_); + RCLCPP_DEBUG(logger_, "Accessing parameter: max force, Value: %f", max_force_); } catch (const std::exception& e) { @@ -210,7 +210,7 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init(cons hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_configure(const rclcpp_lifecycle::State& previous_state) { - RCLCPP_INFO(logger_, "on_configure"); + RCLCPP_DEBUG(logger_, "on_configure"); try { if (hardware_interface::SystemInterface::on_configure(previous_state) != CallbackReturn::SUCCESS) @@ -231,7 +231,7 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_configure hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_activate(const rclcpp_lifecycle::State& /*previous_state*/) { - RCLCPP_INFO(logger_, "on_activate"); + RCLCPP_DEBUG(logger_, "on_activate"); // set some default values for joints if (std::isnan(gripper_gap_)) @@ -300,7 +300,7 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_deactivat std::vector RobotiqGripperHardwareInterface::export_state_interfaces() { - RCLCPP_INFO(logger_, "export_state_interfaces"); + RCLCPP_DEBUG(logger_, "export_state_interfaces"); std::vector state_interfaces; @@ -311,7 +311,7 @@ std::vector RobotiqGripperHardwareInterface: state_interfaces.emplace_back( hardware_interface::StateInterface(info_.joints[1].name, hardware_interface::HW_IF_POSITION, &gripper_angle_)); for (const auto& interface : state_interfaces) { - RCLCPP_INFO(logger_, "Exporting state interface for joint: %s type: %s", + RCLCPP_DEBUG(logger_, "Exporting state interface for joint: %s type: %s", interface.get_name().c_str(), interface.get_interface_name().c_str()); } @@ -354,7 +354,7 @@ std::vector RobotiqGripperHardwareInterfac hardware_interface::CommandInterface("reactivate_gripper", "reactivate_gripper_response", &reactivate_gripper_response_)); for (const auto& interface : command_interfaces) { - RCLCPP_INFO(logger_, "Exporting command interface for joint: %s type: %s", + RCLCPP_DEBUG(logger_, "Exporting command interface for joint: %s type: %s", interface.get_name().c_str(), interface.get_interface_name().c_str()); } @@ -465,11 +465,16 @@ void RobotiqGripperHardwareInterface::performRegularOperations() int scaled_speed = write_speed_.load(); int scaled_force = write_force_.load(); - auto result = socket_adapter_->move(scaled_gap, scaled_speed, scaled_force); - if (!std::get<0>(result)) { - throw std::runtime_error("Failed to send move command to Robotiq gripper."); + if (scaled_gap != last_cmd_gap || scaled_speed != last_cmd_speed || scaled_force != last_cmd_force){ + RCLCPP_DEBUG(logger_, "New move cmd: POS: %d, SPE: %d, FOR: %d", scaled_gap, scaled_speed , scaled_force); + auto result = socket_adapter_->move(scaled_gap, scaled_speed, scaled_force); + if (!std::get<0>(result)) { + throw std::runtime_error("Failed to send move command to Robotiq gripper."); + } + last_cmd_gap = scaled_gap; + last_cmd_speed = scaled_speed; + last_cmd_force = scaled_force; } - int raw_gap = socket_adapter_->position(); gripper_current_state_.store(convertRawToGap(raw_gap)); } @@ -495,9 +500,9 @@ void RobotiqGripperHardwareInterface::configureAdapter(bool useMock) { // Conversion methods double RobotiqGripperHardwareInterface::convertRawToGap(int raw_position) { if (std::isnan(min_position_) || std::isnan(max_position_) || min_position_ >= max_position_) { - throw std::runtime_error("Invalid gripper position limits: min_position_ or max_position_ are not configured correctly."); + throw std::runtime_error("Invalid gripper position limits."); } - double scaled_position = max_position_ - (static_cast(raw_position) / 255.0) * (max_position_ - min_position_); + double scaled_position = min_position_ + ((230 - static_cast(raw_position)) / 227.0) * (max_position_ - min_position_); return std::max(min_position_, std::min(scaled_position, max_position_)); } @@ -509,28 +514,28 @@ double RobotiqGripperHardwareInterface::convertRawToAngle(int raw_position) { return std::max(0.0, std::min(scaled_angle, max_angle_)); } -int RobotiqGripperHardwareInterface::convertGapToRaw(double gap) { +unsigned int RobotiqGripperHardwareInterface::convertGapToRaw(double gap) { if (std::isnan(min_position_) || std::isnan(max_position_) || min_position_ >= max_position_) { - throw std::runtime_error("Invalid gripper position limits: min_position_ or max_position_ are not configured correctly."); + throw std::runtime_error("Invalid gripper position limits."); } - double scaled = 255.0 - (gap - min_position_) / (max_position_ - min_position_) * 255.0; - return static_cast(std::clamp(scaled, 0.0, 255.0)); + unsigned int raw = static_cast(230 - ((gap - min_position_) / (max_position_ - min_position_)) * 227); + return std::clamp(raw, 3u, 230u); // Ensuring the output is within the valid range } -int RobotiqGripperHardwareInterface::convertSpeedToRaw(double speed) { +unsigned int RobotiqGripperHardwareInterface::convertSpeedToRaw(double speed) { if (std::isnan(min_speed_) || std::isnan(max_speed_) || min_speed_ >= max_speed_) { throw std::runtime_error("Invalid gripper speed limits: min_speed_ or max_speed_ are not configured correctly."); } double scaled = (speed - min_speed_) / (max_speed_ - min_speed_) * 255.0; - return static_cast(std::clamp(scaled, 0.0, 255.0)); + return static_cast(std::clamp(scaled, 0.0, 255.0)); } -int RobotiqGripperHardwareInterface::convertForceToRaw(double force) { +unsigned int RobotiqGripperHardwareInterface::convertForceToRaw(double force) { if (std::isnan(min_force_) || std::isnan(max_force_) || min_force_ >= max_force_) { throw std::runtime_error("Invalid gripper force limits: min_force_ or max_force_ are not configured correctly."); } double scaled = (force - min_force_) / (max_force_ - min_force_) * 255.0; - return static_cast(std::clamp(scaled, 0.0, 255.0)); + return static_cast(std::clamp(scaled, 0.0, 255.0)); } } // namespace robotiq_interface