From 0e519442ebf7be874b423223ad8f0094cc8fcb5e Mon Sep 17 00:00:00 2001
From: hello-binit <64861565+hello-binit@users.noreply.github.com>
Date: Wed, 23 Jun 2021 18:59:35 -0400
Subject: [PATCH] Revert all xacros to pre-gazebo merge

---
 stretch_description/urdf/stretch_aruco.xacro  |   6 +-
 stretch_description/urdf/stretch_d435i.xacro  |   6 +-
 .../urdf/stretch_gripper.xacro                |  39 ++--
 .../urdf/stretch_laser_range_finder.xacro     |   1 +
 stretch_description/urdf/stretch_main.xacro   | 202 +++++++-----------
 5 files changed, 109 insertions(+), 145 deletions(-)

diff --git a/stretch_description/urdf/stretch_aruco.xacro b/stretch_description/urdf/stretch_aruco.xacro
index 8902d2a..602f60c 100644
--- a/stretch_description/urdf/stretch_aruco.xacro
+++ b/stretch_description/urdf/stretch_aruco.xacro
@@ -119,7 +119,7 @@
         xyz="-2.77555756156289E-17 2.56739074444567E-16 -0.000125000000000042"
         rpy="0 0 0" />
       <mass
-        value="0" />
+        value="0.000399141592653546" />
       <inertia
         ixx="0"
         ixy="0"
@@ -174,7 +174,7 @@
         xyz="1.11022302462516E-16 3.05311331771918E-16 -0.00012499999999821"
         rpy="0 0 0" />
       <mass
-        value="0" />
+        value="0.000224999999999975" />
       <inertia
         ixx="0"
         ixy="0"
@@ -229,7 +229,7 @@
         xyz="8.32667268468867E-17 1.77635683940025E-15 -0.000125000000000264"
         rpy="0 0 0" />
       <mass
-        value="0" />
+        value="0.000225" />
       <inertia
         ixx="0"
         ixy="0"
diff --git a/stretch_description/urdf/stretch_d435i.xacro b/stretch_description/urdf/stretch_d435i.xacro
index ea8d71c..ff85f30 100644
--- a/stretch_description/urdf/stretch_d435i.xacro
+++ b/stretch_description/urdf/stretch_d435i.xacro
@@ -3,18 +3,18 @@
 
   <xacro:arg name="use_nominal_extrinsics" default="true"/>
   <xacro:arg name="add_plug" default="false"/>
-  <xacro:include filename="$(find realsense2_description)/urdf/_d435i.urdf.xacro"/>
+  <xacro:include filename="$(find realsense2_description)/urdf/_d435.urdf.xacro"/>
 
   <!-- xyz = "
        depth (- recessed into head / + protruding from front)
        up and down (- down / + up)
        sideways (- right / + left)
        " -->
-  <xacro:sensor_d435i parent="link_head_tilt" use_nominal_extrinsics="$(arg use_nominal_extrinsics)">
+  <xacro:sensor_d435 parent="link_head_tilt" use_nominal_extrinsics="$(arg use_nominal_extrinsics)">
     <origin
       xyz="0.03 -0.0122 0.0182"
       rpy="0.0 0.0 0.0" />
-  </xacro:sensor_d435i>
+  </xacro:sensor_d435>
   
 </robot>
 
diff --git a/stretch_description/urdf/stretch_gripper.xacro b/stretch_description/urdf/stretch_gripper.xacro
index 03acc68..14615d8 100644
--- a/stretch_description/urdf/stretch_gripper.xacro
+++ b/stretch_description/urdf/stretch_gripper.xacro
@@ -12,12 +12,12 @@
       <mass
         value="0.101902711393094" />
       <inertia
-        ixx="0.001"
-        ixy="0"
-        ixz="0"
-        iyy="0.001"
-        iyz="0"
-        izz="0.001" />
+        ixx="2.79241618017607E-05"
+        ixy="-1.10819247449605E-05"
+        ixz="-1.50343284036654E-05"
+        iyy="3.67945130513069E-05"
+        iyz="-4.33280448535579E-06"
+        izz="4.14524691955231E-05" />
     </inertial>
     <visual>
       <origin
@@ -69,12 +69,12 @@
       <mass
         value="0.0476207785199474" />
       <inertia
-        ixx="0.001"
+        ixx="0"
         ixy="0"
         ixz="0"
-        iyy="0.001"
+        iyy="0"
         iyz="0"
-        izz="0.001" />
+        izz="0" />
     </inertial>
     <visual>
       <origin
@@ -125,12 +125,12 @@
       <mass
         value="0.00382159917455729" />
       <inertia
-        ixx="0.001"
+        ixx="0"
         ixy="0"
         ixz="0"
-        iyy="0.001"
+        iyy="0"
         iyz="0"
-        izz="0.001" />
+        izz="0" />
     </inertial>
     <visual>
       <origin
@@ -180,12 +180,12 @@
       <mass
         value="0.0476207785199481" />
       <inertia
-        ixx="0.001"
+        ixx="0"
         ixy="0"
         ixz="0"
-        iyy="0.001"
+        iyy="0"
         iyz="0"
-        izz="0.001" />
+        izz="0" />
     </inertial>
     <visual>
       <origin
@@ -236,12 +236,12 @@
       <mass
         value="0.00382160037319545" />
       <inertia
-        ixx="0.001"
+        ixx="0"
         ixy="0"
         ixz="0"
-        iyy="0.001"
+        iyy="0"
         iyz="0"
-        izz="0.001" />
+        izz="0" />
     </inertial>
     <visual>
       <origin
@@ -300,3 +300,6 @@
   </joint>
   
 </robot>
+
+  
+
diff --git a/stretch_description/urdf/stretch_laser_range_finder.xacro b/stretch_description/urdf/stretch_laser_range_finder.xacro
index a770215..af62fb6 100644
--- a/stretch_description/urdf/stretch_laser_range_finder.xacro
+++ b/stretch_description/urdf/stretch_laser_range_finder.xacro
@@ -58,4 +58,5 @@
 
 </robot>
 
+  
 
diff --git a/stretch_description/urdf/stretch_main.xacro b/stretch_description/urdf/stretch_main.xacro
index 7113379..7d878f2 100644
--- a/stretch_description/urdf/stretch_main.xacro
+++ b/stretch_description/urdf/stretch_main.xacro
@@ -1,11 +1,6 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="stretch_main">
-  <xacro:property name="M_PI" value="3.1415926535897931" /> 
-  <xacro:property name="joint_damping" value="21.75"/>
-  <xacro:property name="joint_friction" value="10.48"/>
-  <xacro:property name="joint_spring_stiffness" value="0"/>
-  <xacro:property name="joint_spring_reference" value="0"/>
-
+  
   <link
     name="base_link">
     <inertial>
@@ -54,14 +49,14 @@
         xyz="1.25554620866719E-07 3.54748938447003E-07 0.0239581106165018"
         rpy="0 0 0" />
       <mass
-        value="0.01" />
+        value="0.00356714564938475" />
       <inertia
-        ixx="0.001"
+        ixx="0"
         ixy="0"
         ixz="0"
-        iyy="0.001"
+        iyy="0"
         iyz="0"
-        izz="0.001" />
+        izz="0" />
     </inertial>
     <visual>
       <origin
@@ -92,7 +87,7 @@
     name="joint_right_wheel"
     type="continuous">
     <origin
-      xyz="-0.003 -0.15765 0.05"
+      xyz="-0.00300000000000034 -0.15765 0.0508000000000004"
       rpy="-1.57079632679489 -1.11022302462516E-16 7.28583859910277E-17" />
     <parent
       link="base_link" />
@@ -100,7 +95,6 @@
       link="link_right_wheel" />
     <axis
       xyz="0 0 1" />
-      <dynamics damping="${joint_damping}" friction="${joint_friction}" spring_reference="${joint_spring_reference}" spring_stiffness="{joint_spring_stiffness}"/>
   </joint>
   
   <link
@@ -110,14 +104,14 @@
         xyz="1.2555462092223E-07 -3.54748938502514E-07 -0.0239581106165035"
         rpy="0 0 0" />
       <mass
-        value="0.01" />
+        value="0.00356714564938459" />
       <inertia
-        ixx="0.001"
+        ixx="0"
         ixy="0"
         ixz="0"
-        iyy="0.001"
+        iyy="0"
         iyz="0"
-        izz="0.001" />
+        izz="0" />
     </inertial>
     <visual>
       <origin
@@ -148,7 +142,7 @@
     name="joint_left_wheel"
     type="continuous">
     <origin
-      xyz="-0.003 0.15765 0.05"
+      xyz="-0.00300000000001899 0.15765 0.0507999999999994"
       rpy="-1.5707963267949 3.16227047920818E-31 7.88745438253713E-16" />
     <parent
       link="base_link" />
@@ -156,60 +150,8 @@
       link="link_left_wheel" />
     <axis
       xyz="0 0 1" />
-      <dynamics damping="${joint_damping}" friction="${joint_friction}" spring_reference="${joint_spring_reference}" spring_stiffness="{joint_spring_stiffness}"/>
-  </joint>
-
-  <link name="caster_link">
-    <collision>
-      <geometry>
-        <sphere radius="0.032"/>
-      </geometry>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <surface>
-        <friction>
-          <ode>
-            <mu>0</mu>
-            <mu2>0</mu2>
-            <slip1>1.0</slip1>
-            <slip2>1.0</slip2>
-          </ode>
-        </friction>
-      </surface>
-    </collision>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://stretch_description/meshes/omni_wheel_m.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
-      </material>
-    </visual>
-    <inertial>
-      <mass value="0.01" />
-      <origin xyz="0 0 0" />
-      <inertia
-        ixx="0.001"
-        ixy="0"
-        ixz="0"
-        iyy="0.001"
-        iyz="0"
-        izz="0.001" />
-    </inertial>
-  </link>
-
-  <joint name="caster_joint" type="fixed">
-    <parent link="base_link"/>
-    <child link="caster_link"/>
-    <origin xyz="-0.245 0.0 0.032" rpy="${-M_PI/2} 0 0"/>
-    <axis xyz="0 0 1" />
   </joint>
-
+  
   <link
     name="link_mast">
     <inertial>
@@ -310,7 +252,7 @@
     name="joint_lift"
     type="prismatic">
     <origin
-      xyz="-0.0369217062323472 0.268771199999996 -0.000341653286793524"
+      xyz="-0.0369217062323472 0.165471199999996 -0.000341653286793524"
       rpy="-1.57079632679552 1.5615431375292 -6.2942004366467E-13" />
     <parent
       link="link_mast" />
@@ -318,7 +260,10 @@
       link="link_lift" />
     <axis
 	xyz="0 0 1" />
-  <limit effort="100" lower="-0.1033" upper="0.9967" velocity="1.0"/>
+  <!-- for now: hand copied range_m: from lift: from ~/repos/stretch_fleet/stretch-re1-1001/stretch_re1_factory_params.yaml -->
+  <!--<limit effort="100" lower="0.0" upper="1.095" velocity="1.0"/>-->
+  <!-- copied value did not reach the top of mesh model with GUI sliders and RViz -->
+  <limit effort="100" lower="0.0" upper="1.1" velocity="1.0"/>
   </joint>
   
   <link
@@ -385,12 +330,12 @@
       <mass
         value="0.0626921047058405" />
       <inertia
-        ixx="0.001"
-        ixy="0"
-        ixz="0"
-        iyy="0.001"
-        iyz="0"
-        izz="0.001" />
+        ixx="0.000115246639451415"
+        ixy="4.42139679831176E-14"
+        ixz="1.89711279909763E-09"
+        iyy="0.000115246639420046"
+        iyz="-2.68592949587539E-09"
+        izz="9.38270676408352E-14" />
     </inertial>
     <visual>
       <origin
@@ -429,6 +374,7 @@
       link="link_arm_l3" />
     <axis
 	xyz="0 0 1" />
+    <!-- 0.13 = 0.52/4-->
     <limit effort="100" lower="0.0" upper="0.13" velocity="1.0"/>
   </joint>
   
@@ -441,12 +387,12 @@
       <mass
         value="0.0569074368576238" />
       <inertia
-        ixx="0.001"
-        ixy="0"
-        ixz="0"
-        iyy="0.001"
-        iyz="0"
-        izz="0.001" />
+        ixx="0.000105206697151872"
+        ixy="2.59896259031046E-14"
+        ixz="1.69941454517918E-09"
+        iyy="0.000105206697154717"
+        iyz="-1.60895568923743E-09"
+        izz="5.20570301686304E-14" />
     </inertial>
     <visual>
       <origin
@@ -485,6 +431,7 @@
       link="link_arm_l2" />
     <axis
 	xyz="0 0 1" />
+    <!-- 0.13 = 0.52/4-->
     <limit effort="100" lower="0.0" upper="0.13" velocity="1.0"/>
   </joint>
   
@@ -497,12 +444,12 @@
       <mass
         value="0.0511763619538321" />
       <inertia
-        ixx="0.001"
-        ixy="0"
-        ixz="0"
-        iyy="0.001"
-        iyz="0"
-        izz="0.001" />
+        ixx="9.58490021274768E-05"
+        ixy="2.27446667584309E-14"
+        ixz="1.54951672295709E-09"
+        iyy="9.5849002131875E-05"
+        iyz="-1.40692486873422E-09"
+        izz="4.57014633924348E-14" />
     </inertial>
     <visual>
       <origin
@@ -541,6 +488,7 @@
       link="link_arm_l1" />
     <axis
 	xyz="0 0 1" />
+    <!-- 0.13 = 0.52/4-->
     <limit effort="100" lower="0.0" upper="0.13" velocity="1.0"/>
   </joint>
   
@@ -553,12 +501,12 @@
       <mass
         value="0.101241250325294" />
       <inertia
-        ixx="0.001"
-        ixy="0"
-        ixz="0"
-        iyy="0.001"
-        iyz="0"
-        izz="0.001" />
+        ixx="0.000227156999010352"
+        ixy="8.90680620109423E-06"
+        ixz="-7.91132210985695E-05"
+        iyy="0.000300732790537324"
+        iyz="7.21771824540896E-06"
+        izz="9.35784422621597E-05" />
     </inertial>
     <visual>
       <origin
@@ -597,6 +545,7 @@
       link="link_arm_l0" />
     <axis
 	xyz="0 0 1" />
+    <!-- 0.13 = 0.52/4-->
     <limit effort="100" lower="0.0" upper="0.13" velocity="1.0"/>
   </joint>
   
@@ -609,12 +558,12 @@
       <mass
         value="0.0405398981326229" />
       <inertia
-        ixx="0.001"
-        ixy="0"
-        ixz="0"
-        iyy="0.001"
-        iyz="0"
-        izz="0.001" />
+        ixx="2.20062857967227E-09"
+        ixy="1.15494401227051E-13"
+        ixz="1.02598056874346E-11"
+        iyy="2.20039759185237E-09"
+        iyz="-2.47692875363392E-11"
+        izz="3.26666802416509E-13" />
     </inertial>
     <visual>
       <origin
@@ -627,7 +576,7 @@
       <material
         name="">
         <color
-	rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
+	  rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
       </material>
     </visual>
     <collision>
@@ -653,6 +602,12 @@
       link="link_wrist_yaw" />
     <axis
       xyz="0 0 -1" />
+    <!--
+	stowed to front ~225 deg: 3.15159 x 1.25 = 3.9395
+	using 4.0
+	stowed to back ~100 deg:  100 / 180 x 3.14159 = 1.7453
+	using -1.75
+    -->
     <limit effort="100" lower="-1.75" upper="4.0" velocity="1.0"/>
   </joint>
   
@@ -665,12 +620,12 @@
       <mass
         value="0.129454717596498" />
       <inertia
-        ixx="0.001"
-        ixy="0"
-        ixz="0"
-        iyy="0.001"
-        iyz="0"
-        izz="0.001" />
+        ixx="2.9340596231093E-05"
+        ixy="-7.89638931853666E-07"
+        ixz="-4.74678128515878E-06"
+        iyy="2.68283736177377E-05"
+        iyz="1.80251508621876E-06"
+        izz="1.4617291423745E-05" />
     </inertial>
     <visual>
       <origin
@@ -720,12 +675,12 @@
       <mass
         value="0.035858341182617" />
       <inertia
-        ixx="0.001"
-        ixy="0"
-        ixz="0"
-        iyy="0.001"
-        iyz="0"
-        izz="0.001" />
+        ixx="2.75847937559827E-05"
+        ixy="5.25762707913649E-07"
+        ixz="-9.28796751052893E-07"
+        iyy="2.08815921106858E-05"
+        iyz="1.17265055331737E-05"
+        izz="6.78612505435872E-06" />
     </inertial>
     <visual>
       <origin
@@ -764,6 +719,7 @@
       link="link_head_pan" />
     <axis
 	xyz="0 0 1" />
+    <!-- unconstrained range for now -->
     <limit effort="100" lower="-3.9" upper="1.5" velocity="1.0"/>
   </joint>
   
@@ -776,12 +732,12 @@
       <mass
         value="0.0701242408063442" />
       <inertia
-        ixx="0.001"
-        ixy="0"
-        ixz="0"
-        iyy="0.001"
-        iyz="0"
-        izz="0.001" />
+        ixx="1.73265934767466E-05"
+        ixy="2.38363792824069E-07"
+        ixz="7.96430204321056E-08"
+        iyy="4.11044416077217E-05"
+        iyz="-9.41955638215997E-07"
+        izz="2.38851808140542E-05" />
     </inertial>
     <visual>
       <origin
@@ -820,7 +776,11 @@
       link="link_head_tilt" />
     <axis
       xyz="0 0 1" />
+    <!-- unconstrained range for now -->
     <limit effort="100" lower="-1.53" upper="0.79" velocity="1.0"/>
   </joint>
 
 </robot>
+
+  
+