Adding support for R-2000iC/210L

fanuc_r2000ic_support/CMakeLists.txt

@@ -11,6 +11,7 @@ if (CATKIN_ENABLE_TESTING)
   roslaunch_add_file_check(tests/roslaunch_test_r2000ic125l.xml)
   roslaunch_add_file_check(tests/roslaunch_test_r2000ic165f.xml)
   roslaunch_add_file_check(tests/roslaunch_test_r2000ic210f.xml)
+  roslaunch_add_file_check(tests/roslaunch_test_r2000ic210l.xml)
   roslaunch_add_file_check(tests/roslaunch_test_r2000ic270f.xml)
 endif()
 

fanuc_r2000ic_support/config/joint_names_r2000ic210l.yaml

@@ -0,0 +1 @@
+controller_joint_names: ['joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6']

fanuc_r2000ic_support/launch/load_r2000ic210l.launch

@@ -0,0 +1,4 @@
+<?xml version="1.0"?>
+<launch>
+  <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find fanuc_r2000ic_support)/urdf/r2000ic210l.xacro'" />
+</launch>

fanuc_r2000ic_support/launch/robot_interface_streaming_r2000ic210l.launch

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<!--
+  Manipulator specific version of 'robot_interface_streaming.launch'.
+
+  Defaults provided for R-2000iC/210l:
+   - J23_factor = 1
+   - use_bswap = true
+   - 6 joints
+
+  Usage:
+    robot_interface_streaming_r2000ic210l.launch robot_ip:=<value>
+-->
+<launch>
+  <arg name="robot_ip" doc="IP of controller" />
+  <arg name="J23_factor" default="1" doc="Compensation factor for joint 2-3 coupling (-1, 0 or 1)" />
+  <arg name="use_bswap" default="true" doc="If true, robot driver will byte-swap all incoming and outgoing data" />
+
+  <rosparam command="load" file="$(find fanuc_r2000ic_support)/config/joint_names_r2000ic210l.yaml" />
+
+  <include file="$(find fanuc_driver)/launch/robot_interface_streaming.launch">
+    <arg name="robot_ip"   value="$(arg robot_ip)" />
+    <arg name="J23_factor" value="$(arg J23_factor)" />
+    <arg name="use_bswap"  value="$(arg use_bswap)" />
+  </include>
+</launch>

fanuc_r2000ic_support/launch/robot_state_visualize_r2000ic210l.launch

@@ -0,0 +1,32 @@
+<?xml version="1.0"?>
+<!--
+  Manipulator specific version of the state visualizer.
+
+  Defaults provided for R-2000iC/210l:
+   - J23_factor = 1
+   - use_bswap = true
+   - 6 joints
+
+  Usage:
+    robot_state_visualize_r2000ic210l.launch robot_ip:=<value>
+-->
+<launch>
+  <arg name="robot_ip" doc="IP of controller" />
+  <arg name="J23_factor" default="1" doc="Compensation factor for joint 2-3 coupling (-1, 0 or 1)" />
+  <arg name="use_bswap" default="true" doc="If true, robot driver will byte-swap all incoming and outgoing data" />
+
+  <rosparam command="load" file="$(find fanuc_r2000ic_support)/config/joint_names_r2000ic210l.yaml" />
+
+  <include file="$(find fanuc_driver)/launch/robot_state.launch">
+    <arg name="robot_ip"   value="$(arg robot_ip)" />
+    <arg name="J23_factor" value="$(arg J23_factor)" />
+    <arg name="use_bswap"  value="$(arg use_bswap)" />
+  </include>
+
+  <node name="robot_state_publisher" pkg="robot_state_publisher" 
+    type="robot_state_publisher" />
+
+  <include file="$(find fanuc_r2000ic_support)/launch/load_r2000ic210l.launch" />
+
+  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find industrial_robot_client)/config/robot_state_visualize.rviz" required="true" />
+</launch>

fanuc_r2000ic_support/launch/test_r2000ic210l.launch

@@ -0,0 +1,7 @@
+<?xml version="1.0"?>
+<launch>
+  <include file="$(find fanuc_r2000ic_support)/launch/load_r2000ic210l.launch" />
+  <node name="joint_state_publisher_gui" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" />
+  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
+  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find industrial_robot_client)/config/robot_state_visualize.rviz" required="true" />
+</launch>

fanuc_r2000ic_support/package.xml

@@ -9,8 +9,8 @@
     </p>
     <p>
       This package contains configuration data, 3D models and launch files
-      for Fanuc R-2000iC manipulators. This includes the /125L, /165F, /210F
-      and /270F variants.
+      for Fanuc R-2000iC manipulators. This includes the /125L, /165F, /210F,
+      /210L and /270F variants.
     </p>
     <p>
       <b>Note</b>: for use with current ROS packages, the /210F is identical
@@ -22,6 +22,7 @@
       <li>R-2000iC/125L - "Conventional dress-out"</li>
       <li>R-2000iC/165F - "Conventional dress-out"</li>
       <li>R-2000iC/210F - "Conventional dress-out"</li>
+      <li>R-2000iC/210L - "Conventional dress-out"</li>
       <li>R-2000iC/270F - "Conventional dress-out"</li>
     </ul>
     <p>
@@ -39,8 +40,8 @@
     <p><b>Contributors</b>:</p>
     <p>
       This support package has received contributions from: Timo Birnkraut and
-      Simon Schmeisser (125L), Didier Quirin (210F) and Haris Suwignyo (Alten
-      Nederland) (270F).
+      Simon Schmeisser (125L), Didier Quirin (210F), Haris Suwignyo (Alten
+      Nederland) (270F) and Ademola Oridate (Wilder Systems) (210L).
     </p>
   </description>
   <author>G.A. vd. Hoorn (TU Delft Robotics Institute)</author>

fanuc_r2000ic_support/tests/roslaunch_test_r2000ic210l.xml

@@ -0,0 +1,40 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="ip_str" value="127.0.0.1" />
+
+  <group ns="load_r2000ic210l__">
+    <include file="$(find fanuc_r2000ic_support)/launch/load_r2000ic210l.launch"/>
+  </group>
+
+  <group ns="test_r2000ic210l__">
+    <include file="$(find fanuc_r2000ic_support)/launch/test_r2000ic210l.launch"/>
+  </group>
+
+  <group ns="robot_interface_streaming_r2000ic210l__">
+    <include file="$(find fanuc_r2000ic_support)/launch/robot_interface_streaming_r2000ic210l.launch">
+      <arg name="robot_ip" value="$(arg ip_str)" />
+    </include>
+  </group>
+
+  <group ns="robot_state_visualize_r2000ic210l__">
+    <include file="$(find fanuc_r2000ic_support)/launch/robot_state_visualize_r2000ic210l.launch">
+      <arg name="robot_ip" value="$(arg ip_str)" />
+    </include>
+  </group>
+
+
+  <!-- without bswap -->
+  <group ns="robot_interface_streaming_r2000ic210l_f__">
+    <include file="$(find fanuc_r2000ic_support)/launch/robot_interface_streaming_r2000ic210l.launch">
+      <arg name="robot_ip" value="$(arg ip_str)" />
+      <arg name="use_bswap" value="false" />
+    </include>
+  </group>
+
+  <group ns="robot_state_visualize_r2000ic210l_f__">
+    <include file="$(find fanuc_r2000ic_support)/launch/robot_state_visualize_r2000ic210l.launch">
+      <arg name="robot_ip" value="$(arg ip_str)" />
+      <arg name="use_bswap" value="false" />
+    </include>
+  </group>
+</launch>

fanuc_r2000ic_support/urdf/r2000ic210l.xacro

@@ -0,0 +1,5 @@
+<?xml version="1.0"?>
+<robot name="fanuc_r2000ic210l" xmlns:xacro="http://wiki.ros.org/xacro">
+  <xacro:include filename="$(find fanuc_r2000ic_support)/urdf/r2000ic210l_macro.xacro"/>
+  <xacro:fanuc_r2000ic210l prefix=""/>
+</robot>

fanuc_r2000ic_support/urdf/r2000ic210l_macro.xacro

@@ -0,0 +1,181 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://wiki.ros.org/xacro">
+  <xacro:macro name="fanuc_r2000ic210l" params="prefix">
+    <xacro:include filename="$(find fanuc_resources)/urdf/common_materials.xacro"/>
+
+    <!-- links: main serial chain -->
+    <link name="${prefix}base_link">
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <mesh filename="package://fanuc_r2000ic_support/meshes/r2000ic210l/visual/base_link.stl"/>
+        </geometry>
+        <xacro:material_fanuc_gray40 />
+      </visual>
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <mesh filename="package://fanuc_r2000ic_support/meshes/r2000ic210l/collision/base_link.stl"/>
+        </geometry>
+      </collision>
+    </link>
+    <link name="${prefix}link_1">
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <mesh filename="package://fanuc_r2000ic_support/meshes/r2000ic210l/visual/link_1.stl"/>
+        </geometry>
+        <xacro:material_fanuc_yellow />
+      </visual>
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <mesh filename="package://fanuc_r2000ic_support/meshes/r2000ic210l/collision/link_1.stl"/>
+        </geometry>
+      </collision>
+    </link>
+    <link name="${prefix}link_2">
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <mesh filename="package://fanuc_r2000ic_support/meshes/r2000ic210l/visual/link_2.stl"/>
+        </geometry>
+        <xacro:material_fanuc_yellow />
+      </visual>
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <mesh filename="package://fanuc_r2000ic_support/meshes/r2000ic210l/collision/link_2.stl"/>
+        </geometry>
+      </collision>
+    </link>
+    <link name="${prefix}link_3">
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <mesh filename="package://fanuc_r2000ic_support/meshes/r2000ic210l/visual/link_3.stl"/>
+        </geometry>
+        <xacro:material_fanuc_yellow />
+      </visual>
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <mesh filename="package://fanuc_r2000ic_support/meshes/r2000ic210l/collision/link_3.stl"/>
+        </geometry>
+      </collision>
+    </link>
+    <link name="${prefix}link_4">
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <mesh filename="package://fanuc_r2000ic_support/meshes/r2000ic210l/visual/link_4.stl"/>
+        </geometry>
+        <xacro:material_fanuc_yellow />
+      </visual>
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <mesh filename="package://fanuc_r2000ic_support/meshes/r2000ic210l/collision/link_4.stl"/>
+        </geometry>
+      </collision>
+    </link>
+    <link name="${prefix}link_5">
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <mesh filename="package://fanuc_r2000ic_support/meshes/r2000ic210l/visual/link_5.stl"/>
+        </geometry>
+        <xacro:material_fanuc_yellow />
+      </visual>
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <mesh filename="package://fanuc_r2000ic_support/meshes/r2000ic210l/collision/link_5.stl"/>
+        </geometry>
+      </collision>
+    </link>
+    <link name="${prefix}link_6">
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <mesh filename="package://fanuc_r2000ic_support/meshes/r2000ic210l/visual/link_6.stl"/>
+        </geometry>
+        <xacro:material_fanuc_black />
+      </visual>
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <mesh filename="package://fanuc_r2000ic_support/meshes/r2000ic210l/collision/link_6.stl"/>
+        </geometry>
+      </collision>
+    </link>
+
+    <!-- joints: main serial chain -->
+    <joint name="${prefix}joint_1" type="revolute">
+      <origin xyz="0 0 0.67" rpy="0 0 0"/>
+      <parent link="${prefix}base_link"/>
+      <child link="${prefix}link_1"/>
+      <axis xyz="0 0 1"/>
+      <limit lower="${radians(-185)}" upper="${radians(185)}" effort="0" velocity="${radians(105)}"/>
+    </joint>
+    <joint name="${prefix}joint_2" type="revolute">
+      <origin xyz="0.312 0 0" rpy="0 0 0"/>
+      <parent link="${prefix}link_1"/>
+      <child link="${prefix}link_2"/>
+      <axis xyz="0 1 0"/>
+      <limit lower="${radians(-60)}" upper="${radians(76)}" effort="0" velocity="${radians(90)}"/>
+    </joint>
+    <joint name="${prefix}joint_3" type="revolute">
+      <origin xyz="0 0 1.075" rpy="0 0 0"/>
+      <parent link="${prefix}link_2"/>
+      <child link="${prefix}link_3"/>
+      <axis xyz="0 -1 0"/>
+      <limit lower="${radians(-121)}" upper="${radians(180)}" effort="0" velocity="${radians(85)}"/>
+    </joint>
+    <joint name="${prefix}joint_4" type="revolute">
+      <origin xyz="0 0 0.225" rpy="0 0 0"/>
+      <parent link="${prefix}link_3"/>
+      <child link="${prefix}link_4"/>
+      <axis xyz="-1 0 0"/>
+      <limit lower="${radians(-360)}" upper="${radians(360)}" effort="0" velocity="${radians(120)}"/>
+    </joint>
+    <joint name="${prefix}joint_5" type="revolute">
+      <origin xyz="1.730 0 0" rpy="0 0 0"/>
+      <parent link="${prefix}link_4"/>
+      <child link="${prefix}link_5"/>
+      <axis xyz="0 -1 0"/>
+      <limit lower="${radians(-125)}" upper="${radians(125)}" effort="0" velocity="${radians(120)}"/>
+    </joint>
+    <joint name="${prefix}joint_6" type="revolute">
+      <origin xyz="0.24 0 0" rpy="0 0 0"/>
+      <parent link="${prefix}link_5"/>
+      <child link="${prefix}link_6"/>
+      <axis xyz="-1 0 0"/>
+      <limit lower="${radians(-360)}" upper="${radians(360)}" effort="0" velocity="${radians(200)}"/>
+    </joint>
+
+    <!-- ROS-Industrial 'base' frame: base_link to Fanuc World Coordinates transform -->
+    <link name="${prefix}base" />
+    <joint name="${prefix}base_link-base" type="fixed">
+      <origin xyz="0 0 0.670" rpy="0 0 0"/>
+      <parent link="${prefix}base_link"/>
+      <child link="${prefix}base"/>
+    </joint>
+
+    <!-- ROS-Industrial 'flange' frame: attachment point for EEF models -->
+    <link name="${prefix}flange" />
+    <joint name="${prefix}joint_6-flange" type="fixed">
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <parent link="${prefix}link_6" />
+      <child link="${prefix}flange" />
+    </joint>
+
+    <!-- ROS-Industrial 'tool0' frame: all-zeros tool frame -->
+    <link name="${prefix}tool0" />
+    <joint name="${prefix}link_6-tool0" type="fixed">
+      <origin xyz="0 0 0" rpy="${pi} ${-pi/2.0} 0" />
+      <parent link="${prefix}link_6" />
+      <child link="${prefix}tool0" />
+    </joint>
+  </xacro:macro>
+</robot>