From f56eea545e645c46ec56673c6f12a31bcc0a973b Mon Sep 17 00:00:00 2001
From: Felix Exner <exner@fzi.de>
Date: Thu, 11 Jun 2020 18:14:21 +0200
Subject: [PATCH] Updated cog for ur16 how I understand the documentation

used documentation:
https://www.universal-robots.com/how-tos-and-faqs/faq/ur-faq/parameters-for-calculations-of-kinematics-and-dynamics-45257/

Note: This changes inertia for all robots as the main xacro file is changed.
---
 .../config/ur16e/physical_parameters.yaml     | 38 +++++++++----------
 ur_description/urdf/ur_macro.xacro            | 12 +++---
 2 files changed, 25 insertions(+), 25 deletions(-)

diff --git a/ur_description/config/ur16e/physical_parameters.yaml b/ur_description/config/ur16e/physical_parameters.yaml
index 658d51d7f..e3e9ee1f7 100644
--- a/ur_description/config/ur16e/physical_parameters.yaml
+++ b/ur_description/config/ur16e/physical_parameters.yaml
@@ -53,28 +53,28 @@ inertia_parameters:
       radius: 0.045
       length: 0.05
 
-  center_of_mass: # Adjusted manually
+  center_of_mass: # model referring to https://www.universal-robots.com/how-tos-and-faqs/faq/ur-faq/parameters-for-calculations-of-kinematics-and-dynamics-45257/
     shoulder_cog:
-      x: 0.0
-      y: 0.00244
-      z: -0.037
+      x: 0.0 # model.x
+      y: -0.030 # -model.z
+      z: -0.016 # model.y
     upper_arm_cog:
-      x: 0.00001
-      y: 0.15061
-      z: 0.38757
+      x: -0.1764 # model.x - upperarm_length
+      y: 0.0 # model.y
+      z: 0.16 # model.z
     forearm_cog:
-      x: -0.00012
-      y: 0.06112
-      z: 0.1984
+      x: -0.166 # model.x - forearm_length
+      y: 0.0 # model.y
+      z: 0.065 # model.z
     wrist_1_cog:
-      x: -0.00021
-      y: -0.00112
-      z: 0.02269
+      x: 0.0 # model.x
+      y: -0.011 # -model.z
+      z: -0.009 # model.y
     wrist_2_cog:
-      x: -0.00021
-      y: 0.00112
-      z: 0.002269
+      x: 0.0 # model.x
+      y: 0.012 # model.z
+      z: -0.018 # -model.y
     wrist_3_cog:
-      x: 0.0
-      y: -0.001156
-      z: -0.00149
+      x: 0.0 # model.x
+      y: 0.0 # model.y
+      z: -0.044 # model.z
diff --git a/ur_description/urdf/ur_macro.xacro b/ur_description/urdf/ur_macro.xacro
index 0ad77362d..d57811b2a 100644
--- a/ur_description/urdf/ur_macro.xacro
+++ b/ur_description/urdf/ur_macro.xacro
@@ -121,7 +121,7 @@
         </geometry>
       </collision>
       <xacro:cylinder_inertial radius="${shoulder_inertia_radius}" length="${shoulder_inertia_length}" mass="${shoulder_mass}">
-        <origin xyz="0 0 0" rpy="0 0 0" />
+        <origin xyz="${shoulder_cog}" rpy="0 0 0" />
       </xacro:cylinder_inertial>
     </link>
     <link name="${prefix}upper_arm_link">
@@ -141,7 +141,7 @@
         </geometry>
       </collision>
       <xacro:cylinder_inertial radius="${upperarm_inertia_radius}" length="${upperarm_inertia_length}" mass="${upper_arm_mass}">
-        <origin xyz="${-0.5 * upperarm_inertia_length} 0.0 ${upper_arm_inertia_offset}" rpy="0 ${pi/2} 0" />
+        <origin xyz="${upper_arm_cog}" rpy="0 ${pi/2} 0" />
       </xacro:cylinder_inertial>
     </link>
     <link name="${prefix}forearm_link">
@@ -161,7 +161,7 @@
         </geometry>
       </collision>
       <xacro:cylinder_inertial radius="${forearm_inertia_radius}" length="${forearm_inertia_length}"  mass="${forearm_mass}">
-        <origin xyz="${-0.5 * forearm_inertia_length} 0.0 ${elbow_offset}" rpy="0 ${pi/2} 0" />
+        <origin xyz="${forearm_cog}" rpy="0 ${pi/2} 0" />
       </xacro:cylinder_inertial>
     </link>
     <link name="${prefix}wrist_1_link">
@@ -182,7 +182,7 @@
         </geometry>
       </collision>
       <xacro:cylinder_inertial radius="${wrist_1_inertia_radius}" length="${wrist_1_inertia_length}"  mass="${wrist_1_mass}">
-        <origin xyz="0 0 0" rpy="0 0 0" />
+        <origin xyz="${wrist_1_cog}" rpy="0 0 0" />
       </xacro:cylinder_inertial>
     </link>
     <link name="${prefix}wrist_2_link">
@@ -202,7 +202,7 @@
         </geometry>
       </collision>
       <xacro:cylinder_inertial radius="${wrist_2_inertia_radius}" length="${wrist_2_inertia_length}"  mass="${wrist_2_mass}">
-        <origin xyz="0 0 0" rpy="0 0 0" />
+        <origin xyz="${wrist_2_cog}" rpy="0 0 0" />
       </xacro:cylinder_inertial>
     </link>
     <link name="${prefix}wrist_3_link">
@@ -222,7 +222,7 @@
         </geometry>
       </collision>
       <xacro:cylinder_inertial radius="${wrist_3_inertia_radius}" length="${wrist_3_inertia_length}"  mass="${wrist_3_mass}">
-        <origin xyz="0.0 0.0 ${-0.5 * wrist_3_inertia_length}" rpy="0 0 0" />
+        <origin xyz="${wrist_3_cog}" rpy="0 0 0" />
       </xacro:cylinder_inertial>
     </link>
     <link name="${prefix}ee_link">