Add GP20HL robot support package (#604)

Squashed commits:

* Add GP20HL robot support package
* Correct a2 OPW parameter
* Clarify status of OPW parameters
* Update link_6_t.dae

---------

Co-authored-by: gavanderhoorn <[email protected]>

motoman_gp20hl_support/CMakeLists.txt

@@ -0,0 +1,15 @@
+cmake_minimum_required(VERSION 3.0.2)
+
+project(motoman_gp20hl_support)
+
+find_package(catkin REQUIRED)
+
+catkin_package()
+
+if (CATKIN_ENABLE_TESTING)
+  find_package(roslaunch REQUIRED)
+  roslaunch_add_file_check(test/roslaunch_test_gp20hl.xml)
+endif()
+
+install(DIRECTORY config launch meshes urdf
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})

motoman_gp20hl_support/config/joint_names_gp20hl.yaml

@@ -0,0 +1 @@
+controller_joint_names: ['joint_1_s', 'joint_2_l', 'joint_3_u', 'joint_4_r', 'joint_5_b', 'joint_6_t']

motoman_gp20hl_support/config/opw_parameters_gp20hl.yaml

@@ -0,0 +1,27 @@
+#
+# NOTE: the OPW kinematics parameters provided here have not been verified
+# against a real system. Before using these parameters, be sure to check
+# they are correct for the particular robot model and configuration you intend
+# to use them with.
+#
+
+#
+# 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.145
+    a2: -0.250
+    b:   0.000
+    c1:  0.540
+    c2:  1.150
+    c3:  1.812
+    c4:  0.100
+opw_kinematics_joint_offsets: [0.0, 0.0, deg(-90.0), 0.0, 0.0, deg(180.0)]
+opw_kinematics_joint_sign_corrections: [1, 1, -1, -1, -1, -1]

motoman_gp20hl_support/launch/load_gp20hl.launch

@@ -0,0 +1,3 @@
+<launch>
+  <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find motoman_gp20hl_support)/urdf/gp20hl.xacro'" />
+</launch>

motoman_gp20hl_support/launch/robot_interface_streaming_gp20hl.launch

@@ -0,0 +1,21 @@
+<!--
+  Manipulator specific version of 'robot_interface_streaming.launch'.
+
+  Defaults provided for GP35L:
+   - 6 joints
+
+  Usage:
+    robot_interface_streaming_gp20hl.launch robot_ip:=<value> controller:=<yrc1000>
+-->
+<launch>
+  <arg name="robot_ip" />
+
+  <!-- controller: Controller name (yrc1000) -->
+  <arg name="controller"/>
+
+  <rosparam command="load" file="$(find motoman_gp20hl_support)/config/joint_names_gp20hl.yaml" />
+
+  <include file="$(find motoman_driver)/launch/robot_interface_streaming_$(arg controller).launch">
+    <arg name="robot_ip" value="$(arg robot_ip)" />
+  </include>
+</launch>

motoman_gp20hl_support/launch/robot_state_visualize_gp20hl.launch

@@ -0,0 +1,28 @@
+<!--
+  Manipulator specific version of the state visualizer.
+
+  Defaults provided for GP20HL:
+   - 6 joints
+
+  Usage:
+    robot_state_visualize_gp20hl.launch robot_ip:=<value> controller:=<yrc1000>
+-->
+<launch>
+  <arg name="robot_ip" />
+
+  <!-- controller: Controller name (yrc1000) -->
+  <arg name="controller" />
+
+  <rosparam command="load" file="$(find motoman_gp20hl_support)/config/joint_names_gp20hl.yaml" />
+
+  <include file="$(find motoman_driver)/launch/robot_state_$(arg controller).launch">
+    <arg name="robot_ip" value="$(arg robot_ip)" />
+  </include>
+
+  <node name="robot_state_publisher" pkg="robot_state_publisher" 
+    type="robot_state_publisher" />
+
+  <include file="$(find motoman_gp20hl_support)/launch/load_gp20hl.launch" />
+
+  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find industrial_robot_client)/config/robot_state_visualize.rviz" required="true" />
+</launch>

motoman_gp20hl_support/launch/test_gp20hl.launch

@@ -0,0 +1,6 @@
+<launch>
+  <include file="$(find motoman_gp20hl_support)/launch/load_gp20hl.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>

motoman_gp20hl_support/package.xml

@@ -0,0 +1,62 @@
+<?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>motoman_gp20hl_support</name>
+  <version>0.1.0</version>
+  <description>
+    <p>ROS-Industrial support for the Motoman GP20HL or AR3120.</p>
+    <p>
+      This package contains configuration data, 3D models and launch files
+      for Motoman GP20HL manipulators.
+    </p>
+    <p>
+      <b>Specifications</b>
+    </p>
+    <ul>
+      <li>GP20HL - Default</li>
+   </ul>
+    <p>
+      Joint limits and maximum joint velocities are based on the information 
+      found in <em>YASKAWA MOTOMAN-GP20HL INSTRUCTIONS (HW1485073)</em>
+      version <em>187746-1CD, rev 1</em>.
+    </p>
+    <p>
+      All urdfs are based on the default motion and joint velocity limits, 
+      unless noted otherwise.
+    </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>
+    <p>
+      NOTE: the OPW kinematics parameters provided by this package have not
+      been verified against a real system. Before using these parameters, be
+      sure to check they are correct for the particular robot model and
+      configuration you intend to use them with.
+    </p>
+  </description>
+  <author>Eric Marcil</author>
+  <maintainer email="[email protected]">Eric Marcil</maintainer>
+  <maintainer email="[email protected]">G.A. vd. Hoorn (TU Delft Robotics Institute)</maintainer>
+  <license>BSD</license>
+
+  <url type="website">http://wiki.ros.org/motoman_gp20hl_support</url>
+  <url type="bugtracker">https://github.com/ros-industrial/motoman/issues</url>
+  <url type="repository">https://github.com/ros-industrial/motoman</url>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <test_depend>roslaunch</test_depend>
+
+  <exec_depend>joint_state_publisher_gui</exec_depend>
+  <exec_depend>motoman_driver</exec_depend>
+  <exec_depend>motoman_resources</exec_depend>
+  <exec_depend>robot_state_publisher</exec_depend>
+  <exec_depend>rviz</exec_depend>
+  <exec_depend>xacro</exec_depend>
+
+  <export>
+    <architecture_independent/>
+  </export>
+</package>

motoman_gp20hl_support/test/roslaunch_test_gp20hl.xml

@@ -0,0 +1,30 @@
+<launch>
+  <arg name="req_arg" value="default"/>
+  <arg name="yrc1000" value="yrc1000"/>
+
+  <group ns="load_gp20hl">
+    <include file="$(find motoman_gp20hl_support)/launch/load_gp20hl.launch"/>
+  </group>
+
+  <group ns="test_gp20hl">
+    <include file="$(find motoman_gp20hl_support)/launch/test_gp20hl.launch"/>
+  </group>
+
+  <group ns="robot_interface_streaming_gp20hl">
+    <group ns="yrc1000" >
+      <include file="$(find motoman_gp20hl_support)/launch/robot_interface_streaming_gp20hl.launch">
+        <arg name="robot_ip"   value="$(arg req_arg)" />
+        <arg name="controller" value="$(arg yrc1000)"/>
+      </include>
+    </group>
+  </group>
+
+  <group ns="robot_state_visualize_gp20hl">
+    <group ns="yrc1000" >
+      <include file="$(find motoman_gp20hl_support)/launch/robot_state_visualize_gp20hl.launch">
+        <arg name="robot_ip"   value="$(arg req_arg)" />
+        <arg name="controller" value="$(arg yrc1000)"/>
+      </include>
+    </group>
+  </group>
+</launch>

motoman_gp20hl_support/urdf/gp20hl.xacro

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

motoman_gp20hl_support/urdf/gp20hl_macro.xacro

@@ -0,0 +1,169 @@
+<?xml version="1.0" ?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+  <xacro:macro name="motoman_gp20hl" params="prefix">
+    <xacro:include filename="$(find motoman_resources)/urdf/common_materials.xacro"/>
+
+    <!-- link list -->
+    <link name="${prefix}base_link">
+      <visual>
+        <geometry>
+          <mesh filename="package://motoman_gp20hl_support/meshes/gp20hl/visual/base_link.dae"/>
+        </geometry>
+        <xacro:material_yaskawa_blue/>
+      </visual>
+      <collision>
+        <geometry>
+          <mesh filename="package://motoman_gp20hl_support/meshes/gp20hl/collision/base_link.stl"/>
+        </geometry>
+      </collision>
+    </link>
+    <link name="${prefix}link_1_s">
+      <visual>
+        <geometry>
+          <mesh filename="package://motoman_gp20hl_support/meshes/gp20hl/visual/link_1_s.dae"/>
+        </geometry>
+        <xacro:material_yaskawa_blue/>
+      </visual>
+      <collision>
+        <geometry>
+          <mesh filename="package://motoman_gp20hl_support/meshes/gp20hl/collision/link_1_s.stl"/>
+        </geometry>
+      </collision>
+    </link>
+    <link name="${prefix}link_2_l">
+      <visual>
+        <geometry>
+          <mesh filename="package://motoman_gp20hl_support/meshes/gp20hl/visual/link_2_l.dae"/>
+        </geometry>
+        <xacro:material_yaskawa_blue/>
+      </visual>
+      <collision>
+        <geometry>
+          <mesh filename="package://motoman_gp20hl_support/meshes/gp20hl/collision/link_2_l.stl"/>
+        </geometry>
+      </collision>
+    </link>
+    <link name="${prefix}link_3_u">
+      <visual>
+        <geometry>
+          <mesh filename="package://motoman_gp20hl_support/meshes/gp20hl/visual/link_3_u.dae"/>
+        </geometry>
+        <xacro:material_yaskawa_blue/>
+      </visual>
+      <collision>
+        <geometry>
+          <mesh filename="package://motoman_gp20hl_support/meshes/gp20hl/collision/link_3_u.stl"/>
+        </geometry>
+      </collision>
+    </link>
+    <link name="${prefix}link_4_r">
+      <visual>
+        <geometry>
+          <mesh filename="package://motoman_gp20hl_support/meshes/gp20hl/visual/link_4_r.dae"/>
+        </geometry>
+        <xacro:material_yaskawa_blue/>
+      </visual>
+      <collision>
+        <geometry>
+          <mesh filename="package://motoman_gp20hl_support/meshes/gp20hl/collision/link_4_r.stl"/>
+        </geometry>
+      </collision>
+    </link>
+    <link name="${prefix}link_5_b">
+      <visual>
+        <geometry>
+          <mesh filename="package://motoman_gp20hl_support/meshes/gp20hl/visual/link_5_b.dae"/>
+        </geometry>
+        <xacro:material_yaskawa_blue/>
+      </visual>
+      <collision>
+        <geometry>
+          <mesh filename="package://motoman_gp20hl_support/meshes/gp20hl/collision/link_5_b.stl"/>
+        </geometry>
+      </collision>
+    </link>
+    <link name="${prefix}link_6_t">
+      <visual>
+        <geometry>
+          <mesh filename="package://motoman_gp20hl_support/meshes/gp20hl/visual/link_6_t.dae"/>
+        </geometry>
+        <xacro:material_yaskawa_silver/>
+      </visual>
+      <collision>
+        <geometry>
+          <mesh filename="package://motoman_gp20hl_support/meshes/gp20hl/collision/link_6_t.stl"/>
+        </geometry>
+      </collision>
+    </link>
+    <!-- end of link list -->
+
+    <!-- joint list -->
+    <joint name="${prefix}joint_1_s" type="revolute">
+      <parent link="${prefix}base_link"/>
+      <child link="${prefix}link_1_s"/>
+      <origin xyz="0 0 0.540" rpy="0 0 0" />
+      <axis xyz="0 0 1" />
+      <limit lower="${radians(-180)}" upper="${radians(180)}" effort="5516.2" velocity="${radians(180.0)}"/>
+    </joint>
+    <joint name="${prefix}joint_2_l" type="revolute">
+      <parent link="${prefix}link_1_s"/>
+      <child link="${prefix}link_2_l"/>
+      <origin xyz="0.145 0 0" rpy="0 0 0" />
+      <axis xyz="0 1 0" />
+      <limit lower="${radians(-90)}" upper="${radians(135)}" effort="7060.7" velocity="${radians(180.0)}"/>
+    </joint>
+    <joint name="${prefix}joint_3_u" type="revolute">
+      <parent link="${prefix}link_2_l"/>
+      <child link="${prefix}link_3_u"/>
+      <origin xyz="0 0 1.150" rpy="0 0 0" />
+      <axis xyz="0 -1 0" />
+      <limit lower="${radians(-80)}" upper="${radians(206)}" effort="3089.0" velocity="${radians(180.0)}"/>
+    </joint>
+    <joint name="${prefix}joint_4_r" type="revolute">
+      <parent link="${prefix}link_3_u"/>
+      <child link="${prefix}link_4_r"/>
+      <origin xyz="1.812 0 0.250" rpy="0 0 0" />
+      <axis xyz="-1 0 0" />
+      <limit lower="${radians(-200)}" upper="${radians(200)}" effort="194.4" velocity="${radians(400)}"/>
+    </joint>
+    <joint name="${prefix}joint_5_b" type="revolute">
+      <parent link="${prefix}link_4_r"/>
+      <child link="${prefix}link_5_b"/>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <axis xyz="0 -1 0" />
+      <limit lower="${radians(-150)}" upper="${radians(150)}" effort="70.56" velocity="${radians(430)}"/>
+    </joint>
+    <joint name="${prefix}joint_6_t" type="revolute">
+      <parent link="${prefix}link_5_b"/>
+      <child link="${prefix}link_6_t"/>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <axis xyz="-1 0 0" />
+      <limit lower="${radians(-455)}" upper="${radians(455)}" effort="50.31" velocity="${radians(630)}"/>
+    </joint>
+    <!-- end of joint list -->
+
+    <!-- ROS-Industrial 'flange' frame: attachment point for EEF models -->
+    <link name="${prefix}flange"/>
+    <joint name="${prefix}joint_6_t-flange" type="fixed">
+      <origin xyz="0.100 0 0" rpy="0 0 0"/>
+      <parent link="${prefix}link_6_t"/>
+      <child link="${prefix}flange"/>
+    </joint>
+
+    <!-- ROS-Industrial 'tool0' frame: all-zeros tool frame -->
+    <link name="${prefix}tool0"/>
+    <joint name="${prefix}flange-tool0" type="fixed">
+      <origin xyz="0 0 0" rpy="${radians(180)} ${radians(-90)} 0"/>
+      <parent link="${prefix}flange"/>
+      <child link="${prefix}tool0"/>
+    </joint>
+
+    <!-- ROS base_link to Robot Manufacturer World Coordinates transform -->
+    <link name="${prefix}base" />
+    <joint name="${prefix}base_link-base" type="fixed">
+      <origin xyz="0 0 0.540" rpy="0 0 0"/>
+      <parent link="${prefix}base_link"/>
+      <child link="${prefix}base"/>
+    </joint>
+  </xacro:macro>
+</robot>