Compare commits
No commits in common. "8b3c40bbe7ca22bc9eaa9bd11eee184c9af1f6d2" and "08bec7a499b6927e73d4f0cf63c0ebfbaa876f58" have entirely different histories.
8b3c40bbe7
...
08bec7a499
25
README.md
25
README.md
@ -78,17 +78,11 @@ ros2 launch ur_robotiq_moveit_config ur_robotiq_moveit.launch.py use_fake_hardwa
|
|||||||
```
|
```
|
||||||
|
|
||||||
### servoing
|
### servoing
|
||||||
#### PS5 controller
|
|
||||||
Terminal 3:\
|
Terminal 3:\
|
||||||
**At the moment the servoing doesnt work with the collision checker (self collision still works).**
|
**At the moment the servoing doesnt work with the collision checker (self collision still works).**
|
||||||
```bash
|
```bash
|
||||||
ros2 launch ur_robotiq_servo ps5_servo.launch.py
|
ros2 launch ur_robotiq_servo ps5_servo.launch.py
|
||||||
```
|
```
|
||||||
#### Keyboard
|
|
||||||
Terminal 3:
|
|
||||||
```bash
|
|
||||||
ros2 run servo_keyboard servo_keyboard_input
|
|
||||||
```
|
|
||||||
|
|
||||||
After launching you need to change the controller to enable the servo mode.\
|
After launching you need to change the controller to enable the servo mode.\
|
||||||
Terminal 4:
|
Terminal 4:
|
||||||
@ -97,23 +91,4 @@ ros2 control switch_controllers --deactivate joint_trajectory_controller
|
|||||||
ros2 control switch_controllers --deactivate robotiq_gripper_joint_trajectory_controller
|
ros2 control switch_controllers --deactivate robotiq_gripper_joint_trajectory_controller
|
||||||
ros2 control switch_controllers --activate forward_position_controller
|
ros2 control switch_controllers --activate forward_position_controller
|
||||||
ros2 control switch_controllers --activate forward_gripper_position_controller
|
ros2 control switch_controllers --activate forward_gripper_position_controller
|
||||||
```
|
|
||||||
|
|
||||||
For the real robot you need to change to the scaled position controller:
|
|
||||||
```bash
|
|
||||||
ros2 control switch_controllers --deactivate scaled_joint_trajectory_controller
|
|
||||||
ros2 control switch_controllers --deactivate robotiq_gripper_joint_trajectory_controller
|
|
||||||
ros2 control switch_controllers --activate scaled_forward_position_controller
|
|
||||||
ros2 control switch_controllers --activate forward_gripper_position_controller
|
|
||||||
```
|
|
||||||
|
|
||||||
Terminal 5:
|
|
||||||
Activate the Servo Node
|
|
||||||
```bash
|
|
||||||
ros2 service call ros2 service call /servo_node/start_servo std_srvs/srv/Trigger {}
|
|
||||||
```
|
|
||||||
|
|
||||||
### extract the urdf from xacro file
|
|
||||||
```bash
|
|
||||||
ros2 run xacro xacro src/ur_robotiq_description/urdf/ur_robotiq.urdf.xacro > src/ur_robotiq_description/urdf/ur_robotiq.urdf name:=ur3e
|
|
||||||
```
|
```
|
||||||
@ -7,14 +7,7 @@ repositories:
|
|||||||
type: git
|
type: git
|
||||||
url: https://github.com/NikoFeith/Universal_Robots_ROS2_Driver.git
|
url: https://github.com/NikoFeith/Universal_Robots_ROS2_Driver.git
|
||||||
version: humble
|
version: humble
|
||||||
serial:
|
|
||||||
type: git
|
|
||||||
url: https://github.com/tylerjw/serial.git
|
|
||||||
version: ros2
|
|
||||||
ros2_robotiq_gripper:
|
|
||||||
type: git
|
|
||||||
url: https://github.com/PickNikRobotics/ros2_robotiq_gripper.git
|
|
||||||
version: main
|
|
||||||
cartesian_controllers:
|
cartesian_controllers:
|
||||||
type: git
|
type: git
|
||||||
url: https://github.com/NikoFeith/cartesian_controllers.git
|
url: https://github.com/NikoFeith/cartesian_controllers.git
|
||||||
|
version: ros2
|
||||||
0
src/robotiq_2f/README.md
Normal file
0
src/robotiq_2f/README.md
Normal file
20
src/robotiq_2f/robotiq_2f_description/CMakeLists.txt
Normal file
20
src/robotiq_2f/robotiq_2f_description/CMakeLists.txt
Normal file
@ -0,0 +1,20 @@
|
|||||||
|
cmake_minimum_required(VERSION 3.8)
|
||||||
|
project(robotiq_2f_description)
|
||||||
|
|
||||||
|
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||||
|
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||||
|
endif()
|
||||||
|
|
||||||
|
find_package(ament_cmake REQUIRED)
|
||||||
|
|
||||||
|
install(
|
||||||
|
DIRECTORY
|
||||||
|
config
|
||||||
|
launch
|
||||||
|
meshes
|
||||||
|
rviz
|
||||||
|
urdf
|
||||||
|
DESTINATION
|
||||||
|
share/${PROJECT_NAME}
|
||||||
|
)
|
||||||
|
ament_package()
|
||||||
@ -0,0 +1,8 @@
|
|||||||
|
robotiq_2f_gripper:
|
||||||
|
# Physical limits
|
||||||
|
min_position: 0.0 # Meters (fully closed)
|
||||||
|
max_position: 0.14 # Meters (fully open)
|
||||||
|
min_speed: 0.02 # Meters per second
|
||||||
|
max_speed: 0.15 # Meters per second
|
||||||
|
min_force: 20.0 # Newtons
|
||||||
|
max_force: 235.0 # Newtons
|
||||||
@ -0,0 +1,8 @@
|
|||||||
|
robotiq_2f_gripper:
|
||||||
|
# Physical limits
|
||||||
|
min_position: 0.0 # Meters (fully closed)
|
||||||
|
max_position: 0.085 # Meters (fully open)
|
||||||
|
min_speed: 0.02 # Meters per second
|
||||||
|
max_speed: 0.15 # Meters per second
|
||||||
|
min_force: 20.0 # Newtons
|
||||||
|
max_force: 235.0 # Newtons
|
||||||
@ -0,0 +1,20 @@
|
|||||||
|
controller_manager:
|
||||||
|
ros__parameters:
|
||||||
|
update_rate: 500 # Hz
|
||||||
|
joint_state_broadcaster:
|
||||||
|
type: joint_state_broadcaster/JointStateBroadcaster
|
||||||
|
robotiq_gripper_controller:
|
||||||
|
type: position_controllers/GripperActionController
|
||||||
|
robotiq_activation_controller:
|
||||||
|
type: robotiq_controllers/RobotiqActivationController
|
||||||
|
|
||||||
|
robotiq_gripper_controller:
|
||||||
|
ros__parameters:
|
||||||
|
default: true
|
||||||
|
joint: finger_joint
|
||||||
|
use_effort_interface: true
|
||||||
|
use_speed_interface: true
|
||||||
|
|
||||||
|
robotiq_activation_controller:
|
||||||
|
ros__parameters:
|
||||||
|
default: true
|
||||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
32
src/robotiq_2f/robotiq_2f_description/package.xml
Normal file
32
src/robotiq_2f/robotiq_2f_description/package.xml
Normal file
@ -0,0 +1,32 @@
|
|||||||
|
<?xml version="1.0"?>
|
||||||
|
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||||
|
<package format="3">
|
||||||
|
<name>robotiq_2f_description</name>
|
||||||
|
<version>0.0.0</version>
|
||||||
|
<description>TODO: Package description</description>
|
||||||
|
<maintainer email="nikolaus.feith@unileoben.ac.at">niko</maintainer>
|
||||||
|
<license>TODO: License declaration</license>
|
||||||
|
|
||||||
|
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||||
|
|
||||||
|
<exec_depend>joint_state_publisher_gui</exec_depend>
|
||||||
|
<exec_depend>launch</exec_depend>
|
||||||
|
<exec_depend>launch_ros</exec_depend>
|
||||||
|
<exec_depend>robot_state_publisher</exec_depend>
|
||||||
|
<exec_depend>rviz2</exec_depend>
|
||||||
|
<exec_depend>urdf</exec_depend>
|
||||||
|
<exec_depend>xacro</exec_depend>
|
||||||
|
|
||||||
|
<exec_depend>ros2_control</exec_depend>
|
||||||
|
<exec_depend>ros2_controllers</exec_depend>
|
||||||
|
|
||||||
|
<exec_depend>gripper_controllers</exec_depend>
|
||||||
|
<exec_depend>joint_state_broadcaster</exec_depend>
|
||||||
|
|
||||||
|
<test_depend>ament_lint_auto</test_depend>
|
||||||
|
<test_depend>ament_lint_common</test_depend>
|
||||||
|
|
||||||
|
<export>
|
||||||
|
<build_type>ament_cmake</build_type>
|
||||||
|
</export>
|
||||||
|
</package>
|
||||||
@ -0,0 +1,97 @@
|
|||||||
|
<?xml version="1.0"?>
|
||||||
|
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||||
|
|
||||||
|
<xacro:macro name="robotiq_gripper_ros2_control" params="
|
||||||
|
name
|
||||||
|
prefix
|
||||||
|
use_fake_hardware
|
||||||
|
fake_sensor_commands
|
||||||
|
robot_ip
|
||||||
|
robot_port
|
||||||
|
min_position
|
||||||
|
max_position
|
||||||
|
min_speed
|
||||||
|
max_speed
|
||||||
|
min_force
|
||||||
|
max_force">
|
||||||
|
|
||||||
|
<ros2_control name="${name}" type="system">
|
||||||
|
<!-- Plugins -->
|
||||||
|
<hardware>
|
||||||
|
<plugin>robotiq_2f_interface/RobotiqGripperHardwareInterface</plugin>
|
||||||
|
<xacro:if value="${use_fake_hardware}">
|
||||||
|
<param name="fake_sensor_commands">${fake_sensor_commands}</param>
|
||||||
|
<param name="state_following_offset">0.0</param>
|
||||||
|
</xacro:if>
|
||||||
|
<param name="gripper_closed_position">0.695</param>
|
||||||
|
<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>
|
||||||
|
</hardware>
|
||||||
|
|
||||||
|
<!-- Joint interfaces -->
|
||||||
|
<!-- With Hardware, they handle mimic joints, so we only need this command interface activated -->
|
||||||
|
<joint name="${prefix}finger_joint">
|
||||||
|
<command_interface name="position" />
|
||||||
|
<state_interface name="position">
|
||||||
|
<param name="initial_value">0.695</param>
|
||||||
|
</state_interface>
|
||||||
|
<state_interface name="velocity"/>
|
||||||
|
</joint>
|
||||||
|
<!-- When simulating we need to include the rest of the gripper joints -->
|
||||||
|
<xacro:if value="false">
|
||||||
|
<joint name="${prefix}left_inner_knuckle_joint">
|
||||||
|
<param name="mimic">${prefix}finger_joint</param>
|
||||||
|
<param name="multiplier">-1</param>
|
||||||
|
<command_interface name="position"/>
|
||||||
|
<state_interface name="position"/>
|
||||||
|
<state_interface name="velocity"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="${prefix}left_inner_finger_joint">
|
||||||
|
<param name="mimic">${prefix}finger_joint</param>
|
||||||
|
<param name="multiplier">1</param>
|
||||||
|
<command_interface name="position"/>
|
||||||
|
<state_interface name="position"/>
|
||||||
|
<state_interface name="velocity"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="${prefix}right_outer_knuckle_joint">
|
||||||
|
<param name="mimic">${prefix}finger_joint</param>
|
||||||
|
<param name="multiplier">-1</param>
|
||||||
|
<command_interface name="position"/>
|
||||||
|
<state_interface name="position"/>
|
||||||
|
<state_interface name="velocity"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="${prefix}robotiq_140_left_finger_tip_joint">
|
||||||
|
<param name="mimic">${prefix}robotiq_140_left_knuckle_joint</param>
|
||||||
|
<param name="multiplier">-1</param>
|
||||||
|
<command_interface name="position"/>
|
||||||
|
<state_interface name="position"/>
|
||||||
|
<state_interface name="velocity"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="${prefix}robotiq_140_right_finger_tip_joint">
|
||||||
|
<param name="mimic">${prefix}robotiq_140_left_knuckle_joint</param>
|
||||||
|
<param name="multiplier">1</param>
|
||||||
|
<command_interface name="position"/>
|
||||||
|
<state_interface name="position"/>
|
||||||
|
<state_interface name="velocity"/>
|
||||||
|
</joint>
|
||||||
|
</xacro:if>
|
||||||
|
|
||||||
|
<!-- Only add this with fake hardware mode -->
|
||||||
|
<gpio name="reactivate_gripper">
|
||||||
|
<command_interface name="reactivate_gripper_cmd" />
|
||||||
|
<command_interface name="reactivate_gripper_response" />
|
||||||
|
</gpio>
|
||||||
|
|
||||||
|
</ros2_control>
|
||||||
|
</xacro:macro>
|
||||||
|
|
||||||
|
</robot>
|
||||||
@ -0,0 +1,13 @@
|
|||||||
|
<?xml version="1.0"?>
|
||||||
|
<robot xmlns:xacro="http://wiki.ros.org/xacro" name="robotiq_gripper">
|
||||||
|
<!-- parameters -->
|
||||||
|
<xacro:arg name="use_fake_hardware" default="false" />
|
||||||
|
|
||||||
|
<!-- Import macros -->
|
||||||
|
<xacro:include filename="$(find robotiq_2f_description)/urdf/robotiq_2f_140_macro.urdf.xacro" />
|
||||||
|
|
||||||
|
<link name="world" />
|
||||||
|
<xacro:robotiq_gripper name="RobotiqGripperHardwareInterface" prefix="" parent="world" use_fake_hardware="$(arg use_fake_hardware)">
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
</xacro:robotiq_gripper>
|
||||||
|
</robot>
|
||||||
@ -0,0 +1,437 @@
|
|||||||
|
<?xml version="1.0"?>
|
||||||
|
<robot xmlns:xacro="http://wiki.ros.org/xacro" name="robotiq_gripper">
|
||||||
|
<xacro:macro name="robotiq_gripper" params="
|
||||||
|
name
|
||||||
|
prefix
|
||||||
|
parent
|
||||||
|
*origin
|
||||||
|
include_ros2_control:=true
|
||||||
|
use_fake_hardware:=false
|
||||||
|
fake_sensor_commands:=false
|
||||||
|
robot_ip:=192.168.1.1
|
||||||
|
robot_port:=63352">
|
||||||
|
|
||||||
|
|
||||||
|
<xacro:property name="config_file" value="$(find robotiq_2f_description)/config/robotiq_2f_140_config.yaml" />
|
||||||
|
<xacro:property name="config_dict" value="${xacro.load_yaml(config_file)}" />
|
||||||
|
<xacro:property name="robotiq_2f_gripper_dict" value="${config_dict['robotiq_2f_gripper']}" />
|
||||||
|
<xacro:property name="min_position" value="${robotiq_2f_gripper_dict['min_position']}" />
|
||||||
|
<xacro:property name="max_position" value="${robotiq_2f_gripper_dict['max_position']}" />
|
||||||
|
<xacro:property name="min_speed" value="${robotiq_2f_gripper_dict['min_speed']}" />
|
||||||
|
<xacro:property name="max_speed" value="${robotiq_2f_gripper_dict['max_speed']}" />
|
||||||
|
<xacro:property name="min_force" value="${robotiq_2f_gripper_dict['min_force']}" />
|
||||||
|
<xacro:property name="max_force" value="${robotiq_2f_gripper_dict['max_force']}" />
|
||||||
|
<!-- ros2 control include -->
|
||||||
|
<xacro:include filename="$(find robotiq_2f_description)/urdf/robotiq_2f_140.ros2_control.xacro" />
|
||||||
|
<!-- if we are simulating or directly communicating with the gripper we need a ros2 control instance -->
|
||||||
|
<xacro:if value="${include_ros2_control}">
|
||||||
|
<xacro:robotiq_gripper_ros2_control
|
||||||
|
name="${name}" prefix="${prefix}"
|
||||||
|
use_fake_hardware="${use_fake_hardware}"
|
||||||
|
fake_sensor_commands="${fake_sensor_commands}"
|
||||||
|
robot_ip="${robot_ip}"
|
||||||
|
robot_port="${robot_port}"
|
||||||
|
min_position="${min_position}"
|
||||||
|
max_position="${max_position}"
|
||||||
|
min_speed="${min_speed}"
|
||||||
|
max_speed="${max_speed}"
|
||||||
|
min_force="${min_force}"
|
||||||
|
max_force="${max_force}"/>
|
||||||
|
</xacro:if>
|
||||||
|
|
||||||
|
<!-- this is a temporary link to rotate the 2f-140 gripper to match the 2f-85 -->
|
||||||
|
<link name="${prefix}robotiq_base_link"/>
|
||||||
|
<joint name="${prefix}robotiq_base_joint" type="fixed">
|
||||||
|
<parent link="${parent}" />
|
||||||
|
<child link="${prefix}robotiq_base_link" />
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 ${pi/2}" />
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<link name="${prefix}robotiq_140_base_link">
|
||||||
|
<inertial>
|
||||||
|
<origin xyz="8.625E-08 -4.6583E-06 0.03145" rpy="0 0 0" />
|
||||||
|
<mass value="0.22652" />
|
||||||
|
<inertia ixx="0.00020005" ixy="-4.2442E-10" ixz="-2.9069E-10" iyy="0.00017832" iyz="-3.4402E-08" izz="0.00013478" />
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_140/robotiq_2f_140_base_link.stl" />
|
||||||
|
</geometry>
|
||||||
|
<material name="">
|
||||||
|
<color rgba="0.1 0.1 0.1 1" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_140/robotiq_2f_140_base_link.stl" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<link name="${prefix}robotiq_140_left_outer_knuckle">
|
||||||
|
<inertial>
|
||||||
|
<origin xyz="-0.000200000000003065 0.0199435877845359 0.0292245259211331" rpy="0 0 0" />
|
||||||
|
<mass value="0.00853198276973456" />
|
||||||
|
<inertia
|
||||||
|
ixx="2.89328108496468E-06"
|
||||||
|
ixy="-1.57935047237397E-19"
|
||||||
|
ixz="-1.93980378593255E-19"
|
||||||
|
iyy="1.86719750325683E-06"
|
||||||
|
iyz="-1.21858577871576E-06"
|
||||||
|
izz="1.21905238907251E-06" />
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_140/robotiq_2f_140_outer_knuckle.stl" />
|
||||||
|
</geometry>
|
||||||
|
<material name="">
|
||||||
|
<color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_140/robotiq_2f_140_outer_knuckle.stl" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<link name="${prefix}robotiq_140_right_outer_knuckle">
|
||||||
|
<inertial>
|
||||||
|
<origin xyz="-0.000200000000003065 0.0199435877845359 0.0292245259211331" rpy="0 0 0" />
|
||||||
|
<mass value="0.00853198276973456" />
|
||||||
|
<inertia
|
||||||
|
ixx="2.89328108496468E-06"
|
||||||
|
ixy="-1.57935047237397E-19"
|
||||||
|
ixz="-1.93980378593255E-19"
|
||||||
|
iyy="1.86719750325683E-06"
|
||||||
|
iyz="-1.21858577871576E-06"
|
||||||
|
izz="1.21905238907251E-06" />
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_140/robotiq_2f_140_outer_knuckle.stl" />
|
||||||
|
</geometry>
|
||||||
|
<material name="">
|
||||||
|
<color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_140/robotiq_2f_140_outer_knuckle.stl" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<link name="${prefix}robotiq_140_left_outer_finger">
|
||||||
|
<inertial>
|
||||||
|
<origin xyz="0.00030115855001899 0.0373907951953854 -0.0208027427000385" rpy="0 0 0" />
|
||||||
|
<mass value="0.022614240507152" />
|
||||||
|
<inertia
|
||||||
|
ixx="1.52518312458174E-05"
|
||||||
|
ixy="9.76583423954399E-10"
|
||||||
|
ixz="-5.43838577022588E-10"
|
||||||
|
iyy="6.17694243867776E-06"
|
||||||
|
iyz="6.78636130740228E-06"
|
||||||
|
izz="1.16494917907219E-05" />
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_140/robotiq_2f_140_outer_finger.stl" />
|
||||||
|
</geometry>
|
||||||
|
<material name="">
|
||||||
|
<color rgba="0.1 0.1 0.1 1" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_140/robotiq_2f_140_outer_finger.stl" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<link name="${prefix}robotiq_140_right_outer_finger">
|
||||||
|
<inertial>
|
||||||
|
<origin xyz="0.00030115855001899 0.0373907951953854 -0.0208027427000385" rpy="0 0 0" />
|
||||||
|
<mass value="0.022614240507152" />
|
||||||
|
<inertia
|
||||||
|
ixx="1.52518312458174E-05"
|
||||||
|
ixy="9.76583423954399E-10"
|
||||||
|
ixz="-5.43838577022588E-10"
|
||||||
|
iyy="6.17694243867776E-06"
|
||||||
|
iyz="6.78636130740228E-06"
|
||||||
|
izz="1.16494917907219E-05" />
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_140/robotiq_2f_140_outer_finger.stl" />
|
||||||
|
</geometry>
|
||||||
|
<material name="">
|
||||||
|
<color rgba="0.1 0.1 0.1 1" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_140/robotiq_2f_140_outer_finger.stl" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<link name="${prefix}robotiq_140_left_inner_knuckle">
|
||||||
|
<inertial>
|
||||||
|
<origin xyz="0.000123011831763771 0.0507850843201817 0.00103968640075166" rpy="0 0 0" />
|
||||||
|
<mass value="0.0271177346495152" />
|
||||||
|
<inertia
|
||||||
|
ixx="2.61910379223783E-05"
|
||||||
|
ixy="-2.43616858946494E-07"
|
||||||
|
ixz="-6.37789906117123E-09"
|
||||||
|
iyy="2.8270243746167E-06"
|
||||||
|
iyz="-5.37200748039765E-07"
|
||||||
|
izz="2.83695868220296E-05" />
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_140/robotiq_2f_140_inner_knuckle.stl" />
|
||||||
|
</geometry>
|
||||||
|
<material name="">
|
||||||
|
<color rgba="0.1 0.1 0.1 1" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_140/robotiq_2f_140_inner_knuckle.stl" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<link name="${prefix}robotiq_140_right_inner_knuckle">
|
||||||
|
<inertial>
|
||||||
|
<origin xyz="0.000123011831763771 0.0507850843201817 0.00103968640075166" rpy="0 0 0" />
|
||||||
|
<mass value="0.0271177346495152" />
|
||||||
|
<inertia
|
||||||
|
ixx="2.61910379223783E-05"
|
||||||
|
ixy="-2.43616858946494E-07"
|
||||||
|
ixz="-6.37789906117123E-09"
|
||||||
|
iyy="2.8270243746167E-06"
|
||||||
|
iyz="-5.37200748039765E-07"
|
||||||
|
izz="2.83695868220296E-05" />
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_140/robotiq_2f_140_inner_knuckle.stl" />
|
||||||
|
</geometry>
|
||||||
|
<material name="">
|
||||||
|
<color rgba="0.1 0.1 0.1 1" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_140/robotiq_2f_140_inner_knuckle.stl" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<link name="${prefix}robotiq_140_left_inner_finger">
|
||||||
|
<inertial>
|
||||||
|
<origin xyz="0.000299999999999317 0.0160078233491243 -0.0136945669206257" rpy="0 0 0" />
|
||||||
|
<mass value="0.0104003125914103" />
|
||||||
|
<inertia
|
||||||
|
ixx="2.71909453810972E-06"
|
||||||
|
ixy="1.35402465472579E-21"
|
||||||
|
ixz="-7.1817349065269E-22"
|
||||||
|
iyy="7.69100314106116E-07"
|
||||||
|
iyz="6.74715432769696E-07"
|
||||||
|
izz="2.30315190420171E-06" />
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_140/robotiq_2f_140_inner_finger.stl" />
|
||||||
|
</geometry>
|
||||||
|
<material name="">
|
||||||
|
<color rgba="0.1 0.1 0.1 1" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_140/robotiq_2f_140_inner_finger.stl" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<link name="${prefix}robotiq_140_right_inner_finger">
|
||||||
|
<inertial>
|
||||||
|
<origin xyz="0.000299999999999317 0.0160078233491243 -0.0136945669206257" rpy="0 0 0" />
|
||||||
|
<mass value="0.0104003125914103" />
|
||||||
|
<inertia
|
||||||
|
ixx="2.71909453810972E-06"
|
||||||
|
ixy="1.35402465472579E-21"
|
||||||
|
ixz="-7.1817349065269E-22"
|
||||||
|
iyy="7.69100314106116E-07"
|
||||||
|
iyz="6.74715432769696E-07"
|
||||||
|
izz="2.30315190420171E-06" />
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_140/robotiq_2f_140_inner_finger.stl" />
|
||||||
|
</geometry>
|
||||||
|
<material name="">
|
||||||
|
<color rgba="0.1 0.1 0.1 1" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_140/robotiq_2f_140_inner_finger.stl" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<link name="${prefix}robotiq_140_left_inner_finger_pad">
|
||||||
|
<visual>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<box size="0.027 0.065 0.0075"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="">
|
||||||
|
<color rgba="0.9 0.9 0.9 1" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<box size="0.03 0.07 0.0075"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="">
|
||||||
|
<color rgba="0.9 0.0 0.0 1" />
|
||||||
|
</material>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<link name="${prefix}robotiq_140_right_inner_finger_pad">
|
||||||
|
<visual>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<box size="0.027 0.065 0.0075"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="">
|
||||||
|
<color rgba="0.9 0.9 0.9 1" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<box size="0.03 0.07 0.0075"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="">
|
||||||
|
<color rgba="0.9 0.0 0.0 1" />
|
||||||
|
</material>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
<!-- Joints-->
|
||||||
|
<joint name="${prefix}robotiq_140_base_joint" type="fixed">
|
||||||
|
<parent link="${prefix}robotiq_base_link" />
|
||||||
|
<child link="${prefix}robotiq_140_base_link" />
|
||||||
|
<xacro:insert_block name="origin" />
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<joint name="${prefix}finger_joint" type="revolute">
|
||||||
|
<origin xyz="0 -0.030601 0.054905" rpy="${pi / 2 + .725} 0 0" />
|
||||||
|
<parent link="${prefix}robotiq_140_base_link" />
|
||||||
|
<child link="${prefix}robotiq_140_left_outer_knuckle" />
|
||||||
|
<axis xyz="-1 0 0" />
|
||||||
|
<limit lower="0" upper="0.7" velocity="2.0" effort="1000" />
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<joint name="${prefix}right_outer_knuckle_joint" type="revolute">
|
||||||
|
<origin xyz="0 0.030601 0.054905" rpy="${pi / 2 + .725} 0 ${pi}" />
|
||||||
|
<parent link="${prefix}robotiq_140_base_link" />
|
||||||
|
<child link="${prefix}robotiq_140_right_outer_knuckle" />
|
||||||
|
<axis xyz="1 0 0" />
|
||||||
|
<limit lower="-0.725" upper="0.725" velocity="2.0" effort="1000" />
|
||||||
|
<mimic joint="${prefix}finger_joint" multiplier="-1" offset="0" />
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<joint name="${prefix}left_outer_finger_joint" type="fixed">
|
||||||
|
<origin xyz="0 0.01821998610742 0.0260018192872234" rpy="0 0 0" />
|
||||||
|
<parent link="${prefix}robotiq_140_left_outer_knuckle" />
|
||||||
|
<child link="${prefix}robotiq_140_left_outer_finger" />
|
||||||
|
<axis xyz="1 0 0" />
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<joint name="${prefix}right_outer_finger_joint" type="fixed">
|
||||||
|
<origin xyz="0 0.01821998610742 0.0260018192872234" rpy="0 0 0" />
|
||||||
|
<parent link="${prefix}robotiq_140_right_outer_knuckle" />
|
||||||
|
<child link="${prefix}robotiq_140_right_outer_finger" />
|
||||||
|
<axis xyz="1 0 0" />
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<joint name="${prefix}left_inner_knuckle_joint" type="revolute">
|
||||||
|
<origin xyz="0 -0.0127 0.06142" rpy="${pi / 2 + .725} 0 0" />
|
||||||
|
<parent link="${prefix}robotiq_140_base_link" />
|
||||||
|
<child link="${prefix}robotiq_140_left_inner_knuckle" />
|
||||||
|
<axis xyz="1 0 0" />
|
||||||
|
<limit lower="-0.8757" upper="0.8757" velocity="2.0" effort="1000" />
|
||||||
|
<mimic joint="${prefix}finger_joint" multiplier="-1" offset="0" />
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<joint name="${prefix}right_inner_knuckle_joint" type="revolute">
|
||||||
|
<origin xyz="0 0.0127 0.06142" rpy="${pi / 2 + .725} 0 ${-pi}" />
|
||||||
|
<parent link="${prefix}robotiq_140_base_link" />
|
||||||
|
<child link="${prefix}robotiq_140_right_inner_knuckle" />
|
||||||
|
<axis xyz="1 0 0" />
|
||||||
|
<limit lower="-0.8757" upper="0.8757" velocity="2.0" effort="1000" />
|
||||||
|
<mimic joint="${prefix}finger_joint" multiplier="-1" offset="0" />
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<joint name="${prefix}left_inner_finger_joint" type="revolute">
|
||||||
|
<origin xyz="0 0.0817554015893473 -0.0282203446692936" rpy="-0.725 0 0" />
|
||||||
|
<parent link="${prefix}robotiq_140_left_outer_finger" />
|
||||||
|
<child link="${prefix}robotiq_140_left_inner_finger" />
|
||||||
|
<axis xyz="1 0 0" />
|
||||||
|
<limit lower="-0.8757" upper="0.8757" velocity="2.0" effort="1000" />
|
||||||
|
<mimic joint="${prefix}finger_joint" multiplier="1" offset="0" />
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<joint name="${prefix}right_inner_finger_joint" type="revolute">
|
||||||
|
<origin xyz="0 0.0817554015893473 -0.0282203446692936" rpy="-0.725 0 0" />
|
||||||
|
<parent link="${prefix}robotiq_140_right_outer_finger" />
|
||||||
|
<child link="${prefix}robotiq_140_right_inner_finger" />
|
||||||
|
<axis xyz="1 0 0" />
|
||||||
|
<limit lower="-0.8757" upper="0.8757" velocity="2.0" effort="1000" />
|
||||||
|
<mimic joint="${prefix}finger_joint" multiplier="1" offset="0" />
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<joint name="${prefix}left_inner_finger_pad_joint" type="fixed">
|
||||||
|
<origin xyz="0 0.0457554015893473 -0.0272203446692936" rpy="0 0 0" />
|
||||||
|
<parent link="${prefix}robotiq_140_left_inner_finger" />
|
||||||
|
<child link="${prefix}robotiq_140_left_inner_finger_pad" />
|
||||||
|
<axis xyz="0 0 1" />
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<joint name="${prefix}right_inner_finger_pad_joint" type="fixed">
|
||||||
|
<origin xyz="0 0.0457554015893473 -0.0272203446692936" rpy="0 0 0" />
|
||||||
|
<parent link="${prefix}robotiq_140_right_inner_finger" />
|
||||||
|
<child link="${prefix}robotiq_140_right_inner_finger_pad" />
|
||||||
|
<axis xyz="0 0 1" />
|
||||||
|
</joint>
|
||||||
|
</xacro:macro>
|
||||||
|
</robot>
|
||||||
@ -0,0 +1,100 @@
|
|||||||
|
<?xml version="1.0"?>
|
||||||
|
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||||
|
|
||||||
|
<xacro:macro name="robotiq_gripper_ros2_control" params="
|
||||||
|
name
|
||||||
|
prefix
|
||||||
|
use_fake_hardware
|
||||||
|
fake_sensor_commands
|
||||||
|
robot_ip
|
||||||
|
robot_port
|
||||||
|
min_position
|
||||||
|
max_position
|
||||||
|
min_speed
|
||||||
|
max_speed
|
||||||
|
min_force
|
||||||
|
max_force">
|
||||||
|
|
||||||
|
<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>
|
||||||
|
<param name="state_following_offset">0.0</param>
|
||||||
|
</xacro:if>
|
||||||
|
<xacro:unless value="${use_fake_hardware}">
|
||||||
|
<plugin>robotiq_2f_interface/RobotiqGripperHardwareInterface</plugin>
|
||||||
|
<param name="gripper_closed_position">0.7929</param>
|
||||||
|
<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>
|
||||||
|
</hardware>
|
||||||
|
|
||||||
|
<!-- Joint interfaces -->
|
||||||
|
<!-- With Hardware, they handle mimic joints, so we only need this command interface activated -->
|
||||||
|
<joint name="${prefix}finger_joint">
|
||||||
|
<command_interface name="position" />
|
||||||
|
<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}">
|
||||||
|
<joint name="${prefix}left_inner_knuckle_joint">
|
||||||
|
<param name="mimic">${prefix}finger_joint</param>
|
||||||
|
<param name="multiplier">-1</param>
|
||||||
|
<command_interface name="position"/>
|
||||||
|
<state_interface name="position"/>
|
||||||
|
<state_interface name="velocity"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="${prefix}left_inner_finger_joint">
|
||||||
|
<param name="mimic">${prefix}finger_joint</param>
|
||||||
|
<param name="multiplier">1</param>
|
||||||
|
<command_interface name="position"/>
|
||||||
|
<state_interface name="position"/>
|
||||||
|
<state_interface name="velocity"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="${prefix}right_outer_knuckle_joint">
|
||||||
|
<param name="mimic">${prefix}finger_joint</param>
|
||||||
|
<param name="multiplier">-1</param>
|
||||||
|
<command_interface name="position"/>
|
||||||
|
<state_interface name="position"/>
|
||||||
|
<state_interface name="velocity"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="${prefix}robotiq_140_left_finger_tip_joint">
|
||||||
|
<param name="mimic">${prefix}robotiq_140_left_knuckle_joint</param>
|
||||||
|
<param name="multiplier">-1</param>
|
||||||
|
<command_interface name="position"/>
|
||||||
|
<state_interface name="position"/>
|
||||||
|
<state_interface name="velocity"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="${prefix}robotiq_140_right_finger_tip_joint">
|
||||||
|
<param name="mimic">${prefix}robotiq_140_left_knuckle_joint</param>
|
||||||
|
<param name="multiplier">1</param>
|
||||||
|
<command_interface name="position"/>
|
||||||
|
<state_interface name="position"/>
|
||||||
|
<state_interface name="velocity"/>
|
||||||
|
</joint>
|
||||||
|
</xacro:if>
|
||||||
|
|
||||||
|
<!-- Only add this with fake hardware mode -->
|
||||||
|
<gpio name="reactivate_gripper">
|
||||||
|
<command_interface name="reactivate_gripper_cmd" />
|
||||||
|
<command_interface name="reactivate_gripper_response" />
|
||||||
|
</gpio>
|
||||||
|
|
||||||
|
</ros2_control>
|
||||||
|
</xacro:macro>
|
||||||
|
|
||||||
|
</robot>
|
||||||
@ -0,0 +1,13 @@
|
|||||||
|
<?xml version="1.0"?>
|
||||||
|
<robot xmlns:xacro="http://wiki.ros.org/xacro" name="robotiq_gripper">
|
||||||
|
<!-- parameters -->
|
||||||
|
<xacro:arg name="use_fake_hardware" default="false" />
|
||||||
|
|
||||||
|
<!-- Import macros -->
|
||||||
|
<xacro:include filename="$(find robotiq_2f_description)/urdf/robotiq_2f_85_macro.urdf.xacro" />
|
||||||
|
|
||||||
|
<link name="world" />
|
||||||
|
<xacro:robotiq_gripper name="RobotiqGripperHardwareInterface" prefix="" parent="world" use_fake_hardware="$(arg use_fake_hardware)">
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
</xacro:robotiq_gripper>
|
||||||
|
</robot>
|
||||||
@ -0,0 +1,307 @@
|
|||||||
|
<?xml version="1.0"?>
|
||||||
|
<robot xmlns:xacro="http://wiki.ros.org/xacro" name="robotiq_gripper">
|
||||||
|
<xacro:macro name="robotiq_gripper" params="
|
||||||
|
name
|
||||||
|
prefix
|
||||||
|
parent
|
||||||
|
*origin
|
||||||
|
include_ros2_control:=true
|
||||||
|
use_fake_hardware:=false
|
||||||
|
fake_sensor_commands:=false
|
||||||
|
robot_ip:=192.168.1.1
|
||||||
|
robot_port:=63352">
|
||||||
|
|
||||||
|
<xacro:property name="config_file" value="$(find robotiq_2f_description)/config/robotiq_2f_85_config.yaml" />
|
||||||
|
<xacro:property name="config_dict" value="${xacro.load_yaml(config_file)}" />
|
||||||
|
<xacro:property name="robotiq_2f_gripper_dict" value="${config_dict['robotiq_2f_gripper']}" />
|
||||||
|
<xacro:property name="min_position" value="${robotiq_2f_gripper_dict['min_position']}" />
|
||||||
|
<xacro:property name="max_position" value="${robotiq_2f_gripper_dict['max_position']}" />
|
||||||
|
<xacro:property name="min_speed" value="${robotiq_2f_gripper_dict['min_speed']}" />
|
||||||
|
<xacro:property name="max_speed" value="${robotiq_2f_gripper_dict['max_speed']}" />
|
||||||
|
<xacro:property name="min_force" value="${robotiq_2f_gripper_dict['min_force']}" />
|
||||||
|
<xacro:property name="max_force" value="${robotiq_2f_gripper_dict['max_force']}" />
|
||||||
|
|
||||||
|
<!-- ros2 control include -->
|
||||||
|
<xacro:include filename="$(find robotiq_2f_description)/urdf/robotiq_2f_85.ros2_control.xacro" />
|
||||||
|
<!-- if we are simulating or directly communicating with the gripper we need a ros2 control instance -->
|
||||||
|
<xacro:if value="${include_ros2_control}">
|
||||||
|
<xacro:robotiq_gripper_ros2_control
|
||||||
|
name="${name}" prefix="${prefix}"
|
||||||
|
use_fake_hardware="${use_fake_hardware}"
|
||||||
|
fake_sensor_commands="${fake_sensor_commands}"
|
||||||
|
robot_ip="${robot_ip}"
|
||||||
|
robot_port="${robot_port}"
|
||||||
|
min_position="${min_position}"
|
||||||
|
max_position="${max_position}"
|
||||||
|
min_speed="${min_speed}"
|
||||||
|
max_speed="${max_speed}"
|
||||||
|
min_force="${min_force}"
|
||||||
|
max_force="${max_force}"/>
|
||||||
|
</xacro:if>
|
||||||
|
|
||||||
|
<link name="${prefix}robotiq_85_base_link">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_85/robotiq_base.dae" />
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_85/robotiq_base.stl" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin xyz="0.0 2.274e-05 0.03232288" rpy="0 0 0" />
|
||||||
|
<mass value="6.6320197e-01" />
|
||||||
|
<inertia ixx="5.1617816e-04" iyy="5.8802208e-04" izz="3.9462776e-04" ixy="2.936e-8" ixz="0.0" iyz="-3.2296e-7" />
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<link name="${prefix}robotiq_85_left_knuckle_link">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_85/left_knuckle.dae" />
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_85/left_knuckle.stl" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin xyz="0.01213197 0.0002 -0.00058647" rpy="0 0 0" />
|
||||||
|
<mass value="1.384773208e-02" />
|
||||||
|
<inertia ixx="3.5232e-7" iyy="2.31944e-6" izz="2.23136e-6" ixy="0.0" ixz="1.1744e-7" iyz="0" />
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<link name="${prefix}robotiq_85_right_knuckle_link">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_85/right_knuckle.dae" />
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_85/right_knuckle.stl" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin xyz="-0.01213197 0.0002 -0.00058647" rpy="0 0 0" />
|
||||||
|
<mass value="1.384773208e-02" />
|
||||||
|
<inertia ixx="3.5232e-7" iyy="2.31944e-6" izz="2.23136e-6" ixy="0.0" ixz="-1.1744e-7" iyz="0.0" />
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<link name="${prefix}robotiq_85_left_finger_link">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_85/left_finger.dae" />
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_85/left_finger.stl" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin xyz="0.00346899 -0.00079447 0.01867121" rpy="0 0 0" />
|
||||||
|
<mass value="4.260376752e-02" />
|
||||||
|
<inertia ixx="1.385792000000000e-05" iyy="1.183208e-05" izz="5.19672e-06" ixy="0.0" ixz="-2.17264e-06" iyz="0.0" />
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<link name="${prefix}robotiq_85_right_finger_link">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_85/right_finger.dae" />
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_85/right_finger.stl" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin xyz="-0.00346899 -5.53e-06 0.01867121" rpy="0 0 0" />
|
||||||
|
<mass value="4.260376752000000e-02" />
|
||||||
|
<inertia ixx="1.385792e-05" iyy="1.183208e-05" izz="5.19672e-06" ixy="0.0" ixz="2.17264e-06" iyz="0.0" />
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<link name="${prefix}robotiq_85_left_inner_knuckle_link">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_85/left_inner_knuckle.dae" />
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_85/left_inner_knuckle.stl" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin xyz="0.01897699 0.00015001 0.02247101" rpy="0 0 0" />
|
||||||
|
<mass value="2.969376448e-02" />
|
||||||
|
<inertia ixx="9.57136e-06" iyy="8.69056e-06" izz="8.19144e-06" ixy="0.0" ixz="-3.93424e-06" iyz="0.0" />
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<link name="${prefix}robotiq_85_right_inner_knuckle_link">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_85/right_inner_knuckle.dae" />
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_85/right_inner_knuckle.stl" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin xyz="-0.01926824 5.001e-05 0.02222178" rpy="0 0 0" />
|
||||||
|
<mass value="2.969376448e-02" />
|
||||||
|
<inertia ixx="9.42456e-06" iyy="8.69056e-06" izz="8.33824e-06" ixy="0.0" ixz="3.9636e-06" iyz="0.0" />
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<link name="${prefix}robotiq_85_left_finger_tip_link">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_85/left_finger_tip.dae" />
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_85/left_finger_tip.stl" />
|
||||||
|
</geometry>
|
||||||
|
<surface>
|
||||||
|
<friction>
|
||||||
|
<ode>
|
||||||
|
<mu1>100000.0</mu1>
|
||||||
|
<mu2>100000.0</mu2>
|
||||||
|
</ode>
|
||||||
|
</friction>
|
||||||
|
<contact>
|
||||||
|
<ode>
|
||||||
|
<kp>1e+5</kp>
|
||||||
|
<kd>1</kd>
|
||||||
|
<soft_cfm>0</soft_cfm>
|
||||||
|
<soft_erp>0.2</soft_erp>
|
||||||
|
<minDepth>0.002</minDepth>
|
||||||
|
<maxVel>0</maxVel>
|
||||||
|
</ode>
|
||||||
|
</contact>
|
||||||
|
</surface>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin xyz="-0.01456706 -0.0008 0.01649701" rpy="0 0 0" />
|
||||||
|
<mass value="4.268588744e-02" />
|
||||||
|
<inertia ixx="1.048152e-05" iyy="1.197888e-05" izz="4.22784e-06" ixy="0.0" ixz="3.5232e-6" iyz="0.0" />
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<link name="${prefix}robotiq_85_right_finger_tip_link">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_85/right_finger_tip.dae" />
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_85/right_finger_tip.stl" />
|
||||||
|
</geometry>
|
||||||
|
<surface>
|
||||||
|
<friction>
|
||||||
|
<ode>
|
||||||
|
<mu1>100000.0</mu1>
|
||||||
|
<mu2>100000.0</mu2>
|
||||||
|
</ode>
|
||||||
|
</friction>
|
||||||
|
<contact>
|
||||||
|
<ode>
|
||||||
|
<kp>1e+5</kp>
|
||||||
|
<kd>1</kd>
|
||||||
|
<soft_cfm>0</soft_cfm>
|
||||||
|
<soft_erp>0.2</soft_erp>
|
||||||
|
<minDepth>0.002</minDepth>
|
||||||
|
<maxVel>0</maxVel>
|
||||||
|
</ode>
|
||||||
|
</contact>
|
||||||
|
</surface>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin xyz="0.01456706 5e-05 0.01649701" rpy="0 0 0" />
|
||||||
|
<mass value="4.268588744e-02" />
|
||||||
|
<inertia ixx="1.048152e-05" iyy="1.197888e-05" izz="4.22784e-06" ixy="0.0" ixz="-3.5232e-06" iyz="0.0" />
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<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">
|
||||||
|
<parent link="${prefix}robotiq_85_base_link" />
|
||||||
|
<child link="${prefix}robotiq_85_left_knuckle_link" />
|
||||||
|
<axis xyz="0 -1 0" />
|
||||||
|
<origin xyz="0.03060114 0.0 0.05490452" rpy="0 0 0" />
|
||||||
|
<limit lower="0.0" upper="0.8" velocity="0.5" effort="50" />
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<joint name="${prefix}robotiq_85_right_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" />
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<joint name="${prefix}robotiq_85_left_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">
|
||||||
|
<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">
|
||||||
|
<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" />
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<joint name="${prefix}robotiq_85_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" />
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<joint name="${prefix}robotiq_85_left_finger_tip_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" />
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<joint name="${prefix}robotiq_85_right_finger_tip_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" />
|
||||||
|
</joint>
|
||||||
|
</xacro:macro>
|
||||||
|
</robot>
|
||||||
48
src/robotiq_2f/robotiq_2f_interface/CMakeLists.txt
Normal file
48
src/robotiq_2f/robotiq_2f_interface/CMakeLists.txt
Normal file
@ -0,0 +1,48 @@
|
|||||||
|
cmake_minimum_required(VERSION 3.8)
|
||||||
|
project(robotiq_2f_interface)
|
||||||
|
|
||||||
|
# Default to C++17
|
||||||
|
if(NOT CMAKE_CXX_STANDARD)
|
||||||
|
set(CMAKE_CXX_STANDARD 17)
|
||||||
|
endif()
|
||||||
|
|
||||||
|
find_package(ament_cmake REQUIRED)
|
||||||
|
find_package(rclcpp REQUIRED)
|
||||||
|
find_package(hardware_interface REQUIRED)
|
||||||
|
find_package(pluginlib REQUIRED)
|
||||||
|
|
||||||
|
include_directories(include)
|
||||||
|
|
||||||
|
# Declare a C++ library
|
||||||
|
add_library(${PROJECT_NAME} SHARED
|
||||||
|
src/hardware_interface.cpp
|
||||||
|
src/Robotiq2fSocketAdapter.cpp
|
||||||
|
)
|
||||||
|
|
||||||
|
ament_target_dependencies(${PROJECT_NAME}
|
||||||
|
rclcpp
|
||||||
|
hardware_interface
|
||||||
|
pluginlib
|
||||||
|
)
|
||||||
|
|
||||||
|
pluginlib_export_plugin_description_file(hardware_interface hardware_interface_plugin.xml)
|
||||||
|
|
||||||
|
# Install targets
|
||||||
|
install(TARGETS ${PROJECT_NAME}
|
||||||
|
ARCHIVE DESTINATION lib
|
||||||
|
LIBRARY DESTINATION lib
|
||||||
|
RUNTIME DESTINATION bin
|
||||||
|
)
|
||||||
|
|
||||||
|
# Install header files
|
||||||
|
install(DIRECTORY include/
|
||||||
|
DESTINATION include
|
||||||
|
)
|
||||||
|
|
||||||
|
# Install plugin description file
|
||||||
|
install(FILES
|
||||||
|
hardware_interface_plugin.xml
|
||||||
|
DESTINATION share/${PROJECT_NAME}
|
||||||
|
)
|
||||||
|
|
||||||
|
ament_package()
|
||||||
@ -0,0 +1,9 @@
|
|||||||
|
<library path="robotiq_2f_interface">
|
||||||
|
<class name="robotiq_2f_interface/RobotiqGripperHardwareInterface"
|
||||||
|
type="robotiq_interface::RobotiqGripperHardwareInterface"
|
||||||
|
base_class_type="hardware_interface::SystemInterface">
|
||||||
|
<description>
|
||||||
|
ROS2 controller for the Robotiq gripper.
|
||||||
|
</description>
|
||||||
|
</class>
|
||||||
|
</library>
|
||||||
@ -0,0 +1,95 @@
|
|||||||
|
// MockRobotiq2fSocketAdapter.hpp
|
||||||
|
|
||||||
|
#ifndef MOCK_ROBOTIQ2F_SOCKET_ADAPTER_HPP
|
||||||
|
#define MOCK_ROBOTIQ2F_SOCKET_ADAPTER_HPP
|
||||||
|
|
||||||
|
#include <string>
|
||||||
|
#include <map>
|
||||||
|
#include <atomic>
|
||||||
|
#include <tuple>
|
||||||
|
|
||||||
|
#include "robotiq_2f_interface/SocketAdapterBase.hpp"
|
||||||
|
|
||||||
|
namespace robotiq_interface
|
||||||
|
{
|
||||||
|
class MockRobotiq2fSocketAdapter : public SocketAdapterBase {
|
||||||
|
public:
|
||||||
|
MockRobotiq2fSocketAdapter() : isConnected(false), gripperPosition(0), gripperForce(0), gripperSpeed(0), isActive(false) {}
|
||||||
|
|
||||||
|
~MockRobotiq2fSocketAdapter() override {
|
||||||
|
if (movementThread && movementThread->joinable()) {
|
||||||
|
movementThread->join();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
bool connect(const std::string& hostname, int port) override {
|
||||||
|
isConnected.store(true);
|
||||||
|
return true; // Simulate successful connection
|
||||||
|
}
|
||||||
|
|
||||||
|
void disconnect() override {
|
||||||
|
isConnected.store(false);
|
||||||
|
}
|
||||||
|
|
||||||
|
int force() override {
|
||||||
|
return gripperForce.load();
|
||||||
|
}
|
||||||
|
|
||||||
|
int speed() override {
|
||||||
|
return gripperSpeed.load();
|
||||||
|
}
|
||||||
|
|
||||||
|
int position() override {
|
||||||
|
return gripperPosition.load();
|
||||||
|
}
|
||||||
|
|
||||||
|
bool is_active() override {
|
||||||
|
return isActive.load();
|
||||||
|
}
|
||||||
|
|
||||||
|
void activate(bool autoCalibrate = true) override {
|
||||||
|
isActive.store(true);
|
||||||
|
}
|
||||||
|
|
||||||
|
void deactivate() override {
|
||||||
|
isActive.store(false);
|
||||||
|
}
|
||||||
|
|
||||||
|
std::tuple<bool, int> move(int targetPosition, int speed, int force) override{
|
||||||
|
// Ensure only one movement operation is active at a time
|
||||||
|
if (movementThread && movementThread->joinable()) {
|
||||||
|
movementThread->join(); // Wait for any ongoing movement to complete
|
||||||
|
}
|
||||||
|
|
||||||
|
// Start a new movement operation in a background thread
|
||||||
|
movementThread = std::make_unique<std::thread>(&MockRobotiq2fSocketAdapter::simulateMovement, this, targetPosition, speed, force);
|
||||||
|
|
||||||
|
return std::make_tuple(true, targetPosition); // Immediately return success and the target position
|
||||||
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
std::atomic<bool> isConnected;
|
||||||
|
std::atomic<int> gripperPosition;
|
||||||
|
std::atomic<int> gripperForce;
|
||||||
|
std::atomic<int> gripperSpeed;
|
||||||
|
std::atomic<bool> isActive;
|
||||||
|
|
||||||
|
std::unique_ptr<std::thread> movementThread;
|
||||||
|
|
||||||
|
void simulateMovement(int targetPosition, int speed, int force) {
|
||||||
|
int step = (targetPosition > gripperPosition) ? 1 : -1;
|
||||||
|
|
||||||
|
// Simulate moving towards the target position
|
||||||
|
while (gripperPosition != targetPosition) {
|
||||||
|
gripperPosition += step;
|
||||||
|
std::this_thread::sleep_for(std::chrono::milliseconds(100)); // Simulate time to move a step
|
||||||
|
}
|
||||||
|
|
||||||
|
// Here, you simulate setting the speed and force, though for simplicity, we're just logging it
|
||||||
|
// In a real mock, you might update some internal state to reflect these changes
|
||||||
|
std::cout << "Simulated movement to position " << targetPosition << " with speed " << speed << " and force " << force << std::endl;
|
||||||
|
}
|
||||||
|
};
|
||||||
|
} // end robotiq_interface
|
||||||
|
|
||||||
|
#endif // MOCK_ROBOTIQ2F_SOCKET_ADAPTER_HPP
|
||||||
@ -0,0 +1,360 @@
|
|||||||
|
// Robotiq2fSocketAdapter.hpp
|
||||||
|
|
||||||
|
#ifndef ROBOTIQ2F_SOCKET_ADAPTER_HPP
|
||||||
|
#define ROBOTIQ2F_SOCKET_ADAPTER_HPP
|
||||||
|
|
||||||
|
#include <mutex>
|
||||||
|
#include <thread>
|
||||||
|
|
||||||
|
#include <string>
|
||||||
|
#include <map>
|
||||||
|
#include <vector>
|
||||||
|
#include <sys/time.h>
|
||||||
|
#include <sstream>
|
||||||
|
#include <tuple>
|
||||||
|
|
||||||
|
#include <sys/socket.h>
|
||||||
|
#include <netinet/in.h>
|
||||||
|
#include <arpa/inet.h>
|
||||||
|
|
||||||
|
#include <unistd.h> // For close()
|
||||||
|
#include <cstring> // For memset()
|
||||||
|
#include <sys/select.h> // For select() system call
|
||||||
|
|
||||||
|
#include <stdexcept> // For std::runtime_error
|
||||||
|
#include <cerrno> // For errno
|
||||||
|
#include <iostream>
|
||||||
|
|
||||||
|
#include "robotiq_2f_interface/SocketAdapterBase.hpp"
|
||||||
|
|
||||||
|
namespace robotiq_interface
|
||||||
|
{
|
||||||
|
// Enum for Gripper Status
|
||||||
|
enum class GripperStatus {
|
||||||
|
RESET = 0,
|
||||||
|
ACTIVATING = 1,
|
||||||
|
// UNUSED = 2, // This value is currently not used by the gripper firmware
|
||||||
|
ACTIVE = 3
|
||||||
|
};
|
||||||
|
|
||||||
|
// Enum for Object Status
|
||||||
|
enum class ObjectStatus {
|
||||||
|
MOVING = 0,
|
||||||
|
STOPPED_OUTER_OBJECT = 1,
|
||||||
|
STOPPED_INNER_OBJECT = 2,
|
||||||
|
AT_DEST = 3
|
||||||
|
};
|
||||||
|
|
||||||
|
class Robotiq2fSocketAdapter : public SocketAdapterBase {
|
||||||
|
public:
|
||||||
|
|
||||||
|
Robotiq2fSocketAdapter() ;
|
||||||
|
~Robotiq2fSocketAdapter() override;
|
||||||
|
|
||||||
|
// Connection management
|
||||||
|
/**
|
||||||
|
* Establishes a TCP connection to the Robotiq gripper.
|
||||||
|
*
|
||||||
|
* Attempts to open a socket and connect to the specified hostname and port. If the adapter
|
||||||
|
* is already connected, it will disconnect and attempt to reconnect.
|
||||||
|
*
|
||||||
|
* @param hostname The IP address or hostname of the Robotiq gripper.
|
||||||
|
* @param port The port number on which the gripper is listening for connections.
|
||||||
|
* @return True if the connection was successfully established, False otherwise.
|
||||||
|
*
|
||||||
|
* @throws std::runtime_error if socket creation fails.
|
||||||
|
*/
|
||||||
|
bool connect(const std::string& hostname, int port) override;
|
||||||
|
/**
|
||||||
|
* Closes the connection to the Robotiq gripper.
|
||||||
|
*
|
||||||
|
* Safely shuts down and closes the current socket connection to the gripper. If the socket
|
||||||
|
* is already closed or was never connected, this method has no effect. This method is
|
||||||
|
* thread-safe and can be called concurrently with other operations on the adapter.
|
||||||
|
*/
|
||||||
|
void disconnect() override;
|
||||||
|
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Sets multiple gripper variables using a map of variable names to integer/double values.
|
||||||
|
*
|
||||||
|
* This method allows setting multiple configuration parameters of the gripper simultaneously.
|
||||||
|
* The variable names must match the expected variable identifiers used by the gripper.
|
||||||
|
* Integer/double values are used directly in the command sent to the gripper.
|
||||||
|
*
|
||||||
|
* @param variableMap A map where each key is a string representing the variable name and
|
||||||
|
* each value is an integer representing the variable's desired value.
|
||||||
|
* @return True if the command was successfully sent and acknowledged by the gripper, False otherwise.
|
||||||
|
*
|
||||||
|
* @throws std::runtime_error if the adapter is not connected to the gripper or if sending the command fails.
|
||||||
|
*/
|
||||||
|
bool setGripperVariables(const std::map<std::string, int>& variableMap);
|
||||||
|
bool setGripperVariables(const std::map<std::string, double>& variableMap);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Sets a single gripper variable.
|
||||||
|
*
|
||||||
|
* Allows setting a single configuration parameter of the gripper. The method overloads allow
|
||||||
|
* specifying the variable value as either an integer or a double, depending on the expected
|
||||||
|
* variable type.
|
||||||
|
*
|
||||||
|
* @param variable The variable name, matching the gripper's expected variable identifiers.
|
||||||
|
* @param value The desired value for the variable, either as an int or a double.
|
||||||
|
* @return True if the command was successfully sent and acknowledged by the gripper, False otherwise.
|
||||||
|
*
|
||||||
|
* @throws std::runtime_error if the adapter is not connected to the gripper or if sending the command fails.
|
||||||
|
*/
|
||||||
|
bool setGripperVariable(const std::string& variable, int value);
|
||||||
|
bool setGripperVariable(const std::string& variable, double value);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Retrieves the current value of a specified gripper variable.
|
||||||
|
*
|
||||||
|
* Sends a command to the gripper to report the current value of a specific configuration parameter.
|
||||||
|
* The method parses the gripper's response to extract the variable value.
|
||||||
|
*
|
||||||
|
* @param variable The variable name, matching the gripper's expected variable identifiers.
|
||||||
|
* @return The current value of the variable as an integer. Returns -1 if the command fails or
|
||||||
|
* if the variable is not recognized by the gripper.
|
||||||
|
*
|
||||||
|
* @throws std::runtime_error if the adapter is not connected to the gripper, if sending the command fails,
|
||||||
|
* or if the response from the gripper is malformed.
|
||||||
|
*/
|
||||||
|
int getGripperVariable(const std::string& variable);
|
||||||
|
|
||||||
|
// Gripper status
|
||||||
|
int force() override;
|
||||||
|
int speed() override;
|
||||||
|
int position() override;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Checks if the gripper is currently active.
|
||||||
|
*
|
||||||
|
* Queries the current state of the gripper to determine if it is active. This method interprets
|
||||||
|
* the gripper's status based on the last known communication and does not initiate a new request
|
||||||
|
* to the gripper.
|
||||||
|
*
|
||||||
|
* @return True if the gripper is in the ACTIVE state, False otherwise.
|
||||||
|
*
|
||||||
|
* @throws std::runtime_error if unable to retrieve the gripper's status, typically due to communication issues.
|
||||||
|
*/
|
||||||
|
bool is_active() override;
|
||||||
|
|
||||||
|
|
||||||
|
// Activates the gripper and optionally performs auto-calibration
|
||||||
|
/**
|
||||||
|
* Activates the Robotiq gripper and optionally performs auto-calibration.
|
||||||
|
*
|
||||||
|
* This method sends an activation command to the gripper, bringing it from a RESET or INACTIVE
|
||||||
|
* state to an ACTIVE state. If autoCalibrate is set to true, it also initiates an auto-calibration
|
||||||
|
* procedure to determine the maximum and minimum positions of the gripper's motion range.
|
||||||
|
*
|
||||||
|
* @param autoCalibrate A boolean flag indicating whether to perform auto-calibration after
|
||||||
|
* activation. Default is true.
|
||||||
|
*
|
||||||
|
* @note Activation and auto-calibration may take several seconds to complete. The method
|
||||||
|
* waits for the gripper to acknowledge activation before returning.
|
||||||
|
*
|
||||||
|
* @throws std::runtime_error if the adapter is not connected to the gripper, if sending the
|
||||||
|
* activation command fails, or if the gripper does not reach the expected state within
|
||||||
|
* a reasonable timeframe.
|
||||||
|
*/
|
||||||
|
void activate(bool autoCalibrate = true) override;
|
||||||
|
/**
|
||||||
|
* Performs auto-calibration of the gripper.
|
||||||
|
*
|
||||||
|
* This method initiates an auto-calibration sequence where the gripper fully opens and closes to determine
|
||||||
|
* its operational range. This is useful for ensuring accurate position control, especially if the mechanical
|
||||||
|
* setup of the gripper has been altered or if it is being used for the first time.
|
||||||
|
*
|
||||||
|
* @param log A boolean flag indicating whether to log the calibration results. Defaults to true. When set
|
||||||
|
* to true, the final calibration values will be logged to the standard output.
|
||||||
|
*
|
||||||
|
* @note This method assumes the gripper is already activated. If the gripper is not in an active state,
|
||||||
|
* the calibration sequence may fail. Ensure the gripper is activated before calling this method.
|
||||||
|
*
|
||||||
|
* @throws std::runtime_error if the adapter is not connected to the gripper, if the gripper is not in an
|
||||||
|
* active state, or if the calibration sequence fails.
|
||||||
|
*/
|
||||||
|
void auto_calibrate(bool log=true);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Deactivates the gripper.
|
||||||
|
*
|
||||||
|
* Sends a command to transition the gripper from an active state back to a reset state. This method
|
||||||
|
* is useful for safely shutting down the gripper or preparing it for a state change.
|
||||||
|
*
|
||||||
|
* @note This method should be called before disconnecting from the gripper or when the gripper is not
|
||||||
|
* needed for an extended period to ensure it is left in a safe state.
|
||||||
|
*
|
||||||
|
* @throws std::runtime_error if the adapter is not connected to the gripper or if the deactivation command fails.
|
||||||
|
*/
|
||||||
|
void deactivate() override;
|
||||||
|
|
||||||
|
// Move Commands
|
||||||
|
/**
|
||||||
|
* Commands the gripper to move to a specified position.
|
||||||
|
*
|
||||||
|
* Instructs the gripper to move to a given position with specified speed and force. The position,
|
||||||
|
* speed, and force parameters are automatically clipped to the valid range supported by the gripper.
|
||||||
|
*
|
||||||
|
* @param position The target position for the gripper, typically in the range [0, 255].
|
||||||
|
* @param speed The speed at which the gripper should move, typically in the range [0, 255].
|
||||||
|
* @param force The force to be applied by the gripper, typically in the range [0, 255].
|
||||||
|
* @return A tuple containing a boolean indicating success of the command, and an integer representing the clipped target position.
|
||||||
|
*
|
||||||
|
* @throws std::runtime_error if there's an error in sending the move command or if the adapter is not connected to a gripper.
|
||||||
|
*/
|
||||||
|
std::tuple<bool, int> move(int position, int speed, int force) override;
|
||||||
|
/**
|
||||||
|
* Commands the gripper to move to a specified position and waits for the move to complete.
|
||||||
|
*
|
||||||
|
* This method instructs the gripper to move to the given position with specified speed and force,
|
||||||
|
* then blocks until the gripper acknowledges reaching the target position or stops moving due to
|
||||||
|
* detecting an object.
|
||||||
|
*
|
||||||
|
* @param position The target position for the gripper, typically in the range [0, 255].
|
||||||
|
* @param speed The speed at which the gripper should move, typically in the range [0, 255].
|
||||||
|
* @param force The force to be applied by the gripper, typically in the range [0, 255].
|
||||||
|
* @return A tuple containing the final position of the gripper as an integer and the object status
|
||||||
|
* as an `ObjectStatus` enumeration value.
|
||||||
|
*
|
||||||
|
* @throws std::runtime_error if there's an error in sending the move command, if the adapter is not
|
||||||
|
* connected to a gripper, or if the gripper does not reach the target position within a
|
||||||
|
* reasonable time frame.
|
||||||
|
*/
|
||||||
|
std::tuple<int, ObjectStatus> move_and_wait_for_pos(int position, int speed, int force);
|
||||||
|
|
||||||
|
private:
|
||||||
|
// Custom deleter for the smart pointer managing the socket descriptor
|
||||||
|
struct SocketDeleter {
|
||||||
|
void operator()(int* ptr) {
|
||||||
|
if (ptr && *ptr >= 0) {
|
||||||
|
close(*ptr);
|
||||||
|
}
|
||||||
|
delete ptr;
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
// Smart pointer to manage the socket descriptor with the custom deleter
|
||||||
|
std::unique_ptr<int, SocketDeleter> sockfd_;
|
||||||
|
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_speed_ = 0;
|
||||||
|
int max_speed_ = 255;
|
||||||
|
int min_force_ = 0;
|
||||||
|
int max_force_ = 255;
|
||||||
|
|
||||||
|
int open_position_ = min_position_;
|
||||||
|
int closed_position_ = max_position_;
|
||||||
|
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Sends a command to the connected Robotiq gripper.
|
||||||
|
*
|
||||||
|
* Constructs and sends a command string to the gripper over the established socket connection.
|
||||||
|
* This method ensures thread-safe access to the socket and handles low-level socket write operations.
|
||||||
|
*
|
||||||
|
* @param cmd The command string to be sent to the gripper. Must conform to the gripper's protocol.
|
||||||
|
* @return True if the command was successfully sent, False if there was an error sending the command.
|
||||||
|
*
|
||||||
|
* @throws std::runtime_error if attempted on a disconnected socket.
|
||||||
|
*/
|
||||||
|
bool sendCommand(const std::string& cmd);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Receives a response from the Robotiq gripper.
|
||||||
|
*
|
||||||
|
* Waits for and reads a response from the gripper within a specified timeout period. This method
|
||||||
|
* is blocking but allows specifying a timeout to prevent indefinite waiting. It ensures thread-safe
|
||||||
|
* access to the socket and handles low-level socket read operations.
|
||||||
|
*
|
||||||
|
* @param timeout_ms The timeout in milliseconds to wait for a response. Defaults to 2000 milliseconds.
|
||||||
|
* @return A string containing the response from the gripper. If the operation times out or fails, an empty string is returned.
|
||||||
|
*
|
||||||
|
* @throws std::runtime_error if attempted on a disconnected socket or if a select() call fails.
|
||||||
|
*/
|
||||||
|
std::string receiveResponse(int timeout_ms=2000);
|
||||||
|
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Checks if the received data is an acknowledgement message from the gripper.
|
||||||
|
*
|
||||||
|
* This method parses the received data string to determine if it matches the expected acknowledgement
|
||||||
|
* pattern or message, indicating that a previously sent command was received and executed by the gripper.
|
||||||
|
*
|
||||||
|
* @param data The raw string data received from the gripper.
|
||||||
|
* @return True if the data represents an acknowledgement message, False otherwise.
|
||||||
|
*/
|
||||||
|
bool isAck(const std::string& data);
|
||||||
|
|
||||||
|
// Helper Functions
|
||||||
|
/**
|
||||||
|
* Clips a variable to ensure it falls within a specified range.
|
||||||
|
*
|
||||||
|
* This helper function ensures that a given value does not exceed the minimum and maximum bounds
|
||||||
|
* defined for it. It is particularly useful for sanitizing command parameters like position, speed,
|
||||||
|
* and force values before sending them to the gripper.
|
||||||
|
*
|
||||||
|
* @param min_value The minimum allowable value for the variable.
|
||||||
|
* @param variable The current value of the variable to be clipped.
|
||||||
|
* @param max_value The maximum allowable value for the variable.
|
||||||
|
* @return The clipped value, constrained within the specified min and max bounds.
|
||||||
|
*/
|
||||||
|
int clip_val(int min_value, int variable, int max_value);
|
||||||
|
/**
|
||||||
|
* Blocks until the gripper reaches a specified status.
|
||||||
|
*
|
||||||
|
* This method polls the gripper's status at regular intervals until it matches the expected status
|
||||||
|
* or until a timeout occurs. It is used internally to synchronize actions like activation, deactivation,
|
||||||
|
* and calibration with the gripper's state transitions.
|
||||||
|
*
|
||||||
|
* @param expectedStatus The GripperStatus enumeration value the method waits for the gripper to achieve.
|
||||||
|
*
|
||||||
|
* @throws std::runtime_error if the gripper does not reach the expected status within a reasonable time frame,
|
||||||
|
* indicating a possible communication issue or a failure in executing the command.
|
||||||
|
*/
|
||||||
|
void waitForGripperStatus(GripperStatus expectedStatus);
|
||||||
|
|
||||||
|
// Resets the gripper
|
||||||
|
/**
|
||||||
|
* Resets the gripper to its initial state.
|
||||||
|
*
|
||||||
|
* Sends a command to the gripper to transition it to the RESET state. This is often used as a first step
|
||||||
|
* in a sequence of commands that prepare the gripper for operation, ensuring it starts from a known state.
|
||||||
|
*
|
||||||
|
* @note This method can be used to recover the gripper from error states or to prepare it for reactivation.
|
||||||
|
*
|
||||||
|
* @throws std::runtime_error if the adapter is not connected to the gripper or if the reset command fails.
|
||||||
|
*/
|
||||||
|
void reset();
|
||||||
|
|
||||||
|
// Define the command strings and encoding used for communication
|
||||||
|
const std::string SET_COMMAND = "SET";
|
||||||
|
const std::string GET_COMMAND = "GET";
|
||||||
|
const std::string ACK_MESSAGE = "ack";
|
||||||
|
|
||||||
|
// Commands
|
||||||
|
const std::string CMD_ACT = "ACT"; // Activation
|
||||||
|
const std::string CMD_GTO = "GTO"; // GoTo
|
||||||
|
const std::string CMD_ATR = "ATR"; // Automatic Release
|
||||||
|
const std::string CMD_ADR = "ADR"; // Advanced Control
|
||||||
|
const std::string CMD_FOR = "FOR"; // Force
|
||||||
|
const std::string CMD_SPE = "SPE"; // Speed
|
||||||
|
const std::string CMD_POS = "POS"; // Position
|
||||||
|
const std::string CMD_STA = "STA"; // Status
|
||||||
|
const std::string CMD_PRE = "PRE"; // Position Request
|
||||||
|
const std::string CMD_OBJ = "OBJ"; // Object Detection
|
||||||
|
const std::string CMD_FLT = "FLT"; // Fault
|
||||||
|
|
||||||
|
// Constants buffer sizes
|
||||||
|
const size_t BUFFER_SIZE = 1024; // Adjust as necessary
|
||||||
|
const int MAX_RETRIES = 5;
|
||||||
|
const int RETRY_DELAY_MS = 20;
|
||||||
|
};
|
||||||
|
} // end robotiq_interface
|
||||||
|
|
||||||
|
#endif // ROBOTIQ2F_SOCKET_ADAPTER_HPP
|
||||||
@ -0,0 +1,26 @@
|
|||||||
|
// SocketAdapterBase.hpp
|
||||||
|
|
||||||
|
#ifndef SOCKET_ADAPTER_BASE_HPP
|
||||||
|
#define SOCKET_ADAPTER_BASE_HPP
|
||||||
|
|
||||||
|
#include <string>
|
||||||
|
#include <tuple>
|
||||||
|
|
||||||
|
namespace robotiq_interface
|
||||||
|
{
|
||||||
|
class SocketAdapterBase {
|
||||||
|
public:
|
||||||
|
virtual ~SocketAdapterBase() = default;
|
||||||
|
|
||||||
|
virtual bool connect(const std::string& hostname, int port) = 0;
|
||||||
|
virtual void disconnect() = 0;
|
||||||
|
virtual int force() = 0;
|
||||||
|
virtual int speed() = 0;
|
||||||
|
virtual int position() = 0;
|
||||||
|
virtual bool is_active() = 0;
|
||||||
|
virtual void activate(bool autoCalibrate = true) = 0;
|
||||||
|
virtual void deactivate() = 0;
|
||||||
|
virtual std::tuple<bool, int> move(int position, int speed, int force) = 0;
|
||||||
|
};
|
||||||
|
} // end robotiq_interface
|
||||||
|
#endif // SOCKET_ADAPTER_BASE_HPP
|
||||||
@ -0,0 +1,117 @@
|
|||||||
|
// hardware_interface.hpp
|
||||||
|
|
||||||
|
#ifndef ROBOTIQ2F_HARDWARE_INTERFACE_HPP
|
||||||
|
#define ROBOTIQ2F_HARDWARE_INTERFACE_HPP
|
||||||
|
|
||||||
|
#include <chrono>
|
||||||
|
#include <cmath>
|
||||||
|
#include <limits>
|
||||||
|
#include <memory>
|
||||||
|
#include <vector>
|
||||||
|
#include <atomic>
|
||||||
|
|
||||||
|
#include "robotiq_2f_interface/visibility_control.hpp"
|
||||||
|
|
||||||
|
#include "robotiq_2f_interface/SocketAdapterBase.hpp"
|
||||||
|
#include "robotiq_2f_interface/Robotiq2fSocketAdapter.hpp"
|
||||||
|
#include "robotiq_2f_interface/MockRobotiq2fSocketAdapter.hpp"
|
||||||
|
|
||||||
|
#include <hardware_interface/handle.hpp>
|
||||||
|
#include <hardware_interface/hardware_info.hpp>
|
||||||
|
#include <hardware_interface/system_interface.hpp>
|
||||||
|
#include <hardware_interface/types/hardware_interface_type_values.hpp>
|
||||||
|
#include <hardware_interface/types/hardware_interface_return_values.hpp>
|
||||||
|
|
||||||
|
#include <rclcpp/rclcpp.hpp>
|
||||||
|
#include <rclcpp/clock.hpp>
|
||||||
|
|
||||||
|
#include <stdexcept>
|
||||||
|
|
||||||
|
namespace robotiq_interface
|
||||||
|
{
|
||||||
|
class RobotiqGripperHardwareInterface : public hardware_interface::SystemInterface
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
// Constructors
|
||||||
|
ROBOTIQ_INTERFACE_PUBLIC RobotiqGripperHardwareInterface();
|
||||||
|
ROBOTIQ_INTERFACE_PUBLIC ~RobotiqGripperHardwareInterface();
|
||||||
|
// ROBOTIQ_DRIVER_PUBLIC explicit RobotiqGripperHardwareInterface(std::unique_ptr<SocketFactory> socket_factory);
|
||||||
|
|
||||||
|
// Lifecycle Management
|
||||||
|
ROBOTIQ_INTERFACE_PUBLIC CallbackReturn on_init(const hardware_interface::HardwareInfo& info) override;
|
||||||
|
ROBOTIQ_INTERFACE_PUBLIC CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override;
|
||||||
|
ROBOTIQ_INTERFACE_PUBLIC CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override;
|
||||||
|
ROBOTIQ_INTERFACE_PUBLIC CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override;
|
||||||
|
|
||||||
|
// State and Command Interfaces
|
||||||
|
ROBOTIQ_INTERFACE_PUBLIC std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
|
||||||
|
ROBOTIQ_INTERFACE_PUBLIC std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
|
||||||
|
|
||||||
|
// Data Access (Read/Write)
|
||||||
|
ROBOTIQ_INTERFACE_PUBLIC hardware_interface::return_type read(const rclcpp::Time& time, const rclcpp::Duration& period) override;
|
||||||
|
ROBOTIQ_INTERFACE_PUBLIC hardware_interface::return_type write(const rclcpp::Time& time, const rclcpp::Duration& period) override;
|
||||||
|
|
||||||
|
protected: // Likely changes the access to protected from private
|
||||||
|
// Members for driver interaction
|
||||||
|
bool use_mock_hardware_;
|
||||||
|
std::unique_ptr<SocketAdapterBase> socket_adapter_;
|
||||||
|
void configureAdapter(bool useMock);
|
||||||
|
|
||||||
|
// Background communication thread
|
||||||
|
std::thread communication_thread_;
|
||||||
|
std::atomic<bool> communication_thread_is_running_;
|
||||||
|
std::mutex resource_mutex_;
|
||||||
|
void background_task();
|
||||||
|
|
||||||
|
static constexpr double NO_NEW_CMD_ = std::numeric_limits<double>::quiet_NaN();
|
||||||
|
|
||||||
|
// Conversion Methods
|
||||||
|
double convertRawToPosition(int raw_position);
|
||||||
|
int convertPositionToRaw(double position);
|
||||||
|
int convertSpeedToRaw(double speed);
|
||||||
|
int convertForceToRaw(double force);
|
||||||
|
|
||||||
|
// Gripper Commands
|
||||||
|
std::atomic<uint8_t> write_command_;
|
||||||
|
std::atomic<uint8_t> write_force_;
|
||||||
|
std::atomic<uint8_t> write_speed_;
|
||||||
|
std::atomic<uint8_t> gripper_current_state_;
|
||||||
|
|
||||||
|
|
||||||
|
// Gripper State Tracking
|
||||||
|
double gripper_position_ = 0.0;
|
||||||
|
double gripper_velocity_ = 0.0;
|
||||||
|
double gripper_position_command_ = 0.0;
|
||||||
|
|
||||||
|
rclcpp::Clock::SharedPtr clock_ = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
|
||||||
|
rclcpp::Time last_update_time_ = clock_ -> now();
|
||||||
|
rclcpp::Time current_time_ = clock_ -> now();
|
||||||
|
|
||||||
|
double reactivate_gripper_cmd_ = 0.0;
|
||||||
|
std::atomic<bool> reactivate_gripper_async_cmd_;
|
||||||
|
double reactivate_gripper_response_ = 0.0;
|
||||||
|
double gripper_force_ = 0.0;
|
||||||
|
double gripper_speed_ = 0.0;
|
||||||
|
std::atomic<std::optional<bool>> reactivate_gripper_async_response_;
|
||||||
|
|
||||||
|
// Connection Variables
|
||||||
|
std::string robot_ip_;
|
||||||
|
int robot_port_;
|
||||||
|
|
||||||
|
// Parameters for physical limits and configuration
|
||||||
|
double min_position_;
|
||||||
|
double max_position_;
|
||||||
|
double min_speed_;
|
||||||
|
double max_speed_;
|
||||||
|
double min_force_;
|
||||||
|
double max_force_;
|
||||||
|
|
||||||
|
// loop time
|
||||||
|
const std::chrono::milliseconds gripperCommsLoopPeriod = std::chrono::milliseconds{ 10 };
|
||||||
|
|
||||||
|
// Logger
|
||||||
|
const rclcpp::Logger logger_ = rclcpp::get_logger("RobotiqGripperHardwareInterface");
|
||||||
|
};
|
||||||
|
} // end robotiq_interface
|
||||||
|
|
||||||
|
#endif //ROBOTIQ2F_HARDWARE_INTERFACE_HPP
|
||||||
@ -0,0 +1,56 @@
|
|||||||
|
// Copyright 2017 Open Source Robotics Foundation, Inc.
|
||||||
|
//
|
||||||
|
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||||
|
// you may not use this file except in compliance with the License.
|
||||||
|
// You may obtain a copy of the License at
|
||||||
|
//
|
||||||
|
// http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
//
|
||||||
|
// Unless required by applicable law or agreed to in writing, software
|
||||||
|
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||||
|
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||||
|
// See the License for the specific language governing permissions and
|
||||||
|
// limitations under the License.
|
||||||
|
|
||||||
|
/* This header must be included by all rclcpp headers which declare symbols
|
||||||
|
* which are defined in the rclcpp library. When not building the rclcpp
|
||||||
|
* library, i.e. when using the headers in other package's code, the contents
|
||||||
|
* of this header change the visibility of certain symbols which the rclcpp
|
||||||
|
* library cannot have, but the consuming code must have inorder to link.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef ROBOTIQ_INTERFACE__VISIBILITY_CONTROL_HPP_
|
||||||
|
#define ROBOTIQ_INTERFACE__VISIBILITY_CONTROL_HPP_
|
||||||
|
|
||||||
|
// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
|
||||||
|
// https://gcc.gnu.org/wiki/Visibility
|
||||||
|
|
||||||
|
#if defined _WIN32 || defined __CYGWIN__
|
||||||
|
#ifdef __GNUC__
|
||||||
|
#define ROBOTIQ_INTERFACE_EXPORT __attribute__((dllexport))
|
||||||
|
#define ROBOTIQ_INTERFACE_IMPORT __attribute__((dllimport))
|
||||||
|
#else
|
||||||
|
#define ROBOTIQ_INTERFACE_EXPORT __declspec(dllexport)
|
||||||
|
#define ROBOTIQ_INTERFACE_IMPORT __declspec(dllimport)
|
||||||
|
#endif
|
||||||
|
#ifdef ROBOTIQ_INTERFACE_BUILDING_DLL
|
||||||
|
#define ROBOTIQ_INTERFACE_PUBLIC ROBOTIQ_INTERFACE_EXPORT
|
||||||
|
#else
|
||||||
|
#define ROBOTIQ_INTERFACE_PUBLIC ROBOTIQ_INTERFACE_IMPORT
|
||||||
|
#endif
|
||||||
|
#define ROBOTIQ_INTERFACE_PUBLIC_TYPE ROBOTIQ_INTERFACE_PUBLIC
|
||||||
|
#define ROBOTIQ_INTERFACE_LOCAL
|
||||||
|
#else
|
||||||
|
#define ROBOTIQ_INTERFACE_EXPORT __attribute__((visibility("default")))
|
||||||
|
#define ROBOTIQ_INTERFACE_IMPORT
|
||||||
|
#if __GNUC__ >= 4
|
||||||
|
#define ROBOTIQ_INTERFACE_PUBLIC __attribute__((visibility("default")))
|
||||||
|
#define ROBOTIQ_INTERFACE_LOCAL __attribute__((visibility("hidden")))
|
||||||
|
#else
|
||||||
|
#define ROBOTIQ_INTERFACE_PUBLIC
|
||||||
|
#define ROBOTIQ_INTERFACE_LOCAL
|
||||||
|
#endif
|
||||||
|
#define ROBOTIQ_INTERFACE_PUBLIC_TYPE
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#endif // ROBOTIQ_DRIVER__VISIBILITY_CONTROL_HPP_
|
||||||
23
src/robotiq_2f/robotiq_2f_interface/package.xml
Normal file
23
src/robotiq_2f/robotiq_2f_interface/package.xml
Normal file
@ -0,0 +1,23 @@
|
|||||||
|
<?xml version="1.0"?>
|
||||||
|
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||||
|
<package format="3">
|
||||||
|
<name>robotiq_2f_interface</name>
|
||||||
|
<version>0.0.0</version>
|
||||||
|
<description>TODO: Package description</description>
|
||||||
|
<maintainer email="root@todo.todo">root</maintainer>
|
||||||
|
<license>TODO: License declaration</license>
|
||||||
|
|
||||||
|
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||||
|
|
||||||
|
<depend>rclcpp</depend>
|
||||||
|
<depend>rclcpp_lifecycle</depend>
|
||||||
|
<depend>hardware_interface</depend>
|
||||||
|
<depend>pluginlib</depend>
|
||||||
|
|
||||||
|
<test_depend>ament_lint_auto</test_depend>
|
||||||
|
<test_depend>ament_lint_common</test_depend>
|
||||||
|
|
||||||
|
<export>
|
||||||
|
<build_type>ament_cmake</build_type>
|
||||||
|
</export>
|
||||||
|
</package>
|
||||||
@ -0,0 +1,372 @@
|
|||||||
|
#include "robotiq_2f_interface/Robotiq2fSocketAdapter.hpp"
|
||||||
|
|
||||||
|
|
||||||
|
namespace robotiq_interface
|
||||||
|
{
|
||||||
|
Robotiq2fSocketAdapter::Robotiq2fSocketAdapter() : sockfd_(new int(-1)) {
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
Robotiq2fSocketAdapter::~Robotiq2fSocketAdapter() {
|
||||||
|
disconnect(); // Ensure the socket is closed properly.
|
||||||
|
}
|
||||||
|
|
||||||
|
// Connection and disconnection
|
||||||
|
bool Robotiq2fSocketAdapter::connect(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";
|
||||||
|
disconnect();
|
||||||
|
}
|
||||||
|
|
||||||
|
int sock = socket(AF_INET, SOCK_STREAM, 0);
|
||||||
|
if (sock < 0) {
|
||||||
|
std::cerr << "Socket creation failed: " << strerror(errno) << "\n";
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
*sockfd_ = sock;
|
||||||
|
|
||||||
|
sockaddr_in serv_addr{};
|
||||||
|
memset(&serv_addr, 0, sizeof(serv_addr));
|
||||||
|
serv_addr.sin_family = AF_INET;
|
||||||
|
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";
|
||||||
|
close(*sockfd_);
|
||||||
|
*sockfd_ = -1;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (::connect(*sockfd_, (struct sockaddr*)&serv_addr, sizeof(serv_addr)) < 0) {
|
||||||
|
std::cerr << "Connection to " << hostname << ":" << port << " failed: " << strerror(errno) << "\n";
|
||||||
|
close(*sockfd_);
|
||||||
|
*sockfd_ = -1;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
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";
|
||||||
|
} else {
|
||||||
|
std::cout << "Disconnected successfully.\n";
|
||||||
|
}
|
||||||
|
*sockfd_ = -1;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Utility methods
|
||||||
|
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";
|
||||||
|
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";
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
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";
|
||||||
|
return "";
|
||||||
|
}
|
||||||
|
|
||||||
|
std::string result;
|
||||||
|
char buffer[BUFFER_SIZE];
|
||||||
|
bool complete_response = false;
|
||||||
|
|
||||||
|
timeval tv;
|
||||||
|
tv.tv_sec = timeout_ms / 1000;
|
||||||
|
tv.tv_usec = (timeout_ms % 1000) * 1000;
|
||||||
|
|
||||||
|
// Loop to receive data until a complete response is obtained or a timeout occurs
|
||||||
|
while (!complete_response) {
|
||||||
|
fd_set readfds;
|
||||||
|
FD_ZERO(&readfds);
|
||||||
|
FD_SET(*sockfd_, &readfds);
|
||||||
|
|
||||||
|
int activity = select(*sockfd_ + 1, &readfds, nullptr, nullptr, &tv);
|
||||||
|
if (activity == -1) {
|
||||||
|
std::cerr << "[receiveResponse] Error in select(): " << strerror(errno) << "\n";
|
||||||
|
return "";
|
||||||
|
} else if (activity == 0) {
|
||||||
|
std::cerr << "Timeout occurred while waiting for response.\n";
|
||||||
|
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";
|
||||||
|
}
|
||||||
|
return "";
|
||||||
|
} else if (bytes_read == 0) {
|
||||||
|
std::cerr << "Connection closed by peer.\n";
|
||||||
|
return "";
|
||||||
|
} else {
|
||||||
|
buffer[bytes_read] = '\0';
|
||||||
|
result += buffer;
|
||||||
|
|
||||||
|
// Check if we have a complete response:
|
||||||
|
if (result.find("\n") != std::string::npos) {
|
||||||
|
complete_response = true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return result;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool Robotiq2fSocketAdapter::isAck(const std::string& data) {
|
||||||
|
return data == ACK_MESSAGE;
|
||||||
|
}
|
||||||
|
|
||||||
|
int Robotiq2fSocketAdapter::clip_val(int min_value, int value, int max_value) {
|
||||||
|
return std::max(min_value, std::min(value, max_value));
|
||||||
|
}
|
||||||
|
|
||||||
|
void Robotiq2fSocketAdapter::waitForGripperStatus(GripperStatus expectedStatus) {
|
||||||
|
int retries = 0;
|
||||||
|
int status = getGripperVariable(CMD_STA);
|
||||||
|
while (static_cast<GripperStatus>(status) != expectedStatus && retries < MAX_RETRIES) {
|
||||||
|
std::this_thread::sleep_for(std::chrono::milliseconds(RETRY_DELAY_MS));
|
||||||
|
status = getGripperVariable(CMD_STA);
|
||||||
|
retries++;
|
||||||
|
}
|
||||||
|
if (retries >= MAX_RETRIES) {
|
||||||
|
throw std::runtime_error("Gripper failed to reach expected status.");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// Gripper variable Getter and Setter
|
||||||
|
bool Robotiq2fSocketAdapter::setGripperVariables(const std::map<std::string, int>& variableMap) {
|
||||||
|
return setGripperVariables(reinterpret_cast<const std::map<std::string, double>&>(variableMap));
|
||||||
|
}
|
||||||
|
|
||||||
|
bool Robotiq2fSocketAdapter::setGripperVariables(const std::map<std::string, double>& variableMap) {
|
||||||
|
// Build the command string
|
||||||
|
std::string cmd = SET_COMMAND;
|
||||||
|
for (const auto& [variable, value] : variableMap) {
|
||||||
|
std::stringstream ss;
|
||||||
|
ss << value;
|
||||||
|
cmd += " " + variable + " " + ss.str();
|
||||||
|
}
|
||||||
|
cmd += "\n";
|
||||||
|
|
||||||
|
// Send the command and receive the response
|
||||||
|
std::lock_guard<std::mutex> lock(socket_mutex_);
|
||||||
|
if (*sockfd_ < 0) {
|
||||||
|
throw std::runtime_error("Cannot set variables on a disconnected socket.");
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!sendCommand(cmd)) {
|
||||||
|
throw std::runtime_error("[setGripperVariables] Sending command failed.");
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::string response = receiveResponse();
|
||||||
|
return isAck(response);
|
||||||
|
}
|
||||||
|
|
||||||
|
bool Robotiq2fSocketAdapter::setGripperVariable(const std::string& variable, int value) {
|
||||||
|
return setGripperVariable(variable, static_cast<double>(value)); // Convert int to double
|
||||||
|
}
|
||||||
|
|
||||||
|
bool Robotiq2fSocketAdapter::setGripperVariable(const std::string& variable, double value) {
|
||||||
|
std::stringstream ss;
|
||||||
|
ss << value; // Convert the value to a string
|
||||||
|
|
||||||
|
std::string cmd = SET_COMMAND + " " + variable + " " + ss.str() + "\n";
|
||||||
|
|
||||||
|
std::lock_guard<std::mutex> lock(socket_mutex_);
|
||||||
|
if (*sockfd_ < 0) {
|
||||||
|
throw std::runtime_error("Cannot set variables on a disconnected socket.");
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!sendCommand(cmd)) {
|
||||||
|
throw std::runtime_error("[setGripperVariable] Sending command failed.");
|
||||||
|
return false; // Sending command failed
|
||||||
|
}
|
||||||
|
|
||||||
|
std::string response = receiveResponse();
|
||||||
|
return isAck(response);
|
||||||
|
}
|
||||||
|
|
||||||
|
int Robotiq2fSocketAdapter::getGripperVariable(const std::string& variable) {
|
||||||
|
std::string cmd = GET_COMMAND + " " + variable + "\n";
|
||||||
|
|
||||||
|
std::lock_guard<std::mutex> lock(socket_mutex_);
|
||||||
|
if (*sockfd_ < 0) {
|
||||||
|
throw std::runtime_error("Cannot get variables on a disconnected socket.");
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!sendCommand(cmd)) {
|
||||||
|
throw std::runtime_error("[getGripperVariable] Sending command failed.");
|
||||||
|
return -1; // Error sending command
|
||||||
|
}
|
||||||
|
|
||||||
|
std::string response = receiveResponse();
|
||||||
|
|
||||||
|
// Parse the response (assuming format: "variable_name value")
|
||||||
|
std::istringstream iss(response);
|
||||||
|
std::string var_name, value_str;
|
||||||
|
|
||||||
|
if (!(iss >> var_name >> value_str)) {
|
||||||
|
throw std::runtime_error("Invalid gripper response format.");
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (var_name != variable) {
|
||||||
|
throw std::runtime_error("Unexpected response - variable name mismatch.");
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
|
||||||
|
try {
|
||||||
|
return std::stoi(value_str);
|
||||||
|
} catch (const std::exception& ex) {
|
||||||
|
std::cerr << "Failed to parse gripper response: " << ex.what() << std::endl;
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Activation, deactivation and reset of the Gripper
|
||||||
|
void Robotiq2fSocketAdapter::activate(bool autoCalibrate) {
|
||||||
|
reset(); // Always start with a reset to ensure a known state
|
||||||
|
setGripperVariable(CMD_ACT, 1); // Activate the gripper
|
||||||
|
waitForGripperStatus(GripperStatus::ACTIVE); // Wait until active
|
||||||
|
|
||||||
|
if (autoCalibrate) {
|
||||||
|
auto_calibrate();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void Robotiq2fSocketAdapter::reset() {
|
||||||
|
setGripperVariable(CMD_ACT, 0);
|
||||||
|
setGripperVariable(CMD_ATR, 0);
|
||||||
|
waitForGripperStatus(GripperStatus::RESET);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
void Robotiq2fSocketAdapter::deactivate() {
|
||||||
|
if (is_active()) {
|
||||||
|
reset();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// State subsscriber
|
||||||
|
bool Robotiq2fSocketAdapter::is_active(){
|
||||||
|
GripperStatus status = static_cast<GripperStatus>(getGripperVariable(CMD_STA));
|
||||||
|
return GripperStatus::ACTIVE == status;
|
||||||
|
}
|
||||||
|
|
||||||
|
int Robotiq2fSocketAdapter::force(){
|
||||||
|
return getGripperVariable(CMD_FOR);
|
||||||
|
}
|
||||||
|
|
||||||
|
int Robotiq2fSocketAdapter::speed(){
|
||||||
|
return getGripperVariable(CMD_SPE);
|
||||||
|
}
|
||||||
|
|
||||||
|
int Robotiq2fSocketAdapter::position(){
|
||||||
|
return getGripperVariable(CMD_POS);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Movement Methods
|
||||||
|
std::tuple<bool, int> Robotiq2fSocketAdapter::move(int position, int speed, int force) {
|
||||||
|
int clippedPosition = clip_val(min_position_, position, max_position_);
|
||||||
|
int clippedSpeed = clip_val(min_speed_, speed, max_speed_);
|
||||||
|
int clippedForce = clip_val(min_force_, force, max_force_);
|
||||||
|
|
||||||
|
// Create a map for gripper variables (similar to Python's OrderedDict)
|
||||||
|
std::map<std::string, int> variableMap = {
|
||||||
|
{ CMD_POS, clippedPosition },
|
||||||
|
{ CMD_SPE, clippedSpeed },
|
||||||
|
{ CMD_FOR, clippedForce },
|
||||||
|
{ CMD_GTO, 1 }
|
||||||
|
};
|
||||||
|
|
||||||
|
bool setResult = setGripperVariables(variableMap);
|
||||||
|
return std::make_tuple(setResult, clippedPosition);
|
||||||
|
}
|
||||||
|
|
||||||
|
std::tuple<int, ObjectStatus> Robotiq2fSocketAdapter::move_and_wait_for_pos(int position, int speed, int force) {
|
||||||
|
int clippedPosition = clip_val(min_position_, position, max_position_);
|
||||||
|
int clippedSpeed = clip_val(min_speed_, speed, max_speed_);
|
||||||
|
int clippedForce = clip_val(min_force_, force, max_force_);
|
||||||
|
|
||||||
|
// Set gripper variables to initiate the move
|
||||||
|
std::map<std::string, int> variableMap = {
|
||||||
|
{ CMD_POS, clippedPosition },
|
||||||
|
{ CMD_SPE, clippedSpeed },
|
||||||
|
{ CMD_FOR, clippedForce },
|
||||||
|
{ CMD_GTO, 1 }
|
||||||
|
};
|
||||||
|
bool setResult = setGripperVariables(variableMap);
|
||||||
|
if (!setResult) {
|
||||||
|
// Handle the error case, e.g., throw an exception
|
||||||
|
throw std::runtime_error("Failed to set variables for move.");
|
||||||
|
}
|
||||||
|
|
||||||
|
// Wait until position is acknowledged:
|
||||||
|
while(getGripperVariable(CMD_PRE) != clippedPosition) {
|
||||||
|
std::this_thread::sleep_for(std::chrono::milliseconds(10)); // Adjust sleep as needed
|
||||||
|
}
|
||||||
|
|
||||||
|
// Wait until not moving
|
||||||
|
int currentObjectStatus = getGripperVariable(CMD_OBJ);
|
||||||
|
while (ObjectStatus(currentObjectStatus) == ObjectStatus::MOVING) {
|
||||||
|
std::this_thread::sleep_for(std::chrono::milliseconds(10)); // Adjust sleep as needed
|
||||||
|
currentObjectStatus = getGripperVariable(CMD_OBJ);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Report final position and object status
|
||||||
|
int finalPosition = getGripperVariable(CMD_POS);
|
||||||
|
ObjectStatus finalStatus = static_cast<ObjectStatus>(currentObjectStatus);
|
||||||
|
return std::make_tuple(finalPosition, finalStatus);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
void Robotiq2fSocketAdapter::auto_calibrate(bool log) {
|
||||||
|
// Open in case we are holding an object
|
||||||
|
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.");
|
||||||
|
}
|
||||||
|
|
||||||
|
// Close as far as possible
|
||||||
|
result = move_and_wait_for_pos(closed_position_, 64, 1);
|
||||||
|
if (std::get<1>(result) != ObjectStatus::AT_DEST) {
|
||||||
|
throw std::runtime_error("Calibration failed because of an object.");
|
||||||
|
}
|
||||||
|
max_position_ = std::get<0>(result); // Update max position
|
||||||
|
|
||||||
|
// Open as far as possible
|
||||||
|
result = move_and_wait_for_pos(open_position_, 64, 1);
|
||||||
|
if (std::get<1>(result) != ObjectStatus::AT_DEST) {
|
||||||
|
throw std::runtime_error("Calibration failed because of an object.");
|
||||||
|
}
|
||||||
|
min_position_ = std::get<0>(result); // Update min position
|
||||||
|
|
||||||
|
if (log) {
|
||||||
|
std::cout << "Gripper auto-calibrated to ["
|
||||||
|
<< min_position_ << ", " << max_position_ << "]\n";
|
||||||
|
}
|
||||||
|
}
|
||||||
|
} // end robotiq_interface
|
||||||
469
src/robotiq_2f/robotiq_2f_interface/src/hardware_interface.cpp
Normal file
469
src/robotiq_2f/robotiq_2f_interface/src/hardware_interface.cpp
Normal file
@ -0,0 +1,469 @@
|
|||||||
|
#include "robotiq_2f_interface/hardware_interface.hpp"
|
||||||
|
|
||||||
|
namespace robotiq_interface
|
||||||
|
{
|
||||||
|
|
||||||
|
const std::string robot_ip_parameter_name = "robot_ip";
|
||||||
|
const std::string robot_ip_default = "192.168.1.1";
|
||||||
|
|
||||||
|
const std::string robot_port_parameter_name = "robot_port";
|
||||||
|
const int robot_port_default = 63352;
|
||||||
|
|
||||||
|
const std::string use_mock_hardware_parameter_name = "use_fake_hardware";
|
||||||
|
const bool use_mock_hardware_default = true;
|
||||||
|
|
||||||
|
const std::string min_position_parameter_name = "min_position";
|
||||||
|
const double min_position_default = 0.01;
|
||||||
|
|
||||||
|
const std::string max_position_parameter_name = "max_position";
|
||||||
|
const double max_position_default = 0.0;
|
||||||
|
|
||||||
|
const std::string min_speed_parameter_name = "min_speed";
|
||||||
|
const double min_speed_default = 0.0;
|
||||||
|
|
||||||
|
const std::string max_speed_parameter_name = "max_speed";
|
||||||
|
const double max_speed_default = 0.0;
|
||||||
|
|
||||||
|
const std::string min_force_parameter_name = "min_force";
|
||||||
|
const double min_force_default = 0.0;
|
||||||
|
|
||||||
|
const std::string max_force_parameter_name = "max_force";
|
||||||
|
const double max_force_default = 0.0;
|
||||||
|
|
||||||
|
RobotiqGripperHardwareInterface::RobotiqGripperHardwareInterface()
|
||||||
|
: communication_thread_is_running_(false)
|
||||||
|
{
|
||||||
|
RCLCPP_INFO(logger_, "Constructor");
|
||||||
|
}
|
||||||
|
|
||||||
|
RobotiqGripperHardwareInterface::~RobotiqGripperHardwareInterface()
|
||||||
|
{
|
||||||
|
if (communication_thread_is_running_.load())
|
||||||
|
{
|
||||||
|
communication_thread_is_running_.store(false);
|
||||||
|
if (communication_thread_.joinable())
|
||||||
|
{
|
||||||
|
communication_thread_.join();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init(const hardware_interface::HardwareInfo& info)
|
||||||
|
{
|
||||||
|
RCLCPP_DEBUG(logger_, "on_init");
|
||||||
|
|
||||||
|
if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS)
|
||||||
|
{
|
||||||
|
return CallbackReturn::ERROR;
|
||||||
|
}
|
||||||
|
|
||||||
|
try
|
||||||
|
{
|
||||||
|
if (info.hardware_parameters.count(robot_ip_parameter_name) > 0) {
|
||||||
|
robot_ip_ = info.hardware_parameters.at(robot_ip_parameter_name);
|
||||||
|
} else {
|
||||||
|
robot_ip_ = robot_ip_default;
|
||||||
|
}
|
||||||
|
RCLCPP_INFO(logger_, "Accessing parameter: Robot IP, Value: %s", robot_ip_.c_str());
|
||||||
|
|
||||||
|
if (info.hardware_parameters.count(robot_port_parameter_name) > 0) {
|
||||||
|
robot_port_ = std::stoi(info.hardware_parameters.at(robot_port_parameter_name));
|
||||||
|
} else {
|
||||||
|
robot_port_ = robot_port_default;
|
||||||
|
}
|
||||||
|
RCLCPP_INFO(logger_, "Accessing parameter: Robot Port, Value: %d", robot_port_);
|
||||||
|
|
||||||
|
std::string param_value;
|
||||||
|
if (info.hardware_parameters.count(use_mock_hardware_parameter_name) > 0) {
|
||||||
|
param_value = info.hardware_parameters.at(use_mock_hardware_parameter_name);
|
||||||
|
use_mock_hardware_ = (param_value == "true" || param_value == "True");
|
||||||
|
} else {
|
||||||
|
use_mock_hardware_ = use_mock_hardware_default;
|
||||||
|
}
|
||||||
|
RCLCPP_INFO(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_);
|
||||||
|
|
||||||
|
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_);
|
||||||
|
|
||||||
|
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_);
|
||||||
|
|
||||||
|
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_);
|
||||||
|
|
||||||
|
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_);
|
||||||
|
|
||||||
|
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_);
|
||||||
|
}
|
||||||
|
catch (const std::exception& e)
|
||||||
|
{
|
||||||
|
RCLCPP_FATAL(logger_, "Failed to load parameters: %s", e.what());
|
||||||
|
return CallbackReturn::ERROR;
|
||||||
|
}
|
||||||
|
|
||||||
|
gripper_position_ = std::numeric_limits<double>::quiet_NaN();
|
||||||
|
gripper_velocity_ = std::numeric_limits<double>::quiet_NaN();
|
||||||
|
gripper_position_command_ = std::numeric_limits<double>::quiet_NaN();
|
||||||
|
reactivate_gripper_cmd_ = NO_NEW_CMD_;
|
||||||
|
reactivate_gripper_async_cmd_.store(false);
|
||||||
|
|
||||||
|
const hardware_interface::ComponentInfo& joint = info_.joints[0];
|
||||||
|
|
||||||
|
// There is one command interface: position.
|
||||||
|
if (joint.command_interfaces.size() != 1)
|
||||||
|
{
|
||||||
|
RCLCPP_FATAL(logger_, "Joint '%s' has %zu command interfaces found. 1 expected.", joint.name.c_str(),
|
||||||
|
joint.command_interfaces.size());
|
||||||
|
return CallbackReturn::ERROR;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION)
|
||||||
|
{
|
||||||
|
RCLCPP_FATAL(logger_, "Joint '%s' has %s command interfaces found. '%s' expected.", joint.name.c_str(),
|
||||||
|
joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION);
|
||||||
|
return CallbackReturn::ERROR;
|
||||||
|
}
|
||||||
|
|
||||||
|
// There are two state interfaces: position and velocity.
|
||||||
|
if (joint.state_interfaces.size() != 2)
|
||||||
|
{
|
||||||
|
RCLCPP_FATAL(logger_, "Joint '%s' has %zu state interface. 2 expected.", joint.name.c_str(),
|
||||||
|
joint.state_interfaces.size());
|
||||||
|
return CallbackReturn::ERROR;
|
||||||
|
}
|
||||||
|
|
||||||
|
for (int i = 0; i < 2; ++i)
|
||||||
|
{
|
||||||
|
if (!(joint.state_interfaces[i].name == hardware_interface::HW_IF_POSITION ||
|
||||||
|
joint.state_interfaces[i].name == hardware_interface::HW_IF_VELOCITY))
|
||||||
|
{
|
||||||
|
RCLCPP_FATAL(logger_, "Joint '%s' has %s state interface. Expected %s or %s.", joint.name.c_str(),
|
||||||
|
joint.state_interfaces[i].name.c_str(), hardware_interface::HW_IF_POSITION,
|
||||||
|
hardware_interface::HW_IF_VELOCITY);
|
||||||
|
return CallbackReturn::ERROR;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return CallbackReturn::SUCCESS;
|
||||||
|
}
|
||||||
|
|
||||||
|
hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_configure(const rclcpp_lifecycle::State& previous_state)
|
||||||
|
{
|
||||||
|
RCLCPP_DEBUG(logger_, "on_configure");
|
||||||
|
try
|
||||||
|
{
|
||||||
|
if (hardware_interface::SystemInterface::on_configure(previous_state) != CallbackReturn::SUCCESS)
|
||||||
|
{
|
||||||
|
return CallbackReturn::ERROR;
|
||||||
|
}
|
||||||
|
|
||||||
|
configureAdapter(use_mock_hardware_);
|
||||||
|
if (!socket_adapter_->connect(robot_ip_, robot_port_))
|
||||||
|
{
|
||||||
|
RCLCPP_ERROR(logger_, "Cannot connect to the Robotiq gripper at %s:%d", robot_ip_.c_str(), robot_port_);
|
||||||
|
return CallbackReturn::ERROR;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
catch (const std::exception& e)
|
||||||
|
{
|
||||||
|
RCLCPP_ERROR(logger_, "Cannot configure the Robotiq gripper: %s", e.what());
|
||||||
|
return CallbackReturn::ERROR;
|
||||||
|
}
|
||||||
|
|
||||||
|
return CallbackReturn::SUCCESS;
|
||||||
|
}
|
||||||
|
|
||||||
|
hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_activate(const rclcpp_lifecycle::State& /*previous_state*/)
|
||||||
|
{
|
||||||
|
RCLCPP_DEBUG(logger_, "on_activate");
|
||||||
|
|
||||||
|
// set some default values for joints
|
||||||
|
if (std::isnan(gripper_position_))
|
||||||
|
{
|
||||||
|
gripper_position_ = 0;
|
||||||
|
gripper_velocity_ = 0;
|
||||||
|
gripper_position_command_ = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Activate the gripper.
|
||||||
|
try
|
||||||
|
{
|
||||||
|
socket_adapter_->deactivate();
|
||||||
|
socket_adapter_->activate();
|
||||||
|
|
||||||
|
communication_thread_is_running_.store(true);
|
||||||
|
communication_thread_ = std::thread([this] { this->background_task(); });
|
||||||
|
}
|
||||||
|
catch (const std::exception& e)
|
||||||
|
{
|
||||||
|
RCLCPP_FATAL(logger_, "Failed to communicate with the Robotiq gripper: %s", e.what());
|
||||||
|
return CallbackReturn::ERROR;
|
||||||
|
}
|
||||||
|
|
||||||
|
RCLCPP_INFO(logger_, "Robotiq Gripper successfully activated!");
|
||||||
|
return CallbackReturn::SUCCESS;
|
||||||
|
}
|
||||||
|
|
||||||
|
hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_deactivate(const rclcpp_lifecycle::State& /*previous_state*/)
|
||||||
|
{
|
||||||
|
RCLCPP_DEBUG(logger_, "on_deactivate");
|
||||||
|
|
||||||
|
communication_thread_is_running_.store(false);
|
||||||
|
communication_thread_.join();
|
||||||
|
if (communication_thread_.joinable())
|
||||||
|
{
|
||||||
|
communication_thread_.join();
|
||||||
|
}
|
||||||
|
|
||||||
|
try
|
||||||
|
{
|
||||||
|
socket_adapter_->deactivate();
|
||||||
|
}
|
||||||
|
catch (const std::exception& e)
|
||||||
|
{
|
||||||
|
RCLCPP_ERROR(logger_, "Failed to deactivate the Robotiq gripper: %s", e.what());
|
||||||
|
return CallbackReturn::ERROR;
|
||||||
|
}
|
||||||
|
RCLCPP_INFO(logger_, "Robotiq Gripper successfully deactivated!");
|
||||||
|
return CallbackReturn::SUCCESS;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::vector<hardware_interface::StateInterface> RobotiqGripperHardwareInterface::export_state_interfaces()
|
||||||
|
{
|
||||||
|
RCLCPP_DEBUG(logger_, "export_state_interfaces");
|
||||||
|
|
||||||
|
std::vector<hardware_interface::StateInterface> state_interfaces;
|
||||||
|
|
||||||
|
state_interfaces.emplace_back(
|
||||||
|
hardware_interface::StateInterface(info_.joints[0].name, hardware_interface::HW_IF_POSITION, &gripper_position_));
|
||||||
|
state_interfaces.emplace_back(
|
||||||
|
hardware_interface::StateInterface(info_.joints[0].name, hardware_interface::HW_IF_VELOCITY, &gripper_velocity_));
|
||||||
|
|
||||||
|
for (const auto& interface : state_interfaces) {
|
||||||
|
RCLCPP_DEBUG(logger_, "Exporting state interface for joint: %s type: %s",
|
||||||
|
interface.get_name().c_str(), interface.get_interface_name().c_str());
|
||||||
|
}
|
||||||
|
|
||||||
|
return state_interfaces;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::vector<hardware_interface::CommandInterface> RobotiqGripperHardwareInterface::export_command_interfaces()
|
||||||
|
{
|
||||||
|
RCLCPP_DEBUG(logger_, "export_command_interfaces");
|
||||||
|
|
||||||
|
std::vector<hardware_interface::CommandInterface> command_interfaces;
|
||||||
|
|
||||||
|
command_interfaces.emplace_back(
|
||||||
|
hardware_interface::CommandInterface(
|
||||||
|
info_.joints[0].name, hardware_interface::HW_IF_POSITION, &gripper_position_command_
|
||||||
|
)
|
||||||
|
);
|
||||||
|
|
||||||
|
// Handling potential uninitialized parameter 'gripper_speed_multiplier'
|
||||||
|
double gripper_speed_multiplier = 1.0;
|
||||||
|
if (info_.hardware_parameters.find("gripper_speed_multiplier") != info_.hardware_parameters.end()) {
|
||||||
|
gripper_speed_multiplier = std::stod(info_.hardware_parameters["gripper_speed_multiplier"]);
|
||||||
|
}
|
||||||
|
gripper_speed_ = gripper_speed_multiplier;
|
||||||
|
|
||||||
|
// Handling potential uninitialized parameter 'gripper_force_multiplier'
|
||||||
|
double gripper_force_multiplier = 1.0;
|
||||||
|
if (info_.hardware_parameters.find("gripper_force_multiplier") != info_.hardware_parameters.end()) {
|
||||||
|
gripper_force_multiplier = std::stod(info_.hardware_parameters["gripper_force_multiplier"]);
|
||||||
|
}
|
||||||
|
gripper_force_ = gripper_force_multiplier;
|
||||||
|
|
||||||
|
command_interfaces.emplace_back(
|
||||||
|
hardware_interface::CommandInterface(info_.joints[0].name, "set_gripper_max_velocity", &gripper_speed_));
|
||||||
|
command_interfaces.emplace_back(
|
||||||
|
hardware_interface::CommandInterface(info_.joints[0].name, "set_gripper_max_effort", &gripper_force_));
|
||||||
|
command_interfaces.emplace_back(
|
||||||
|
hardware_interface::CommandInterface("reactivate_gripper", "reactivate_gripper_cmd", &reactivate_gripper_cmd_));
|
||||||
|
command_interfaces.emplace_back(
|
||||||
|
hardware_interface::CommandInterface("reactivate_gripper", "reactivate_gripper_response", &reactivate_gripper_response_));
|
||||||
|
|
||||||
|
for (const auto& interface : command_interfaces) {
|
||||||
|
RCLCPP_DEBUG(logger_, "Exporting command interface for joint: %s type: %s",
|
||||||
|
interface.get_name().c_str(), interface.get_interface_name().c_str());
|
||||||
|
}
|
||||||
|
|
||||||
|
return command_interfaces;
|
||||||
|
}
|
||||||
|
|
||||||
|
hardware_interface::return_type RobotiqGripperHardwareInterface::read(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/)
|
||||||
|
{
|
||||||
|
RCLCPP_DEBUG(logger_, "Reading state from the gripper");
|
||||||
|
|
||||||
|
// Ensure that the node uses the appropriate clock source
|
||||||
|
|
||||||
|
current_time_ = clock_->now();
|
||||||
|
|
||||||
|
try {
|
||||||
|
int raw_position = socket_adapter_->position();
|
||||||
|
double new_position = convertRawToPosition(raw_position);
|
||||||
|
|
||||||
|
if (!std::isnan(gripper_position_)) {
|
||||||
|
double time_difference = (current_time_ - last_update_time_).seconds();
|
||||||
|
if (time_difference > 0) {
|
||||||
|
gripper_velocity_ = (new_position - gripper_position_) / time_difference;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
gripper_position_ = new_position;
|
||||||
|
last_update_time_ = current_time_;
|
||||||
|
|
||||||
|
} catch (const std::exception& e) {
|
||||||
|
RCLCPP_ERROR(logger_, "Failed to read from Robotiq gripper: %s", e.what());
|
||||||
|
return hardware_interface::return_type::ERROR;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Process asynchronous reactivation response if available
|
||||||
|
if (reactivate_gripper_async_response_.load().has_value()) {
|
||||||
|
reactivate_gripper_response_ = reactivate_gripper_async_response_.load().value();
|
||||||
|
reactivate_gripper_async_response_.store(std::nullopt); // Reset response
|
||||||
|
}
|
||||||
|
|
||||||
|
return hardware_interface::return_type::OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
hardware_interface::return_type RobotiqGripperHardwareInterface::write(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/)
|
||||||
|
{
|
||||||
|
try {
|
||||||
|
// Use the conversion methods to scale the command values to the range expected by the hardware
|
||||||
|
int scaled_position = convertPositionToRaw(gripper_position_command_);
|
||||||
|
int scaled_speed = convertSpeedToRaw(gripper_speed_);
|
||||||
|
int scaled_force = convertForceToRaw(gripper_force_);
|
||||||
|
|
||||||
|
// Store the scaled values into atomic variables
|
||||||
|
write_command_.store(scaled_position);
|
||||||
|
write_speed_.store(scaled_speed);
|
||||||
|
write_force_.store(scaled_force);
|
||||||
|
|
||||||
|
} catch (const std::exception& e) {
|
||||||
|
RCLCPP_ERROR(logger_, "Failed to write to Robotiq gripper: %s", e.what());
|
||||||
|
return hardware_interface::return_type::ERROR;
|
||||||
|
}
|
||||||
|
|
||||||
|
return hardware_interface::return_type::OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
void RobotiqGripperHardwareInterface::background_task()
|
||||||
|
{
|
||||||
|
while (communication_thread_is_running_.load())
|
||||||
|
{
|
||||||
|
std::lock_guard<std::mutex> guard(resource_mutex_);
|
||||||
|
try
|
||||||
|
{
|
||||||
|
// Re-activate the gripper
|
||||||
|
// This can be used, for example, to re-run the auto-calibration.
|
||||||
|
if (reactivate_gripper_async_cmd_.load())
|
||||||
|
{
|
||||||
|
socket_adapter_->deactivate();
|
||||||
|
socket_adapter_->activate();
|
||||||
|
reactivate_gripper_async_cmd_.store(false);
|
||||||
|
reactivate_gripper_async_response_.store(true);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Write the latest command to the gripper.
|
||||||
|
int scaled_position = write_command_.load();
|
||||||
|
int scaled_speed = write_speed_.load();
|
||||||
|
int scaled_force = write_force_.load();
|
||||||
|
|
||||||
|
auto result = socket_adapter_->move(scaled_position, scaled_speed, scaled_force);
|
||||||
|
if (!std::get<0>(result)) {
|
||||||
|
throw std::runtime_error("Failed to send move command to Robotiq gripper.");
|
||||||
|
}
|
||||||
|
|
||||||
|
// Read the state of the gripper.
|
||||||
|
int raw_position = socket_adapter_->position();
|
||||||
|
gripper_current_state_.store(convertRawToPosition(raw_position));
|
||||||
|
}
|
||||||
|
catch (const std::exception& e)
|
||||||
|
{
|
||||||
|
RCLCPP_ERROR(logger_, "Error in background task: %s", e.what());
|
||||||
|
}
|
||||||
|
|
||||||
|
std::this_thread::sleep_for(gripperCommsLoopPeriod); // Sleep for a predefined period
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// Mock Hardware
|
||||||
|
void RobotiqGripperHardwareInterface::configureAdapter(bool useMock) {
|
||||||
|
if (useMock) {
|
||||||
|
RCLCPP_INFO(logger_, "Using Mock Robotiq Adapter");
|
||||||
|
socket_adapter_ = std::make_unique<MockRobotiq2fSocketAdapter>();
|
||||||
|
} else {
|
||||||
|
RCLCPP_INFO(logger_, "Using Real Robotiq Adapter");
|
||||||
|
socket_adapter_ = std::make_unique<Robotiq2fSocketAdapter>();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// Conversion methods
|
||||||
|
double RobotiqGripperHardwareInterface::convertRawToPosition(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.");
|
||||||
|
}
|
||||||
|
double scaled_position = min_position_ + (static_cast<double>(raw_position) / 255.0) * (max_position_ - min_position_);
|
||||||
|
return std::max(min_position_, std::min(scaled_position, max_position_));
|
||||||
|
}
|
||||||
|
|
||||||
|
int RobotiqGripperHardwareInterface::convertPositionToRaw(double 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.");
|
||||||
|
}
|
||||||
|
double scaled = (position - min_position_) / (max_position_ - min_position_) * 255.0;
|
||||||
|
return static_cast<uint8_t>(std::clamp(scaled, 0.0, 255.0));
|
||||||
|
}
|
||||||
|
|
||||||
|
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));
|
||||||
|
}
|
||||||
|
|
||||||
|
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));
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace robotiq_interface
|
||||||
|
|
||||||
|
#include "pluginlib/class_list_macros.hpp"
|
||||||
|
|
||||||
|
PLUGINLIB_EXPORT_CLASS(robotiq_interface::RobotiqGripperHardwareInterface, hardware_interface::SystemInterface)
|
||||||
@ -0,0 +1,69 @@
|
|||||||
|
#include <gtest/gtest.h>
|
||||||
|
#include "RobotiqGripperHardwareInterface.hpp"
|
||||||
|
|
||||||
|
class TestableRobotiqGripperHardwareInterface : public robotiq_driver::RobotiqGripperHardwareInterface {
|
||||||
|
public:
|
||||||
|
using robotiq_driver::RobotiqGripperHardwareInterface::convertPositionToRaw;
|
||||||
|
using robotiq_driver::RobotiqGripperHardwareInterface::convertRawToPosition;
|
||||||
|
using robotiq_driver::RobotiqGripperHardwareInterface::convertSpeedToRaw;
|
||||||
|
using robotiq_driver::RobotiqGripperHardwareInterface::convertForceToRaw;
|
||||||
|
};
|
||||||
|
|
||||||
|
class RobotiqGripperConversionTests : public ::testing::Test {
|
||||||
|
protected:
|
||||||
|
TestableRobotiqGripperHardwareInterface gripper_interface;
|
||||||
|
|
||||||
|
void SetUp() override {
|
||||||
|
gripper_interface.min_position_ = 0.0;
|
||||||
|
gripper_interface.max_position_ = 0.14;
|
||||||
|
gripper_interface.min_speed_ = 0.02;
|
||||||
|
gripper_interface.max_speed_ = 0.15;
|
||||||
|
gripper_interface.min_force_ = 20.0;
|
||||||
|
gripper_interface.max_force_ = 235.0;
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
// Test for convertPositionToRaw
|
||||||
|
TEST_F(RobotiqGripperConversionTests, PositionToRawValidInput) {
|
||||||
|
EXPECT_EQ(gripper_interface.convertPositionToRaw(0.07), 128);
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST_F(RobotiqGripperConversionTests, PositionToRawBoundaryInput) {
|
||||||
|
EXPECT_EQ(gripper_interface.convertPositionToRaw(0.0), 0);
|
||||||
|
EXPECT_EQ(gripper_interface.convertPositionToRaw(0.14), 255);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Test for convertRawToPosition
|
||||||
|
TEST_F(RobotiqGripperConversionTests, RawToPositionValidInput) {
|
||||||
|
ASSERT_NEAR(gripper_interface.convertRawToPosition(128), 0.07, 0.01);
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST_F(RobotiqGripperConversionTests, RawToPositionBoundaryInput) {
|
||||||
|
ASSERT_NEAR(gripper_interface.convertRawToPosition(0), 0.0, 0.01);
|
||||||
|
ASSERT_NEAR(gripper_interface.convertRawToPosition(255), 0.14, 0.01);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Test for convertSpeedToRaw
|
||||||
|
TEST_F(RobotiqGripperConversionTests, SpeedToRawValidInput) {
|
||||||
|
EXPECT_EQ(gripper_interface.convertSpeedToRaw(0.085), 128);
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST_F(RobotiqGripperConversionTests, SpeedToRawBoundaryInput) {
|
||||||
|
EXPECT_EQ(gripper_interface.convertSpeedToRaw(0.02), 0);
|
||||||
|
EXPECT_EQ(gripper_interface.convertSpeedToRaw(0.15), 255);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Test for convertForceToRaw
|
||||||
|
TEST_F(RobotiqGripperConversionTests, ForceToRawValidInput) {
|
||||||
|
EXPECT_EQ(gripper_interface.convertForceToRaw(127.5), 128);
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST_F(RobotiqGripperConversionTests, ForceToRawBoundaryInput) {
|
||||||
|
EXPECT_EQ(gripper_interface.convertForceToRaw(20.0), 0);
|
||||||
|
EXPECT_EQ(gripper_interface.convertForceToRaw(235.0), 255);
|
||||||
|
}
|
||||||
|
|
||||||
|
int main(int argc, char **argv) {
|
||||||
|
::testing::InitGoogleTest(&argc, argv);
|
||||||
|
return RUN_ALL_TESTS();
|
||||||
|
}
|
||||||
@ -11,17 +11,11 @@
|
|||||||
|
|
||||||
<!-- Dependencies -->
|
<!-- Dependencies -->
|
||||||
<depend>rclcpp</depend>
|
<depend>rclcpp</depend>
|
||||||
<<<<<<<< HEAD:src/servo_keyboard/package.xml
|
|
||||||
<depend>control_msgs</depend>
|
<depend>control_msgs</depend>
|
||||||
<depend>geometry_msgs</depend>
|
<depend>geometry_msgs</depend>
|
||||||
<build_depend>controller_manager_msgs</build_depend>
|
<build_depend>controller_manager_msgs</build_depend>
|
||||||
<exec_depend>controller_manager_msgs</exec_depend>
|
<exec_depend>controller_manager_msgs</exec_depend>
|
||||||
|
|
||||||
========
|
|
||||||
<depend>rclcpp_lifecycle</depend>
|
|
||||||
<depend>hardware_interface</depend>
|
|
||||||
<depend>pluginlib</depend>
|
|
||||||
>>>>>>>> origin/main:src/robotiq_2f/robotiq_2f_interface/package.xml
|
|
||||||
|
|
||||||
<test_depend>ament_lint_auto</test_depend>
|
<test_depend>ament_lint_auto</test_depend>
|
||||||
<test_depend>ament_lint_common</test_depend>
|
<test_depend>ament_lint_common</test_depend>
|
||||||
|
|||||||
@ -1,9 +1,9 @@
|
|||||||
<?xml version="1.0" ?>
|
<?xml version="1.0" ?>
|
||||||
<!-- =================================================================================== -->
|
<!-- =================================================================================== -->
|
||||||
<!-- | This document was autogenerated by xacro from src/ur_robotiq_description/urdf/ur_robotiq.urdf.xacro | -->
|
<!-- | This document was autogenerated by xacro from ur_robotiq.urdf.xacro | -->
|
||||||
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
|
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
|
||||||
<!-- =================================================================================== -->
|
<!-- =================================================================================== -->
|
||||||
<robot name="ur">
|
<robot name="ur_manipulator">
|
||||||
<!--
|
<!--
|
||||||
Base UR robot series xacro macro.
|
Base UR robot series xacro macro.
|
||||||
|
|
||||||
@ -59,7 +59,7 @@
|
|||||||
this location. Nor can they rely on the existence of the macro.
|
this location. Nor can they rely on the existence of the macro.
|
||||||
-->
|
-->
|
||||||
<link name="world"/>
|
<link name="world"/>
|
||||||
<ros2_control name="ur" type="system">
|
<ros2_control name="ur_manipulator" type="system">
|
||||||
<hardware>
|
<hardware>
|
||||||
<plugin>ur_robot_driver/URPositionHardwareInterface</plugin>
|
<plugin>ur_robot_driver/URPositionHardwareInterface</plugin>
|
||||||
<param name="robot_ip">0.0.0.0</param>
|
<param name="robot_ip">0.0.0.0</param>
|
||||||
@ -99,7 +99,7 @@
|
|||||||
</command_interface>
|
</command_interface>
|
||||||
<state_interface name="position">
|
<state_interface name="position">
|
||||||
<!-- initial position for the FakeSystem and simulation -->
|
<!-- initial position for the FakeSystem and simulation -->
|
||||||
<param name="initial_value">1.4</param>
|
<param name="initial_value">0.0</param>
|
||||||
</state_interface>
|
</state_interface>
|
||||||
<state_interface name="velocity"/>
|
<state_interface name="velocity"/>
|
||||||
<state_interface name="effort"/>
|
<state_interface name="effort"/>
|
||||||
@ -131,7 +131,7 @@
|
|||||||
</command_interface>
|
</command_interface>
|
||||||
<state_interface name="position">
|
<state_interface name="position">
|
||||||
<!-- initial position for the FakeSystem and simulation -->
|
<!-- initial position for the FakeSystem and simulation -->
|
||||||
<param name="initial_value">1.57</param>
|
<param name="initial_value">0.0</param>
|
||||||
</state_interface>
|
</state_interface>
|
||||||
<state_interface name="velocity"/>
|
<state_interface name="velocity"/>
|
||||||
<state_interface name="effort"/>
|
<state_interface name="effort"/>
|
||||||
@ -163,7 +163,7 @@
|
|||||||
</command_interface>
|
</command_interface>
|
||||||
<state_interface name="position">
|
<state_interface name="position">
|
||||||
<!-- initial position for the FakeSystem and simulation -->
|
<!-- initial position for the FakeSystem and simulation -->
|
||||||
<param name="initial_value">-1.57</param>
|
<param name="initial_value">0.0</param>
|
||||||
</state_interface>
|
</state_interface>
|
||||||
<state_interface name="velocity"/>
|
<state_interface name="velocity"/>
|
||||||
<state_interface name="effort"/>
|
<state_interface name="effort"/>
|
||||||
@ -591,7 +591,7 @@
|
|||||||
<visual>
|
<visual>
|
||||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
<geometry>
|
<geometry>
|
||||||
<mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_base_link.stl"/>
|
<mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_base_link.stl"/>
|
||||||
</geometry>
|
</geometry>
|
||||||
<material name="">
|
<material name="">
|
||||||
<color rgba="0.1 0.1 0.1 1"/>
|
<color rgba="0.1 0.1 0.1 1"/>
|
||||||
@ -600,7 +600,7 @@
|
|||||||
<collision>
|
<collision>
|
||||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
<geometry>
|
<geometry>
|
||||||
<mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_base_link.stl"/>
|
<mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_base_link.stl"/>
|
||||||
</geometry>
|
</geometry>
|
||||||
</collision>
|
</collision>
|
||||||
</link>
|
</link>
|
||||||
@ -618,7 +618,7 @@
|
|||||||
<visual>
|
<visual>
|
||||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
<geometry>
|
<geometry>
|
||||||
<mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_outer_knuckle.stl"/>
|
<mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_outer_knuckle.stl"/>
|
||||||
</geometry>
|
</geometry>
|
||||||
<material name="">
|
<material name="">
|
||||||
<color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
|
<color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
|
||||||
@ -627,7 +627,7 @@
|
|||||||
<collision>
|
<collision>
|
||||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
<geometry>
|
<geometry>
|
||||||
<mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_outer_knuckle.stl"/>
|
<mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_outer_knuckle.stl"/>
|
||||||
</geometry>
|
</geometry>
|
||||||
</collision>
|
</collision>
|
||||||
</link>
|
</link>
|
||||||
@ -640,7 +640,7 @@
|
|||||||
<visual>
|
<visual>
|
||||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
<geometry>
|
<geometry>
|
||||||
<mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_outer_finger.stl"/>
|
<mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_outer_finger.stl"/>
|
||||||
</geometry>
|
</geometry>
|
||||||
<material name="">
|
<material name="">
|
||||||
<color rgba="0.1 0.1 0.1 1"/>
|
<color rgba="0.1 0.1 0.1 1"/>
|
||||||
@ -649,7 +649,7 @@
|
|||||||
<collision>
|
<collision>
|
||||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
<geometry>
|
<geometry>
|
||||||
<mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_outer_finger.stl"/>
|
<mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_outer_finger.stl"/>
|
||||||
</geometry>
|
</geometry>
|
||||||
</collision>
|
</collision>
|
||||||
</link>
|
</link>
|
||||||
@ -662,7 +662,7 @@
|
|||||||
<visual>
|
<visual>
|
||||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
<geometry>
|
<geometry>
|
||||||
<mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_inner_finger.stl"/>
|
<mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_inner_finger.stl"/>
|
||||||
</geometry>
|
</geometry>
|
||||||
<material name="">
|
<material name="">
|
||||||
<color rgba="0.1 0.1 0.1 1"/>
|
<color rgba="0.1 0.1 0.1 1"/>
|
||||||
@ -671,7 +671,7 @@
|
|||||||
<collision>
|
<collision>
|
||||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
<geometry>
|
<geometry>
|
||||||
<mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_inner_finger.stl"/>
|
<mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_inner_finger.stl"/>
|
||||||
</geometry>
|
</geometry>
|
||||||
</collision>
|
</collision>
|
||||||
</link>
|
</link>
|
||||||
@ -704,7 +704,7 @@
|
|||||||
<visual>
|
<visual>
|
||||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
<geometry>
|
<geometry>
|
||||||
<mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_inner_knuckle.stl"/>
|
<mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_inner_knuckle.stl"/>
|
||||||
</geometry>
|
</geometry>
|
||||||
<material name="">
|
<material name="">
|
||||||
<color rgba="0.1 0.1 0.1 1"/>
|
<color rgba="0.1 0.1 0.1 1"/>
|
||||||
@ -713,7 +713,7 @@
|
|||||||
<collision>
|
<collision>
|
||||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
<geometry>
|
<geometry>
|
||||||
<mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_inner_knuckle.stl"/>
|
<mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_inner_knuckle.stl"/>
|
||||||
</geometry>
|
</geometry>
|
||||||
</collision>
|
</collision>
|
||||||
</link>
|
</link>
|
||||||
@ -726,7 +726,7 @@
|
|||||||
<visual>
|
<visual>
|
||||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
<geometry>
|
<geometry>
|
||||||
<mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_outer_knuckle.stl"/>
|
<mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_outer_knuckle.stl"/>
|
||||||
</geometry>
|
</geometry>
|
||||||
<material name="">
|
<material name="">
|
||||||
<color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
|
<color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
|
||||||
@ -735,7 +735,7 @@
|
|||||||
<collision>
|
<collision>
|
||||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
<geometry>
|
<geometry>
|
||||||
<mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_outer_knuckle.stl"/>
|
<mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_outer_knuckle.stl"/>
|
||||||
</geometry>
|
</geometry>
|
||||||
</collision>
|
</collision>
|
||||||
</link>
|
</link>
|
||||||
@ -748,7 +748,7 @@
|
|||||||
<visual>
|
<visual>
|
||||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
<geometry>
|
<geometry>
|
||||||
<mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_outer_finger.stl"/>
|
<mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_outer_finger.stl"/>
|
||||||
</geometry>
|
</geometry>
|
||||||
<material name="">
|
<material name="">
|
||||||
<color rgba="0.1 0.1 0.1 1"/>
|
<color rgba="0.1 0.1 0.1 1"/>
|
||||||
@ -757,7 +757,7 @@
|
|||||||
<collision>
|
<collision>
|
||||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
<geometry>
|
<geometry>
|
||||||
<mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_outer_finger.stl"/>
|
<mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_outer_finger.stl"/>
|
||||||
</geometry>
|
</geometry>
|
||||||
</collision>
|
</collision>
|
||||||
</link>
|
</link>
|
||||||
@ -770,7 +770,7 @@
|
|||||||
<visual>
|
<visual>
|
||||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
<geometry>
|
<geometry>
|
||||||
<mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_inner_finger.stl"/>
|
<mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_inner_finger.stl"/>
|
||||||
</geometry>
|
</geometry>
|
||||||
<material name="">
|
<material name="">
|
||||||
<color rgba="0.1 0.1 0.1 1"/>
|
<color rgba="0.1 0.1 0.1 1"/>
|
||||||
@ -779,7 +779,7 @@
|
|||||||
<collision>
|
<collision>
|
||||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
<geometry>
|
<geometry>
|
||||||
<mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_inner_finger.stl"/>
|
<mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_inner_finger.stl"/>
|
||||||
</geometry>
|
</geometry>
|
||||||
</collision>
|
</collision>
|
||||||
</link>
|
</link>
|
||||||
@ -812,7 +812,7 @@
|
|||||||
<visual>
|
<visual>
|
||||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
<geometry>
|
<geometry>
|
||||||
<mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_inner_knuckle.stl"/>
|
<mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_inner_knuckle.stl"/>
|
||||||
</geometry>
|
</geometry>
|
||||||
<material name="">
|
<material name="">
|
||||||
<color rgba="0.1 0.1 0.1 1"/>
|
<color rgba="0.1 0.1 0.1 1"/>
|
||||||
@ -821,7 +821,7 @@
|
|||||||
<collision>
|
<collision>
|
||||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
<geometry>
|
<geometry>
|
||||||
<mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_inner_knuckle.stl"/>
|
<mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_inner_knuckle.stl"/>
|
||||||
</geometry>
|
</geometry>
|
||||||
</collision>
|
</collision>
|
||||||
</link>
|
</link>
|
||||||
|
|||||||
@ -0,0 +1,39 @@
|
|||||||
|
<?xml version="1.0" encoding="utf-8"?>
|
||||||
|
<robot xmlns:xacro="http://ros.org/wiki/xacro" name="ur_robotiq_adapter">
|
||||||
|
<xacro:macro name="ur_to_robotiq" params="prefix parent child rotation:=^|${0.0}">
|
||||||
|
|
||||||
|
<joint name="${prefix}ur_to_robotiq_joint" type="fixed">
|
||||||
|
<!-- The parent link must be read from the robot model it is attached to. -->
|
||||||
|
<parent link="${parent}"/>
|
||||||
|
<child link="${prefix}ur_to_robotiq_link"/>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 ${rotation}"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<link name="${prefix}ur_to_robotiq_link">
|
||||||
|
<visual>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="package://robotiq_description/meshes/visual/2f_85/ur_to_robotiq_adapter.dae"/>
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="package://robotiq_description/meshes/collision/2f_85/ur_to_robotiq_adapter.stl"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.01" />
|
||||||
|
<origin xyz="0 0 0" />
|
||||||
|
<inertia ixx="0.000044" ixy="0.0" ixz="0.0" iyy="0.000027" iyz="0.0" izz="0.000027" />
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<joint name="${prefix}gripper_side_joint" type="fixed">
|
||||||
|
<parent link="${prefix}ur_to_robotiq_link"/>
|
||||||
|
<child link="${child}"/>
|
||||||
|
<!-- <origin xyz="0 0 0.011" rpy="0 ${-pi/2} ${pi/2}"/> -->
|
||||||
|
<origin xyz="0 0 0.011" rpy="0 0 0"/>
|
||||||
|
</joint>
|
||||||
|
</xacro:macro>
|
||||||
|
</robot>
|
||||||
Loading…
Reference in New Issue
Block a user