Add GP400 support package

motoman_gp400_support/CMakeLists.txt

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

motoman_gp400_support/config/joint_names_gp400.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_gp400_support/config/opw_parameters_gp400.yaml

@@ -0,0 +1,20 @@
+#
+# 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.325
+    a2: -0.300
+    b: 0.000
+    c1: 0.650
+    c2: 1.150
+    c3: 1.225
+    c4: 0.250
+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_gp400_support/launch/load_gp400.launch

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

motoman_gp400_support/launch/robot_interface_streaming_gp400.launch

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

motoman_gp400_support/launch/robot_state_visualize_gp400.launch

@@ -0,0 +1,25 @@
+<!--
+  Manipulator specific version of the state visualizer.
+  Defaults provided for gp400:
+   - joints
+  Usage:
+    robot_state_visualize_gp400.launch robot_ip:=<value> controller:=<yrc1000>
+-->
+<launch>
+  <arg name="robot_ip" />
+
+  <!-- controller: Controller name (yrc1000) -->
+  <arg name="controller" />
+
+  <rosparam command="load" file="$(find motoman_gp400_support)/config/joint_names_gp400.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_gp400_support)/launch/load_gp400.launch" />
+
+  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find industrial_robot_client)/config/robot_state_visualize.rviz" required="true" />
+</launch>

motoman_gp400_support/launch/test_gp400.launch

@@ -0,0 +1,8 @@
+<launch>
+  <include file="$(find motoman_gp400_support)/launch/load_gp400.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_gp400_support/meshes/gp400/visual/link_2_l_2.dae

@@ -0,0 +1,89 @@
+<?xml version="1.0" encoding="utf-8"?>
+<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance">
+  <asset>
+    <contributor>
+      <author>Blender User</author>
+      <authoring_tool>Blender 4.2.0 commit date:2024-07-16, commit time:06:20, hash:a51f293548ad</authoring_tool>
+    </contributor>
+    <created>2025-08-13T11:40:53</created>
+    <modified>2025-08-13T11:40:53</modified>
+    <unit name="meter" meter="1"/>
+    <up_axis>Z_UP</up_axis>
+  </asset>
+  <library_effects>
+    <effect id="Surface_1_006-effect">
+      <profile_COMMON>
+        <technique sid="common">
+          <lambert>
+            <emission>
+              <color sid="emission">0 0 0 1</color>
+            </emission>
+            <diffuse>
+              <color sid="diffuse">0 0.200784 0.4 1</color>
+            </diffuse>
+            <index_of_refraction>
+              <float sid="ior">1</float>
+            </index_of_refraction>
+          </lambert>
+        </technique>
+      </profile_COMMON>
+    </effect>
+  </library_effects>
+  <library_images/>
+  <library_materials>
+    <material id="Surface_1_006-material" name="Surface_1.006">
+      <instance_effect url="#Surface_1_006-effect"/>
+    </material>
+  </library_materials>
+  <library_geometries>
+    <geometry id="polygons_004-mesh" name="polygons.004">
+      <mesh>
+        <source id="polygons_004-mesh-positions">
+          <float_array id="polygons_004-mesh-positions-array" count="108">-0.15737 -0.06999397 0.869523 -0.168074 -0.06996798 0.915439 -0.05695897 -0.07005798 0.986767 -0.1998209 -0.06946796 1.178514 -0.09948998 -0.06958299 1.177937 -0.200265 -0.06716996 1.330122 -0.10054 -0.06318998 1.509163 -0.20079 -0.06440997 1.686472 -0.09952998 -0.06954497 1.830572 -0.198649 -0.06941598 1.903054 -0.08511096 -0.06968599 1.87331 -0.04286199 -0.06964695 1.91836 -0.03555196 -0.06975001 1.966008 -0.05469697 -0.06999397 1.996618 -0.102398 -0.06932598 2.01741 -0.1498979 -0.06980097 1.993442 -0.118676 -0.06932598 0.83487 -0.06435 -0.069866 0.844977 -0.03595495 -0.06954497 0.882807 -0.07860797 -0.110766 2.013945 -0.04104495 -0.109996 1.97727 -0.03898495 -0.110343 1.923558 -0.1455349 -0.110728 1.999794 -0.1984069 -0.110805 1.902188 -0.2007499 -0.11576 1.626984 -0.19978 -0.110535 1.178514 -0.16876 -0.110176 0.915439 -0.1419 -0.111549 0.845555 -0.09678399 -0.110176 1.848476 -0.09948998 -0.11404 1.80487 -0.03538995 -0.110381 0.910529 -0.07569998 -0.110458 1.046254 -0.09993398 -0.110445 1.182268 -0.09993398 -0.114386 1.448809 -0.08583796 -0.110278 0.835448 -0.04617398 -0.110188 0.861149</float_array>
+          <technique_common>
+            <accessor source="#polygons_004-mesh-positions-array" count="36" stride="3">
+              <param name="X" type="float"/>
+              <param name="Y" type="float"/>
+              <param name="Z" type="float"/>
+            </accessor>
+          </technique_common>
+        </source>
+        <source id="polygons_004-mesh-normals">
+          <float_array id="polygons_004-mesh-normals-array" count="312">-0.004222095 0.9999824 0.004162311 0.001339614 0.9999989 -8.25015e-4 -0.001590073 0.999998 0.001197934 0.001361787 0.999979 -0.006354629 -0.001011729 0.9999245 -0.01224768 -0.009977281 0.9998505 -0.01413375 -0.002840161 0.999993 -0.002472221 0.004918038 0.9999206 0.0116074 0.01455044 0.9997821 0.01497155 0.002864122 0.9999822 0.005249023 0.002336025 0.9999973 -1.49937e-4 -0.001500606 0.9999972 0.001827776 -0.007458508 0.9999668 0.00330609 0.001215338 0.9999992 -4.43277e-4 -0.00334388 0.9999924 0.002028167 -0.01668834 0.9997729 0.01325947 -0.004249215 0.9999822 0.004186213 -0.001625418 0.999998 0.001227438 1.43727e-4 0.9999887 0.004758059 1.44534e-4 0.9999887 0.004758358 -0.001620054 0.999998 0.001231491 -0.002137601 0.9999967 0.001468598 -0.001651644 0.9999979 0.001233041 -0.01408576 0.9998986 0.002088248 -0.002165555 0.9999966 0.001471221 0.6703819 0.02713626 0.74152 0.5600496 0.02347218 0.8281266 0.8905414 -0.06953132 0.449557 0.989195 -0.006032347 0.1464818 0.9675065 0.03292727 -0.2506929 0.9892213 -0.005973756 0.146307 0.9280186 0.02031594 -0.3719796 0.8987389 0.02975463 -0.4374735 0.3260493 -0.05518013 0.943741 0.6702802 0.02719068 0.7416098 0.1034507 -0.02378612 0.9943501 -0.7683024 0.01905107 0.6398035 -0.6977383 0.03668773 0.7154127 -0.9443213 -0.01413327 0.3287211 -0.9854895 -0.01097905 0.1693813 -0.9999694 -0.005381762 0.005671441 -0.9999896 -0.00454545 5.58319e-4 -0.9999927 -0.0031538 -0.002149105 -0.9991257 -0.003073632 -0.04169619 -0.9967986 0.00500524 -0.07979738 -0.987877 0.01089024 -0.1548562 -0.9715387 -0.03605389 -0.2341213 -0.8896955 0.01827186 -0.456189 -0.8249327 0.01873302 -0.5649206 -0.6484 -0.0426045 -0.7601069 -0.8896203 0.01835548 -0.4563321 -0.4378446 0.01337856 -0.8989512 0.008667826 -0.9999622 -7.1609e-4 0.02783817 -0.9995841 0.007524967 0.01412534 -0.9998992 0.001504659 0.001720249 -0.9999951 -0.002646982 0.01070511 -0.9996463 0.02434587 0.02739131 -0.9988191 0.04012715 0.01719456 -0.9992488 0.03473168 -0.001623928 -0.999996 -0.002359807 -0.00226444 -0.9999973 -5.44796e-4 0.003397583 -0.9999869 0.003836035 -3.45526e-5 -0.9999849 -0.005489289 -0.001498043 -0.9999566 -0.009207665 0.003261327 -0.9999586 -0.008497714 -0.002181112 -0.9999946 0.002472817 0.02814048 -0.9991403 0.03044587 0.01450878 -0.9997616 0.01632273 -4.72275e-4 -0.9999995 -8.35488e-4 -0.6975973 0.03670889 0.7155491 -0.6521767 -0.03835129 0.7570964 -0.3324014 -0.05428242 0.9415746 -0.09245121 0.01635384 0.9955829 -0.3322986 -0.05439752 0.9416043 0.1032653 -0.02386343 0.9943676 0.9088035 -0.006168305 -0.4171791 0.8133381 -0.03636097 -0.580654 0.5327253 0.0561434 -0.8444239 0.1945437 -0.03877079 -0.9801273 0.5325866 0.0562402 -0.844505 0.1944704 -0.03863406 -0.9801473 0.005503952 -0.009719252 -0.9999377 -0.2346011 0.05709075 -0.9704138 0.1944137 -0.03876608 -0.9801533 -0.4377502 0.01325863 -0.8989989 0.9999908 0.004301488 -2.03446e-4 0.9982028 0.003010988 0.05985164 0.9901954 -0.04493731 0.132264 0.9728885 -0.04982852 0.2258429 0.97224 -0.003355205 0.2339621 0.9957005 -0.00309503 0.09258013 0.9999331 0.008479475 -0.007883489 0.974672 0.04800868 -0.2184258 0.995711 -0.003190755 0.09246307 0.8134359 -0.03642964 -0.5805128 0.8986557 0.02970594 -0.4376478 0.8645577 0.0694164 -0.4977163 0.8369638 0.01681059 -0.5470001 0.8369992 0.01683628 -0.546945 0.927959 0.02033865 -0.3721272 0.9395632 0.03651452 -0.3404229 0.9913671 -0.01193904 -0.1305721 0.9995667 0.0181086 -0.02320945 0.9999234 0.0123772 -4.60656e-4</float_array>
+          <technique_common>
+            <accessor source="#polygons_004-mesh-normals-array" count="104" stride="3">
+              <param name="X" type="float"/>
+              <param name="Y" type="float"/>
+              <param name="Z" type="float"/>
+            </accessor>
+          </technique_common>
+        </source>
+        <vertices id="polygons_004-mesh-vertices">
+          <input semantic="POSITION" source="#polygons_004-mesh-positions"/>
+        </vertices>
+        <triangles material="Surface_1_006-material" count="68">
+          <input semantic="VERTEX" source="#polygons_004-mesh-vertices" offset="0"/>
+          <input semantic="NORMAL" source="#polygons_004-mesh-normals" offset="1"/>
+          <p>0 0 1 1 2 2 2 2 1 1 3 3 2 2 3 3 4 4 4 4 3 3 5 5 4 4 5 5 6 6 6 6 5 5 7 7 6 6 7 7 8 8 8 8 7 7 9 9 8 8 9 9 10 10 10 10 9 9 11 11 12 12 11 11 13 13 13 13 11 11 9 9 13 13 9 9 14 14 14 14 9 9 15 15 0 16 2 17 16 18 16 19 2 20 17 21 2 22 18 23 17 24 13 25 19 26 20 27 13 25 20 27 12 28 12 28 20 27 21 29 12 30 21 31 11 32 19 33 13 34 14 35 22 36 15 37 9 38 22 36 9 38 23 39 23 39 9 38 7 40 23 39 7 40 24 41 7 40 5 42 24 41 24 41 5 42 3 43 24 41 3 43 25 44 25 44 3 43 1 45 25 44 1 45 26 46 26 46 1 45 0 47 26 46 0 47 27 48 27 49 0 50 16 51 21 52 20 53 19 54 19 54 22 55 21 52 21 52 22 55 23 56 21 52 23 56 28 57 28 57 23 56 29 58 30 59 31 60 26 61 26 61 31 60 32 62 26 61 32 62 25 63 25 63 32 62 33 64 25 63 33 64 24 65 24 65 33 64 29 58 24 65 29 58 23 56 27 66 34 67 26 61 26 61 34 67 35 68 26 61 35 68 30 59 15 69 22 70 14 71 14 72 22 73 19 74 18 75 35 76 17 77 17 77 35 76 34 78 17 79 34 80 16 81 16 82 34 83 27 84 6 85 32 86 4 87 4 87 32 86 31 88 4 87 31 88 2 89 2 89 31 88 30 90 2 89 30 90 18 91 18 92 30 93 35 94 11 95 21 96 10 97 10 98 21 99 28 100 10 98 28 100 8 101 8 101 28 100 29 102 8 101 29 102 6 85 6 85 29 102 33 103 6 85 33 103 32 86</p>
+        </triangles>
+      </mesh>
+    </geometry>
+  </library_geometries>
+  <library_visual_scenes>
+    <visual_scene id="Scene" name="Scene">
+      <node id="mh00400-j00_lk2_2" name="mh00400-j00_lk2_2" type="NODE">
+        <matrix sid="transform">1 0 0 0.1 0 1 0 0 0 0 1 -0.9 0 0 0 1</matrix>
+        <instance_geometry url="#polygons_004-mesh" name="mh00400-j00_lk2_2">
+          <bind_material>
+            <technique_common>
+              <instance_material symbol="Surface_1_006-material" target="#Surface_1_006-material"/>
+            </technique_common>
+          </bind_material>
+        </instance_geometry>
+      </node>
+    </visual_scene>
+  </library_visual_scenes>
+  <scene>
+    <instance_visual_scene url="#Scene"/>
+  </scene>
+</COLLADA>