From 985ff827b0fe46f279421eef43b2adfffb567e29 Mon Sep 17 00:00:00 2001
From: dan <dan@polarworks.no>
Date: Thu, 23 Mar 2023 13:14:28 -0400
Subject: [PATCH] Add friction for Gazebo Classic max_force hack

---
 urdf/ur_macro.xacro | 13 +++++++------
 1 file changed, 7 insertions(+), 6 deletions(-)

diff --git a/urdf/ur_macro.xacro b/urdf/ur_macro.xacro
index 7e43bcd..d0b1a29 100644
--- a/urdf/ur_macro.xacro
+++ b/urdf/ur_macro.xacro
@@ -278,6 +278,7 @@
       -->
       <origin xyz="0 0 0" rpy="0 0 ${pi}" />
     </joint>
+    <!-- Conditional dynamics tags set the joint friction to 100x the joint's effort limit per discussion at Universal_Robots_ROS2_Gazebo_Simulation/issues/19 -->
     <joint name="${prefix}shoulder_pan_joint" type="revolute">
       <parent link="${prefix}base_link_inertia" />
       <child link="${prefix}shoulder_link" />
@@ -288,7 +289,7 @@
       <xacro:if value="${safety_limits}">
          <safety_controller soft_lower_limit="${shoulder_pan_lower_limit + safety_pos_margin}" soft_upper_limit="${shoulder_pan_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
       </xacro:if>
-      <dynamics damping="0" friction="0"/>
+      <dynamics damping="0" friction="${shoulder_pan_effort_limit*100.0 if sim_gazebo else 0}"/>
     </joint>
     <joint name="${prefix}shoulder_lift_joint" type="revolute">
       <parent link="${prefix}shoulder_link" />
@@ -300,7 +301,7 @@
       <xacro:if value="${safety_limits}">
          <safety_controller soft_lower_limit="${shoulder_lift_lower_limit + safety_pos_margin}" soft_upper_limit="${shoulder_lift_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
       </xacro:if>
-      <dynamics damping="0" friction="0"/>
+      <dynamics damping="0" friction="${shoulder_lift_effort_limit*100.0 if sim_gazebo else 0}"/>
     </joint>
     <joint name="${prefix}elbow_joint" type="revolute">
       <parent link="${prefix}upper_arm_link" />
@@ -312,7 +313,7 @@
       <xacro:if value="${safety_limits}">
          <safety_controller soft_lower_limit="${elbow_joint_lower_limit + safety_pos_margin}" soft_upper_limit="${elbow_joint_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
       </xacro:if>
-      <dynamics damping="0" friction="0"/>
+      <dynamics damping="0" friction="${elbow_joint_effort_limit*100.0 if sim_gazebo else 0}"/>
     </joint>
     <joint name="${prefix}wrist_1_joint" type="revolute">
       <parent link="${prefix}forearm_link" />
@@ -324,7 +325,7 @@
       <xacro:if value="${safety_limits}">
          <safety_controller soft_lower_limit="${wrist_1_lower_limit + safety_pos_margin}" soft_upper_limit="${wrist_1_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
       </xacro:if>
-      <dynamics damping="0" friction="0"/>
+      <dynamics damping="0" friction="${wrist_1_effort_limit*100.0 if sim_gazebo else 0}"/>
     </joint>
     <joint name="${prefix}wrist_2_joint" type="revolute">
       <parent link="${prefix}wrist_1_link" />
@@ -336,7 +337,7 @@
       <xacro:if value="${safety_limits}">
          <safety_controller soft_lower_limit="${wrist_2_lower_limit + safety_pos_margin}" soft_upper_limit="${wrist_2_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
       </xacro:if>
-      <dynamics damping="0" friction="0"/>
+      <dynamics damping="0" friction="${wrist_2_effort_limit*100.0 if sim_gazebo else 0}"/>
     </joint>
     <joint name="${prefix}wrist_3_joint" type="revolute">
       <parent link="${prefix}wrist_2_link" />
@@ -348,7 +349,7 @@
       <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"/>
+      <dynamics damping="0" friction="${wrist_3_effort_limit*100.0 if sim_gazebo else 0}"/>
     </joint>
 
     <link name="${prefix}ft_frame"/>