From 34000a7e10594079713c789d528e9810cdbeda02 Mon Sep 17 00:00:00 2001
From: Felix Mauch <mauch@fzi.de>
Date: Thu, 4 May 2017 19:05:17 +0200
Subject: [PATCH] added support for R900

---
 .../config/joint_names_kr10r900sixx.yaml        |  1 +
 .../launch/load_kr10r900sixx.launch             |  4 ++++
 .../launch/test_kr10r900sixx.launch             |  9 +++++++++
 kuka_kr10_support/package.xml                   |  3 ++-
 kuka_kr10_support/urdf/kr10r900sixx.xacro       |  5 +++++
 kuka_kr6_support/launch/load_kr6r900sixx.launch |  2 +-
 kuka_kr6_support/urdf/kr6r900sixx.xacro         |  4 ++--
 ...sixx_macro.xacro => krXr900sixx_macro.xacro} | 17 ++++++++---------
 8 files changed, 32 insertions(+), 13 deletions(-)
 create mode 100644 kuka_kr10_support/config/joint_names_kr10r900sixx.yaml
 create mode 100644 kuka_kr10_support/launch/load_kr10r900sixx.launch
 create mode 100644 kuka_kr10_support/launch/test_kr10r900sixx.launch
 create mode 100644 kuka_kr10_support/urdf/kr10r900sixx.xacro
 rename kuka_kr6_support/urdf/{kr6r900sixx_macro.xacro => krXr900sixx_macro.xacro} (87%)

diff --git a/kuka_kr10_support/config/joint_names_kr10r900sixx.yaml b/kuka_kr10_support/config/joint_names_kr10r900sixx.yaml
new file mode 100644
index 000000000..1a23ced85
--- /dev/null
+++ b/kuka_kr10_support/config/joint_names_kr10r900sixx.yaml
@@ -0,0 +1 @@
+controller_joint_names: ['joint_a1', 'joint_a2', 'joint_a3', 'joint_a4', 'joint_a5', 'joint_a6']
diff --git a/kuka_kr10_support/launch/load_kr10r900sixx.launch b/kuka_kr10_support/launch/load_kr10r900sixx.launch
new file mode 100644
index 000000000..5f73a202e
--- /dev/null
+++ b/kuka_kr10_support/launch/load_kr10r900sixx.launch
@@ -0,0 +1,4 @@
+<?xml version="1.0"?>
+<launch>
+  <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find kuka_kr10_support)/urdf/kr10r900sixx.xacro'"/>
+</launch>
diff --git a/kuka_kr10_support/launch/test_kr10r900sixx.launch b/kuka_kr10_support/launch/test_kr10r900sixx.launch
new file mode 100644
index 000000000..4f6fbdffa
--- /dev/null
+++ b/kuka_kr10_support/launch/test_kr10r900sixx.launch
@@ -0,0 +1,9 @@
+<?xml version="1.0"?>
+<launch>
+  <include file="$(find kuka_kr10_support)/launch/load_kr10r900sixx.launch" />
+  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
+    <param name="use_gui" value="true" />
+  </node>
+  <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/kuka_kr10_support/package.xml b/kuka_kr10_support/package.xml
index 464108fe7..97b1b32e2 100644
--- a/kuka_kr10_support/package.xml
+++ b/kuka_kr10_support/package.xml
@@ -8,10 +8,11 @@
     </p>
     <p>
       This package contains configuration data, 3D models and launch files
-      for KUKA KR 10 manipulators. This currently includes the R1100 sixx only.
+      for KUKA KR 10 manipulators. This currently includes the R900 sixx and R1100 sixx only.
     </p>
     <p><b>Specifications</b>:</p>
     <ul>
+      <li>KR 10 R900 sixx - Default</li>
       <li>KR 10 R1100 sixx - Default</li>
     </ul>
     <p>
diff --git a/kuka_kr10_support/urdf/kr10r900sixx.xacro b/kuka_kr10_support/urdf/kr10r900sixx.xacro
new file mode 100644
index 000000000..c19296f5a
--- /dev/null
+++ b/kuka_kr10_support/urdf/kr10r900sixx.xacro
@@ -0,0 +1,5 @@
+<?xml version="1.0"?>
+<robot name="kuka_kr10r900sixx" xmlns:xacro="http://wiki.ros.org/xacro">
+  <xacro:include filename="$(find kuka_kr6_support)/urdf/krXr900sixx_macro.xacro" />
+  <xacro:kuka_krXr900sixx prefix=""/>
+</robot>
diff --git a/kuka_kr6_support/launch/load_kr6r900sixx.launch b/kuka_kr6_support/launch/load_kr6r900sixx.launch
index 7c276c7dc..b3658fc9c 100644
--- a/kuka_kr6_support/launch/load_kr6r900sixx.launch
+++ b/kuka_kr6_support/launch/load_kr6r900sixx.launch
@@ -1,4 +1,4 @@
 <?xml version="1.0"?>
 <launch>
-  <param name="robot_description" command="$(find xacro)/xacro.py '$(find kuka_kr6_support)/urdf/kr6r900sixx.xacro'"/>
+  <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find kuka_kr6_support)/urdf/kr6r900sixx.xacro'"/>
 </launch>
diff --git a/kuka_kr6_support/urdf/kr6r900sixx.xacro b/kuka_kr6_support/urdf/kr6r900sixx.xacro
index bea136db0..f072301bb 100644
--- a/kuka_kr6_support/urdf/kr6r900sixx.xacro
+++ b/kuka_kr6_support/urdf/kr6r900sixx.xacro
@@ -1,5 +1,5 @@
 <?xml version="1.0"?>
 <robot name="kuka_kr6r900sixx" xmlns:xacro="http://wiki.ros.org/xacro">
-  <xacro:include filename="$(find kuka_kr6_support)/urdf/kr6r900sixx_macro.xacro" />
-  <xacro:kuka_kr6r900sixx prefix=""/>
+  <xacro:include filename="$(find kuka_kr6_support)/urdf/krXr900sixx_macro.xacro" />
+  <xacro:kuka_krXr900sixx prefix=""/>
 </robot>
diff --git a/kuka_kr6_support/urdf/kr6r900sixx_macro.xacro b/kuka_kr6_support/urdf/krXr900sixx_macro.xacro
similarity index 87%
rename from kuka_kr6_support/urdf/kr6r900sixx_macro.xacro
rename to kuka_kr6_support/urdf/krXr900sixx_macro.xacro
index e44dd7af4..26a3a64ea 100644
--- a/kuka_kr6_support/urdf/kr6r900sixx_macro.xacro
+++ b/kuka_kr6_support/urdf/krXr900sixx_macro.xacro
@@ -1,9 +1,8 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://wiki.ros.org/xacro">
-  <xacro:include filename="$(find kuka_resources)/urdf/common_constants.xacro"/>
   <xacro:include filename="$(find kuka_resources)/urdf/common_materials.xacro"/>
 
-  <xacro:macro name="kuka_kr6r900sixx" params="prefix">
+  <xacro:macro name="kuka_krXr900sixx" params="prefix">
     <link name="${prefix}base_link">
       <visual>
         <origin xyz="0 0 0" rpy="0 0 0"/>
@@ -117,47 +116,47 @@
       <parent link="${prefix}base_link"/>
       <child link="${prefix}link_1"/>
       <axis xyz="0 0 -1"/>
-      <limit effort="0" lower="${-DEG2RAD * 170}" upper="${DEG2RAD * 170}" velocity="${DEG2RAD * 360}"/>
+      <limit effort="0" lower="${radians(-170)}" upper="${radians(170)}" velocity="${radians(360)}"/>
     </joint>
     <joint name="${prefix}joint_a2" type="revolute">
       <origin xyz="0.025 0 0" rpy="0 0 0"/>
       <parent link="${prefix}link_1"/>
       <child link="${prefix}link_2"/>
       <axis xyz="0 1 0"/>
-      <limit effort="0" lower="${-DEG2RAD * 190}" upper="${DEG2RAD * 45}" velocity="${DEG2RAD * 300}"/>
+      <limit effort="0" lower="${radians(-190)}" upper="${radians(45)}" velocity="${radians(300)}"/>
     </joint>
     <joint name="${prefix}joint_a3" type="revolute">
       <origin xyz="0.455 0 0" rpy="0 0 0"/>
       <parent link="${prefix}link_2"/>
       <child link="${prefix}link_3"/>
       <axis xyz="0 1 0"/>
-      <limit effort="0" lower="${-DEG2RAD * 120}" upper="${DEG2RAD * 156}" velocity="${DEG2RAD * 360}"/>
+      <limit effort="0" lower="${radians(-120)}" upper="${radians(156)}" velocity="${radians(360)}"/>
     </joint>
     <joint name="${prefix}joint_a4" type="revolute">
       <origin xyz="0 0 0.035" rpy="0 0 0"/>
       <parent link="${prefix}link_3"/>
       <child link="${prefix}link_4"/>
       <axis xyz="-1 0 0"/>
-      <limit effort="0" lower="${-DEG2RAD * 185}" upper="${DEG2RAD * 185}" velocity="${DEG2RAD * 381}"/>
+      <limit effort="0" lower="${radians(-185)}" upper="${radians(185)}" velocity="${radians(381)}"/>
     </joint>
     <joint name="${prefix}joint_a5" type="revolute">
       <origin xyz="0.420 0 0" rpy="0 0 0"/>
       <parent link="${prefix}link_4"/>
       <child link="${prefix}link_5"/>
       <axis xyz="0 1 0"/>
-      <limit effort="0" lower="${-DEG2RAD * 120}" upper="${DEG2RAD * 120}" velocity="${DEG2RAD * 388}"/>
+      <limit effort="0" lower="${radians(-120)}" upper="${radians(120)}" velocity="${radians(388)}"/>
     </joint>
     <joint name="${prefix}joint_a6" type="revolute">
       <origin xyz="0.080 0 0" rpy="0 0 0"/>
       <parent link="${prefix}link_5"/>
       <child link="${prefix}link_6"/>
       <axis xyz="-1 0 0"/>
-      <limit effort="0" lower="${-DEG2RAD * 350}" upper="${DEG2RAD * 350}" velocity="${DEG2RAD * 615}"/>
+      <limit effort="0" lower="${radians(-350)}" upper="${radians(350)}" velocity="${radians(615)}"/>
     </joint>
     <joint name="${prefix}joint_a6-tool0" type="fixed">
       <parent link="${prefix}link_6"/>
       <child link="${prefix}tool0"/>
-      <origin xyz="0 0 0" rpy="0 ${DEG2RAD * 90} 0"/>
+      <origin xyz="0 0 0" rpy="0 ${radians(90)} 0"/>
     </joint>
 
     <!-- ROS base_link to KUKA $ROBROOT coordinate system transform -->