diff --git a/motoman_mh5_support/CHANGELOG.rst b/motoman_mh5_support/CHANGELOG.rst
index 72eca754..0b8b4505 100644
--- a/motoman_mh5_support/CHANGELOG.rst
+++ b/motoman_mh5_support/CHANGELOG.rst
@@ -2,6 +2,20 @@
 Changelog for package motoman_mh5_support
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+0.3.6 (UNRELEASED)
+------------------
+** Added support for MH5 SHORT and LONG variants
+* Each variant has unique launch, test, urdf, and (L & U) mesh files. Config/joint_names and remaining mesh files are shared.
+* Fixed collision meshes
+* Fixed tab/spacing in all code
+* Renamed links & joints to ros-i convention
+* Updated appropriate motoman controllers list (fs100, dx100, dx200)
+* Rotated BASE_LINK mesh 180deg about Z-axis (allowing code to match convention)
+* Macro level definition of material_color properties
+* Added tool0, flange, base
+* Added Max Accel/Deccel (provided by EricMarcil)
+Contributors: acbuynak
+
 0.3.5 (2016-07-03)
 ------------------
 * No changes
diff --git a/motoman_mh5_support/CMakeLists.txt b/motoman_mh5_support/CMakeLists.txt
index bf174cd2..266556a6 100644
--- a/motoman_mh5_support/CMakeLists.txt
+++ b/motoman_mh5_support/CMakeLists.txt
@@ -8,7 +8,8 @@ catkin_package()
 
 if (CATKIN_ENABLE_TESTING)
   find_package(roslaunch REQUIRED)
-  roslaunch_add_file_check(test/launch_test.xml)
+  roslaunch_add_file_check(test/launch_test_mh5.xml)
+  roslaunch_add_file_check(test/launch_test_mh5l.xml)
 endif()
 
 install(DIRECTORY config launch meshes urdf
diff --git a/motoman_mh5_support/config/joint_names_mh5.yaml b/motoman_mh5_support/config/joint_names_mh5.yaml
index 245a3060..c23d10d4 100644
--- a/motoman_mh5_support/config/joint_names_mh5.yaml
+++ b/motoman_mh5_support/config/joint_names_mh5.yaml
@@ -1 +1 @@
-controller_joint_names: ['joint_s', 'joint_l', 'joint_u', 'joint_r', 'joint_b', 'joint_t']
+controller_joint_names: ['joint_1_s', 'joint_2_l', 'joint_3_u', 'joint_4_r', 'joint_5_b', 'joint_6_t']
diff --git a/motoman_mh5_support/config/opw_parameters_mh5.yaml b/motoman_mh5_support/config/opw_parameters_mh5.yaml
new file mode 100644
index 00000000..aa94752b
--- /dev/null
+++ b/motoman_mh5_support/config/opw_parameters_mh5.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.088
+    a2: -0.040
+    b:   0.000
+    c1:  0.330
+    c2:  0.310
+    c3:  0.305
+    c4:  0.080
+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]
diff --git a/motoman_mh5_support/config/opw_parameters_mh5l.yaml b/motoman_mh5_support/config/opw_parameters_mh5l.yaml
new file mode 100644
index 00000000..0655ddeb
--- /dev/null
+++ b/motoman_mh5_support/config/opw_parameters_mh5l.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.088
+    a2: -0.040
+    b:   0.000
+    c1:  0.330
+    c2:  0.400
+    c3:  0.405
+    c4:  0.080
+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]
diff --git a/motoman_mh5_support/launch/load_mh5.launch b/motoman_mh5_support/launch/load_mh5.launch
index 4ec4fc78..d5dc7238 100644
--- a/motoman_mh5_support/launch/load_mh5.launch
+++ b/motoman_mh5_support/launch/load_mh5.launch
@@ -1,4 +1,4 @@
 
 <launch>
-	<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find motoman_mh5_support)/urdf/mh5.xacro'" />
+  <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find motoman_mh5_support)/urdf/mh5.xacro'" />
 </launch>
diff --git a/motoman_mh5_support/launch/load_mh5l.launch b/motoman_mh5_support/launch/load_mh5l.launch
new file mode 100644
index 00000000..8a066839
--- /dev/null
+++ b/motoman_mh5_support/launch/load_mh5l.launch
@@ -0,0 +1,4 @@
+
+<launch>
+  <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find motoman_mh5_support)/urdf/mh5l.xacro'" />
+</launch>
diff --git a/motoman_mh5_support/launch/robot_interface_streaming_mh5.launch b/motoman_mh5_support/launch/robot_interface_streaming_mh5.launch
index 97a5f2a1..23535ad0 100644
--- a/motoman_mh5_support/launch/robot_interface_streaming_mh5.launch
+++ b/motoman_mh5_support/launch/robot_interface_streaming_mh5.launch
@@ -1,22 +1,19 @@
-
 <!--
   Manipulator specific version of 'robot_interface_streaming.launch'.
-
-  Defaults provided for mh5:
-   - 7 joints
-
+  Defaults provided for MH5:
+   - 6 joints
   Usage:
-    robot_interface_streaming_mh5.launch robot_ip:=<value> controller:=<fs100|dx100>
+    robot_interface_streaming_mh5.launch robot_ip:=<value> controller:=<fs100|dx100|dx200>
 -->
 <launch>
-	<arg name="robot_ip" doc="IP of controller" />
-	
-  <!-- controller: Controller name (fs100 or dx100) -->
-  <arg name="controller" doc="Series of the controller (dx100, dx200, fs100 or yrc1000)"/>
+  <arg name="robot_ip" doc="IP of controller" />
+
+  <!-- controller: Controller name (fs100,dx100, dx200) -->
+  <arg name="controller" doc="Series of the controller (fs100, dx100, dx200)"/>
 
-	<rosparam command="load" file="$(find motoman_mh5_support)/config/joint_names_mh5.yaml" />
+  <rosparam command="load" file="$(find motoman_mh5_support)/config/joint_names_mh5.yaml" />
 
-	<include file="$(find motoman_driver)/launch/robot_interface_streaming_$(arg controller).launch">
-		<arg name="robot_ip"   value="$(arg robot_ip)" />
-	</include>
+  <include file="$(find motoman_driver)/launch/robot_interface_streaming_$(arg controller).launch">
+    <arg name="robot_ip"   value="$(arg robot_ip)" />
+  </include>
 </launch>
diff --git a/motoman_mh5_support/launch/robot_interface_streaming_mh5l.launch b/motoman_mh5_support/launch/robot_interface_streaming_mh5l.launch
new file mode 100644
index 00000000..f18dd6d2
--- /dev/null
+++ b/motoman_mh5_support/launch/robot_interface_streaming_mh5l.launch
@@ -0,0 +1,19 @@
+<!--
+  Manipulator specific version of 'robot_interface_streaming.launch'.
+  Defaults provided for MH5L:
+   - 6 joints
+  Usage:
+    robot_interface_streaming_mh5l.launch robot_ip:=<value> controller:=<fs100|dx100|dx200>
+-->
+<launch>
+  <arg name="robot_ip" doc="IP of controller" />
+
+  <!-- controller: Controller name (fs100,dx100, dx200) -->
+  <arg name="controller" doc="Series of the controller (fs100, dx100, dx200)"/>
+
+  <rosparam command="load" file="$(find motoman_mh5_support)/config/joint_names_mh5.yaml" />
+
+  <include file="$(find motoman_driver)/launch/robot_interface_streaming_$(arg controller).launch">
+    <arg name="robot_ip"   value="$(arg robot_ip)" />
+  </include>
+</launch>
diff --git a/motoman_mh5_support/launch/robot_state_visualize_mh5.launch b/motoman_mh5_support/launch/robot_state_visualize_mh5.launch
index e68f23c3..c1039fbc 100644
--- a/motoman_mh5_support/launch/robot_state_visualize_mh5.launch
+++ b/motoman_mh5_support/launch/robot_state_visualize_mh5.launch
@@ -1,29 +1,25 @@
-
 <!--
   Manipulator specific version of the state visualizer.
-
-  Defaults provided for mh5:
-   - 7 joints
-
+  Defaults provided for MH5:
+   - 6 joints
   Usage:
-    robot_state_visualize_sia20d.launch robot_ip:=<value> controller:=<fs100|dx100>
+    robot_state_visualize_mh5.launch robot_ip:=<value> controller:=<fs100|dx100|dx200>
 -->
 <launch>
-	<arg name="robot_ip" doc="IP of controller" />
+  <arg name="robot_ip" doc="IP of controller" />
 
-  <!-- controller: Controller name (fs100 or dx100) -->
-  <arg name="controller" doc="Series of the controller (dx100, dx200, fs100 or yrc1000)"/>
+  <!-- controller: Controller name (fs100,dx100, dx200) -->
+  <arg name="controller" doc="Series of the controller (fs100, dx100, dx200)"/>
 
-	<rosparam command="load" file="$(find motoman_mh5_support)/config/joint_names_mh5.yaml" />
+  <rosparam command="load" file="$(find motoman_mh5_support)/config/joint_names_mh5.yaml" />
 
-	<include file="$(find motoman_driver)/launch/robot_state_$(arg controller).launch">
-		<arg name="robot_ip"   value="$(arg robot_ip)" />
-	</include>
+  <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" />
+  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
 
-	<include file="$(find motoman_mh5_support)/launch/load_mh5.launch" />
+  <include file="$(find motoman_mh5_support)/launch/load_mh5.launch" />
 
-	<node name="rviz" pkg="rviz" type="rviz" args="-d $(find industrial_robot_client)/config/robot_state_visualize.rviz" required="true" />
+  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find industrial_robot_client)/config/robot_state_visualize.rviz" required="true" />
 </launch>
diff --git a/motoman_mh5_support/launch/robot_state_visualize_mh5l.launch b/motoman_mh5_support/launch/robot_state_visualize_mh5l.launch
new file mode 100644
index 00000000..2360bd8d
--- /dev/null
+++ b/motoman_mh5_support/launch/robot_state_visualize_mh5l.launch
@@ -0,0 +1,25 @@
+<!--
+  Manipulator specific version of the state visualizer.
+  Defaults provided for MH5L:
+   - 6 joints
+  Usage:
+    robot_state_visualize_mh5l.launch robot_ip:=<value> controller:=<fs100|dx100|dx200>
+-->
+<launch>
+  <arg name="robot_ip" doc="IP of controller" />
+
+  <!-- controller: Controller name (fs100,dx100, dx200) -->
+  <arg name="controller" doc="Series of the controller (fs100, dx100, dx200)"/>
+
+  <rosparam command="load" file="$(find motoman_mh5_support)/config/joint_names_mh5.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_mh5_support)/launch/load_mh5l.launch" />
+
+  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find industrial_robot_client)/config/robot_state_visualize.rviz" required="true" />
+</launch>
diff --git a/motoman_mh5_support/launch/test_mh5.launch b/motoman_mh5_support/launch/test_mh5.launch
index 3655718f..2a4c5150 100644
--- a/motoman_mh5_support/launch/test_mh5.launch
+++ b/motoman_mh5_support/launch/test_mh5.launch
@@ -1,8 +1,8 @@
-
 <launch>
-	<include file="$(find motoman_mh5_support)/launch/load_mh5.launch" />
-	<param name="use_gui" value="true" />
-	<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
-	<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" />
+  <include file="$(find motoman_mh5_support)/launch/load_mh5.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>
diff --git a/motoman_mh5_support/launch/test_mh5l.launch b/motoman_mh5_support/launch/test_mh5l.launch
new file mode 100644
index 00000000..e94efad3
--- /dev/null
+++ b/motoman_mh5_support/launch/test_mh5l.launch
@@ -0,0 +1,8 @@
+<launch>
+  <include file="$(find motoman_mh5_support)/launch/load_mh5l.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>
diff --git a/motoman_mh5_support/meshes/mh5/collision/MH5_BASE_AXIS.stl b/motoman_mh5_support/meshes/mh5/collision/MH5_BASE_AXIS.stl
deleted file mode 100644
index af75b21c..00000000
Binary files a/motoman_mh5_support/meshes/mh5/collision/MH5_BASE_AXIS.stl and /dev/null differ
diff --git a/motoman_mh5_support/meshes/mh5/collision/MH5_B_AXIS.stl b/motoman_mh5_support/meshes/mh5/collision/MH5_B_AXIS.stl
deleted file mode 100644
index 0aa218ce..00000000
Binary files a/motoman_mh5_support/meshes/mh5/collision/MH5_B_AXIS.stl and /dev/null differ
diff --git a/motoman_mh5_support/meshes/mh5/collision/MH5_L_AXIS.stl b/motoman_mh5_support/meshes/mh5/collision/MH5_L_AXIS.stl
deleted file mode 100644
index 718d8365..00000000
Binary files a/motoman_mh5_support/meshes/mh5/collision/MH5_L_AXIS.stl and /dev/null differ
diff --git a/motoman_mh5_support/meshes/mh5/collision/MH5_R_AXIS.stl b/motoman_mh5_support/meshes/mh5/collision/MH5_R_AXIS.stl
deleted file mode 100644
index 27a3e619..00000000
Binary files a/motoman_mh5_support/meshes/mh5/collision/MH5_R_AXIS.stl and /dev/null differ
diff --git a/motoman_mh5_support/meshes/mh5/collision/MH5_S_AXIS.stl b/motoman_mh5_support/meshes/mh5/collision/MH5_S_AXIS.stl
deleted file mode 100644
index ab08d78c..00000000
Binary files a/motoman_mh5_support/meshes/mh5/collision/MH5_S_AXIS.stl and /dev/null differ
diff --git a/motoman_mh5_support/meshes/mh5/collision/MH5_T_AXIS.stl b/motoman_mh5_support/meshes/mh5/collision/MH5_T_AXIS.stl
deleted file mode 100644
index 014a76ed..00000000
Binary files a/motoman_mh5_support/meshes/mh5/collision/MH5_T_AXIS.stl and /dev/null differ
diff --git a/motoman_mh5_support/meshes/mh5/collision/MH5_U_AXIS.stl b/motoman_mh5_support/meshes/mh5/collision/MH5_U_AXIS.stl
deleted file mode 100644
index b071954f..00000000
Binary files a/motoman_mh5_support/meshes/mh5/collision/MH5_U_AXIS.stl and /dev/null differ
diff --git a/motoman_mh5_support/meshes/mh5/collision/base_link.stl b/motoman_mh5_support/meshes/mh5/collision/base_link.stl
new file mode 100644
index 00000000..72f6fcf0
Binary files /dev/null and b/motoman_mh5_support/meshes/mh5/collision/base_link.stl differ
diff --git a/motoman_mh5_support/meshes/mh5/collision/link_1_s.stl b/motoman_mh5_support/meshes/mh5/collision/link_1_s.stl
new file mode 100644
index 00000000..a02ab490
Binary files /dev/null and b/motoman_mh5_support/meshes/mh5/collision/link_1_s.stl differ
diff --git a/motoman_mh5_support/meshes/mh5/collision/link_2_l.stl b/motoman_mh5_support/meshes/mh5/collision/link_2_l.stl
new file mode 100644
index 00000000..2f174964
Binary files /dev/null and b/motoman_mh5_support/meshes/mh5/collision/link_2_l.stl differ
diff --git a/motoman_mh5_support/meshes/mh5/collision/link_3_u.stl b/motoman_mh5_support/meshes/mh5/collision/link_3_u.stl
new file mode 100644
index 00000000..1e6a6c7f
Binary files /dev/null and b/motoman_mh5_support/meshes/mh5/collision/link_3_u.stl differ
diff --git a/motoman_mh5_support/meshes/mh5/collision/link_4_r.stl b/motoman_mh5_support/meshes/mh5/collision/link_4_r.stl
new file mode 100644
index 00000000..222094e5
Binary files /dev/null and b/motoman_mh5_support/meshes/mh5/collision/link_4_r.stl differ
diff --git a/motoman_mh5_support/meshes/mh5/collision/link_5_b.stl b/motoman_mh5_support/meshes/mh5/collision/link_5_b.stl
new file mode 100644
index 00000000..520b2a0e
Binary files /dev/null and b/motoman_mh5_support/meshes/mh5/collision/link_5_b.stl differ
diff --git a/motoman_mh5_support/meshes/mh5/collision/link_6_t.stl b/motoman_mh5_support/meshes/mh5/collision/link_6_t.stl
new file mode 100644
index 00000000..3007c8bf
Binary files /dev/null and b/motoman_mh5_support/meshes/mh5/collision/link_6_t.stl differ
diff --git a/motoman_mh5_support/meshes/mh5/visual/MH5_BASE_AXIS.stl b/motoman_mh5_support/meshes/mh5/visual/MH5_BASE_AXIS.stl
deleted file mode 100644
index 38ab29a2..00000000
Binary files a/motoman_mh5_support/meshes/mh5/visual/MH5_BASE_AXIS.stl and /dev/null differ
diff --git a/motoman_mh5_support/meshes/mh5/visual/MH5_R_AXIS.stl b/motoman_mh5_support/meshes/mh5/visual/MH5_R_AXIS.stl
deleted file mode 100644
index 8587b44b..00000000
Binary files a/motoman_mh5_support/meshes/mh5/visual/MH5_R_AXIS.stl and /dev/null differ
diff --git a/motoman_mh5_support/meshes/mh5/visual/MH5_S_AXIS.stl b/motoman_mh5_support/meshes/mh5/visual/MH5_S_AXIS.stl
deleted file mode 100644
index af545928..00000000
Binary files a/motoman_mh5_support/meshes/mh5/visual/MH5_S_AXIS.stl and /dev/null differ
diff --git a/motoman_mh5_support/meshes/mh5/visual/base_link.stl b/motoman_mh5_support/meshes/mh5/visual/base_link.stl
new file mode 100644
index 00000000..831bdfaf
Binary files /dev/null and b/motoman_mh5_support/meshes/mh5/visual/base_link.stl differ
diff --git a/motoman_mh5_support/meshes/mh5/visual/link_1_s.stl b/motoman_mh5_support/meshes/mh5/visual/link_1_s.stl
new file mode 100644
index 00000000..bb72f6ef
Binary files /dev/null and b/motoman_mh5_support/meshes/mh5/visual/link_1_s.stl differ
diff --git a/motoman_mh5_support/meshes/mh5/visual/MH5_L_AXIS.stl b/motoman_mh5_support/meshes/mh5/visual/link_2_l.stl
similarity index 100%
rename from motoman_mh5_support/meshes/mh5/visual/MH5_L_AXIS.stl
rename to motoman_mh5_support/meshes/mh5/visual/link_2_l.stl
diff --git a/motoman_mh5_support/meshes/mh5/visual/MH5_U_AXIS.stl b/motoman_mh5_support/meshes/mh5/visual/link_3_u.stl
similarity index 100%
rename from motoman_mh5_support/meshes/mh5/visual/MH5_U_AXIS.stl
rename to motoman_mh5_support/meshes/mh5/visual/link_3_u.stl
diff --git a/motoman_mh5_support/meshes/mh5/visual/link_4_r.stl b/motoman_mh5_support/meshes/mh5/visual/link_4_r.stl
new file mode 100644
index 00000000..3c96f3ec
Binary files /dev/null and b/motoman_mh5_support/meshes/mh5/visual/link_4_r.stl differ
diff --git a/motoman_mh5_support/meshes/mh5/visual/MH5_B_AXIS.stl b/motoman_mh5_support/meshes/mh5/visual/link_5_b.stl
similarity index 100%
rename from motoman_mh5_support/meshes/mh5/visual/MH5_B_AXIS.stl
rename to motoman_mh5_support/meshes/mh5/visual/link_5_b.stl
diff --git a/motoman_mh5_support/meshes/mh5/visual/MH5_T_AXIS.stl b/motoman_mh5_support/meshes/mh5/visual/link_6_t.stl
similarity index 100%
rename from motoman_mh5_support/meshes/mh5/visual/MH5_T_AXIS.stl
rename to motoman_mh5_support/meshes/mh5/visual/link_6_t.stl
diff --git a/motoman_mh5_support/meshes/mh5_old_variants/collision/link_3_u.stl b/motoman_mh5_support/meshes/mh5_old_variants/collision/link_3_u.stl
new file mode 100644
index 00000000..8e26da4d
Binary files /dev/null and b/motoman_mh5_support/meshes/mh5_old_variants/collision/link_3_u.stl differ
diff --git a/motoman_mh5_support/meshes/mh5_old_variants/visual/link_3_u.stl b/motoman_mh5_support/meshes/mh5_old_variants/visual/link_3_u.stl
new file mode 100644
index 00000000..cc20dd0a
Binary files /dev/null and b/motoman_mh5_support/meshes/mh5_old_variants/visual/link_3_u.stl differ
diff --git a/motoman_mh5_support/meshes/mh5l/collision/link_2_l.stl b/motoman_mh5_support/meshes/mh5l/collision/link_2_l.stl
new file mode 100644
index 00000000..357f972e
Binary files /dev/null and b/motoman_mh5_support/meshes/mh5l/collision/link_2_l.stl differ
diff --git a/motoman_mh5_support/meshes/mh5l/collision/link_4_r.stl b/motoman_mh5_support/meshes/mh5l/collision/link_4_r.stl
new file mode 100644
index 00000000..67c4672a
Binary files /dev/null and b/motoman_mh5_support/meshes/mh5l/collision/link_4_r.stl differ
diff --git a/motoman_mh5_support/meshes/mh5l/visual/link_2_l.stl b/motoman_mh5_support/meshes/mh5l/visual/link_2_l.stl
new file mode 100644
index 00000000..632e7c57
Binary files /dev/null and b/motoman_mh5_support/meshes/mh5l/visual/link_2_l.stl differ
diff --git a/motoman_mh5_support/meshes/mh5l/visual/link_4_r.stl b/motoman_mh5_support/meshes/mh5l/visual/link_4_r.stl
new file mode 100644
index 00000000..d216ad70
Binary files /dev/null and b/motoman_mh5_support/meshes/mh5l/visual/link_4_r.stl differ
diff --git a/motoman_mh5_support/package.xml b/motoman_mh5_support/package.xml
index 85ac5e24..22fdbddc 100644
--- a/motoman_mh5_support/package.xml
+++ b/motoman_mh5_support/package.xml
@@ -2,24 +2,27 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>motoman_mh5_support</name>
-  <version>0.3.5</version>
+  <version>0.3.6</version>
   <description>
-    <p>ROS-Industrial support for the Motoman MH5 (and variants).</p>
+    <p>ROS-Industrial support for Motoman MH5 variants.</p>
     <p>
       This package contains configuration data, 3D models and launch files
-      for Motoman MH5 manipulators.
+      for Motoman MH5 variants.
     </p>
     <p>
       <b>Specifications</b>
     </p>
     <ul>
-      <li>MH5 - Default</li>
+      <li>MH5  - Short Variants: MH5F(FS100),  MH5SII(DX200),  MH5S(DX100)</li>
+      <li>MH5L - Long Variants:  MH5LF(FS100), MH5SLII(DX200), MH5SL(DX100)</li>
     </ul>
+    <p>Models within each short / long variant are the same (physically and parameters).</p>
     <p>
-      Joint limits and maximum joint velocities are based on the information 
-      found in the online 
-      http://www.motoman.com/datasheets/mh5.pdf
-      All urdfs are based on the default motion and joint velocity limits, 
+      Maximum joint velocities are based on the information
+      found online https://www.motoman.com/en-us/products/robots/industrial/assembly-handling/mh-series/mh5ls-ii
+      Joint limits and inertial values were provided by Motoman support and found
+      under ros-industrial/motoman issue # 377. See explanation in issue.
+      All urdfs are based on the default motion and joint velocity limits,
       unless noted otherwise.
     </p>
     <p>
@@ -41,7 +44,7 @@
 
   <test_depend>roslaunch</test_depend>
 
-  <exec_depend>joint_state_publisher</exec_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>
diff --git a/motoman_mh5_support/test/launch_test.xml b/motoman_mh5_support/test/launch_test.xml
deleted file mode 100644
index 4443dd7c..00000000
--- a/motoman_mh5_support/test/launch_test.xml
+++ /dev/null
@@ -1,75 +0,0 @@
-
-<!--
-  launch_test.xml - ROSlaunch tests
-
-  Usage Instructions:
-
-  1. Add the following to your CMakeLists.txt:
-    
-    find_package(roslaunch)
-    roslaunch_add_file_check(test/launch_test.xml)
-
-  2. Create a test directory under your package
-  3. Add the "launch_text.xml" file and fill out the test below.  Use the
-     following conventions:
-    a. Encapsulate each launch file test in it's own namespace.  By
-       convention the namespace should have the same name as the launch
-       file (minus ".launch" extension)
-    b. Create tests for each possible combination of parameters.  Utilize
-       sub-namespaces under the main namespace.
-
-  Notes:
-
-  1. XML extension is used in order to avoid beinging included
-  in roslaunch auto-complete.
-
-  2. Group tags with namespaces are used to avoid node name collisions when
-  testing multpile launch files
--->
-<launch>
-	<arg name="req_arg" value="default"/>
-  <arg name="dx100" value="dx100"/>
-  <arg name="fs100" value="fs100" />
-
-  <group ns="load_mh5">
-	  <include file="$(find motoman_mh5_support)/launch/load_mh5.launch"/>
-  </group>
-
-  <group ns="test_mh5">
-	  <include file="$(find motoman_mh5_support)/launch/test_mh5.launch"/>
-  </group>
-
-
-  <group ns="robot_interface_streaming_mh5">
-    <group ns="dx100" >
-	    <include file="$(find motoman_mh5_support)/launch/robot_interface_streaming_mh5.launch">
-		    <arg name="robot_ip"   value="$(arg req_arg)" />
-        <arg name="controller" value="$(arg dx100)"/>
-	    </include>
-    </group>
-
-    <group ns="fs100" >
-	    <include file="$(find motoman_mh5_support)/launch/robot_interface_streaming_mh5.launch">
-		    <arg name="robot_ip"   value="$(arg req_arg)" />
-        <arg name="controller" value="$(arg fs100)"/>
-	    </include>
-    </group>
-  </group>
-
-  <group ns="robot_state_visualize_mh5">
-    <group ns="dx100" >
-	    <include file="$(find motoman_mh5_support)/launch/robot_state_visualize_mh5.launch">
-		    <arg name="robot_ip"   value="$(arg req_arg)" />
-        <arg name="controller" value="$(arg dx100)"/>
-	    </include>
-    </group>
-
-    <group ns="fs100" >
-	    <include file="$(find motoman_mh5_support)/launch/robot_state_visualize_mh5.launch">
-		    <arg name="robot_ip"   value="$(arg req_arg)" />
-        <arg name="controller" value="$(arg fs100)"/>
-	    </include>
-    </group>
-  </group>
-
-</launch>
diff --git a/motoman_mh5_support/test/launch_test_mh5.xml b/motoman_mh5_support/test/launch_test_mh5.xml
new file mode 100644
index 00000000..3837056c
--- /dev/null
+++ b/motoman_mh5_support/test/launch_test_mh5.xml
@@ -0,0 +1,64 @@
+<!-- launch_test_mh5.xml - ROSlaunch tests -->
+<launch>
+  <arg name="req_arg" value="default"/>
+  <arg name="dx200" value="dx200"/>
+  <arg name="dx100" value="dx100"/>
+  <arg name="fs100" value="fs100" />
+
+  <group ns="load_mh5">
+    <include file="$(find motoman_mh5_support)/launch/load_mh5.launch"/>
+  </group>
+
+  <group ns="test_mh5">
+    <include file="$(find motoman_mh5_support)/launch/test_mh5.launch"/>
+  </group>
+
+  <group ns="robot_interface_streaming_mh5">
+    <group ns="dx200" >
+      <include file="$(find motoman_mh5_support)/launch/robot_interface_streaming_mh5.launch">
+        <arg name="robot_ip"   value="$(arg req_arg)" />
+        <arg name="controller" value="$(arg dx200)"/>
+      </include>
+    </group>
+
+    <group ns="dx100" >
+      <include file="$(find motoman_mh5_support)/launch/robot_interface_streaming_mh5.launch">
+        <arg name="robot_ip"   value="$(arg req_arg)" />
+        <arg name="controller" value="$(arg dx100)"/>
+      </include>
+    </group>
+
+    <group ns="fs100" >
+      <include file="$(find motoman_mh5_support)/launch/robot_interface_streaming_mh5.launch">
+        <arg name="robot_ip"   value="$(arg req_arg)" />
+        <arg name="controller" value="$(arg fs100)"/>
+      </include>
+    </group>
+  </group>
+
+  <group ns="robot_state_visualize_mh5">
+    <group ns="dx200" >
+      <include file="$(find motoman_mh5_support)/launch/robot_state_visualize_mh5.launch">
+        <arg name="robot_ip"   value="$(arg req_arg)" />
+        <arg name="controller" value="$(arg dx200)"/>
+      </include>
+    </group>
+
+    <group ns="robot_state_visualize_mh5">
+      <group ns="dx100" >
+        <include file="$(find motoman_mh5_support)/launch/robot_state_visualize_mh5.launch">
+          <arg name="robot_ip"   value="$(arg req_arg)" />
+          <arg name="controller" value="$(arg dx100)"/>
+        </include>
+      </group>
+    </group>
+
+    <group ns="fs100" >
+      <include file="$(find motoman_mh5_support)/launch/robot_state_visualize_mh5.launch">
+        <arg name="robot_ip"   value="$(arg req_arg)" />
+        <arg name="controller" value="$(arg fs100)"/>
+      </include>
+    </group>
+  </group>
+
+</launch>
diff --git a/motoman_mh5_support/test/launch_test_mh5l.xml b/motoman_mh5_support/test/launch_test_mh5l.xml
new file mode 100644
index 00000000..88f29f65
--- /dev/null
+++ b/motoman_mh5_support/test/launch_test_mh5l.xml
@@ -0,0 +1,64 @@
+<!-- launch_test_mh5l.xml - ROSlaunch tests -->
+<launch>
+  <arg name="req_arg" value="default"/>
+  <arg name="dx200" value="dx200"/>
+  <arg name="dx100" value="dx100"/>
+  <arg name="fs100" value="fs100" />
+
+  <group ns="load_mh5l">
+    <include file="$(find motoman_mh5_support)/launch/load_mh5l.launch"/>
+  </group>
+
+  <group ns="test_mh5l">
+    <include file="$(find motoman_mh5_support)/launch/test_mh5l.launch"/>
+  </group>
+
+  <group ns="robot_interface_streaming_mh5l">
+    <group ns="dx200" >
+      <include file="$(find motoman_mh5_support)/launch/robot_interface_streaming_mh5l.launch">
+        <arg name="robot_ip"   value="$(arg req_arg)" />
+        <arg name="controller" value="$(arg dx200)"/>
+      </include>
+    </group>
+
+    <group ns="dx100" >
+      <include file="$(find motoman_mh5_support)/launch/robot_interface_streaming_mh5l.launch">
+        <arg name="robot_ip"   value="$(arg req_arg)" />
+        <arg name="controller" value="$(arg dx100)"/>
+      </include>
+    </group>
+
+    <group ns="fs100" >
+      <include file="$(find motoman_mh5_support)/launch/robot_interface_streaming_mh5l.launch">
+        <arg name="robot_ip"   value="$(arg req_arg)" />
+        <arg name="controller" value="$(arg fs100)"/>
+      </include>
+    </group>
+  </group>
+
+  <group ns="robot_state_visualize_mh5l">
+    <group ns="dx200" >
+      <include file="$(find motoman_mh5_support)/launch/robot_state_visualize_mh5l.launch">
+        <arg name="robot_ip"   value="$(arg req_arg)" />
+        <arg name="controller" value="$(arg dx200)"/>
+      </include>
+    </group>
+
+    <group ns="robot_state_visualize_mh5l">
+      <group ns="dx100" >
+        <include file="$(find motoman_mh5_support)/launch/robot_state_visualize_mh5l.launch">
+          <arg name="robot_ip"   value="$(arg req_arg)" />
+          <arg name="controller" value="$(arg dx100)"/>
+        </include>
+      </group>
+    </group>
+
+    <group ns="fs100" >
+      <include file="$(find motoman_mh5_support)/launch/robot_state_visualize_mh5l.launch">
+        <arg name="robot_ip"   value="$(arg req_arg)" />
+        <arg name="controller" value="$(arg fs100)"/>
+      </include>
+    </group>
+  </group>
+
+</launch>
diff --git a/motoman_mh5_support/urdf/mh5.urdf b/motoman_mh5_support/urdf/mh5.urdf
deleted file mode 100644
index 6d7f8dcd..00000000
--- a/motoman_mh5_support/urdf/mh5.urdf
+++ /dev/null
@@ -1,172 +0,0 @@
-<?xml version="1.0" ?>
-<!-- =================================================================================== -->
-<!-- |    This document was autogenerated by xacro from mh5.xacro                      | -->
-<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
-<!-- =================================================================================== -->
-<robot name="motoman_mh5" xmlns:xacro="http://ros.org/wiki/xacro">
-  <!-- link list -->
-  <link name="base_link">
-    <visual>
-      <origin rpy="0 0 3.14159"/>
-      <geometry>
-        <mesh filename="package://motoman_mh5_support/meshes/mh5/visual/MH5_BASE_AXIS.stl"/>
-      </geometry>
-      <material name="blue">
-        <color rgba="0 0 1 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin rpy="0 0 3.14159"/>
-      <geometry>
-        <mesh filename="package://motoman_mh5_support/meshes/mh5/collision/MH5_BASE_AXIS.stl"/>
-      </geometry>
-      <material name="yellow">
-        <color rgba="0 1 1 1"/>
-      </material>
-    </collision>
-  </link>
-  <link name="link_s">
-    <visual>
-      <geometry>
-        <mesh filename="package://motoman_mh5_support/meshes/mh5/visual/MH5_S_AXIS.stl"/>
-      </geometry>
-      <material name="silver">
-        <color rgba="0.8 0.8 0.8 1"/>
-      </material>
-    </visual>
-    <collision>
-      <geometry>
-        <mesh filename="package://motoman_mh5_support/meshes/mh5/collision/MH5_S_AXIS.stl"/>
-      </geometry>
-      <material name="yellow"/>
-    </collision>
-  </link>
-  <link name="link_l">
-    <visual>
-      <!-- <origin rpy="1.57 3.1416 0" xyz="0 0 0"/> -->
-      <geometry>
-        <mesh filename="package://motoman_mh5_support/meshes/mh5/visual/MH5_L_AXIS.stl"/>
-      </geometry>
-      <material name="blue"/>
-    </visual>
-    <collision>
-      <!-- <origin rpy="1.57 3.1416 0" xyz="0 0 0"/> -->
-      <geometry>
-        <mesh filename="package://motoman_mh5_support/meshes/mh5/collision/MH5_L_AXIS.stl"/>
-      </geometry>
-      <material name="yellow"/>
-    </collision>
-  </link>
-  <link name="link_u">
-    <visual>
-      <!-- <origin rpy="1.57 -3.1415 0" xyz="0 0 0"/> -->
-      <geometry>
-        <mesh filename="package://motoman_mh5_support/meshes/mh5/visual/MH5_U_AXIS.stl"/>
-      </geometry>
-      <material name="blue"/>
-    </visual>
-    <collision>
-      <!-- <origin rpy="1.57 -3.1415 0" xyz="0 0 0"/> -->
-      <geometry>
-        <mesh filename="package://motoman_mh5_support/meshes/mh5/collision/MH5_U_AXIS.stl"/>
-      </geometry>
-      <material name="yellow"/>
-    </collision>
-  </link>
-  <link name="link_r">
-    <visual>
-      <!-- <origin rpy="0 0 3.1416" xyz="0 0 0"/> -->
-      <geometry>
-        <mesh filename="package://motoman_mh5_support/meshes/mh5/visual/MH5_R_AXIS.stl"/>
-      </geometry>
-      <material name="silver"/>
-    </visual>
-    <collision>
-      <!-- <origin rpy="0 0 3.1416" xyz="0 0 0"/> -->
-      <geometry>
-        <mesh filename="package://motoman_mh5_support/meshes/mh5/collision/MH5_R_AXIS.stl"/>
-      </geometry>
-      <material name="yellow"/>
-    </collision>
-  </link>
-  <link name="link_b">
-    <visual>
-      <!-- <origin rpy="-1.57 0 3.1416" xyz="0 0 0"/> -->
-      <geometry>
-        <mesh filename="package://motoman_mh5_support/meshes/mh5/visual/MH5_B_AXIS.stl"/>
-      </geometry>
-      <material name="blue"/>
-    </visual>
-    <collision>
-      <!-- <origin rpy="-1.57 0 3.1416" xyz="0 0 0"/> -->
-      <geometry>
-        <mesh filename="package://motoman_mh5_support/meshes/mh5/collision/MH5_B_AXIS.stl"/>
-      </geometry>
-      <material name="yellow"/>
-    </collision>
-  </link>
-  <link name="link_t">
-    <visual>
-      <geometry>
-        <mesh filename="package://motoman_mh5_support/meshes/mh5/visual/MH5_T_AXIS.stl"/>
-      </geometry>
-      <material name="silver"/>
-    </visual>
-    <collision>
-      <geometry>
-        <mesh filename="package://motoman_mh5_support/meshes/mh5/collision/MH5_T_AXIS.stl"/>
-      </geometry>
-      <material name="yellow"/>
-    </collision>
-  </link>
-  <!-- end of link list -->
-  <!-- joint list -->
-  <joint name="joint_s" type="revolute">
-    <parent link="base_link"/>
-    <child link="link_s"/>
-    <origin rpy="0 0 0" xyz="0 0 0.1990"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="100" lower="-2.9671" upper="2.9671" velocity="2.96"/>
-  </joint>
-  <joint name="joint_l" type="revolute">
-    <parent link="link_s"/>
-    <child link="link_l"/>
-    <origin rpy="0 0 0" xyz="0.0880 0 0.1310"/>
-    <axis xyz="0 1 0"/>
-    <limit effort="100" lower="-1.1345" upper="2.6180" velocity="2.96"/>
-  </joint>
-  <joint name="joint_u" type="revolute">
-    <parent link="link_l"/>
-    <child link="link_u"/>
-    <origin rpy="0 0 0" xyz="0 0 0.3100"/>
-    <axis xyz="0 -1 0"/>
-    <!-- Old limits: lower="-2.3736" upper="4.4506" The new limits
-           below were measured from an MH5F.  They ensure that link_r
-           won't hit link_l even when joint_r is at 45 degrees.  The
-           old limits were converted directly from the MH5 data sheet,
-           which was clearly way off. -->
-    <limit effort="100" lower="-1.0122" upper="3.2218" velocity="2.96"/>
-  </joint>
-  <joint name="joint_r" type="revolute">
-    <parent link="link_u"/>
-    <child link="link_r"/>
-    <origin rpy="0 0 0" xyz="0.0715 0 0.0399"/>
-    <axis xyz="-1 0 0"/>
-    <limit effort="100" lower="-3.3161" upper="3.3161" velocity="3.48"/>
-  </joint>
-  <joint name="joint_b" type="revolute">
-    <parent link="link_r"/>
-    <child link="link_b"/>
-    <origin rpy="0 0 0" xyz="0.2335 0 0"/>
-    <axis xyz="0 -1 0"/>
-    <limit effort="100" lower="-2.1817" upper="2.1817" velocity="3.48"/>
-  </joint>
-  <joint name="joint_t" type="revolute">
-    <parent link="link_b"/>
-    <child link="link_t"/>
-    <origin rpy="0 0 0" xyz="0.0865 0 0"/>
-    <axis xyz="-1 0 0"/>
-    <limit effort="100" lower="-6.2831" upper="6.2831" velocity="6.97"/>
-  </joint>
-  <!-- end of joint list -->
-</robot>
diff --git a/motoman_mh5_support/urdf/mh5.xacro b/motoman_mh5_support/urdf/mh5.xacro
index 850f7226..5efc7d55 100644
--- a/motoman_mh5_support/urdf/mh5.xacro
+++ b/motoman_mh5_support/urdf/mh5.xacro
@@ -1,8 +1,5 @@
 <?xml version="1.0" ?>
-
 <robot name="motoman_mh5" xmlns:xacro="http://ros.org/wiki/xacro">
-	<xacro:include filename="$(find motoman_mh5_support)/urdf/mh5_macro.xacro" />
-	<xacro:motoman_mh5 prefix=""/>
+  <xacro:include filename="$(find motoman_mh5_support)/urdf/mh5_macro.xacro" />
+  <xacro:motoman_mh5 prefix=""/>
 </robot>
-
-
diff --git a/motoman_mh5_support/urdf/mh5_macro.xacro b/motoman_mh5_support/urdf/mh5_macro.xacro
index d4d2700a..fb850bc8 100644
--- a/motoman_mh5_support/urdf/mh5_macro.xacro
+++ b/motoman_mh5_support/urdf/mh5_macro.xacro
@@ -1,167 +1,186 @@
 <?xml version="1.0" ?>
 <robot xmlns:xacro="http://ros.org/wiki/xacro">
-	<xacro:macro name="motoman_mh5" params="prefix">
-		<xacro:include filename="$(find motoman_resources)/urdf/common_materials.xacro"/>
+  <xacro:macro name="motoman_mh5" params="prefix">
+    <xacro:include filename="$(find motoman_resources)/urdf/common_materials.xacro"/>
 
-		<!-- link list -->
-		<link name="${prefix}base_link">
-			<visual>
-			        <origin rpy="0 0 3.14159"/>
-				<geometry>
-					<mesh filename="package://motoman_mh5_support/meshes/mh5/visual/MH5_BASE_AXIS.stl" />
-				</geometry>
-				<xacro:material_yaskawa_blue/>
-			</visual>
-			<collision>
-			        <origin rpy="0 0 3.14159"/>
-				<geometry>
-					<mesh filename="package://motoman_mh5_support/meshes/mh5/collision/MH5_BASE_AXIS.stl" />
-				</geometry>
-				<xacro:material_yaskawa_yellow/>
-			</collision>
-		</link>
-		<link name="${prefix}link_s">
-			<visual>
-				<geometry>
-					<mesh filename="package://motoman_mh5_support/meshes/mh5/visual/MH5_S_AXIS.stl"/>
-				</geometry>
-				<xacro:material_yaskawa_white/>
-			</visual>
-			<collision>
-				<geometry>
-					<mesh filename="package://motoman_mh5_support/meshes/mh5/collision/MH5_S_AXIS.stl" />
-				</geometry>
-				<xacro:material_yaskawa_yellow/>
-			</collision>
-		</link>
-		<link name="${prefix}link_l">
-			<visual>
-				<!-- <origin rpy="1.57 3.1416 0" xyz="0 0 0"/> -->
-				<geometry>
-					<mesh filename="package://motoman_mh5_support/meshes/mh5/visual/MH5_L_AXIS.stl"/>
-				</geometry>
-				<xacro:material_yaskawa_blue/>
-			</visual>
-			<collision>
-				<!-- <origin rpy="1.57 3.1416 0" xyz="0 0 0"/> -->
-				<geometry>
-					<mesh filename="package://motoman_mh5_support/meshes/mh5/collision/MH5_L_AXIS.stl" />
-				</geometry>
-				<xacro:material_yaskawa_yellow/>
-			</collision>
-		</link>
-		<link name="${prefix}link_u">
-			<visual>
-				<!-- <origin rpy="1.57 -3.1415 0" xyz="0 0 0"/> -->
-				<geometry>
-					<mesh filename="package://motoman_mh5_support/meshes/mh5/visual/MH5_U_AXIS.stl"/>
-				</geometry>
-				<xacro:material_yaskawa_blue/>
-			</visual>
-			<collision>
-				<!-- <origin rpy="1.57 -3.1415 0" xyz="0 0 0"/> -->
-				<geometry>
-					<mesh filename="package://motoman_mh5_support/meshes/mh5/collision/MH5_U_AXIS.stl" />
-				</geometry>
-				<xacro:material_yaskawa_yellow/>
-			</collision>
-		</link>
-		<link name="${prefix}link_r">
-			<visual>
-				<!-- <origin rpy="0 0 3.1416" xyz="0 0 0"/> -->
-				<geometry>
-					<mesh filename="package://motoman_mh5_support/meshes/mh5/visual/MH5_R_AXIS.stl"/>
-				</geometry>
-				<xacro:material_yaskawa_white/>
-			</visual>
-			<collision>
-				<!-- <origin rpy="0 0 3.1416" xyz="0 0 0"/> -->
-				<geometry>
-					<mesh filename="package://motoman_mh5_support/meshes/mh5/collision/MH5_R_AXIS.stl" />
-				</geometry>
-				<xacro:material_yaskawa_yellow/>
-			</collision>
-		</link>
-		<link name="${prefix}link_b">
-			<visual>
-				<!-- <origin rpy="-1.57 0 3.1416" xyz="0 0 0"/> -->
-				<geometry>
-					<mesh filename="package://motoman_mh5_support/meshes/mh5/visual/MH5_B_AXIS.stl"/>
-				</geometry>
-				<xacro:material_yaskawa_blue/>
-			</visual>
-			<collision>
-				<!-- <origin rpy="-1.57 0 3.1416" xyz="0 0 0"/> -->
-				<geometry>
-					<mesh filename="package://motoman_mh5_support/meshes/mh5/collision/MH5_B_AXIS.stl" />
-				</geometry>
-				<xacro:material_yaskawa_yellow/>
-			</collision>
-		</link>
-		<link name="${prefix}link_t">
-			<visual>
-				<geometry>
-					<mesh filename="package://motoman_mh5_support/meshes/mh5/visual/MH5_T_AXIS.stl" />
-				</geometry>
-				<xacro:material_yaskawa_white/>
-			</visual>
-			<collision>
-				<geometry>
-					<mesh filename="package://motoman_mh5_support/meshes/mh5/collision/MH5_T_AXIS.stl" />
-				</geometry>
-				<xacro:material_yaskawa_yellow/>
-			</collision>
-		</link>
-		<!-- end of link list -->
-		<!-- joint list -->
-		<joint name="${prefix}joint_s" type="revolute">
-			<parent link="${prefix}base_link"/>
-			<child link="${prefix}link_s"/>
-			<origin xyz="0 0 0.1990" rpy="0 0 0"/>
-			<axis xyz="0 0 1" />
-			<limit lower="-2.9671" upper="2.9671" effort="100" velocity="2.96" />
-		</joint>
-		<joint name="${prefix}joint_l" type="revolute">
-			<parent link="${prefix}link_s"/>
-			<child link="${prefix}link_l"/>
-			<origin xyz="0.0880 0 0.1310" rpy="0 0 0"/>
-			<axis xyz="0 1 0" />
-			<limit lower="-1.1345" upper="2.6180" effort="100" velocity="2.96" />
-		</joint>
-		<joint name="${prefix}joint_u" type="revolute">
-			<parent link="${prefix}link_l"/>
-			<child link="${prefix}link_u"/>
-			<origin xyz="0 0 0.3100" rpy="0 0 0"/>
-			<axis xyz="0 -1 0" />
-      <!-- Old limits: lower="-2.3736" upper="4.4506" The new limits
-           below were measured from an MH5F.  They ensure that link_r
-           won't hit link_l even when joint_r is at 45 degrees.  The
-           old limits were converted directly from the MH5 data sheet,
-           which was clearly way off. -->
-			<limit lower="-1.0122" upper="3.2218" effort="100" velocity="2.96" />
-		</joint>
-		<joint name="${prefix}joint_r" type="revolute">
-			<parent link="${prefix}link_u"/>
-			<child link="${prefix}link_r"/>
-			<origin xyz="0.0715 0 0.0399" rpy="0 0 0"/>
-			<axis xyz="-1 0 0" />
-			<limit lower="-3.3161" upper="3.3161" effort="100" velocity="3.48" />
-		</joint>
-		<joint name="${prefix}joint_b" type="revolute">
-			<parent link="${prefix}link_r"/>
-			<child link="${prefix}link_b"/>
-			<origin xyz="0.2335 0 0" rpy="0 0 0"/>
-			<axis xyz="0 -1 0" />
-			<limit lower="-2.1817" upper="2.1817" effort="100" velocity="3.48" />
-		</joint>
-		<joint name="${prefix}joint_t" type="revolute">
-			<parent link="${prefix}link_b"/>
-			<child link="${prefix}link_t"/>
-			<origin xyz="0.0865 0 0" rpy="0 0 0"/>
-			<axis xyz="-1 0 0" />
-			<limit lower="-6.2831" upper="6.2831" effort="100" velocity="6.97" />
-		</joint>
-		<!-- end of joint list -->
-	</xacro:macro>
-</robot>
+    <!-- link list -->
+    <link name="${prefix}base_link">
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://motoman_mh5_support/meshes/mh5/visual/base_link.stl" />
+        </geometry>
+        <xacro:material_yaskawa_white/>
+      </visual>
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://motoman_mh5_support/meshes/mh5/collision/base_link.stl" />
+        </geometry>
+      </collision>
+    </link>
+    <link name="${prefix}link_1_s">
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://motoman_mh5_support/meshes/mh5/visual/link_1_s.stl"/>
+        </geometry>
+        <xacro:material_yaskawa_white/>
+      </visual>
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://motoman_mh5_support/meshes/mh5/collision/link_1_s.stl" />
+        </geometry>
+      </collision>
+    </link>
+    <link name="${prefix}link_2_l">
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://motoman_mh5_support/meshes/mh5/visual/link_2_l.stl"/>
+        </geometry>
+        <xacro:material_yaskawa_white/>
+      </visual>
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://motoman_mh5_support/meshes/mh5/collision/link_2_l.stl" />
+        </geometry>
+      </collision>
+    </link>
+    <link name="${prefix}link_3_u">
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://motoman_mh5_support/meshes/mh5/visual/link_3_u.stl"/>
+          <!-- <mesh filename="package://motoman_mh5_support/meshes/mh5_old_variants/visual/link_3_u.stl"/> --><!-- Uncomment & switch with above to use alternative U-link. -->
+        </geometry>
+        <xacro:material_yaskawa_white/>
+      </visual>
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://motoman_mh5_support/meshes/mh5/collision/link_3_u.stl" />
+          <!-- <mesh filename="package://motoman_mh5_support/meshes/mh5_old_variants/collision/link_3_u.stl" /> --><!-- Uncomment & switch with above to use alternative U-link. -->
+        </geometry>
+      </collision>
+    </link>
+    <link name="${prefix}link_4_r">
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://motoman_mh5_support/meshes/mh5/visual/link_4_r.stl"/>
+        </geometry>
+        <xacro:material_yaskawa_white/>
+      </visual>
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://motoman_mh5_support/meshes/mh5/collision/link_4_r.stl" />
+        </geometry>
+      </collision>
+    </link>
+    <link name="${prefix}link_5_b">
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://motoman_mh5_support/meshes/mh5/visual/link_5_b.stl"/>
+        </geometry>
+        <xacro:material_yaskawa_white/>
+      </visual>
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://motoman_mh5_support/meshes/mh5/collision/link_5_b.stl" />
+        </geometry>
+      </collision>
+    </link>
+    <link name="${prefix}link_6_t">
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://motoman_mh5_support/meshes/mh5/visual/link_6_t.stl" />
+        </geometry>
+        <xacro:material_yaskawa_white/>
+      </visual>
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://motoman_mh5_support/meshes/mh5/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.330" rpy="0 0 0"/>
+      <axis xyz="0 0 1" />
+      <limit lower="-${radians(170)}" upper="${radians(170)}" effort="215.6" velocity="${radians(376)}" />
+    </joint>
+    <joint name="${prefix}joint_2_l" type="revolute">
+      <parent link="${prefix}link_1_s"/>
+      <child link="${prefix}link_2_l"/>
+      <origin xyz="0.0880 0 0.00" rpy="0 0 0"/>
+      <axis xyz="0 1 0" />
+      <limit lower="-${radians(65)}" upper="${radians(150)}" effort="176.4" velocity="${radians(350)}" />
+    </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 0.310" rpy="0 0 0"/>
+      <axis xyz="0 -1 0" />
+      <limit lower="-${radians(70)}" upper="${radians(190)}" effort="76.8" velocity="${radians(400)}" />
+    </joint>
+    <joint name="${prefix}joint_4_r" type="revolute">
+      <parent link="${prefix}link_3_u"/>
+      <child link="${prefix}link_4_r"/>
+      <origin xyz="0 0 0.040" rpy="0 0 0"/>
+      <axis xyz="-1 0 0" />
+      <limit lower="-${radians(190)}" upper="${radians(190)}" effort="44.7" velocity="${radians(450)}" />
+    </joint>
+    <joint name="${prefix}joint_5_b" type="revolute">
+      <parent link="${prefix}link_4_r"/>
+      <child link="${prefix}link_5_b"/>
+      <origin xyz="0.305 0 0" rpy="0 0 0"/>
+      <axis xyz="0 -1 0" />
+      <limit lower="-${radians(135)}" upper="${radians(135)}" effort="18.0" velocity="${radians(450)}" />
+    </joint>
+    <joint name="${prefix}joint_6_t" type="revolute">
+      <parent link="${prefix}link_5_b"/>
+      <child link="${prefix}link_6_t"/>
+      <origin xyz="0.080 0 0" rpy="0 0 0"/>
+      <axis xyz="-1 0 0" />
+      <limit lower="-${radians(360)}" upper="${radians(360)}" effort="18.0" velocity="${radians(720)}" />
+    </joint>
+    <!-- end of joint list -->
+
+    <!-- ROS base_link to Robot Manufacturer (Motoman) World Coordinates transform -->
+    <link name="${prefix}base" />
+    <joint name="${prefix}base_link-base" type="fixed">
+        <parent link="${prefix}base_link"/>
+        <child link="${prefix}base"/>
+        <origin xyz="0 0 0.330" rpy="0 0 0"/>
+    </joint>
 
+    <!-- ROS-Industrial 'flange' frame: attachment point for EEF models -->
+    <link name="${prefix}flange" />
+    <joint name="${prefix}joint_6_t-flange" type="fixed">
+      <parent link="${prefix}link_6_t" />
+      <child link="${prefix}flange" />
+      <origin xyz="0 0 0" rpy="0 0 0" />
+    </joint>
+
+    <!-- ROS-Industrial 'tool0' frame: all-zeros tool frame -->
+    <link name="${prefix}tool0"/>
+    <joint name="${prefix}joint_6_t-tool0" type="fixed">
+        <parent link="${prefix}link_6_t"/>
+        <child link="${prefix}tool0"/>
+        <origin xyz="0 0 0" rpy="${radians(180)} ${radians(-90)} 0"/>
+    </joint>
+
+  </xacro:macro>
+</robot>
diff --git a/motoman_mh5_support/urdf/mh5l.xacro b/motoman_mh5_support/urdf/mh5l.xacro
new file mode 100644
index 00000000..a3196336
--- /dev/null
+++ b/motoman_mh5_support/urdf/mh5l.xacro
@@ -0,0 +1,5 @@
+<?xml version="1.0" ?>
+<robot name="motoman_mh5l" xmlns:xacro="http://ros.org/wiki/xacro">
+  <xacro:include filename="$(find motoman_mh5_support)/urdf/mh5l_macro.xacro" />
+  <xacro:motoman_mh5l prefix=""/>
+</robot>
diff --git a/motoman_mh5_support/urdf/mh5l_macro.xacro b/motoman_mh5_support/urdf/mh5l_macro.xacro
new file mode 100644
index 00000000..b9b08713
--- /dev/null
+++ b/motoman_mh5_support/urdf/mh5l_macro.xacro
@@ -0,0 +1,184 @@
+<?xml version="1.0" ?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+  <xacro:macro name="motoman_mh5l" params="prefix">
+    <xacro:include filename="$(find motoman_resources)/urdf/common_materials.xacro"/>
+
+    <!-- link list -->
+    <link name="${prefix}base_link">
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://motoman_mh5_support/meshes/mh5/visual/base_link.stl" />
+        </geometry>
+        <xacro:material_yaskawa_white/>
+      </visual>
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://motoman_mh5_support/meshes/mh5/collision/base_link.stl" />
+        </geometry>
+      </collision>
+    </link>
+    <link name="${prefix}link_1_s">
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://motoman_mh5_support/meshes/mh5/visual/link_1_s.stl"/>
+        </geometry>
+        <xacro:material_yaskawa_white/>
+      </visual>
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://motoman_mh5_support/meshes/mh5/collision/link_1_s.stl" />
+        </geometry>
+      </collision>
+    </link>
+    <link name="${prefix}link_2_l">
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://motoman_mh5_support/meshes/mh5l/visual/link_2_l.stl"/>
+        </geometry>
+        <xacro:material_yaskawa_white/>
+      </visual>
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://motoman_mh5_support/meshes/mh5l/collision/link_2_l.stl" />
+        </geometry>
+      </collision>
+    </link>
+    <link name="${prefix}link_3_u">
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://motoman_mh5_support/meshes/mh5/visual/link_3_u.stl"/>
+        </geometry>
+        <xacro:material_yaskawa_white/>
+      </visual>
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://motoman_mh5_support/meshes/mh5/collision/link_3_u.stl" />
+        </geometry>
+      </collision>
+    </link>
+    <link name="${prefix}link_4_r">
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://motoman_mh5_support/meshes/mh5l/visual/link_4_r.stl"/>
+        </geometry>
+        <xacro:material_yaskawa_white/>
+      </visual>
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://motoman_mh5_support/meshes/mh5l/collision/link_4_r.stl" />
+        </geometry>
+      </collision>
+    </link>
+    <link name="${prefix}link_5_b">
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://motoman_mh5_support/meshes/mh5/visual/link_5_b.stl"/>
+        </geometry>
+        <xacro:material_yaskawa_white/>
+      </visual>
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://motoman_mh5_support/meshes/mh5/collision/link_5_b.stl" />
+        </geometry>
+      </collision>
+    </link>
+    <link name="${prefix}link_6_t">
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://motoman_mh5_support/meshes/mh5/visual/link_6_t.stl" />
+        </geometry>
+        <xacro:material_yaskawa_white/>
+      </visual>
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://motoman_mh5_support/meshes/mh5/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.330" rpy="0 0 0"/>
+      <axis xyz="0 0 1" />
+      <limit lower="-${radians(170)}" upper="${radians(170)}" effort="303.8" velocity="${radians(270)}" />
+    </joint>
+    <joint name="${prefix}joint_2_l" type="revolute">
+      <parent link="${prefix}link_1_s"/>
+      <child link="${prefix}link_2_l"/>
+      <origin xyz="0.0880 0 0.00" rpy="0 0 0"/>
+      <axis xyz="0 1 0" />
+      <limit lower="-${radians(65)}" upper="${radians(150)}" effort="205.8" velocity="${radians(280)}" />
+    </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 0.400" rpy="0 0 0"/>
+      <axis xyz="0 -1 0" />
+      <limit lower="-${radians(70)}" upper="${radians(190)}" effort="85.5" velocity="${radians(300)}" />
+    </joint>
+    <joint name="${prefix}joint_4_r" type="revolute">
+      <parent link="${prefix}link_3_u"/>
+      <child link="${prefix}link_4_r"/>
+      <origin xyz="0 0 0.040" rpy="0 0 0"/>
+      <axis xyz="-1 0 0" />
+      <limit lower="-${radians(190)}" upper="${radians(190)}" effort="44.7" velocity="${radians(450)}" />
+    </joint>
+    <joint name="${prefix}joint_5_b" type="revolute">
+      <parent link="${prefix}link_4_r"/>
+      <child link="${prefix}link_5_b"/>
+      <origin xyz="0.405 0 0" rpy="0 0 0"/>
+      <axis xyz="0 -1 0" />
+      <limit lower="-${radians(135)}" upper="${radians(135)}" effort="18.0" velocity="${radians(450)}" />
+    </joint>
+    <joint name="${prefix}joint_6_t" type="revolute">
+      <parent link="${prefix}link_5_b"/>
+      <child link="${prefix}link_6_t"/>
+      <origin xyz="0.080 0 0" rpy="0 0 0"/>
+      <axis xyz="-1 0 0" />
+      <limit lower="-${radians(360)}" upper="${radians(360)}" effort="18.0" velocity="${radians(720)}" />
+    </joint>
+    <!-- end of joint list -->
+
+    <!-- ROS base_link to Robot Manufacturer (Motoman) World Coordinates transform -->
+    <link name="${prefix}base" />
+    <joint name="${prefix}base_link-base" type="fixed">
+        <parent link="${prefix}base_link"/>
+        <child link="${prefix}base"/>
+        <origin xyz="0 0 0.330" rpy="0 0 0"/>
+    </joint>
+
+    <!-- ROS-Industrial 'flange' frame: attachment point for EEF models -->
+    <link name="${prefix}flange" />
+    <joint name="${prefix}joint_6_t-flange" type="fixed">
+      <parent link="${prefix}link_6_t" />
+      <child link="${prefix}flange" />
+      <origin xyz="0 0 0" rpy="0 0 0" />
+    </joint>
+
+    <!-- ROS-Industrial 'tool0' frame: all-zeros tool frame -->
+    <link name="${prefix}tool0"/>
+    <joint name="${prefix}joint_6_t-tool0" type="fixed">
+        <parent link="${prefix}link_6_t"/>
+        <child link="${prefix}tool0"/>
+        <origin xyz="0 0 0" rpy="${radians(180)} ${radians(-90)} 0"/>
+    </joint>
+
+  </xacro:macro>
+</robot>