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

hardwareinterface doesnt export the command interfaces correctly

Browse Source 
Issue probably in the Robotiq2fSocketAdapter.cpp since the mock hardware socket works fine

fixed some issues in 85 urdf
This commit is contained in:
NikoFeith 2024-05-02 13:55:10 +02:00
parent dc82ec4787
commit a31be7d77e
7 changed files with 100 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/robotiq_2f/robotiq_2f_description/launch/view_gripper.launch.py
View File

@ -16,7 +16,7 @@ def generate_launch_description():
default_model_path = os.path.join(
pkg_share, "urdf", "robotiq_2f_140.urdf.xacro"
)
default_rviz_config_path = os.path.join(pkg_share, "rviz", "view_urdf.rviz")
default_rviz_config_path = os.path.join(pkg_share, "rviz", "view_robot.rviz")
args = []
args.append(

0
src/robotiq_2f/robotiq_2f_description/rviz/view_urdf.rviz → src/robotiq_2f/robotiq_2f_description/rviz/view_robot.rviz
View File

34
src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_85.ros2_control.xacro
View File

@ -10,6 +10,7 @@
robot_port
min_position
max_position
max_angle
min_speed
max_speed
min_force
@ -18,9 +19,10 @@
<ros2_control name="${name}" type="system">
<!-- Plugins -->
<hardware>
<xacro:if value="${use_fake_hardware}">
<plugin>mock_components/GenericSystem</plugin>
<param name="fake_sensor_commands">${fake_sensor_commands}</param>
<xacro:if value="${use_fake_hardware}">
<plugin>robotiq_2f_interface/RobotiqGripperHardwareInterface</plugin>
<param name="use_fake_hardware">${use_fake_hardware}</param>
<param name="mock_sensor_commands">${fake_sensor_commands}</param>
<param name="state_following_offset">0.0</param>
</xacro:if>
<xacro:unless value="${use_fake_hardware}">
@ -29,25 +31,31 @@
<param name="robot_ip">${robot_ip}</param>
<param name="robot_port">${robot_port}</param>
<param name="use_fake_hardware">${use_fake_hardware}</param>
<param name="gripper_speed_multiplier">1.0</param>
<param name="gripper_force_multiplier">0.5</param>
<param name="min_position">${min_position}</param>
<param name="max_position">${max_position}</param>
<param name="min_speed">${min_speed}</param>
<param name="max_speed">${max_speed}</param>
<param name="min_force">${min_force}</param>
<param name="max_force">${max_force}</param>
</xacro:unless>
<param name="gripper_speed_multiplier">1.0</param>
<param name="gripper_force_multiplier">0.5</param>
<param name="min_position">${min_position}</param>
<param name="max_position">${max_position}</param>
<param name="max_angle">${max_angle}</param>
<param name="min_speed">${min_speed}</param>
<param name="max_speed">${max_speed}</param>
<param name="min_force">${min_force}</param>
<param name="max_force">${max_force}</param>
</hardware>
<!-- Joint interfaces -->
<!-- With Hardware, they handle mimic joints, so we only need this command interface activated -->
<joint name="${prefix}finger_joint">
<joint name="${prefix}gripper_gap">
<command_interface name="position" />
<state_interface name="position">
<param name="initial_value">0.0</param>
</state_interface>
<state_interface name="velocity"/>
</joint>
<joint name="${prefix}finger_joint">
<state_interface name="position">
<param name="initial_value">0.7929</param>
</state_interface>
<state_interface name="velocity"/>
</joint>
<!-- When simulating we need to include the rest of the gripper joints -->
<xacro:if value="${use_fake_hardware}">

54
src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_85_macro.urdf.xacro
View File

@ -237,13 +237,14 @@
</inertial>
</link>
<!-- Joints-->
<joint name="${prefix}robotiq_85_base_joint" type="fixed">
<parent link="${parent}" />
<child link="${prefix}robotiq_85_base_link" />
<xacro:insert_block name="origin" />
</joint>
<joint name="${prefix}robotiq_85_left_knuckle_joint" type="revolute">
<joint name="${prefix}finger_joint" type="revolute">
<parent link="${prefix}robotiq_85_base_link" />
<child link="${prefix}robotiq_85_left_knuckle_link" />
<axis xyz="0 -1 0" />
@ -251,57 +252,84 @@
<limit lower="0.0" upper="0.8" velocity="0.5" effort="50" />
</joint>
<joint name="${prefix}robotiq_85_right_knuckle_joint" type="revolute">
<joint name="${prefix}right_outer_knuckle_joint" type="revolute">
<parent link="${prefix}robotiq_85_base_link" />
<child link="${prefix}robotiq_85_right_knuckle_link" />
<axis xyz="0 -1 0" />
<origin xyz="-0.03060114 0.0 0.05490452" rpy="0 0 0" />
<limit lower="-0.8" upper="0.0" velocity="0.5" effort="50" />
<mimic joint="${prefix}robotiq_85_left_knuckle_joint" multiplier="-1" />
<mimic joint="${prefix}finger_joint" multiplier="-1" />
</joint>
<joint name="${prefix}robotiq_85_left_finger_joint" type="fixed">
<joint name="${prefix}left_outer_finger_joint" type="fixed">
<parent link="${prefix}robotiq_85_left_knuckle_link" />
<child link="${prefix}robotiq_85_left_finger_link" />
<origin xyz="0.03152616 0.0 -0.00376347" rpy="0 0 0" />
</joint>
<joint name="${prefix}robotiq_85_right_finger_joint" type="fixed">
<joint name="${prefix}right_outer_finger_joint" type="fixed">
<parent link="${prefix}robotiq_85_right_knuckle_link" />
<child link="${prefix}robotiq_85_right_finger_link" />
<origin xyz="-0.03152616 0.0 -0.00376347" rpy="0 0 0" />
</joint>
<joint name="${prefix}robotiq_85_left_inner_knuckle_joint" type="continuous">
<joint name="${prefix}left_inner_knuckle_joint" type="continuous">
<parent link="${prefix}robotiq_85_base_link" />
<child link="${prefix}robotiq_85_left_inner_knuckle_link" />
<axis xyz="0 -1 0" />
<origin xyz="0.0127 0.0 0.06142" rpy="0 0 0" />
<mimic joint="${prefix}robotiq_85_left_knuckle_joint" />
<mimic joint="${prefix}finger_joint" />
</joint>
<joint name="${prefix}robotiq_85_right_inner_knuckle_joint" type="continuous">
<joint name="${prefix}right_inner_knuckle_joint" type="continuous">
<parent link="${prefix}robotiq_85_base_link" />
<child link="${prefix}robotiq_85_right_inner_knuckle_link" />
<axis xyz="0 -1 0" />
<origin xyz="-0.0127 0.0 0.06142" rpy="0 0 0" />
<mimic joint="${prefix}robotiq_85_left_knuckle_joint" multiplier="-1" />
<mimic joint="${prefix}finger_joint" multiplier="-1" />
</joint>
<joint name="${prefix}robotiq_85_left_finger_tip_joint" type="continuous">
<joint name="${prefix}left_inner_finger_pad_joint" type="continuous">
<parent link="${prefix}robotiq_85_left_finger_link" />
<child link="${prefix}robotiq_85_left_finger_tip_link" />
<axis xyz="0 -1 0" />
<origin xyz="0.00563134 0.0 0.04718515" rpy="0 0 0" />
<mimic joint="${prefix}robotiq_85_left_knuckle_joint" multiplier="-1" />
<mimic joint="${prefix}finger_joint" multiplier="-1" />
</joint>
<joint name="${prefix}robotiq_85_right_finger_tip_joint" type="continuous">
<joint name="${prefix}right_inner_finger_pad_joint" type="continuous">
<parent link="${prefix}robotiq_85_right_finger_link" />
<child link="${prefix}robotiq_85_right_finger_tip_link" />
<axis xyz="0 -1 0" />
<origin xyz="-0.00563134 0.0 0.04718515" rpy="0 0 0" />
<mimic joint="${prefix}robotiq_85_left_knuckle_joint" />
<mimic joint="${prefix}finger_joint" />
</joint>
<!-- Pseudo Link and Joint for gripper gap -->
<link name="${prefix}dummy_link">
<visual>
<geometry>
<box size="0.001 0.001 0.001"/> <!-- Very small, effectively invisible -->
</geometry>
<material name="transparent">
<color rgba="0.0 0.0 0.0 0.0"/> <!-- Transparent color -->
</material>
</visual>
</link>
<joint name="base_to_dummy" type="fixed">
<parent link="${prefix}robotiq_140_base_link"/>
<child link="dummy_link"/>
<origin xyz="0 0 0" rpy="0 0 0"/> <!-- Adjust origin as needed -->
</joint>
<joint name="gripper_gap" type="prismatic">
<parent link="${prefix}dummy_link"/>
<child link="${prefix}robotiq_85_right_finger_tip_link"/>
<origin xyz="0 0 0" rpy="0 0 0"/> <!-- Set origin appropriately -->
<axis xyz="0 1 0"/> <!-- Axis of movement; adjust according to actual alignment -->
<limit lower="0.0" upper="0.085" effort="235" velocity="0.15"/>
</joint>
</xacro:macro>
</robot>

2
src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/MockRobotiq2fSocketAdapter.hpp
View File

@ -97,7 +97,7 @@ private:
std::this_thread::sleep_for(std::chrono::milliseconds(10)); // Simulate time to move a step
}
std::cout << "Reached position " << targetPosition << " with speed " << speed << " and force " << force << std::endl;
// std::cout << "Reached position " << targetPosition << " with speed " << speed << " and force " << force << std::endl;
}
};
} // end robotiq_interface

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

@ -161,7 +161,7 @@ public:
* activation command fails, or if the gripper does not reach the expected state within
* a reasonable timeframe.
*/
void activate(bool autoCalibrate = true) override;
void activate(bool autoCalibrate = false) override;
/**
* Performs auto-calibration of the gripper.
*
@ -354,7 +354,7 @@ private:
// Constants buffer sizes
const size_t BUFFER_SIZE = 1024; // Adjust as necessary
const int MAX_RETRIES = 2000;
const int MAX_RETRIES = 200;
const int RETRY_DELAY_MS = 50;
// Logging

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

@ -16,14 +16,14 @@ Robotiq2fSocketAdapter::~Robotiq2fSocketAdapter() {
bool Robotiq2fSocketAdapter::connecting(const std::string& hostname, int port) {
std::lock_guard<std::mutex> lock(socket_mutex_);
if (*sockfd_ >= 0) {
std::cerr << "Already connected. Disconnecting to reconnect.\n";
// std::cerr << "Already connected. Disconnecting to reconnect.\n";
RCLCPP_ERROR(logger_, "Already connected. Disconnecting to reconnect.");
disconnect();
}
int sock = socket(AF_INET, SOCK_STREAM, 0);
if (sock < 0) {
std::cerr << "Socket creation failed: " << strerror(errno) << "\n";
// std::cerr << "Socket creation failed: " << strerror(errno) << "\n";
RCLCPP_ERROR(logger_, "Socket creation failed!");
return false;
}
@ -37,7 +37,7 @@ bool Robotiq2fSocketAdapter::connecting(const std::string& hostname, int port) {
serv_addr.sin_port = htons(port);
if (inet_pton(AF_INET, hostname.c_str(), &serv_addr.sin_addr) <= 0) {
std::cerr << "Invalid address: " << hostname << "\n";
// std::cerr << "Invalid address: " << hostname << "\n";
RCLCPP_ERROR(logger_, "Invalid address: %s", hostname.c_str());
close(*sockfd_);
*sockfd_ = -1;
@ -45,7 +45,7 @@ bool Robotiq2fSocketAdapter::connecting(const std::string& hostname, int port) {
}
if (connect(*sockfd_, (struct sockaddr*)&serv_addr, sizeof(serv_addr)) != 0) {
std::cerr << "Connection to " << hostname << ":" << port << " failed: " << strerror(errno) << "\n";
// std::cerr << "Connection to " << hostname << ":" << port << " failed: " << strerror(errno) << "\n";
RCLCPP_ERROR(logger_, "Connection to %s: %d failed!", hostname.c_str(), port);
close(*sockfd_);
*sockfd_ = -1;
@ -63,10 +63,10 @@ void Robotiq2fSocketAdapter::disconnect() {
std::lock_guard<std::mutex> lock(socket_mutex_);
if (*sockfd_ >= 0) {
if (close(*sockfd_) == -1) {
std::cerr << "Error closing socket: " << strerror(errno) << "\n";
// std::cerr << "Error closing socket: " << strerror(errno) << "\n";
RCLCPP_ERROR(logger_, "Error closing socket: %s", strerror(errno));
} else {
std::cout << "Disconnected successfully.\n";
// std::cout << "Disconnected successfully.\n";
RCLCPP_INFO(logger_, "Disconnected successfully.");
}
*sockfd_ = -1;
@ -77,13 +77,14 @@ void Robotiq2fSocketAdapter::disconnect() {
bool Robotiq2fSocketAdapter::sendCommand(const std::string& cmd) {
std::lock_guard<std::mutex> lock(socket_mutex_);
if (*sockfd_ < 0) {
std::cerr << "Attempted to send command on a disconnected socket.\n";
// std::cerr << "Attempted to send command on a disconnected socket.\n";
RCLCPP_ERROR(logger_, "Attempted to send command on a disconnected socket.");
return false;
}
ssize_t result = send(*sockfd_, cmd.c_str(), cmd.length(), 0);
if (result < 0) {
// std::cerr << "Failed to send command: " << strerror(errno) << "\n";
RCLCPP_ERROR(logger_, "Failed to send command: %s", strerror(errno));
std::cerr << "Failed to send command: " << strerror(errno) << "\n";
return false;
}
@ -93,7 +94,8 @@ bool Robotiq2fSocketAdapter::sendCommand(const std::string& cmd) {
std::string Robotiq2fSocketAdapter::receiveResponse(int timeout_ms) {
std::lock_guard<std::mutex> lock(socket_mutex_);
if (*sockfd_ < 0) {
std::cerr << "Attempted to receive response on a disconnected socket.\n";
// std::cerr << "Attempted to send command on a disconnected socket.\n";
RCLCPP_ERROR(logger_, "Attempted to send command on a disconnected socket.");
return "";
}
@ -111,30 +113,37 @@ std::string Robotiq2fSocketAdapter::receiveResponse(int timeout_ms) {
FD_ZERO(&readfds);
FD_SET(*sockfd_, &readfds);
select(*sockfd_ + 1, &readfds, nullptr, nullptr, &tv);
/*
int activity = select(*sockfd_ + 1, &readfds, nullptr, nullptr, &tv);
if (activity == -1) {
std::cerr << "[receiveResponse] Error in select(): " << strerror(errno) << "\n";
// std::cerr << "[receiveResponse] Error in select(): " << strerror(errno) << "\n";
RCLCPP_ERROR(logger_, "[receiveResponse] Error in select(): %s", strerror(errno));
return "";
} else if (activity == 0) {
RCLCPP_ERROR(logger_, "[receiveResponse] Activity is Zero!");
return "";
}
}*/
ssize_t bytes_read = recv(*sockfd_, buffer, BUFFER_SIZE - 1, 0);
if (bytes_read < 0) {
if (errno != EAGAIN && errno != EWOULDBLOCK) {
std::cerr << "Failed to receive response: " << strerror(errno) << "\n";
// std::cerr << "Failed to receive response: " << strerror(errno) << "\n";
RCLCPP_ERROR(logger_, "Failed to receive response: %s", strerror(errno));
}
return "";
} else if (bytes_read == 0) {
std::cerr << "Connection closed by peer.\n";
// std::cerr << "Connection closed by peer.\n";
RCLCPP_ERROR(logger_, "Connection closed by peer.");
return "";
} else {
buffer[bytes_read] = '\0';
result += buffer;
RCLCPP_INFO(logger_, "Result: %s", result.c_str());
// Check if we have a complete response:
if (result.find("\n") != std::string::npos) {
complete_response = true;
} else if (result == "ack"){
complete_response = true;
}
}
}
@ -167,7 +176,7 @@ void Robotiq2fSocketAdapter::waitForGripperStatus(GripperStatus expectedStatus)
// Gripper variable Getter and Setter
bool Robotiq2fSocketAdapter::setGripperVariables(const std::map<std::string, unsigned int>& variableMap) {
// Build the command string
// Build the command string
std::string cmd = SET_COMMAND;
for (const auto& [variable, value] : variableMap) {
std::stringstream ss;
@ -189,7 +198,7 @@ bool Robotiq2fSocketAdapter::setGripperVariables(const std::map<std::string, uns
}
std::string response = receiveResponse();
RCLCPP_INFO(logger_, "[setGripperVariables]Command: %s and Response: %s", cmd.c_str(), response.c_str());
RCLCPP_DEBUG(logger_, "[setGripperVariables]Command: %s and Response: %s", cmd.c_str(), response.c_str());
return isAck(response);
}
@ -216,7 +225,7 @@ bool Robotiq2fSocketAdapter::setGripperVariables(const std::map<std::string, dou
}
std::string response = receiveResponse();
RCLCPP_INFO(logger_, "[setGripperVariables]Command: %s and Response: %s", cmd.c_str(), response.c_str());
RCLCPP_DEBUG(logger_, "[setGripperVariables]Command: %s and Response: %s", cmd.c_str(), response.c_str());
return isAck(response);
}
@ -237,7 +246,7 @@ bool Robotiq2fSocketAdapter::setGripperVariable(const std::string& variable, uns
}
std::string response = receiveResponse();
RCLCPP_INFO(logger_, "[setGripperVariable]Command: %s and Response: %s", cmd.c_str(), response.c_str());
RCLCPP_DEBUG(logger_, "[setGripperVariable]Command: %s and Response: %s", cmd.c_str(), response.c_str());
return isAck(response);
}
@ -258,7 +267,7 @@ bool Robotiq2fSocketAdapter::setGripperVariable(const std::string& variable, dou
}
std::string response = receiveResponse();
RCLCPP_INFO(logger_, "[setGripperVariable]Command: %s and Response: %s", cmd.c_str(), response.c_str());
RCLCPP_DEBUG(logger_, "[setGripperVariable]Command: %s and Response: %s", cmd.c_str(), response.c_str());
return isAck(response);
}
@ -274,7 +283,7 @@ int Robotiq2fSocketAdapter::getGripperVariable(const std::string& variable) {
return -1; // Error sending command
}
std::string response = receiveResponse();
RCLCPP_INFO(logger_, "[getGripperVariable]Command: %s and Response: %s", cmd.c_str(), response.c_str());
RCLCPP_DEBUG(logger_, "[getGripperVariable]Command: %s and Response: %s", cmd.c_str(), response.c_str());
std::istringstream iss(response);
std::string var_name, value_str;
@ -291,7 +300,8 @@ int Robotiq2fSocketAdapter::getGripperVariable(const std::string& variable) {
try {
return std::stoi(value_str);
} catch (const std::exception& ex) {
std::cerr << "Failed to parse gripper response: " << ex.what() << std::endl;
// std::cerr << "Failed to parse gripper response: " << ex.what() << std::endl;
RCLCPP_ERROR(logger_, "Failed to parse gripper response: %s", ex.what());
return -1;
}
}
@ -345,9 +355,6 @@ 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_INFO(logger_, "Clipped Position: %d, Clipped Speed: %d, Clipped Force: %d", clippedPosition, clippedSpeed, clippedForce);
// Create a map for gripper variables (similar to Python's OrderedDict)
std::map<std::string, unsigned int> variableMap = {
{ CMD_POS, clippedPosition },
@ -365,8 +372,6 @@ 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_);
RCLCPP_INFO(logger_, "Clipped Position: %d, Clipped Speed: %d, Clipped Force: %d", clippedPosition, clippedSpeed, clippedForce);
// Set gripper variables to initiate the move
std::map<std::string, unsigned int> variableMap = {
{ CMD_POS, clippedPosition },
@ -421,8 +426,9 @@ void Robotiq2fSocketAdapter::auto_calibrate(bool log) {
min_position_ = std::get<0>(result); // Update min position
if (log) {
std::cout << "Gripper auto-calibrated to ["
<< min_position_ << ", " << max_position_ << "]\n";
// std::cout << "Gripper auto-calibrated to ["
// << min_position_ << ", " << max_position_ << "]\n";
RCLCPP_INFO(logger_, "Gripper auto-calibrated to [%d, %d]", min_position_, max_position_);
}
}
} // end robotiq_interface