Add initial support for Fanuc M-20iD/25

fanuc_m20id_support/CMakeLists.txt

@@ -0,0 +1,17 @@
+cmake_minimum_required(VERSION 3.0.2)
+
+project(fanuc_m20id_support)
+
+find_package(catkin REQUIRED)
+
+catkin_package()
+
+if (CATKIN_ENABLE_TESTING)
+  find_package(roslaunch REQUIRED)
+  roslaunch_add_file_check(tests/roslaunch_test_m20id25.xml)
+endif()
+
+install(DIRECTORY config launch meshes urdf
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+
+install(FILES readme.md DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})

fanuc_m20id_support/config/joint_names_m20id25.yaml

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

fanuc_m20id_support/config/opw_parameters_m20id25.yaml

@@ -0,0 +1,21 @@
+#
+# Parameters for use with IK solvers which support OPW (Ortho-Parallel Wrist)
+# kinematic configurations, as described in the paper "An Analytical Solution
+# of the Inverse Kinematics Problem of Industrial Serial Manipulators with an
+# Ortho-parallel Basis and a Spherical Wrist" by Mathias Brandstötter, Arthur
+# Angerer, and Michael Hofbaur (Proceedings of the Austrian Robotics Workshop
+# 2014, 22-23 May, 2014, Linz, Austria).
+#
+# The moveit_opw_kinematics_plugin package provides such a solver.
+#
+opw_kinematics_geometric_parameters:
+    a1:  0.075
+    a2:  -0.215
+    b:   0.0
+    c1:  0.425
+    c2:  0.84
+    c3:  0.89
+    c4:  0.09
+
+opw_kinematics_joint_offsets: [0.0, 0.0, deg(-90.0), 0.0, 0.0, 0.0]
+opw_kinematics_joint_sign_corrections: [1, 1, -1, -1, -1, -1]

fanuc_m20id_support/launch/load_m20id25.launch

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

fanuc_m20id_support/launch/robot_interface_streaming_m20id25.launch

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<!--
+  Manipulator specific version of 'robot_interface_streaming.launch'.
+
+  Defaults provided for M-20iD/25:
+   - J23_factor = 1
+   - use_bswap = true
+   - 6 joints
+
+  Usage:
+    robot_interface_streaming_m20id25.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_m20id_support)/config/joint_names_m20id25.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_m20id_support/launch/robot_state_visualize_m20id25.launch

@@ -0,0 +1,32 @@
+<?xml version="1.0"?>
+<!--
+  Manipulator specific version of the state visualizer.
+
+  Defaults provided for M-20iD/25:
+   - J23_factor = 1
+   - use_bswap = true
+   - 6 joints
+
+  Usage:
+    robot_state_visualize_m20id25.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_m20id_support)/config/joint_names_m20id25.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_m20id_support)/launch/load_m20id25.launch" />
+
+  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find industrial_robot_client)/config/robot_state_visualize.rviz" required="true" />
+</launch>

fanuc_m20id_support/launch/test_m20id25.launch

@@ -0,0 +1,7 @@
+<?xml version="1.0"?>
+<launch>
+  <include file="$(find fanuc_m20id_support)/launch/load_m20id25.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_m20id_support/package.xml

@@ -0,0 +1,67 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="2">
+  <name>fanuc_m20id_support</name>
+  <version>0.6.0</version>
+  <description>
+    <p>
+      ROS-Industrial support for the Fanuc M-20iD (and variants).
+    </p>
+    <p>
+      This package contains configuration data, 3D models and launch files
+      for Fanuc M-20iD manipulators. This currently includes the /25 model only.
+    </p>
+    <p><b>Specifications</b>:</p>
+    <ul>
+      <li>M-20iD/25 - "Cable integrated J3 Arm"</li>
+    </ul>
+    <p>
+      Joint limits and maximum joint velocities are based on the information in
+      the <em>FANUC Robot M-20iD Mechanical Unit Operator's Manual</em> version
+      <em>B-84074EN/08</em>. All urdfs are based on the default motion and
+      joint velocity limits, unless noted otherwise (ie: no support for high
+      speed joints, extended / limited motion ranges or other options).
+    </p>
+    <p>
+      Before using any of the configuration files and / or meshes included
+      in this package, be sure to check they are correct for the particular
+      robot model and configuration you intend to use them with.
+    </p>
+  </description>
+  <author>Timo Birnkraut</author>
+  <maintainer email="g.a.vanderhoorn@tudelft.nl">G.A. vd. Hoorn (TU Delft Robotics Institute)</maintainer>
+  <license>BSD</license>
+
+  <url type="website">http://wiki.ros.org/fanuc_m20id_support</url>
+  <url type="bugtracker">https://github.com/ros-industrial/fanuc/issues</url>
+  <url type="repository">https://github.com/ros-industrial/fanuc</url>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <test_depend>roslaunch</test_depend>
+
+  <exec_depend>fanuc_driver</exec_depend>
+  <exec_depend>fanuc_resources</exec_depend>
+  <exec_depend>industrial_robot_client</exec_depend>
+  <exec_depend>joint_state_publisher_gui</exec_depend>
+  <exec_depend>robot_state_publisher</exec_depend>
+  <exec_depend>rviz</exec_depend>
+  <exec_depend>xacro</exec_depend>
+
+  <export>
+    <architecture_independent />
+    <rosindex>
+      <tags>
+        <tag>support_package</tag>
+        <tag>description</tag>
+        <tag>fanuc</tag>
+        <tag>industrial</tag>
+        <tag>ros-industrial</tag>
+        <tag>m20id</tag>
+      </tags>
+    </rosindex>
+    <ros_industrial>
+      <support_level value="community" />
+    </ros_industrial>
+  </export>
+</package>

fanuc_m20id_support/readme.md

@@ -0,0 +1,14 @@
+# fanuc_m20id_support
+
+## Overview
+
+This package is part of the [ROS-Industrial][] program. See the main [fanuc][]
+page on the ROS wiki for more information on usage.
+
+## Contents
+
+See `package.xml` for information about the contents of this package.
+
+
+[ROS-Industrial]: http://wiki.ros.org/Industrial
+[fanuc]: http://wiki.ros.org/fanuc

fanuc_m20id_support/tests/roslaunch_test_m20id25.xml

@@ -0,0 +1,40 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="ip_str" value="127.0.0.1" />
+
+  <group ns="load_m20id25__">
+    <include file="$(find fanuc_m20id_support)/launch/load_m20id25.launch"/>
+  </group>
+
+  <group ns="test_m20id25__">
+    <include file="$(find fanuc_m20id_support)/launch/test_m20id25.launch"/>
+  </group>
+
+  <group ns="robot_interface_streaming_m20id25__">
+    <include file="$(find fanuc_m20id_support)/launch/robot_interface_streaming_m20id25.launch">
+      <arg name="robot_ip" value="$(arg ip_str)" />
+    </include>
+  </group>
+
+  <group ns="robot_state_visualize_m20id25__">
+    <include file="$(find fanuc_m20id_support)/launch/robot_state_visualize_m20id25.launch">
+      <arg name="robot_ip" value="$(arg ip_str)" />
+    </include>
+  </group>
+
+
+  <!-- without bswap -->
+  <group ns="robot_interface_streaming_m20id25_f__">
+    <include file="$(find fanuc_m20id_support)/launch/robot_interface_streaming_m20id25.launch">
+      <arg name="robot_ip" value="$(arg ip_str)" />
+      <arg name="use_bswap" value="false" />
+    </include>
+  </group>
+
+  <group ns="robot_state_visualize_m20id25_f__">
+    <include file="$(find fanuc_m20id_support)/launch/robot_state_visualize_m20id25.launch">
+      <arg name="robot_ip" value="$(arg ip_str)" />
+      <arg name="use_bswap" value="false" />
+    </include>
+  </group>
+</launch>

fanuc_m20id_support/urdf/m20id25.xacro

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