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 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;

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;
}
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

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;
}
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;
}

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

@ -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

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
// 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;

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
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<uint8_t> 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 };

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

@ -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<unsigned int>(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<bool, int> 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<std::string, unsigned int> variableMap = {
{ CMD_POS, clippedPosition },
@ -362,8 +367,15 @@ std::tuple<bool, int> Robotiq2fSocketAdapter::move(int position, int speed, int
{ CMD_FOR, clippedForce },
{ CMD_GTO, static_cast<unsigned int>(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<int, ObjectStatus> 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<std::string, unsigned int> variableMap = {
{ CMD_POS, clippedPosition },
{ CMD_SPE, clippedSpeed },
{ CMD_FOR, clippedForce },
{ CMD_GTO, static_cast<unsigned int>(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<int, ObjectStatus> 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<int, ObjectStatus> 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.");

65
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()
: 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<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;
@ -311,7 +311,7 @@ std::vector<hardware_interface::StateInterface> 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<hardware_interface::CommandInterface> 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<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_));
}
@ -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<uint8_t>(std::clamp(scaled, 0.0, 255.0));
unsigned int raw = static_cast<unsigned int>(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<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_) {
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<uint8_t>(std::clamp(scaled, 0.0, 255.0));
return static_cast<unsigned int>(std::clamp(scaled, 0.0, 255.0));
}
} // namespace robotiq_interface