From 5743127663b32bd4631996e3cc476d04d2878993 Mon Sep 17 00:00:00 2001
From: URJala <jala@universal-robots.com>
Date: Tue, 16 Jul 2024 07:25:04 +0000
Subject: [PATCH 1/3] Change URDF to reflect ur3(e)'s infinite wrist 3 joint

---
 config/ur3/joint_limits.yaml  |  4 +---
 config/ur3e/joint_limits.yaml |  4 +---
 urdf/inc/ur_common.xacro      |  6 +++--
 urdf/ur_macro.xacro           | 42 +++++++++++++++++++++++++----------
 4 files changed, 36 insertions(+), 20 deletions(-)

diff --git a/config/ur3/joint_limits.yaml b/config/ur3/joint_limits.yaml
index d163e878..bdb45d9f 100644
--- a/config/ur3/joint_limits.yaml
+++ b/config/ur3/joint_limits.yaml
@@ -72,9 +72,7 @@ joint_limits:
     # acceleration limits are not publicly available
     has_acceleration_limits: false
     has_effort_limits: true
-    has_position_limits: true
+    has_position_limits: false
     has_velocity_limits: true
     max_effort: 12.0
-    max_position: !degrees  360.0
     max_velocity: !degrees  360.0
-    min_position: !degrees -360.0
diff --git a/config/ur3e/joint_limits.yaml b/config/ur3e/joint_limits.yaml
index 21fad58b..f2449d98 100644
--- a/config/ur3e/joint_limits.yaml
+++ b/config/ur3e/joint_limits.yaml
@@ -72,9 +72,7 @@ joint_limits:
     # acceleration limits are not publicly available
     has_acceleration_limits: false
     has_effort_limits: true
-    has_position_limits: true
+    has_position_limits: false
     has_velocity_limits: true
     max_effort: 9.0
-    max_position: !degrees  360.0
     max_velocity: !degrees  360.0
-    min_position: !degrees -360.0
diff --git a/urdf/inc/ur_common.xacro b/urdf/inc/ur_common.xacro
index 54f0b444..9919147c 100644
--- a/urdf/inc/ur_common.xacro
+++ b/urdf/inc/ur_common.xacro
@@ -78,8 +78,10 @@
     <xacro:property name="wrist_2_upper_limit" value="${sec_limits['wrist_2_joint']['max_position']}" scope="parent"/>
     <xacro:property name="wrist_2_velocity_limit" value="${sec_limits['wrist_2_joint']['max_velocity']}" scope="parent"/>
     <xacro:property name="wrist_2_effort_limit" value="${sec_limits['wrist_2_joint']['max_effort']}" scope="parent"/>
-    <xacro:property name="wrist_3_lower_limit" value="${sec_limits['wrist_3_joint']['min_position']}" scope="parent"/>
-    <xacro:property name="wrist_3_upper_limit" value="${sec_limits['wrist_3_joint']['max_position']}" scope="parent"/>
+    <xacro:if value="${sec_limits['wrist_3_joint']['has_position_limits']}">
+      <xacro:property name="wrist_3_lower_limit" value="${sec_limits['wrist_3_joint']['min_position']}" scope="parent"/>
+      <xacro:property name="wrist_3_upper_limit" value="${sec_limits['wrist_3_joint']['max_position']}" scope="parent"/>
+    </xacro:if>
     <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"/>
 
diff --git a/urdf/ur_macro.xacro b/urdf/ur_macro.xacro
index bde7e7ef..d6218475 100644
--- a/urdf/ur_macro.xacro
+++ b/urdf/ur_macro.xacro
@@ -75,6 +75,9 @@
       visual_parameters_file="${visual_parameters_file}"
       force_abs_paths="${force_abs_paths}"/>
 
+    <xacro:property name="config_joint_limit_parameters" value="${xacro.load_yaml(joint_limits_parameters_file)}"/>
+    <xacro:property name="sec_limits" value="${config_joint_limit_parameters['joint_limits']}"/>
+
     <!-- links -  main serial chain -->
     <link name="${tf_prefix}base_link"/>
     <link name="${tf_prefix}base_link_inertia">
@@ -331,18 +334,33 @@
       </xacro:if>
       <dynamics damping="0" friction="0"/>
     </joint>
-    <joint name="${tf_prefix}wrist_3_joint" type="revolute">
-      <parent link="${tf_prefix}wrist_2_link" />
-      <child link="${tf_prefix}wrist_3_link" />
-      <origin xyz="${wrist_3_x} ${wrist_3_y} ${wrist_3_z}" rpy="${wrist_3_roll} ${wrist_3_pitch} ${wrist_3_yaw}" />
-      <axis xyz="0 0 1" />
-      <limit lower="${wrist_3_lower_limit}" upper="${wrist_3_upper_limit}"
-             effort="${wrist_3_effort_limit}" velocity="${wrist_3_velocity_limit}"/>
-      <xacro:if value="${safety_limits}">
-         <safety_controller soft_lower_limit="${wrist_3_lower_limit + safety_pos_margin}" soft_upper_limit="${wrist_3_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
-      </xacro:if>
-      <dynamics damping="0" friction="0"/>
-    </joint>
+    <xacro:if value="${sec_limits['wrist_3_joint']['has_position_limits']}">
+      <joint name="${tf_prefix}wrist_3_joint" type="revolute">
+        <parent link="${tf_prefix}wrist_2_link" />
+        <child link="${tf_prefix}wrist_3_link" />
+        <origin xyz="${wrist_3_x} ${wrist_3_y} ${wrist_3_z}" rpy="${wrist_3_roll} ${wrist_3_pitch} ${wrist_3_yaw}" />
+        <axis xyz="0 0 1" />
+        <limit lower="${wrist_3_lower_limit}" upper="${wrist_3_upper_limit}"
+              effort="${wrist_3_effort_limit}" velocity="${wrist_3_velocity_limit}"/>
+        <xacro:if value="${safety_limits}">
+          <safety_controller soft_lower_limit="${wrist_3_lower_limit + safety_pos_margin}" soft_upper_limit="${wrist_3_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+        </xacro:if>
+        <dynamics damping="0" friction="0"/>
+      </joint>
+    </xacro:if>
+    <xacro:unless value="${sec_limits['wrist_3_joint']['has_position_limits']}">
+      <joint name="${tf_prefix}wrist_3_joint" type="continuous">
+        <parent link="${tf_prefix}wrist_2_link" />
+        <child link="${tf_prefix}wrist_3_link" />
+        <origin xyz="${wrist_3_x} ${wrist_3_y} ${wrist_3_z}" rpy="${wrist_3_roll} ${wrist_3_pitch} ${wrist_3_yaw}" />
+        <axis xyz="0 0 1" />
+        <limit effort="${wrist_3_effort_limit}" velocity="${wrist_3_velocity_limit}"/>
+        <xacro:if value="${safety_limits}">
+          <safety_controller k_position="${safety_k_position}" k_velocity="0.0"/>
+        </xacro:if>
+        <dynamics damping="0" friction="0"/>
+      </joint>
+    </xacro:unless>
 
     <link name="${tf_prefix}ft_frame"/>
     <joint name="${tf_prefix}wrist_3_link-ft_frame" type="fixed">

From a0c29082ac321c44dbce0570cada824d94258ce9 Mon Sep 17 00:00:00 2001
From: URJala <jala@universal-robots.com>
Date: Wed, 25 Sep 2024 02:20:19 +0000
Subject: [PATCH 2/3] Cut down on code duplication

---
 urdf/inc/ur_common.xacro |  4 ++++
 urdf/ur_macro.xacro      | 36 +++++++++++++-----------------------
 2 files changed, 17 insertions(+), 23 deletions(-)

diff --git a/urdf/inc/ur_common.xacro b/urdf/inc/ur_common.xacro
index 9919147c..a9693ec1 100644
--- a/urdf/inc/ur_common.xacro
+++ b/urdf/inc/ur_common.xacro
@@ -79,9 +79,13 @@
     <xacro:property name="wrist_2_velocity_limit" value="${sec_limits['wrist_2_joint']['max_velocity']}" scope="parent"/>
     <xacro:property name="wrist_2_effort_limit" value="${sec_limits['wrist_2_joint']['max_effort']}" scope="parent"/>
     <xacro:if value="${sec_limits['wrist_3_joint']['has_position_limits']}">
+      <xacro:property name="wrist_3_joint_type" value="revolute" scope="parent"/>
       <xacro:property name="wrist_3_lower_limit" value="${sec_limits['wrist_3_joint']['min_position']}" scope="parent"/>
       <xacro:property name="wrist_3_upper_limit" value="${sec_limits['wrist_3_joint']['max_position']}" scope="parent"/>
     </xacro:if>
+    <xacro:unless value="${sec_limits['wrist_3_joint']['has_position_limits']}">
+      <xacro:property name="wrist_3_joint_type" value="continuous" scope="parent"/>
+    </xacro:unless>
     <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"/>
 
diff --git a/urdf/ur_macro.xacro b/urdf/ur_macro.xacro
index d6218475..f8c30f28 100644
--- a/urdf/ur_macro.xacro
+++ b/urdf/ur_macro.xacro
@@ -75,9 +75,6 @@
       visual_parameters_file="${visual_parameters_file}"
       force_abs_paths="${force_abs_paths}"/>
 
-    <xacro:property name="config_joint_limit_parameters" value="${xacro.load_yaml(joint_limits_parameters_file)}"/>
-    <xacro:property name="sec_limits" value="${config_joint_limit_parameters['joint_limits']}"/>
-
     <!-- links -  main serial chain -->
     <link name="${tf_prefix}base_link"/>
     <link name="${tf_prefix}base_link_inertia">
@@ -334,33 +331,26 @@
       </xacro:if>
       <dynamics damping="0" friction="0"/>
     </joint>
-    <xacro:if value="${sec_limits['wrist_3_joint']['has_position_limits']}">
-      <joint name="${tf_prefix}wrist_3_joint" type="revolute">
-        <parent link="${tf_prefix}wrist_2_link" />
-        <child link="${tf_prefix}wrist_3_link" />
-        <origin xyz="${wrist_3_x} ${wrist_3_y} ${wrist_3_z}" rpy="${wrist_3_roll} ${wrist_3_pitch} ${wrist_3_yaw}" />
-        <axis xyz="0 0 1" />
-        <limit lower="${wrist_3_lower_limit}" upper="${wrist_3_upper_limit}"
-              effort="${wrist_3_effort_limit}" velocity="${wrist_3_velocity_limit}"/>
+    <joint name="${tf_prefix}wrist_3_joint" type="${wrist_3_joint_type}">
+      <parent link="${tf_prefix}wrist_2_link" />
+      <child link="${tf_prefix}wrist_3_link" />
+      <origin xyz="${wrist_3_x} ${wrist_3_y} ${wrist_3_z}" rpy="${wrist_3_roll} ${wrist_3_pitch} ${wrist_3_yaw}" />
+      <axis xyz="0 0 1" />
+      <xacro:if value="${wrist_3_joint_type != 'continuous'}">
+        <limit lower="${wrist_3_lower_limit}" upper="${wrist_3_upper_limit}" 
+          effort="${wrist_3_effort_limit}" velocity="${wrist_3_velocity_limit}"/>
         <xacro:if value="${safety_limits}">
           <safety_controller soft_lower_limit="${wrist_3_lower_limit + safety_pos_margin}" soft_upper_limit="${wrist_3_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
         </xacro:if>
-        <dynamics damping="0" friction="0"/>
-      </joint>
-    </xacro:if>
-    <xacro:unless value="${sec_limits['wrist_3_joint']['has_position_limits']}">
-      <joint name="${tf_prefix}wrist_3_joint" type="continuous">
-        <parent link="${tf_prefix}wrist_2_link" />
-        <child link="${tf_prefix}wrist_3_link" />
-        <origin xyz="${wrist_3_x} ${wrist_3_y} ${wrist_3_z}" rpy="${wrist_3_roll} ${wrist_3_pitch} ${wrist_3_yaw}" />
-        <axis xyz="0 0 1" />
+      </xacro:if>
+      <xacro:unless value="${wrist_3_joint_type != 'continuous'}">
         <limit effort="${wrist_3_effort_limit}" velocity="${wrist_3_velocity_limit}"/>
         <xacro:if value="${safety_limits}">
           <safety_controller k_position="${safety_k_position}" k_velocity="0.0"/>
         </xacro:if>
-        <dynamics damping="0" friction="0"/>
-      </joint>
-    </xacro:unless>
+      </xacro:unless>
+      <dynamics damping="0" friction="0"/>
+    </joint>
 
     <link name="${tf_prefix}ft_frame"/>
     <joint name="${tf_prefix}wrist_3_link-ft_frame" type="fixed">

From 3e641e77d1882bafb6199c9d25aa3ce056cabd82 Mon Sep 17 00:00:00 2001
From: URJala <jala@universal-robots.com>
Date: Wed, 25 Sep 2024 02:26:35 +0000
Subject: [PATCH 3/3] Formatting

---
 urdf/ur_macro.xacro | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/urdf/ur_macro.xacro b/urdf/ur_macro.xacro
index f8c30f28..7e644595 100644
--- a/urdf/ur_macro.xacro
+++ b/urdf/ur_macro.xacro
@@ -337,7 +337,7 @@
       <origin xyz="${wrist_3_x} ${wrist_3_y} ${wrist_3_z}" rpy="${wrist_3_roll} ${wrist_3_pitch} ${wrist_3_yaw}" />
       <axis xyz="0 0 1" />
       <xacro:if value="${wrist_3_joint_type != 'continuous'}">
-        <limit lower="${wrist_3_lower_limit}" upper="${wrist_3_upper_limit}" 
+        <limit lower="${wrist_3_lower_limit}" upper="${wrist_3_upper_limit}"
           effort="${wrist_3_effort_limit}" velocity="${wrist_3_velocity_limit}"/>
         <xacro:if value="${safety_limits}">
           <safety_controller soft_lower_limit="${wrist_3_lower_limit + safety_pos_margin}" soft_upper_limit="${wrist_3_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>