Niko/UR_Robotiq
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Interface working

Browse Source 
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
This commit is contained in:
NikoFeith 2024-05-07 16:22:18 +02:00
parent a31be7d77e
commit cb62bc6bd0
8 changed files with 78 additions and 61 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/robotiq_2f/robotiq_2f_controllers/include/robotiq_2f_controllers/robotiq_gripper_command_controller.hpp
View File

@ -134,7 +134,7 @@ protected:
// Const // Const
const int MAX_STALL_COUNT = 200; 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_SPEED = 0.0;
const double MIN_EFFORT = 0.0; const double MIN_EFFORT = 0.0;

6
src/robotiq_2f/robotiq_2f_controllers/src/robotiq_forward_controller.cpp
View File

@ -21,7 +21,7 @@ controller_interface::CallbackReturn RobotiqForwardController::on_init()
return CallbackReturn::ERROR; return CallbackReturn::ERROR;
} }
else{ 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; return CallbackReturn::SUCCESS;
@ -67,9 +67,9 @@ controller_interface::CallbackReturn RobotiqForwardController::on_configure(cons
controller_interface::CallbackReturn RobotiqForwardController::on_activate(const rclcpp_lifecycle::State &) 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_) { 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 // clear out vectors in case of restart

10
src/robotiq_2f/robotiq_2f_controllers/src/robotiq_gripper_command_controller.cpp
View File

@ -22,7 +22,7 @@ controller_interface::CallbackReturn RobotiqGripperCommandController::on_init()
return CallbackReturn::ERROR; return CallbackReturn::ERROR;
} }
else{ 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; return CallbackReturn::SUCCESS;
@ -76,9 +76,9 @@ controller_interface::CallbackReturn RobotiqGripperCommandController::on_configu
controller_interface::CallbackReturn RobotiqGripperCommandController::on_activate(const rclcpp_lifecycle::State & ) 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_) { 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 // 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); 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_()) if (!command_interface_checker_())
{ {
return CallbackReturn::ERROR; 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; return CallbackReturn::SUCCESS;
} }

2
src/robotiq_2f/robotiq_2f_description/config/robotiq_2f_140_config.yaml
View File

@ -1,7 +1,7 @@
robotiq_2f_gripper: robotiq_2f_gripper:
# Physical limits # Physical limits
min_position: 0.0 # Meters (fully closed) 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) max_angle: 0.695 # radiants (closed)
min_speed: 0.02 # Meters per second min_speed: 0.02 # Meters per second
max_speed: 0.15 # Meters per second max_speed: 0.15 # Meters per second

4
src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/Robotiq2fSocketAdapter.hpp
View File

@ -243,8 +243,8 @@ private:
std::mutex socket_mutex_; // Mutex for socket access synchronization std::mutex socket_mutex_; // Mutex for socket access synchronization
// bounds of the encoded gripper states // bounds of the encoded gripper states
int min_position_ = 0; int min_position_ = 3;
int max_position_ = 255; int max_position_ = 230;
int min_speed_ = 0; int min_speed_ = 0;
int max_speed_ = 255; int max_speed_ = 255;
int min_force_ = 0; int min_force_ = 0;

12
src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/hardware_interface.hpp
View File

@ -69,9 +69,9 @@ protected: // Likely changes the access to protected from private
// Conversion Methods // Conversion Methods
double convertRawToGap(int raw_position); double convertRawToGap(int raw_position);
double convertRawToAngle(int raw_position); double convertRawToAngle(int raw_position);
int convertGapToRaw(double gap); unsigned int convertGapToRaw(double gap);
int convertSpeedToRaw(double speed); unsigned int convertSpeedToRaw(double speed);
int convertForceToRaw(double force); unsigned int convertForceToRaw(double force);
// Gripper Commands // Gripper Commands
std::atomic<uint8_t> write_command_; std::atomic<uint8_t> write_command_;
@ -110,6 +110,12 @@ protected: // Likely changes the access to protected from private
double min_force_; double min_force_;
double max_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 // loop time
const std::chrono::milliseconds gripperCommsLoopPeriod = std::chrono::milliseconds{ 2 }; const std::chrono::milliseconds gripperCommsLoopPeriod = std::chrono::milliseconds{ 2 };

38
src/robotiq_2f/robotiq_2f_interface/src/Robotiq2fSocketAdapter.cpp
View File

@ -4,7 +4,6 @@
namespace robotiq_interface namespace robotiq_interface
{ {
Robotiq2fSocketAdapter::Robotiq2fSocketAdapter() : sockfd_(new int(-1)) { Robotiq2fSocketAdapter::Robotiq2fSocketAdapter() : sockfd_(new int(-1)) {
RCLCPP_INFO(logger_, "Constructor");
} }
@ -147,7 +146,6 @@ std::string Robotiq2fSocketAdapter::receiveResponse(int timeout_ms) {
} }
} }
} }
return result; return result;
} }
@ -287,7 +285,11 @@ int Robotiq2fSocketAdapter::getGripperVariable(const std::string& variable) {
std::istringstream iss(response); std::istringstream iss(response);
std::string var_name, value_str; 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."); throw std::runtime_error("Invalid gripper response format.");
return -1; return -1;
} }
@ -312,10 +314,10 @@ void Robotiq2fSocketAdapter::activate(bool autoCalibrate) {
setGripperVariable(CMD_ACT, static_cast<unsigned int>(1)); // Activate the gripper setGripperVariable(CMD_ACT, static_cast<unsigned int>(1)); // Activate the gripper
waitForGripperStatus(GripperStatus::ACTIVE); // Wait until active waitForGripperStatus(GripperStatus::ACTIVE); // Wait until active
if (autoCalibrate) { if (!autoCalibrate) {
auto_calibrate(); auto_calibrate();
} }
RCLCPP_INFO(logger_, "Socket adapter activated."); RCLCPP_DEBUG(logger_, "Socket adapter activated.");
} }
void Robotiq2fSocketAdapter::reset() { void Robotiq2fSocketAdapter::reset() {
@ -355,6 +357,9 @@ std::tuple<bool, int> Robotiq2fSocketAdapter::move(int position, int speed, int
unsigned int clippedSpeed = clip_val(min_speed_, speed, max_speed_); unsigned int clippedSpeed = clip_val(min_speed_, speed, max_speed_);
unsigned int clippedForce = clip_val(min_force_, force, max_force_); 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) // Create a map for gripper variables (similar to Python's OrderedDict)
std::map<std::string, unsigned int> variableMap = { std::map<std::string, unsigned int> variableMap = {
{ CMD_POS, clippedPosition }, { CMD_POS, clippedPosition },
@ -362,8 +367,15 @@ std::tuple<bool, int> Robotiq2fSocketAdapter::move(int position, int speed, int
{ CMD_FOR, clippedForce }, { CMD_FOR, clippedForce },
{ CMD_GTO, static_cast<unsigned int>(1) } { CMD_GTO, static_cast<unsigned int>(1) }
}; };
bool setResult = setGripperVariables(variableMap); 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); return std::make_tuple(setResult, clippedPosition);
} }
@ -372,21 +384,14 @@ std::tuple<int, ObjectStatus> Robotiq2fSocketAdapter::move_and_wait_for_pos(int
unsigned int clippedSpeed = clip_val(min_speed_, speed, max_speed_); unsigned int clippedSpeed = clip_val(min_speed_, speed, max_speed_);
unsigned int clippedForce = clip_val(min_force_, force, max_force_); unsigned int clippedForce = clip_val(min_force_, force, max_force_);
// Set gripper variables to initiate the move auto move_result = move(clippedPosition, clippedSpeed, clippedForce);
std::map<std::string, unsigned int> variableMap = { if (!std::get<0>(move_result)) {
{ CMD_POS, clippedPosition },
{ CMD_SPE, clippedSpeed },
{ CMD_FOR, clippedForce },
{ CMD_GTO, static_cast<unsigned int>(1) }
};
bool setResult = setGripperVariables(variableMap);
if (!setResult) {
// Handle the error case, e.g., throw an exception // Handle the error case, e.g., throw an exception
RCLCPP_ERROR(logger_, "Failed to set variables for move."); RCLCPP_ERROR(logger_, "Failed to set variables for move.");
} }
// Wait until position is acknowledged: // 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 std::this_thread::sleep_for(std::chrono::milliseconds(10)); // Adjust sleep as needed
} }
@ -406,6 +411,7 @@ std::tuple<int, ObjectStatus> Robotiq2fSocketAdapter::move_and_wait_for_pos(int
void Robotiq2fSocketAdapter::auto_calibrate(bool log) { void Robotiq2fSocketAdapter::auto_calibrate(bool log) {
// Open in case we are holding an object // Open in case we are holding an object
RCLCPP_INFO(logger_, "Calibration started");
std::tuple<int, ObjectStatus> result = move_and_wait_for_pos(open_position_, 64, 1); std::tuple<int, ObjectStatus> result = move_and_wait_for_pos(open_position_, 64, 1);
if (std::get<1>(result) != ObjectStatus::AT_DEST) { if (std::get<1>(result) != ObjectStatus::AT_DEST) {
throw std::runtime_error("Calibration failed opening to start."); throw std::runtime_error("Calibration failed opening to start.");

59
src/robotiq_2f/robotiq_2f_interface/src/hardware_interface.cpp
View File

@ -36,7 +36,7 @@ const double max_force_default = 0.0;
RobotiqGripperHardwareInterface::RobotiqGripperHardwareInterface() RobotiqGripperHardwareInterface::RobotiqGripperHardwareInterface()
: communication_thread_is_running_(false) : communication_thread_is_running_(false)
{ {
RCLCPP_INFO(logger_, "Constructor"); RCLCPP_DEBUG(logger_, "Constructor");
} }
RobotiqGripperHardwareInterface::~RobotiqGripperHardwareInterface() RobotiqGripperHardwareInterface::~RobotiqGripperHardwareInterface()
@ -53,7 +53,7 @@ RobotiqGripperHardwareInterface::~RobotiqGripperHardwareInterface()
hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init(const hardware_interface::HardwareInfo& info) 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) if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS)
{ {
@ -75,7 +75,7 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init(cons
} else { } else {
robot_port_ = robot_port_default; 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; std::string param_value;
if (info.hardware_parameters.count(use_mock_hardware_parameter_name) > 0) { if (info.hardware_parameters.count(use_mock_hardware_parameter_name) > 0) {
@ -84,21 +84,21 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init(cons
} else { } else {
use_mock_hardware_ = use_mock_hardware_default; 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) { if (info.hardware_parameters.count(min_position_parameter_name) > 0) {
min_position_ = std::stod(info.hardware_parameters.at(min_position_parameter_name)); min_position_ = std::stod(info.hardware_parameters.at(min_position_parameter_name));
} else { } else {
min_position_ = min_position_default; 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) { if (info.hardware_parameters.count(max_position_parameter_name) > 0) {
max_position_ = std::stod(info.hardware_parameters.at(max_position_parameter_name)); max_position_ = std::stod(info.hardware_parameters.at(max_position_parameter_name));
} else { } else {
max_position_ = max_position_default; 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) { if (info.hardware_parameters.count(max_angle_parameter_name) > 0) {
max_angle_ = std::stod(info.hardware_parameters.at(max_angle_parameter_name)); 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!"); RCLCPP_WARN(logger_, "max angle not found falling back to default value!");
max_angle_ = max_angle_default; 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) { if (info.hardware_parameters.count(min_speed_parameter_name) > 0) {
min_speed_ = std::stod(info.hardware_parameters.at(min_speed_parameter_name)); min_speed_ = std::stod(info.hardware_parameters.at(min_speed_parameter_name));
} else { } else {
min_speed_ = min_speed_default; 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) { if (info.hardware_parameters.count(max_speed_parameter_name) > 0) {
max_speed_ = std::stod(info.hardware_parameters.at(max_speed_parameter_name)); max_speed_ = std::stod(info.hardware_parameters.at(max_speed_parameter_name));
} else { } else {
max_speed_ = max_speed_default; 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) { if (info.hardware_parameters.count(min_force_parameter_name) > 0) {
min_force_ = std::stod(info.hardware_parameters.at(min_force_parameter_name)); min_force_ = std::stod(info.hardware_parameters.at(min_force_parameter_name));
} else { } else {
min_force_ = robot_port_default; 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) { if (info.hardware_parameters.count(max_force_parameter_name) > 0) {
max_force_ = std::stod(info.hardware_parameters.at(max_force_parameter_name)); max_force_ = std::stod(info.hardware_parameters.at(max_force_parameter_name));
} else { } else {
max_force_ = max_force_default; 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) 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) hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_configure(const rclcpp_lifecycle::State& previous_state)
{ {
RCLCPP_INFO(logger_, "on_configure"); RCLCPP_DEBUG(logger_, "on_configure");
try try
{ {
if (hardware_interface::SystemInterface::on_configure(previous_state) != CallbackReturn::SUCCESS) 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*/) 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 // set some default values for joints
if (std::isnan(gripper_gap_)) if (std::isnan(gripper_gap_))
@ -300,7 +300,7 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_deactivat
std::vector<hardware_interface::StateInterface> RobotiqGripperHardwareInterface::export_state_interfaces() std::vector<hardware_interface::StateInterface> RobotiqGripperHardwareInterface::export_state_interfaces()
{ {
RCLCPP_INFO(logger_, "export_state_interfaces"); RCLCPP_DEBUG(logger_, "export_state_interfaces");
std::vector<hardware_interface::StateInterface> state_interfaces; std::vector<hardware_interface::StateInterface> state_interfaces;
@ -311,7 +311,7 @@ std::vector<hardware_interface::StateInterface> RobotiqGripperHardwareInterface:
state_interfaces.emplace_back( state_interfaces.emplace_back(
hardware_interface::StateInterface(info_.joints[1].name, hardware_interface::HW_IF_POSITION, &gripper_angle_)); hardware_interface::StateInterface(info_.joints[1].name, hardware_interface::HW_IF_POSITION, &gripper_angle_));
for (const auto& interface : state_interfaces) { 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()); interface.get_name().c_str(), interface.get_interface_name().c_str());
} }
@ -354,7 +354,7 @@ std::vector<hardware_interface::CommandInterface> RobotiqGripperHardwareInterfac
hardware_interface::CommandInterface("reactivate_gripper", "reactivate_gripper_response", &reactivate_gripper_response_)); hardware_interface::CommandInterface("reactivate_gripper", "reactivate_gripper_response", &reactivate_gripper_response_));
for (const auto& interface : command_interfaces) { 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()); 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_speed = write_speed_.load();
int scaled_force = write_force_.load(); int scaled_force = write_force_.load();
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); auto result = socket_adapter_->move(scaled_gap, scaled_speed, scaled_force);
if (!std::get<0>(result)) { if (!std::get<0>(result)) {
throw std::runtime_error("Failed to send move command to Robotiq gripper."); 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(); int raw_gap = socket_adapter_->position();
gripper_current_state_.store(convertRawToGap(raw_gap)); gripper_current_state_.store(convertRawToGap(raw_gap));
} }
@ -495,9 +500,9 @@ void RobotiqGripperHardwareInterface::configureAdapter(bool useMock) {
// Conversion methods // Conversion methods
double RobotiqGripperHardwareInterface::convertRawToGap(int raw_position) { double RobotiqGripperHardwareInterface::convertRawToGap(int raw_position) {
if (std::isnan(min_position_) || std::isnan(max_position_) || min_position_ >= max_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<double>(raw_position) / 255.0) * (max_position_ - min_position_); double scaled_position = min_position_ + ((230 - static_cast<double>(raw_position)) / 227.0) * (max_position_ - min_position_);
return std::max(min_position_, std::min(scaled_position, max_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_)); 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_) { 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; unsigned int raw = static_cast<unsigned int>(230 - ((gap - min_position_) / (max_position_ - min_position_)) * 227);
return static_cast<uint8_t>(std::clamp(scaled, 0.0, 255.0)); 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_) { 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."); 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; double scaled = (speed - min_speed_) / (max_speed_ - min_speed_) * 255.0;
return static_cast<uint8_t>(std::clamp(scaled, 0.0, 255.0)); return static_cast<unsigned int>(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_) { 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."); 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; double scaled = (force - min_force_) / (max_force_ - min_force_) * 255.0;
return static_cast<uint8_t>(std::clamp(scaled, 0.0, 255.0)); return static_cast<unsigned int>(std::clamp(scaled, 0.0, 255.0));
} }
} // namespace robotiq_interface } // namespace robotiq_interface