Integrate the Hand-E

.pre-commit-config.yaml

@@ -46,7 +46,7 @@ repos:
     rev: v2.2.5
     hooks:
       - id: codespell
-        args: ['--write-changes', '-L', 'atleast']  # Provide a comma-separated list of misspelled words that codespell should ignore (for example: '-L', 'word1,word2,word3').
+        args: ['--write-changes', '-L', 'atleast', 'hande']  # Provide a comma-separated list of misspelled words that codespell should ignore (for example: '-L', 'word1,word2,word3').
         exclude: \.(svg|pyc|stl|dae|lock)$
 
   - repo: https://github.com/pre-commit/mirrors-clang-format

robotiq_description/config/robotiq_controllers.yaml

@@ -4,7 +4,7 @@ controller_manager:
     joint_state_broadcaster:
       type: joint_state_broadcaster/JointStateBroadcaster
     robotiq_gripper_controller:
-      type: position_controllers/GripperActionController
+      type: parallel_gripper_action_controller/GripperActionController
     robotiq_activation_controller:
       type: robotiq_controllers/RobotiqActivationController
 
@@ -15,6 +15,20 @@ robotiq_gripper_controller:
     use_effort_interface: true
     use_speed_interface: true
 
+robotiq_gripper_controller:
+  ros__parameters:
+
+    joint: hande_finger_distance
+    state_interfaces: ["position", "velocity"]
+
+    allow_stalling: true
+
+    max_effort_interface: robotiq_85_left_knuckle_joint/set_gripper_max_effort
+    max_effort: 20.0  #adjustable between 20-235 N (2F-85) and 10-125 N (2F-140)
+
+    max_velocity_interface: robotiq_85_left_knuckle_joint/set_gripper_max_velocity
+    max_velocity: 0.15  # adjustable between 0.02-0.15 m/s (2F-85) and 0.3-0.250 m/s (2F-140)
+
 robotiq_activation_controller:
   ros__parameters:
     default: true

robotiq_description/config/robotiq_hand_e_controllers.yaml

@@ -0,0 +1,27 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 500  # Hz
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+    robotiq_gripper_controller:
+      type: parallel_gripper_action_controller/GripperActionController
+    robotiq_activation_controller:
+      type: robotiq_controllers/RobotiqActivationController
+
+robotiq_gripper_controller:
+  ros__parameters:
+
+    joint: hande_finger_distance
+    state_interfaces: ["position", "velocity"]
+
+    allow_stalling: true
+
+    max_effort_interface: hande_finger_distance/set_gripper_max_effort
+    max_effort: 20.0  #adjustable between 20 and 185 N
+
+    max_velocity_interface: hande_finger_distance/set_gripper_max_velocity
+    max_velocity: 0.15  # adjustable between 0.02 and 0.15 m/s
+
+robotiq_activation_controller:
+  ros__parameters:
+    default: true

robotiq_description/launch/hand-e_control.launch.xml

@@ -0,0 +1,49 @@
+<launch>
+  <arg name="runtime_config_package"
+       default="robotiq_description"
+       description="Package with the controller's configuration in 'config' folder. Usually the argument is not set, it enables use of a custom setup."/>
+  <arg name="controllers_file"
+       default="robotiq_hand_e_controllers"
+       description="YAML file (without extension) with the controllers configuration."/>
+  <arg name="description_package"
+       default="robotiq_description"
+       description="Description package with robot URDF/xacro files. Usually the argument is not set, it enables use of a custom description."/>
+  <arg name="description_file"
+       default="robotiq_hand_e_gripper"
+       description="URDF/XACRO description file (without extension) with the robot."/>
+  <arg name="prefix"
+       default=""
+       description="Prefix of the joint names, useful for multi-robot setup. If changed than also joint names in the controllers' configuration have to be updated."/>
+  <arg name="use_mock_hardware"
+       default="false"
+       description="Start robot with mock hardware mirroring command to its states."/>
+  <arg name="mock_sensor_commands"
+       default="false"
+       description="Enable mock command interfaces for sensors used for simple simulations. Used only if 'use_mock_hardware' parameter is true."/>
+
+  <let name="robot_description_content" value="$(command '$(find-exec xacro) $(find-pkg-share $(var description_package))/urdf/$(var description_file).urdf.xacro prefix:=$(var prefix) use_mock_hardware:=$(var use_mock_hardware) mock_sensor_commands:=$(var mock_sensor_commands)')"/>
+
+
+  <!--robot_state_pub_node-->
+  <node pkg="robot_state_publisher" exec="robot_state_publisher" output="both">
+    <param name="robot_description" value="$(var robot_description_content)"/>
+  </node>
+
+  <!--control_node-->
+  <node pkg="controller_manager" exec="ros2_control_node" output="both">
+    <param from="$(find-pkg-share $(var runtime_config_package))/config/$(var controllers_file).yaml"/>
+  </node>
+
+  <!--rviz_node-->
+  <node pkg="rviz2" exec="rviz2" output="log" args="-d $(find-pkg-share $(var description_package))/rviz/view_urdf.rviz"/>
+
+  <!--joint_state_braodcaster_spawner-->
+  <node pkg="controller_manager" exec="spawner" args="joint_state_broadcaster"/>
+
+  <!-- Robotiq Activation Controller spawner -->
+  <node pkg="controller_manager" exec="spawner" args="robotiq_activation_controller"/>
+
+  <!-- Robotiq Gripper Controller spawner -->
+  <node pkg="controller_manager" exec="spawner" args="robotiq_gripper_controller"/>
+
+</launch>

robotiq_description/launch/view_hand-e.launch.xml

@@ -0,0 +1,14 @@
+<launch>
+  <let name="robot_description_content" value="$(command '$(find-exec xacro) $(find-pkg-share robotiq_description)/urdf/robotiq_hand_e_gripper.urdf.xacro')"/>
+
+  <!--robot_state_pub_node-->
+  <node pkg="robot_state_publisher" exec="robot_state_publisher" output="screen">
+    <param name="robot_description" value="$(var robot_description_content)"/>
+  </node>
+
+  <node pkg="joint_state_publisher_gui" exec="joint_state_publisher_gui" output="screen" />
+
+  <!--rviz_node-->
+  <node pkg="rviz2" exec="rviz2" output="log" args="-d $(find-pkg-share robotiq_description)/rviz/view_urdf.rviz"/>
+
+</launch>

robotiq_description/package.xml

@@ -19,10 +19,11 @@
   <exec_depend>urdf</exec_depend>
   <exec_depend>xacro</exec_depend>
 
+  <exec_depend>controller_manager</exec_depend>
   <exec_depend>ros2_control</exec_depend>
   <exec_depend>ros2_controllers</exec_depend>
 
-  <exec_depend>gripper_controllers</exec_depend>
+  <exec_depend>parallel_gripper_controller</exec_depend>
   <exec_depend>joint_state_broadcaster</exec_depend>
 
   <test_depend>ament_lint_auto</test_depend>

robotiq_description/rviz/view_urdf.rviz

@@ -1,12 +1,13 @@
 Panels:
   - Class: rviz_common/Displays
-    Help Height: 78
+    Help Height: 83
     Name: Displays
     Property Tree Widget:
       Expanded:
+        - /Global Options1
         - /Status1
       Splitter Ratio: 0.6264705657958984
-    Tree Height: 555
+    Tree Height: 1067
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -64,79 +65,38 @@ Visualization Manager:
         Expand Link Details: false
         Expand Tree: false
         Link Tree Style: Links in Alphabetic Order
-        arm_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
         base_link:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-          Value: true
-        dummy_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        end_effector_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        forearm_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        gripper_base_link:
+        hand_e_base_link:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        left_finger_dist_link:
+        hande_finger_distance_link:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-          Value: true
-        left_finger_prox_link:
+        hande_left_finger:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        lower_wrist_link:
+        hande_right_finger:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        right_finger_dist_link:
+        tcp_link:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-          Value: true
-        right_finger_prox_link:
+        tcp_link_rack:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-          Value: true
-        shoulder_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        table:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        tool_frame:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        upper_wrist_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        world:
+        tcp_link_tube:
           Alpha: 1
           Show Axes: false
           Show Trail: false
@@ -151,7 +111,7 @@ Visualization Manager:
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
-    Fixed Frame: world
+    Fixed Frame: base_link
     Frame Rate: 30
   Name: root
   Tools:
@@ -194,33 +154,33 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 2.1567115783691406
+      Distance: 0.3602047562599182
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: -0.09681572020053864
-        Y: -0.10843408107757568
-        Z: 0.1451336145401001
+        X: -0.008749352768063545
+        Y: 0.0027605914510786533
+        Z: 0.08944482356309891
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.785398006439209
+      Pitch: 1.5697963237762451
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 0.785398006439209
+      Yaw: 1.480397343635559
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 846
+  Height: 1412
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd000000040000000000000156000002b4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002b4000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000024400fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd000000040000000000000166000004cbfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004a000004cb000000f300fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000004cbfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000004a000004cb000000d300fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006b700000049fc0100000002fb0000000800540069006d00650100000000000006b70000029800fffffffb0000000800540069006d0065010000000000000450000000000000000000000440000004cb00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -229,6 +189,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: false
-  Width: 1200
-  X: 1989
-  Y: 261
+  Width: 1719
+  X: 0
+  Y: 0

robotiq_description/urdf/2f_140.ros2_control.xacro

@@ -30,10 +30,15 @@
                 </xacro:if>
                 <xacro:unless value="${use_fake_hardware or sim_gazebo or sim_isaac}">
                     <plugin>robotiq_driver/RobotiqGripperHardwareInterface</plugin>
-                    <param name="gripper_closed_position">0.695</param>
+                    <!-- default data from docs - https://blog.robotiq.com/hubfs/support-files/Product-sheet-2F-Adaptive-Gripper-EN-Tomahawk.pdf -->
                     <param name="COM_port">${com_port}</param>
-                    <param name="gripper_speed_multiplier">1.0</param>
-                    <param name="gripper_force_multiplier">0.5</param>
+                    <param name="gripper_protocol_min_pos_value">3</param> <!-- Docs 0x00 -->
+                    <param name="gripper_protocol_max_pos_value">230</param> <!-- Docs 0xFF -->
+                    <param name="gripper_closed_position">0.695</param>  <!-- Docs 0.140 -->
+                    <param name="max_speed">0.150</param>  <!-- m/s -->
+                    <param name="speed_multiplier">1.0</param>
+                    <param name="max_force">125.0</param>  <!-- N -->
+                    <param name="force_multiplier">0.5</param>
                 </xacro:unless>
             </hardware>
 
@@ -44,8 +49,18 @@
                 <state_interface name="position">
                     <param name="initial_value">0.695</param>
                 </state_interface>
-                <state_interface name="velocity"/>
+                <state_interface name="velocity">
+                    <param name="initial_value">0.0</param>
+                </state_interface>
             </joint>
+            <command_interface name="set_gripper_max_velocity">
+                <param name="min">0.030</param>
+                <param name="max">0.250</param>
+            </command_interface>
+            <command_interface name="set_gripper_max_effort">
+                <param name="min">10</param>
+                <param name="max">125</param>
+            </command_interface>
             <!-- When simulating we need to include the rest of the gripper joints -->
             <xacro:if value="${use_fake_hardware or sim_isaac or sim_gazebo}">
                 <joint name="${prefix}left_inner_knuckle_joint">

robotiq_description/urdf/2f_85.ros2_control.xacro

@@ -35,10 +35,15 @@
                 </xacro:if>
                 <xacro:unless value="${use_fake_hardware or sim_gazebo or sim_isaac}">
                     <plugin>robotiq_driver/RobotiqGripperHardwareInterface</plugin>
-                    <param name="gripper_closed_position">0.7929</param>
+                    <!-- default data from docs - https://blog.robotiq.com/hubfs/support-files/Product-sheet-2F-Adaptive-Gripper-EN-Tomahawk.pdf -->
                     <param name="COM_port">${com_port}</param>
-                    <param name="gripper_speed_multiplier">1.0</param>
-                    <param name="gripper_force_multiplier">0.5</param>
+                    <param name="gripper_protocol_min_pos_value">0</param>
+                    <param name="gripper_protocol_max_pos_value">255</param>
+                    <param name="gripper_closed_position">0.085</param>
+                    <param name="max_speed">0.150</param>  <!-- m/s -->
+                    <param name="speed_multiplier">1.0</param>
+                    <param name="max_force">235.0</param>  <!-- N -->
+                    <param name="force_multiplier">0.5</param>
                 </xacro:unless>
             </hardware>
 
@@ -49,8 +54,18 @@
                 <state_interface name="position">
                     <param name="initial_value">0.7929</param>
                 </state_interface>
-                <state_interface name="velocity"/>
+                <state_interface name="velocity">
+                    <param name="initial_value">0.0</param>
+                </state_interface>
             </joint>
+            <command_interface name="set_gripper_max_velocity">
+                <param name="min">0.020</param>
+                <param name="max">0.150</param>
+            </command_interface>
+            <command_interface name="set_gripper_max_effort">
+                <param name="min">20</param>
+                <param name="max">235</param>
+            </command_interface>
             <!-- When simulating we need to include the rest of the gripper joints -->
             <xacro:if value="${use_fake_hardware or sim_isaac or sim_gazebo}">
                 <joint name="${prefix}robotiq_85_right_knuckle_joint">