From 92bd9ac26c68787be91b6f45a569d2be670589cc Mon Sep 17 00:00:00 2001
From: Chris Schindlbeck <schindlbeck@imr.uni-hannover.de>
Date: Mon, 16 Apr 2018 14:40:14 +0200
Subject: [PATCH 1/7] Gazebo support for KUKA KR10

---
 kuka_kr10_gazebo/CMakeLists.txt               |   9 ++
 kuka_kr10_gazebo/README.md                    |  13 ++
 .../config/joint_state_controller.yaml        |   3 +
 .../config/kr10_arm_controller.yaml           |  21 ++++
 kuka_kr10_gazebo/launch/kr10_control.launch   |  10 ++
 kuka_kr10_gazebo/launch/kr10_gazebo.launch    |  34 ++++++
 kuka_kr10_gazebo/launch/load_kr10.launch      |   4 +
 kuka_kr10_gazebo/launch/spawn_kr10.launch     |  25 ++++
 kuka_kr10_gazebo/package.xml                  |  40 +++++++
 kuka_kr10_gazebo/urdf/kr10.xacro              |  15 +++
 kuka_kr10_gazebo/urdf/kr10_macro.xacro        | 111 ++++++++++++++++++
 11 files changed, 285 insertions(+)
 create mode 100644 kuka_kr10_gazebo/CMakeLists.txt
 create mode 100644 kuka_kr10_gazebo/README.md
 create mode 100644 kuka_kr10_gazebo/config/joint_state_controller.yaml
 create mode 100644 kuka_kr10_gazebo/config/kr10_arm_controller.yaml
 create mode 100644 kuka_kr10_gazebo/launch/kr10_control.launch
 create mode 100644 kuka_kr10_gazebo/launch/kr10_gazebo.launch
 create mode 100644 kuka_kr10_gazebo/launch/load_kr10.launch
 create mode 100644 kuka_kr10_gazebo/launch/spawn_kr10.launch
 create mode 100644 kuka_kr10_gazebo/package.xml
 create mode 100644 kuka_kr10_gazebo/urdf/kr10.xacro
 create mode 100644 kuka_kr10_gazebo/urdf/kr10_macro.xacro

diff --git a/kuka_kr10_gazebo/CMakeLists.txt b/kuka_kr10_gazebo/CMakeLists.txt
new file mode 100644
index 000000000..169bda83a
--- /dev/null
+++ b/kuka_kr10_gazebo/CMakeLists.txt
@@ -0,0 +1,9 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(kuka_kr10_gazebo)
+
+find_package(catkin REQUIRED)
+
+catkin_package()
+
+install(DIRECTORY config launch urdf
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
diff --git a/kuka_kr10_gazebo/README.md b/kuka_kr10_gazebo/README.md
new file mode 100644
index 000000000..c074bafaf
--- /dev/null
+++ b/kuka_kr10_gazebo/README.md
@@ -0,0 +1,13 @@
+kuka_kr10_gazebo
+================
+
+<!-- ![gazebo screenshot](doc/kuka_kr10_gazebo_ros_control.png) -->
+
+## Usage
+The simulation starts up paused by default. This enables us to turn off the physics engine, this is important to get the desired behaviour from the ros-control position controller.
+
+
+To bring up the simulated robot in Gazebo, run:
+```
+roslaunch kuka_kr10_gazebo kr10_gazebo.launch
+```
diff --git a/kuka_kr10_gazebo/config/joint_state_controller.yaml b/kuka_kr10_gazebo/config/joint_state_controller.yaml
new file mode 100644
index 000000000..9cb3a6599
--- /dev/null
+++ b/kuka_kr10_gazebo/config/joint_state_controller.yaml
@@ -0,0 +1,3 @@
+joint_state_controller:
+  type: joint_state_controller/JointStateController
+  publish_rate: 50  
diff --git a/kuka_kr10_gazebo/config/kr10_arm_controller.yaml b/kuka_kr10_gazebo/config/kr10_arm_controller.yaml
new file mode 100644
index 000000000..ad488e3b4
--- /dev/null
+++ b/kuka_kr10_gazebo/config/kr10_arm_controller.yaml
@@ -0,0 +1,21 @@
+arm_controller:
+  type: position_controllers/JointTrajectoryController
+  joints:
+    - joint_a1
+    - joint_a2
+    - joint_a3
+    - joint_a4
+    - joint_a5
+    - joint_a6
+  constraints:
+    goal_time: 0.6
+    stopped_velocity_tolerance: 0.05
+    joint_1: {trajectory: 0.1, goal: 0.1}
+    joint_2: {trajectory: 0.1, goal: 0.1}
+    joint_3: {trajectory: 0.1, goal: 0.1}
+    joint_4: {trajectory: 0.1, goal: 0.1}
+    joint_5: {trajectory: 0.1, goal: 0.1}
+    joint_6: {trajectory: 0.1, goal: 0.1}
+  stop_trajectory_duration: 0.5
+  state_publish_rate:  25
+  action_monitor_rate: 10
diff --git a/kuka_kr10_gazebo/launch/kr10_control.launch b/kuka_kr10_gazebo/launch/kr10_control.launch
new file mode 100644
index 000000000..355183429
--- /dev/null
+++ b/kuka_kr10_gazebo/launch/kr10_control.launch
@@ -0,0 +1,10 @@
+<?xml version="1.0"?>
+<launch>
+  <!-- load the joint state controller -->
+  <rosparam file="$(find kuka_kr10_gazebo)/config/joint_state_controller.yaml" command="load" />
+  <node name="joint_state_controller_spawner" pkg="controller_manager" type="spawner" args="joint_state_controller" />
+
+  <!-- load the arm controller -->
+  <rosparam file="$(find kuka_kr10_gazebo)/config/kr10_arm_controller.yaml" command="load" />
+  <node name="kuka_kr10_controller_spawner" pkg="controller_manager" type="spawner" args="arm_controller" />
+</launch>
diff --git a/kuka_kr10_gazebo/launch/kr10_gazebo.launch b/kuka_kr10_gazebo/launch/kr10_gazebo.launch
new file mode 100644
index 000000000..b469dd1b2
--- /dev/null
+++ b/kuka_kr10_gazebo/launch/kr10_gazebo.launch
@@ -0,0 +1,34 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="paused" default="false"/>
+  <arg name="gui" default="true"/>
+  <arg name="remap" default="true"/>
+
+  <group if="$(arg remap)">
+    <!-- remap topics to conform to ROS-I specifications -->
+    <remap from="/arm_controller/follow_joint_trajectory" to="/joint_trajectory_action"/>
+    <remap from="/arm_controller/state" to="/feedback_states"/>
+    <remap from="/arm_controller/command" to="/joint_path_command"/>
+
+    <!-- startup simulated world -->
+    <include file="$(find gazebo_ros)/launch/empty_world.launch">
+      <arg name="world_name" value="worlds/empty.world"/>
+      <arg name="gui" value="$(arg gui)"/>
+      <arg name="paused" value="$(arg paused)"/>
+    </include>
+  </group>
+
+  <group unless="$(arg remap)">
+    <!-- startup simulated world -->
+    <include file="$(find gazebo_ros)/launch/empty_world.launch">
+      <arg name="world_name" value="worlds/empty.world"/>
+      <arg name="gui" value="$(arg gui)"/>
+      <arg name="paused" value="$(arg paused)"/>
+    </include>
+  </group>
+
+  <!-- send robot URDF to ROS param server,
+       and spawn robot in Gazebo, along with necessary ROS nodes -->
+  <include file="$(find kuka_kr10_gazebo)/launch/spawn_kr10.launch"/>
+
+</launch>
diff --git a/kuka_kr10_gazebo/launch/load_kr10.launch b/kuka_kr10_gazebo/launch/load_kr10.launch
new file mode 100644
index 000000000..96ad6f241
--- /dev/null
+++ b/kuka_kr10_gazebo/launch/load_kr10.launch
@@ -0,0 +1,4 @@
+<?xml version="1.0"?>
+<launch>
+  <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find kuka_kr10_gazebo)/urdf/kr10.xacro'"/>
+</launch>
diff --git a/kuka_kr10_gazebo/launch/spawn_kr10.launch b/kuka_kr10_gazebo/launch/spawn_kr10.launch
new file mode 100644
index 000000000..cc906b616
--- /dev/null
+++ b/kuka_kr10_gazebo/launch/spawn_kr10.launch
@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<launch>
+  <!-- send robot urdf to ROS param server -->
+  <include file="$(find kuka_kr10_gazebo)/launch/load_kr10.launch"/>
+
+  <!-- push robot_description to factory and spawn robot in gazebo -->
+  <node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model"
+        output="screen" respawn="false"
+        args="-urdf
+              -param robot_description
+              -model kuka_kr10r1100sixx
+              -x 0.0
+              -y 0.0
+              -z 0.0
+              -R 0.0
+              -P 0.0
+              -Y 0.0" />
+
+  <!-- convert joint states to TF transforms for rviz, etc -->
+  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
+
+  <!-- init and start Gazebo ros_control interface -->
+  <include file="$(find kuka_kr10_gazebo)/launch/kr10_control.launch"/>
+
+</launch>
diff --git a/kuka_kr10_gazebo/package.xml b/kuka_kr10_gazebo/package.xml
new file mode 100644
index 000000000..52969ee50
--- /dev/null
+++ b/kuka_kr10_gazebo/package.xml
@@ -0,0 +1,40 @@
+<package>
+  <name>kuka_kr10_gazebo</name>
+  <version>0.0.1</version>
+  <description>
+    <p>
+      ROS-Industrial Gazebo support package for the Kuka kr10 (and variants).
+    </p>
+    <p>
+      This package contains the configuration data and launch files required
+      to simulate the Kuka kr10 manipulator in Gazebo. This includes the base
+      model, and the kr10.
+    </p>
+    <p>
+      Before using any of the configuration files included in this package, be
+      sure to check they are correct for the particular robot model and
+      configuration you intend to use them with.
+    </p>
+  </description>
+  <author email="schindlbeck@imr.uni-hannover.de">Christopher Schindlbeck (Leibniz University Hannover)</author>
+  <maintainer email="g.a.vanderhoorn@tudelft.nl">G.A. vd. Hoorn (TU Delft Robotics Institute)</maintainer>
+  <license>Apache2.0</license>
+
+  <url type="website">http://wiki.ros.org/kuka_experimental</url>
+  <url type="bugtracker">https://github.com/ros-industrial/kuka_experimental/issues</url>
+  <url type="repository">https://github.com/ros-industrial/kuka_experimental</url>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <run_depend>kuka_resources</run_depend>
+  <run_depend>kuka_kr10_support</run_depend>
+  <run_depend>gazebo</run_depend>
+  <run_depend>gazebo_ros</run_depend>
+  <run_depend>gazebo_ros_control</run_depend>
+  <run_depend>gazebo_ros_pkgs</run_depend>
+  <run_depend>joint_state_controller</run_depend>
+  <run_depend>position_controllers</run_depend>
+  <run_depend>joint_trajectory_controller</run_depend>
+  <run_depend>robot_state_publisher</run_depend>
+  <run_depend>ros_controllers</run_depend>
+</package>
diff --git a/kuka_kr10_gazebo/urdf/kr10.xacro b/kuka_kr10_gazebo/urdf/kr10.xacro
new file mode 100644
index 000000000..de2e5ef24
--- /dev/null
+++ b/kuka_kr10_gazebo/urdf/kr10.xacro
@@ -0,0 +1,15 @@
+<?xml version="1.0" ?>
+<robot name="kuka_kr10_gazebo" xmlns:xacro="http://ros.org/wiki/xacro">
+  <!-- kr10 -->
+  <xacro:include filename="$(find kuka_kr10_gazebo)/urdf/kr10_macro.xacro"/>
+
+  <xacro:kuka_kr10_robot prefix=""/>
+
+  <!-- Fix robot to Gazebo world -->
+  <link name="world"/>
+  <joint name="world-base_link-fixed" type="fixed">
+    <parent link="world"/>
+    <child link="base_link"/>
+    <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
+  </joint>
+</robot>
diff --git a/kuka_kr10_gazebo/urdf/kr10_macro.xacro b/kuka_kr10_gazebo/urdf/kr10_macro.xacro
new file mode 100644
index 000000000..f73459431
--- /dev/null
+++ b/kuka_kr10_gazebo/urdf/kr10_macro.xacro
@@ -0,0 +1,111 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+<xacro:include filename="$(find kuka_kr10_support)/urdf/kr10r1100sixx_macro.xacro"/>
+<xacro:macro name="kuka_kr10_robot" params="prefix">
+
+  <!-- get URDF description of Kuka kr10 model -->
+  <xacro:kuka_kr10r1100sixx prefix="${prefix}" />
+
+  <!-- transmission list -->
+  <transmission name="${prefix}tran1">
+    <robotNamespace>/${prefix}</robotNamespace>
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="${prefix}joint_a1">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="${prefix}motor1">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="${prefix}tran2">
+    <robotNamespace>/${prefix}</robotNamespace>
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="${prefix}joint_a2">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="${prefix}motor2">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="${prefix}tran3">
+    <robotNamespace>/${prefix}</robotNamespace>
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="${prefix}joint_a3">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="${prefix}motor3">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="${prefix}tran4">
+    <robotNamespace>/${prefix}</robotNamespace>
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="${prefix}joint_a4">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="${prefix}motor4">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="${prefix}tran5">
+    <robotNamespace>/${prefix}</robotNamespace>
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="${prefix}joint_a5">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="${prefix}motor5">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="${prefix}tran6">
+    <robotNamespace>/${prefix}</robotNamespace>
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="${prefix}joint_a6">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="${prefix}motor6">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <!-- end of transmission list -->
+
+  <!-- Gazebo-specific link properties -->
+  <gazebo reference="${prefix}base_link">
+    <material>Gazebo/Black</material>
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+  <gazebo reference="${prefix}link_1">
+    <material>Gazebo/DarkGrey</material>
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+  <gazebo reference="${prefix}link_2">
+    <material>Gazebo/DarkGrey</material>
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+  <gazebo reference="${prefix}link_3">
+    <material>Gazebo/DarkGrey</material>
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+  <gazebo reference="${prefix}link_4">
+    <material>Gazebo/DarkGrey</material>
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+  <gazebo reference="${prefix}link_5">
+    <material>Gazebo/DarkGrey</material>
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+  <gazebo reference="${prefix}link_6">
+    <material>Gazebo/Grey</material>
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+
+  <!-- ros_control plugin -->
+  <gazebo>
+    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
+      <robotNamespace>/${prefix}</robotNamespace>
+      <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
+    </plugin>
+  </gazebo>
+
+  </xacro:macro>
+</robot>

From 04a55886c28fef6605efaa5a2b4c6cbaeb0abb4f Mon Sep 17 00:00:00 2001
From: Chris Schindlbeck <schindlbeck@imr.uni-hannover.de>
Date: Tue, 17 Apr 2018 10:45:51 +0200
Subject: [PATCH 2/7] Revert changes to indigo

---
 kuka_kr10_gazebo/README.md               |  4 +---
 kuka_kr10_gazebo/launch/load_kr10.launch |  2 +-
 kuka_kr10_gazebo/urdf/kr10.xacro         |  2 +-
 kuka_kr10_gazebo/urdf/kr10_macro.xacro   | 12 ++++++------
 4 files changed, 9 insertions(+), 11 deletions(-)

diff --git a/kuka_kr10_gazebo/README.md b/kuka_kr10_gazebo/README.md
index c074bafaf..5374a6b77 100644
--- a/kuka_kr10_gazebo/README.md
+++ b/kuka_kr10_gazebo/README.md
@@ -1,10 +1,8 @@
 kuka_kr10_gazebo
 ================
 
-<!-- ![gazebo screenshot](doc/kuka_kr10_gazebo_ros_control.png) -->
-
 ## Usage
-The simulation starts up paused by default. This enables us to turn off the physics engine, this is important to get the desired behaviour from the ros-control position controller.
+The simulation starts up paused by default. This enables us to turn off the physics engine, this is important to get the desired behavior from the ros-control position controller.
 
 
 To bring up the simulated robot in Gazebo, run:
diff --git a/kuka_kr10_gazebo/launch/load_kr10.launch b/kuka_kr10_gazebo/launch/load_kr10.launch
index 96ad6f241..e1d3473cb 100644
--- a/kuka_kr10_gazebo/launch/load_kr10.launch
+++ b/kuka_kr10_gazebo/launch/load_kr10.launch
@@ -1,4 +1,4 @@
 <?xml version="1.0"?>
 <launch>
-  <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find kuka_kr10_gazebo)/urdf/kr10.xacro'"/>
+  <param name="robot_description" command="$(find xacro)/xacro.py '$(find kuka_kr10_gazebo)/urdf/kr10.xacro'"/>
 </launch>
diff --git a/kuka_kr10_gazebo/urdf/kr10.xacro b/kuka_kr10_gazebo/urdf/kr10.xacro
index de2e5ef24..5834678e2 100644
--- a/kuka_kr10_gazebo/urdf/kr10.xacro
+++ b/kuka_kr10_gazebo/urdf/kr10.xacro
@@ -1,5 +1,5 @@
 <?xml version="1.0" ?>
-<robot name="kuka_kr10_gazebo" xmlns:xacro="http://ros.org/wiki/xacro">
+<robot name="kuka_kr10_gazebo" xmlns:xacro="http://wiki.ros.org/xacro">
   <!-- kr10 -->
   <xacro:include filename="$(find kuka_kr10_gazebo)/urdf/kr10_macro.xacro"/>
 
diff --git a/kuka_kr10_gazebo/urdf/kr10_macro.xacro b/kuka_kr10_gazebo/urdf/kr10_macro.xacro
index f73459431..c21f87af6 100644
--- a/kuka_kr10_gazebo/urdf/kr10_macro.xacro
+++ b/kuka_kr10_gazebo/urdf/kr10_macro.xacro
@@ -1,5 +1,5 @@
 <?xml version="1.0"?>
-<robot xmlns:xacro="http://ros.org/wiki/xacro">
+<robot xmlns:xacro="http://wiki.ros.org/xacro">
 <xacro:include filename="$(find kuka_kr10_support)/urdf/kr10r1100sixx_macro.xacro"/>
 <xacro:macro name="kuka_kr10_robot" params="prefix">
 
@@ -75,23 +75,23 @@
     <turnGravityOff>true</turnGravityOff>
   </gazebo>
   <gazebo reference="${prefix}link_1">
-    <material>Gazebo/DarkGrey</material>
+    <material>Gazebo/Orange</material>
     <turnGravityOff>true</turnGravityOff>
   </gazebo>
   <gazebo reference="${prefix}link_2">
-    <material>Gazebo/DarkGrey</material>
+    <material>Gazebo/Orange</material>
     <turnGravityOff>true</turnGravityOff>
   </gazebo>
   <gazebo reference="${prefix}link_3">
-    <material>Gazebo/DarkGrey</material>
+    <material>Gazebo/Orange</material>
     <turnGravityOff>true</turnGravityOff>
   </gazebo>
   <gazebo reference="${prefix}link_4">
-    <material>Gazebo/DarkGrey</material>
+    <material>Gazebo/Orange</material>
     <turnGravityOff>true</turnGravityOff>
   </gazebo>
   <gazebo reference="${prefix}link_5">
-    <material>Gazebo/DarkGrey</material>
+    <material>Gazebo/Orange</material>
     <turnGravityOff>true</turnGravityOff>
   </gazebo>
   <gazebo reference="${prefix}link_6">

From 7af6a0516bf85da52170408f5858308ce1949c6c Mon Sep 17 00:00:00 2001
From: Chris Schindlbeck <schindlbeck@imr.uni-hannover.de>
Date: Tue, 17 Apr 2018 10:55:16 +0200
Subject: [PATCH 3/7] Added dummy inertial values in kr10 for gazebo

---
 kuka_kr10_support/urdf/kr10r1100sixx.xacro    |  1 +
 .../urdf/kr10r1100sixx_macro.xacro            | 49 +++++++++++++++++++
 2 files changed, 50 insertions(+)

diff --git a/kuka_kr10_support/urdf/kr10r1100sixx.xacro b/kuka_kr10_support/urdf/kr10r1100sixx.xacro
index a3a7e2e6f..3fd5ab1b0 100644
--- a/kuka_kr10_support/urdf/kr10r1100sixx.xacro
+++ b/kuka_kr10_support/urdf/kr10r1100sixx.xacro
@@ -1,4 +1,5 @@
 <?xml version="1.0"?>
+
 <robot name="kuka_kr10r1100sixx" xmlns:xacro="http://wiki.ros.org/xacro">
   <xacro:include filename="$(find kuka_kr10_support)/urdf/kr10r1100sixx_macro.xacro" />
   <xacro:kuka_kr10r1100sixx prefix=""/>
diff --git a/kuka_kr10_support/urdf/kr10r1100sixx_macro.xacro b/kuka_kr10_support/urdf/kr10r1100sixx_macro.xacro
index dee4b6065..dcf5dbf36 100644
--- a/kuka_kr10_support/urdf/kr10r1100sixx_macro.xacro
+++ b/kuka_kr10_support/urdf/kr10r1100sixx_macro.xacro
@@ -18,6 +18,13 @@
           <mesh filename="package://kuka_kr10_support/meshes/kr10r1100sixx/collision/base_link.stl" />
         </geometry>
       </collision>
+      <inertial> <!-- DUMMY VALUES -->
+        <mass value="10.733459"/>
+        <origin xyz="0.014288 0.000001 0.122264" rpy="0.0 0.0 0.0"/>
+        <inertia ixx="0.096590" ixy="-0.000005" ixz="-0.003803"
+                                iyy="0.087591"  iyz="-0.000001"
+                                                izz="0.081632"/>
+      </inertial>
     </link>
     <link name="${prefix}link_1">
       <visual>
@@ -33,6 +40,13 @@
           <mesh filename="package://kuka_kr10_support/meshes/kr10r1100sixx/collision/link_1.stl" />
         </geometry>
       </collision>
+      <inertial> <!-- DUMMY VALUES -->
+        <mass value="10.733459"/>
+        <origin xyz="0.014288 0.000001 0.122264" rpy="0.0 0.0 0.0"/>
+        <inertia ixx="0.096590" ixy="-0.000005" ixz="-0.003803"
+                                iyy="0.087591"  iyz="-0.000001"
+                                                izz="0.081632"/>
+      </inertial>
     </link>
     <link name="${prefix}link_2">
       <visual>
@@ -48,6 +62,13 @@
           <mesh filename="package://kuka_kr10_support/meshes/kr10r1100sixx/collision/link_2.stl" />
         </geometry>
       </collision>
+      <inertial> <!-- DUMMY VALUES -->
+        <mass value="10.733459"/>
+        <origin xyz="0.014288 0.000001 0.122264" rpy="0.0 0.0 0.0"/>
+        <inertia ixx="0.096590" ixy="-0.000005" ixz="-0.003803"
+                                iyy="0.087591"  iyz="-0.000001"
+                                                izz="0.081632"/>
+      </inertial>
     </link>
     <link name="${prefix}link_3">
       <visual>
@@ -63,6 +84,13 @@
           <mesh filename="package://kuka_kr10_support/meshes/kr10r1100sixx/collision/link_3.stl" />
         </geometry>
       </collision>
+      <inertial> <!-- DUMMY VALUES -->
+        <mass value="10.733459"/>
+        <origin xyz="0.014288 0.000001 0.122264" rpy="0.0 0.0 0.0"/>
+        <inertia ixx="0.096590" ixy="-0.000005" ixz="-0.003803"
+                                iyy="0.087591"  iyz="-0.000001"
+                                                izz="0.081632"/>
+      </inertial>
     </link>
     <link name="${prefix}link_4">
       <visual>
@@ -78,6 +106,13 @@
           <mesh filename="package://kuka_kr10_support/meshes/kr10r1100sixx/collision/link_4.stl" />
         </geometry>
       </collision>
+      <inertial> <!-- DUMMY VALUES -->
+        <mass value="10.733459"/>
+        <origin xyz="0.014288 0.000001 0.122264" rpy="0.0 0.0 0.0"/>
+        <inertia ixx="0.096590" ixy="-0.000005" ixz="-0.003803"
+                                iyy="0.087591"  iyz="-0.000001"
+                                                izz="0.081632"/>
+      </inertial>
     </link>
     <link name="${prefix}link_5">
       <visual>
@@ -93,6 +128,13 @@
           <mesh filename="package://kuka_kr10_support/meshes/kr10r1100sixx/collision/link_5.stl" />
         </geometry>
       </collision>
+      <inertial> <!-- DUMMY VALUES -->
+        <mass value="10.733459"/>
+        <origin xyz="0.014288 0.000001 0.122264" rpy="0.0 0.0 0.0"/>
+        <inertia ixx="0.096590" ixy="-0.000005" ixz="-0.003803"
+                                iyy="0.087591"  iyz="-0.000001"
+                                                izz="0.081632"/>
+      </inertial>
     </link>
     <link name="${prefix}link_6">
       <visual>
@@ -108,6 +150,13 @@
           <mesh filename="package://kuka_kr10_support/meshes/kr10r1100sixx/collision/link_6.stl" />
         </geometry>
       </collision>
+      <inertial> <!-- DUMMY VALUES -->
+        <mass value="10.733459"/>
+        <origin xyz="0.014288 0.000001 0.122264" rpy="0.0 0.0 0.0"/>
+        <inertia ixx="0.096590" ixy="-0.000005" ixz="-0.003803"
+                                iyy="0.087591"  iyz="-0.000001"
+                                                izz="0.081632"/>
+      </inertial>
     </link>
     <!-- This frame corresponds to the $FLANGE coordinate system in KUKA KRC controllers. -->
     <link name="${prefix}tool0"/>

From 8999ef92a35c94a30283ca077a8c286ff382a239 Mon Sep 17 00:00:00 2001
From: Chris Schindlbeck <schindlbeck@imr.uni-hannover.de>
Date: Thu, 26 Apr 2018 10:45:43 +0200
Subject: [PATCH 4/7] New macro for cylindrical inertia tensor

---
 kuka_resources/urdf/common_inertia.xacro | 15 +++++++++++++++
 1 file changed, 15 insertions(+)
 create mode 100644 kuka_resources/urdf/common_inertia.xacro

diff --git a/kuka_resources/urdf/common_inertia.xacro b/kuka_resources/urdf/common_inertia.xacro
new file mode 100644
index 000000000..c84dfc933
--- /dev/null
+++ b/kuka_resources/urdf/common_inertia.xacro
@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://wiki.ros.org/xacro">
+
+  <!-- Cylindrical inertia tensor -->
+  <xacro:macro name="cylinder_inertial" params="radius length mass *origin">
+   <inertial>
+     <mass value="${mass}" />
+     <xacro:insert_block name="origin" />
+     <inertia ixx="${0.0833333 * mass * (3 * radius * radius + length * length)}" ixy="0.0" ixz="0.0"
+       iyy="${0.0833333 * mass * (3 * radius * radius + length * length)}" iyz="0.0"
+       izz="${0.5 * mass * radius * radius}" />
+   </inertial>
+ </xacro:macro>
+ 
+</robot>

From 542678e8d0762c5392c14714d46033cbcf1ead88 Mon Sep 17 00:00:00 2001
From: Chris Schindlbeck <schindlbeck@imr.uni-hannover.de>
Date: Thu, 26 Apr 2018 12:43:36 +0200
Subject: [PATCH 5/7] Added link and mass macros to common_parameters and
 replaced corresponding data in xacro

---
 .../urdf/kr10r1100sixx_macro.xacro            | 83 +++++++------------
 kuka_resources/urdf/common_inertia.xacro      | 15 ----
 .../urdf/common_robotparameters.xacro         | 39 +++++++++
 3 files changed, 67 insertions(+), 70 deletions(-)
 delete mode 100644 kuka_resources/urdf/common_inertia.xacro
 create mode 100644 kuka_resources/urdf/common_robotparameters.xacro

diff --git a/kuka_kr10_support/urdf/kr10r1100sixx_macro.xacro b/kuka_kr10_support/urdf/kr10r1100sixx_macro.xacro
index dcf5dbf36..20d1e055f 100644
--- a/kuka_kr10_support/urdf/kr10r1100sixx_macro.xacro
+++ b/kuka_kr10_support/urdf/kr10r1100sixx_macro.xacro
@@ -2,6 +2,7 @@
 <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:include filename="$(find kuka_resources)/urdf/common_robotparameters.xacro"/>
 
   <xacro:macro name="kuka_kr10r1100sixx" params="prefix">
     <link name="${prefix}base_link">
@@ -18,13 +19,9 @@
           <mesh filename="package://kuka_kr10_support/meshes/kr10r1100sixx/collision/base_link.stl" />
         </geometry>
       </collision>
-      <inertial> <!-- DUMMY VALUES -->
-        <mass value="10.733459"/>
-        <origin xyz="0.014288 0.000001 0.122264" rpy="0.0 0.0 0.0"/>
-        <inertia ixx="0.096590" ixy="-0.000005" ixz="-0.003803"
-                                iyy="0.087591"  iyz="-0.000001"
-                                                izz="0.081632"/>
-      </inertial>
+      <xacro:cylinder_inertial radius="${kuka_kr10r1100sixx_d1/4}" length="${kuka_kr10r1100sixx_d1/2}" mass="${kuka_kr10r1100sixx_base_link_mass}">
+        <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
+      </xacro:cylinder_inertial>
     </link>
     <link name="${prefix}link_1">
       <visual>
@@ -40,13 +37,9 @@
           <mesh filename="package://kuka_kr10_support/meshes/kr10r1100sixx/collision/link_1.stl" />
         </geometry>
       </collision>
-      <inertial> <!-- DUMMY VALUES -->
-        <mass value="10.733459"/>
-        <origin xyz="0.014288 0.000001 0.122264" rpy="0.0 0.0 0.0"/>
-        <inertia ixx="0.096590" ixy="-0.000005" ixz="-0.003803"
-                                iyy="0.087591"  iyz="-0.000001"
-                                                izz="0.081632"/>
-      </inertial>
+      <xacro:cylinder_inertial radius="${kuka_kr10r1100sixx_d1/4}" length="${kuka_kr10r1100sixx_d1/2}" mass="${kuka_kr10r1100sixx_link_1_mass}">
+        <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
+      </xacro:cylinder_inertial>
     </link>
     <link name="${prefix}link_2">
       <visual>
@@ -62,13 +55,9 @@
           <mesh filename="package://kuka_kr10_support/meshes/kr10r1100sixx/collision/link_2.stl" />
         </geometry>
       </collision>
-      <inertial> <!-- DUMMY VALUES -->
-        <mass value="10.733459"/>
-        <origin xyz="0.014288 0.000001 0.122264" rpy="0.0 0.0 0.0"/>
-        <inertia ixx="0.096590" ixy="-0.000005" ixz="-0.003803"
-                                iyy="0.087591"  iyz="-0.000001"
-                                                izz="0.081632"/>
-      </inertial>
+      <xacro:cylinder_inertial radius="${kuka_kr10r1100sixx_d1/4}" length="${kuka_kr10r1100sixx_a2}" mass="${kuka_kr10r1100sixx_link_2_mass}">
+        <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
+      </xacro:cylinder_inertial>
     </link>
     <link name="${prefix}link_3">
       <visual>
@@ -84,13 +73,9 @@
           <mesh filename="package://kuka_kr10_support/meshes/kr10r1100sixx/collision/link_3.stl" />
         </geometry>
       </collision>
-      <inertial> <!-- DUMMY VALUES -->
-        <mass value="10.733459"/>
-        <origin xyz="0.014288 0.000001 0.122264" rpy="0.0 0.0 0.0"/>
-        <inertia ixx="0.096590" ixy="-0.000005" ixz="-0.003803"
-                                iyy="0.087591"  iyz="-0.000001"
-                                                izz="0.081632"/>
-      </inertial>
+      <xacro:cylinder_inertial radius="${kuka_kr10r1100sixx_a3}" length="${kuka_kr10r1100sixx_a3*10}" mass="${kuka_kr10r1100sixx_link_3_mass}">
+        <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
+      </xacro:cylinder_inertial>
     </link>
     <link name="${prefix}link_4">
       <visual>
@@ -106,13 +91,9 @@
           <mesh filename="package://kuka_kr10_support/meshes/kr10r1100sixx/collision/link_4.stl" />
         </geometry>
       </collision>
-      <inertial> <!-- DUMMY VALUES -->
-        <mass value="10.733459"/>
-        <origin xyz="0.014288 0.000001 0.122264" rpy="0.0 0.0 0.0"/>
-        <inertia ixx="0.096590" ixy="-0.000005" ixz="-0.003803"
-                                iyy="0.087591"  iyz="-0.000001"
-                                                izz="0.081632"/>
-      </inertial>
+      <xacro:cylinder_inertial radius="${kuka_kr10r1100sixx_d1/4}" length="${kuka_kr10r1100sixx_d4}" mass="${kuka_kr10r1100sixx_link_4_mass}">
+        <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
+      </xacro:cylinder_inertial>
     </link>
     <link name="${prefix}link_5">
       <visual>
@@ -128,13 +109,9 @@
           <mesh filename="package://kuka_kr10_support/meshes/kr10r1100sixx/collision/link_5.stl" />
         </geometry>
       </collision>
-      <inertial> <!-- DUMMY VALUES -->
-        <mass value="10.733459"/>
-        <origin xyz="0.014288 0.000001 0.122264" rpy="0.0 0.0 0.0"/>
-        <inertia ixx="0.096590" ixy="-0.000005" ixz="-0.003803"
-                                iyy="0.087591"  iyz="-0.000001"
-                                                izz="0.081632"/>
-      </inertial>
+      <xacro:cylinder_inertial radius="${kuka_kr10r1100sixx_a5/2}" length="${kuka_kr10r1100sixx_a5}" mass="${kuka_kr10r1100sixx_link_5_mass}">
+        <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
+      </xacro:cylinder_inertial>
     </link>
     <link name="${prefix}link_6">
       <visual>
@@ -150,54 +127,50 @@
           <mesh filename="package://kuka_kr10_support/meshes/kr10r1100sixx/collision/link_6.stl" />
         </geometry>
       </collision>
-      <inertial> <!-- DUMMY VALUES -->
-        <mass value="10.733459"/>
-        <origin xyz="0.014288 0.000001 0.122264" rpy="0.0 0.0 0.0"/>
-        <inertia ixx="0.096590" ixy="-0.000005" ixz="-0.003803"
-                                iyy="0.087591"  iyz="-0.000001"
-                                                izz="0.081632"/>
-      </inertial>
+      <xacro:cylinder_inertial radius="${kuka_kr10r1100sixx_a5/2}" length="${kuka_kr10r1100sixx_a5/10}" mass="${kuka_kr10r1100sixx_link_6_mass}">
+        <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
+      </xacro:cylinder_inertial>
     </link>
     <!-- This frame corresponds to the $FLANGE coordinate system in KUKA KRC controllers. -->
     <link name="${prefix}tool0"/>
 
     <joint name="${prefix}joint_a1" type="revolute">
-      <origin xyz="0 0 0.400" rpy="0 0 0"/>
+      <origin xyz="0 0 ${kuka_kr10r1100sixx_d1}" rpy="0 0 0"/>
       <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}"/>
     </joint>
     <joint name="${prefix}joint_a2" type="revolute">
-      <origin xyz="0.025 0 0" rpy="0 0 0"/>
+      <origin xyz="${kuka_kr10r1100sixx_a1} 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}"/>
     </joint>
     <joint name="${prefix}joint_a3" type="revolute">
-      <origin xyz="0.560 0 0" rpy="0 0 0"/>
+      <origin xyz="${kuka_kr10r1100sixx_a2} 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}"/>
     </joint>
     <joint name="${prefix}joint_a4" type="revolute">
-      <origin xyz="0 0 0.035" rpy="0 0 0"/>
+      <origin xyz="0 0 ${kuka_kr10r1100sixx_a3}" 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}"/>
     </joint>
     <joint name="${prefix}joint_a5" type="revolute">
-      <origin xyz="0.515 0 0" rpy="0 0 0"/>
+      <origin xyz="${kuka_kr10r1100sixx_d4} 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}"/>
     </joint>
     <joint name="${prefix}joint_a6" type="revolute">
-      <origin xyz="0.080 0 0" rpy="0 0 0"/>
+      <origin xyz="${kuka_kr10r1100sixx_a5} 0 0" rpy="0 0 0"/>
       <parent link="${prefix}link_5"/>
       <child link="${prefix}link_6"/>
       <axis xyz="-1 0 0"/>
diff --git a/kuka_resources/urdf/common_inertia.xacro b/kuka_resources/urdf/common_inertia.xacro
deleted file mode 100644
index c84dfc933..000000000
--- a/kuka_resources/urdf/common_inertia.xacro
+++ /dev/null
@@ -1,15 +0,0 @@
-<?xml version="1.0"?>
-<robot xmlns:xacro="http://wiki.ros.org/xacro">
-
-  <!-- Cylindrical inertia tensor -->
-  <xacro:macro name="cylinder_inertial" params="radius length mass *origin">
-   <inertial>
-     <mass value="${mass}" />
-     <xacro:insert_block name="origin" />
-     <inertia ixx="${0.0833333 * mass * (3 * radius * radius + length * length)}" ixy="0.0" ixz="0.0"
-       iyy="${0.0833333 * mass * (3 * radius * radius + length * length)}" iyz="0.0"
-       izz="${0.5 * mass * radius * radius}" />
-   </inertial>
- </xacro:macro>
- 
-</robot>
diff --git a/kuka_resources/urdf/common_robotparameters.xacro b/kuka_resources/urdf/common_robotparameters.xacro
new file mode 100644
index 000000000..693825fdb
--- /dev/null
+++ b/kuka_resources/urdf/common_robotparameters.xacro
@@ -0,0 +1,39 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://wiki.ros.org/xacro">
+
+  <!-- kuka_kr10r1100sixx -->
+  <!-- Mass parameters (guessed), total mass should be 55 kg -->
+  <xacro:property name="kuka_kr10r1100sixx_base_link_mass" value="4.0" />
+  <xacro:property name="kuka_kr10r1100sixx_link_1_mass" value="4.0" />
+  <xacro:property name="kuka_kr10r1100sixx_link_2_mass" value="19.0" />
+  <xacro:property name="kuka_kr10r1100sixx_link_3_mass" value="10.0" />
+  <xacro:property name="kuka_kr10r1100sixx_link_4_mass" value="15.0" />
+  <xacro:property name="kuka_kr10r1100sixx_link_5_mass" value="2.8" />
+  <xacro:property name="kuka_kr10r1100sixx_link_6_mass" value="0.2" />
+
+  <!-- DH-Parameters -->
+  <xacro:property name="kuka_kr10r1100sixx_d1" value="0.400" />
+  <xacro:property name="kuka_kr10r1100sixx_d2" value="0" />
+  <xacro:property name="kuka_kr10r1100sixx_d3" value="0" />
+  <xacro:property name="kuka_kr10r1100sixx_d4" value="0.515" />
+  <xacro:property name="kuka_kr10r1100sixx_d5" value="0" />
+  <xacro:property name="kuka_kr10r1100sixx_d6" value="0" />
+  <xacro:property name="kuka_kr10r1100sixx_a1" value="0.025" />
+  <xacro:property name="kuka_kr10r1100sixx_a2" value="0.560" />
+  <xacro:property name="kuka_kr10r1100sixx_a3" value="0.035" />
+  <xacro:property name="kuka_kr10r1100sixx_a4" value="0" />
+  <xacro:property name="kuka_kr10r1100sixx_a5" value="0.080" />
+  <xacro:property name="kuka_kr10r1100sixx_a6" value="0" />
+
+  <!-- Cylindrical inertia tensor -->
+  <xacro:macro name="cylinder_inertial" params="radius length mass *origin">
+   <inertial>
+     <mass value="${mass}" />
+     <xacro:insert_block name="origin" />
+     <inertia ixx="${0.0833333 * mass * (3 * radius * radius + length * length)}" ixy="0.0" ixz="0.0"
+       iyy="${0.0833333 * mass * (3 * radius * radius + length * length)}" iyz="0.0"
+       izz="${0.5 * mass * radius * radius}" />
+   </inertial>
+ </xacro:macro>
+
+</robot>

From b1bbeb3c961b0bfd8cc234cdedf3a63034f573c1 Mon Sep 17 00:00:00 2001
From: Chris Schindlbeck <schindlbeck@imr.uni-hannover.de>
Date: Tue, 22 May 2018 15:26:59 +0200
Subject: [PATCH 6/7] Integrated robotparameters to kr10r1100sixx_macro.xacro

---
 .../urdf/kr10r1100sixx_macro.xacro            | 30 +++++++++++++-
 .../urdf/common_robotparameters.xacro         | 39 -------------------
 2 files changed, 29 insertions(+), 40 deletions(-)
 delete mode 100644 kuka_resources/urdf/common_robotparameters.xacro

diff --git a/kuka_kr10_support/urdf/kr10r1100sixx_macro.xacro b/kuka_kr10_support/urdf/kr10r1100sixx_macro.xacro
index 20d1e055f..17edb8bc6 100644
--- a/kuka_kr10_support/urdf/kr10r1100sixx_macro.xacro
+++ b/kuka_kr10_support/urdf/kr10r1100sixx_macro.xacro
@@ -2,7 +2,35 @@
 <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:include filename="$(find kuka_resources)/urdf/common_robotparameters.xacro"/>
+
+  <!-- Mass parameters (guessed), total mass should be 55 kg -->
+  <xacro:property name="kuka_kr10r1100sixx_base_link_mass" value="4.0" />
+  <xacro:property name="kuka_kr10r1100sixx_link_1_mass" value="4.0" />
+  <xacro:property name="kuka_kr10r1100sixx_link_2_mass" value="19.0" />
+  <xacro:property name="kuka_kr10r1100sixx_link_3_mass" value="10.0" />
+  <xacro:property name="kuka_kr10r1100sixx_link_4_mass" value="15.0" />
+  <xacro:property name="kuka_kr10r1100sixx_link_5_mass" value="2.8" />
+  <xacro:property name="kuka_kr10r1100sixx_link_6_mass" value="0.2" />
+
+  <!-- Kinematic parameters -->
+  <xacro:property name="kuka_kr10r1100sixx_d1" value="0.400" />
+  <xacro:property name="kuka_kr10r1100sixx_d4" value="0.515" />
+  <xacro:property name="kuka_kr10r1100sixx_a1" value="0.025" />
+  <xacro:property name="kuka_kr10r1100sixx_a2" value="0.560" />
+  <xacro:property name="kuka_kr10r1100sixx_a3" value="0.035" />
+  <xacro:property name="kuka_kr10r1100sixx_a5" value="0.080" />
+
+  <!-- Cylindrical inertia tensor -->
+  <xacro:macro name="cylinder_inertial" params="radius length mass *origin">
+   <inertial>
+     <mass value="${mass}" />
+     <xacro:insert_block name="origin" />
+     <inertia ixx="${0.0833333 * mass * (3 * radius * radius + length * length)}" ixy="0.0" ixz="0.0"
+       iyy="${0.0833333 * mass * (3 * radius * radius + length * length)}" iyz="0.0"
+       izz="${0.5 * mass * radius * radius}" />
+   </inertial>
+ </xacro:macro>
+
 
   <xacro:macro name="kuka_kr10r1100sixx" params="prefix">
     <link name="${prefix}base_link">
diff --git a/kuka_resources/urdf/common_robotparameters.xacro b/kuka_resources/urdf/common_robotparameters.xacro
deleted file mode 100644
index 693825fdb..000000000
--- a/kuka_resources/urdf/common_robotparameters.xacro
+++ /dev/null
@@ -1,39 +0,0 @@
-<?xml version="1.0"?>
-<robot xmlns:xacro="http://wiki.ros.org/xacro">
-
-  <!-- kuka_kr10r1100sixx -->
-  <!-- Mass parameters (guessed), total mass should be 55 kg -->
-  <xacro:property name="kuka_kr10r1100sixx_base_link_mass" value="4.0" />
-  <xacro:property name="kuka_kr10r1100sixx_link_1_mass" value="4.0" />
-  <xacro:property name="kuka_kr10r1100sixx_link_2_mass" value="19.0" />
-  <xacro:property name="kuka_kr10r1100sixx_link_3_mass" value="10.0" />
-  <xacro:property name="kuka_kr10r1100sixx_link_4_mass" value="15.0" />
-  <xacro:property name="kuka_kr10r1100sixx_link_5_mass" value="2.8" />
-  <xacro:property name="kuka_kr10r1100sixx_link_6_mass" value="0.2" />
-
-  <!-- DH-Parameters -->
-  <xacro:property name="kuka_kr10r1100sixx_d1" value="0.400" />
-  <xacro:property name="kuka_kr10r1100sixx_d2" value="0" />
-  <xacro:property name="kuka_kr10r1100sixx_d3" value="0" />
-  <xacro:property name="kuka_kr10r1100sixx_d4" value="0.515" />
-  <xacro:property name="kuka_kr10r1100sixx_d5" value="0" />
-  <xacro:property name="kuka_kr10r1100sixx_d6" value="0" />
-  <xacro:property name="kuka_kr10r1100sixx_a1" value="0.025" />
-  <xacro:property name="kuka_kr10r1100sixx_a2" value="0.560" />
-  <xacro:property name="kuka_kr10r1100sixx_a3" value="0.035" />
-  <xacro:property name="kuka_kr10r1100sixx_a4" value="0" />
-  <xacro:property name="kuka_kr10r1100sixx_a5" value="0.080" />
-  <xacro:property name="kuka_kr10r1100sixx_a6" value="0" />
-
-  <!-- Cylindrical inertia tensor -->
-  <xacro:macro name="cylinder_inertial" params="radius length mass *origin">
-   <inertial>
-     <mass value="${mass}" />
-     <xacro:insert_block name="origin" />
-     <inertia ixx="${0.0833333 * mass * (3 * radius * radius + length * length)}" ixy="0.0" ixz="0.0"
-       iyy="${0.0833333 * mass * (3 * radius * radius + length * length)}" iyz="0.0"
-       izz="${0.5 * mass * radius * radius}" />
-   </inertial>
- </xacro:macro>
-
-</robot>

From aa013a0359befed1023e3b91b49f5d130994f595 Mon Sep 17 00:00:00 2001
From: Chris Schindlbeck <schindlbeck@imr.uni-hannover.de>
Date: Fri, 25 May 2018 13:35:07 +0200
Subject: [PATCH 7/7] More descriptive parameter names

---
 .../urdf/kr10r1100sixx_macro.xacro            | 52 +++++++++----------
 1 file changed, 26 insertions(+), 26 deletions(-)

diff --git a/kuka_kr10_support/urdf/kr10r1100sixx_macro.xacro b/kuka_kr10_support/urdf/kr10r1100sixx_macro.xacro
index 17edb8bc6..0385d0cbc 100644
--- a/kuka_kr10_support/urdf/kr10r1100sixx_macro.xacro
+++ b/kuka_kr10_support/urdf/kr10r1100sixx_macro.xacro
@@ -4,21 +4,21 @@
   <xacro:include filename="$(find kuka_resources)/urdf/common_materials.xacro"/>
 
   <!-- Mass parameters (guessed), total mass should be 55 kg -->
-  <xacro:property name="kuka_kr10r1100sixx_base_link_mass" value="4.0" />
-  <xacro:property name="kuka_kr10r1100sixx_link_1_mass" value="4.0" />
-  <xacro:property name="kuka_kr10r1100sixx_link_2_mass" value="19.0" />
-  <xacro:property name="kuka_kr10r1100sixx_link_3_mass" value="10.0" />
-  <xacro:property name="kuka_kr10r1100sixx_link_4_mass" value="15.0" />
-  <xacro:property name="kuka_kr10r1100sixx_link_5_mass" value="2.8" />
-  <xacro:property name="kuka_kr10r1100sixx_link_6_mass" value="0.2" />
+  <xacro:property name="base_mass" value="4.0" />
+  <xacro:property name="link1_mass" value="4.0" />
+  <xacro:property name="link2_mass" value="19.0" />
+  <xacro:property name="link3_mass" value="10.0" />
+  <xacro:property name="link4_mass" value="15.0" />
+  <xacro:property name="link5_mass" value="2.8" />
+  <xacro:property name="link6_mass" value="0.2" />
 
   <!-- Kinematic parameters -->
-  <xacro:property name="kuka_kr10r1100sixx_d1" value="0.400" />
-  <xacro:property name="kuka_kr10r1100sixx_d4" value="0.515" />
-  <xacro:property name="kuka_kr10r1100sixx_a1" value="0.025" />
-  <xacro:property name="kuka_kr10r1100sixx_a2" value="0.560" />
-  <xacro:property name="kuka_kr10r1100sixx_a3" value="0.035" />
-  <xacro:property name="kuka_kr10r1100sixx_a5" value="0.080" />
+  <xacro:property name="base_link1_length" value="0.400" />
+  <xacro:property name="link1_link2_offset" value="0.025" />
+  <xacro:property name="link2_length" value="0.560" />
+  <xacro:property name="link2_link3_offset" value="0.035" />
+  <xacro:property name="link3_link4_length" value="0.515" />
+  <xacro:property name="link5_link6_length" value="0.080" />
 
   <!-- Cylindrical inertia tensor -->
   <xacro:macro name="cylinder_inertial" params="radius length mass *origin">
@@ -47,7 +47,7 @@
           <mesh filename="package://kuka_kr10_support/meshes/kr10r1100sixx/collision/base_link.stl" />
         </geometry>
       </collision>
-      <xacro:cylinder_inertial radius="${kuka_kr10r1100sixx_d1/4}" length="${kuka_kr10r1100sixx_d1/2}" mass="${kuka_kr10r1100sixx_base_link_mass}">
+      <xacro:cylinder_inertial radius="${base_link1_length/4}" length="${base_link1_length/2}" mass="${base_mass}">
         <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
       </xacro:cylinder_inertial>
     </link>
@@ -65,7 +65,7 @@
           <mesh filename="package://kuka_kr10_support/meshes/kr10r1100sixx/collision/link_1.stl" />
         </geometry>
       </collision>
-      <xacro:cylinder_inertial radius="${kuka_kr10r1100sixx_d1/4}" length="${kuka_kr10r1100sixx_d1/2}" mass="${kuka_kr10r1100sixx_link_1_mass}">
+      <xacro:cylinder_inertial radius="${base_link1_length/4}" length="${base_link1_length/2}" mass="${link1_mass}">
         <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
       </xacro:cylinder_inertial>
     </link>
@@ -83,7 +83,7 @@
           <mesh filename="package://kuka_kr10_support/meshes/kr10r1100sixx/collision/link_2.stl" />
         </geometry>
       </collision>
-      <xacro:cylinder_inertial radius="${kuka_kr10r1100sixx_d1/4}" length="${kuka_kr10r1100sixx_a2}" mass="${kuka_kr10r1100sixx_link_2_mass}">
+      <xacro:cylinder_inertial radius="${base_link1_length/4}" length="${link2_length}" mass="${link2_mass}">
         <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
       </xacro:cylinder_inertial>
     </link>
@@ -101,7 +101,7 @@
           <mesh filename="package://kuka_kr10_support/meshes/kr10r1100sixx/collision/link_3.stl" />
         </geometry>
       </collision>
-      <xacro:cylinder_inertial radius="${kuka_kr10r1100sixx_a3}" length="${kuka_kr10r1100sixx_a3*10}" mass="${kuka_kr10r1100sixx_link_3_mass}">
+      <xacro:cylinder_inertial radius="${link2_link3_offset}" length="${link2_link3_offset*10}" mass="${link3_mass}">
         <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
       </xacro:cylinder_inertial>
     </link>
@@ -119,7 +119,7 @@
           <mesh filename="package://kuka_kr10_support/meshes/kr10r1100sixx/collision/link_4.stl" />
         </geometry>
       </collision>
-      <xacro:cylinder_inertial radius="${kuka_kr10r1100sixx_d1/4}" length="${kuka_kr10r1100sixx_d4}" mass="${kuka_kr10r1100sixx_link_4_mass}">
+      <xacro:cylinder_inertial radius="${base_link1_length/4}" length="${link3_link4_length}" mass="${link4_mass}">
         <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
       </xacro:cylinder_inertial>
     </link>
@@ -137,7 +137,7 @@
           <mesh filename="package://kuka_kr10_support/meshes/kr10r1100sixx/collision/link_5.stl" />
         </geometry>
       </collision>
-      <xacro:cylinder_inertial radius="${kuka_kr10r1100sixx_a5/2}" length="${kuka_kr10r1100sixx_a5}" mass="${kuka_kr10r1100sixx_link_5_mass}">
+      <xacro:cylinder_inertial radius="${link5_link6_length/2}" length="${link5_link6_length}" mass="${link5_mass}">
         <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
       </xacro:cylinder_inertial>
     </link>
@@ -155,7 +155,7 @@
           <mesh filename="package://kuka_kr10_support/meshes/kr10r1100sixx/collision/link_6.stl" />
         </geometry>
       </collision>
-      <xacro:cylinder_inertial radius="${kuka_kr10r1100sixx_a5/2}" length="${kuka_kr10r1100sixx_a5/10}" mass="${kuka_kr10r1100sixx_link_6_mass}">
+      <xacro:cylinder_inertial radius="${link5_link6_length/2}" length="${link5_link6_length/10}" mass="${link6_mass}">
         <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
       </xacro:cylinder_inertial>
     </link>
@@ -163,42 +163,42 @@
     <link name="${prefix}tool0"/>
 
     <joint name="${prefix}joint_a1" type="revolute">
-      <origin xyz="0 0 ${kuka_kr10r1100sixx_d1}" rpy="0 0 0"/>
+      <origin xyz="0 0 ${base_link1_length}" rpy="0 0 0"/>
       <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}"/>
     </joint>
     <joint name="${prefix}joint_a2" type="revolute">
-      <origin xyz="${kuka_kr10r1100sixx_a1} 0 0" rpy="0 0 0"/>
+      <origin xyz="${link1_link2_offset} 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}"/>
     </joint>
     <joint name="${prefix}joint_a3" type="revolute">
-      <origin xyz="${kuka_kr10r1100sixx_a2} 0 0" rpy="0 0 0"/>
+      <origin xyz="${link2_length} 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}"/>
     </joint>
     <joint name="${prefix}joint_a4" type="revolute">
-      <origin xyz="0 0 ${kuka_kr10r1100sixx_a3}" rpy="0 0 0"/>
+      <origin xyz="0 0 ${link2_link3_offset}" 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}"/>
     </joint>
     <joint name="${prefix}joint_a5" type="revolute">
-      <origin xyz="${kuka_kr10r1100sixx_d4} 0 0" rpy="0 0 0"/>
+      <origin xyz="${link3_link4_length} 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}"/>
     </joint>
     <joint name="${prefix}joint_a6" type="revolute">
-      <origin xyz="${kuka_kr10r1100sixx_a5} 0 0" rpy="0 0 0"/>
+      <origin xyz="${link5_link6_length} 0 0" rpy="0 0 0"/>
       <parent link="${prefix}link_5"/>
       <child link="${prefix}link_6"/>
       <axis xyz="-1 0 0"/>