[WIP] Gripper

ar4_description/config/link_properties.yaml

@@ -246,3 +246,65 @@ link_6:
       dr: 0.0
       dp: 0.0
       dY: 0.0
+
+gripper_base:
+  inertial:
+    size:
+      dx: 0.035
+      dy: 0.02
+      dz: 0.02
+    mass: 0.08
+    origin:
+      dx: 0.0
+      dy: -0.025
+      dz: 0.0
+      dr: 0.0
+      dp: 0.0
+      dY: 0.0
+  visual:
+    origin:
+      dx: 0.0
+      dy: 0.0
+      dz: 0.0
+      dr: 0.0
+      dp: 0.0
+      dY: 0.0
+  collision:
+    origin:
+      dx: 0.0
+      dy: 0.0
+      dz: 0.0
+      dr: 0.0
+      dp: 0.0
+      dY: 0.0
+
+finger:
+  inertial:
+    size:
+      dx: 0.007
+      dy: 0.015
+      dz: 0.02
+    mass: 0.01
+    origin:
+      dx: -0.005
+      dy: -0.015
+      dz: 0.0
+      dr: 0.0
+      dp: 0.0
+      dY: 0.0
+  visual:
+    origin:
+      dx: 0.0
+      dy: 0.0
+      dz: 0.0
+      dr: 0.0
+      dp: 0.0
+      dY: 0.0
+  collision:
+    origin:
+      dx: 0.0
+      dy: 0.0
+      dz: 0.0
+      dr: 0.0
+      dp: 0.0
+      dY: 0.0

ar4_description/urdf/ar4.urdf.xacro

@@ -12,16 +12,18 @@
               inertial="${robot_properties['base_link']['inertial']}"
               visual="${robot_properties['base_link']['visual']}"
               collision="${robot_properties['base_link']['collision']}"
-              meshes="${['base_link', 'base_link_bottom', 'base_link_motor']}"
-              materials="${['grey', 'blue', 'black']}">
+              visual_meshes="${['base_link', 'base_link_bottom', 'base_link_motor']}"
+              materials="${['grey', 'blue', 'black']}"
+              collision_meshes="${['base_link_collision']}">
               <origin xyz="0 0 0" rpy="0 0 0" />
   </xacro:link>
   <xacro:link link_name="link_1"
               inertial="${robot_properties['link_1']['inertial']}"
               visual="${robot_properties['link_1']['visual']}"
               collision="${robot_properties['link_1']['collision']}"
-              meshes="${['link_1', 'link_1_motor']}"
-              materials="${['grey', 'black']}">
+              visual_meshes="${['link_1', 'link_1_motor']}"
+              materials="${['grey', 'black']}"
+              collision_meshes="${['link_1_collision']}">
     <origin xyz="0 0 0" rpy="0 0 0" />
   </xacro:link>
   <joint name="joint_1" type="revolute">
@@ -36,8 +38,9 @@
               inertial="${robot_properties['link_2']['inertial']}"
               visual="${robot_properties['link_2']['visual']}"
               collision="${robot_properties['link_2']['collision']}"
-              meshes="${['link_2', 'link_2_motor', 'link_2_cover', 'link_2_cover_logo']}"
-              materials="${['grey', 'black', 'white', 'blue']}">
+              visual_meshes="${['link_2', 'link_2_motor', 'link_2_cover', 'link_2_cover_logo']}"
+              materials="${['grey', 'black', 'white', 'blue']}"
+              collision_meshes="${['link_2_collision']}">
     <origin xyz="0 0 0" rpy="0 0 0" />
   </xacro:link>
   <joint name="joint_2" type="revolute">
@@ -52,8 +55,9 @@
               inertial="${robot_properties['link_3']['inertial']}"
               visual="${robot_properties['link_3']['visual']}"
               collision="${robot_properties['link_3']['collision']}"
-              meshes="${['link_3', 'link_3_motor', 'link_3_misc']}"
-              materials="${['grey', 'black', 'black']}">
+              visual_meshes="${['link_3', 'link_3_motor', 'link_3_misc']}"
+              materials="${['grey', 'black', 'black']}"
+              collision_meshes="${['link_3_collision']}">
     <origin xyz="0 0 0" rpy="0 0 0" />
   </xacro:link>
   <joint name="joint_3" type="revolute">
@@ -68,8 +72,9 @@
               inertial="${robot_properties['link_4']['inertial']}"
               visual="${robot_properties['link_4']['visual']}"
               collision="${robot_properties['link_4']['collision']}"
-              meshes="${['link_4', 'link_4_motor']}"
-              materials="${['grey', 'black']}">
+              visual_meshes="${['link_4', 'link_4_motor']}"
+              materials="${['grey', 'black']}"
+              collision_meshes="${['link_4_collision']}">
     <origin xyz="0 0 0" rpy="0 0 0" />
   </xacro:link>
   <joint name="joint_4" type="revolute">
@@ -84,8 +89,9 @@
               inertial="${robot_properties['link_5']['inertial']}"
               visual="${robot_properties['link_5']['visual']}"
               collision="${robot_properties['link_5']['collision']}"
-              meshes="${['link_5', 'link_5_motor']}"
-              materials="${['grey', 'black']}">
+              visual_meshes="${['link_5', 'link_5_motor']}"
+              materials="${['grey', 'black']}"
+              collision_meshes="${['link_5_collision']}">
     <origin xyz="0 0 0" rpy="0 0 0" />
   </xacro:link>
   <joint name="joint_5" type="revolute">
@@ -100,8 +106,9 @@
               inertial="${robot_properties['link_6']['inertial']}"
               visual="${robot_properties['link_6']['visual']}"
               collision="${robot_properties['link_6']['collision']}"
-              meshes="${['link_6']}"
-              materials="${['grey']}">
+              visual_meshes="${['link_6']}"
+              materials="${['grey']}"
+              collision_meshes="${['link_6_collision']}">
     <origin xyz="0 0 0" rpy="0 0 0" />
   </xacro:link>
   <joint name="joint_6" type="revolute">
@@ -111,4 +118,40 @@
     <axis xyz="0 0 1"/>
     <limit lower="${-1.0 * robot_parameters['j6_limit']}" upper="${robot_parameters['j6_limit']}" effort="100" velocity="1.0"/>
   </joint>
+
+  <!-- Dummy joint to remove the  "Joint '*_mimic' not found in model 'ar4'" error-->
+  <!-- See https://github.com/ros-controls/gazebo_ros2_control/issues/173 -->
+
+  <joint name="finger_joint_2_mimic" type="fixed">
+    <parent link="base_link" />
+    <child link="dummy" />
+    <origin xyz="0 0 0" rpy="0 0 0" />
+  </joint>
+  <link name="dummy"/>
+
+  <xacro:gripper_base parent="link_6"
+                      inertial="${robot_properties['gripper_base']['inertial']}"
+                      visual="${robot_properties['gripper_base']['visual']}"
+                      collision="${robot_properties['gripper_base']['collision']}">
+    <origin xyz="0 0 0" rpy="0 0 0" />
+  </xacro:gripper_base>
+
+  <xacro:finger number="1"
+                pitch_rotation="0.0"
+                mimic="false"
+                inertial="${robot_properties['finger']['inertial']}"
+                visual="${robot_properties['finger']['visual']}"
+                collision="${robot_properties['finger']['collision']}">
+    <origin xyz="0 0 0" rpy="0 0 0" />
+  </xacro:finger>
+
+  <xacro:finger number="2"
+                pitch_rotation="${-pi}"
+                mimic="true"
+                inertial="${robot_properties['finger']['inertial']}"
+                visual="${robot_properties['finger']['visual']}"
+                collision="${robot_properties['finger']['collision']}">
+    <origin xyz="0 0 0" rpy="0 0 0" />
+  </xacro:finger>
+
 </robot>

ar4_description/urdf/include/common_macros.urdf.xacro

@@ -3,7 +3,7 @@
 <!-- ===================== Arm link xacro ================================== -->
 
 <xacro:include filename="$(find ${package_name})/urdf/include/materials.xacro" />
-<xacro:macro name="link" params="link_name *origin inertial visual collision meshes:=^ materials:=^">
+<xacro:macro name="link" params="link_name *origin inertial visual collision visual_meshes:=^ materials:=^ collision_meshes:=^">
   <link name="${link_name}">
     <xacro:insert_block name="origin"/>
 
@@ -15,12 +15,13 @@
                        o_rpy="${inertial['origin']['dr']} ${inertial['origin']['dp']} ${inertial['origin']['dY']}">
     </xacro:box_inertia>
 
-    <xacro:mesh_loop meshes="${meshes}" materials="${materials}" visual="${visual}" collision="${collision}" />
+    <xacro:mesh_loop_visuals meshes="${visual_meshes}" materials="${materials}" visual="${visual}" />
+    <xacro:mesh_loop_collisions meshes="${collision_meshes}" collision="${collision}" />
   </link>
 </xacro:macro>
 
-<!-- Recursive macro to iterate over meshes -->
-<xacro:macro name="mesh_loop" params="meshes:=^ materials:=^ visual collision">
+<!-- Recursive macro to iterate over visual meshes -->
+<xacro:macro name="mesh_loop_visuals" params="meshes:=^ materials:=^ visual">
   <xacro:if value="${meshes}">
     <xacro:property name="mesh" value="${meshes.pop(0)}"/>
     <xacro:property name="material" value="${materials.pop(0)}"/>
@@ -34,6 +35,16 @@
       <material name="${material}"/>
     </visual>
 
+    <!-- Recursive call to process the remaining list -->
+    <xacro:mesh_loop_visuals meshes="${meshes}" materials="${materials}" visual="${visual}" />
+  </xacro:if>
+</xacro:macro>
+
+<!-- Recursive macro to iterate over collision meshes -->
+<xacro:macro name="mesh_loop_collisions" params="meshes:=^ collision">
+  <xacro:if value="${meshes}">
+    <xacro:property name="mesh" value="${meshes.pop(0)}"/>
+
     <collision>
       <origin rpy="${collision['origin']['dr']} ${collision['origin']['dp']} ${collision['origin']['dY']}"
               xyz="${collision['origin']['dx']} ${collision['origin']['dy']} ${collision['origin']['dz']}"/>
@@ -43,10 +54,67 @@
     </collision>
 
     <!-- Recursive call to process the remaining list -->
-    <xacro:mesh_loop meshes="${meshes}" materials="${materials}" visual="${visual}" collision="${collision}" />
+    <xacro:mesh_loop_collisions meshes="${meshes}" collision="${collision}" />
   </xacro:if>
 </xacro:macro>
 
+<!-- ===================== Gripper base xacro ================================== -->
+  <xacro:macro name="gripper_base" params="parent *origin inertial visual collision">
+
+    <joint name="gripper_base_joint" type="fixed">
+      <origin xyz="0.0 0.0 0.0" rpy="${-pi/2} 0.0 0"/>
+      <parent link="${parent}"/>
+      <child link="gripper_base_link"/>
+    </joint>
+
+    <link name="gripper_base_link">
+      <xacro:insert_block name="origin"/>
+
+      <xacro:box_inertia m="${inertial['mass']}"
+                         x="${inertial['size']['dx']}"
+                         y="${inertial['size']['dy']}"
+                         z="${inertial['size']['dz']}"
+                         o_xyz="${inertial['origin']['dx']} ${inertial['origin']['dy']} ${inertial['origin']['dz']}"
+                         o_rpy="${inertial['origin']['dr']} ${inertial['origin']['dp']} ${inertial['origin']['dY']}">
+      </xacro:box_inertia>
+
+      <xacro:mesh_loop_visuals meshes="${['gripper_base']}" materials="${['grey']}" visual="${visual}" />
+      <xacro:mesh_loop_collisions meshes="${['gripper_base_collision']}" collision="${collision}" />
+
+    </link>
+
+  </xacro:macro>
+
+<!-- ===================== Gripper finger ================================== -->
+  <xacro:macro name="finger" params="number pitch_rotation mimic *origin inertial visual collision">
+
+    <joint name="finger_joint_${number}" type="prismatic">
+      <origin xyz="0.0 -0.036 0" rpy="0.0 ${pitch_rotation} 0.0"/>
+      <parent link="gripper_base_link"/>
+      <child link="finger_link_${number}"/>
+      <axis xyz="1 0 0"/>
+      <limit lower="-0.014" upper="0" effort="100" velocity="1.0"/>
+      <xacro:if value="${mimic}">
+        <mimic joint="finger_joint_1" multiplier="1" offset="0"/>
+      </xacro:if>
+    </joint>
+
+    <link name="finger_link_${number}">
+      <xacro:insert_block name="origin"/>
+
+      <xacro:box_inertia m="${inertial['mass']}"
+                         x="${inertial['size']['dx']}"
+                         y="${inertial['size']['dy']}"
+                         z="${inertial['size']['dz']}"
+                         o_xyz="${inertial['origin']['dx']} ${inertial['origin']['dy']} ${inertial['origin']['dz']}"
+                         o_rpy="${inertial['origin']['dr']} ${inertial['origin']['dp']} ${inertial['origin']['dY']}">
+      </xacro:box_inertia>
+
+      <xacro:mesh_loop_visuals meshes="${['gripper_finger']}" materials="${['black']}" visual="${visual}" />
+      <xacro:mesh_loop_collisions meshes="${['gripper_finger_collision']}" collision="${collision}" />
+    </link>
+
+  </xacro:macro>
 
 
 <!-- ===================== Box inertia xacro ================================== -->

ar4_gazebo_bringup/README.md

@@ -15,7 +15,8 @@ ros2 launch ar4_gazebo_bringup main.launch.py
 
 #### Launch file arguments
 
-- 'rsp':
-    - Run [`robot state publisher`](https://github.com/ros/robot_state_publisher) node. (default: 'false')
-- 'rviz':
-    - Start RViz. (default: 'false')
+To see a detailed list of the arguments, run:
+
+```bash
+ros2 launch ar4_gazebo_bringup main.launch.py --show-args
+```

ar4_gazebo_sim/launch/main.launch.py

@@ -121,7 +121,7 @@ def generate_launch_description():
     spawn_joint_trajectory_controller = Node(
         package="controller_manager",
         executable="spawner",
-        arguments=["arm_controller"],
+        arguments=["arm_controller", "gripper_controller"],
         output="screen",
     )
 

ar4_gazebo_sim/urdf/gz_ar4.urdf.xacro

@@ -10,4 +10,7 @@
   <!-- ros2_control extensions -->
   <xacro:include filename="$(find ar4_gazebo_sim)/urdf/includes/ros2_control.xacro" />
 
+  <!-- ros2_control gripper extensions -->
+  <xacro:include filename="$(find ar4_gazebo_sim)/urdf/includes/gripper.ros2_control.xacro" />
+
 </robot>

ar4_gazebo_sim/urdf/includes/gazebo.xacro

@@ -22,6 +22,12 @@
     <gazebo reference="link_6">
         <selfCollide>true</selfCollide>
     </gazebo>
+    <gazebo reference="finger_link_1">
+        <selfCollide>true</selfCollide>
+    </gazebo>
+    <gazebo reference="finger_link_2">
+        <selfCollide>true</selfCollide>
+    </gazebo>
 
     <gazebo>
         <plugin filename="libign_ros2_control-system.so"

ar4_gazebo_sim/urdf/includes/gripper.ros2_control.xacro

@@ -0,0 +1,32 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+    <ros2_control name="GripperGazeboSystem" type="system">
+
+      <hardware>
+        <plugin>ign_ros2_control/IgnitionSystem</plugin>
+      </hardware>
+
+      <joint name="finger_joint_1">
+        <command_interface name="position" />
+        <state_interface name="position">
+          <param name="initial_value">0.0</param>
+        </state_interface>
+        <state_interface name="velocity">
+          <param name="initial_value">0.0</param>
+        </state_interface>
+      </joint>
+      <joint name="finger_joint_2">
+        <param name="mimic">finger_joint_1</param>
+        <param name="multiplier">1</param>
+        <state_interface name="position">
+          <param name="initial_value">0.0</param>
+        </state_interface>
+        <state_interface name="velocity">
+          <param name="initial_value">0.0</param>
+        </state_interface>
+      </joint>
+
+    </ros2_control>
+
+</robot>