From 8ecc89efc2a64b9d21fc2ca1fc06d7585569b6b1 Mon Sep 17 00:00:00 2001
From: Chalongrath Pholsiri <112415882+chalongrath@users.noreply.github.com>
Date: Fri, 20 Sep 2024 03:55:47 -0500
Subject: [PATCH] Update dynamic properties (#195)
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

* Remove unused dh_parameters
* Rename prop__wrist_2_cog to prop_wrist_2_cog for consistency
* Update mass and cog and use full inertia matrix.
* Update max_efforts (torque limits)
* Remove code and values that are no longer needed.

---------

Co-authored-by: Rune Søe-Knudsen <41109954+urrsk@users.noreply.github.com>
(cherry picked from commit 60e6974c65bf5ebe611c51c022cf2eccf88fae32)
---
 config/ur10/physical_parameters.yaml  | 134 ++++++++++++++++--------
 config/ur10e/joint_limits.yaml        |   6 +-
 config/ur10e/physical_parameters.yaml | 134 ++++++++++++++++--------
 config/ur16e/joint_limits.yaml        |   6 +-
 config/ur16e/physical_parameters.yaml | 137 +++++++++++++++---------
 config/ur20/joint_limits.yaml         |  12 +--
 config/ur20/physical_parameters.yaml  | 140 ++++++++++++++++---------
 config/ur3/physical_parameters.yaml   | 137 ++++++++++++++++--------
 config/ur30/joint_limits.yaml         |  12 +--
 config/ur30/physical_parameters.yaml  | 144 ++++++++++++++++---------
 config/ur3e/joint_limits.yaml         |  10 +-
 config/ur3e/physical_parameters.yaml  | 136 ++++++++++++++++--------
 config/ur5/physical_parameters.yaml   | 145 ++++++++++++++++----------
 config/ur5e/physical_parameters.yaml  | 136 ++++++++++++++++--------
 urdf/inc/ur_common.xacro              |  87 +++++++++++-----
 urdf/ur_macro.xacro                   |  91 ++++++++++++----
 16 files changed, 975 insertions(+), 492 deletions(-)

diff --git a/config/ur10/physical_parameters.yaml b/config/ur10/physical_parameters.yaml
index 4364a7b..4f57ad9 100644
--- a/config/ur10/physical_parameters.yaml
+++ b/config/ur10/physical_parameters.yaml
@@ -1,13 +1,5 @@
 # Physical parameters
 
-dh_parameters:
-  d1: 0.1273
-  a2: -0.612
-  a3: -0.5723
-  d4: 0.163941
-  d5: 0.1157
-  d6: 0.0922
-
 offsets:
   shoulder_offset: 0.220941 # measured from model
   elbow_offset: 0.049042 # measured from model
@@ -16,7 +8,6 @@ inertia_parameters:
   base_mass: 4.0  # This mass might be incorrect
   shoulder_mass: 7.1
   upper_arm_mass: 12.7
-  upper_arm_inertia_offset: 0.175
   forearm_mass: 4.27
   wrist_1_mass: 2.0
   wrist_2_mass: 2.0
@@ -32,47 +23,100 @@ inertia_parameters:
     base:
       radius: 0.075
       length: 0.038
-    shoulder:
-      radius: 0.075
-      length: 0.178
-    upperarm:
-      radius: 0.075
-      length: 0.612
-    forearm:
-      radius: 0.075
-      length: 0.5723
-    wrist_1:
-      radius: 0.075
-      length: 0.12
-    wrist_2:
-      radius: 0.075
-      length: 0.09
-    wrist_3:
-      radius: 0.045
-      length: 0.0305
 
   center_of_mass:
     shoulder_cog:
-      x: 0.0
-      y: 0.00244
-      z: -0.037
+      x: 0.021      # model.x
+      y: -0.027     # -model.z
+      z: 0.0        # model.y
     upper_arm_cog:
-      x: 0.00001
-      y: 0.15061
-      z: 0.38757
+      x: -0.232     # model.x - upper_arm_length
+      y: 0.0        # model.y
+      z: 0.158      # model.z
     forearm_cog:
-      x: -0.00012
-      y: 0.06112
-      z: 0.1984
+      x: -0.3323    # model.x - forearm_length
+      y: 0.0        # model.y
+      z: 0.068      # model.z
     wrist_1_cog:
-      x: -0.00021
-      y: -0.00112
-      z: 0.02269
+      x: 0.0        # model.x
+      y: -0.018     # -model.z
+      z: 0.007      # model.y
     wrist_2_cog:
-      x: -0.00021
-      y: 0.00112
-      z: 0.002269
+      x: 0.0        # model.x
+      y: 0.018      # model.z
+      z: -0.007     # -model.y
     wrist_3_cog:
-      x: 0.0
-      y: -0.001156
-      z: -0.00149
+      x: 0.0        # model.x
+      y: 0          # model.y
+      z: -0.026     # model.z
+
+  rotation:
+    shoulder:
+      roll: 1.570796326794897
+      pitch: 0
+      yaw: 0
+    upper_arm:
+      roll: 0
+      pitch: 0
+      yaw: 0
+    forearm:
+      roll: 0
+      pitch: 0
+      yaw: 0
+    wrist_1:
+      roll: 1.570796326794897
+      pitch: 0
+      yaw: 0
+    wrist_2:
+      roll: -1.570796326794897
+      pitch: 0
+      yaw: 0
+    wrist_3:
+      roll: 0
+      pitch: 0
+      yaw: 0
+
+  # extracted from URSim
+  tensor:
+    shoulder:
+      ixx: 0.03408
+      ixy: 0.00002
+      ixz: -0.00425
+      iyy: 0.03529
+      iyz: 0.00008
+      izz: 0.02156
+    upper_arm:
+      ixx: 0.02814
+      ixy: 0.00005
+      ixz: -0.01561
+      iyy: 0.77068
+      iyz: 0.00002
+      izz: 0.76943
+    forearm:
+      ixx: 0.01014
+      ixy: 0.00008
+      ixz: 0.00916
+      iyy: 0.30928
+      iyz: 0.0
+      izz: 0.30646
+    wrist_1:
+      ixx: 0.00296
+      ixy: -0.00001
+      ixz: 0.0
+      iyy: 0.00222
+      iyz: -0.00024
+      izz: 0.00258
+    wrist_2:
+      ixx: 0.00296
+      ixy: -0.00001
+      ixz: 0.0
+      iyy: 0.00222
+      iyz: -0.00024
+      izz: 0.00258
+    wrist_3:
+      ixx: 0.00040
+      ixy: 0.0
+      ixz: 0.0
+      iyy: 0.00041
+      iyz: 0.0
+      izz: 0.00034
diff --git a/config/ur10e/joint_limits.yaml b/config/ur10e/joint_limits.yaml
index a7adb67..44be1f4 100644
--- a/config/ur10e/joint_limits.yaml
+++ b/config/ur10e/joint_limits.yaml
@@ -54,7 +54,7 @@ joint_limits:
     has_effort_limits: true
     has_position_limits: true
     has_velocity_limits: true
-    max_effort: 56.0
+    max_effort: 54.0
     max_position: !degrees  360.0
     max_velocity: !degrees  180.0
     min_position: !degrees -360.0
@@ -64,7 +64,7 @@ joint_limits:
     has_effort_limits: true
     has_position_limits: true
     has_velocity_limits: true
-    max_effort: 56.0
+    max_effort: 54.0
     max_position: !degrees  360.0
     max_velocity: !degrees  180.0
     min_position: !degrees -360.0
@@ -74,7 +74,7 @@ joint_limits:
     has_effort_limits: true
     has_position_limits: true
     has_velocity_limits: true
-    max_effort: 56.0
+    max_effort: 54.0
     max_position: !degrees  360.0
     max_velocity: !degrees  180.0
     min_position: !degrees -360.0
diff --git a/config/ur10e/physical_parameters.yaml b/config/ur10e/physical_parameters.yaml
index 34b4e3b..daaae73 100644
--- a/config/ur10e/physical_parameters.yaml
+++ b/config/ur10e/physical_parameters.yaml
@@ -1,13 +1,5 @@
 # Physical parameters
 
-dh_parameters:
-  d1: 0.181
-  a2: -0.613
-  a3: -0.571
-  d4: 0.174        # wrist1_length = d4 - elbow_offset - shoulder_offset
-  d5: 0.120
-  d6: 0.117
-
 offsets:
   shoulder_offset: 0.1762 # measured from model
   elbow_offset: 0.0393 # measured from model
@@ -16,7 +8,6 @@ inertia_parameters:
   base_mass: 4.0  # This mass might be incorrect
   shoulder_mass: 7.369
   upper_arm_mass: 13.051
-  upper_arm_inertia_offset: 0.175 # measured from model
   forearm_mass: 3.989
   wrist_1_mass: 2.1
   wrist_2_mass: 1.98
@@ -32,47 +23,100 @@ inertia_parameters:
     base:
       radius: 0.075
       length: 0.038
-    shoulder:
-      radius: 0.075
-      length: 0.178
-    upperarm:
-      radius: 0.075
-      length: 0.612
-    forearm:
-      radius: 0.075
-      length: 0.57155
-    wrist_1:
-      radius: 0.075
-      length: 0.12
-    wrist_2:
-      radius: 0.075
-      length: 0.12
-    wrist_3:
-      radius: 0.045
-      length: 0.05
 
+  # model referring to https://www.universal-robots.com/articles/ur/application-installation/dh-parameters-for-calculations-of-kinematics-and-dynamics/
   center_of_mass:
     shoulder_cog:
-      x: 0.0
-      y: 0.00244
-      z: -0.037
+      x: 0.021      # model.x
+      y: -0.027     # -model.z
+      z: 0.0        # model.y
     upper_arm_cog:
-      x: 0.00001
-      y: 0.15061
-      z: 0.38757
+      x: -0.2327    # model.x - upper_arm_length
+      y: 0.0        # model.y
+      z: 0.158      # model.z
     forearm_cog:
-      x: -0.00012
-      y: 0.06112
-      z: 0.1984
+      x: -0.33155   # model.x - forearm_length
+      y: 0.0        # model.y
+      z: 0.068      # model.z
     wrist_1_cog:
-      x: -0.00021
-      y: -0.00112
-      z: 0.02269
+      x: 0.0        # model.x
+      y: -0.018     # -model.z
+      z: 0.007      # model.y
     wrist_2_cog:
-      x: -0.00021
-      y: 0.00112
-      z: 0.002269
+      x: 0.0        # model.x
+      y: 0.018      # model.z
+      z: -0.007     # -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.026     # model.z
+
+  rotation:
+    shoulder:
+      roll: 1.570796326794897
+      pitch: 0
+      yaw: 0
+    upper_arm:
+      roll: 0
+      pitch: 0
+      yaw: 0
+    forearm:
+      roll: 0
+      pitch: 0
+      yaw: 0
+    wrist_1:
+      roll: 1.570796326794897
+      pitch: 0
+      yaw: 0
+    wrist_2:
+      roll: -1.570796326794897
+      pitch: 0
+      yaw: 0
+    wrist_3:
+      roll: 0
+      pitch: 0
+      yaw: 0
+
+  tensor:
+    shoulder:
+      ixx: 0.03408
+      ixy: 0.00002
+      ixz: -0.00425
+      iyy: 0.03529
+      iyz: 0.00008
+      izz: 0.02156
+    upper_arm:
+      ixx: 0.02814
+      ixy: 0.00005
+      ixz: -0.01561
+      iyy: 0.77068
+      iyz: 0.00002
+      izz: 0.76943
+    forearm:
+      ixx: 0.01014
+      ixy: 0.00008
+      ixz: 0.00916
+      iyy: 0.30928
+      iyz: 0.0
+      izz: 0.30646
+    wrist_1:
+      ixx: 0.00296
+      ixy: -0.00001
+      ixz: 0.0
+      iyy: 0.00222
+      iyz: -0.00024
+      izz: 0.00258
+    wrist_2:
+      ixx: 0.00296
+      ixy: -0.00001
+      ixz: 0.0
+      iyy: 0.00222
+      iyz: -0.00024
+      izz: 0.00258
+    wrist_3:
+      ixx: 0.00040
+      ixy: 0.0
+      ixz: 0.0
+      iyy: 0.00041
+      iyz: 0.0
+      izz: 0.00034
diff --git a/config/ur16e/joint_limits.yaml b/config/ur16e/joint_limits.yaml
index 33182a6..84e547c 100644
--- a/config/ur16e/joint_limits.yaml
+++ b/config/ur16e/joint_limits.yaml
@@ -54,7 +54,7 @@ joint_limits:
     has_effort_limits: true
     has_position_limits: true
     has_velocity_limits: true
-    max_effort: 56.0
+    max_effort: 54.0
     max_position: !degrees  360.0
     max_velocity: !degrees  180.0
     min_position: !degrees -360.0
@@ -64,7 +64,7 @@ joint_limits:
     has_effort_limits: true
     has_position_limits: true
     has_velocity_limits: true
-    max_effort: 56.0
+    max_effort: 54.0
     max_position: !degrees  360.0
     max_velocity: !degrees  180.0
     min_position: !degrees -360.0
@@ -74,7 +74,7 @@ joint_limits:
     has_effort_limits: true
     has_position_limits: true
     has_velocity_limits: true
-    max_effort: 56.0
+    max_effort: 54.0
     max_position: !degrees  360.0
     max_velocity: !degrees  180.0
     min_position: !degrees -360.0
diff --git a/config/ur16e/physical_parameters.yaml b/config/ur16e/physical_parameters.yaml
index 658d51d..b9d27fb 100644
--- a/config/ur16e/physical_parameters.yaml
+++ b/config/ur16e/physical_parameters.yaml
@@ -1,14 +1,5 @@
 # Physical parameters
 
-# from https://www.universal-robots.com/how-tos-and-faqs/faq/ur-faq/parameters-for-calculations-of-kinematics-and-dynamics-45257/
-dh_parameters:
-  d1: 0.1807
-  a2: -0.4784
-  a3: -0.36
-  d4: 0.17415
-  d5: 0.11985
-  d6: 0.11655
-
 offsets:
   shoulder_offset: 0.176 # measured from model
   elbow_offset: 0.040 # measured from model
@@ -18,7 +9,6 @@ inertia_parameters:
   base_mass: 4.0  # This mass might be incorrect
   shoulder_mass: 7.369
   upper_arm_mass: 10.450
-  upper_arm_inertia_offset: 0.175 # measured from model
   forearm_mass: 4.321
   wrist_1_mass: 2.180
   wrist_2_mass: 2.033
@@ -34,47 +24,100 @@ inertia_parameters:
     base:
       radius: 0.075
       length: 0.038
-    shoulder:
-      radius: 0.075
-      length: 0.178
-    upperarm:
-      radius: 0.075
-      length: 0.4784
-    forearm:
-      radius: 0.075
-      length: 0.36
-    wrist_1:
-      radius: 0.075
-      length: 0.12
-    wrist_2:
-      radius: 0.075
-      length: 0.12
-    wrist_3:
-      radius: 0.045
-      length: 0.05
 
-  center_of_mass: # Adjusted manually
+  # model referring to https://www.universal-robots.com/articles/ur/application-installation/dh-parameters-for-calculations-of-kinematics-and-dynamics/
+  center_of_mass:
     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
+
+  rotation:
+    shoulder:
+      roll: 1.570796326794897
+      pitch: 0
+      yaw: 0
+    upper_arm:
+      roll: 0
+      pitch: 0
+      yaw: 0
+    forearm:
+      roll: 0
+      pitch: 0
+      yaw: 0
+    wrist_1:
+      roll: 1.570796326794897
+      pitch: 0
+      yaw: 0
+    wrist_2:
+      roll: -1.570796326794897
+      pitch: 0
+      yaw: 0
+    wrist_3:
+      roll: 0
+      pitch: 0
+      yaw: 0
+
+  tensor:
+    shoulder:
+      ixx: 0.03351
+      ixy: 0.00002
+      ixz: -0.00001
+      iyy: 0.03374
+      iyz: 0.00374
+      izz: 0.02100
+    upper_arm:
+      ixx: 0.02796
+      ixy: -0.00010
+      ixz: -0.00720
+      iyy: 0.47558
+      iyz: 0.00003
+      izz: 0.47635
+    forearm:
+      ixx: 0.01091
+      ixy: 0.00006
+      ixz: 0.01012
+      iyy: 0.12060
+      iyz: 0.00001
+      izz: 0.11714
+    wrist_1:
+      ixx: 0.00609
+      ixy: -0.00001
+      ixz: 0.0
+      iyy: 0.00245
+      iyz: 0.00083
+      izz: 0.00579
+    wrist_2:
+      ixx: 0.00389
+      ixy: -0.00001
+      ixz: 0.0
+      iyy: 0.00219
+      iyz: -0.00045
+      izz: 0.00363
+    wrist_3:
+      ixx: 0.00117
+      ixy: 0.0
+      ixz: 0.0
+      iyy: 0.00118
+      iyz: 0.0
+      izz: 0.00084
diff --git a/config/ur20/joint_limits.yaml b/config/ur20/joint_limits.yaml
index b122784..0d4b0fb 100644
--- a/config/ur20/joint_limits.yaml
+++ b/config/ur20/joint_limits.yaml
@@ -11,7 +11,7 @@ joint_limits:
     has_effort_limits: true
     has_position_limits: true
     has_velocity_limits: true
-    max_effort: 730.0
+    max_effort: 738.0
     max_position: !degrees  360.0
     max_velocity: !degrees  120.0
     min_position: !degrees -360.0
@@ -21,7 +21,7 @@ joint_limits:
     has_effort_limits: true
     has_position_limits: true
     has_velocity_limits: true
-    max_effort: 730.0
+    max_effort: 738.0
     max_position: !degrees  360.0
     max_velocity: !degrees  120.0
     min_position: !degrees -360.0
@@ -31,7 +31,7 @@ joint_limits:
     has_effort_limits: true
     has_position_limits: true
     has_velocity_limits: true
-    max_effort: 430.0
+    max_effort: 433.0
     # we artificially limit this joint to half its actual joint position limit
     # to avoid (MoveIt/OMPL) planning problems, as due to the physical
     # construction of the robot, it's impossible to rotate the 'elbow_joint'
@@ -51,7 +51,7 @@ joint_limits:
     has_effort_limits: true
     has_position_limits: true
     has_velocity_limits: true
-    max_effort: 100.0
+    max_effort: 107.0
     max_position: !degrees  360.0
     max_velocity: !degrees  210.0
     min_position: !degrees -360.0
@@ -61,7 +61,7 @@ joint_limits:
     has_effort_limits: true
     has_position_limits: true
     has_velocity_limits: true
-    max_effort: 100.0
+    max_effort: 107.0
     max_position: !degrees  360.0
     max_velocity: !degrees  210.0
     min_position: !degrees -360.0
@@ -71,7 +71,7 @@ joint_limits:
     has_effort_limits: true
     has_position_limits: true
     has_velocity_limits: true
-    max_effort: 100.0
+    max_effort: 107.0
     max_position: !degrees  360.0
     max_velocity: !degrees  210.0
     min_position: !degrees -360.0
diff --git a/config/ur20/physical_parameters.yaml b/config/ur20/physical_parameters.yaml
index 77d4499..71e7314 100644
--- a/config/ur20/physical_parameters.yaml
+++ b/config/ur20/physical_parameters.yaml
@@ -1,28 +1,18 @@
 # Physical parameters
 
-# from https://www.universal-robots.com/how-tos-and-faqs/faq/ur-faq/parameters-for-calculations-of-kinematics-and-dynamics-45257/
-dh_parameters:
-  d1: 0.2363
-  a2: -0.8620
-  a3: -0.7287
-  d4: 0.201
-  d5: 0.1593
-  d6: 0.1543
-
 offsets:
   shoulder_offset: 0.260 # measured from model
   elbow_offset: 0.043 # measured from model
 
 inertia_parameters:
-  # taken from https://www.universal-robots.com/how-tos-and-faqs/faq/ur-faq/parameters-for-calculations-of-kinematics-and-dynamics-45257/
+  # taken from https://www.universal-robots.com/articles/ur/application-installation/dh-parameters-for-calculations-of-kinematics-and-dynamics/
   base_mass: 4.0  # This mass might be incorrect
   shoulder_mass: 16.343
   upper_arm_mass: 29.632
-  upper_arm_inertia_offset: 0.275 # measured from model
   forearm_mass: 7.879
   wrist_1_mass: 3.054
   wrist_2_mass: 3.126
-  wrist_3_mass: 0.846
+  wrist_3_mass: 0.926
 
   shoulder_radius: x0.060   # FROM UR5 CURRENTLY NOT USED ANYMORE
   upper_arm_radius: x0.054  # FROM UR5 CURRENTLY NOT USED ANYMORE
@@ -34,47 +24,99 @@ inertia_parameters:
     base:
       radius: 0.075
       length: 0.038
-    shoulder:
-      radius: 0.075
-      length: 0.178
-    upperarm:
-      radius: 0.075
-      length: 0.852
-    forearm:
-      radius: 0.075
-      length: 0.7287
-    wrist_1:
-      radius: 0.075
-      length: 0.12
-    wrist_2:
-      radius: 0.075
-      length: 0.12
-    wrist_3:
-      radius: 0.045
-      length: 0.05
 
-  center_of_mass: # Adjusted manually
+  center_of_mass:
     shoulder_cog:
-      x: 0.0
-      y: 0.00244
-      z: -0.037
+      x: 0.0 # model.x
+      y: -0.0062 # -model.z
+      z: -0.061 # model.y
     upper_arm_cog:
-      x: 0.00001
-      y: 0.15061
-      z: 0.48757
+      x: -0.3394 # model.x - upperarm_length
+      y: 0.0 # model.y
+      z: 0.2098 # model.z
     forearm_cog:
-      x: -0.00012
-      y: 0.06112
-      z: 0.2984
+      x: -0.4053 # model.x - forearm_length
+      y: 0.0 # model.y
+      z: 0.0604 # model.z
     wrist_1_cog:
-      x: -0.00021
-      y: -0.00112
-      z: 0.02269
+      x: 0.0 # model.x
+      y: -0.0393 # -model.z
+      z: -0.0026 # model.y
     wrist_2_cog:
-      x: -0.00021
-      y: 0.00112
-      z: 0.002269
+      x: 0.0 # model.x
+      y: 0.0379 # model.z
+      z: -0.0024 # -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.0293 # model.z
+
+  rotation:
+    shoulder:
+      roll: 1.570796326794897
+      pitch: 0
+      yaw: 0
+    upper_arm:
+      roll: 0
+      pitch: 0
+      yaw: 0
+    forearm:
+      roll: 0
+      pitch: 0
+      yaw: 0
+    wrist_1:
+      roll: 1.570796326794897
+      pitch: 0
+      yaw: 0
+    wrist_2:
+      roll: -1.570796326794897
+      pitch: 0
+      yaw: 0
+    wrist_3:
+      roll: 0
+      pitch: 0
+      yaw: 0
+
+  tensor:
+    shoulder:
+      ixx: 0.08866
+      ixy: -0.00011
+      ixz: -0.00011
+      iyy: 0.07632
+      iyz: 0.00720
+      izz: 0.08418
+    upper_arm:
+      ixx: 0.14670
+      ixy: 0.00019
+      ixz: -0.05163
+      iyy: 4.66590
+      iyz: 0.00004
+      izz: 4.63480
+    forearm:
+      ixx: 0.02612
+      ixy: -0.00005
+      ixz: -0.02898
+      iyy: 0.75763
+      iyz: -0.00001
+      izz: 0.75327
+    wrist_1:
+      ixx: 0.00555
+      ixy: -0.00001
+      ixz: -0.00002
+      iyy: 0.00537
+      iyz: 0.00036
+      izz: 0.00402
+    wrist_2:
+      ixx: 0.00586
+      ixy: -0.00001
+      ixz: -0.00002
+      iyy: 0.00578
+      iyz: -0.00037
+      izz: 0.00427
+    wrist_3:
+      ixx: 0.00092
+      ixy: 0.0
+      ixz: 0.0
+      iyy: 0.00091
+      iyz: 0.0
+      izz: 0.00117
diff --git a/config/ur3/physical_parameters.yaml b/config/ur3/physical_parameters.yaml
index ce207b7..e5715df 100644
--- a/config/ur3/physical_parameters.yaml
+++ b/config/ur3/physical_parameters.yaml
@@ -1,13 +1,5 @@
 # Physical parameters
 
-dh_parameters:
-  d1: 0.1519
-  a2: -0.24365
-  a3: -0.21325
-  d4: 0.11235
-  d5: 0.08535
-  d6: 0.0819
-
 offsets:
   shoulder_offset: 0.1198 # measured from model
   elbow_offset: 0.0275 # measured from model
@@ -16,7 +8,6 @@ inertia_parameters:
   base_mass: 2.0  # This mass might be incorrect
   shoulder_mass: 2.0
   upper_arm_mass: 3.42
-  upper_arm_inertia_offset: 0.12
   forearm_mass: 1.26
   wrist_1_mass: 0.8
   wrist_2_mass: 0.8
@@ -32,47 +23,103 @@ inertia_parameters:
     base:
       radius: 0.075
       length: 0.038
-    shoulder:
-      radius: 0.075
-      length: 0.178
-    upperarm:
-      radius: 0.075
-      length: 0.24365
-    forearm:
-      radius: 0.075
-      length: 0.21325
-    wrist_1:
-      radius: 0.075
-      length: 0.08535
-    wrist_2:
-      radius: 0.075
-      length: 0.0819
-    wrist_3:
-      radius: 0.032
-      length: 0.04
 
+  # model referring to https://www.universal-robots.com/articles/ur/application-installation/dh-parameters-for-calculations-of-kinematics-and-dynamics/
   center_of_mass:
     shoulder_cog:
-      x: 0.0
-      y: -0.02
-      z: 0.0
+      x: 0.0       # model.x
+      y: 0.0       # -model.z
+      z: -0.02     # model.y
     upper_arm_cog:
-      x: 0.13
-      y: 0.0
-      z: 0.1157
+      x: -0.11365  # model.x - upper_arm_length
+      y: 0.0       # model.y
+      z: 0.1157    # model.z
     forearm_cog:
-      x: 0.05
-      y: 0.0
-      z: 0.0238
+      x: -0.16325  # model.x - forearm_length
+      y: 0.0       # model.y
+      z: 0.0238    # model.z
     wrist_1_cog:
-      x: 0.0
-      y: 0.0
-      z: 0.01
+      x: 0.0       # model.x
+      y: -0.01     # -model.z
+      z: 0.0       # model.y
     wrist_2_cog:
-      x: 0.0
-      y: 0.0
-      z: 0.01
+      x: 0.0       # model.x
+      y: 0.01      # model.z
+      z: 0.0       # -model.y
     wrist_3_cog:
-      x: 0.0
-      y: 0.0
-      z: -0.02
+      x: 0.0       # model.x
+      y: 0.0       # model.y
+      z: -0.02     # model.z
+
+  # compatible with cylinder approximation
+  rotation:
+    shoulder:
+      roll: 0
+      pitch: 0
+      yaw: 0
+    upper_arm:
+      roll: 0
+      pitch: 1.570796326794897
+      yaw: 0
+    forearm:
+      roll: 0
+      pitch: 1.570796326794897
+      yaw: 0
+    wrist_1:
+      roll: 0
+      pitch: 0
+      yaw: 0
+    wrist_2:
+      roll: 0
+      pitch: 0
+      yaw: 0
+    wrist_3:
+      roll: 0
+      pitch: 0
+      yaw: 0
+
+
+  # generated using cylinder approximation
+  tensor:
+    shoulder:
+      ixx: 0.008093166666666665
+      ixy: 0
+      ixz: 0
+      iyy: 0.008093166666666665
+      iyz: 0
+      izz: 0.005625
+    upper_arm:
+      ixx: 0.021728491912499998
+      ixy: 0
+      ixz: 0
+      iyy: 0.021728491912499998
+      iyz: 0
+      izz: 0.00961875
+    forearm:
+      ixx: 0.0065468090625
+      ixy: 0
+      ixz: 0
+      iyy: 0.0065468090625
+      iyz: 0
+      izz: 0.00354375
+    wrist_1:
+      ixx: 0.0016106414999999998
+      ixy: 0
+      ixz: 0
+      iyy: 0.0016106414999999998
+      iyz: 0
+      izz: 0.00225
+    wrist_2:
+      ixx: 0.0015721739999999998
+      ixy: 0
+      ixz: 0
+      iyy: 0.0015721739999999998
+      iyz: 0
+      izz: 0.00225
+    wrist_3:
+      ixx: 0.00013626666666666665
+      ixy: 0
+      ixz: 0
+      iyy: 0.00013626666666666665
+      iyz: 0
+      izz: 0.0001792
diff --git a/config/ur30/joint_limits.yaml b/config/ur30/joint_limits.yaml
index b122784..0d4b0fb 100644
--- a/config/ur30/joint_limits.yaml
+++ b/config/ur30/joint_limits.yaml
@@ -11,7 +11,7 @@ joint_limits:
     has_effort_limits: true
     has_position_limits: true
     has_velocity_limits: true
-    max_effort: 730.0
+    max_effort: 738.0
     max_position: !degrees  360.0
     max_velocity: !degrees  120.0
     min_position: !degrees -360.0
@@ -21,7 +21,7 @@ joint_limits:
     has_effort_limits: true
     has_position_limits: true
     has_velocity_limits: true
-    max_effort: 730.0
+    max_effort: 738.0
     max_position: !degrees  360.0
     max_velocity: !degrees  120.0
     min_position: !degrees -360.0
@@ -31,7 +31,7 @@ joint_limits:
     has_effort_limits: true
     has_position_limits: true
     has_velocity_limits: true
-    max_effort: 430.0
+    max_effort: 433.0
     # we artificially limit this joint to half its actual joint position limit
     # to avoid (MoveIt/OMPL) planning problems, as due to the physical
     # construction of the robot, it's impossible to rotate the 'elbow_joint'
@@ -51,7 +51,7 @@ joint_limits:
     has_effort_limits: true
     has_position_limits: true
     has_velocity_limits: true
-    max_effort: 100.0
+    max_effort: 107.0
     max_position: !degrees  360.0
     max_velocity: !degrees  210.0
     min_position: !degrees -360.0
@@ -61,7 +61,7 @@ joint_limits:
     has_effort_limits: true
     has_position_limits: true
     has_velocity_limits: true
-    max_effort: 100.0
+    max_effort: 107.0
     max_position: !degrees  360.0
     max_velocity: !degrees  210.0
     min_position: !degrees -360.0
@@ -71,7 +71,7 @@ joint_limits:
     has_effort_limits: true
     has_position_limits: true
     has_velocity_limits: true
-    max_effort: 100.0
+    max_effort: 107.0
     max_position: !degrees  360.0
     max_velocity: !degrees  210.0
     min_position: !degrees -360.0
diff --git a/config/ur30/physical_parameters.yaml b/config/ur30/physical_parameters.yaml
index 695fe62..ca80fd5 100644
--- a/config/ur30/physical_parameters.yaml
+++ b/config/ur30/physical_parameters.yaml
@@ -1,28 +1,18 @@
 # Physical parameters
 
-# from https://www.universal-robots.com/how-tos-and-faqs/faq/ur-faq/parameters-for-calculations-of-kinematics-and-dynamics-45257/
-dh_parameters:
-  d1: 0.2363
-  a2: -0.8620
-  a3: -0.7287
-  d4: 0.201
-  d5: 0.1593
-  d6: 0.1543
-
 offsets:
   shoulder_offset: 0.260 # measured from model
   elbow_offset: 0.043 # measured from model
 
 inertia_parameters:
-  # taken from https://www.universal-robots.com/how-tos-and-faqs/faq/ur-faq/parameters-for-calculations-of-kinematics-and-dynamics-45257/
+  # taken from https://www.universal-robots.com/articles/ur/application-installation/dh-parameters-for-calculations-of-kinematics-and-dynamics/
   base_mass: 4.0  # This mass might be incorrect
   shoulder_mass: 16.343
-  upper_arm_mass: 29.632
-  upper_arm_inertia_offset: 0.275 # measured from model
-  forearm_mass: 7.879
+  upper_arm_mass: 28.542
+  forearm_mass: 7.156
   wrist_1_mass: 3.054
   wrist_2_mass: 3.126
-  wrist_3_mass: 0.846
+  wrist_3_mass: 0.926
 
   shoulder_radius: x0.060   # FROM UR5 CURRENTLY NOT USED ANYMORE
   upper_arm_radius: x0.054  # FROM UR5 CURRENTLY NOT USED ANYMORE
@@ -34,47 +24,99 @@ inertia_parameters:
     base:
       radius: 0.075
       length: 0.038
-    shoulder:
-      radius: 0.075
-      length: 0.178
-    upperarm:
-      radius: 0.075
-      length: 0.637
-    forearm:
-      radius: 0.075
-      length: 0.5037
-    wrist_1:
-      radius: 0.075
-      length: 0.12
-    wrist_2:
-      radius: 0.075
-      length: 0.12
-    wrist_3:
-      radius: 0.045
-      length: 0.05
 
-  center_of_mass: # Adjusted manually
+  center_of_mass:
     shoulder_cog:
-      x: 0.0
-      y: 0.00244
-      z: -0.037
+      x: -0.0001 # model.x
+      y: -0.0069 # -model.z
+      z: -0.06 # model.y
     upper_arm_cog:
-      x: 0.00001
-      y: 0.15061
-      z: 0.40757
+      x: -0.2476 # model.x - upperarm_length
+      y: 0.0 # model.y
+      z: 0.2103 # model.z
     forearm_cog:
-      x: -0.00012
-      y: 0.06112
-      z: 0.2084
+      x: -0.278 # model.x - forearm_length
+      y: 0.0 # model.y
+      z: 0.0629 # model.z
     wrist_1_cog:
-      x: -0.00021
-      y: -0.00112
-      z: 0.02269
+      x: 0.0 # model.x
+      y: -0.0353 # -model.z
+      z: -0.0048 # model.y
     wrist_2_cog:
-      x: -0.00021
-      y: 0.00112
-      z: 0.002269
+      x: 0.0 # model.x
+      y: 0.0341 # model.z
+      z: -0.0046 # -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.0293 # model.z
+
+  rotation:
+    shoulder:
+      roll: 1.570796326794897
+      pitch: 0
+      yaw: 0
+    upper_arm:
+      roll: 0
+      pitch: 0
+      yaw: 0
+    forearm:
+      roll: 0
+      pitch: 0
+      yaw: 0
+    wrist_1:
+      roll: 1.570796326794897
+      pitch: 0
+      yaw: 0
+    wrist_2:
+      roll: -1.570796326794897
+      pitch: 0
+      yaw: 0
+    wrist_3:
+      roll: 0
+      pitch: 0
+      yaw: 0
+
+  tensor:
+    shoulder:
+      ixx: 0.03351
+      ixy: 0.00002
+      ixz: -0.00001
+      iyy: 0.03374
+      iyz: 0.00374
+      izz: 0.02100
+    upper_arm:
+      ixx: 0.02796
+      ixy: -0.00010
+      ixz: -0.00720
+      iyy: 0.47558
+      iyz: 0.00003
+      izz: 0.47635
+    forearm:
+      ixx: 0.01091
+      ixy: 0.00006
+      ixz: 0.01012
+      iyy: 0.12060
+      iyz: 0.00001
+      izz: 0.11714
+    wrist_1:
+      ixx: 0.00609
+      ixy: -0.00001
+      ixz: 0.0
+      iyy: 0.00245
+      iyz: 0.00083
+      izz: 0.00579
+    wrist_2:
+      ixx: 0.00389
+      ixy: 0.00001
+      ixz: 0.0
+      iyy: 0.00219
+      iyz: -0.00045
+      izz: 0.00363
+    wrist_3:
+      ixx: 0.00117
+      ixy: 0.0
+      ixz: 0.0
+      iyy: 0.00118
+      iyz: 0.0
+      izz: 0.00084
diff --git a/config/ur3e/joint_limits.yaml b/config/ur3e/joint_limits.yaml
index 203a3a3..21fad58 100644
--- a/config/ur3e/joint_limits.yaml
+++ b/config/ur3e/joint_limits.yaml
@@ -14,7 +14,7 @@ joint_limits:
     has_effort_limits: true
     has_position_limits: true
     has_velocity_limits: true
-    max_effort: 56.0
+    max_effort: 54.0
     max_position: !degrees  360.0
     max_velocity: !degrees  180.0
     min_position: !degrees -360.0
@@ -24,7 +24,7 @@ joint_limits:
     has_effort_limits: true
     has_position_limits: true
     has_velocity_limits: true
-    max_effort: 56.0
+    max_effort: 54.0
     max_position: !degrees  360.0
     max_velocity: !degrees  180.0
     min_position: !degrees -360.0
@@ -54,7 +54,7 @@ joint_limits:
     has_effort_limits: true
     has_position_limits: true
     has_velocity_limits: true
-    max_effort: 12.0
+    max_effort: 9.0
     max_position: !degrees  360.0
     max_velocity: !degrees  360.0
     min_position: !degrees -360.0
@@ -64,7 +64,7 @@ joint_limits:
     has_effort_limits: true
     has_position_limits: true
     has_velocity_limits: true
-    max_effort: 12.0
+    max_effort: 9.0
     max_position: !degrees  360.0
     max_velocity: !degrees  360.0
     min_position: !degrees -360.0
@@ -74,7 +74,7 @@ joint_limits:
     has_effort_limits: true
     has_position_limits: true
     has_velocity_limits: true
-    max_effort: 12.0
+    max_effort: 9.0
     max_position: !degrees  360.0
     max_velocity: !degrees  360.0
     min_position: !degrees -360.0
diff --git a/config/ur3e/physical_parameters.yaml b/config/ur3e/physical_parameters.yaml
index a8d0b10..4dbff96 100644
--- a/config/ur3e/physical_parameters.yaml
+++ b/config/ur3e/physical_parameters.yaml
@@ -1,13 +1,5 @@
 # Physical parameters
 
-dh_parameters:
-  d1: 0.152
-  a2: -0.244
-  a3: -0.213
-  d4: 0.131  # wrist1_length = d4 - elbow_offset - shoulder_offset
-  d5: 0.085
-  d6: 0.092
-
 offsets:
   shoulder_offset: 0.120 # measured from model
   elbow_offset: 0.027 # measured from model
@@ -16,7 +8,6 @@ inertia_parameters:
   base_mass: 2.0  # This mass might be incorrect
   shoulder_mass: 1.98
   upper_arm_mass: 3.4445
-  upper_arm_inertia_offset: 0.12 # measured from model
   forearm_mass: 1.437
   wrist_1_mass: 0.871
   wrist_2_mass: 0.805
@@ -32,47 +23,102 @@ inertia_parameters:
     base:
       radius: 0.075
       length: 0.038
-    shoulder:
-      radius: 0.075
-      length: 0.178
-    upperarm:
-      radius: 0.075
-      length: 0.24365
-    forearm:
-      radius: 0.075
-      length: 0.2132
-    wrist_1:
-      radius: 0.075
-      length: 0.12
-    wrist_2:
-      radius: 0.075
-      length: 0.12
-    wrist_3:
-      radius: 0.032
-      length: 0.04
 
+  # model referring to https://www.universal-robots.com/articles/ur/application-installation/dh-parameters-for-calculations-of-kinematics-and-dynamics/
   center_of_mass:
     shoulder_cog:
-      x: 0.0
-      y: -0.02
-      z: 0.0
+      x: 0.0        # model.x
+      y: 0.0        # -model.z
+      z: -0.02      # model.y
     upper_arm_cog:
-      x: 0.13
-      y: 0.0
-      z: 0.1157
+      x: -0.11355   # model.x - upper_arm_length
+      y: 0.0        # model.y
+      z: 0.1157     # model.z
     forearm_cog:
-      x: 0.05
-      y: 0.0
-      z: 0.0238
+      x: -0.1632    # model.x - forearm_length
+      y: 0.0        # model.y
+      z: 0.0238     # model.z
     wrist_1_cog:
-      x: 0.0
-      y: 0.0
-      z: 0.01
+      x: 0.0        # model.x
+      y: -0.01      # -model.z
+      z: 0.0        # model.y
     wrist_2_cog:
-      x: 0.0
-      y: 0.0
-      z: 0.01
+      x: 0.0        # model.x
+      y: 0.01       # model.z
+      z: 0.0        # -model.y
     wrist_3_cog:
-      x: 0.0
-      y: 0.0
-      z: -0.02
+      x: 0.0        # model.x
+      y: 0.0        # model.y
+      z: -0.02      # model.z
+
+  # compatible with cylinder approximation
+  rotation:
+    shoulder:
+      roll: 0
+      pitch: 0
+      yaw: 0
+    upper_arm:
+      roll: 0
+      pitch: 1.570796326794897
+      yaw: 0
+    forearm:
+      roll: 0
+      pitch: 1.570796326794897
+      yaw: 0
+    wrist_1:
+      roll: 0
+      pitch: 0
+      yaw: 0
+    wrist_2:
+      roll: 0
+      pitch: 0
+      yaw: 0
+    wrist_3:
+      roll: 0
+      pitch: 0
+      yaw: 0
+
+  # generated using cylinder approximation
+  tensor:
+    shoulder:
+      ixx: 0.008093166666666665
+      ixy: 0
+      ixz: 0
+      iyy: 0.008093166666666665
+      iyz: 0
+      izz: 0.005625
+    upper_arm:
+      ixx: 0.021728491912499998
+      ixy: 0
+      ixz: 0
+      iyy: 0.021728491912499998
+      iyz: 0
+      izz: 0.00961875
+    forearm:
+      ixx: 0.006544570199999999
+      ixy: 0
+      ixz: 0
+      iyy: 0.006544570199999999
+      iyz: 0
+      izz: 0.00354375
+    wrist_1:
+      ixx: 0.0020849999999999996
+      ixy: 0
+      ixz: 0
+      iyy: 0.0020849999999999996
+      iyz: 0
+      izz: 0.00225
+    wrist_2:
+      ixx: 0.0020849999999999996
+      ixy: 0
+      ixz: 0
+      iyy: 0.0020849999999999996
+      iyz: 0
+      izz: 0.00225
+    wrist_3:
+      ixx: 0.00013626666666666665
+      ixy: 0
+      ixz: 0
+      iyy: 0.00013626666666666665
+      iyz: 0
+      izz: 0.0001792
diff --git a/config/ur5/physical_parameters.yaml b/config/ur5/physical_parameters.yaml
index 07d1ebc..52eb585 100644
--- a/config/ur5/physical_parameters.yaml
+++ b/config/ur5/physical_parameters.yaml
@@ -1,13 +1,5 @@
 # Physical parameters
 
-dh_parameters:
-  d1: 0.089159
-  a2: -0.42500
-  a3: -0.39225
-  d4: 0.10915
-  d5: 0.09465
-  d6: 0.0823
-
 offsets:
   shoulder_offset: 0.13585 # measured from model
   elbow_offset: 0.0165 # measured from model
@@ -16,7 +8,6 @@ inertia_parameters:
   base_mass: 4.0  # This mass might be incorrect
   shoulder_mass: 3.7000
   upper_arm_mass: 8.3930
-  upper_arm_inertia_offset: 0.136
   forearm_mass: 2.33
   wrist_1_mass: 1.2190
   wrist_2_mass: 1.2190
@@ -32,56 +23,102 @@ inertia_parameters:
     base:
       radius: 0.06
       length: 0.05
-    shoulder:
-      radius: 0.075
-      length: 0.178
-    upperarm:
-      radius: 0.06
-      length: 0.425
-    forearm:
-      radius: 0.06
-      length: 0.39225
-    wrist_1:
-      radius: 0.06
-      length: 0.12
-    wrist_2:
-      radius: 0.06
-      length: 0.12
-    wrist_3:
-      radius: 0.0375
-      length: 0.0345
-    wrist_1:
-      radius: 0.06
-      length: 0.095
-    wrist_2:
-      radius: 0.06
-      length: 0.085
-    wrist_3:
-      radius: 0.0375
-      length: 0.0305
 
+  # model referring to https://www.universal-robots.com/articles/ur/application-installation/dh-parameters-for-calculations-of-kinematics-and-dynamics/
   center_of_mass:
     shoulder_cog:
-      x: 0.0
-      y: 0.00193
-      z: -0.02561
+      x: 0.0       # model.x
+      y: -0.00193  # -model.z
+      z: -0.02561  # model.y
     upper_arm_cog:
-      x: 0.0
-      y: -0.024201
-      z: 0.2125
+      x: -0.2125   # model.x - upper_arm_length
+      y: 0.0       # model.y
+      z: 0.11336   # model.z
     forearm_cog:
-      x: 0.0
-      y: 0.0265
-      z: 0.11993
+      x: -0.24225  # model.x - forearm_length
+      y: 0.0       # model.y
+      z: 0.0265    # model.z
     wrist_1_cog:
-      x: 0.0
-      y: 0.110949
-      z: 0.01634
+      x: 0.0       # model.x
+      y: -0.01634  # -model.z
+      z: -0.0018   # model.y
     wrist_2_cog:
-      x: 0.0
-      y: 0.0018
-      z: 0.11099
+      x: 0.0       # model.x
+      y: 0.01634   # model.z
+      z: -0.0018   # -model.y
     wrist_3_cog:
-      x: 0.0
-      y: 0.001159
-      z: 0.0
+      x: 0.0       # model.x
+      y: 0.0       # model.y
+      z: -0.001159 # model.z
+
+  # compatible with cylinder approximation
+  rotation:
+    shoulder:
+      roll: 0
+      pitch: 0
+      yaw: 0
+    upper_arm:
+      roll: 0
+      pitch: 1.570796326794897
+      yaw: 0
+    forearm:
+      roll: 0
+      pitch: 1.570796326794897
+      yaw: 0
+    wrist_1:
+      roll: 0
+      pitch: 0
+      yaw: 0
+    wrist_2:
+      roll: 0
+      pitch: 0
+      yaw: 0
+    wrist_3:
+      roll: 0
+      pitch: 0
+      yaw: 0
+
+  # generated using cylinder approximation
+  tensor:
+    shoulder:
+      ixx: 0.014972358333333331
+      ixy: 0
+      ixz: 0
+      iyy: 0.014972358333333331
+      iyz: 0
+      izz: 0.01040625
+    upper_arm:
+      ixx: 0.13388583541666665
+      ixy: 0
+      ixz: 0
+      iyy: 0.13388583541666665
+      iyz: 0
+      izz: 0.0151074
+    forearm:
+      ixx: 0.031216803515624995
+      ixy: 0
+      ixz: 0
+      iyy: 0.031216803515624995
+      iyz: 0
+      izz: 0.004095
+    wrist_1:
+      ixx: 0.002013889583333333
+      ixy: 0
+      ixz: 0
+      iyy: 0.002013889583333333
+      iyz: 0
+      izz: 0.0021942
+    wrist_2:
+      ixx: 0.0018310395833333333
+      ixy: 0
+      ixz: 0
+      iyy: 0.0018310395833333333
+      iyz: 0
+      izz: 0.0021942
+    wrist_3:
+      ixx: 8.062475833333332e-05
+      ixy: 0
+      ixz: 0
+      iyy: 8.062475833333332e-05
+      iyz: 0
+      izz: 0.0001321171875
diff --git a/config/ur5e/physical_parameters.yaml b/config/ur5e/physical_parameters.yaml
index 6686a21..8779dbe 100644
--- a/config/ur5e/physical_parameters.yaml
+++ b/config/ur5e/physical_parameters.yaml
@@ -1,13 +1,5 @@
 # Physical parameters
 
-dh_parameters:
-  d1: 0.163
-  a2: -0.42500
-  a3: -0.39225
-  d4: 0.134         # wrist1_length = d4 - elbow_offset - shoulder_offset
-  d5: 0.100
-  d6: 0.100
-
 offsets:
   shoulder_offset: 0.138 # measured from model
   elbow_offset: 0.007 # measured from model
@@ -16,7 +8,6 @@ inertia_parameters:
   base_mass: 4.0  # This mass might be incorrect
   shoulder_mass: 3.761
   upper_arm_mass: 8.058
-  upper_arm_inertia_offset: 0.138 # measured from model
   forearm_mass: 2.846
   wrist_1_mass: 1.37
   wrist_2_mass: 1.3
@@ -32,47 +23,102 @@ inertia_parameters:
     base:
       radius: 0.06
       length: 0.05
-    shoulder:
-      radius: 0.06
-      length: 0.15
-    upperarm:
-      radius: 0.06
-      length: 0.425
-    forearm:
-      radius: 0.06
-      length: 0.3922
-    wrist_1:
-      radius: 0.06
-      length: 0.12
-    wrist_2:
-      radius: 0.06
-      length: 0.12
-    wrist_3:
-      radius: 0.0375
-      length: 0.0458
 
+  # model referring to https://www.universal-robots.com/articles/ur/application-installation/dh-parameters-for-calculations-of-kinematics-and-dynamics/
   center_of_mass:
     shoulder_cog:
-      x: 0.0
-      y: 0.00193
-      z: -0.02561
+      x: 0.0        # model.x
+      y: -0.00193   # -model.z
+      z: -0.02561   # model.y
     upper_arm_cog:
-      x: 0.0
-      y: -0.024201
-      z: 0.2125
+      x: -0.2125    # model.x - upper_arm_length
+      y: 0.0        # model.y
+      z: 0.11336    # model.z
     forearm_cog:
-      x: 0.0
-      y: 0.0265
-      z: 0.11993
+      x: -0.2422    # model.x - forearm_length
+      y: 0.0        # model.y
+      z: 0.0265     # model.z
     wrist_1_cog:
-      x: 0.0
-      y: 0.110949
-      z: 0.01634
+      x: 0.0        # model.x
+      y: -0.01634   # -model.z
+      z: -0.0018    # model.y
     wrist_2_cog:
-      x: 0.0
-      y: 0.0018
-      z: 0.11099
+      x: 0.0        # model.x
+      y: 0.01634    # model.z
+      z: -0.0018    # -model.y
     wrist_3_cog:
-      x: 0.0
-      y: 0.001159
-      z: 0.0
+      x: 0.0        # model.x
+      y: 0.0        # model.y
+      z: -0.001159  # model.z
+
+  # compatible with cylinder approximation
+  rotation:
+    shoulder:
+      roll: 0
+      pitch: 0
+      yaw: 0
+    upper_arm:
+      roll: 0
+      pitch: 1.570796326794897
+      yaw: 0
+    forearm:
+      roll: 0
+      pitch: 1.570796326794897
+      yaw: 0
+    wrist_1:
+      roll: 0
+      pitch: 0
+      yaw: 0
+    wrist_2:
+      roll: 0
+      pitch: 0
+      yaw: 0
+    wrist_3:
+      roll: 0
+      pitch: 0
+      yaw: 0
+
+  # generated using cylinder approximation
+  tensor:
+    shoulder:
+      ixx: 0.010267499999999999
+      ixy: 0
+      ixz: 0
+      iyy: 0.010267499999999999
+      iyz: 0
+      izz: 0.00666
+    upper_arm:
+      ixx: 0.13388583541666665
+      ixy: 0
+      ixz: 0
+      iyy: 0.13388583541666665
+      iyz: 0
+      izz: 0.0151074
+    forearm:
+      ixx: 0.03120936758333333
+      ixy: 0
+      ixz: 0
+      iyy: 0.03120936758333333
+      iyz: 0
+      izz: 0.004095
+    wrist_1:
+      ixx: 0.0025599
+      ixy: 0
+      ixz: 0
+      iyy: 0.0025599
+      iyz: 0
+      izz: 0.0021942
+    wrist_2:
+      ixx: 0.0025599
+      ixy: 0
+      ixz: 0
+      iyy: 0.0025599
+      iyz: 0
+      izz: 0.0021942
+    wrist_3:
+      ixx: 9.890414008333333e-05
+      ixy: 0
+      ixz: 0
+      iyy: 9.890414008333333e-05
+      iyz: 0
+      izz: 0.0001321171875
diff --git a/urdf/inc/ur_common.xacro b/urdf/inc/ur_common.xacro
index a763e78..54f0b44 100644
--- a/urdf/inc/ur_common.xacro
+++ b/urdf/inc/ur_common.xacro
@@ -52,7 +52,6 @@
 
     <!-- Extract subsections from yaml dictionaries -->
     <xacro:property name="sec_limits" value="${config_joint_limit_parameters['joint_limits']}"/>
-    <xacro:property name="sec_dh_parameters" value="${config_physical_parameters['dh_parameters']}"/>
     <xacro:property name="sec_offsets" value="${config_physical_parameters['offsets']}"/>
     <xacro:property name="sec_inertia_parameters" value="${config_physical_parameters['inertia_parameters']}" />
     <xacro:property name="sec_mesh_files" value="${config_visual_parameters['mesh_files']}" scope="parent"/>
@@ -84,14 +83,6 @@
     <xacro:property name="wrist_3_velocity_limit" value="${sec_limits['wrist_3_joint']['max_velocity']}" scope="parent"/>
     <xacro:property name="wrist_3_effort_limit" value="${sec_limits['wrist_3_joint']['max_effort']}" scope="parent"/>
 
-    <!-- DH PARAMETERS -->
-    <xacro:property name="d1" value="${sec_dh_parameters['d1']}" scope="parent"/>
-    <xacro:property name="a2" value="${sec_dh_parameters['a2']}" scope="parent"/>
-    <xacro:property name="a3" value="${sec_dh_parameters['a3']}" scope="parent"/>
-    <xacro:property name="d4" value="${sec_dh_parameters['d4']}" scope="parent"/>
-    <xacro:property name="d5" value="${sec_dh_parameters['d5']}" scope="parent"/>
-    <xacro:property name="d6" value="${sec_dh_parameters['d6']}" scope="parent"/>
-
     <!-- kinematics -->
     <xacro:property name="shoulder_x" value="${sec_kinematics['shoulder']['x']}" scope="parent"/>
     <xacro:property name="shoulder_y" value="${sec_kinematics['shoulder']['y']}" scope="parent"/>
@@ -144,7 +135,6 @@
     <xacro:property name="base_mass" value="${sec_inertia_parameters['base_mass']}" scope="parent"/>
     <xacro:property name="shoulder_mass" value="${sec_inertia_parameters['shoulder_mass']}" scope="parent"/>
     <xacro:property name="upper_arm_mass" value="${sec_inertia_parameters['upper_arm_mass']}" scope="parent"/>
-    <xacro:property name="upper_arm_inertia_offset" value="${sec_inertia_parameters['upper_arm_inertia_offset']}" scope="parent"/>
     <xacro:property name="forearm_mass" value="${sec_inertia_parameters['forearm_mass']}" scope="parent"/>
     <xacro:property name="wrist_1_mass" value="${sec_inertia_parameters['wrist_1_mass']}" scope="parent"/>
     <xacro:property name="wrist_2_mass" value="${sec_inertia_parameters['wrist_2_mass']}" scope="parent"/>
@@ -153,33 +143,82 @@
     <xacro:property name="intertia_links" value="${sec_inertia_parameters['links']}" scope="parent"/>
     <xacro:property name="base_inertia_radius" value="${intertia_links['base']['radius']}" scope="parent"/>
     <xacro:property name="base_inertia_length" value="${intertia_links['base']['length']}" scope="parent"/>
-    <xacro:property name="shoulder_inertia_radius" value="${intertia_links['shoulder']['radius']}" scope="parent"/>
-    <xacro:property name="shoulder_inertia_length" value="${intertia_links['shoulder']['length']}" scope="parent"/>
-    <xacro:property name="upperarm_inertia_radius" value="${intertia_links['upperarm']['radius']}" scope="parent"/>
-    <xacro:property name="upperarm_inertia_length" value="${intertia_links['upperarm']['length']}" scope="parent"/>
-    <xacro:property name="forearm_inertia_radius" value="${intertia_links['forearm']['radius']}" scope="parent"/>
-    <xacro:property name="forearm_inertia_length" value="${intertia_links['forearm']['length']}" scope="parent"/>
-    <xacro:property name="wrist_1_inertia_radius" value="${intertia_links['wrist_1']['radius']}" scope="parent"/>
-    <xacro:property name="wrist_1_inertia_length" value="${intertia_links['wrist_1']['length']}" scope="parent"/>
-    <xacro:property name="wrist_2_inertia_radius" value="${intertia_links['wrist_2']['radius']}" scope="parent"/>
-    <xacro:property name="wrist_2_inertia_length" value="${intertia_links['wrist_2']['length']}" scope="parent"/>
-    <xacro:property name="wrist_3_inertia_radius" value="${intertia_links['wrist_3']['radius']}" scope="parent"/>
-    <xacro:property name="wrist_3_inertia_length" value="${intertia_links['wrist_3']['length']}" scope="parent"/>
 
     <!-- center of mass -->
     <xacro:property name="prop_shoulder_cog" value="${sec_inertia_parameters['center_of_mass']['shoulder_cog']}" scope="parent"/>
     <xacro:property name="prop_upper_arm_cog" value="${sec_inertia_parameters['center_of_mass']['upper_arm_cog']}" scope="parent"/>
     <xacro:property name="prop_forearm_cog" value="${sec_inertia_parameters['center_of_mass']['forearm_cog']}" scope="parent"/>
     <xacro:property name="prop_wrist_1_cog" value="${sec_inertia_parameters['center_of_mass']['wrist_1_cog']}" scope="parent"/>
-    <xacro:property name="prop__wrist_2_cog" value="${sec_inertia_parameters['center_of_mass']['wrist_2_cog']}" scope="parent"/>
+    <xacro:property name="prop_wrist_2_cog" value="${sec_inertia_parameters['center_of_mass']['wrist_2_cog']}" scope="parent"/>
     <xacro:property name="prop_wrist_3_cog" value="${sec_inertia_parameters['center_of_mass']['wrist_3_cog']}" scope="parent"/>
 
     <xacro:property name="shoulder_cog" value="${prop_shoulder_cog['x']} ${prop_shoulder_cog['y']} ${prop_shoulder_cog['z']}" scope="parent"/>
     <xacro:property name="upper_arm_cog" value="${prop_upper_arm_cog['x']} ${prop_upper_arm_cog['y']} ${prop_upper_arm_cog['z']}" scope="parent"/>
     <xacro:property name="forearm_cog" value="${prop_forearm_cog['x']} ${prop_forearm_cog['y']} ${prop_forearm_cog['z']}" scope="parent"/>
     <xacro:property name="wrist_1_cog" value="${prop_wrist_1_cog['x']} ${prop_wrist_1_cog['y']} ${prop_wrist_1_cog['z']}" scope="parent"/>
-    <xacro:property name="wrist_2_cog" value="${prop__wrist_2_cog['x']} ${prop__wrist_2_cog['y']} ${prop__wrist_2_cog['z']}" scope="parent"/>
+    <xacro:property name="wrist_2_cog" value="${prop_wrist_2_cog['x']} ${prop_wrist_2_cog['y']} ${prop_wrist_2_cog['z']}" scope="parent"/>
     <xacro:property name="wrist_3_cog" value="${prop_wrist_3_cog['x']} ${prop_wrist_3_cog['y']} ${prop_wrist_3_cog['z']}" scope="parent"/>
+
+    <!-- inertia rotation -->
+    <xacro:property name="prop_shoulder_inertia_rotation" value="${sec_inertia_parameters['rotation']['shoulder']}" scope="parent"/>
+    <xacro:property name="prop_upper_arm_inertia_rotation" value="${sec_inertia_parameters['rotation']['upper_arm']}" scope="parent"/>
+    <xacro:property name="prop_forearm_inertia_rotation" value="${sec_inertia_parameters['rotation']['forearm']}" scope="parent"/>
+    <xacro:property name="prop_wrist_1_inertia_rotation" value="${sec_inertia_parameters['rotation']['wrist_1']}" scope="parent"/>
+    <xacro:property name="prop_wrist_2_inertia_rotation" value="${sec_inertia_parameters['rotation']['wrist_2']}" scope="parent"/>
+    <xacro:property name="prop_wrist_3_inertia_rotation" value="${sec_inertia_parameters['rotation']['wrist_3']}" scope="parent"/>
+
+    <xacro:property name="shoulder_inertia_rotation" value="${prop_shoulder_inertia_rotation['roll']} ${prop_shoulder_inertia_rotation['pitch']} ${prop_shoulder_inertia_rotation['yaw']}" scope="parent"/>
+    <xacro:property name="upper_arm_inertia_rotation" value="${prop_upper_arm_inertia_rotation['roll']} ${prop_upper_arm_inertia_rotation['pitch']} ${prop_upper_arm_inertia_rotation['yaw']}" scope="parent"/>
+    <xacro:property name="forearm_inertia_rotation" value="${prop_forearm_inertia_rotation['roll']} ${prop_forearm_inertia_rotation['pitch']} ${prop_forearm_inertia_rotation['yaw']}" scope="parent"/>
+    <xacro:property name="wrist_1_inertia_rotation" value="${prop_wrist_1_inertia_rotation['roll']} ${prop_wrist_1_inertia_rotation['pitch']} ${prop_wrist_1_inertia_rotation['yaw']}" scope="parent"/>
+    <xacro:property name="wrist_2_inertia_rotation" value="${prop_wrist_2_inertia_rotation['roll']} ${prop_wrist_2_inertia_rotation['pitch']} ${prop_wrist_2_inertia_rotation['yaw']}" scope="parent"/>
+    <xacro:property name="wrist_3_inertia_rotation" value="${prop_wrist_3_inertia_rotation['roll']} ${prop_wrist_3_inertia_rotation['pitch']} ${prop_wrist_3_inertia_rotation['yaw']}" scope="parent"/>
+
+    <!-- tensors -->
+    <xacro:property name="prop_shoulder_inertia" value="${sec_inertia_parameters['tensor']['shoulder']}" scope="parent"/>
+    <xacro:property name="prop_upper_arm_inertia" value="${sec_inertia_parameters['tensor']['upper_arm']}" scope="parent"/>
+    <xacro:property name="prop_forearm_inertia" value="${sec_inertia_parameters['tensor']['forearm']}" scope="parent"/>
+    <xacro:property name="prop_wrist_1_inertia" value="${sec_inertia_parameters['tensor']['wrist_1']}" scope="parent"/>
+    <xacro:property name="prop_wrist_2_inertia" value="${sec_inertia_parameters['tensor']['wrist_2']}" scope="parent"/>
+    <xacro:property name="prop_wrist_3_inertia" value="${sec_inertia_parameters['tensor']['wrist_3']}" scope="parent"/>
+
+    <xacro:property name="shoulder_inertia_ixx" value="${prop_shoulder_inertia['ixx']}" scope="parent"/>
+    <xacro:property name="shoulder_inertia_ixy" value="${prop_shoulder_inertia['ixy']}" scope="parent"/>
+    <xacro:property name="shoulder_inertia_ixz" value="${prop_shoulder_inertia['ixz']}" scope="parent"/>
+    <xacro:property name="shoulder_inertia_iyy" value="${prop_shoulder_inertia['iyy']}" scope="parent"/>
+    <xacro:property name="shoulder_inertia_iyz" value="${prop_shoulder_inertia['iyz']}" scope="parent"/>
+    <xacro:property name="shoulder_inertia_izz" value="${prop_shoulder_inertia['izz']}" scope="parent"/>
+    <xacro:property name="upper_arm_inertia_ixx" value="${prop_upper_arm_inertia['ixx']}" scope="parent"/>
+    <xacro:property name="upper_arm_inertia_ixy" value="${prop_upper_arm_inertia['ixy']}" scope="parent"/>
+    <xacro:property name="upper_arm_inertia_ixz" value="${prop_upper_arm_inertia['ixz']}" scope="parent"/>
+    <xacro:property name="upper_arm_inertia_iyy" value="${prop_upper_arm_inertia['iyy']}" scope="parent"/>
+    <xacro:property name="upper_arm_inertia_iyz" value="${prop_upper_arm_inertia['iyz']}" scope="parent"/>
+    <xacro:property name="upper_arm_inertia_izz" value="${prop_upper_arm_inertia['izz']}" scope="parent"/>
+    <xacro:property name="forearm_inertia_ixx" value="${prop_forearm_inertia['ixx']}" scope="parent"/>
+    <xacro:property name="forearm_inertia_ixy" value="${prop_forearm_inertia['ixy']}" scope="parent"/>
+    <xacro:property name="forearm_inertia_ixz" value="${prop_forearm_inertia['ixz']}" scope="parent"/>
+    <xacro:property name="forearm_inertia_iyy" value="${prop_forearm_inertia['iyy']}" scope="parent"/>
+    <xacro:property name="forearm_inertia_iyz" value="${prop_forearm_inertia['iyz']}" scope="parent"/>
+    <xacro:property name="forearm_inertia_izz" value="${prop_forearm_inertia['izz']}" scope="parent"/>
+    <xacro:property name="wrist_1_inertia_ixx" value="${prop_wrist_1_inertia['ixx']}" scope="parent"/>
+    <xacro:property name="wrist_1_inertia_ixy" value="${prop_wrist_1_inertia['ixy']}" scope="parent"/>
+    <xacro:property name="wrist_1_inertia_ixz" value="${prop_wrist_1_inertia['ixz']}" scope="parent"/>
+    <xacro:property name="wrist_1_inertia_iyy" value="${prop_wrist_1_inertia['iyy']}" scope="parent"/>
+    <xacro:property name="wrist_1_inertia_iyz" value="${prop_wrist_1_inertia['iyz']}" scope="parent"/>
+    <xacro:property name="wrist_1_inertia_izz" value="${prop_wrist_1_inertia['izz']}" scope="parent"/>
+    <xacro:property name="wrist_2_inertia_ixx" value="${prop_wrist_2_inertia['ixx']}" scope="parent"/>
+    <xacro:property name="wrist_2_inertia_ixy" value="${prop_wrist_2_inertia['ixy']}" scope="parent"/>
+    <xacro:property name="wrist_2_inertia_ixz" value="${prop_wrist_2_inertia['ixz']}" scope="parent"/>
+    <xacro:property name="wrist_2_inertia_iyy" value="${prop_wrist_2_inertia['iyy']}" scope="parent"/>
+    <xacro:property name="wrist_2_inertia_iyz" value="${prop_wrist_2_inertia['iyz']}" scope="parent"/>
+    <xacro:property name="wrist_2_inertia_izz" value="${prop_wrist_2_inertia['izz']}" scope="parent"/>
+    <xacro:property name="wrist_3_inertia_ixx" value="${prop_wrist_3_inertia['ixx']}" scope="parent"/>
+    <xacro:property name="wrist_3_inertia_ixy" value="${prop_wrist_3_inertia['ixy']}" scope="parent"/>
+    <xacro:property name="wrist_3_inertia_ixz" value="${prop_wrist_3_inertia['ixz']}" scope="parent"/>
+    <xacro:property name="wrist_3_inertia_iyy" value="${prop_wrist_3_inertia['iyy']}" scope="parent"/>
+    <xacro:property name="wrist_3_inertia_iyz" value="${prop_wrist_3_inertia['iyz']}" scope="parent"/>
+    <xacro:property name="wrist_3_inertia_izz" value="${prop_wrist_3_inertia['izz']}" scope="parent"/>
+
     <!-- cylinder radius -->
     <xacro:property name="shoulder_radius" value="${sec_inertia_parameters['shoulder_radius']}" scope="parent"/>
     <xacro:property name="upper_arm_radius" value="${sec_inertia_parameters['upper_arm_radius']}" scope="parent"/>
diff --git a/urdf/ur_macro.xacro b/urdf/ur_macro.xacro
index 32031e5..8b2dc86 100644
--- a/urdf/ur_macro.xacro
+++ b/urdf/ur_macro.xacro
@@ -178,10 +178,18 @@
           <xacro:get_mesh name="shoulder" type="collision"/>
         </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" />
-      </xacro:cylinder_inertial>
-    </link>
+      <inertial>
+        <mass value="${shoulder_mass}"/>
+        <origin rpy="${shoulder_inertia_rotation}" xyz="${shoulder_cog}"/>
+        <inertia
+            ixx="${shoulder_inertia_ixx}"
+            ixy="${shoulder_inertia_ixy}"
+            ixz="${shoulder_inertia_ixz}"
+            iyy="${shoulder_inertia_iyy}"
+            iyz="${shoulder_inertia_iyz}"
+            izz="${shoulder_inertia_izz}"
+        />
+      </inertial>    </link>
     <link name="${tf_prefix}upper_arm_link">
       <visual>
         <origin xyz="0 0 ${shoulder_offset}" rpy="${pi/2} 0 ${-pi/2}"/>
@@ -195,9 +203,18 @@
           <xacro:get_mesh name="upper_arm" type="collision"/>
         </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" />
-      </xacro:cylinder_inertial>
+      <inertial>
+        <mass value="${upper_arm_mass}"/>
+        <origin rpy="${upper_arm_inertia_rotation}" xyz="${upper_arm_cog}"/>
+        <inertia
+            ixx="${upper_arm_inertia_ixx}"
+            ixy="${upper_arm_inertia_ixy}"
+            ixz="${upper_arm_inertia_ixz}"
+            iyy="${upper_arm_inertia_iyy}"
+            iyz="${upper_arm_inertia_iyz}"
+            izz="${upper_arm_inertia_izz}"
+        />
+      </inertial>
     </link>
     <link name="${tf_prefix}forearm_link">
       <visual>
@@ -212,9 +229,18 @@
           <xacro:get_mesh name="forearm" type="collision"/>
         </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" />
-      </xacro:cylinder_inertial>
+      <inertial>
+        <mass value="${forearm_mass}"/>
+        <origin rpy="${forearm_inertia_rotation}" xyz="${forearm_cog}"/>
+        <inertia
+            ixx="${forearm_inertia_ixx}"
+            ixy="${forearm_inertia_ixy}"
+            ixz="${forearm_inertia_ixz}"
+            iyy="${forearm_inertia_iyy}"
+            iyz="${forearm_inertia_iyz}"
+            izz="${forearm_inertia_izz}"
+        />
+      </inertial>
     </link>
     <link name="${tf_prefix}wrist_1_link">
 	  <xacro:get_visual_params name="wrist_1" type="visual_offset"/>
@@ -230,9 +256,18 @@
           <xacro:get_mesh name="wrist_1" type="collision"/>
         </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" />
-      </xacro:cylinder_inertial>
+      <inertial>
+        <mass value="${wrist_1_mass}"/>
+        <origin rpy="${wrist_1_inertia_rotation}" xyz="${wrist_1_cog}"/>
+        <inertia
+            ixx="${wrist_1_inertia_ixx}"
+            ixy="${wrist_1_inertia_ixy}"
+            ixz="${wrist_1_inertia_ixz}"
+            iyy="${wrist_1_inertia_iyy}"
+            iyz="${wrist_1_inertia_iyz}"
+            izz="${wrist_1_inertia_izz}"
+        />
+      </inertial>
     </link>
     <link name="${tf_prefix}wrist_2_link">
 	  <xacro:get_visual_params name="wrist_2" type="visual_offset"/>
@@ -248,9 +283,18 @@
           <xacro:get_mesh name="wrist_2" type="collision"/>
         </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" />
-      </xacro:cylinder_inertial>
+      <inertial>
+        <mass value="${wrist_2_mass}"/>
+        <origin rpy="${wrist_2_inertia_rotation}" xyz="${wrist_2_cog}"/>
+        <inertia
+            ixx="${wrist_2_inertia_ixx}"
+            ixy="${wrist_2_inertia_ixy}"
+            ixz="${wrist_2_inertia_ixz}"
+            iyy="${wrist_2_inertia_iyy}"
+            iyz="${wrist_2_inertia_iyz}"
+            izz="${wrist_2_inertia_izz}"
+        />
+      </inertial>
     </link>
     <link name="${tf_prefix}wrist_3_link">
 	  <xacro:get_visual_params name="wrist_3" type="visual_offset"/>
@@ -266,9 +310,18 @@
           <xacro:get_mesh name="wrist_3" type="collision"/>
         </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" />
-      </xacro:cylinder_inertial>
+      <inertial>
+        <mass value="${wrist_3_mass}"/>
+        <origin rpy="${wrist_3_inertia_rotation}" xyz="${wrist_3_cog}"/>
+        <inertia
+            ixx="${wrist_3_inertia_ixx}"
+            ixy="${wrist_3_inertia_ixy}"
+            ixz="${wrist_3_inertia_ixz}"
+            iyy="${wrist_3_inertia_iyy}"
+            iyz="${wrist_3_inertia_iyz}"
+            izz="${wrist_3_inertia_izz}"
+        />
+      </inertial>
     </link>
 
     <!-- base_joint fixes base_link to the environment -->